diff options
author | Tal Hadad <tal_hd@hotmail.com> | 2015-12-20 16:24:53 +0200 |
---|---|---|
committer | Tal Hadad <tal_hd@hotmail.com> | 2015-12-20 16:24:53 +0200 |
commit | bfed274df36a9244c067b3658c057cac4e13c886 (patch) | |
tree | f7ba42e62a7185f2d807f92a2820f14551ea1cdb /unsupported/Eigen/src | |
parent | b091b7e6ea0d78718fb41a56251021e06bbb15be (diff) |
Use RotationBase, test quaternions and support ranges.
Diffstat (limited to 'unsupported/Eigen/src')
-rw-r--r-- | unsupported/Eigen/src/EulerAngles/EulerAngles.h | 137 | ||||
-rw-r--r-- | unsupported/Eigen/src/EulerAngles/EulerSystem.h | 61 |
2 files changed, 158 insertions, 40 deletions
diff --git a/unsupported/Eigen/src/EulerAngles/EulerAngles.h b/unsupported/Eigen/src/EulerAngles/EulerAngles.h index ccde28eb6..3362d9c3e 100644 --- a/unsupported/Eigen/src/EulerAngles/EulerAngles.h +++ b/unsupported/Eigen/src/EulerAngles/EulerAngles.h @@ -57,10 +57,32 @@ namespace Eigen EulerAngles() {} inline EulerAngles(Scalar a0, Scalar a1, Scalar a2) : m_angles(a0, a1, a2) {} - inline EulerAngles(const QuaternionType& q) { *this = q; } - inline EulerAngles(const AngleAxisType& aa) { *this = aa; } + template<typename Derived> inline EulerAngles(const MatrixBase<Derived>& m) { *this = m; } + + template<typename Derived> + inline EulerAngles( + const MatrixBase<Derived>& m, + bool positiveRangeHeading, + bool positiveRangePitch, + bool positiveRangeRoll) { + + fromRotation(m, positiveRangeHeading, positiveRangePitch, positiveRangeRoll); + } + + template<typename Derived> + inline EulerAngles(const RotationBase<Derived, 3>& rot) { *this = rot; } + + template<typename Derived> + inline EulerAngles( + const RotationBase<Derived, 3>& rot, + bool positiveRangeHeading, + bool positiveRangePitch, + bool positiveRangeRoll) { + + fromRotation(rot, positiveRangeHeading, positiveRangePitch, positiveRangeRoll); + } // TODO: Support assignment from euler to euler @@ -104,65 +126,108 @@ namespace Eigen /** Constructs and \returns an equivalent 3x3 rotation matrix. */ template<typename Derived> - // TODO: Add booleans which let the user control desired output angles range( (-PI, PI) or [0, 2*PI) ) - EulerAngles& fromRotationMatrix(const MatrixBase<Derived>& m) + EulerAngles& fromRotation(const MatrixBase<Derived>& m) { System::eulerAngles(*this, m); return *this; } - /** Set \c *this from a rotation matrix(i.e. pure orthogonal matrix with determinent of +1). - */ + template< + bool PositiveRangeHeading, + bool PositiveRangePitch, + bool PositiveRangeRoll, + typename Derived> + EulerAngles& fromRotation(const MatrixBase<Derived>& m) + { + System::eulerAngles<PositiveRangeHeading, PositiveRangePitch, PositiveRangeRoll>(*this, m); + return *this; + } + template<typename Derived> - EulerAngles& operator=(const MatrixBase<Derived>& mat){ - return fromRotationMatrix(mat); + EulerAngles& fromRotation( + const MatrixBase<Derived>& m, + bool positiveRangeHeading, + bool positiveRangePitch, + bool positiveRangeRoll) + { + System::eulerAngles(*this, m, positiveRangeHeading, positiveRangePitch, positiveRangeRoll); + return *this; } - - // TODO: Assign and construct from another EulerAngle (with different system) - /** Set \c *this from a quaternion. - * The axis is normalized. - */ - EulerAngles& operator=(const QuaternionType& q){ - // TODO: Implement it in a better way + template<typename Derived> + EulerAngles& fromRotation(const RotationBase<Derived, 3>& rot) + { + return fromRotation(rot.toRotationMatrix()); + } + + template< + bool PositiveRangeHeading, + bool PositiveRangePitch, + bool PositiveRangeRoll, + typename Derived> + 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); + } + + /*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. - fromRotationMatrix(q.toRotationMatrix()); - // 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))); + //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 determinent of +1). */ - - return *this; + template<typename Derived> + EulerAngles& operator=(const MatrixBase<Derived>& mat) { + return fromRotation(mat); } + + // TODO: Assign and construct from another EulerAngle (with different system) - // TODO: Support isApprox function + /** Set \c *this from a rotation. + */ + template<typename Derived> + EulerAngles& operator=(const RotationBase<Derived, 3>& rot) { + return fromRotation(rot.toRotationMatrix()); + } - /** Set \c *this from AngleAxis \a ea. - */ - EulerAngles& operator=(const AngleAxisType& ea) + // TODO: Support isApprox function + + Matrix3 toRotationMatrix() const { - // TODO: Implement it in a better way - return *this = ea.toRotationMatrix(); + return static_cast<QuaternionType>(*this).toRotationMatrix(); } - // TODO: Fix this function, and make it generic - Matrix3 toRotationMatrix(void) const + QuaternionType toQuaternion() const { - return static_cast<QuaternionType>(*this).toRotationMatrix(); + return + AngleAxisType(h(), HeadingAxisVector()) * + AngleAxisType(p(), PitchAxisVector()) * + AngleAxisType(r(), RollAxisVector()); } operator QuaternionType() const { - return - AngleAxisType((System::IsHeadingOpposite ? -1 : 1) * h(), Vector3::Unit(System::HeadingAxisAbs - 1)) * - AngleAxisType((System::IsPitchOpposite ? -1 : 1) * p(), Vector3::Unit(System::PitchAxisAbs - 1)) * - AngleAxisType((System::IsRollOpposite ? -1 : 1) * r(), Vector3::Unit(System::RollAxisAbs - 1)); + return toQuaternion(); } }; diff --git a/unsupported/Eigen/src/EulerAngles/EulerSystem.h b/unsupported/Eigen/src/EulerAngles/EulerSystem.h index ba33d5400..9699dd10d 100644 --- a/unsupported/Eigen/src/EulerAngles/EulerSystem.h +++ b/unsupported/Eigen/src/EulerAngles/EulerSystem.h @@ -19,7 +19,7 @@ namespace Eigen namespace internal { // TODO: Check if already exists on the rest API - template <int Num, bool IsPossitive = (Num > 0)> + template <int Num, bool IsPositive = (Num > 0)> struct Abs { enum { value = Num }; @@ -73,6 +73,26 @@ namespace Eigen 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 @@ -196,17 +216,50 @@ namespace Eigen public: template<typename Scalar> - static void eulerAngles(EulerAngles<Scalar, EulerSystem>& res, const typename EulerAngles<Scalar, EulerSystem>::Matrix3& mat) + static void eulerAngles( + EulerAngles<Scalar, EulerSystem>& res, + const typename EulerAngles<Scalar, EulerSystem>::Matrix3& mat) + { + eulerAngles(res, mat, false, false, false); + } + + template< + typename Scalar, + bool PositiveRangeHeading, + bool PositiveRangePitch, + bool PositiveRangeRoll> + static void eulerAngles( + EulerAngles<Scalar, EulerSystem>& res, + const typename EulerAngles<Scalar, EulerSystem>::Matrix3& mat) + { + eulerAngles(res, mat, PositiveRangeHeading, PositiveRangePitch, PositiveRangeRoll); + } + + template<typename Scalar> + static void eulerAngles( + EulerAngles<Scalar, EulerSystem>& res, + const typename EulerAngles<Scalar, EulerSystem>::Matrix3& mat, + bool positiveRangeHeading, + bool positiveRangePitch, + bool positiveRangeRoll) { eulerAngles_imp( res.coeffs(), 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()); + + // Saturate results to the requested range + if (positiveRangeHeading && (res.h() < 0)) + res.h() += Scalar(2 * EIGEN_PI); + + if (positiveRangePitch && (res.p() < 0)) + res.p() += Scalar(2 * EIGEN_PI); + + if (positiveRangeRoll && (res.r() < 0)) + res.r() += Scalar(2 * EIGEN_PI); } }; |