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. Eigen itself is part of the KDE project.
00003 //
00004 // Copyright (C) 2008 Gael Guennebaud <g.gael@free.fr>
00005 // Copyright (C) 2008 Benoit Jacob <jacob.benoit.1@gmail.com>
00006 //
00007 // Eigen is free software; you can redistribute it and/or
00008 // modify it under the terms of the GNU Lesser General Public
00009 // License as published by the Free Software Foundation; either
00010 // version 3 of the License, or (at your option) any later version.
00011 //
00012 // Alternatively, you can redistribute it and/or
00013 // modify it under the terms of the GNU General Public License as
00014 // published by the Free Software Foundation; either version 2 of
00015 // the License, or (at your option) any later version.
00016 //
00017 // Eigen is distributed in the hope that it will be useful, but WITHOUT ANY
00018 // WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
00019 // FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License or the
00020 // GNU General Public License for more details.
00021 //
00022 // You should have received a copy of the GNU Lesser General Public
00023 // License and a copy of the GNU General Public License along with
00024 // Eigen. If not, see <http://www.gnu.org/licenses/>.
00025 
00026 // no include guard, we'll include this twice from All.h from Eigen2Support, and it's internal anyway
00027 
00045 template <typename _Scalar, int _AmbientDim>
00046 class Hyperplane
00047 {
00048 public:
00049   EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF_VECTORIZABLE_FIXED_SIZE(_Scalar,_AmbientDim==Dynamic ? Dynamic : _AmbientDim+1)
00050   enum { AmbientDimAtCompileTime = _AmbientDim };
00051   typedef _Scalar Scalar;
00052   typedef typename NumTraits<Scalar>::Real RealScalar;
00053   typedef Matrix<Scalar,AmbientDimAtCompileTime,1> VectorType;
00054   typedef Matrix<Scalar,int(AmbientDimAtCompileTime)==Dynamic
00055                         ? Dynamic
00056                         : int(AmbientDimAtCompileTime)+1,1> Coefficients;
00057   typedef Block<Coefficients,AmbientDimAtCompileTime,1> NormalReturnType;
00058 
00060   inline explicit Hyperplane() {}
00061 
00064   inline explicit Hyperplane(int _dim) : m_coeffs(_dim+1) {}
00065 
00069   inline Hyperplane(const VectorType& n, const VectorType& e)
00070     : m_coeffs(n.size()+1)
00071   {
00072     normal() = n;
00073     offset() = -e.eigen2_dot(n);
00074   }
00075 
00080   inline Hyperplane(const VectorType& n, Scalar d)
00081     : m_coeffs(n.size()+1)
00082   {
00083     normal() = n;
00084     offset() = d;
00085   }
00086 
00090   static inline Hyperplane Through(const VectorType& p0, const VectorType& p1)
00091   {
00092     Hyperplane result(p0.size());
00093     result.normal() = (p1 - p0).unitOrthogonal();
00094     result.offset() = -result.normal().eigen2_dot(p0);
00095     return result;
00096   }
00097 
00101   static inline Hyperplane Through(const VectorType& p0, const VectorType& p1, const VectorType& p2)
00102   {
00103     EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(VectorType, 3)
00104     Hyperplane result(p0.size());
00105     result.normal() = (p2 - p0).cross(p1 - p0).normalized();
00106     result.offset() = -result.normal().eigen2_dot(p0);
00107     return result;
00108   }
00109 
00114   // FIXME to be consitent with the rest this could be implemented as a static Through function ??
00115   explicit Hyperplane(const ParametrizedLine<Scalar, AmbientDimAtCompileTime>& parametrized)
00116   {
00117     normal() = parametrized.direction().unitOrthogonal();
00118     offset() = -normal().eigen2_dot(parametrized.origin());
00119   }
00120 
00121   ~Hyperplane() {}
00122 
00124   inline int dim() const { return int(AmbientDimAtCompileTime)==Dynamic ? m_coeffs.size()-1 : int(AmbientDimAtCompileTime); }
00125 
00127   void normalize(void)
00128   {
00129     m_coeffs /= normal().norm();
00130   }
00131 
00135   inline Scalar signedDistance(const VectorType& p) const { return p.eigen2_dot(normal()) + offset(); }
00136 
00140   inline Scalar absDistance(const VectorType& p) const { return ei_abs(signedDistance(p)); }
00141 
00144   inline VectorType projection(const VectorType& p) const { return p - signedDistance(p) * normal(); }
00145 
00149   inline const NormalReturnType normal() const { return NormalReturnType(*const_cast<Coefficients*>(&m_coeffs),0,0,dim(),1); }
00150 
00154   inline NormalReturnType normal() { return NormalReturnType(m_coeffs,0,0,dim(),1); }
00155 
00159   inline const Scalar& offset() const { return m_coeffs.coeff(dim()); }
00160 
00163   inline Scalar& offset() { return m_coeffs(dim()); }
00164 
00168   inline const Coefficients& coeffs() const { return m_coeffs; }
00169 
00173   inline Coefficients& coeffs() { return m_coeffs; }
00174 
00181   VectorType intersection(const Hyperplane& other)
00182   {
00183     EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(VectorType, 2)
00184     Scalar det = coeffs().coeff(0) * other.coeffs().coeff(1) - coeffs().coeff(1) * other.coeffs().coeff(0);
00185     // since the line equations ax+by=c are normalized with a^2+b^2=1, the following tests
00186     // whether the two lines are approximately parallel.
00187     if(ei_isMuchSmallerThan(det, Scalar(1)))
00188     {   // special case where the two lines are approximately parallel. Pick any point on the first line.
00189         if(ei_abs(coeffs().coeff(1))>ei_abs(coeffs().coeff(0)))
00190             return VectorType(coeffs().coeff(1), -coeffs().coeff(2)/coeffs().coeff(1)-coeffs().coeff(0));
00191         else
00192             return VectorType(-coeffs().coeff(2)/coeffs().coeff(0)-coeffs().coeff(1), coeffs().coeff(0));
00193     }
00194     else
00195     {   // general case
00196         Scalar invdet = Scalar(1) / det;
00197         return VectorType(invdet*(coeffs().coeff(1)*other.coeffs().coeff(2)-other.coeffs().coeff(1)*coeffs().coeff(2)),
00198                           invdet*(other.coeffs().coeff(0)*coeffs().coeff(2)-coeffs().coeff(0)*other.coeffs().coeff(2)));
00199     }
00200   }
00201 
00208   template<typename XprType>
00209   inline Hyperplane& transform(const MatrixBase<XprType>& mat, TransformTraits traits = Affine)
00210   {
00211     if (traits==Affine)
00212       normal() = mat.inverse().transpose() * normal();
00213     else if (traits==Isometry)
00214       normal() = mat * normal();
00215     else
00216     {
00217       ei_assert("invalid traits value in Hyperplane::transform()");
00218     }
00219     return *this;
00220   }
00221 
00229   inline Hyperplane& transform(const Transform<Scalar,AmbientDimAtCompileTime>& t,
00230                                 TransformTraits traits = Affine)
00231   {
00232     transform(t.linear(), traits);
00233     offset() -= t.translation().eigen2_dot(normal());
00234     return *this;
00235   }
00236 
00242   template<typename NewScalarType>
00243   inline typename internal::cast_return_type<Hyperplane,
00244            Hyperplane<NewScalarType,AmbientDimAtCompileTime> >::type cast() const
00245   {
00246     return typename internal::cast_return_type<Hyperplane,
00247                     Hyperplane<NewScalarType,AmbientDimAtCompileTime> >::type(*this);
00248   }
00249 
00251   template<typename OtherScalarType>
00252   inline explicit Hyperplane(const Hyperplane<OtherScalarType,AmbientDimAtCompileTime>& other)
00253   { m_coeffs = other.coeffs().template cast<Scalar>(); }
00254 
00259   bool isApprox(const Hyperplane& other, typename NumTraits<Scalar>::Real prec = precision<Scalar>()) const
00260   { return m_coeffs.isApprox(other.m_coeffs, prec); }
00261 
00262 protected:
00263 
00264   Coefficients m_coeffs;
00265 };


libicr
Author(s): Robert Krug
autogenerated on Mon Jan 6 2014 11:32:48