| // This file is part of Eigen, a lightweight C++ template library |
| // for linear algebra. |
| // |
| // Copyright (C) 2015 Tal Hadad <tal_hd@hotmail.com> |
| // |
| // This Source Code Form is subject to the terms of the Mozilla |
| // Public License v. 2.0. If a copy of the MPL was not distributed |
| // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. |
| |
| #ifndef EIGEN_EULERSYSTEM_H |
| #define EIGEN_EULERSYSTEM_H |
| |
| namespace Eigen |
| { |
| // Forward declerations |
| template <typename _Scalar, class _System> |
| class EulerAngles; |
| |
| namespace internal |
| { |
| // TODO: Check if already exists on the rest API |
| template <int Num, bool IsPositive = (Num > 0)> |
| struct Abs |
| { |
| enum { value = Num }; |
| }; |
| |
| template <int Num> |
| struct Abs<Num, false> |
| { |
| enum { value = -Num }; |
| }; |
| |
| template <int Axis> |
| struct IsValidAxis |
| { |
| enum { value = Axis != 0 && Abs<Axis>::value <= 3 }; |
| }; |
| } |
| |
| #define EIGEN_EULER_ANGLES_CLASS_STATIC_ASSERT(COND,MSG) typedef char static_assertion_##MSG[(COND)?1:-1] |
| |
| /** \brief Representation of a fixed signed rotation axis for EulerSystem. |
| * |
| * \ingroup EulerAngles_Module |
| * |
| * Values here represent: |
| * - The axis of the rotation: X, Y or Z. |
| * - The sign (i.e. direction of the rotation along the axis): positive(+) or negative(-) |
| * |
| * Therefore, this could express all the axes {+X,+Y,+Z,-X,-Y,-Z} |
| * |
| * For positive axis, use +EULER_{axis}, and for negative axis use -EULER_{axis}. |
| */ |
| enum EulerAxis |
| { |
| EULER_X = 1, /*!< the X axis */ |
| EULER_Y = 2, /*!< the Y axis */ |
| EULER_Z = 3 /*!< the Z axis */ |
| }; |
| |
| /** \class EulerSystem |
| * |
| * \ingroup EulerAngles_Module |
| * |
| * \brief Represents a fixed Euler rotation system. |
| * |
| * This meta-class goal is to represent the Euler system in compilation time, for EulerAngles. |
| * |
| * You can use this class to get two things: |
| * - Build an Euler system, and then pass it as a template parameter to EulerAngles. |
| * - Query some compile time data about an Euler system. (e.g. Whether it's tait bryan) |
| * |
| * Euler rotation is a set of three rotation on fixed axes. (see \ref EulerAngles) |
| * This meta-class store constantly those signed axes. (see \ref EulerAxis) |
| * |
| * ### Types of Euler systems ### |
| * |
| * All and only valid 3 dimension Euler rotation over standard |
| * signed axes{+X,+Y,+Z,-X,-Y,-Z} are supported: |
| * - all axes X, Y, Z in each valid order (see below what order is valid) |
| * - rotation over the axis is supported both over the positive and negative directions. |
| * - both tait bryan and proper/classic Euler angles (i.e. the opposite). |
| * |
| * Since EulerSystem support both positive and negative directions, |
| * you may call this rotation distinction in other names: |
| * - _right handed_ or _left handed_ |
| * - _counterclockwise_ or _clockwise_ |
| * |
| * Notice all axed combination are valid, and would trigger a static assertion. |
| * Same unsigned axes can't be neighbors, e.g. {X,X,Y} is invalid. |
| * This yield two and only two classes: |
| * - _tait bryan_ - all unsigned axes are distinct, e.g. {X,Y,Z} |
| * - _proper/classic Euler angles_ - The first and the third unsigned axes is equal, |
| * and the second is different, e.g. {X,Y,X} |
| * |
| * ### Intrinsic vs extrinsic Euler systems ### |
| * |
| * Only intrinsic Euler systems are supported for simplicity. |
| * If you want to use extrinsic Euler systems, |
| * just use the equal intrinsic opposite order for axes and angles. |
| * I.e axes (A,B,C) becomes (C,B,A), and angles (a,b,c) becomes (c,b,a). |
| * |
| * ### Convenient user typedefs ### |
| * |
| * Convenient typedefs for EulerSystem exist (only for positive axes Euler systems), |
| * in a form of EulerSystem{A}{B}{C}, e.g. \ref EulerSystemXYZ. |
| * |
| * ### Additional reading ### |
| * |
| * More information about Euler angles: https://en.wikipedia.org/wiki/Euler_angles |
| * |
| * \tparam _AlphaAxis the first fixed EulerAxis |
| * |
| * \tparam _AlphaAxis the second fixed EulerAxis |
| * |
| * \tparam _AlphaAxis the third fixed EulerAxis |
| */ |
| template <int _AlphaAxis, int _BetaAxis, int _GammaAxis> |
| class EulerSystem |
| { |
| public: |
| // It's defined this way and not as enum, because I think |
| // that enum is not guerantee to support negative numbers |
| |
| /** The first rotation axis */ |
| static const int AlphaAxis = _AlphaAxis; |
| |
| /** The second rotation axis */ |
| static const int BetaAxis = _BetaAxis; |
| |
| /** The third rotation axis */ |
| static const int GammaAxis = _GammaAxis; |
| |
| enum |
| { |
| AlphaAxisAbs = internal::Abs<AlphaAxis>::value, /*!< the first rotation axis unsigned */ |
| BetaAxisAbs = internal::Abs<BetaAxis>::value, /*!< the second rotation axis unsigned */ |
| GammaAxisAbs = internal::Abs<GammaAxis>::value, /*!< the third rotation axis unsigned */ |
| |
| IsAlphaOpposite = (AlphaAxis < 0) ? 1 : 0, /*!< weather alpha axis is negative */ |
| IsBetaOpposite = (BetaAxis < 0) ? 1 : 0, /*!< weather beta axis is negative */ |
| IsGammaOpposite = (GammaAxis < 0) ? 1 : 0, /*!< weather gamma axis is negative */ |
| |
| IsOdd = ((AlphaAxisAbs)%3 == (BetaAxisAbs - 1)%3) ? 0 : 1, /*!< weather the Euler system is odd */ |
| IsEven = IsOdd ? 0 : 1, /*!< weather the Euler system is even */ |
| |
| IsTaitBryan = ((unsigned)AlphaAxisAbs != (unsigned)GammaAxisAbs) ? 1 : 0 /*!< weather the Euler system is tait bryan */ |
| }; |
| |
| private: |
| |
| EIGEN_EULER_ANGLES_CLASS_STATIC_ASSERT(internal::IsValidAxis<AlphaAxis>::value, |
| ALPHA_AXIS_IS_INVALID); |
| |
| EIGEN_EULER_ANGLES_CLASS_STATIC_ASSERT(internal::IsValidAxis<BetaAxis>::value, |
| BETA_AXIS_IS_INVALID); |
| |
| EIGEN_EULER_ANGLES_CLASS_STATIC_ASSERT(internal::IsValidAxis<GammaAxis>::value, |
| GAMMA_AXIS_IS_INVALID); |
| |
| EIGEN_EULER_ANGLES_CLASS_STATIC_ASSERT((unsigned)AlphaAxisAbs != (unsigned)BetaAxisAbs, |
| ALPHA_AXIS_CANT_BE_EQUAL_TO_BETA_AXIS); |
| |
| EIGEN_EULER_ANGLES_CLASS_STATIC_ASSERT((unsigned)BetaAxisAbs != (unsigned)GammaAxisAbs, |
| BETA_AXIS_CANT_BE_EQUAL_TO_GAMMA_AXIS); |
| |
| enum |
| { |
| // I, J, K are the pivot indexes permutation for the rotation matrix, that match this Euler system. |
| // They are used in this class converters. |
| // They are always different from each other, and their possible values are: 0, 1, or 2. |
| I = AlphaAxisAbs - 1, |
| J = (AlphaAxisAbs - 1 + 1 + IsOdd)%3, |
| K = (AlphaAxisAbs - 1 + 2 - IsOdd)%3 |
| }; |
| |
| // TODO: Get @mat parameter in form that avoids double evaluation. |
| template <typename Derived> |
| static void CalcEulerAngles_imp(Matrix<typename MatrixBase<Derived>::Scalar, 3, 1>& res, const MatrixBase<Derived>& mat, internal::true_type /*isTaitBryan*/) |
| { |
| using std::atan2; |
| using std::sin; |
| using std::cos; |
| |
| typedef typename Derived::Scalar Scalar; |
| typedef Matrix<Scalar,2,1> Vector2; |
| |
| res[0] = atan2(mat(J,K), mat(K,K)); |
| Scalar c2 = Vector2(mat(I,I), mat(I,J)).norm(); |
| if((IsOdd && res[0]<Scalar(0)) || ((!IsOdd) && res[0]>Scalar(0))) { |
| if(res[0] > Scalar(0)) { |
| res[0] -= Scalar(EIGEN_PI); |
| } |
| else { |
| res[0] += Scalar(EIGEN_PI); |
| } |
| res[1] = atan2(-mat(I,K), -c2); |
| } |
| else |
| res[1] = atan2(-mat(I,K), c2); |
| Scalar s1 = sin(res[0]); |
| Scalar c1 = cos(res[0]); |
| res[2] = atan2(s1*mat(K,I)-c1*mat(J,I), c1*mat(J,J) - s1 * mat(K,J)); |
| } |
| |
| template <typename Derived> |
| static void CalcEulerAngles_imp(Matrix<typename MatrixBase<Derived>::Scalar,3,1>& res, const MatrixBase<Derived>& mat, internal::false_type /*isTaitBryan*/) |
| { |
| using std::atan2; |
| using std::sin; |
| using std::cos; |
| |
| typedef typename Derived::Scalar Scalar; |
| typedef Matrix<Scalar,2,1> Vector2; |
| |
| res[0] = atan2(mat(J,I), mat(K,I)); |
| if((IsOdd && res[0]<Scalar(0)) || ((!IsOdd) && res[0]>Scalar(0))) |
| { |
| if(res[0] > Scalar(0)) { |
| res[0] -= Scalar(EIGEN_PI); |
| } |
| else { |
| res[0] += Scalar(EIGEN_PI); |
| } |
| Scalar s2 = Vector2(mat(J,I), mat(K,I)).norm(); |
| res[1] = -atan2(s2, mat(I,I)); |
| } |
| else |
| { |
| Scalar s2 = Vector2(mat(J,I), mat(K,I)).norm(); |
| res[1] = atan2(s2, mat(I,I)); |
| } |
| |
| // With a=(0,1,0), we have i=0; j=1; k=2, and after computing the first two angles, |
| // we can compute their respective rotation, and apply its inverse to M. Since the result must |
| // be a rotation around x, we have: |
| // |
| // c2 s1.s2 c1.s2 1 0 0 |
| // 0 c1 -s1 * M = 0 c3 s3 |
| // -s2 s1.c2 c1.c2 0 -s3 c3 |
| // |
| // Thus: m11.c1 - m21.s1 = c3 & m12.c1 - m22.s1 = s3 |
| |
| Scalar s1 = sin(res[0]); |
| Scalar c1 = cos(res[0]); |
| res[2] = atan2(c1*mat(J,K)-s1*mat(K,K), c1*mat(J,J) - s1 * mat(K,J)); |
| } |
| |
| template<typename Scalar> |
| static void CalcEulerAngles( |
| EulerAngles<Scalar, EulerSystem>& res, |
| const typename EulerAngles<Scalar, EulerSystem>::Matrix3& mat) |
| { |
| CalcEulerAngles(res, mat, false, false, false); |
| } |
| |
| template< |
| bool PositiveRangeAlpha, |
| bool PositiveRangeBeta, |
| bool PositiveRangeGamma, |
| typename Scalar> |
| static void CalcEulerAngles( |
| EulerAngles<Scalar, EulerSystem>& res, |
| const typename EulerAngles<Scalar, EulerSystem>::Matrix3& mat) |
| { |
| CalcEulerAngles(res, mat, PositiveRangeAlpha, PositiveRangeBeta, PositiveRangeGamma); |
| } |
| |
| template<typename Scalar> |
| static void CalcEulerAngles( |
| EulerAngles<Scalar, EulerSystem>& res, |
| const typename EulerAngles<Scalar, EulerSystem>::Matrix3& mat, |
| bool PositiveRangeAlpha, |
| bool PositiveRangeBeta, |
| bool PositiveRangeGamma) |
| { |
| CalcEulerAngles_imp( |
| res.angles(), mat, |
| typename internal::conditional<IsTaitBryan, internal::true_type, internal::false_type>::type()); |
| |
| if (IsAlphaOpposite == IsOdd) |
| res.alpha() = -res.alpha(); |
| |
| if (IsBetaOpposite == IsOdd) |
| res.beta() = -res.beta(); |
| |
| if (IsGammaOpposite == IsOdd) |
| res.gamma() = -res.gamma(); |
| |
| // Saturate results to the requested range |
| if (PositiveRangeAlpha && (res.alpha() < 0)) |
| res.alpha() += Scalar(2 * EIGEN_PI); |
| |
| if (PositiveRangeBeta && (res.beta() < 0)) |
| res.beta() += Scalar(2 * EIGEN_PI); |
| |
| if (PositiveRangeGamma && (res.gamma() < 0)) |
| res.gamma() += Scalar(2 * EIGEN_PI); |
| } |
| |
| template <typename _Scalar, class _System> |
| friend class Eigen::EulerAngles; |
| }; |
| |
| #define EIGEN_EULER_SYSTEM_TYPEDEF(A, B, C) \ |
| /** \ingroup EulerAngles_Module */ \ |
| typedef EulerSystem<EULER_##A, EULER_##B, EULER_##C> EulerSystem##A##B##C; |
| |
| EIGEN_EULER_SYSTEM_TYPEDEF(X,Y,Z) |
| EIGEN_EULER_SYSTEM_TYPEDEF(X,Y,X) |
| EIGEN_EULER_SYSTEM_TYPEDEF(X,Z,Y) |
| EIGEN_EULER_SYSTEM_TYPEDEF(X,Z,X) |
| |
| EIGEN_EULER_SYSTEM_TYPEDEF(Y,Z,X) |
| EIGEN_EULER_SYSTEM_TYPEDEF(Y,Z,Y) |
| EIGEN_EULER_SYSTEM_TYPEDEF(Y,X,Z) |
| EIGEN_EULER_SYSTEM_TYPEDEF(Y,X,Y) |
| |
| EIGEN_EULER_SYSTEM_TYPEDEF(Z,X,Y) |
| EIGEN_EULER_SYSTEM_TYPEDEF(Z,X,Z) |
| EIGEN_EULER_SYSTEM_TYPEDEF(Z,Y,X) |
| EIGEN_EULER_SYSTEM_TYPEDEF(Z,Y,Z) |
| } |
| |
| #endif // EIGEN_EULERSYSTEM_H |
