ParametrizedLine.h
Go to the documentation of this file.
00001 // This file is part of Eigen, a lightweight C++ template library
00002 // for linear algebra.
00003 //
00004 // Copyright (C) 2008 Gael Guennebaud <gael.guennebaud@inria.fr>
00005 // Copyright (C) 2008 Benoit Jacob <jacob.benoit.1@gmail.com>
00006 //
00007 // This Source Code Form is subject to the terms of the Mozilla
00008 // Public License v. 2.0. If a copy of the MPL was not distributed
00009 // with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
00010 
00011 #ifndef EIGEN_PARAMETRIZEDLINE_H
00012 #define EIGEN_PARAMETRIZEDLINE_H
00013 
00014 namespace Eigen { 
00015 
00029 template <typename _Scalar, int _AmbientDim, int _Options>
00030 class ParametrizedLine
00031 {
00032 public:
00033   EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF_VECTORIZABLE_FIXED_SIZE(_Scalar,_AmbientDim)
00034   enum {
00035     AmbientDimAtCompileTime = _AmbientDim,
00036     Options = _Options
00037   };
00038   typedef _Scalar Scalar;
00039   typedef typename NumTraits<Scalar>::Real RealScalar;
00040   typedef DenseIndex Index;
00041   typedef Matrix<Scalar,AmbientDimAtCompileTime,1,Options> VectorType;
00042 
00044   inline ParametrizedLine() {}
00045   
00046   template<int OtherOptions>
00047   ParametrizedLine(const ParametrizedLine<Scalar,AmbientDimAtCompileTime,OtherOptions>& other)
00048    : m_origin(other.origin()), m_direction(other.direction())
00049   {}
00050 
00053   inline explicit ParametrizedLine(Index _dim) : m_origin(_dim), m_direction(_dim) {}
00054 
00058   ParametrizedLine(const VectorType& origin, const VectorType& direction)
00059     : m_origin(origin), m_direction(direction) {}
00060 
00061   template <int OtherOptions>
00062   explicit ParametrizedLine(const Hyperplane<_Scalar, _AmbientDim, OtherOptions>& hyperplane);
00063 
00065   static inline ParametrizedLine Through(const VectorType& p0, const VectorType& p1)
00066   { return ParametrizedLine(p0, (p1-p0).normalized()); }
00067 
00068   ~ParametrizedLine() {}
00069 
00071   inline Index dim() const { return m_direction.size(); }
00072 
00073   const VectorType& origin() const { return m_origin; }
00074   VectorType& origin() { return m_origin; }
00075 
00076   const VectorType& direction() const { return m_direction; }
00077   VectorType& direction() { return m_direction; }
00078 
00082   RealScalar squaredDistance(const VectorType& p) const
00083   {
00084     VectorType diff = p - origin();
00085     return (diff - direction().dot(diff) * direction()).squaredNorm();
00086   }
00090   RealScalar distance(const VectorType& p) const { using std::sqrt; return sqrt(squaredDistance(p)); }
00091 
00093   VectorType projection(const VectorType& p) const
00094   { return origin() + direction().dot(p-origin()) * direction(); }
00095 
00096   VectorType pointAt(const Scalar& t) const;
00097   
00098   template <int OtherOptions>
00099   Scalar intersectionParameter(const Hyperplane<_Scalar, _AmbientDim, OtherOptions>& hyperplane) const;
00100  
00101   template <int OtherOptions>
00102   Scalar intersection(const Hyperplane<_Scalar, _AmbientDim, OtherOptions>& hyperplane) const;
00103   
00104   template <int OtherOptions>
00105   VectorType intersectionPoint(const Hyperplane<_Scalar, _AmbientDim, OtherOptions>& hyperplane) const;
00106 
00112   template<typename NewScalarType>
00113   inline typename internal::cast_return_type<ParametrizedLine,
00114            ParametrizedLine<NewScalarType,AmbientDimAtCompileTime,Options> >::type cast() const
00115   {
00116     return typename internal::cast_return_type<ParametrizedLine,
00117                     ParametrizedLine<NewScalarType,AmbientDimAtCompileTime,Options> >::type(*this);
00118   }
00119 
00121   template<typename OtherScalarType,int OtherOptions>
00122   inline explicit ParametrizedLine(const ParametrizedLine<OtherScalarType,AmbientDimAtCompileTime,OtherOptions>& other)
00123   {
00124     m_origin = other.origin().template cast<Scalar>();
00125     m_direction = other.direction().template cast<Scalar>();
00126   }
00127 
00132   bool isApprox(const ParametrizedLine& other, typename NumTraits<Scalar>::Real prec = NumTraits<Scalar>::dummy_precision()) const
00133   { return m_origin.isApprox(other.m_origin, prec) && m_direction.isApprox(other.m_direction, prec); }
00134 
00135 protected:
00136 
00137   VectorType m_origin, m_direction;
00138 };
00139 
00144 template <typename _Scalar, int _AmbientDim, int _Options>
00145 template <int OtherOptions>
00146 inline ParametrizedLine<_Scalar, _AmbientDim,_Options>::ParametrizedLine(const Hyperplane<_Scalar, _AmbientDim,OtherOptions>& hyperplane)
00147 {
00148   EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(VectorType, 2)
00149   direction() = hyperplane.normal().unitOrthogonal();
00150   origin() = -hyperplane.normal()*hyperplane.offset();
00151 }
00152 
00155 template <typename _Scalar, int _AmbientDim, int _Options>
00156 inline typename ParametrizedLine<_Scalar, _AmbientDim,_Options>::VectorType
00157 ParametrizedLine<_Scalar, _AmbientDim,_Options>::pointAt(const _Scalar& t) const
00158 {
00159   return origin() + (direction()*t); 
00160 }
00161 
00164 template <typename _Scalar, int _AmbientDim, int _Options>
00165 template <int OtherOptions>
00166 inline _Scalar ParametrizedLine<_Scalar, _AmbientDim,_Options>::intersectionParameter(const Hyperplane<_Scalar, _AmbientDim, OtherOptions>& hyperplane) const
00167 {
00168   return -(hyperplane.offset()+hyperplane.normal().dot(origin()))
00169           / hyperplane.normal().dot(direction());
00170 }
00171 
00172 
00176 template <typename _Scalar, int _AmbientDim, int _Options>
00177 template <int OtherOptions>
00178 inline _Scalar ParametrizedLine<_Scalar, _AmbientDim,_Options>::intersection(const Hyperplane<_Scalar, _AmbientDim, OtherOptions>& hyperplane) const
00179 {
00180   return intersectionParameter(hyperplane);
00181 }
00182 
00185 template <typename _Scalar, int _AmbientDim, int _Options>
00186 template <int OtherOptions>
00187 inline typename ParametrizedLine<_Scalar, _AmbientDim,_Options>::VectorType
00188 ParametrizedLine<_Scalar, _AmbientDim,_Options>::intersectionPoint(const Hyperplane<_Scalar, _AmbientDim, OtherOptions>& hyperplane) const
00189 {
00190   return pointAt(intersectionParameter(hyperplane));
00191 }
00192 
00193 } // end namespace Eigen
00194 
00195 #endif // EIGEN_PARAMETRIZEDLINE_H


acado
Author(s): Milan Vukov, Rien Quirynen
autogenerated on Sat Jun 8 2019 19:38:15