Home | History | Annotate | Download | only in test
      1 // This file is part of Eigen, a lightweight C++ template library
      2 // for linear algebra.
      3 //
      4 // Copyright (C) 2008-2009 Gael Guennebaud <gael.guennebaud (at) inria.fr>
      5 // Copyright (C) 2009 Mathieu Gautier <mathieu.gautier (at) cea.fr>
      6 //
      7 // This Source Code Form is subject to the terms of the Mozilla
      8 // Public License v. 2.0. If a copy of the MPL was not distributed
      9 // with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
     10 
     11 #include "main.h"
     12 #include <Eigen/Geometry>
     13 #include <Eigen/LU>
     14 #include <Eigen/SVD>
     15 
     16 template<typename T> T bounded_acos(T v)
     17 {
     18   using std::acos;
     19   using std::min;
     20   using std::max;
     21   return acos((max)(T(-1),(min)(v,T(1))));
     22 }
     23 
     24 template<typename QuatType> void check_slerp(const QuatType& q0, const QuatType& q1)
     25 {
     26   using std::abs;
     27   typedef typename QuatType::Scalar Scalar;
     28   typedef AngleAxis<Scalar> AA;
     29 
     30   Scalar largeEps = test_precision<Scalar>();
     31 
     32   Scalar theta_tot = AA(q1*q0.inverse()).angle();
     33   if(theta_tot>M_PI)
     34     theta_tot = Scalar(2.*M_PI)-theta_tot;
     35   for(Scalar t=0; t<=Scalar(1.001); t+=Scalar(0.1))
     36   {
     37     QuatType q = q0.slerp(t,q1);
     38     Scalar theta = AA(q*q0.inverse()).angle();
     39     VERIFY(abs(q.norm() - 1) < largeEps);
     40     if(theta_tot==0)  VERIFY(theta_tot==0);
     41     else              VERIFY(abs(theta - t * theta_tot) < largeEps);
     42   }
     43 }
     44 
     45 template<typename Scalar, int Options> void quaternion(void)
     46 {
     47   /* this test covers the following files:
     48      Quaternion.h
     49   */
     50   using std::abs;
     51   typedef Matrix<Scalar,3,1> Vector3;
     52   typedef Matrix<Scalar,4,1> Vector4;
     53   typedef Quaternion<Scalar,Options> Quaternionx;
     54   typedef AngleAxis<Scalar> AngleAxisx;
     55 
     56   Scalar largeEps = test_precision<Scalar>();
     57   if (internal::is_same<Scalar,float>::value)
     58     largeEps = 1e-3f;
     59 
     60   Scalar eps = internal::random<Scalar>() * Scalar(1e-2);
     61 
     62   Vector3 v0 = Vector3::Random(),
     63           v1 = Vector3::Random(),
     64           v2 = Vector3::Random(),
     65           v3 = Vector3::Random();
     66 
     67   Scalar  a = internal::random<Scalar>(-Scalar(M_PI), Scalar(M_PI)),
     68           b = internal::random<Scalar>(-Scalar(M_PI), Scalar(M_PI));
     69 
     70   // Quaternion: Identity(), setIdentity();
     71   Quaternionx q1, q2;
     72   q2.setIdentity();
     73   VERIFY_IS_APPROX(Quaternionx(Quaternionx::Identity()).coeffs(), q2.coeffs());
     74   q1.coeffs().setRandom();
     75   VERIFY_IS_APPROX(q1.coeffs(), (q1*q2).coeffs());
     76 
     77   // concatenation
     78   q1 *= q2;
     79 
     80   q1 = AngleAxisx(a, v0.normalized());
     81   q2 = AngleAxisx(a, v1.normalized());
     82 
     83   // angular distance
     84   Scalar refangle = abs(AngleAxisx(q1.inverse()*q2).angle());
     85   if (refangle>Scalar(M_PI))
     86     refangle = Scalar(2)*Scalar(M_PI) - refangle;
     87 
     88   if((q1.coeffs()-q2.coeffs()).norm() > 10*largeEps)
     89   {
     90     VERIFY_IS_MUCH_SMALLER_THAN(abs(q1.angularDistance(q2) - refangle), Scalar(1));
     91   }
     92 
     93   // rotation matrix conversion
     94   VERIFY_IS_APPROX(q1 * v2, q1.toRotationMatrix() * v2);
     95   VERIFY_IS_APPROX(q1 * q2 * v2,
     96     q1.toRotationMatrix() * q2.toRotationMatrix() * v2);
     97 
     98   VERIFY(  (q2*q1).isApprox(q1*q2, largeEps)
     99         || !(q2 * q1 * v2).isApprox(q1.toRotationMatrix() * q2.toRotationMatrix() * v2));
    100 
    101   q2 = q1.toRotationMatrix();
    102   VERIFY_IS_APPROX(q1*v1,q2*v1);
    103 
    104 
    105   // angle-axis conversion
    106   AngleAxisx aa = AngleAxisx(q1);
    107   VERIFY_IS_APPROX(q1 * v1, Quaternionx(aa) * v1);
    108 
    109   // Do not execute the test if the rotation angle is almost zero, or
    110   // the rotation axis and v1 are almost parallel.
    111   if (abs(aa.angle()) > 5*test_precision<Scalar>()
    112       && (aa.axis() - v1.normalized()).norm() < 1.99
    113       && (aa.axis() + v1.normalized()).norm() < 1.99)
    114   {
    115     VERIFY_IS_NOT_APPROX(q1 * v1, Quaternionx(AngleAxisx(aa.angle()*2,aa.axis())) * v1);
    116   }
    117 
    118   // from two vector creation
    119   VERIFY_IS_APPROX( v2.normalized(),(q2.setFromTwoVectors(v1, v2)*v1).normalized());
    120   VERIFY_IS_APPROX( v1.normalized(),(q2.setFromTwoVectors(v1, v1)*v1).normalized());
    121   VERIFY_IS_APPROX(-v1.normalized(),(q2.setFromTwoVectors(v1,-v1)*v1).normalized());
    122   if (internal::is_same<Scalar,double>::value)
    123   {
    124     v3 = (v1.array()+eps).matrix();
    125     VERIFY_IS_APPROX( v3.normalized(),(q2.setFromTwoVectors(v1, v3)*v1).normalized());
    126     VERIFY_IS_APPROX(-v3.normalized(),(q2.setFromTwoVectors(v1,-v3)*v1).normalized());
    127   }
    128 
    129   // from two vector creation static function
    130   VERIFY_IS_APPROX( v2.normalized(),(Quaternionx::FromTwoVectors(v1, v2)*v1).normalized());
    131   VERIFY_IS_APPROX( v1.normalized(),(Quaternionx::FromTwoVectors(v1, v1)*v1).normalized());
    132   VERIFY_IS_APPROX(-v1.normalized(),(Quaternionx::FromTwoVectors(v1,-v1)*v1).normalized());
    133   if (internal::is_same<Scalar,double>::value)
    134   {
    135     v3 = (v1.array()+eps).matrix();
    136     VERIFY_IS_APPROX( v3.normalized(),(Quaternionx::FromTwoVectors(v1, v3)*v1).normalized());
    137     VERIFY_IS_APPROX(-v3.normalized(),(Quaternionx::FromTwoVectors(v1,-v3)*v1).normalized());
    138   }
    139 
    140   // inverse and conjugate
    141   VERIFY_IS_APPROX(q1 * (q1.inverse() * v1), v1);
    142   VERIFY_IS_APPROX(q1 * (q1.conjugate() * v1), v1);
    143 
    144   // test casting
    145   Quaternion<float> q1f = q1.template cast<float>();
    146   VERIFY_IS_APPROX(q1f.template cast<Scalar>(),q1);
    147   Quaternion<double> q1d = q1.template cast<double>();
    148   VERIFY_IS_APPROX(q1d.template cast<Scalar>(),q1);
    149 
    150   // test bug 369 - improper alignment.
    151   Quaternionx *q = new Quaternionx;
    152   delete q;
    153 
    154   q1 = AngleAxisx(a, v0.normalized());
    155   q2 = AngleAxisx(b, v1.normalized());
    156   check_slerp(q1,q2);
    157 
    158   q1 = AngleAxisx(b, v1.normalized());
    159   q2 = AngleAxisx(b+Scalar(M_PI), v1.normalized());
    160   check_slerp(q1,q2);
    161 
    162   q1 = AngleAxisx(b,  v1.normalized());
    163   q2 = AngleAxisx(-b, -v1.normalized());
    164   check_slerp(q1,q2);
    165 
    166   q1.coeffs() = Vector4::Random().normalized();
    167   q2.coeffs() = -q1.coeffs();
    168   check_slerp(q1,q2);
    169 }
    170 
    171 template<typename Scalar> void mapQuaternion(void){
    172   typedef Map<Quaternion<Scalar>, Aligned> MQuaternionA;
    173   typedef Map<const Quaternion<Scalar>, Aligned> MCQuaternionA;
    174   typedef Map<Quaternion<Scalar> > MQuaternionUA;
    175   typedef Map<const Quaternion<Scalar> > MCQuaternionUA;
    176   typedef Quaternion<Scalar> Quaternionx;
    177   typedef Matrix<Scalar,3,1> Vector3;
    178   typedef AngleAxis<Scalar> AngleAxisx;
    179 
    180   Vector3 v0 = Vector3::Random(),
    181           v1 = Vector3::Random();
    182   Scalar  a = internal::random<Scalar>(-Scalar(M_PI), Scalar(M_PI));
    183 
    184   EIGEN_ALIGN16 Scalar array1[4];
    185   EIGEN_ALIGN16 Scalar array2[4];
    186   EIGEN_ALIGN16 Scalar array3[4+1];
    187   Scalar* array3unaligned = array3+1;
    188 
    189   MQuaternionA    mq1(array1);
    190   MCQuaternionA   mcq1(array1);
    191   MQuaternionA    mq2(array2);
    192   MQuaternionUA   mq3(array3unaligned);
    193   MCQuaternionUA  mcq3(array3unaligned);
    194 
    195 //  std::cerr << array1 << " " << array2 << " " << array3 << "\n";
    196   mq1 = AngleAxisx(a, v0.normalized());
    197   mq2 = mq1;
    198   mq3 = mq1;
    199 
    200   Quaternionx q1 = mq1;
    201   Quaternionx q2 = mq2;
    202   Quaternionx q3 = mq3;
    203   Quaternionx q4 = MCQuaternionUA(array3unaligned);
    204 
    205   VERIFY_IS_APPROX(q1.coeffs(), q2.coeffs());
    206   VERIFY_IS_APPROX(q1.coeffs(), q3.coeffs());
    207   VERIFY_IS_APPROX(q4.coeffs(), q3.coeffs());
    208   #ifdef EIGEN_VECTORIZE
    209   if(internal::packet_traits<Scalar>::Vectorizable)
    210     VERIFY_RAISES_ASSERT((MQuaternionA(array3unaligned)));
    211   #endif
    212 
    213   VERIFY_IS_APPROX(mq1 * (mq1.inverse() * v1), v1);
    214   VERIFY_IS_APPROX(mq1 * (mq1.conjugate() * v1), v1);
    215 
    216   VERIFY_IS_APPROX(mcq1 * (mcq1.inverse() * v1), v1);
    217   VERIFY_IS_APPROX(mcq1 * (mcq1.conjugate() * v1), v1);
    218 
    219   VERIFY_IS_APPROX(mq3 * (mq3.inverse() * v1), v1);
    220   VERIFY_IS_APPROX(mq3 * (mq3.conjugate() * v1), v1);
    221 
    222   VERIFY_IS_APPROX(mcq3 * (mcq3.inverse() * v1), v1);
    223   VERIFY_IS_APPROX(mcq3 * (mcq3.conjugate() * v1), v1);
    224 
    225   VERIFY_IS_APPROX(mq1*mq2, q1*q2);
    226   VERIFY_IS_APPROX(mq3*mq2, q3*q2);
    227   VERIFY_IS_APPROX(mcq1*mq2, q1*q2);
    228   VERIFY_IS_APPROX(mcq3*mq2, q3*q2);
    229 }
    230 
    231 template<typename Scalar> void quaternionAlignment(void){
    232   typedef Quaternion<Scalar,AutoAlign> QuaternionA;
    233   typedef Quaternion<Scalar,DontAlign> QuaternionUA;
    234 
    235   EIGEN_ALIGN16 Scalar array1[4];
    236   EIGEN_ALIGN16 Scalar array2[4];
    237   EIGEN_ALIGN16 Scalar array3[4+1];
    238   Scalar* arrayunaligned = array3+1;
    239 
    240   QuaternionA *q1 = ::new(reinterpret_cast<void*>(array1)) QuaternionA;
    241   QuaternionUA *q2 = ::new(reinterpret_cast<void*>(array2)) QuaternionUA;
    242   QuaternionUA *q3 = ::new(reinterpret_cast<void*>(arrayunaligned)) QuaternionUA;
    243 
    244   q1->coeffs().setRandom();
    245   *q2 = *q1;
    246   *q3 = *q1;
    247 
    248   VERIFY_IS_APPROX(q1->coeffs(), q2->coeffs());
    249   VERIFY_IS_APPROX(q1->coeffs(), q3->coeffs());
    250   #if defined(EIGEN_VECTORIZE) && EIGEN_ALIGN_STATICALLY
    251   if(internal::packet_traits<Scalar>::Vectorizable)
    252     VERIFY_RAISES_ASSERT((::new(reinterpret_cast<void*>(arrayunaligned)) QuaternionA));
    253   #endif
    254 }
    255 
    256 template<typename PlainObjectType> void check_const_correctness(const PlainObjectType&)
    257 {
    258   // there's a lot that we can't test here while still having this test compile!
    259   // the only possible approach would be to run a script trying to compile stuff and checking that it fails.
    260   // CMake can help with that.
    261 
    262   // verify that map-to-const don't have LvalueBit
    263   typedef typename internal::add_const<PlainObjectType>::type ConstPlainObjectType;
    264   VERIFY( !(internal::traits<Map<ConstPlainObjectType> >::Flags & LvalueBit) );
    265   VERIFY( !(internal::traits<Map<ConstPlainObjectType, Aligned> >::Flags & LvalueBit) );
    266   VERIFY( !(Map<ConstPlainObjectType>::Flags & LvalueBit) );
    267   VERIFY( !(Map<ConstPlainObjectType, Aligned>::Flags & LvalueBit) );
    268 }
    269 
    270 void test_geo_quaternion()
    271 {
    272   for(int i = 0; i < g_repeat; i++) {
    273     CALL_SUBTEST_1(( quaternion<float,AutoAlign>() ));
    274     CALL_SUBTEST_1( check_const_correctness(Quaternionf()) );
    275     CALL_SUBTEST_2(( quaternion<double,AutoAlign>() ));
    276     CALL_SUBTEST_2( check_const_correctness(Quaterniond()) );
    277     CALL_SUBTEST_3(( quaternion<float,DontAlign>() ));
    278     CALL_SUBTEST_4(( quaternion<double,DontAlign>() ));
    279     CALL_SUBTEST_5(( quaternionAlignment<float>() ));
    280     CALL_SUBTEST_6(( quaternionAlignment<double>() ));
    281     CALL_SUBTEST_1( mapQuaternion<float>() );
    282     CALL_SUBTEST_2( mapQuaternion<double>() );
    283   }
    284 }
    285