diff options
| -rw-r--r-- | unsupported/Eigen/EulerAngles | 18 | ||||
| -rw-r--r-- | unsupported/Eigen/src/EulerAngles/EulerAngles.h | 168 | ||||
| -rw-r--r-- | unsupported/Eigen/src/EulerAngles/EulerSystem.h | 153 | ||||
| -rw-r--r-- | unsupported/test/EulerAngles.cpp | 74 |
4 files changed, 165 insertions, 248 deletions
diff --git a/unsupported/Eigen/EulerAngles b/unsupported/Eigen/EulerAngles index a1aa5d829..08cc74237 100644 --- a/unsupported/Eigen/EulerAngles +++ b/unsupported/Eigen/EulerAngles @@ -16,13 +16,19 @@ #include "Eigen/src/Core/util/DisableStupidWarnings.h" +namespace Eigen { + /** - * \defgroup EulerAngles_Module EulerAngles module - * - * - * - * - */ + * \defgroup EulerAngles_Module EulerAngles module + * \brief This module provides generic euler angles rotation. + * + * \code + * #include <unsupported/Eigen/EulerAngles> + * \endcode + * + */ + +} #include "src/EulerAngles/EulerSystem.h" #include "src/EulerAngles/EulerAngles.h" diff --git a/unsupported/Eigen/src/EulerAngles/EulerAngles.h b/unsupported/Eigen/src/EulerAngles/EulerAngles.h index d21f52491..66050a426 100644 --- a/unsupported/Eigen/src/EulerAngles/EulerAngles.h +++ b/unsupported/Eigen/src/EulerAngles/EulerAngles.h @@ -29,7 +29,7 @@ namespace Eigen class EulerAngles : public RotationBase<EulerAngles<_Scalar, _System>, 3> { public: - /** the scalar type of the coefficients */ + /** the scalar type of the angles */ typedef _Scalar Scalar; typedef _System System; @@ -38,16 +38,19 @@ namespace Eigen typedef Quaternion<Scalar> QuaternionType; typedef AngleAxis<Scalar> AngleAxisType; - static Vector3 HeadingAxisVector() { - return internal::NegativeIf<System::IsHeadingOpposite>::run(Vector3::Unit(System::HeadingAxisAbs - 1)); + static Vector3 AlphaAxisVector() { + const Vector3& u = Vector3::Unit(System::AlphaAxisAbs - 1); + return System::IsAlphaOpposite ? -u : u; } - static Vector3 PitchAxisVector() { - return internal::NegativeIf<System::IsPitchOpposite>::run(Vector3::Unit(System::PitchAxisAbs - 1)); + static Vector3 BetaAxisVector() { + const Vector3& u = Vector3::Unit(System::BetaAxisAbs - 1); + return System::IsBetaOpposite ? -u : u; } - static Vector3 RollAxisVector() { - return internal::NegativeIf<System::IsRollOpposite>::run(Vector3::Unit(System::RollAxisAbs - 1)); + static Vector3 GammaAxisVector() { + const Vector3& u = Vector3::Unit(System::GammaAxisAbs - 1); + return System::IsGammaOpposite ? -u : u; } private: @@ -56,7 +59,7 @@ namespace Eigen public: EulerAngles() {} - inline EulerAngles(Scalar a0, Scalar a1, Scalar a2) : m_angles(a0, a1, a2) {} + inline EulerAngles(Scalar alpha, Scalar beta, Scalar gamma) : m_angles(alpha, beta, gamma) {} template<typename Derived> inline EulerAngles(const MatrixBase<Derived>& m) { *this = m; } @@ -64,11 +67,11 @@ namespace Eigen template<typename Derived> inline EulerAngles( const MatrixBase<Derived>& m, - bool positiveRangeHeading, - bool positiveRangePitch, - bool positiveRangeRoll) { + bool positiveRangeAlpha, + bool positiveRangeBeta, + bool positiveRangeGamma) { - fromRotation(m, positiveRangeHeading, positiveRangePitch, positiveRangeRoll); + System::CalcEulerAngles(*this, m, positiveRangeAlpha, positiveRangeBeta, positiveRangeGamma); } template<typename Derived> @@ -77,35 +80,24 @@ namespace Eigen template<typename Derived> inline EulerAngles( const RotationBase<Derived, 3>& rot, - bool positiveRangeHeading, - bool positiveRangePitch, - bool positiveRangeRoll) { + bool positiveRangeAlpha, + bool positiveRangeBeta, + bool positiveRangeGamma) { - fromRotation(rot, positiveRangeHeading, positiveRangePitch, positiveRangeRoll); + System::CalcEulerAngles(*this, rot.toRotationMatrix(), positiveRangeAlpha, positiveRangeBeta, positiveRangeGamma); } - Scalar angle(int i) const { return m_angles.coeff(i); } - Scalar& angle(int i) { return m_angles.coeffRef(i); } + const Vector3& angles() const { return m_angles; } + Vector3& angles() { return m_angles; } - const Vector3& coeffs() const { return m_angles; } - Vector3& coeffs() { return m_angles; } + Scalar alpha() const { return m_angles[0]; } + Scalar& alpha() { return m_angles[0]; } - Scalar h() const { return m_angles[0]; } - Scalar& h() { return m_angles[0]; } + Scalar beta() const { return m_angles[1]; } + Scalar& beta() { return m_angles[1]; } - Scalar p() const { return m_angles[1]; } - Scalar& p() { return m_angles[1]; } - - Scalar r() const { return m_angles[2]; } - Scalar& r() { return m_angles[2]; } - - EulerAngles invert() const - { - //m_angles = -m_angles;// I want to do this but there could be an aliasing issue! - m_angles *= -1; - - return *this; - } + Scalar gamma() const { return m_angles[2]; } + Scalar& gamma() { return m_angles[2]; } EulerAngles inverse() const { @@ -121,59 +113,26 @@ namespace Eigen /** Constructs and \returns an equivalent 3x3 rotation matrix. */ - template<typename Derived> - EulerAngles& fromRotation(const MatrixBase<Derived>& m) - { - System::eulerAngles(*this, m); - return *this; - } - template< - bool PositiveRangeHeading, - bool PositiveRangePitch, - bool PositiveRangeRoll, + bool PositiveRangeAlpha, + bool PositiveRangeBeta, + bool PositiveRangeGamma, typename Derived> - EulerAngles& fromRotation(const MatrixBase<Derived>& m) + static EulerAngles FromRotation(const MatrixBase<Derived>& m) { - System::eulerAngles<PositiveRangeHeading, PositiveRangePitch, PositiveRangeRoll>(*this, m); - return *this; - } - - template<typename Derived> - EulerAngles& fromRotation( - const MatrixBase<Derived>& m, - bool positiveRangeHeading, - bool positiveRangePitch, - bool positiveRangeRoll) - { - System::eulerAngles(*this, m, positiveRangeHeading, positiveRangePitch, positiveRangeRoll); - return *this; - } - - template<typename Derived> - EulerAngles& fromRotation(const RotationBase<Derived, 3>& rot) - { - return fromRotation(rot.toRotationMatrix()); + EulerAngles e; + System::CalcEulerAngles<PositiveRangeAlpha, PositiveRangeBeta, PositiveRangeGamma>(e, m); + return e; } template< - bool PositiveRangeHeading, - bool PositiveRangePitch, - bool PositiveRangeRoll, + bool PositiveRangeAlpha, + bool PositiveRangeBeta, + bool PositiveRangeGamma, typename Derived> - EulerAngles& fromRotation(const RotationBase<Derived, 3>& rot) + static EulerAngles& FromRotation(const RotationBase<Derived, 3>& rot) { - return fromRotation<PositiveRangeHeading, PositiveRangePitch, PositiveRangeRoll>(rot.toRotationMatrix()); - } - - template<typename Derived> - EulerAngles& fromRotation( - const RotationBase<Derived, 3>& rot, - bool positiveRangeHeading, - bool positiveRangePitch, - bool positiveRangeRoll) - { - return fromRotation(rot.toRotationMatrix(), positiveRangeHeading, positiveRangePitch, positiveRangeRoll); + return FromRotation<PositiveRangeAlpha, PositiveRangeBeta, PositiveRangeGamma>(rot.toRotationMatrix()); } /*EulerAngles& fromQuaternion(const QuaternionType& q) @@ -193,8 +152,9 @@ namespace Eigen /** Set \c *this from a rotation matrix(i.e. pure orthogonal matrix with determinent of +1). */ template<typename Derived> - EulerAngles& operator=(const MatrixBase<Derived>& mat) { - return fromRotation(mat); + EulerAngles& operator=(const MatrixBase<Derived>& m) { + System::CalcEulerAngles(*this, m); + return *this; } // TODO: Assign and construct from another EulerAngles (with different system) @@ -203,7 +163,8 @@ namespace Eigen */ template<typename Derived> EulerAngles& operator=(const RotationBase<Derived, 3>& rot) { - return fromRotation(rot.toRotationMatrix()); + System::CalcEulerAngles(*this, rot.toRotationMatrix()); + return *this; } // TODO: Support isApprox function @@ -216,9 +177,9 @@ namespace Eigen QuaternionType toQuaternion() const { return - AngleAxisType(h(), HeadingAxisVector()) * - AngleAxisType(p(), PitchAxisVector()) * - AngleAxisType(r(), RollAxisVector()); + AngleAxisType(alpha(), AlphaAxisVector()) * + AngleAxisType(beta(), BetaAxisVector()) * + AngleAxisType(gamma(), GammaAxisVector()); } operator QuaternionType() const @@ -227,20 +188,27 @@ namespace Eigen } }; - typedef EulerAngles<double, EulerSystemXYZ> EulerAnglesXYZd; - typedef EulerAngles<double, EulerSystemXYX> EulerAnglesXYXd; - typedef EulerAngles<double, EulerSystemXZY> EulerAnglesXZYd; - typedef EulerAngles<double, EulerSystemXZX> EulerAnglesXZXd; - - typedef EulerAngles<double, EulerSystemYZX> EulerAnglesYZXd; - typedef EulerAngles<double, EulerSystemYZY> EulerAnglesYZYd; - typedef EulerAngles<double, EulerSystemYXZ> EulerAnglesYXZd; - typedef EulerAngles<double, EulerSystemYXY> EulerAnglesYXYd; - - typedef EulerAngles<double, EulerSystemZXY> EulerAnglesZXYd; - typedef EulerAngles<double, EulerSystemZXZ> EulerAnglesZXZd; - typedef EulerAngles<double, EulerSystemZYX> EulerAnglesZYXd; - typedef EulerAngles<double, EulerSystemZYZ> EulerAnglesZYZd; +#define EIGEN_EULER_ANGLES_SINGLE_TYPEDEF(SYSTEM, SCALAR_TYPE, SCALAR_POSTFIX) \ + typedef EulerAngles<SCALAR_TYPE, SYSTEM> SYSTEM##SCALAR_POSTFIX; + +#define EIGEN_EULER_ANGLES_TYPEDEFS(SCALAR_TYPE, SCALAR_POSTFIX) \ + EIGEN_EULER_ANGLES_SINGLE_TYPEDEF(EulerSystemXYZ, SCALAR_TYPE, SCALAR_POSTFIX) \ + EIGEN_EULER_ANGLES_SINGLE_TYPEDEF(EulerSystemXYX, SCALAR_TYPE, SCALAR_POSTFIX) \ + EIGEN_EULER_ANGLES_SINGLE_TYPEDEF(EulerSystemXZY, SCALAR_TYPE, SCALAR_POSTFIX) \ + EIGEN_EULER_ANGLES_SINGLE_TYPEDEF(EulerSystemXZX, SCALAR_TYPE, SCALAR_POSTFIX) \ + \ + EIGEN_EULER_ANGLES_SINGLE_TYPEDEF(EulerSystemYZX, SCALAR_TYPE, SCALAR_POSTFIX) \ + EIGEN_EULER_ANGLES_SINGLE_TYPEDEF(EulerSystemYZY, SCALAR_TYPE, SCALAR_POSTFIX) \ + EIGEN_EULER_ANGLES_SINGLE_TYPEDEF(EulerSystemYXZ, SCALAR_TYPE, SCALAR_POSTFIX) \ + EIGEN_EULER_ANGLES_SINGLE_TYPEDEF(EulerSystemYXY, SCALAR_TYPE, SCALAR_POSTFIX) \ + \ + EIGEN_EULER_ANGLES_SINGLE_TYPEDEF(EulerSystemZXY, SCALAR_TYPE, SCALAR_POSTFIX) \ + EIGEN_EULER_ANGLES_SINGLE_TYPEDEF(EulerSystemZXZ, SCALAR_TYPE, SCALAR_POSTFIX) \ + EIGEN_EULER_ANGLES_SINGLE_TYPEDEF(EulerSystemZYX, SCALAR_TYPE, SCALAR_POSTFIX) \ + EIGEN_EULER_ANGLES_SINGLE_TYPEDEF(EulerSystemZYZ, SCALAR_TYPE, SCALAR_POSTFIX) + +EIGEN_EULER_ANGLES_TYPEDEFS(float, f) +EIGEN_EULER_ANGLES_TYPEDEFS(double, d) namespace internal { diff --git a/unsupported/Eigen/src/EulerAngles/EulerSystem.h b/unsupported/Eigen/src/EulerAngles/EulerSystem.h index 9699dd10d..f37c62f8e 100644 --- a/unsupported/Eigen/src/EulerAngles/EulerSystem.h +++ b/unsupported/Eigen/src/EulerAngles/EulerSystem.h @@ -30,69 +30,6 @@ namespace Eigen { enum { value = -Num }; }; - - template <bool Cond> - struct NegativeIf - { - template <typename T> - static T run(const T& t) - { - return -t; - } - }; - - template <> - struct NegativeIf<false> - { - template <typename T> - static T run(const T& t) - { - return t; - } - }; - - template <bool Cond> - struct NegateIf - { - template <typename T> - static void run(T& t) - { - t = -t; - } - }; - - template <> - struct NegateIf<false> - { - template <typename T> - static void run(T&) - { - // no op - } - }; - - template <bool Cond1, bool Cond2> - struct NegateIfXor : NegateIf<Cond1 != Cond2> {}; - - template <typename Type, Type value, bool Cond> - struct AddConstIf - { - template <typename T> - static void run(T& t) - { - t += T(value); - } - }; - - template <typename Type, Type value> - struct AddConstIf<Type, value, false> - { - template <typename T> - static void run(T&) - { - // no op - } - }; template <int Axis> struct IsValidAxis @@ -108,36 +45,36 @@ namespace Eigen EULER_Z = 3 }; - template <int _HeadingAxis, int _PitchAxis, int _RollAxis> + 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 - static const int HeadingAxis = _HeadingAxis; - static const int PitchAxis = _PitchAxis; - static const int RollAxis = _RollAxis; + static const int AlphaAxis = _AlphaAxis; + static const int BetaAxis = _BetaAxis; + static const int GammaAxis = _GammaAxis; enum { - HeadingAxisAbs = internal::Abs<HeadingAxis>::value, - PitchAxisAbs = internal::Abs<PitchAxis>::value, - RollAxisAbs = internal::Abs<RollAxis>::value, + AlphaAxisAbs = internal::Abs<AlphaAxis>::value, + BetaAxisAbs = internal::Abs<BetaAxis>::value, + GammaAxisAbs = internal::Abs<GammaAxis>::value, - IsHeadingOpposite = (HeadingAxis < 0) ? 1 : 0, - IsPitchOpposite = (PitchAxis < 0) ? 1 : 0, - IsRollOpposite = (RollAxis < 0) ? 1 : 0, + IsAlphaOpposite = (AlphaAxis < 0) ? 1 : 0, + IsBetaOpposite = (BetaAxis < 0) ? 1 : 0, + IsGammaOpposite = (GammaAxis < 0) ? 1 : 0, - IsOdd = ((HeadingAxisAbs)%3 == (PitchAxisAbs - 1)%3) ? 0 : 1, + IsOdd = ((AlphaAxisAbs)%3 == (BetaAxisAbs - 1)%3) ? 0 : 1, IsEven = IsOdd ? 0 : 1, // TODO: Assert this, and sort it in a better way - IsValid = ((unsigned)HeadingAxisAbs != (unsigned)PitchAxisAbs && - (unsigned)PitchAxisAbs != (unsigned)RollAxisAbs && - internal::IsValidAxis<HeadingAxis>::value && internal::IsValidAxis<PitchAxis>::value && internal::IsValidAxis<RollAxis>::value) ? 1 : 0, + IsValid = ((unsigned)AlphaAxisAbs != (unsigned)BetaAxisAbs && + (unsigned)BetaAxisAbs != (unsigned)GammaAxisAbs && + internal::IsValidAxis<AlphaAxis>::value && internal::IsValidAxis<BetaAxis>::value && internal::IsValidAxis<GammaAxis>::value) ? 1 : 0, // TODO: After a proper assertation, remove the "IsValid" from this expression - IsTaitBryan = (IsValid && (unsigned)HeadingAxisAbs != (unsigned)RollAxisAbs) ? 1 : 0 + IsTaitBryan = (IsValid && (unsigned)AlphaAxisAbs != (unsigned)GammaAxisAbs) ? 1 : 0 }; private: @@ -147,13 +84,14 @@ namespace Eigen // 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 = HeadingAxisAbs - 1, - J = (HeadingAxisAbs - 1 + 1 + IsOdd)%3, - K = (HeadingAxisAbs - 1 + 2 - IsOdd)%3 + 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 eulerAngles_imp(Matrix<typename MatrixBase<Derived>::Scalar, 3, 1>& res, const MatrixBase<Derived>& mat, internal::true_type /*isTaitBryan*/) + 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; @@ -176,7 +114,7 @@ namespace Eigen } template <typename Derived> - static void eulerAngles_imp(Matrix<typename MatrixBase<Derived>::Scalar,3,1>& res, const MatrixBase<Derived>& mat, internal::false_type /*isTaitBryan*/) + 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; @@ -216,50 +154,55 @@ namespace Eigen public: template<typename Scalar> - static void eulerAngles( + static void CalcEulerAngles( EulerAngles<Scalar, EulerSystem>& res, const typename EulerAngles<Scalar, EulerSystem>::Matrix3& mat) { - eulerAngles(res, mat, false, false, false); + CalcEulerAngles(res, mat, false, false, false); } template< typename Scalar, - bool PositiveRangeHeading, - bool PositiveRangePitch, - bool PositiveRangeRoll> - static void eulerAngles( + bool PositiveRangeAlpha, + bool PositiveRangeBeta, + bool PositiveRangeGamma> + static void CalcEulerAngles( EulerAngles<Scalar, EulerSystem>& res, const typename EulerAngles<Scalar, EulerSystem>::Matrix3& mat) { - eulerAngles(res, mat, PositiveRangeHeading, PositiveRangePitch, PositiveRangeRoll); + CalcEulerAngles(res, mat, PositiveRangeAlpha, PositiveRangeBeta, PositiveRangeGamma); } template<typename Scalar> - static void eulerAngles( + static void CalcEulerAngles( EulerAngles<Scalar, EulerSystem>& res, const typename EulerAngles<Scalar, EulerSystem>::Matrix3& mat, - bool positiveRangeHeading, - bool positiveRangePitch, - bool positiveRangeRoll) + bool PositiveRangeAlpha, + bool PositiveRangeBeta, + bool PositiveRangeGamma) { - eulerAngles_imp( - res.coeffs(), mat, + CalcEulerAngles_imp( + res.angles(), mat, typename internal::conditional<IsTaitBryan, internal::true_type, internal::false_type>::type()); - internal::NegateIfXor<IsHeadingOpposite, IsEven>::run(res.h()); - internal::NegateIfXor<IsPitchOpposite, IsEven>::run(res.p()); - internal::NegateIfXor<IsRollOpposite, IsEven>::run(res.r()); + 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 (positiveRangeHeading && (res.h() < 0)) - res.h() += Scalar(2 * EIGEN_PI); + if (PositiveRangeAlpha && (res.alpha() < 0)) + res.alpha() += Scalar(2 * EIGEN_PI); - if (positiveRangePitch && (res.p() < 0)) - res.p() += Scalar(2 * EIGEN_PI); + if (PositiveRangeBeta && (res.beta() < 0)) + res.beta() += Scalar(2 * EIGEN_PI); - if (positiveRangeRoll && (res.r() < 0)) - res.r() += Scalar(2 * EIGEN_PI); + if (PositiveRangeGamma && (res.gamma() < 0)) + res.gamma() += Scalar(2 * EIGEN_PI); } }; diff --git a/unsupported/test/EulerAngles.cpp b/unsupported/test/EulerAngles.cpp index e65399ffa..a8cb52864 100644 --- a/unsupported/test/EulerAngles.cpp +++ b/unsupported/test/EulerAngles.cpp @@ -15,7 +15,7 @@ using namespace Eigen; template<typename EulerSystem, typename Scalar> void verify_euler_ranged(const Matrix<Scalar,3,1>& ea, - bool positiveRangeHeading, bool positiveRangePitch, bool positiveRangeRoll) + bool positiveRangeAlpha, bool positiveRangeBeta, bool positiveRangeGamma) { typedef EulerAngles<Scalar, EulerSystem> EulerAnglesType; typedef Matrix<Scalar,3,3> Matrix3; @@ -24,64 +24,64 @@ void verify_euler_ranged(const Matrix<Scalar,3,1>& ea, typedef AngleAxis<Scalar> AngleAxisType; using std::abs; - Scalar headingRangeStart, headingRangeEnd; - Scalar pitchRangeStart, pitchRangeEnd; - Scalar rollRangeStart, rollRangeEnd; + Scalar alphaRangeStart, alphaRangeEnd; + Scalar betaRangeStart, betaRangeEnd; + Scalar gammaRangeStart, gammaRangeEnd; - if (positiveRangeHeading) + if (positiveRangeAlpha) { - headingRangeStart = Scalar(0); - headingRangeEnd = Scalar(2 * EIGEN_PI); + alphaRangeStart = Scalar(0); + alphaRangeEnd = Scalar(2 * EIGEN_PI); } else { - headingRangeStart = -Scalar(EIGEN_PI); - headingRangeEnd = Scalar(EIGEN_PI); + alphaRangeStart = -Scalar(EIGEN_PI); + alphaRangeEnd = Scalar(EIGEN_PI); } - if (positiveRangePitch) + if (positiveRangeBeta) { - pitchRangeStart = Scalar(0); - pitchRangeEnd = Scalar(2 * EIGEN_PI); + betaRangeStart = Scalar(0); + betaRangeEnd = Scalar(2 * EIGEN_PI); } else { - pitchRangeStart = -Scalar(EIGEN_PI); - pitchRangeEnd = Scalar(EIGEN_PI); + betaRangeStart = -Scalar(EIGEN_PI); + betaRangeEnd = Scalar(EIGEN_PI); } - if (positiveRangeRoll) + if (positiveRangeGamma) { - rollRangeStart = Scalar(0); - rollRangeEnd = Scalar(2 * EIGEN_PI); + gammaRangeStart = Scalar(0); + gammaRangeEnd = Scalar(2 * EIGEN_PI); } else { - rollRangeStart = -Scalar(EIGEN_PI); - rollRangeEnd = Scalar(EIGEN_PI); + gammaRangeStart = -Scalar(EIGEN_PI); + gammaRangeEnd = Scalar(EIGEN_PI); } - const int i = EulerSystem::HeadingAxisAbs - 1; - const int j = EulerSystem::PitchAxisAbs - 1; - const int k = EulerSystem::RollAxisAbs - 1; + const int i = EulerSystem::AlphaAxisAbs - 1; + const int j = EulerSystem::BetaAxisAbs - 1; + const int k = EulerSystem::GammaAxisAbs - 1; - const int iFactor = EulerSystem::IsHeadingOpposite ? -1 : 1; - const int jFactor = EulerSystem::IsPitchOpposite ? -1 : 1; - const int kFactor = EulerSystem::IsRollOpposite ? -1 : 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::HeadingAxisVector(); - const Vector3 J = EulerAnglesType::PitchAxisVector(); - const Vector3 K = EulerAnglesType::RollAxisVector(); + 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, positiveRangeHeading, positiveRangePitch, positiveRangeRoll).coeffs(); + Vector3 eabis = EulerAnglesType(m, positiveRangeAlpha, positiveRangeBeta, positiveRangeGamma).angles(); // Check that eabis in range - VERIFY(headingRangeStart <= eabis[0] && eabis[0] <= headingRangeEnd); - VERIFY(pitchRangeStart <= eabis[1] && eabis[1] <= pitchRangeEnd); - VERIFY(rollRangeStart <= eabis[2] && eabis[2] <= rollRangeEnd); + 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); @@ -91,11 +91,11 @@ void verify_euler_ranged(const Matrix<Scalar,3,1>& ea, eabis2[2] *= kFactor; // Saturate the angles to the correct range - if (positiveRangeHeading && (eabis2[0] < 0)) + if (positiveRangeAlpha && (eabis2[0] < 0)) eabis2[0] += Scalar(2 * EIGEN_PI); - if (positiveRangePitch && (eabis2[1] < 0)) + if (positiveRangeBeta && (eabis2[1] < 0)) eabis2[1] += Scalar(2 * EIGEN_PI); - if (positiveRangeRoll && (eabis2[2] < 0)) + 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 @@ -104,7 +104,7 @@ void verify_euler_ranged(const Matrix<Scalar,3,1>& ea, VERIFY_IS_APPROX(m, mbis); // Tests that are only relevant for no possitive range - if (!(positiveRangeHeading || positiveRangePitch || positiveRangeRoll)) + if (!(positiveRangeAlpha || positiveRangeBeta || 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. */ @@ -117,7 +117,7 @@ void verify_euler_ranged(const Matrix<Scalar,3,1>& ea, // Quaternions QuaternionType q(e); - eabis = EulerAnglesType(q, positiveRangeHeading, positiveRangePitch, positiveRangeRoll).coeffs(); + eabis = EulerAnglesType(q, positiveRangeAlpha, positiveRangeBeta, positiveRangeGamma).angles(); VERIFY_IS_APPROX(eabis, eabis2);// Verify that the euler angles are still the same } |