* Add an HyperPlane class in the Geometry module

  with its respective unit-test.
  Feel free to discuss the API on the ML.
* Some bugfix in unitOrthogonal found by the hyperplane unit test.

3 files changed, 174 insertions, 7 deletions

diff --git a/Eigen/Geometry b/Eigen/Geometry
index 61b7d7038..065d4ee5a 100644
--- a/Eigen/Geometry
+++ b/Eigen/Geometry
@@ -22,15 +22,16 @@ namespace Eigen {
   * \endcode
   */
 
+// the Geometry module use cwiseCos and cwiseSin which are defined in the Array module
+#include "src/Array/CwiseOperators.h"
+#include "src/Array/Functors.h"
+
 #include "src/Geometry/OrthoMethods.h"
 #include "src/Geometry/Quaternion.h"
 #include "src/Geometry/AngleAxis.h"
 #include "src/Geometry/Rotation.h"
 #include "src/Geometry/Transform.h"
-
-// the Geometry module use cwiseCos and cwiseSin which are defined in the Array module
-#include "src/Array/CwiseOperators.h"
-#include "src/Array/Functors.h"
+#include "src/Geometry/HyperPlane.h"
 
 } // namespace Eigen
 
diff --git a/Eigen/src/Geometry/HyperPlane.h b/Eigen/src/Geometry/HyperPlane.h
new file mode 100644
index 000000000..769bbb345
--- /dev/null
+++ b/Eigen/src/Geometry/HyperPlane.h
@@ -0,0 +1,166 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra. Eigen itself is part of the KDE project.
+//
+// Copyright (C) 2008 Gael Guennebaud <g.gael@free.fr>
+//
+// Eigen is free software; you can redistribute it and/or
+// modify it under the terms of the GNU Lesser General Public
+// License as published by the Free Software Foundation; either
+// version 3 of the License, or (at your option) any later version.
+//
+// Alternatively, you can redistribute it and/or
+// modify it under the terms of the GNU General Public License as
+// published by the Free Software Foundation; either version 2 of
+// the License, or (at your option) any later version.
+//
+// Eigen is distributed in the hope that it will be useful, but WITHOUT ANY
+// WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
+// FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License or the
+// GNU General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public
+// License and a copy of the GNU General Public License along with
+// Eigen. If not, see <http://www.gnu.org/licenses/>.
+
+#ifndef EIGEN_HYPERPLANE_H
+#define EIGEN_HYPERPLANE_H
+
+/** \geometry_module \ingroup GeometryModule
+  *
+  * \class HyperPlane
+  *
+  * \brief Represents an hyper plane in any dimensions
+  *
+  * \param _Scalar the scalar type, i.e., the type of the coefficients
+  * \param _Dim the  dimension of the space, can be a compile time value or Dynamic
+  *
+  * This class represents an hyper-plane as the zero set of the implicit equation
+  * \f$ n \cdot x + d = 0 \f$ where \f$ n \f$ is the normal of the plane (linear part)
+  * and \f$ d \f$ is the distance (offset) to the origin.
+  *
+  */
+
+// FIXME default to 3 (because plane => dim=3, or default to Dynamic ?)
+template <typename _Scalar, int _Dim = 3>
+class HyperPlane
+{
+
+  public:
+
+    enum { DimAtCompileTime = _Dim };
+    typedef _Scalar Scalar;
+    typedef typename NumTraits<Scalar>::Real RealScalar;
+    typedef Matrix<Scalar,DimAtCompileTime,1> VectorType;
+
+    HyperPlane(int _dim = DimAtCompileTime)
+      : m_normal(_dim)
+    {}
+    
+    /** Construct a plane from its normal \a normal and a point \a e onto the plane.
+      * \warning the vector normal is assumed to be normalized.
+      */
+    HyperPlane(const VectorType& normal, const VectorType e)
+        : m_normal(normal), m_offset(-e.dot(normal))
+    {}
+    
+    /** Constructs a plane from its normal \a normal and distance to the origin \a d.
+      * \warning the vector normal is assumed to be normalized.
+      */
+    HyperPlane(const VectorType& normal, Scalar d)
+      : m_normal(normal), m_offset(d)
+    {}
+    
+    ~HyperPlane() {}
+
+    /** \returns the dimension in which the plane holds */
+    int dim() const { return m_normal.size(); }
+
+    /** normalizes \c *this */
+    void normalize(void)
+    {
+      RealScalar l = Scalar(1)/m_normal.norm();
+      m_normal *= l;
+      m_offset *= l;
+    }
+    
+    /** \returns the signed distance between the plane \c *this and a point \a p.
+      */
+    inline Scalar distanceTo(const VectorType& p) const
+    {
+      return p.dot(m_normal) + m_offset;
+    }
+    
+    /** \returns the projection of a point \a p onto the plane \c *this.
+      */
+    inline VectorType project(const VectorType& p) const
+    {
+      return p - distanceTo(p) * m_normal;
+    }
+
+    /**  \returns the normal of the plane, which corresponds to the linear part of the implicit equation. */
+    inline const VectorType& normal(void) const { return m_normal; }
+
+    /** \returns the distance to the origin, which is also the constant part
+      * of the implicit equation */
+    inline Scalar offset(void) const { return m_offset; }
+    
+    /** Set the normal of the plane.
+      * \warning the vector normal is assumed to be normalized. */
+    inline void setNormal(const VectorType& normal) { m_normal = normal; }
+
+    /** Set the distance to origin */
+    inline void setOffset(Scalar d) { m_offset = d; }
+
+    /** \returns a pointer the coefficients c_i of the plane equation:
+      *  c_0*x_0 + ... + c_d-1*x_d-1 + offset = 0
+      * \warning this is only for fixed size dimensions !
+      */
+    inline const Scalar* equation(void) const { return m_normal.data(); }
+    
+    /** \brief Plane/ray intersection.
+        Returns the parameter value of the intersection between the plane \a *this
+        and the parametric ray of origin \a rayOrigin and axis \a rayDir
+    */
+    Scalar rayIntersection(const VectorType& rayOrigin, const VectorType& rayDir)
+    {
+      return -(m_offset+rayOrigin.dot(m_normal))/(rayDir.dot(m_normal));
+    }
+
+    // TODO some convenient functions to fit a 3D plane on 3 points etc...
+//     void makePassBy(const VectorType& p0, const VectorType& p1, const VectorType& p2)
+//     {
+//       EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(3);
+//       m_normal = (p2 - p0).cross(p1 - p0).normalized();
+//       m_offset = -m_normal.dot(p0);
+//     }
+// 
+//     void makePassBy(const VectorType& p0, const VectorType& p1)
+//     {
+//       EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(2);
+//       m_normal = (p2 - p0).cross(p1 - p0).normalized();
+//       m_offset = -m_normal.dot(p0);
+//     }
+
+protected:
+
+    VectorType m_normal;
+    Scalar m_offset;
+};
+
+/** \addtogroup GeometryModule */
+//@{
+typedef HyperPlane<float, 2> HyperPlane2f;
+typedef HyperPlane<double,2> HyperPlane2d;
+typedef HyperPlane<float, 3> HyperPlane3f;
+typedef HyperPlane<double,3> HyperPlane3d;
+
+typedef HyperPlane<float, 2> Linef;
+typedef HyperPlane<double,2> Lined;
+typedef HyperPlane<float, 3> Planef;
+typedef HyperPlane<double,3> Planed;
+
+typedef HyperPlane<float, Dynamic> HyperPlaneXf;
+typedef HyperPlane<double,Dynamic> HyperPlaneXd;
+//@}
+
+#endif // EIGEN_HYPERPLANE_H
diff --git a/Eigen/src/Geometry/OrthoMethods.h b/Eigen/src/Geometry/OrthoMethods.h
index 9ca107541..3c310badd 100644
--- a/Eigen/src/Geometry/OrthoMethods.h
+++ b/Eigen/src/Geometry/OrthoMethods.h
@@ -54,7 +54,7 @@ struct ei_unitOrthogonal_selector
   typedef typename NumTraits<Scalar>::Real RealScalar;
   inline static VectorType run(const Derived& src)
   {
-    VectorType perp;
+    VectorType perp(src.size());
     /* Let us compute the crossed product of *this with a vector
      * that is not too close to being colinear to *this.
      */
@@ -65,7 +65,7 @@ struct ei_unitOrthogonal_selector
     if((!ei_isMuchSmallerThan(src.x(), src.z()))
     || (!ei_isMuchSmallerThan(src.y(), src.z())))
     {
-      RealScalar invnm = Scalar(1)/src.template start<2>().norm();
+      RealScalar invnm = RealScalar(1)/src.template start<2>().norm();
       perp.coeffRef(0) = -ei_conj(src.y())*invnm;
       perp.coeffRef(1) = ei_conj(src.x())*invnm;
       perp.coeffRef(2) = 0;
@@ -76,7 +76,7 @@ struct ei_unitOrthogonal_selector
      */
     else
     {
-      RealScalar invnm = Scalar(1)/src.template end<2>().norm();
+      RealScalar invnm = RealScalar(1)/src.template end<2>().norm();
       perp.coeffRef(0) = 0;
       perp.coeffRef(1) = -ei_conj(src.z())*invnm;
       perp.coeffRef(2) = ei_conj(src.y())*invnm;