geo_quaternion.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-2009 Gael Guennebaud <gael.guennebaud@inria.fr>
00005 // Copyright (C) 2009 Mathieu Gautier <mathieu.gautier@cea.fr>
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/SVD>
00030 
00031 template<typename Scalar, int Options> void quaternion(void)
00032 {
00033   /* this test covers the following files:
00034      Quaternion.h
00035   */
00036 
00037   typedef Matrix<Scalar,3,3> Matrix3;
00038   typedef Matrix<Scalar,3,1> Vector3;
00039   typedef Quaternion<Scalar,Options> Quaternionx;
00040   typedef AngleAxis<Scalar> AngleAxisx;
00041 
00042   Scalar largeEps = test_precision<Scalar>();
00043   if (internal::is_same<Scalar,float>::value)
00044     largeEps = 1e-3f;
00045 
00046   Scalar eps = internal::random<Scalar>() * Scalar(1e-2);
00047 
00048   Vector3 v0 = Vector3::Random(),
00049           v1 = Vector3::Random(),
00050           v2 = Vector3::Random(),
00051           v3 = Vector3::Random();
00052 
00053   Scalar a = internal::random<Scalar>(-Scalar(M_PI), Scalar(M_PI));
00054 
00055   // Quaternion: Identity(), setIdentity();
00056   Quaternionx q1, q2;
00057   q2.setIdentity();
00058   VERIFY_IS_APPROX(Quaternionx(Quaternionx::Identity()).coeffs(), q2.coeffs());
00059   q1.coeffs().setRandom();
00060   VERIFY_IS_APPROX(q1.coeffs(), (q1*q2).coeffs());
00061 
00062   // concatenation
00063   q1 *= q2;
00064 
00065   q1 = AngleAxisx(a, v0.normalized());
00066   q2 = AngleAxisx(a, v1.normalized());
00067 
00068   // angular distance
00069   Scalar refangle = internal::abs(AngleAxisx(q1.inverse()*q2).angle());
00070   if (refangle>Scalar(M_PI))
00071     refangle = Scalar(2)*Scalar(M_PI) - refangle;
00072 
00073   if((q1.coeffs()-q2.coeffs()).norm() > 10*largeEps)
00074   {
00075     VERIFY_IS_MUCH_SMALLER_THAN(internal::abs(q1.angularDistance(q2) - refangle), Scalar(1));
00076   }
00077 
00078   // rotation matrix conversion
00079   VERIFY_IS_APPROX(q1 * v2, q1.toRotationMatrix() * v2);
00080   VERIFY_IS_APPROX(q1 * q2 * v2,
00081     q1.toRotationMatrix() * q2.toRotationMatrix() * v2);
00082 
00083   VERIFY(  (q2*q1).isApprox(q1*q2, largeEps)
00084         || !(q2 * q1 * v2).isApprox(q1.toRotationMatrix() * q2.toRotationMatrix() * v2));
00085 
00086   q2 = q1.toRotationMatrix();
00087   VERIFY_IS_APPROX(q1*v1,q2*v1);
00088 
00089 
00090   // angle-axis conversion
00091   AngleAxisx aa = AngleAxisx(q1);
00092   VERIFY_IS_APPROX(q1 * v1, Quaternionx(aa) * v1);
00093 
00094   // Do not execute the test if the rotation angle is almost zero, or
00095   // the rotation axis and v1 are almost parallel.
00096   if (internal::abs(aa.angle()) > 5*test_precision<Scalar>()
00097       && (aa.axis() - v1.normalized()).norm() < 1.99
00098       && (aa.axis() + v1.normalized()).norm() < 1.99) 
00099   {
00100     VERIFY_IS_NOT_APPROX(q1 * v1, Quaternionx(AngleAxisx(aa.angle()*2,aa.axis())) * v1);
00101   }
00102 
00103   // from two vector creation
00104   VERIFY_IS_APPROX( v2.normalized(),(q2.setFromTwoVectors(v1, v2)*v1).normalized());
00105   VERIFY_IS_APPROX( v1.normalized(),(q2.setFromTwoVectors(v1, v1)*v1).normalized());
00106   VERIFY_IS_APPROX(-v1.normalized(),(q2.setFromTwoVectors(v1,-v1)*v1).normalized());
00107   if (internal::is_same<Scalar,double>::value)
00108   {
00109     v3 = (v1.array()+eps).matrix();
00110     VERIFY_IS_APPROX( v3.normalized(),(q2.setFromTwoVectors(v1, v3)*v1).normalized());
00111     VERIFY_IS_APPROX(-v3.normalized(),(q2.setFromTwoVectors(v1,-v3)*v1).normalized());
00112   }
00113 
00114   // inverse and conjugate
00115   VERIFY_IS_APPROX(q1 * (q1.inverse() * v1), v1);
00116   VERIFY_IS_APPROX(q1 * (q1.conjugate() * v1), v1);
00117 
00118   // test casting
00119   Quaternion<float> q1f = q1.template cast<float>();
00120   VERIFY_IS_APPROX(q1f.template cast<Scalar>(),q1);
00121   Quaternion<double> q1d = q1.template cast<double>();
00122   VERIFY_IS_APPROX(q1d.template cast<Scalar>(),q1);
00123 }
00124 
00125 template<typename Scalar> void mapQuaternion(void){
00126   typedef Map<Quaternion<Scalar>, Aligned> MQuaternionA;
00127   typedef Map<Quaternion<Scalar> > MQuaternionUA;
00128   typedef Map<const Quaternion<Scalar> > MCQuaternionUA;
00129   typedef Quaternion<Scalar> Quaternionx;
00130 
00131   EIGEN_ALIGN16 Scalar array1[4];
00132   EIGEN_ALIGN16 Scalar array2[4];
00133   EIGEN_ALIGN16 Scalar array3[4+1];
00134   Scalar* array3unaligned = array3+1;
00135 
00136 //  std::cerr << array1 << " " << array2 << " " << array3 << "\n";
00137   MQuaternionA(array1).coeffs().setRandom();
00138   (MQuaternionA(array2)) = MQuaternionA(array1);
00139   (MQuaternionUA(array3unaligned)) = MQuaternionA(array1);
00140 
00141   Quaternionx q1 = MQuaternionA(array1);
00142   Quaternionx q2 = MQuaternionA(array2);
00143   Quaternionx q3 = MQuaternionUA(array3unaligned);
00144   Quaternionx q4 = MCQuaternionUA(array3unaligned);
00145 
00146   VERIFY_IS_APPROX(q1.coeffs(), q2.coeffs());
00147   VERIFY_IS_APPROX(q1.coeffs(), q3.coeffs());
00148   VERIFY_IS_APPROX(q4.coeffs(), q3.coeffs());
00149   #ifdef EIGEN_VECTORIZE
00150   if(internal::packet_traits<Scalar>::Vectorizable)
00151     VERIFY_RAISES_ASSERT((MQuaternionA(array3unaligned)));
00152   #endif
00153 }
00154 
00155 template<typename Scalar> void quaternionAlignment(void){
00156   typedef Quaternion<Scalar,AutoAlign> QuaternionA;
00157   typedef Quaternion<Scalar,DontAlign> QuaternionUA;
00158 
00159   EIGEN_ALIGN16 Scalar array1[4];
00160   EIGEN_ALIGN16 Scalar array2[4];
00161   EIGEN_ALIGN16 Scalar array3[4+1];
00162   Scalar* arrayunaligned = array3+1;
00163 
00164   QuaternionA *q1 = ::new(reinterpret_cast<void*>(array1)) QuaternionA;
00165   QuaternionUA *q2 = ::new(reinterpret_cast<void*>(array2)) QuaternionUA;
00166   QuaternionUA *q3 = ::new(reinterpret_cast<void*>(arrayunaligned)) QuaternionUA;
00167 
00168   q1->coeffs().setRandom();
00169   *q2 = *q1;
00170   *q3 = *q1;
00171 
00172   VERIFY_IS_APPROX(q1->coeffs(), q2->coeffs());
00173   VERIFY_IS_APPROX(q1->coeffs(), q3->coeffs());
00174   #if defined(EIGEN_VECTORIZE) && EIGEN_ALIGN_STATICALLY
00175   if(internal::packet_traits<Scalar>::Vectorizable)
00176     VERIFY_RAISES_ASSERT((::new(reinterpret_cast<void*>(arrayunaligned)) QuaternionA));
00177   #endif
00178 }
00179 
00180 template<typename PlainObjectType> void check_const_correctness(const PlainObjectType&)
00181 {
00182   // there's a lot that we can't test here while still having this test compile!
00183   // the only possible approach would be to run a script trying to compile stuff and checking that it fails.
00184   // CMake can help with that.
00185 
00186   // verify that map-to-const don't have LvalueBit
00187   typedef typename internal::add_const<PlainObjectType>::type ConstPlainObjectType;
00188   VERIFY( !(internal::traits<Map<ConstPlainObjectType> >::Flags & LvalueBit) );
00189   VERIFY( !(internal::traits<Map<ConstPlainObjectType, Aligned> >::Flags & LvalueBit) );
00190   VERIFY( !(Map<ConstPlainObjectType>::Flags & LvalueBit) );
00191   VERIFY( !(Map<ConstPlainObjectType, Aligned>::Flags & LvalueBit) );
00192 }
00193 
00194 
00195 void test_geo_quaternion()
00196 {
00197   for(int i = 0; i < g_repeat; i++) {
00198     CALL_SUBTEST_1(( quaternion<float,AutoAlign>() ));
00199     CALL_SUBTEST_1( check_const_correctness(Quaternionf()) );
00200     CALL_SUBTEST_2(( quaternion<double,AutoAlign>() ));
00201     CALL_SUBTEST_2( check_const_correctness(Quaterniond()) );
00202     CALL_SUBTEST_3(( quaternion<float,DontAlign>() ));
00203     CALL_SUBTEST_4(( quaternion<double,DontAlign>() ));
00204     CALL_SUBTEST_5(( quaternionAlignment<float>() ));
00205     CALL_SUBTEST_6(( quaternionAlignment<double>() ));
00206     CALL_SUBTEST_1( mapQuaternion<float>() );
00207     CALL_SUBTEST_2( mapQuaternion<double>() );
00208   }
00209 }


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