$treeview $search $mathjax
Eigen
3.2.5
$projectbrief
|
$projectbrief
|
$searchbox |
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 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 00053 inline Hyperplane() {} 00054 00055 template<int OtherOptions> 00056 Hyperplane(const Hyperplane<Scalar,AmbientDimAtCompileTime,OtherOptions>& other) 00057 : m_coeffs(other.coeffs()) 00058 {} 00059 00062 inline explicit Hyperplane(Index _dim) : m_coeffs(_dim+1) {} 00063 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 00078 inline Hyperplane(const VectorType& n, const Scalar& d) 00079 : m_coeffs(n.size()+1) 00080 { 00081 normal() = n; 00082 offset() = d; 00083 } 00084 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 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 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 00132 inline Index dim() const { return AmbientDimAtCompileTime==Dynamic ? m_coeffs.size()-1 : Index(AmbientDimAtCompileTime); } 00133 00135 void normalize(void) 00136 { 00137 m_coeffs /= normal().norm(); 00138 } 00139 00143 inline Scalar signedDistance(const VectorType& p) const { return normal().dot(p) + offset(); } 00144 00148 inline Scalar absDistance(const VectorType& p) const { using std::abs; return abs(signedDistance(p)); } 00149 00152 inline VectorType projection(const VectorType& p) const { return p - signedDistance(p) * normal(); } 00153 00157 inline ConstNormalReturnType normal() const { return ConstNormalReturnType(m_coeffs,0,0,dim(),1); } 00158 00162 inline NormalReturnType normal() { return NormalReturnType(m_coeffs,0,0,dim(),1); } 00163 00167 inline const Scalar& offset() const { return m_coeffs.coeff(dim()); } 00168 00171 inline Scalar& offset() { return m_coeffs(dim()); } 00172 00176 inline const Coefficients& coeffs() const { return m_coeffs; } 00177 00181 inline Coefficients& coeffs() { return m_coeffs; } 00182 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 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 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 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 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 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