![]() |
Eigen
3.3.3
|
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 Eigen::Index 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 EIGEN_DEVICE_FUNC inline Hyperplane() {} 00054 00055 template<int OtherOptions> 00056 EIGEN_DEVICE_FUNC Hyperplane(const Hyperplane<Scalar,AmbientDimAtCompileTime,OtherOptions>& other) 00057 : m_coeffs(other.coeffs()) 00058 {} 00059 00062 EIGEN_DEVICE_FUNC inline explicit Hyperplane(Index _dim) : m_coeffs(_dim+1) {} 00063 00067 EIGEN_DEVICE_FUNC 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 EIGEN_DEVICE_FUNC inline Hyperplane(const VectorType& n, const Scalar& d) 00079 : m_coeffs(n.size()+1) 00080 { 00081 normal() = n; 00082 offset() = d; 00083 } 00084 00088 EIGEN_DEVICE_FUNC 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 EIGEN_DEVICE_FUNC 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 EIGEN_DEVICE_FUNC explicit Hyperplane(const ParametrizedLine<Scalar, AmbientDimAtCompileTime>& parametrized) 00124 { 00125 normal() = parametrized.direction().unitOrthogonal(); 00126 offset() = -parametrized.origin().dot(normal()); 00127 } 00128 00129 EIGEN_DEVICE_FUNC ~Hyperplane() {} 00130 00132 EIGEN_DEVICE_FUNC inline Index dim() const { return AmbientDimAtCompileTime==Dynamic ? m_coeffs.size()-1 : Index(AmbientDimAtCompileTime); } 00133 00135 EIGEN_DEVICE_FUNC void normalize(void) 00136 { 00137 m_coeffs /= normal().norm(); 00138 } 00139 00143 EIGEN_DEVICE_FUNC inline Scalar signedDistance(const VectorType& p) const { return normal().dot(p) + offset(); } 00144 00148 EIGEN_DEVICE_FUNC inline Scalar absDistance(const VectorType& p) const { return numext::abs(signedDistance(p)); } 00149 00152 EIGEN_DEVICE_FUNC inline VectorType projection(const VectorType& p) const { return p - signedDistance(p) * normal(); } 00153 00157 EIGEN_DEVICE_FUNC inline ConstNormalReturnType normal() const { return ConstNormalReturnType(m_coeffs,0,0,dim(),1); } 00158 00162 EIGEN_DEVICE_FUNC inline NormalReturnType normal() { return NormalReturnType(m_coeffs,0,0,dim(),1); } 00163 00167 EIGEN_DEVICE_FUNC inline const Scalar& offset() const { return m_coeffs.coeff(dim()); } 00168 00171 EIGEN_DEVICE_FUNC inline Scalar& offset() { return m_coeffs(dim()); } 00172 00176 EIGEN_DEVICE_FUNC inline const Coefficients& coeffs() const { return m_coeffs; } 00177 00181 EIGEN_DEVICE_FUNC inline Coefficients& coeffs() { return m_coeffs; } 00182 00189 EIGEN_DEVICE_FUNC VectorType intersection(const Hyperplane& other) const 00190 { 00191 EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(VectorType, 2) 00192 Scalar det = coeffs().coeff(0) * other.coeffs().coeff(1) - coeffs().coeff(1) * other.coeffs().coeff(0); 00193 // since the line equations ax+by=c are normalized with a^2+b^2=1, the following tests 00194 // whether the two lines are approximately parallel. 00195 if(internal::isMuchSmallerThan(det, Scalar(1))) 00196 { // special case where the two lines are approximately parallel. Pick any point on the first line. 00197 if(numext::abs(coeffs().coeff(1))>numext::abs(coeffs().coeff(0))) 00198 return VectorType(coeffs().coeff(1), -coeffs().coeff(2)/coeffs().coeff(1)-coeffs().coeff(0)); 00199 else 00200 return VectorType(-coeffs().coeff(2)/coeffs().coeff(0)-coeffs().coeff(1), coeffs().coeff(0)); 00201 } 00202 else 00203 { // general case 00204 Scalar invdet = Scalar(1) / det; 00205 return VectorType(invdet*(coeffs().coeff(1)*other.coeffs().coeff(2)-other.coeffs().coeff(1)*coeffs().coeff(2)), 00206 invdet*(other.coeffs().coeff(0)*coeffs().coeff(2)-coeffs().coeff(0)*other.coeffs().coeff(2))); 00207 } 00208 } 00209 00216 template<typename XprType> 00217 EIGEN_DEVICE_FUNC inline Hyperplane& transform(const MatrixBase<XprType>& mat, TransformTraits traits = Affine) 00218 { 00219 if (traits==Affine) 00220 { 00221 normal() = mat.inverse().transpose() * normal(); 00222 m_coeffs /= normal().norm(); 00223 } 00224 else if (traits==Isometry) 00225 normal() = mat * normal(); 00226 else 00227 { 00228 eigen_assert(0 && "invalid traits value in Hyperplane::transform()"); 00229 } 00230 return *this; 00231 } 00232 00240 template<int TrOptions> 00241 EIGEN_DEVICE_FUNC inline Hyperplane& transform(const Transform<Scalar,AmbientDimAtCompileTime,Affine,TrOptions>& t, 00242 TransformTraits traits = Affine) 00243 { 00244 transform(t.linear(), traits); 00245 offset() -= normal().dot(t.translation()); 00246 return *this; 00247 } 00248 00254 template<typename NewScalarType> 00255 EIGEN_DEVICE_FUNC inline typename internal::cast_return_type<Hyperplane, 00256 Hyperplane<NewScalarType,AmbientDimAtCompileTime,Options> >::type cast() const 00257 { 00258 return typename internal::cast_return_type<Hyperplane, 00259 Hyperplane<NewScalarType,AmbientDimAtCompileTime,Options> >::type(*this); 00260 } 00261 00263 template<typename OtherScalarType,int OtherOptions> 00264 EIGEN_DEVICE_FUNC inline explicit Hyperplane(const Hyperplane<OtherScalarType,AmbientDimAtCompileTime,OtherOptions>& other) 00265 { m_coeffs = other.coeffs().template cast<Scalar>(); } 00266 00271 template<int OtherOptions> 00272 EIGEN_DEVICE_FUNC bool isApprox(const Hyperplane<Scalar,AmbientDimAtCompileTime,OtherOptions>& other, const typename NumTraits<Scalar>::Real& prec = NumTraits<Scalar>::dummy_precision()) const 00273 { return m_coeffs.isApprox(other.m_coeffs, prec); } 00274 00275 protected: 00276 00277 Coefficients m_coeffs; 00278 }; 00279 00280 } // end namespace Eigen 00281 00282 #endif // EIGEN_HYPERPLANE_H