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 <g.gael@free.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 // no include guard, we'll include this twice from All.h from Eigen2Support, and it's internal anyway
00012 
00013 namespace Eigen { 
00014 
00028 template <typename _Scalar, int _AmbientDim>
00029 class ParametrizedLine
00030 {
00031 public:
00032   EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF_VECTORIZABLE_FIXED_SIZE(_Scalar,_AmbientDim)
00033   enum { AmbientDimAtCompileTime = _AmbientDim };
00034   typedef _Scalar Scalar;
00035   typedef typename NumTraits<Scalar>::Real RealScalar;
00036   typedef Matrix<Scalar,AmbientDimAtCompileTime,1> VectorType;
00037 
00039   inline ParametrizedLine() {}
00040 
00043   inline explicit ParametrizedLine(int _dim) : m_origin(_dim), m_direction(_dim) {}
00044 
00048   ParametrizedLine(const VectorType& origin, const VectorType& direction)
00049     : m_origin(origin), m_direction(direction) {}
00050 
00051   explicit ParametrizedLine(const Hyperplane<_Scalar, _AmbientDim>& hyperplane);
00052 
00054   static inline ParametrizedLine Through(const VectorType& p0, const VectorType& p1)
00055   { return ParametrizedLine(p0, (p1-p0).normalized()); }
00056 
00057   ~ParametrizedLine() {}
00058 
00060   inline int dim() const { return m_direction.size(); }
00061 
00062   const VectorType& origin() const { return m_origin; }
00063   VectorType& origin() { return m_origin; }
00064 
00065   const VectorType& direction() const { return m_direction; }
00066   VectorType& direction() { return m_direction; }
00067 
00071   RealScalar squaredDistance(const VectorType& p) const
00072   {
00073     VectorType diff = p-origin();
00074     return (diff - diff.eigen2_dot(direction())* direction()).squaredNorm();
00075   }
00079   RealScalar distance(const VectorType& p) const { return ei_sqrt(squaredDistance(p)); }
00080 
00082   VectorType projection(const VectorType& p) const
00083   { return origin() + (p-origin()).eigen2_dot(direction()) * direction(); }
00084 
00085   Scalar intersection(const Hyperplane<_Scalar, _AmbientDim>& hyperplane);
00086 
00092   template<typename NewScalarType>
00093   inline typename internal::cast_return_type<ParametrizedLine,
00094            ParametrizedLine<NewScalarType,AmbientDimAtCompileTime> >::type cast() const
00095   {
00096     return typename internal::cast_return_type<ParametrizedLine,
00097                     ParametrizedLine<NewScalarType,AmbientDimAtCompileTime> >::type(*this);
00098   }
00099 
00101   template<typename OtherScalarType>
00102   inline explicit ParametrizedLine(const ParametrizedLine<OtherScalarType,AmbientDimAtCompileTime>& other)
00103   {
00104     m_origin = other.origin().template cast<Scalar>();
00105     m_direction = other.direction().template cast<Scalar>();
00106   }
00107 
00112   bool isApprox(const ParametrizedLine& other, typename NumTraits<Scalar>::Real prec = precision<Scalar>()) const
00113   { return m_origin.isApprox(other.m_origin, prec) && m_direction.isApprox(other.m_direction, prec); }
00114 
00115 protected:
00116 
00117   VectorType m_origin, m_direction;
00118 };
00119 
00124 template <typename _Scalar, int _AmbientDim>
00125 inline ParametrizedLine<_Scalar, _AmbientDim>::ParametrizedLine(const Hyperplane<_Scalar, _AmbientDim>& hyperplane)
00126 {
00127   EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(VectorType, 2)
00128   direction() = hyperplane.normal().unitOrthogonal();
00129   origin() = -hyperplane.normal()*hyperplane.offset();
00130 }
00131 
00134 template <typename _Scalar, int _AmbientDim>
00135 inline _Scalar ParametrizedLine<_Scalar, _AmbientDim>::intersection(const Hyperplane<_Scalar, _AmbientDim>& hyperplane)
00136 {
00137   return -(hyperplane.offset()+origin().eigen2_dot(hyperplane.normal()))
00138           /(direction().eigen2_dot(hyperplane.normal()));
00139 }
00140 
00141 } // end namespace Eigen


turtlebot_exploration_3d
Author(s): Bona , Shawn
autogenerated on Thu Jun 6 2019 20:59:07