geo_hyperplane.cpp
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 <gael.guennebaud@inria.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 #include "main.h"
00027 #include <Eigen/Geometry>
00028 #include <Eigen/LU>
00029 #include <Eigen/QR>
00030 
00031 template<typename HyperplaneType> void hyperplane(const HyperplaneType& _plane)
00032 {
00033   /* this test covers the following files:
00034      Hyperplane.h
00035   */
00036   typedef typename HyperplaneType::Index Index;
00037   const Index dim = _plane.dim();
00038   enum { Options = HyperplaneType::Options };
00039   typedef typename HyperplaneType::Scalar Scalar;
00040   typedef typename NumTraits<Scalar>::Real RealScalar;
00041   typedef Matrix<Scalar, HyperplaneType::AmbientDimAtCompileTime, 1> VectorType;
00042   typedef Matrix<Scalar, HyperplaneType::AmbientDimAtCompileTime,
00043                          HyperplaneType::AmbientDimAtCompileTime> MatrixType;
00044 
00045   VectorType p0 = VectorType::Random(dim);
00046   VectorType p1 = VectorType::Random(dim);
00047 
00048   VectorType n0 = VectorType::Random(dim).normalized();
00049   VectorType n1 = VectorType::Random(dim).normalized();
00050 
00051   HyperplaneType pl0(n0, p0);
00052   HyperplaneType pl1(n1, p1);
00053   HyperplaneType pl2 = pl1;
00054 
00055   Scalar s0 = internal::random<Scalar>();
00056   Scalar s1 = internal::random<Scalar>();
00057 
00058   VERIFY_IS_APPROX( n1.dot(n1), Scalar(1) );
00059 
00060   VERIFY_IS_MUCH_SMALLER_THAN( pl0.absDistance(p0), Scalar(1) );
00061   VERIFY_IS_APPROX( pl1.signedDistance(p1 + n1 * s0), s0 );
00062   VERIFY_IS_MUCH_SMALLER_THAN( pl1.signedDistance(pl1.projection(p0)), Scalar(1) );
00063   VERIFY_IS_MUCH_SMALLER_THAN( pl1.absDistance(p1 +  pl1.normal().unitOrthogonal() * s1), Scalar(1) );
00064 
00065   // transform
00066   if (!NumTraits<Scalar>::IsComplex)
00067   {
00068     MatrixType rot = MatrixType::Random(dim,dim).householderQr().householderQ();
00069     DiagonalMatrix<Scalar,HyperplaneType::AmbientDimAtCompileTime> scaling(VectorType::Random());
00070     Translation<Scalar,HyperplaneType::AmbientDimAtCompileTime> translation(VectorType::Random());
00071 
00072     pl2 = pl1;
00073     VERIFY_IS_MUCH_SMALLER_THAN( pl2.transform(rot).absDistance(rot * p1), Scalar(1) );
00074     pl2 = pl1;
00075     VERIFY_IS_MUCH_SMALLER_THAN( pl2.transform(rot,Isometry).absDistance(rot * p1), Scalar(1) );
00076     pl2 = pl1;
00077     VERIFY_IS_MUCH_SMALLER_THAN( pl2.transform(rot*scaling).absDistance((rot*scaling) * p1), Scalar(1) );
00078     pl2 = pl1;
00079     VERIFY_IS_MUCH_SMALLER_THAN( pl2.transform(rot*scaling*translation)
00080                                  .absDistance((rot*scaling*translation) * p1), Scalar(1) );
00081     pl2 = pl1;
00082     VERIFY_IS_MUCH_SMALLER_THAN( pl2.transform(rot*translation,Isometry)
00083                                  .absDistance((rot*translation) * p1), Scalar(1) );
00084   }
00085 
00086   // casting
00087   const int Dim = HyperplaneType::AmbientDimAtCompileTime;
00088   typedef typename GetDifferentType<Scalar>::type OtherScalar;
00089   Hyperplane<OtherScalar,Dim,Options> hp1f = pl1.template cast<OtherScalar>();
00090   VERIFY_IS_APPROX(hp1f.template cast<Scalar>(),pl1);
00091   Hyperplane<Scalar,Dim,Options> hp1d = pl1.template cast<Scalar>();
00092   VERIFY_IS_APPROX(hp1d.template cast<Scalar>(),pl1);
00093 }
00094 
00095 template<typename Scalar> void lines()
00096 {
00097   typedef Hyperplane<Scalar, 2> HLine;
00098   typedef ParametrizedLine<Scalar, 2> PLine;
00099   typedef Matrix<Scalar,2,1> Vector;
00100   typedef Matrix<Scalar,3,1> CoeffsType;
00101 
00102   for(int i = 0; i < 10; i++)
00103   {
00104     Vector center = Vector::Random();
00105     Vector u = Vector::Random();
00106     Vector v = Vector::Random();
00107     Scalar a = internal::random<Scalar>();
00108     while (internal::abs(a-1) < 1e-4) a = internal::random<Scalar>();
00109     while (u.norm() < 1e-4) u = Vector::Random();
00110     while (v.norm() < 1e-4) v = Vector::Random();
00111 
00112     HLine line_u = HLine::Through(center + u, center + a*u);
00113     HLine line_v = HLine::Through(center + v, center + a*v);
00114 
00115     // the line equations should be normalized so that a^2+b^2=1
00116     VERIFY_IS_APPROX(line_u.normal().norm(), Scalar(1));
00117     VERIFY_IS_APPROX(line_v.normal().norm(), Scalar(1));
00118 
00119     Vector result = line_u.intersection(line_v);
00120 
00121     // the lines should intersect at the point we called "center"
00122     VERIFY_IS_APPROX(result, center);
00123 
00124     // check conversions between two types of lines
00125     PLine pl(line_u); // gcc 3.3 will commit suicide if we don't name this variable
00126     CoeffsType converted_coeffs = HLine(pl).coeffs();
00127     converted_coeffs *= (line_u.coeffs()[0])/(converted_coeffs[0]);
00128     VERIFY(line_u.coeffs().isApprox(converted_coeffs));
00129   }
00130 }
00131 
00132 template<typename Scalar> void hyperplane_alignment()
00133 {
00134   typedef Hyperplane<Scalar,3,AutoAlign> Plane3a;
00135   typedef Hyperplane<Scalar,3,DontAlign> Plane3u;
00136 
00137   EIGEN_ALIGN16 Scalar array1[4];
00138   EIGEN_ALIGN16 Scalar array2[4];
00139   EIGEN_ALIGN16 Scalar array3[4+1];
00140   Scalar* array3u = array3+1;
00141 
00142   Plane3a *p1 = ::new(reinterpret_cast<void*>(array1)) Plane3a;
00143   Plane3u *p2 = ::new(reinterpret_cast<void*>(array2)) Plane3u;
00144   Plane3u *p3 = ::new(reinterpret_cast<void*>(array3u)) Plane3u;
00145   
00146   p1->coeffs().setRandom();
00147   *p2 = *p1;
00148   *p3 = *p1;
00149 
00150   VERIFY_IS_APPROX(p1->coeffs(), p2->coeffs());
00151   VERIFY_IS_APPROX(p1->coeffs(), p3->coeffs());
00152   
00153   #if defined(EIGEN_VECTORIZE) && EIGEN_ALIGN_STATICALLY
00154   if(internal::packet_traits<Scalar>::Vectorizable)
00155     VERIFY_RAISES_ASSERT((::new(reinterpret_cast<void*>(array3u)) Plane3a));
00156   #endif
00157 }
00158 
00159 
00160 void test_geo_hyperplane()
00161 {
00162   for(int i = 0; i < g_repeat; i++) {
00163     CALL_SUBTEST_1( hyperplane(Hyperplane<float,2>()) );
00164     CALL_SUBTEST_2( hyperplane(Hyperplane<float,3>()) );
00165     CALL_SUBTEST_2( hyperplane(Hyperplane<float,3,DontAlign>()) );
00166     CALL_SUBTEST_2( hyperplane_alignment<float>() );
00167     CALL_SUBTEST_3( hyperplane(Hyperplane<double,4>()) );
00168     CALL_SUBTEST_4( hyperplane(Hyperplane<std::complex<double>,5>()) );
00169     CALL_SUBTEST_1( lines<float>() );
00170     CALL_SUBTEST_3( lines<double>() );
00171   }
00172 }


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