EulerSystem.h
00001 // This file is part of Eigen, a lightweight C++ template library
00002 // for linear algebra.
00003 //
00004 // Copyright (C) 2015 Tal Hadad <tal_hd@hotmail.com>
00005 //
00006 // This Source Code Form is subject to the terms of the Mozilla
00007 // Public License v. 2.0. If a copy of the MPL was not distributed
00008 // with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
00009 
00010 #ifndef EIGEN_EULERSYSTEM_H
00011 #define EIGEN_EULERSYSTEM_H
00012 
00013 namespace Eigen
00014 {
00015   // Forward declerations
00016   template <typename _Scalar, class _System>
00017   class EulerAngles;
00018   
00019   namespace internal
00020   {
00021     // TODO: Check if already exists on the rest API
00022     template <int Num, bool IsPositive = (Num > 0)>
00023     struct Abs
00024     {
00025       enum { value = Num };
00026     };
00027   
00028     template <int Num>
00029     struct Abs<Num, false>
00030     {
00031       enum { value = -Num };
00032     };
00033 
00034     template <int Axis>
00035     struct IsValidAxis
00036     {
00037       enum { value = Axis != 0 && Abs<Axis>::value <= 3 };
00038     };
00039   }
00040   
00041   #define EIGEN_EULER_ANGLES_CLASS_STATIC_ASSERT(COND,MSG) typedef char static_assertion_##MSG[(COND)?1:-1]
00042   
00055   enum EulerAxis
00056   {
00057     EULER_X = 1, 
00058     EULER_Y = 2, 
00059     EULER_Z = 3  
00060   };
00061   
00119   template <int _AlphaAxis, int _BetaAxis, int _GammaAxis>
00120   class EulerSystem
00121   {
00122     public:
00123     // It's defined this way and not as enum, because I think
00124     //  that enum is not guerantee to support negative numbers
00125     
00127     static const int AlphaAxis = _AlphaAxis;
00128     
00130     static const int BetaAxis = _BetaAxis;
00131     
00133     static const int GammaAxis = _GammaAxis;
00134 
00135     enum
00136     {
00137       AlphaAxisAbs = internal::Abs<AlphaAxis>::value, 
00138       BetaAxisAbs = internal::Abs<BetaAxis>::value, 
00139       GammaAxisAbs = internal::Abs<GammaAxis>::value, 
00141       IsAlphaOpposite = (AlphaAxis < 0) ? 1 : 0, 
00142       IsBetaOpposite = (BetaAxis < 0) ? 1 : 0, 
00143       IsGammaOpposite = (GammaAxis < 0) ? 1 : 0, 
00145       IsOdd = ((AlphaAxisAbs)%3 == (BetaAxisAbs - 1)%3) ? 0 : 1, 
00146       IsEven = IsOdd ? 0 : 1, 
00148       IsTaitBryan = ((unsigned)AlphaAxisAbs != (unsigned)GammaAxisAbs) ? 1 : 0 
00149     };
00150     
00151     private:
00152     
00153     EIGEN_EULER_ANGLES_CLASS_STATIC_ASSERT(internal::IsValidAxis<AlphaAxis>::value,
00154       ALPHA_AXIS_IS_INVALID);
00155       
00156     EIGEN_EULER_ANGLES_CLASS_STATIC_ASSERT(internal::IsValidAxis<BetaAxis>::value,
00157       BETA_AXIS_IS_INVALID);
00158       
00159     EIGEN_EULER_ANGLES_CLASS_STATIC_ASSERT(internal::IsValidAxis<GammaAxis>::value,
00160       GAMMA_AXIS_IS_INVALID);
00161       
00162     EIGEN_EULER_ANGLES_CLASS_STATIC_ASSERT((unsigned)AlphaAxisAbs != (unsigned)BetaAxisAbs,
00163       ALPHA_AXIS_CANT_BE_EQUAL_TO_BETA_AXIS);
00164       
00165     EIGEN_EULER_ANGLES_CLASS_STATIC_ASSERT((unsigned)BetaAxisAbs != (unsigned)GammaAxisAbs,
00166       BETA_AXIS_CANT_BE_EQUAL_TO_GAMMA_AXIS);
00167 
00168     enum
00169     {
00170       // I, J, K are the pivot indexes permutation for the rotation matrix, that match this Euler system. 
00171       // They are used in this class converters.
00172       // They are always different from each other, and their possible values are: 0, 1, or 2.
00173       I = AlphaAxisAbs - 1,
00174       J = (AlphaAxisAbs - 1 + 1 + IsOdd)%3,
00175       K = (AlphaAxisAbs - 1 + 2 - IsOdd)%3
00176     };
00177     
00178     // TODO: Get @mat parameter in form that avoids double evaluation.
00179     template <typename Derived>
00180     static void CalcEulerAngles_imp(Matrix<typename MatrixBase<Derived>::Scalar, 3, 1>& res, const MatrixBase<Derived>& mat, internal::true_type /*isTaitBryan*/)
00181     {
00182       using std::atan2;
00183       using std::sin;
00184       using std::cos;
00185       
00186       typedef typename Derived::Scalar Scalar;
00187       typedef Matrix<Scalar,2,1> Vector2;
00188       
00189       res[0] = atan2(mat(J,K), mat(K,K));
00190       Scalar c2 = Vector2(mat(I,I), mat(I,J)).norm();
00191       if((IsOdd && res[0]<Scalar(0)) || ((!IsOdd) && res[0]>Scalar(0))) {
00192         if(res[0] > Scalar(0)) {
00193           res[0] -= Scalar(EIGEN_PI);
00194         }
00195         else {
00196           res[0] += Scalar(EIGEN_PI);
00197         }
00198         res[1] = atan2(-mat(I,K), -c2);
00199       }
00200       else
00201         res[1] = atan2(-mat(I,K), c2);
00202       Scalar s1 = sin(res[0]);
00203       Scalar c1 = cos(res[0]);
00204       res[2] = atan2(s1*mat(K,I)-c1*mat(J,I), c1*mat(J,J) - s1 * mat(K,J));
00205     }
00206 
00207     template <typename Derived>
00208     static void CalcEulerAngles_imp(Matrix<typename MatrixBase<Derived>::Scalar,3,1>& res, const MatrixBase<Derived>& mat, internal::false_type /*isTaitBryan*/)
00209     {
00210       using std::atan2;
00211       using std::sin;
00212       using std::cos;
00213 
00214       typedef typename Derived::Scalar Scalar;
00215       typedef Matrix<Scalar,2,1> Vector2;
00216       
00217       res[0] = atan2(mat(J,I), mat(K,I));
00218       if((IsOdd && res[0]<Scalar(0)) || ((!IsOdd) && res[0]>Scalar(0)))
00219       {
00220         if(res[0] > Scalar(0)) {
00221           res[0] -= Scalar(EIGEN_PI);
00222         }
00223         else {
00224           res[0] += Scalar(EIGEN_PI);
00225         }
00226         Scalar s2 = Vector2(mat(J,I), mat(K,I)).norm();
00227         res[1] = -atan2(s2, mat(I,I));
00228       }
00229       else
00230       {
00231         Scalar s2 = Vector2(mat(J,I), mat(K,I)).norm();
00232         res[1] = atan2(s2, mat(I,I));
00233       }
00234 
00235       // With a=(0,1,0), we have i=0; j=1; k=2, and after computing the first two angles,
00236       // we can compute their respective rotation, and apply its inverse to M. Since the result must
00237       // be a rotation around x, we have:
00238       //
00239       //  c2  s1.s2 c1.s2                   1  0   0 
00240       //  0   c1    -s1       *    M    =   0  c3  s3
00241       //  -s2 s1.c2 c1.c2                   0 -s3  c3
00242       //
00243       //  Thus:  m11.c1 - m21.s1 = c3  &   m12.c1 - m22.s1 = s3
00244 
00245       Scalar s1 = sin(res[0]);
00246       Scalar c1 = cos(res[0]);
00247       res[2] = atan2(c1*mat(J,K)-s1*mat(K,K), c1*mat(J,J) - s1 * mat(K,J));
00248     }
00249     
00250     template<typename Scalar>
00251     static void CalcEulerAngles(
00252       EulerAngles<Scalar, EulerSystem>& res,
00253       const typename EulerAngles<Scalar, EulerSystem>::Matrix3& mat)
00254     {
00255       CalcEulerAngles(res, mat, false, false, false);
00256     }
00257     
00258     template<
00259       bool PositiveRangeAlpha,
00260       bool PositiveRangeBeta,
00261       bool PositiveRangeGamma,
00262       typename Scalar>
00263     static void CalcEulerAngles(
00264       EulerAngles<Scalar, EulerSystem>& res,
00265       const typename EulerAngles<Scalar, EulerSystem>::Matrix3& mat)
00266     {
00267       CalcEulerAngles(res, mat, PositiveRangeAlpha, PositiveRangeBeta, PositiveRangeGamma);
00268     }
00269     
00270     template<typename Scalar>
00271     static void CalcEulerAngles(
00272       EulerAngles<Scalar, EulerSystem>& res,
00273       const typename EulerAngles<Scalar, EulerSystem>::Matrix3& mat,
00274       bool PositiveRangeAlpha,
00275       bool PositiveRangeBeta,
00276       bool PositiveRangeGamma)
00277     {
00278       CalcEulerAngles_imp(
00279         res.angles(), mat,
00280         typename internal::conditional<IsTaitBryan, internal::true_type, internal::false_type>::type());
00281 
00282       if (IsAlphaOpposite == IsOdd)
00283         res.alpha() = -res.alpha();
00284         
00285       if (IsBetaOpposite == IsOdd)
00286         res.beta() = -res.beta();
00287         
00288       if (IsGammaOpposite == IsOdd)
00289         res.gamma() = -res.gamma();
00290       
00291       // Saturate results to the requested range
00292       if (PositiveRangeAlpha && (res.alpha() < 0))
00293         res.alpha() += Scalar(2 * EIGEN_PI);
00294       
00295       if (PositiveRangeBeta && (res.beta() < 0))
00296         res.beta() += Scalar(2 * EIGEN_PI);
00297       
00298       if (PositiveRangeGamma && (res.gamma() < 0))
00299         res.gamma() += Scalar(2 * EIGEN_PI);
00300     }
00301     
00302     template <typename _Scalar, class _System>
00303     friend class Eigen::EulerAngles;
00304   };
00305 
00306 #define EIGEN_EULER_SYSTEM_TYPEDEF(A, B, C) \
00307  \
00308   typedef EulerSystem<EULER_##A, EULER_##B, EULER_##C> EulerSystem##A##B##C;
00309   
00310   EIGEN_EULER_SYSTEM_TYPEDEF(X,Y,Z)
00311   EIGEN_EULER_SYSTEM_TYPEDEF(X,Y,X)
00312   EIGEN_EULER_SYSTEM_TYPEDEF(X,Z,Y)
00313   EIGEN_EULER_SYSTEM_TYPEDEF(X,Z,X)
00314   
00315   EIGEN_EULER_SYSTEM_TYPEDEF(Y,Z,X)
00316   EIGEN_EULER_SYSTEM_TYPEDEF(Y,Z,Y)
00317   EIGEN_EULER_SYSTEM_TYPEDEF(Y,X,Z)
00318   EIGEN_EULER_SYSTEM_TYPEDEF(Y,X,Y)
00319   
00320   EIGEN_EULER_SYSTEM_TYPEDEF(Z,X,Y)
00321   EIGEN_EULER_SYSTEM_TYPEDEF(Z,X,Z)
00322   EIGEN_EULER_SYSTEM_TYPEDEF(Z,Y,X)
00323   EIGEN_EULER_SYSTEM_TYPEDEF(Z,Y,Z)
00324 }
00325 
00326 #endif // EIGEN_EULERSYSTEM_H
 All Classes Functions Variables Typedefs Enumerator