diff --git a/unsupported/Eigen/src/EulerAngles/EulerAngles.h b/unsupported/Eigen/src/EulerAngles/EulerAngles.h index 13a0da1ab..da86cc13b 100644 --- a/unsupported/Eigen/src/EulerAngles/EulerAngles.h +++ b/unsupported/Eigen/src/EulerAngles/EulerAngles.h @@ -55,33 +55,25 @@ namespace Eigen * Additionally, some axes related computation is done in compile time. * * #### Euler angles ranges in conversions #### + * Rotations representation as EulerAngles are not singular (unlike matrices), and even have infinite EulerAngles representations.
+ * For example, add or subtract 2*PI from either angle of EulerAngles + * and you'll get the same rotation. + * This is the reason for infinite representation, but it's not the only reason for non-singularity. * - * When converting some rotation to Euler angles, there are some ways you can guarantee - * the Euler angles ranges. + * When converting rotation to EulerAngles, this class convert it to specific ranges + * When converting some rotation to EulerAngles, the rules for ranges are as follow: + * - If the rotation we converting from is an EulerAngles + * (even when it represented as RotationBase explicitly), angles ranges are __undefined__. + * - otherwise, Alpha and Gamma angles will be in the range [-PI, PI].
+ * As for Beta angle: + * - If the system is Tait-Bryan, the beta angle will be in the range [-PI, PI]. + * - otherwise: + * - If the beta axis is positive, the beta angle will be in the range [0, 2*PI] + * - If the beta axis is negative, the beta angle will be in the range [-2*PI, 0] * - * #### implicit ranges #### - * When using implicit ranges, all angles are guarantee to be in the range [-PI, +PI], - * unless you convert from some other Euler angles. - * In this case, the range is __undefined__ (might be even less than -PI or greater than +2*PI). * \sa EulerAngles(const MatrixBase&) * \sa EulerAngles(const RotationBase&) * - * #### explicit ranges #### - * When using explicit ranges, all angles are guarantee to be in the range you choose. - * In the range Boolean parameter, you're been ask whether you prefer the positive range or not: - * - _true_ - force the range between [0, +2*PI] - * - _false_ - force the range between [-PI, +PI] - * - * ##### compile time ranges ##### - * This is when you have compile time ranges and you prefer to - * use template parameter. (e.g. for performance) - * \sa FromRotation() - * - * ##### run-time time ranges ##### - * Run-time ranges are also supported. - * \sa EulerAngles(const MatrixBase&, bool, bool, bool) - * \sa EulerAngles(const RotationBase&, bool, bool, bool) - * * ### Convenient user typedefs ### * * Convenient typedefs for EulerAngles exist for float and double scalar, @@ -152,61 +144,43 @@ namespace Eigen /** Constructs and initialize Euler angles from a 3x3 rotation matrix \p m. * - * \note All angles will be in the range [-PI, PI]. + * \note Alpha and Gamma angles will be in the range [-PI, PI].
+ * As for Beta angle: + * - If the system is Tait-Bryan, the beta angle will be in the range [-PI, PI]. + * - otherwise: + * - If the beta axis is positive, the beta angle will be in the range [0, 2*PI] + * - If the beta axis is negative, the beta angle will be in the range [-2*PI, 0] */ template - EulerAngles(const MatrixBase& m) { *this = m; } - - /** Constructs and initialize Euler angles from a 3x3 rotation matrix \p m, - * with options to choose for each angle the requested range. - * - * If positive range is true, then the specified angle will be in the range [0, +2*PI]. - * Otherwise, the specified angle will be in the range [-PI, +PI]. - * - * \param m The 3x3 rotation matrix to convert - * \param positiveRangeAlpha If true, alpha will be in [0, 2*PI]. Otherwise, in [-PI, +PI]. - * \param positiveRangeBeta If true, beta will be in [0, 2*PI]. Otherwise, in [-PI, +PI]. - * \param positiveRangeGamma If true, gamma will be in [0, 2*PI]. Otherwise, in [-PI, +PI]. - */ - template - EulerAngles( - const MatrixBase& m, - bool positiveRangeAlpha, - bool positiveRangeBeta, - bool positiveRangeGamma) { - - System::CalcEulerAngles(*this, m, positiveRangeAlpha, positiveRangeBeta, positiveRangeGamma); - } + EulerAngles(const MatrixBase& m) { System::CalcEulerAngles(*this, m); } /** Constructs and initialize Euler angles from a rotation \p rot. * - * \note All angles will be in the range [-PI, PI], unless \p rot is an EulerAngles. - * If rot is an EulerAngles, expected EulerAngles range is __undefined__. - * (Use other functions here for enforcing range if this effect is desired) + * \note If \p rot is an EulerAngles (even when it represented as RotationBase explicitly), + * angles ranges are __undefined__. + * Otherwise, Alpha and Gamma angles will be in the range [-PI, PI].
+ * As for Beta angle: + * - If the system is Tait-Bryan, the beta angle will be in the range [-PI, PI]. + * - otherwise: + * - If the beta axis is positive, the beta angle will be in the range [0, 2*PI] + * - If the beta axis is negative, the beta angle will be in the range [-2*PI, 0] */ template - EulerAngles(const RotationBase& rot) { *this = rot; } + EulerAngles(const RotationBase& rot) { System::CalcEulerAngles(*this, rot.toRotationMatrix()); } - /** Constructs and initialize Euler angles from a rotation \p rot, - * with options to choose for each angle the requested range. - * - * If positive range is true, then the specified angle will be in the range [0, +2*PI]. - * Otherwise, the specified angle will be in the range [-PI, +PI]. - * - * \param rot The 3x3 rotation matrix to convert - * \param positiveRangeAlpha If true, alpha will be in [0, 2*PI]. Otherwise, in [-PI, +PI]. - * \param positiveRangeBeta If true, beta will be in [0, 2*PI]. Otherwise, in [-PI, +PI]. - * \param positiveRangeGamma If true, gamma will be in [0, 2*PI]. Otherwise, in [-PI, +PI]. - */ - template - EulerAngles( - const RotationBase& rot, - bool positiveRangeAlpha, - bool positiveRangeBeta, - bool positiveRangeGamma) { - - System::CalcEulerAngles(*this, rot.toRotationMatrix(), positiveRangeAlpha, positiveRangeBeta, positiveRangeGamma); - } + /*EulerAngles(const QuaternionType& q) + { + // TODO: Implement it in a faster way for quaternions + // According to http://www.euclideanspace.com/maths/geometry/rotations/conversions/quaternionToEuler/ + // we can compute only the needed matrix cells and then convert to euler angles. (see ZYX example below) + // Currently we compute all matrix cells from quaternion. + + // Special case only for ZYX + //Scalar y2 = q.y() * q.y(); + //m_angles[0] = std::atan2(2*(q.w()*q.z() + q.x()*q.y()), (1 - 2*(y2 + q.z()*q.z()))); + //m_angles[1] = std::asin( 2*(q.w()*q.y() - q.z()*q.x())); + //m_angles[2] = std::atan2(2*(q.w()*q.x() + q.y()*q.z()), (1 - 2*(q.x()*q.x() + y2))); + }*/ /** \returns The angle values stored in a vector (alpha, beta, gamma). */ const Vector3& angles() const { return m_angles; } @@ -246,68 +220,11 @@ namespace Eigen return inverse(); } - /** Constructs and initialize Euler angles from a 3x3 rotation matrix \p m, - * with options to choose for each angle the requested range (__only in compile time__). + /** Set \c *this from a rotation matrix(i.e. pure orthogonal matrix with determinant of +1). * - * If positive range is true, then the specified angle will be in the range [0, +2*PI]. - * Otherwise, the specified angle will be in the range [-PI, +PI]. - * - * \param m The 3x3 rotation matrix to convert - * \tparam positiveRangeAlpha If true, alpha will be in [0, 2*PI]. Otherwise, in [-PI, +PI]. - * \tparam positiveRangeBeta If true, beta will be in [0, 2*PI]. Otherwise, in [-PI, +PI]. - * \tparam positiveRangeGamma If true, gamma will be in [0, 2*PI]. Otherwise, in [-PI, +PI]. - */ - template< - bool PositiveRangeAlpha, - bool PositiveRangeBeta, - bool PositiveRangeGamma, - typename Derived> - static EulerAngles FromRotation(const MatrixBase& m) - { - EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Derived, 3, 3) - - EulerAngles e; - System::template CalcEulerAngles< - PositiveRangeAlpha, PositiveRangeBeta, PositiveRangeGamma, _Scalar>(e, m); - return e; - } - - /** Constructs and initialize Euler angles from a rotation \p rot, - * with options to choose for each angle the requested range (__only in compile time__). - * - * If positive range is true, then the specified angle will be in the range [0, +2*PI]. - * Otherwise, the specified angle will be in the range [-PI, +PI]. - * - * \param rot The 3x3 rotation matrix to convert - * \tparam positiveRangeAlpha If true, alpha will be in [0, 2*PI]. Otherwise, in [-PI, +PI]. - * \tparam positiveRangeBeta If true, beta will be in [0, 2*PI]. Otherwise, in [-PI, +PI]. - * \tparam positiveRangeGamma If true, gamma will be in [0, 2*PI]. Otherwise, in [-PI, +PI]. + * See EulerAngles(const MatrixBase&) for more information about + * angles ranges output. */ - template< - bool PositiveRangeAlpha, - bool PositiveRangeBeta, - bool PositiveRangeGamma, - typename Derived> - static EulerAngles FromRotation(const RotationBase& rot) - { - return FromRotation(rot.toRotationMatrix()); - } - - /*EulerAngles& fromQuaternion(const QuaternionType& q) - { - // TODO: Implement it in a faster way for quaternions - // According to http://www.euclideanspace.com/maths/geometry/rotations/conversions/quaternionToEuler/ - // we can compute only the needed matrix cells and then convert to euler angles. (see ZYX example below) - // Currently we compute all matrix cells from quaternion. - - // Special case only for ZYX - //Scalar y2 = q.y() * q.y(); - //m_angles[0] = std::atan2(2*(q.w()*q.z() + q.x()*q.y()), (1 - 2*(y2 + q.z()*q.z()))); - //m_angles[1] = std::asin( 2*(q.w()*q.y() - q.z()*q.x())); - //m_angles[2] = std::atan2(2*(q.w()*q.x() + q.y()*q.z()), (1 - 2*(q.x()*q.x() + y2))); - }*/ - - /** Set \c *this from a rotation matrix(i.e. pure orthogonal matrix with determinant of +1). */ template EulerAngles& operator=(const MatrixBase& m) { EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Derived, 3, 3) @@ -318,7 +235,11 @@ namespace Eigen // TODO: Assign and construct from another EulerAngles (with different system) - /** Set \c *this from a rotation. */ + /** Set \c *this from a rotation. + * + * See EulerAngles(const RotationBase&) for more information about + * angles ranges output. + */ template EulerAngles& operator=(const RotationBase& rot) { System::CalcEulerAngles(*this, rot.toRotationMatrix()); @@ -330,6 +251,7 @@ namespace Eigen /** \returns an equivalent 3x3 rotation matrix. */ Matrix3 toRotationMatrix() const { + // TODO: Calc it faster return static_cast(*this).toRotationMatrix(); } diff --git a/unsupported/Eigen/src/EulerAngles/EulerSystem.h b/unsupported/Eigen/src/EulerAngles/EulerSystem.h index 98f9f647d..aa96461f9 100644 --- a/unsupported/Eigen/src/EulerAngles/EulerSystem.h +++ b/unsupported/Eigen/src/EulerAngles/EulerSystem.h @@ -69,7 +69,7 @@ namespace Eigen * * 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) + * - 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) @@ -80,7 +80,7 @@ namespace Eigen * 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). + * - 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: @@ -90,7 +90,7 @@ namespace Eigen * 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} + * - _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} * @@ -112,9 +112,9 @@ namespace Eigen * * \tparam _AlphaAxis the first fixed EulerAxis * - * \tparam _AlphaAxis the second fixed EulerAxis + * \tparam _BetaAxis the second fixed EulerAxis * - * \tparam _AlphaAxis the third fixed EulerAxis + * \tparam _GammaAxis the third fixed EulerAxis */ template class EulerSystem @@ -138,14 +138,16 @@ namespace Eigen BetaAxisAbs = internal::Abs::value, /*!< the second rotation axis unsigned */ GammaAxisAbs = internal::Abs::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 */ + IsAlphaOpposite = (AlphaAxis < 0) ? 1 : 0, /*!< whether alpha axis is negative */ + IsBetaOpposite = (BetaAxis < 0) ? 1 : 0, /*!< whether beta axis is negative */ + IsGammaOpposite = (GammaAxis < 0) ? 1 : 0, /*!< whether gamma axis is negative */ - IsTaitBryan = ((unsigned)AlphaAxisAbs != (unsigned)GammaAxisAbs) ? 1 : 0 /*!< weather the Euler system is tait bryan */ + // Parity is even if alpha axis X is followed by beta axis Y, or Y is followed + // by Z, or Z is followed by X; otherwise it is odd. + IsOdd = ((AlphaAxisAbs)%3 == (BetaAxisAbs - 1)%3) ? 0 : 1, /*!< whether the Euler system is odd */ + IsEven = IsOdd ? 0 : 1, /*!< whether the Euler system is even */ + + IsTaitBryan = ((unsigned)AlphaAxisAbs != (unsigned)GammaAxisAbs) ? 1 : 0 /*!< whether the Euler system is Tait-Bryan */ }; private: @@ -180,123 +182,89 @@ namespace Eigen static void CalcEulerAngles_imp(Matrix::Scalar, 3, 1>& res, const MatrixBase& mat, internal::true_type /*isTaitBryan*/) { using std::atan2; - using std::sin; - using std::cos; + using std::sqrt; typedef typename Derived::Scalar Scalar; - typedef Matrix 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))) { - if(res[0] > Scalar(0)) { - res[0] -= Scalar(EIGEN_PI); - } - else { - res[0] += Scalar(EIGEN_PI); - } - res[1] = atan2(-mat(I,K), -c2); + + Scalar plusMinus = IsEven? 1 : -1; + Scalar minusPlus = IsOdd? 1 : -1; + + 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); + res[1] = atan2(plusMinus * mat(I,K), Rsum); + + // There is a singularity when cos(beta) = 0 + if(Rsum > 4 * NumTraits::epsilon()) { + res[0] = atan2(minusPlus * mat(J, K), mat(K, K)); + res[2] = atan2(minusPlus * mat(I, J), mat(I, I)); + } + else if(plusMinus * mat(I, K) > 0) { + Scalar spos = mat(J, I) + plusMinus * mat(K, J); // 2*sin(alpha + plusMinus * gamma) + Scalar cpos = mat(J, J) + minusPlus * mat(K, I); // 2*cos(alpha + plusMinus * gamma); + Scalar alphaPlusMinusGamma = atan2(spos, cpos); + res[0] = alphaPlusMinusGamma; + res[2] = 0; + } + else { + Scalar sneg = plusMinus * (mat(K, J) + minusPlus * mat(J, I)); // 2*sin(alpha + minusPlus*gamma) + Scalar cneg = mat(J, J) + plusMinus * mat(K, I); // 2*cos(alpha + minusPlus*gamma) + Scalar alphaMinusPlusBeta = atan2(sneg, cneg); + res[0] = alphaMinusPlusBeta; + res[2] = 0; } - 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 - static void CalcEulerAngles_imp(Matrix::Scalar,3,1>& res, const MatrixBase& mat, internal::false_type /*isTaitBryan*/) + static void CalcEulerAngles_imp(Matrix::Scalar,3,1>& res, + const MatrixBase& mat, internal::false_type /*isTaitBryan*/) { using std::atan2; - using std::sin; - using std::cos; + using std::sqrt; typedef typename Derived::Scalar Scalar; - typedef Matrix Vector2; - - res[0] = atan2(mat(J,I), mat(K,I)); - if((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)); + + Scalar plusMinus = IsEven? 1 : -1; + Scalar minusPlus = IsOdd? 1 : -1; + + 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); + + res[1] = atan2(Rsum, mat(I, I)); + + if(Rsum > 4 * NumTraits::epsilon()) { + res[0] = atan2(mat(J, I), minusPlus * mat(K, I)); + res[2] = atan2(mat(I, J), plusMinus * mat(I, K)); } - else - { - Scalar s2 = Vector2(mat(J,I), mat(K,I)).norm(); - res[1] = atan2(s2, mat(I,I)); + else if( mat(I, I) > 0) { + Scalar spos = plusMinus * mat(K, J) + minusPlus * mat(J, K); // 2*sin(alpha + gamma) + Scalar cpos = mat(J, J) + mat(K, K); // 2*cos(alpha + gamma) + res[0] = atan2(spos, cpos); + res[2] = 0; + } + else { + Scalar sneg = plusMinus * mat(K, J) + plusMinus * mat(J, K); // 2*sin(alpha - gamma) + Scalar cneg = mat(J, J) - mat(K, K); // 2*cos(alpha - gamma) + res[0] = atan2(sneg, cneg); + res[1] = 0; } - // 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 static void CalcEulerAngles( EulerAngles& res, const typename EulerAngles::Matrix3& mat) - { - CalcEulerAngles(res, mat, false, false, false); - } - - template< - bool PositiveRangeAlpha, - bool PositiveRangeBeta, - bool PositiveRangeGamma, - typename Scalar> - static void CalcEulerAngles( - EulerAngles& res, - const typename EulerAngles::Matrix3& mat) - { - CalcEulerAngles(res, mat, PositiveRangeAlpha, PositiveRangeBeta, PositiveRangeGamma); - } - - template - static void CalcEulerAngles( - EulerAngles& res, - const typename EulerAngles::Matrix3& mat, - bool PositiveRangeAlpha, - bool PositiveRangeBeta, - bool PositiveRangeGamma) { CalcEulerAngles_imp( res.angles(), mat, typename internal::conditional::type()); - if (IsAlphaOpposite == IsOdd) + if (IsAlphaOpposite) res.alpha() = -res.alpha(); - if (IsBetaOpposite == IsOdd) + if (IsBetaOpposite) res.beta() = -res.beta(); - if (IsGammaOpposite == IsOdd) + if (IsGammaOpposite) 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 diff --git a/unsupported/doc/examples/EulerAngles.cpp b/unsupported/doc/examples/EulerAngles.cpp index 1ef6aee18..3f8ca8c17 100644 --- a/unsupported/doc/examples/EulerAngles.cpp +++ b/unsupported/doc/examples/EulerAngles.cpp @@ -23,7 +23,7 @@ int main() // Some Euler angles representation that our plane use. EulerAnglesZYZd planeAngles(0.78474, 0.5271, -0.513794); - MyArmyAngles planeAnglesInMyArmyAngles = MyArmyAngles::FromRotation(planeAngles); + MyArmyAngles planeAnglesInMyArmyAngles(planeAngles); std::cout << "vehicle angles(MyArmy): " << vehicleAngles << std::endl; std::cout << "plane angles(ZYZ): " << planeAngles << std::endl; @@ -37,7 +37,7 @@ int main() Quaterniond planeRotated = AngleAxisd(-0.342, Vector3d::UnitY()) * planeAngles; planeAngles = planeRotated; - planeAnglesInMyArmyAngles = MyArmyAngles::FromRotation(planeRotated); + planeAnglesInMyArmyAngles = planeRotated; std::cout << "new plane angles(ZYZ): " << planeAngles << std::endl; std::cout << "new plane angles(MyArmy): " << planeAnglesInMyArmyAngles << std::endl; diff --git a/unsupported/test/EulerAngles.cpp b/unsupported/test/EulerAngles.cpp index a8cb52864..8b4706686 100644 --- a/unsupported/test/EulerAngles.cpp +++ b/unsupported/test/EulerAngles.cpp @@ -13,125 +13,80 @@ using namespace Eigen; +// Verify that x is in the approxed range [a, b] +#define VERIFY_APPROXED_RANGE(a, x, b) \ + do { \ + VERIFY_IS_APPROX_OR_LESS_THAN(a, x); \ + VERIFY_IS_APPROX_OR_LESS_THAN(x, b); \ + } while(0) + template -void verify_euler_ranged(const Matrix& ea, - bool positiveRangeAlpha, bool positiveRangeBeta, bool positiveRangeGamma) +void verify_euler(const Matrix& ea) { typedef EulerAngles EulerAnglesType; typedef Matrix Matrix3; typedef Matrix Vector3; typedef Quaternion QuaternionType; typedef AngleAxis AngleAxisType; - using std::abs; - Scalar alphaRangeStart, alphaRangeEnd; + const Scalar ONE = Scalar(1); + const Scalar HALF_PI = Scalar(EIGEN_PI / 2); + const Scalar PI = Scalar(EIGEN_PI); + Scalar betaRangeStart, betaRangeEnd; - Scalar gammaRangeStart, gammaRangeEnd; - - if (positiveRangeAlpha) + if (EulerSystem::IsTaitBryan) { - alphaRangeStart = Scalar(0); - alphaRangeEnd = Scalar(2 * EIGEN_PI); + betaRangeStart = -HALF_PI; + betaRangeEnd = HALF_PI; } else { - alphaRangeStart = -Scalar(EIGEN_PI); - alphaRangeEnd = Scalar(EIGEN_PI); + betaRangeStart = -PI; + betaRangeEnd = PI; } - if (positiveRangeBeta) - { - betaRangeStart = Scalar(0); - betaRangeEnd = Scalar(2 * EIGEN_PI); - } - else - { - betaRangeStart = -Scalar(EIGEN_PI); - betaRangeEnd = Scalar(EIGEN_PI); - } - - if (positiveRangeGamma) - { - gammaRangeStart = Scalar(0); - gammaRangeEnd = Scalar(2 * EIGEN_PI); - } - else - { - gammaRangeStart = -Scalar(EIGEN_PI); - gammaRangeEnd = Scalar(EIGEN_PI); - } - - const int i = EulerSystem::AlphaAxisAbs - 1; - const int j = EulerSystem::BetaAxisAbs - 1; - const int k = EulerSystem::GammaAxisAbs - 1; - - const int iFactor = EulerSystem::IsAlphaOpposite ? -1 : 1; - const int jFactor = EulerSystem::IsBetaOpposite ? -1 : 1; - const int kFactor = EulerSystem::IsGammaOpposite ? -1 : 1; - const Vector3 I = EulerAnglesType::AlphaAxisVector(); const Vector3 J = EulerAnglesType::BetaAxisVector(); const Vector3 K = EulerAnglesType::GammaAxisVector(); EulerAnglesType e(ea[0], ea[1], ea[2]); - + Matrix3 m(e); - Vector3 eabis = EulerAnglesType(m, positiveRangeAlpha, positiveRangeBeta, positiveRangeGamma).angles(); + + Vector3 eabis = static_cast(m).angles(); // Check that eabis in range - VERIFY(alphaRangeStart <= eabis[0] && eabis[0] <= alphaRangeEnd); - VERIFY(betaRangeStart <= eabis[1] && eabis[1] <= betaRangeEnd); - VERIFY(gammaRangeStart <= eabis[2] && eabis[2] <= gammaRangeEnd); - - Vector3 eabis2 = m.eulerAngles(i, j, k); - - // Invert the relevant axes - eabis2[0] *= iFactor; - eabis2[1] *= jFactor; - eabis2[2] *= kFactor; - - // Saturate the angles to the correct range - if (positiveRangeAlpha && (eabis2[0] < 0)) - eabis2[0] += Scalar(2 * EIGEN_PI); - if (positiveRangeBeta && (eabis2[1] < 0)) - eabis2[1] += Scalar(2 * EIGEN_PI); - if (positiveRangeGamma && (eabis2[2] < 0)) - eabis2[2] += Scalar(2 * EIGEN_PI); - - VERIFY_IS_APPROX(eabis, eabis2);// Verify that our estimation is the same as m.eulerAngles() is - + VERIFY_APPROXED_RANGE(-PI, eabis[0], PI); + VERIFY_APPROXED_RANGE(betaRangeStart, eabis[1], betaRangeEnd); + VERIFY_APPROXED_RANGE(-PI, eabis[2], PI); + Matrix3 mbis(AngleAxisType(eabis[0], I) * AngleAxisType(eabis[1], J) * AngleAxisType(eabis[2], K)); VERIFY_IS_APPROX(m, mbis); - - // Tests that are only relevant for no possitive range - if (!(positiveRangeAlpha || positiveRangeBeta || positiveRangeGamma)) + + // Test if ea and eabis are the same + // Need to check both singular and non-singular cases + // There are two singular cases. + // 1. When I==K and sin(ea(1)) == 0 + // 2. When I!=K and cos(ea(1)) == 0 + + // Tests that are only relevant for no positive range + /*if (!(positiveRangeAlpha || positiveRangeGamma)) { - /* If I==K, and ea[1]==0, then there no unique solution. */ - /* The remark apply in the case where I!=K, and |ea[1]| is close to pi/2. */ + // If I==K, and ea[1]==0, then there no unique solution. + // The remark apply in the case where I!=K, and |ea[1]| is close to pi/2. if( (i!=k || ea[1]!=0) && (i==k || !internal::isApprox(abs(ea[1]),Scalar(EIGEN_PI/2),test_precision())) ) VERIFY((ea-eabis).norm() <= test_precision()); // approx_or_less_than does not work for 0 - VERIFY(0 < eabis[0] || test_isMuchSmallerThan(eabis[0], Scalar(1))); - } + VERIFY(0 < eabis[0] || VERIFY_IS_MUCH_SMALLER_THAN(eabis[0], Scalar(1))); + }*/ // Quaternions QuaternionType q(e); - eabis = EulerAnglesType(q, positiveRangeAlpha, positiveRangeBeta, positiveRangeGamma).angles(); - VERIFY_IS_APPROX(eabis, eabis2);// Verify that the euler angles are still the same -} - -template -void verify_euler(const Matrix& ea) -{ - verify_euler_ranged(ea, false, false, false); - verify_euler_ranged(ea, false, false, true); - verify_euler_ranged(ea, false, true, false); - verify_euler_ranged(ea, false, true, true); - verify_euler_ranged(ea, true, false, false); - verify_euler_ranged(ea, true, false, true); - verify_euler_ranged(ea, true, true, false); - verify_euler_ranged(ea, true, true, true); + eabis = static_cast(q).angles(); + QuaternionType qbis(AngleAxisType(eabis[0], I) * AngleAxisType(eabis[1], J) * AngleAxisType(eabis[2], K)); + VERIFY_IS_APPROX(std::abs(q.dot(qbis)), ONE); + //VERIFY_IS_APPROX(eabis, eabis2);// Verify that the euler angles are still the same } template void check_all_var(const Matrix& ea) @@ -150,6 +105,8 @@ template void check_all_var(const Matrix& ea) verify_euler(ea); verify_euler(ea); verify_euler(ea); + + // TODO: Test negative axes as well! (only test if the angles get negative when needed) } template void eulerangles()