From 9b4d46c82ebd6cd71f1ca559f73ea5864c6ef835 Mon Sep 17 00:00:00 2001 From: Gael Guennebaud <g.gael@free.fr> Date: Thu, 28 Aug 2008 17:44:27 +0000 Subject: [PATCH] * 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. --- Eigen/Geometry | 9 +- Eigen/src/Geometry/HyperPlane.h | 166 ++++++++++++++++++++++++++++++ Eigen/src/Geometry/OrthoMethods.h | 6 +- test/CMakeLists.txt | 5 +- test/hyperplane.cpp | 71 +++++++++++++ 5 files changed, 248 insertions(+), 9 deletions(-) create mode 100644 Eigen/src/Geometry/HyperPlane.h create mode 100644 test/hyperplane.cpp 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; diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt index c81876596..5453bfcf0 100644 --- a/test/CMakeLists.txt +++ b/test/CMakeLists.txt @@ -116,9 +116,10 @@ EI_ADD_TEST(determinant) EI_ADD_TEST(inverse) EI_ADD_TEST(qr) EI_ADD_TEST(eigensolver) -EI_ADD_TEST(geometry) -EI_ADD_TEST(regression) EI_ADD_TEST(svd) +EI_ADD_TEST(geometry) +EI_ADD_TEST(hyperplane) +EI_ADD_TEST(regression) EI_ADD_TEST(sparse) ENDIF(BUILD_TESTS) diff --git a/test/hyperplane.cpp b/test/hyperplane.cpp new file mode 100644 index 000000000..5b72d7c47 --- /dev/null +++ b/test/hyperplane.cpp @@ -0,0 +1,71 @@ +// 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/>. + +#include "main.h" +#include <Eigen/Geometry> +#include <Eigen/LU> + +template<typename PlaneType> void hyperplane(const PlaneType& _plane) +{ + /* this test covers the following files: + HyperPlane.h + */ + + const int dim = _plane.dim(); + typedef typename PlaneType::Scalar Scalar; + typedef typename NumTraits<Scalar>::Real RealScalar; + typedef Matrix<Scalar, PlaneType::DimAtCompileTime, 1> VectorType; + + VectorType p0 = VectorType::Random(dim); + VectorType p1 = VectorType::Random(dim); + + VectorType n0 = VectorType::Random(dim).normalized(); + VectorType n1 = VectorType::Random(dim).normalized(); + + PlaneType pl0(n0, p0); + PlaneType pl1(n1, p1); + + Scalar s0 = ei_random<Scalar>(); + Scalar s1 = ei_random<Scalar>(); + + VERIFY_IS_APPROX( n1.dot(n1), Scalar(1) ); + VERIFY_IS_APPROX( n1.dot(n1), Scalar(1) ); + + VERIFY_IS_MUCH_SMALLER_THAN( pl0.distanceTo(p0), Scalar(1) ); + VERIFY_IS_APPROX( pl1.distanceTo(p1 + n1 * s0), s0 ); + VERIFY_IS_MUCH_SMALLER_THAN( pl1.distanceTo(pl1.project(p0)), Scalar(1) ); + VERIFY_IS_MUCH_SMALLER_THAN( pl1.distanceTo(p1 + pl1.normal().unitOrthogonal() * s1), Scalar(1) ); + +} + +void test_hyperplane() +{ + for(int i = 0; i < g_repeat; i++) { + CALL_SUBTEST( hyperplane(HyperPlane<float,2>()) ); + CALL_SUBTEST( hyperplane(HyperPlane<float,3>()) ); + CALL_SUBTEST( hyperplane(HyperPlane<double,4>()) ); + CALL_SUBTEST( hyperplane(HyperPlane<std::complex<double>,5>()) ); + CALL_SUBTEST( hyperplane(HyperPlane<double,Dynamic>(13)) ); + } +}