src/EulerAngles/EulerSystem.h

0001 // This file is part of Eigen, a lightweight C++ template library
0002 // for linear algebra.
0003 //
0004 // Copyright (C) 2015 Tal Hadad <tal_hd@hotmail.com>
0005 //
0006 // This Source Code Form is subject to the terms of the Mozilla
0007 // Public License v. 2.0. If a copy of the MPL was not distributed
0008 // with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
0009
0010 #ifndef EIGEN_EULERSYSTEM_H
0011 #define EIGEN_EULERSYSTEM_H
0012
0013 namespace Eigen
0014 {
0015   // Forward declarations
0016   template <typename _Scalar, class _System>
0017   class EulerAngles;
0018
0019   namespace internal
0020   {
0021     // TODO: Add this trait to the Eigen internal API?
0022     template <int Num, bool IsPositive = (Num > 0)>
0023     struct Abs
0024     {
0025       enum { value = Num };
0026     };
0027
0028     template <int Num>
0029     struct Abs<Num, false>
0030     {
0031       enum { value = -Num };
0032     };
0033
0034     template <int Axis>
0035     struct IsValidAxis
0036     {
0037       enum { value = Axis != 0 && Abs<Axis>::value <= 3 };
0038     };
0039
0040     template<typename System,
0041             typename Other,
0042             int OtherRows=Other::RowsAtCompileTime,
0043             int OtherCols=Other::ColsAtCompileTime>
0044     struct eulerangles_assign_impl;
0045   }
0046
0047   #define EIGEN_EULER_ANGLES_CLASS_STATIC_ASSERT(COND,MSG) typedef char static_assertion_##MSG[(COND)?1:-1]
0048
0049   /** \brief Representation of a fixed signed rotation axis for EulerSystem.
0050     *
0051     * \ingroup EulerAngles_Module
0052     *
0053     * Values here represent:
0054     *  - The axis of the rotation: X, Y or Z.
0055     *  - The sign (i.e. direction of the rotation along the axis): positive(+) or negative(-)
0056     *
0057     * Therefore, this could express all the axes {+X,+Y,+Z,-X,-Y,-Z}
0058     *
0059     * For positive axis, use +EULER_{axis}, and for negative axis use -EULER_{axis}.
0060     */
0061   enum EulerAxis
0062   {
0063     EULER_X = 1, /*!< the X axis */
0064     EULER_Y = 2, /*!< the Y axis */
0065     EULER_Z = 3  /*!< the Z axis */
0066   };
0067
0068   /** \class EulerSystem
0069     *
0070     * \ingroup EulerAngles_Module
0071     *
0072     * \brief Represents a fixed Euler rotation system.
0073     *
0074     * This meta-class goal is to represent the Euler system in compilation time, for EulerAngles.
0075     *
0076     * You can use this class to get two things:
0077     *  - Build an Euler system, and then pass it as a template parameter to EulerAngles.
0078     *  - Query some compile time data about an Euler system. (e.g. Whether it's Tait-Bryan)
0079     *
0080     * Euler rotation is a set of three rotation on fixed axes. (see \ref EulerAngles)
0081     * This meta-class store constantly those signed axes. (see \ref EulerAxis)
0082     *
0083     * ### Types of Euler systems ###
0084     *
0085     * All and only valid 3 dimension Euler rotation over standard
0086     *  signed axes{+X,+Y,+Z,-X,-Y,-Z} are supported:
0087     *  - all axes X, Y, Z in each valid order (see below what order is valid)
0088     *  - rotation over the axis is supported both over the positive and negative directions.
0089     *  - both Tait-Bryan and proper/classic Euler angles (i.e. the opposite).
0090     *
0091     * Since EulerSystem support both positive and negative directions,
0092     *  you may call this rotation distinction in other names:
0093     *  - _right handed_ or _left handed_
0094     *  - _counterclockwise_ or _clockwise_
0095     *
0096     * Notice all axed combination are valid, and would trigger a static assertion.
0097     * Same unsigned axes can't be neighbors, e.g. {X,X,Y} is invalid.
0098     * This yield two and only two classes:
0099     *  - _Tait-Bryan_ - all unsigned axes are distinct, e.g. {X,Y,Z}
0100     *  - _proper/classic Euler angles_ - The first and the third unsigned axes is equal,
0101     *     and the second is different, e.g. {X,Y,X}
0102     *
0103     * ### Intrinsic vs extrinsic Euler systems ###
0104     *
0105     * Only intrinsic Euler systems are supported for simplicity.
0106     *  If you want to use extrinsic Euler systems,
0107     *   just use the equal intrinsic opposite order for axes and angles.
0108     *  I.e axes (A,B,C) becomes (C,B,A), and angles (a,b,c) becomes (c,b,a).
0109     *
0110     * ### Convenient user typedefs ###
0111     *
0112     * Convenient typedefs for EulerSystem exist (only for positive axes Euler systems),
0113     *  in a form of EulerSystem{A}{B}{C}, e.g. \ref EulerSystemXYZ.
0114     *
0115     * ### Additional reading ###
0116     *
0117     * More information about Euler angles: https://en.wikipedia.org/wiki/Euler_angles
0118     *
0119     * \tparam _AlphaAxis the first fixed EulerAxis
0120     *
0121     * \tparam _BetaAxis the second fixed EulerAxis
0122     *
0123     * \tparam _GammaAxis the third fixed EulerAxis
0124     */
0125   template <int _AlphaAxis, int _BetaAxis, int _GammaAxis>
0126   class EulerSystem
0127   {
0128     public:
0129     // It's defined this way and not as enum, because I think
0130     //  that enum is not guerantee to support negative numbers
0131
0132     /** The first rotation axis */
0133     static const int AlphaAxis = _AlphaAxis;
0134
0135     /** The second rotation axis */
0136     static const int BetaAxis = _BetaAxis;
0137
0138     /** The third rotation axis */
0139     static const int GammaAxis = _GammaAxis;
0140
0141     enum
0142     {
0143       AlphaAxisAbs = internal::Abs<AlphaAxis>::value, /*!< the first rotation axis unsigned */
0144       BetaAxisAbs = internal::Abs<BetaAxis>::value, /*!< the second rotation axis unsigned */
0145       GammaAxisAbs = internal::Abs<GammaAxis>::value, /*!< the third rotation axis unsigned */
0146
0147       IsAlphaOpposite = (AlphaAxis < 0) ? 1 : 0, /*!< whether alpha axis is negative */
0148       IsBetaOpposite = (BetaAxis < 0) ? 1 : 0, /*!< whether beta axis is negative */
0149       IsGammaOpposite = (GammaAxis < 0) ? 1 : 0, /*!< whether gamma axis is negative */
0150
0151       // Parity is even if alpha axis X is followed by beta axis Y, or Y is followed
0152       // by Z, or Z is followed by X; otherwise it is odd.
0153       IsOdd = ((AlphaAxisAbs)%3 == (BetaAxisAbs - 1)%3) ? 0 : 1, /*!< whether the Euler system is odd */
0154       IsEven = IsOdd ? 0 : 1, /*!< whether the Euler system is even */
0155
0156       IsTaitBryan = ((unsigned)AlphaAxisAbs != (unsigned)GammaAxisAbs) ? 1 : 0 /*!< whether the Euler system is Tait-Bryan */
0157     };
0158
0159     private:
0160
0161     EIGEN_EULER_ANGLES_CLASS_STATIC_ASSERT(internal::IsValidAxis<AlphaAxis>::value,
0162       ALPHA_AXIS_IS_INVALID);
0163
0164     EIGEN_EULER_ANGLES_CLASS_STATIC_ASSERT(internal::IsValidAxis<BetaAxis>::value,
0165       BETA_AXIS_IS_INVALID);
0166
0167     EIGEN_EULER_ANGLES_CLASS_STATIC_ASSERT(internal::IsValidAxis<GammaAxis>::value,
0168       GAMMA_AXIS_IS_INVALID);
0169
0170     EIGEN_EULER_ANGLES_CLASS_STATIC_ASSERT((unsigned)AlphaAxisAbs != (unsigned)BetaAxisAbs,
0171       ALPHA_AXIS_CANT_BE_EQUAL_TO_BETA_AXIS);
0172
0173     EIGEN_EULER_ANGLES_CLASS_STATIC_ASSERT((unsigned)BetaAxisAbs != (unsigned)GammaAxisAbs,
0174       BETA_AXIS_CANT_BE_EQUAL_TO_GAMMA_AXIS);
0175
0176     static const int
0177       // I, J, K are the pivot indexes permutation for the rotation matrix, that match this Euler system.
0178       // They are used in this class converters.
0179       // They are always different from each other, and their possible values are: 0, 1, or 2.
0180       I_ = AlphaAxisAbs - 1,
0181       J_ = (AlphaAxisAbs - 1 + 1 + IsOdd)%3,
0182       K_ = (AlphaAxisAbs - 1 + 2 - IsOdd)%3
0183     ;
0184
0185     // TODO: Get @mat parameter in form that avoids double evaluation.
0186     template <typename Derived>
0187     static void CalcEulerAngles_imp(Matrix<typename MatrixBase<Derived>::Scalar, 3, 1>& res, const MatrixBase<Derived>& mat, internal::true_type /*isTaitBryan*/)
0188     {
0189       using std::atan2;
0190       using std::sqrt;
0191
0192       typedef typename Derived::Scalar Scalar;
0193
0194       const Scalar plusMinus = IsEven? 1 : -1;
0195       const Scalar minusPlus = IsOdd?  1 : -1;
0196
0197       const Scalar Rsum = sqrt((mat(I_,I_) * mat(I_,I_) + mat(I_,J_) * mat(I_,J_) + mat(J_,K_) * mat(J_,K_) + mat(K_,K_) * mat(K_,K_))/2);
0198       res[1] = atan2(plusMinus * mat(I_,K_), Rsum);
0199
0200       // There is a singularity when cos(beta) == 0
0201       if(Rsum > 4 * NumTraits<Scalar>::epsilon()) {// cos(beta) != 0
0202         res[0] = atan2(minusPlus * mat(J_, K_), mat(K_, K_));
0203         res[2] = atan2(minusPlus * mat(I_, J_), mat(I_, I_));
0204       }
0205       else if(plusMinus * mat(I_, K_) > 0) {// cos(beta) == 0 and sin(beta) == 1
0206         Scalar spos = mat(J_, I_) + plusMinus * mat(K_, J_); // 2*sin(alpha + plusMinus * gamma
0207         Scalar cpos = mat(J_, J_) + minusPlus * mat(K_, I_); // 2*cos(alpha + plusMinus * gamma)
0208         Scalar alphaPlusMinusGamma = atan2(spos, cpos);
0209         res[0] = alphaPlusMinusGamma;
0210         res[2] = 0;
0211       }
0212       else {// cos(beta) == 0 and sin(beta) == -1
0213         Scalar sneg = plusMinus * (mat(K_, J_) + minusPlus * mat(J_, I_)); // 2*sin(alpha + minusPlus*gamma)
0214         Scalar cneg = mat(J_, J_) + plusMinus * mat(K_, I_);               // 2*cos(alpha + minusPlus*gamma)
0215         Scalar alphaMinusPlusBeta = atan2(sneg, cneg);
0216         res[0] = alphaMinusPlusBeta;
0217         res[2] = 0;
0218       }
0219     }
0220
0221     template <typename Derived>
0222     static void CalcEulerAngles_imp(Matrix<typename MatrixBase<Derived>::Scalar,3,1>& res,
0223                                     const MatrixBase<Derived>& mat, internal::false_type /*isTaitBryan*/)
0224     {
0225       using std::atan2;
0226       using std::sqrt;
0227
0228       typedef typename Derived::Scalar Scalar;
0229
0230       const Scalar plusMinus = IsEven? 1 : -1;
0231       const Scalar minusPlus = IsOdd?  1 : -1;
0232
0233       const Scalar Rsum = sqrt((mat(I_, J_) * mat(I_, J_) + mat(I_, K_) * mat(I_, K_) + mat(J_, I_) * mat(J_, I_) + mat(K_, I_) * mat(K_, I_)) / 2);
0234
0235       res[1] = atan2(Rsum, mat(I_, I_));
0236
0237       // There is a singularity when sin(beta) == 0
0238       if(Rsum > 4 * NumTraits<Scalar>::epsilon()) {// sin(beta) != 0
0239         res[0] = atan2(mat(J_, I_), minusPlus * mat(K_, I_));
0240         res[2] = atan2(mat(I_, J_), plusMinus * mat(I_, K_));
0241       }
0242       else if(mat(I_, I_) > 0) {// sin(beta) == 0 and cos(beta) == 1
0243         Scalar spos = plusMinus * mat(K_, J_) + minusPlus * mat(J_, K_); // 2*sin(alpha + gamma)
0244         Scalar cpos = mat(J_, J_) + mat(K_, K_);                         // 2*cos(alpha + gamma)
0245         res[0] = atan2(spos, cpos);
0246         res[2] = 0;
0247       }
0248       else {// sin(beta) == 0 and cos(beta) == -1
0249         Scalar sneg = plusMinus * mat(K_, J_) + plusMinus * mat(J_, K_); // 2*sin(alpha - gamma)
0250         Scalar cneg = mat(J_, J_) - mat(K_, K_);                         // 2*cos(alpha - gamma)
0251         res[0] = atan2(sneg, cneg);
0252         res[2] = 0;
0253       }
0254     }
0255
0256     template<typename Scalar>
0257     static void CalcEulerAngles(
0258       EulerAngles<Scalar, EulerSystem>& res,
0259       const typename EulerAngles<Scalar, EulerSystem>::Matrix3& mat)
0260     {
0261       CalcEulerAngles_imp(
0262         res.angles(), mat,
0263         typename internal::conditional<IsTaitBryan, internal::true_type, internal::false_type>::type());
0264
0265       if (IsAlphaOpposite)
0266         res.alpha() = -res.alpha();
0267
0268       if (IsBetaOpposite)
0269         res.beta() = -res.beta();
0270
0271       if (IsGammaOpposite)
0272         res.gamma() = -res.gamma();
0273     }
0274
0275     template <typename _Scalar, class _System>
0276     friend class Eigen::EulerAngles;
0277
0278     template<typename System,
0279             typename Other,
0280             int OtherRows,
0281             int OtherCols>
0282     friend struct internal::eulerangles_assign_impl;
0283   };
0284
0285 #define EIGEN_EULER_SYSTEM_TYPEDEF(A, B, C) \
0286   /** \ingroup EulerAngles_Module */ \
0287   typedef EulerSystem<EULER_##A, EULER_##B, EULER_##C> EulerSystem##A##B##C;
0288
0289   EIGEN_EULER_SYSTEM_TYPEDEF(X,Y,Z)
0290   EIGEN_EULER_SYSTEM_TYPEDEF(X,Y,X)
0291   EIGEN_EULER_SYSTEM_TYPEDEF(X,Z,Y)
0292   EIGEN_EULER_SYSTEM_TYPEDEF(X,Z,X)
0293
0294   EIGEN_EULER_SYSTEM_TYPEDEF(Y,Z,X)
0295   EIGEN_EULER_SYSTEM_TYPEDEF(Y,Z,Y)
0296   EIGEN_EULER_SYSTEM_TYPEDEF(Y,X,Z)
0297   EIGEN_EULER_SYSTEM_TYPEDEF(Y,X,Y)
0298
0299   EIGEN_EULER_SYSTEM_TYPEDEF(Z,X,Y)
0300   EIGEN_EULER_SYSTEM_TYPEDEF(Z,X,Z)
0301   EIGEN_EULER_SYSTEM_TYPEDEF(Z,Y,X)
0302   EIGEN_EULER_SYSTEM_TYPEDEF(Z,Y,Z)
0303 }
0304
0305 #endif // EIGEN_EULERSYSTEM_H