Eigne Matrix Class Library
Dependents: Eigen_test Odometry_test AttitudeEstimation_usingTicker MPU9250_Quaternion_Binary_Serial ... more
Hyperplane.h
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_HYPERPLANE_H 00012 #define EIGEN_HYPERPLANE_H 00013 00014 namespace Eigen { 00015 00016 /** \geometry_module \ingroup Geometry_Module 00017 * 00018 * \class Hyperplane 00019 * 00020 * \brief A hyperplane 00021 * 00022 * A hyperplane is an affine subspace of dimension n-1 in a space of dimension n. 00023 * For example, a hyperplane in a plane is a line; a hyperplane in 3-space is a plane. 00024 * 00025 * \param _Scalar the scalar type, i.e., the type of the coefficients 00026 * \param _AmbientDim the dimension of the ambient space, can be a compile time value or Dynamic. 00027 * Notice that the dimension of the hyperplane is _AmbientDim-1. 00028 * 00029 * This class represents an hyperplane as the zero set of the implicit equation 00030 * \f$ n \cdot x + d = 0 \f$ where \f$ n \f$ is a unit normal vector of the plane (linear part) 00031 * and \f$ d \f$ is the distance (offset) to the origin. 00032 */ 00033 template <typename _Scalar, int _AmbientDim, int _Options> 00034 class Hyperplane 00035 { 00036 public: 00037 EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF_VECTORIZABLE_FIXED_SIZE(_Scalar,_AmbientDim==Dynamic ? Dynamic : _AmbientDim+1) 00038 enum { 00039 AmbientDimAtCompileTime = _AmbientDim, 00040 Options = _Options 00041 }; 00042 typedef _Scalar Scalar; 00043 typedef typename NumTraits<Scalar>::Real RealScalar; 00044 typedef DenseIndex Index; 00045 typedef Matrix<Scalar,AmbientDimAtCompileTime,1> VectorType ; 00046 typedef Matrix<Scalar,Index(AmbientDimAtCompileTime)==Dynamic 00047 ? Dynamic 00048 : Index(AmbientDimAtCompileTime)+1,1,Options> Coefficients ; 00049 typedef Block<Coefficients,AmbientDimAtCompileTime,1> NormalReturnType; 00050 typedef const Block<const Coefficients,AmbientDimAtCompileTime,1> ConstNormalReturnType; 00051 00052 /** Default constructor without initialization */ 00053 inline Hyperplane() {} 00054 00055 template<int OtherOptions> 00056 Hyperplane(const Hyperplane<Scalar,AmbientDimAtCompileTime,OtherOptions> & other) 00057 : m_coeffs(other.coeffs ()) 00058 {} 00059 00060 /** Constructs a dynamic-size hyperplane with \a _dim the dimension 00061 * of the ambient space */ 00062 inline explicit Hyperplane(Index _dim) : m_coeffs(_dim+1) {} 00063 00064 /** Construct a plane from its normal \a n and a point \a e onto the plane. 00065 * \warning the vector normal is assumed to be normalized. 00066 */ 00067 inline Hyperplane(const VectorType & n, const VectorType & e) 00068 : m_coeffs(n.size()+1) 00069 { 00070 normal () = n; 00071 offset () = -n.dot(e); 00072 } 00073 00074 /** Constructs a plane from its normal \a n and distance to the origin \a d 00075 * such that the algebraic equation of the plane is \f$ n \cdot x + d = 0 \f$. 00076 * \warning the vector normal is assumed to be normalized. 00077 */ 00078 inline Hyperplane(const VectorType & n, const Scalar& d) 00079 : m_coeffs(n.size()+1) 00080 { 00081 normal () = n; 00082 offset () = d; 00083 } 00084 00085 /** Constructs a hyperplane passing through the two points. If the dimension of the ambient space 00086 * is greater than 2, then there isn't uniqueness, so an arbitrary choice is made. 00087 */ 00088 static inline Hyperplane Through(const VectorType & p0, const VectorType & p1) 00089 { 00090 Hyperplane result(p0.size()); 00091 result.normal () = (p1 - p0).unitOrthogonal(); 00092 result.offset() = -p0.dot(result.normal()); 00093 return result; 00094 } 00095 00096 /** Constructs a hyperplane passing through the three points. The dimension of the ambient space 00097 * is required to be exactly 3. 00098 */ 00099 static inline Hyperplane Through(const VectorType & p0, const VectorType & p1, const VectorType & p2) 00100 { 00101 EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(VectorType , 3) 00102 Hyperplane result(p0.size()); 00103 VectorType v0(p2 - p0), v1(p1 - p0); 00104 result.normal() = v0.cross(v1); 00105 RealScalar norm = result.normal().norm(); 00106 if(norm <= v0.norm() * v1.norm() * NumTraits<RealScalar>::epsilon()) 00107 { 00108 Matrix<Scalar,2,3> m; m << v0.transpose(), v1.transpose(); 00109 JacobiSVD<Matrix<Scalar,2,3> > svd(m, ComputeFullV); 00110 result.normal() = svd.matrixV ().col(2); 00111 } 00112 else 00113 result.normal() /= norm; 00114 result.offset() = -p0.dot(result.normal()); 00115 return result; 00116 } 00117 00118 /** Constructs a hyperplane passing through the parametrized line \a parametrized. 00119 * If the dimension of the ambient space is greater than 2, then there isn't uniqueness, 00120 * so an arbitrary choice is made. 00121 */ 00122 // FIXME to be consitent with the rest this could be implemented as a static Through function ?? 00123 explicit Hyperplane(const ParametrizedLine<Scalar, AmbientDimAtCompileTime> & parametrized) 00124 { 00125 normal () = parametrized.direction().unitOrthogonal(); 00126 offset () = -parametrized.origin().dot(normal ()); 00127 } 00128 00129 ~Hyperplane () {} 00130 00131 /** \returns the dimension in which the plane holds */ 00132 inline Index dim () const { return AmbientDimAtCompileTime==Dynamic ? m_coeffs.size()-1 : Index(AmbientDimAtCompileTime); } 00133 00134 /** normalizes \c *this */ 00135 void normalize(void) 00136 { 00137 m_coeffs /= normal ().norm(); 00138 } 00139 00140 /** \returns the signed distance between the plane \c *this and a point \a p. 00141 * \sa absDistance() 00142 */ 00143 inline Scalar signedDistance (const VectorType & p) const { return normal ().dot(p) + offset (); } 00144 00145 /** \returns the absolute distance between the plane \c *this and a point \a p. 00146 * \sa signedDistance() 00147 */ 00148 inline Scalar absDistance (const VectorType & p) const { using std::abs; return abs(signedDistance (p)); } 00149 00150 /** \returns the projection of a point \a p onto the plane \c *this. 00151 */ 00152 inline VectorType projection (const VectorType & p) const { return p - signedDistance (p) * normal (); } 00153 00154 /** \returns a constant reference to the unit normal vector of the plane, which corresponds 00155 * to the linear part of the implicit equation. 00156 */ 00157 inline ConstNormalReturnType normal () const { return ConstNormalReturnType(m_coeffs,0,0,dim (),1); } 00158 00159 /** \returns a non-constant reference to the unit normal vector of the plane, which corresponds 00160 * to the linear part of the implicit equation. 00161 */ 00162 inline NormalReturnType normal () { return NormalReturnType(m_coeffs,0,0,dim (),1); } 00163 00164 /** \returns the distance to the origin, which is also the "constant term" of the implicit equation 00165 * \warning the vector normal is assumed to be normalized. 00166 */ 00167 inline const Scalar& offset () const { return m_coeffs.coeff(dim ()); } 00168 00169 /** \returns a non-constant reference to the distance to the origin, which is also the constant part 00170 * of the implicit equation */ 00171 inline Scalar& offset () { return m_coeffs(dim ()); } 00172 00173 /** \returns a constant reference to the coefficients c_i of the plane equation: 00174 * \f$ c_0*x_0 + ... + c_{d-1}*x_{d-1} + c_d = 0 \f$ 00175 */ 00176 inline const Coefficients & coeffs () const { return m_coeffs; } 00177 00178 /** \returns a non-constant reference to the coefficients c_i of the plane equation: 00179 * \f$ c_0*x_0 + ... + c_{d-1}*x_{d-1} + c_d = 0 \f$ 00180 */ 00181 inline Coefficients & coeffs () { return m_coeffs; } 00182 00183 /** \returns the intersection of *this with \a other. 00184 * 00185 * \warning The ambient space must be a plane, i.e. have dimension 2, so that \c *this and \a other are lines. 00186 * 00187 * \note If \a other is approximately parallel to *this, this method will return any point on *this. 00188 */ 00189 VectorType intersection (const Hyperplane & other) const 00190 { 00191 using std::abs; 00192 EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(VectorType , 2) 00193 Scalar det = coeffs ().coeff(0) * other.coeffs ().coeff(1) - coeffs ().coeff(1) * other.coeffs ().coeff(0); 00194 // since the line equations ax+by=c are normalized with a^2+b^2=1, the following tests 00195 // whether the two lines are approximately parallel. 00196 if(internal::isMuchSmallerThan(det, Scalar(1))) 00197 { // special case where the two lines are approximately parallel. Pick any point on the first line. 00198 if(abs(coeffs ().coeff(1))>abs(coeffs ().coeff(0))) 00199 return VectorType (coeffs ().coeff(1), -coeffs ().coeff(2)/coeffs ().coeff(1)-coeffs ().coeff(0)); 00200 else 00201 return VectorType (-coeffs ().coeff(2)/coeffs ().coeff(0)-coeffs ().coeff(1), coeffs ().coeff(0)); 00202 } 00203 else 00204 { // general case 00205 Scalar invdet = Scalar(1) / det; 00206 return VectorType (invdet*(coeffs ().coeff(1)*other.coeffs ().coeff(2)-other.coeffs ().coeff(1)*coeffs ().coeff(2)), 00207 invdet*(other.coeffs ().coeff(0)*coeffs ().coeff(2)-coeffs ().coeff(0)*other.coeffs ().coeff(2))); 00208 } 00209 } 00210 00211 /** Applies the transformation matrix \a mat to \c *this and returns a reference to \c *this. 00212 * 00213 * \param mat the Dim x Dim transformation matrix 00214 * \param traits specifies whether the matrix \a mat represents an #Isometry 00215 * or a more generic #Affine transformation. The default is #Affine. 00216 */ 00217 template<typename XprType> 00218 inline Hyperplane & transform(const MatrixBase<XprType>& mat, TransformTraits traits = Affine) 00219 { 00220 if (traits==Affine) 00221 normal () = mat.inverse ().transpose() * normal (); 00222 else if (traits==Isometry) 00223 normal () = mat * normal (); 00224 else 00225 { 00226 eigen_assert(0 && "invalid traits value in Hyperplane::transform()"); 00227 } 00228 return *this; 00229 } 00230 00231 /** Applies the transformation \a t to \c *this and returns a reference to \c *this. 00232 * 00233 * \param t the transformation of dimension Dim 00234 * \param traits specifies whether the transformation \a t represents an #Isometry 00235 * or a more generic #Affine transformation. The default is #Affine. 00236 * Other kind of transformations are not supported. 00237 */ 00238 template<int TrOptions> 00239 inline Hyperplane & transform(const Transform<Scalar,AmbientDimAtCompileTime,Affine,TrOptions> & t, 00240 TransformTraits traits = Affine) 00241 { 00242 transform(t.linear (), traits); 00243 offset () -= normal ().dot(t.translation ()); 00244 return *this; 00245 } 00246 00247 /** \returns \c *this with scalar type casted to \a NewScalarType 00248 * 00249 * Note that if \a NewScalarType is equal to the current scalar type of \c *this 00250 * then this function smartly returns a const reference to \c *this. 00251 */ 00252 template<typename NewScalarType> 00253 inline typename internal::cast_return_type<Hyperplane, 00254 Hyperplane<NewScalarType,AmbientDimAtCompileTime,Options> >::type cast () const 00255 { 00256 return typename internal::cast_return_type<Hyperplane, 00257 Hyperplane<NewScalarType,AmbientDimAtCompileTime,Options> >::type(*this); 00258 } 00259 00260 /** Copy constructor with scalar type conversion */ 00261 template<typename OtherScalarType,int OtherOptions> 00262 inline explicit Hyperplane(const Hyperplane<OtherScalarType,AmbientDimAtCompileTime,OtherOptions> & other) 00263 { m_coeffs = other.coeffs ().template cast<Scalar>(); } 00264 00265 /** \returns \c true if \c *this is approximately equal to \a other, within the precision 00266 * determined by \a prec. 00267 * 00268 * \sa MatrixBase::isApprox() */ 00269 template<int OtherOptions> 00270 bool isApprox (const Hyperplane<Scalar,AmbientDimAtCompileTime,OtherOptions> & other, const typename NumTraits<Scalar>::Real& prec = NumTraits<Scalar>::dummy_precision()) const 00271 { return m_coeffs.isApprox(other.m_coeffs, prec); } 00272 00273 protected: 00274 00275 Coefficients m_coeffs; 00276 }; 00277 00278 } // end namespace Eigen 00279 00280 #endif // EIGEN_HYPERPLANE_H
Generated on Tue Jul 12 2022 17:46:55 by 1.7.2