src/Geometry/Hyperplane.h

0001 // This file is part of Eigen, a lightweight C++ template library
0002 // for linear algebra.
0003 //
0004 // Copyright (C) 2008 Gael Guennebaud <gael.guennebaud@inria.fr>
0005 // Copyright (C) 2008 Benoit Jacob <jacob.benoit.1@gmail.com>
0006 //
0007 // This Source Code Form is subject to the terms of the Mozilla
0008 // Public License v. 2.0. If a copy of the MPL was not distributed
0009 // with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
0010
0011 #ifndef EIGEN_HYPERPLANE_H
0012 #define EIGEN_HYPERPLANE_H
0013
0014 namespace Eigen {
0015
0016 /** \geometry_module \ingroup Geometry_Module
0017   *
0018   * \class Hyperplane
0019   *
0020   * \brief A hyperplane
0021   *
0022   * A hyperplane is an affine subspace of dimension n-1 in a space of dimension n.
0023   * For example, a hyperplane in a plane is a line; a hyperplane in 3-space is a plane.
0024   *
0025   * \tparam _Scalar the scalar type, i.e., the type of the coefficients
0026   * \tparam _AmbientDim the dimension of the ambient space, can be a compile time value or Dynamic.
0027   *             Notice that the dimension of the hyperplane is _AmbientDim-1.
0028   *
0029   * This class represents an hyperplane as the zero set of the implicit equation
0030   * \f$ n \cdot x + d = 0 \f$ where \f$ n \f$ is a unit normal vector of the plane (linear part)
0031   * and \f$ d \f$ is the distance (offset) to the origin.
0032   */
0033 template <typename _Scalar, int _AmbientDim, int _Options>
0034 class Hyperplane
0035 {
0036 public:
0037   EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF_VECTORIZABLE_FIXED_SIZE(_Scalar,_AmbientDim==Dynamic ? Dynamic : _AmbientDim+1)
0038   enum {
0039     AmbientDimAtCompileTime = _AmbientDim,
0040     Options = _Options
0041   };
0042   typedef _Scalar Scalar;
0043   typedef typename NumTraits<Scalar>::Real RealScalar;
0044   typedef Eigen::Index Index; ///< \deprecated since Eigen 3.3
0045   typedef Matrix<Scalar,AmbientDimAtCompileTime,1> VectorType;
0046   typedef Matrix<Scalar,Index(AmbientDimAtCompileTime)==Dynamic
0047                         ? Dynamic
0048                         : Index(AmbientDimAtCompileTime)+1,1,Options> Coefficients;
0049   typedef Block<Coefficients,AmbientDimAtCompileTime,1> NormalReturnType;
0050   typedef const Block<const Coefficients,AmbientDimAtCompileTime,1> ConstNormalReturnType;
0051
0052   /** Default constructor without initialization */
0053   EIGEN_DEVICE_FUNC inline Hyperplane() {}
0054
0055   template<int OtherOptions>
0056   EIGEN_DEVICE_FUNC Hyperplane(const Hyperplane<Scalar,AmbientDimAtCompileTime,OtherOptions>& other)
0057    : m_coeffs(other.coeffs())
0058   {}
0059
0060   /** Constructs a dynamic-size hyperplane with \a _dim the dimension
0061     * of the ambient space */
0062   EIGEN_DEVICE_FUNC inline explicit Hyperplane(Index _dim) : m_coeffs(_dim+1) {}
0063
0064   /** Construct a plane from its normal \a n and a point \a e onto the plane.
0065     * \warning the vector normal is assumed to be normalized.
0066     */
0067   EIGEN_DEVICE_FUNC inline Hyperplane(const VectorType& n, const VectorType& e)
0068     : m_coeffs(n.size()+1)
0069   {
0070     normal() = n;
0071     offset() = -n.dot(e);
0072   }
0073
0074   /** Constructs a plane from its normal \a n and distance to the origin \a d
0075     * such that the algebraic equation of the plane is \f$ n \cdot x + d = 0 \f$.
0076     * \warning the vector normal is assumed to be normalized.
0077     */
0078   EIGEN_DEVICE_FUNC inline Hyperplane(const VectorType& n, const Scalar& d)
0079     : m_coeffs(n.size()+1)
0080   {
0081     normal() = n;
0082     offset() = d;
0083   }
0084
0085   /** Constructs a hyperplane passing through the two points. If the dimension of the ambient space
0086     * is greater than 2, then there isn't uniqueness, so an arbitrary choice is made.
0087     */
0088   EIGEN_DEVICE_FUNC static inline Hyperplane Through(const VectorType& p0, const VectorType& p1)
0089   {
0090     Hyperplane result(p0.size());
0091     result.normal() = (p1 - p0).unitOrthogonal();
0092     result.offset() = -p0.dot(result.normal());
0093     return result;
0094   }
0095
0096   /** Constructs a hyperplane passing through the three points. The dimension of the ambient space
0097     * is required to be exactly 3.
0098     */
0099   EIGEN_DEVICE_FUNC static inline Hyperplane Through(const VectorType& p0, const VectorType& p1, const VectorType& p2)
0100   {
0101     EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(VectorType, 3)
0102     Hyperplane result(p0.size());
0103     VectorType v0(p2 - p0), v1(p1 - p0);
0104     result.normal() = v0.cross(v1);
0105     RealScalar norm = result.normal().norm();
0106     if(norm <= v0.norm() * v1.norm() * NumTraits<RealScalar>::epsilon())
0107     {
0108       Matrix<Scalar,2,3> m; m << v0.transpose(), v1.transpose();
0109       JacobiSVD<Matrix<Scalar,2,3> > svd(m, ComputeFullV);
0110       result.normal() = svd.matrixV().col(2);
0111     }
0112     else
0113       result.normal() /= norm;
0114     result.offset() = -p0.dot(result.normal());
0115     return result;
0116   }
0117
0118   /** Constructs a hyperplane passing through the parametrized line \a parametrized.
0119     * If the dimension of the ambient space is greater than 2, then there isn't uniqueness,
0120     * so an arbitrary choice is made.
0121     */
0122   // FIXME to be consistent with the rest this could be implemented as a static Through function ??
0123   EIGEN_DEVICE_FUNC explicit Hyperplane(const ParametrizedLine<Scalar, AmbientDimAtCompileTime>& parametrized)
0124   {
0125     normal() = parametrized.direction().unitOrthogonal();
0126     offset() = -parametrized.origin().dot(normal());
0127   }
0128
0129   EIGEN_DEVICE_FUNC ~Hyperplane() {}
0130
0131   /** \returns the dimension in which the plane holds */
0132   EIGEN_DEVICE_FUNC inline Index dim() const { return AmbientDimAtCompileTime==Dynamic ? m_coeffs.size()-1 : Index(AmbientDimAtCompileTime); }
0133
0134   /** normalizes \c *this */
0135   EIGEN_DEVICE_FUNC void normalize(void)
0136   {
0137     m_coeffs /= normal().norm();
0138   }
0139
0140   /** \returns the signed distance between the plane \c *this and a point \a p.
0141     * \sa absDistance()
0142     */
0143   EIGEN_DEVICE_FUNC inline Scalar signedDistance(const VectorType& p) const { return normal().dot(p) + offset(); }
0144
0145   /** \returns the absolute distance between the plane \c *this and a point \a p.
0146     * \sa signedDistance()
0147     */
0148   EIGEN_DEVICE_FUNC inline Scalar absDistance(const VectorType& p) const { return numext::abs(signedDistance(p)); }
0149
0150   /** \returns the projection of a point \a p onto the plane \c *this.
0151     */
0152   EIGEN_DEVICE_FUNC inline VectorType projection(const VectorType& p) const { return p - signedDistance(p) * normal(); }
0153
0154   /** \returns a constant reference to the unit normal vector of the plane, which corresponds
0155     * to the linear part of the implicit equation.
0156     */
0157   EIGEN_DEVICE_FUNC inline ConstNormalReturnType normal() const { return ConstNormalReturnType(m_coeffs,0,0,dim(),1); }
0158
0159   /** \returns a non-constant reference to the unit normal vector of the plane, which corresponds
0160     * to the linear part of the implicit equation.
0161     */
0162   EIGEN_DEVICE_FUNC inline NormalReturnType normal() { return NormalReturnType(m_coeffs,0,0,dim(),1); }
0163
0164   /** \returns the distance to the origin, which is also the "constant term" of the implicit equation
0165     * \warning the vector normal is assumed to be normalized.
0166     */
0167   EIGEN_DEVICE_FUNC inline const Scalar& offset() const { return m_coeffs.coeff(dim()); }
0168
0169   /** \returns a non-constant reference to the distance to the origin, which is also the constant part
0170     * of the implicit equation */
0171   EIGEN_DEVICE_FUNC inline Scalar& offset() { return m_coeffs(dim()); }
0172
0173   /** \returns a constant reference to the coefficients c_i of the plane equation:
0174     * \f$ c_0*x_0 + ... + c_{d-1}*x_{d-1} + c_d = 0 \f$
0175     */
0176   EIGEN_DEVICE_FUNC inline const Coefficients& coeffs() const { return m_coeffs; }
0177
0178   /** \returns a non-constant reference to the coefficients c_i of the plane equation:
0179     * \f$ c_0*x_0 + ... + c_{d-1}*x_{d-1} + c_d = 0 \f$
0180     */
0181   EIGEN_DEVICE_FUNC inline Coefficients& coeffs() { return m_coeffs; }
0182
0183   /** \returns the intersection of *this with \a other.
0184     *
0185     * \warning The ambient space must be a plane, i.e. have dimension 2, so that \c *this and \a other are lines.
0186     *
0187     * \note If \a other is approximately parallel to *this, this method will return any point on *this.
0188     */
0189   EIGEN_DEVICE_FUNC VectorType intersection(const Hyperplane& other) const
0190   {
0191     EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(VectorType, 2)
0192     Scalar det = coeffs().coeff(0) * other.coeffs().coeff(1) - coeffs().coeff(1) * other.coeffs().coeff(0);
0193     // since the line equations ax+by=c are normalized with a^2+b^2=1, the following tests
0194     // whether the two lines are approximately parallel.
0195     if(internal::isMuchSmallerThan(det, Scalar(1)))
0196     {   // special case where the two lines are approximately parallel. Pick any point on the first line.
0197         if(numext::abs(coeffs().coeff(1))>numext::abs(coeffs().coeff(0)))
0198             return VectorType(coeffs().coeff(1), -coeffs().coeff(2)/coeffs().coeff(1)-coeffs().coeff(0));
0199         else
0200             return VectorType(-coeffs().coeff(2)/coeffs().coeff(0)-coeffs().coeff(1), coeffs().coeff(0));
0201     }
0202     else
0203     {   // general case
0204         Scalar invdet = Scalar(1) / det;
0205         return VectorType(invdet*(coeffs().coeff(1)*other.coeffs().coeff(2)-other.coeffs().coeff(1)*coeffs().coeff(2)),
0206                           invdet*(other.coeffs().coeff(0)*coeffs().coeff(2)-coeffs().coeff(0)*other.coeffs().coeff(2)));
0207     }
0208   }
0209
0210   /** Applies the transformation matrix \a mat to \c *this and returns a reference to \c *this.
0211     *
0212     * \param mat the Dim x Dim transformation matrix
0213     * \param traits specifies whether the matrix \a mat represents an #Isometry
0214     *               or a more generic #Affine transformation. The default is #Affine.
0215     */
0216   template<typename XprType>
0217   EIGEN_DEVICE_FUNC inline Hyperplane& transform(const MatrixBase<XprType>& mat, TransformTraits traits = Affine)
0218   {
0219     if (traits==Affine)
0220     {
0221       normal() = mat.inverse().transpose() * normal();
0222       m_coeffs /= normal().norm();
0223     }
0224     else if (traits==Isometry)
0225       normal() = mat * normal();
0226     else
0227     {
0228       eigen_assert(0 && "invalid traits value in Hyperplane::transform()");
0229     }
0230     return *this;
0231   }
0232
0233   /** Applies the transformation \a t to \c *this and returns a reference to \c *this.
0234     *
0235     * \param t the transformation of dimension Dim
0236     * \param traits specifies whether the transformation \a t represents an #Isometry
0237     *               or a more generic #Affine transformation. The default is #Affine.
0238     *               Other kind of transformations are not supported.
0239     */
0240   template<int TrOptions>
0241   EIGEN_DEVICE_FUNC inline Hyperplane& transform(const Transform<Scalar,AmbientDimAtCompileTime,Affine,TrOptions>& t,
0242                                 TransformTraits traits = Affine)
0243   {
0244     transform(t.linear(), traits);
0245     offset() -= normal().dot(t.translation());
0246     return *this;
0247   }
0248
0249   /** \returns \c *this with scalar type casted to \a NewScalarType
0250     *
0251     * Note that if \a NewScalarType is equal to the current scalar type of \c *this
0252     * then this function smartly returns a const reference to \c *this.
0253     */
0254   template<typename NewScalarType>
0255   EIGEN_DEVICE_FUNC inline typename internal::cast_return_type<Hyperplane,
0256            Hyperplane<NewScalarType,AmbientDimAtCompileTime,Options> >::type cast() const
0257   {
0258     return typename internal::cast_return_type<Hyperplane,
0259                     Hyperplane<NewScalarType,AmbientDimAtCompileTime,Options> >::type(*this);
0260   }
0261
0262   /** Copy constructor with scalar type conversion */
0263   template<typename OtherScalarType,int OtherOptions>
0264   EIGEN_DEVICE_FUNC inline explicit Hyperplane(const Hyperplane<OtherScalarType,AmbientDimAtCompileTime,OtherOptions>& other)
0265   { m_coeffs = other.coeffs().template cast<Scalar>(); }
0266
0267   /** \returns \c true if \c *this is approximately equal to \a other, within the precision
0268     * determined by \a prec.
0269     *
0270     * \sa MatrixBase::isApprox() */
0271   template<int OtherOptions>
0272   EIGEN_DEVICE_FUNC bool isApprox(const Hyperplane<Scalar,AmbientDimAtCompileTime,OtherOptions>& other, const typename NumTraits<Scalar>::Real& prec = NumTraits<Scalar>::dummy_precision()) const
0273   { return m_coeffs.isApprox(other.m_coeffs, prec); }
0274
0275 protected:
0276
0277   Coefficients m_coeffs;
0278 };
0279
0280 } // end namespace Eigen
0281
0282 #endif // EIGEN_HYPERPLANE_H