Hyperplane.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 
00032 template <typename _Scalar, int _AmbientDim>
00033 class Hyperplane
00034 {
00035 public:
00036   EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF_VECTORIZABLE_FIXED_SIZE(_Scalar,_AmbientDim==Dynamic ? Dynamic : _AmbientDim+1)
00037   enum { AmbientDimAtCompileTime = _AmbientDim };
00038   typedef _Scalar Scalar;
00039   typedef typename NumTraits<Scalar>::Real RealScalar;
00040   typedef Matrix<Scalar,AmbientDimAtCompileTime,1> VectorType;
00041   typedef Matrix<Scalar,int(AmbientDimAtCompileTime)==Dynamic
00042                         ? Dynamic
00043                         : int(AmbientDimAtCompileTime)+1,1> Coefficients;
00044   typedef Block<Coefficients,AmbientDimAtCompileTime,1> NormalReturnType;
00045 
00047   inline Hyperplane() {}
00048 
00051   inline explicit Hyperplane(int _dim) : m_coeffs(_dim+1) {}
00052 
00056   inline Hyperplane(const VectorType& n, const VectorType& e)
00057     : m_coeffs(n.size()+1)
00058   {
00059     normal() = n;
00060     offset() = -e.eigen2_dot(n);
00061   }
00062 
00067   inline Hyperplane(const VectorType& n, Scalar d)
00068     : m_coeffs(n.size()+1)
00069   {
00070     normal() = n;
00071     offset() = d;
00072   }
00073 
00077   static inline Hyperplane Through(const VectorType& p0, const VectorType& p1)
00078   {
00079     Hyperplane result(p0.size());
00080     result.normal() = (p1 - p0).unitOrthogonal();
00081     result.offset() = -result.normal().eigen2_dot(p0);
00082     return result;
00083   }
00084 
00088   static inline Hyperplane Through(const VectorType& p0, const VectorType& p1, const VectorType& p2)
00089   {
00090     EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(VectorType, 3)
00091     Hyperplane result(p0.size());
00092     result.normal() = (p2 - p0).cross(p1 - p0).normalized();
00093     result.offset() = -result.normal().eigen2_dot(p0);
00094     return result;
00095   }
00096 
00101   // FIXME to be consitent with the rest this could be implemented as a static Through function ??
00102   explicit Hyperplane(const ParametrizedLine<Scalar, AmbientDimAtCompileTime>& parametrized)
00103   {
00104     normal() = parametrized.direction().unitOrthogonal();
00105     offset() = -normal().eigen2_dot(parametrized.origin());
00106   }
00107 
00108   ~Hyperplane() {}
00109 
00111   inline int dim() const { return int(AmbientDimAtCompileTime)==Dynamic ? m_coeffs.size()-1 : int(AmbientDimAtCompileTime); }
00112 
00114   void normalize(void)
00115   {
00116     m_coeffs /= normal().norm();
00117   }
00118 
00122   inline Scalar signedDistance(const VectorType& p) const { return p.eigen2_dot(normal()) + offset(); }
00123 
00127   inline Scalar absDistance(const VectorType& p) const { return ei_abs(signedDistance(p)); }
00128 
00131   inline VectorType projection(const VectorType& p) const { return p - signedDistance(p) * normal(); }
00132 
00136   inline const NormalReturnType normal() const { return NormalReturnType(*const_cast<Coefficients*>(&m_coeffs),0,0,dim(),1); }
00137 
00141   inline NormalReturnType normal() { return NormalReturnType(m_coeffs,0,0,dim(),1); }
00142 
00146   inline const Scalar& offset() const { return m_coeffs.coeff(dim()); }
00147 
00150   inline Scalar& offset() { return m_coeffs(dim()); }
00151 
00155   inline const Coefficients& coeffs() const { return m_coeffs; }
00156 
00160   inline Coefficients& coeffs() { return m_coeffs; }
00161 
00168   VectorType intersection(const Hyperplane& other)
00169   {
00170     EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(VectorType, 2)
00171     Scalar det = coeffs().coeff(0) * other.coeffs().coeff(1) - coeffs().coeff(1) * other.coeffs().coeff(0);
00172     // since the line equations ax+by=c are normalized with a^2+b^2=1, the following tests
00173     // whether the two lines are approximately parallel.
00174     if(ei_isMuchSmallerThan(det, Scalar(1)))
00175     {   // special case where the two lines are approximately parallel. Pick any point on the first line.
00176         if(ei_abs(coeffs().coeff(1))>ei_abs(coeffs().coeff(0)))
00177             return VectorType(coeffs().coeff(1), -coeffs().coeff(2)/coeffs().coeff(1)-coeffs().coeff(0));
00178         else
00179             return VectorType(-coeffs().coeff(2)/coeffs().coeff(0)-coeffs().coeff(1), coeffs().coeff(0));
00180     }
00181     else
00182     {   // general case
00183         Scalar invdet = Scalar(1) / det;
00184         return VectorType(invdet*(coeffs().coeff(1)*other.coeffs().coeff(2)-other.coeffs().coeff(1)*coeffs().coeff(2)),
00185                           invdet*(other.coeffs().coeff(0)*coeffs().coeff(2)-coeffs().coeff(0)*other.coeffs().coeff(2)));
00186     }
00187   }
00188 
00195   template<typename XprType>
00196   inline Hyperplane& transform(const MatrixBase<XprType>& mat, TransformTraits traits = Affine)
00197   {
00198     if (traits==Affine)
00199       normal() = mat.inverse().transpose() * normal();
00200     else if (traits==Isometry)
00201       normal() = mat * normal();
00202     else
00203     {
00204       ei_assert("invalid traits value in Hyperplane::transform()");
00205     }
00206     return *this;
00207   }
00208 
00216   inline Hyperplane& transform(const Transform<Scalar,AmbientDimAtCompileTime>& t,
00217                                 TransformTraits traits = Affine)
00218   {
00219     transform(t.linear(), traits);
00220     offset() -= t.translation().eigen2_dot(normal());
00221     return *this;
00222   }
00223 
00229   template<typename NewScalarType>
00230   inline typename internal::cast_return_type<Hyperplane,
00231            Hyperplane<NewScalarType,AmbientDimAtCompileTime> >::type cast() const
00232   {
00233     return typename internal::cast_return_type<Hyperplane,
00234                     Hyperplane<NewScalarType,AmbientDimAtCompileTime> >::type(*this);
00235   }
00236 
00238   template<typename OtherScalarType>
00239   inline explicit Hyperplane(const Hyperplane<OtherScalarType,AmbientDimAtCompileTime>& other)
00240   { m_coeffs = other.coeffs().template cast<Scalar>(); }
00241 
00246   bool isApprox(const Hyperplane& other, typename NumTraits<Scalar>::Real prec = precision<Scalar>()) const
00247   { return m_coeffs.isApprox(other.m_coeffs, prec); }
00248 
00249 protected:
00250 
00251   Coefficients m_coeffs;
00252 };
00253 
00254 } // end namespace Eigen


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