From 5e0a178df2b70829e990e84602fd70f92131484f Mon Sep 17 00:00:00 2001 From: Tal Hadad Date: Sun, 27 Sep 2015 16:51:24 +0300 Subject: Initial fork of unsupported module EulerAngles. --- unsupported/Eigen/EulerAngles | 32 ++++ unsupported/Eigen/src/CMakeLists.txt | 1 + unsupported/Eigen/src/EulerAngles/CMakeLists.txt | 6 + unsupported/Eigen/src/EulerAngles/EulerAngles.h | 183 ++++++++++++++++++++ unsupported/Eigen/src/EulerAngles/EulerSystem.h | 211 +++++++++++++++++++++++ unsupported/test/CMakeLists.txt | 2 + unsupported/test/EulerAngles.cpp | 18 ++ 7 files changed, 453 insertions(+) create mode 100644 unsupported/Eigen/EulerAngles create mode 100644 unsupported/Eigen/src/EulerAngles/CMakeLists.txt create mode 100644 unsupported/Eigen/src/EulerAngles/EulerAngles.h create mode 100644 unsupported/Eigen/src/EulerAngles/EulerSystem.h create mode 100644 unsupported/test/EulerAngles.cpp diff --git a/unsupported/Eigen/EulerAngles b/unsupported/Eigen/EulerAngles new file mode 100644 index 000000000..a1aa5d829 --- /dev/null +++ b/unsupported/Eigen/EulerAngles @@ -0,0 +1,32 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// Copyright (C) 2015 Tal Hadad +// +// 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_EULERANGLES_MODULE_H +#define EIGEN_EULERANGLES_MODULE_H + + +#include "Eigen/Core" +#include "Eigen/Geometry" + +#include "Eigen/src/Core/util/DisableStupidWarnings.h" + +/** + * \defgroup EulerAngles_Module EulerAngles module + * + * + * + * + */ + +#include "src/EulerAngles/EulerSystem.h" +#include "src/EulerAngles/EulerAngles.h" + +#include "Eigen/src/Core/util/ReenableStupidWarnings.h" + +#endif // EIGEN_EULERANGLES_MODULE_H diff --git a/unsupported/Eigen/src/CMakeLists.txt b/unsupported/Eigen/src/CMakeLists.txt index 8eb2808e3..fae1c5854 100644 --- a/unsupported/Eigen/src/CMakeLists.txt +++ b/unsupported/Eigen/src/CMakeLists.txt @@ -12,3 +12,4 @@ ADD_SUBDIRECTORY(Skyline) ADD_SUBDIRECTORY(SparseExtra) ADD_SUBDIRECTORY(KroneckerProduct) ADD_SUBDIRECTORY(Splines) +ADD_SUBDIRECTORY(EulerAngles) diff --git a/unsupported/Eigen/src/EulerAngles/CMakeLists.txt b/unsupported/Eigen/src/EulerAngles/CMakeLists.txt new file mode 100644 index 000000000..7986afc5e --- /dev/null +++ b/unsupported/Eigen/src/EulerAngles/CMakeLists.txt @@ -0,0 +1,6 @@ +FILE(GLOB Eigen_IterativeSolvers_SRCS "*.h") + +INSTALL(FILES + ${Eigen_IterativeSolvers_SRCS} + DESTINATION ${INCLUDE_INSTALL_DIR}/unsupported/Eigen/src/IterativeSolvers COMPONENT Devel + ) diff --git a/unsupported/Eigen/src/EulerAngles/EulerAngles.h b/unsupported/Eigen/src/EulerAngles/EulerAngles.h new file mode 100644 index 000000000..b3bd66441 --- /dev/null +++ b/unsupported/Eigen/src/EulerAngles/EulerAngles.h @@ -0,0 +1,183 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// Copyright (C) 2015 Tal Hadad +// +// 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_EULERANGLESCLASS_H// TODO: Fix previous "EIGEN_EULERANGLES_H" definition? +#define EIGEN_EULERANGLESCLASS_H + +namespace Eigen +{ + /*template + struct ei_eulerangles_assign_impl;*/ + + /** \class EulerAngles + * + * \brief Represents a rotation in a 3 dimensional space as three Euler angles + * + * \param _Scalar the scalar type, i.e., the type of the angles. + * + * \sa cl + */ + template + class EulerAngles : public RotationBase, 3> + { + public: + /** the scalar type of the coefficients */ + typedef _Scalar Scalar; + typedef _System System; + + typedef Matrix Matrix3; + typedef Matrix Vector3; + typedef Quaternion QuaternionType; + typedef AngleAxis AngleAxisType; + + protected: + + Vector3 m_angles; + + public: + + EulerAngles() {} + inline EulerAngles(Scalar a0, Scalar a1, Scalar a2) : m_angles(a0, a1, a2) {} + inline EulerAngles(Vector3 angles) : m_angles(angles) {} + inline EulerAngles(const QuaternionType& q) { *this = q; } + inline EulerAngles(const AngleAxisType& aa) { *this = aa; } + template + inline EulerAngles(const MatrixBase& m) { *this = m; } + + // TODO: Support assignment from euler to euler + + Scalar angle(int i) const { return m_angles.coeff(i); } + Scalar& angle(int i) { return m_angles.coeffRef(i); } + + const Vector3& coeffs() const { return m_angles; } + Vector3& coeffs() { return m_angles; } + + // TODO: Add set/get functions + + Scalar h() const { return m_angles[0]; } + Scalar& h() { return m_angles[0]; } + + 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; + } + + EulerAngles inverse() const + { + EulerAngles res; + res.m_angles = -m_angles; + return res; + } + + EulerAngles operator -() const + { + return inverse(); + } + + /** Constructs and \returns an equivalent 3x3 rotation matrix. + */ + template + // TODO: Add booleans which let the user control desired output angles range( (-PI, PI) or [0, 2*PI) ) + EulerAngles& fromRotationMatrix(const MatrixBase& m) + { + System::eulerAngles(*this, m); + return *this; + } + + /** Set \c *this from a rotation matrix(i.e. pure orthogonal matrix with determinent of +1). + */ + template + EulerAngles& operator=(const MatrixBase& mat){ + return fromRotationMatrix(mat); + } + + // 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 + // 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. + // 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))); + */ + + return *this; + } + + /** Set \c *this from AngleAxis \a ea. + */ + EulerAngles& operator=(const AngleAxisType& ea) + { + // TODO: Implement it in a better way + return *this = ea.toRotationMatrix(); + } + + // TODO: Fix this function, and make it generic + Matrix3 toRotationMatrix(void) const + { + return static_cast(*this).toRotationMatrix(); + } + + 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)); + } + }; + + typedef EulerAngles EulerAnglesXYZd; + typedef EulerAngles EulerAnglesXYXd; + typedef EulerAngles EulerAnglesXZYd; + typedef EulerAngles EulerAnglesXZXd; + + typedef EulerAngles EulerAnglesYZXd; + typedef EulerAngles EulerAnglesYZYd; + typedef EulerAngles EulerAnglesYXZd; + typedef EulerAngles EulerAnglesYXYd; + + typedef EulerAngles EulerAnglesZXYd; + typedef EulerAngles EulerAnglesZXZd; + typedef EulerAngles EulerAnglesZYXd; + typedef EulerAngles EulerAnglesZYZd; + + namespace internal + { + template + struct traits > + { + typedef _Scalar Scalar; + }; + } + +} + +#endif // EIGEN_EULERANGLESCLASS_H diff --git a/unsupported/Eigen/src/EulerAngles/EulerSystem.h b/unsupported/Eigen/src/EulerAngles/EulerSystem.h new file mode 100644 index 000000000..fc782e914 --- /dev/null +++ b/unsupported/Eigen/src/EulerAngles/EulerSystem.h @@ -0,0 +1,211 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// Copyright (C) 2015 Tal Hadad +// +// 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 + class EulerAngles; + + namespace internal + { + // TODO: Check if already exists on the rest API + template 0)> + struct Abs + { + enum { value = Num }; + }; + + template + struct Abs + { + enum { value = -Num }; + }; + + template + struct NegateIf + { + template + static void run(T& t) + { + t = -t; + } + }; + + template <> + struct NegateIf + { + template + static void run(T& t) + { + // no op + } + }; + + template + struct NegateIfXor : NegateIf {}; + + template + struct IsValidAxis + { + enum { value = Axis != 0 && Abs::value <= 3 }; + }; + } + + enum EulerAxis + { + EULER_X = 1, + EULER_Y = 2, + EULER_Z = 3 + }; + + template + 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; + + enum + { + HeadingAxisAbs = internal::Abs::value, + PitchAxisAbs = internal::Abs::value, + RollAxisAbs = internal::Abs::value, + + IsHeadingOpposite = (HeadingAxis < 0) ? 1 : 0, + IsPitchOpposite = (PitchAxis < 0) ? 1 : 0, + IsRollOpposite = (RollAxis < 0) ? 1 : 0, + + IsOdd = ((HeadingAxisAbs)%3 == (PitchAxisAbs - 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::value && internal::IsValidAxis::value && internal::IsValidAxis::value) ? 1 : 0, + + // TODO: After a proper assertation, remove the "IsValid" from this expression + IsTaitBryan = (IsValid && (unsigned)HeadingAxisAbs != (unsigned)RollAxisAbs) ? 1 : 0 + }; + + private: + + 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 = HeadingAxisAbs - 1, + J = (HeadingAxisAbs - 1 + 1 + IsOdd)%3, + K = (HeadingAxisAbs - 1 + 2 - IsOdd)%3 + }; + + template + static void eulerAngles_imp(Matrix::Scalar, 3, 1>& res, const MatrixBase& mat, internal::true_type isTaitBryan) + { + using std::atan2; + using std::sin; + using std::cos; + + 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))) { + res[0] = (res[0] > Scalar(0)) ? res[0] - Scalar(M_PI) : res[0] + Scalar(M_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 + static void eulerAngles_imp(Matrix::Scalar,3,1>& res, const MatrixBase& mat, internal::false_type isTaitBryan) + { + using std::atan2; + using std::sin; + using std::cos; + + typedef typename Derived::Scalar Scalar; + typedef Matrix Vector2; + + res[0] = atan2(mat(J,I), mat(K,I)); + if((IsOdd && res[0]Scalar(0))) + { + res[0] = (res[0] > Scalar(0)) ? res[0] - Scalar(M_PI) : res[0] + Scalar(M_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)); + } + + public: + + // TODO: Support headingAxisVector(), .. + + template + static void eulerAngles(EulerAngles& res, const typename EulerAngles::Matrix3& mat) + { + eulerAngles_imp( + res.coeffs(), mat, + typename internal::conditional::type()); + + internal::NegateIfXor::run(res.h()); + + internal::NegateIfXor::run(res.p()); + + internal::NegateIfXor::run(res.r()); + } + }; + + typedef EulerSystem EulerSystemXYZ; + typedef EulerSystem EulerSystemXYX; + typedef EulerSystem EulerSystemXZY; + typedef EulerSystem EulerSystemXZX; + + typedef EulerSystem EulerSystemYZX; + typedef EulerSystem EulerSystemYZY; + typedef EulerSystem EulerSystemYXZ; + typedef EulerSystem EulerSystemYXY; + + typedef EulerSystem EulerSystemZXY; + typedef EulerSystem EulerSystemZXZ; + typedef EulerSystem EulerSystemZYX; + typedef EulerSystem EulerSystemZYZ; +} + +#endif // EIGEN_EULERSYSTEM_H diff --git a/unsupported/test/CMakeLists.txt b/unsupported/test/CMakeLists.txt index 79e70ced4..653392e40 100644 --- a/unsupported/test/CMakeLists.txt +++ b/unsupported/test/CMakeLists.txt @@ -45,6 +45,8 @@ ei_add_test(alignedvector3) ei_add_test(FFT) +ei_add_test(EulerAngles) + find_package(MPFR 2.3.0) find_package(GMP) if(MPFR_FOUND) diff --git a/unsupported/test/EulerAngles.cpp b/unsupported/test/EulerAngles.cpp new file mode 100644 index 000000000..d03db1ac3 --- /dev/null +++ b/unsupported/test/EulerAngles.cpp @@ -0,0 +1,18 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// Copyright (C) 2015 Tal Hadad +// +// 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/. + +#include "main.h" + +#include + +void test_EulerAngles() +{ + //CALL_SUBTEST( test_return_by_value(32) ); + +} -- cgit v1.2.3 From 6752a69aa521d4a0979d575af79b4acfd54dd089 Mon Sep 17 00:00:00 2001 From: Tal Hadad Date: Sun, 20 Dec 2015 12:49:12 +0200 Subject: Much better tests, and a little bit more functionality. --- unsupported/Eigen/src/EulerAngles/EulerAngles.h | 20 +++- unsupported/Eigen/src/EulerAngles/EulerSystem.h | 26 ++++- unsupported/test/EulerAngles.cpp | 121 +++++++++++++++++++++++- 3 files changed, 158 insertions(+), 9 deletions(-) diff --git a/unsupported/Eigen/src/EulerAngles/EulerAngles.h b/unsupported/Eigen/src/EulerAngles/EulerAngles.h index b3bd66441..ccde28eb6 100644 --- a/unsupported/Eigen/src/EulerAngles/EulerAngles.h +++ b/unsupported/Eigen/src/EulerAngles/EulerAngles.h @@ -37,16 +37,26 @@ namespace Eigen typedef Matrix Vector3; typedef Quaternion QuaternionType; typedef AngleAxis AngleAxisType; + + static Vector3 HeadingAxisVector() { + return internal::NegativeIf::run(Vector3::Unit(System::HeadingAxisAbs - 1)); + } + + static Vector3 PitchAxisVector() { + return internal::NegativeIf::run(Vector3::Unit(System::PitchAxisAbs - 1)); + } + + static Vector3 RollAxisVector() { + return internal::NegativeIf::run(Vector3::Unit(System::RollAxisAbs - 1)); + } - protected: - + private: Vector3 m_angles; public: EulerAngles() {} inline EulerAngles(Scalar a0, Scalar a1, Scalar a2) : m_angles(a0, a1, a2) {} - inline EulerAngles(Vector3 angles) : m_angles(angles) {} inline EulerAngles(const QuaternionType& q) { *this = q; } inline EulerAngles(const AngleAxisType& aa) { *this = aa; } template @@ -116,7 +126,7 @@ namespace Eigen EulerAngles& operator=(const QuaternionType& q){ // TODO: Implement it in a better way // 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. + // 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()); @@ -131,6 +141,8 @@ namespace Eigen return *this; } + // TODO: Support isApprox function + /** Set \c *this from AngleAxis \a ea. */ EulerAngles& operator=(const AngleAxisType& ea) diff --git a/unsupported/Eigen/src/EulerAngles/EulerSystem.h b/unsupported/Eigen/src/EulerAngles/EulerSystem.h index fc782e914..6ee3f51df 100644 --- a/unsupported/Eigen/src/EulerAngles/EulerSystem.h +++ b/unsupported/Eigen/src/EulerAngles/EulerSystem.h @@ -31,6 +31,26 @@ namespace Eigen enum { value = -Num }; }; + template + struct NegativeIf + { + template + static T run(const T& t) + { + return -t; + } + }; + + template <> + struct NegativeIf + { + template + static T run(const T& t) + { + return t; + } + }; + template struct NegateIf { @@ -45,7 +65,7 @@ namespace Eigen struct NegateIf { template - static void run(T& t) + static void run(T&) { // no op } @@ -113,7 +133,7 @@ namespace Eigen }; template - static void eulerAngles_imp(Matrix::Scalar, 3, 1>& res, const MatrixBase& mat, internal::true_type isTaitBryan) + static void eulerAngles_imp(Matrix::Scalar, 3, 1>& res, const MatrixBase& mat, internal::true_type /*isTaitBryan*/) { using std::atan2; using std::sin; @@ -136,7 +156,7 @@ namespace Eigen } template - static void eulerAngles_imp(Matrix::Scalar,3,1>& res, const MatrixBase& mat, internal::false_type isTaitBryan) + static void eulerAngles_imp(Matrix::Scalar,3,1>& res, const MatrixBase& mat, internal::false_type /*isTaitBryan*/) { using std::atan2; using std::sin; diff --git a/unsupported/test/EulerAngles.cpp b/unsupported/test/EulerAngles.cpp index d03db1ac3..57a34776a 100644 --- a/unsupported/test/EulerAngles.cpp +++ b/unsupported/test/EulerAngles.cpp @@ -11,8 +11,125 @@ #include -void test_EulerAngles() +using namespace Eigen; + +template +void verify_euler(const Matrix& ea) +{ + typedef EulerAngles EulerAnglesType; + typedef Matrix Matrix3; + typedef Matrix Vector3; + typedef AngleAxis AngleAxisx; + using std::abs; + + const int i = EulerSystem::HeadingAxisAbs - 1; + const int j = EulerSystem::PitchAxisAbs - 1; + const int k = EulerSystem::RollAxisAbs - 1; + + const int iFactor = EulerSystem::IsHeadingOpposite ? -1 : 1; + const int jFactor = EulerSystem::IsPitchOpposite ? -1 : 1; + const int kFactor = EulerSystem::IsRollOpposite ? -1 : 1; + + const Vector3 I = EulerAnglesType::HeadingAxisVector(); + const Vector3 J = EulerAnglesType::PitchAxisVector(); + const Vector3 K = EulerAnglesType::RollAxisVector(); + + EulerAnglesType e(ea[0], ea[1], ea[2]); + + Matrix3 m(e); + Vector3 eabis = EulerAnglesType(m).coeffs(); + Vector3 eabis2 = m.eulerAngles(i, j, k); + eabis2[0] *= iFactor; + eabis2[1] *= jFactor; + eabis2[2] *= kFactor; + + VERIFY_IS_APPROX(eabis, eabis2);// Verify that our estimation is the same as m.eulerAngles() is + + Matrix3 mbis(AngleAxisx(eabis[0], I) * AngleAxisx(eabis[1], J) * AngleAxisx(eabis[2], K)); + VERIFY_IS_APPROX(m, mbis); + /* 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_IS_APPROX_OR_LESS_THAN(eabis[0], Scalar(EIGEN_PI)); + VERIFY_IS_APPROX_OR_LESS_THAN(-Scalar(EIGEN_PI), eabis[1]); + VERIFY_IS_APPROX_OR_LESS_THAN(eabis[1], Scalar(EIGEN_PI)); + VERIFY_IS_APPROX_OR_LESS_THAN(-Scalar(EIGEN_PI), eabis[2]); + VERIFY_IS_APPROX_OR_LESS_THAN(eabis[2], Scalar(EIGEN_PI)); +} + +template void check_all_var(const Matrix& ea) +{ + verify_euler(ea); + verify_euler(ea); + verify_euler(ea); + verify_euler(ea); + + verify_euler(ea); + verify_euler(ea); + verify_euler(ea); + verify_euler(ea); + + verify_euler(ea); + verify_euler(ea); + verify_euler(ea); + verify_euler(ea); +} + +template void eulerangles() { - //CALL_SUBTEST( test_return_by_value(32) ); + typedef Matrix Matrix3; + typedef Matrix Vector3; + typedef Array Array3; + typedef Quaternion Quaternionx; + typedef AngleAxis AngleAxisx; + + Scalar a = internal::random(-Scalar(EIGEN_PI), Scalar(EIGEN_PI)); + Quaternionx q1; + q1 = AngleAxisx(a, Vector3::Random().normalized()); + Matrix3 m; + m = q1; + + Vector3 ea = m.eulerAngles(0,1,2); + check_all_var(ea); + ea = m.eulerAngles(0,1,0); + check_all_var(ea); + + // Check with purely random Quaternion: + q1.coeffs() = Quaternionx::Coefficients::Random().normalized(); + m = q1; + ea = m.eulerAngles(0,1,2); + check_all_var(ea); + ea = m.eulerAngles(0,1,0); + check_all_var(ea); + + // Check with random angles in range [0:pi]x[-pi:pi]x[-pi:pi]. + ea = (Array3::Random() + Array3(1,0,0))*Scalar(EIGEN_PI)*Array3(0.5,1,1); + check_all_var(ea); + + ea[2] = ea[0] = internal::random(0,Scalar(EIGEN_PI)); + check_all_var(ea); + ea[0] = ea[1] = internal::random(0,Scalar(EIGEN_PI)); + check_all_var(ea); + + ea[1] = 0; + check_all_var(ea); + + ea.head(2).setZero(); + check_all_var(ea); + + ea.setZero(); + check_all_var(ea); +} + +void test_EulerAngles() +{ + for(int i = 0; i < g_repeat; i++) { + CALL_SUBTEST_1( eulerangles() ); + CALL_SUBTEST_2( eulerangles() ); + } } -- cgit v1.2.3 From b091b7e6ea0d78718fb41a56251021e06bbb15be Mon Sep 17 00:00:00 2001 From: Tal Hadad Date: Sun, 20 Dec 2015 13:00:07 +0200 Subject: Remove unneccesary comment. --- unsupported/Eigen/src/EulerAngles/EulerSystem.h | 2 -- 1 file changed, 2 deletions(-) diff --git a/unsupported/Eigen/src/EulerAngles/EulerSystem.h b/unsupported/Eigen/src/EulerAngles/EulerSystem.h index 6ee3f51df..ba33d5400 100644 --- a/unsupported/Eigen/src/EulerAngles/EulerSystem.h +++ b/unsupported/Eigen/src/EulerAngles/EulerSystem.h @@ -194,8 +194,6 @@ namespace Eigen } public: - - // TODO: Support headingAxisVector(), .. template static void eulerAngles(EulerAngles& res, const typename EulerAngles::Matrix3& mat) -- cgit v1.2.3 From bfed274df36a9244c067b3658c057cac4e13c886 Mon Sep 17 00:00:00 2001 From: Tal Hadad Date: Sun, 20 Dec 2015 16:24:53 +0200 Subject: Use RotationBase, test quaternions and support ranges. --- unsupported/Eigen/src/EulerAngles/EulerAngles.h | 137 +++++++++++++++++------- unsupported/Eigen/src/EulerAngles/EulerSystem.h | 61 ++++++++++- unsupported/test/EulerAngles.cpp | 137 ++++++++++++++++++------ 3 files changed, 263 insertions(+), 72 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 inline EulerAngles(const MatrixBase& m) { *this = m; } + + template + inline EulerAngles( + const MatrixBase& m, + bool positiveRangeHeading, + bool positiveRangePitch, + bool positiveRangeRoll) { + + fromRotation(m, positiveRangeHeading, positiveRangePitch, positiveRangeRoll); + } + + template + inline EulerAngles(const RotationBase& rot) { *this = rot; } + + template + inline EulerAngles( + const RotationBase& 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 - // TODO: Add booleans which let the user control desired output angles range( (-PI, PI) or [0, 2*PI) ) - EulerAngles& fromRotationMatrix(const MatrixBase& m) + EulerAngles& fromRotation(const MatrixBase& 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& m) + { + System::eulerAngles(*this, m); + return *this; + } + template - EulerAngles& operator=(const MatrixBase& mat){ - return fromRotationMatrix(mat); + EulerAngles& fromRotation( + const MatrixBase& 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 + EulerAngles& fromRotation(const RotationBase& rot) + { + return fromRotation(rot.toRotationMatrix()); + } + + template< + bool PositiveRangeHeading, + bool PositiveRangePitch, + bool PositiveRangeRoll, + typename Derived> + EulerAngles& fromRotation(const RotationBase& rot) + { + return fromRotation(rot.toRotationMatrix()); + } + + template + EulerAngles& fromRotation( + const RotationBase& 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 + EulerAngles& operator=(const MatrixBase& 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 + EulerAngles& operator=(const RotationBase& 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(*this).toRotationMatrix(); } - // TODO: Fix this function, and make it generic - Matrix3 toRotationMatrix(void) const + QuaternionType toQuaternion() const { - return static_cast(*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 0)> + template 0)> struct Abs { enum { value = Num }; @@ -73,6 +73,26 @@ namespace Eigen template struct NegateIfXor : NegateIf {}; + + template + struct AddConstIf + { + template + static void run(T& t) + { + t += T(value); + } + }; + + template + struct AddConstIf + { + template + static void run(T&) + { + // no op + } + }; template struct IsValidAxis @@ -196,17 +216,50 @@ namespace Eigen public: template - static void eulerAngles(EulerAngles& res, const typename EulerAngles::Matrix3& mat) + static void eulerAngles( + EulerAngles& res, + const typename EulerAngles::Matrix3& mat) + { + eulerAngles(res, mat, false, false, false); + } + + template< + typename Scalar, + bool PositiveRangeHeading, + bool PositiveRangePitch, + bool PositiveRangeRoll> + static void eulerAngles( + EulerAngles& res, + const typename EulerAngles::Matrix3& mat) + { + eulerAngles(res, mat, PositiveRangeHeading, PositiveRangePitch, PositiveRangeRoll); + } + + template + static void eulerAngles( + EulerAngles& res, + const typename EulerAngles::Matrix3& mat, + bool positiveRangeHeading, + bool positiveRangePitch, + bool positiveRangeRoll) { eulerAngles_imp( res.coeffs(), mat, typename internal::conditional::type()); internal::NegateIfXor::run(res.h()); - internal::NegateIfXor::run(res.p()); - internal::NegateIfXor::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); } }; diff --git a/unsupported/test/EulerAngles.cpp b/unsupported/test/EulerAngles.cpp index 57a34776a..e65399ffa 100644 --- a/unsupported/test/EulerAngles.cpp +++ b/unsupported/test/EulerAngles.cpp @@ -14,14 +14,53 @@ using namespace Eigen; template -void verify_euler(const Matrix& ea) +void verify_euler_ranged(const Matrix& ea, + bool positiveRangeHeading, bool positiveRangePitch, bool positiveRangeRoll) { typedef EulerAngles EulerAnglesType; typedef Matrix Matrix3; typedef Matrix Vector3; - typedef AngleAxis AngleAxisx; + typedef Quaternion QuaternionType; + typedef AngleAxis AngleAxisType; using std::abs; + Scalar headingRangeStart, headingRangeEnd; + Scalar pitchRangeStart, pitchRangeEnd; + Scalar rollRangeStart, rollRangeEnd; + + if (positiveRangeHeading) + { + headingRangeStart = Scalar(0); + headingRangeEnd = Scalar(2 * EIGEN_PI); + } + else + { + headingRangeStart = -Scalar(EIGEN_PI); + headingRangeEnd = Scalar(EIGEN_PI); + } + + if (positiveRangePitch) + { + pitchRangeStart = Scalar(0); + pitchRangeEnd = Scalar(2 * EIGEN_PI); + } + else + { + pitchRangeStart = -Scalar(EIGEN_PI); + pitchRangeEnd = Scalar(EIGEN_PI); + } + + if (positiveRangeRoll) + { + rollRangeStart = Scalar(0); + rollRangeEnd = Scalar(2 * EIGEN_PI); + } + else + { + rollRangeStart = -Scalar(EIGEN_PI); + rollRangeEnd = Scalar(EIGEN_PI); + } + const int i = EulerSystem::HeadingAxisAbs - 1; const int j = EulerSystem::PitchAxisAbs - 1; const int k = EulerSystem::RollAxisAbs - 1; @@ -37,46 +76,80 @@ void verify_euler(const Matrix& ea) EulerAnglesType e(ea[0], ea[1], ea[2]); Matrix3 m(e); - Vector3 eabis = EulerAnglesType(m).coeffs(); + Vector3 eabis = EulerAnglesType(m, positiveRangeHeading, positiveRangePitch, positiveRangeRoll).coeffs(); + + // 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); + 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 (positiveRangeHeading && (eabis2[0] < 0)) + eabis2[0] += Scalar(2 * EIGEN_PI); + if (positiveRangePitch && (eabis2[1] < 0)) + eabis2[1] += Scalar(2 * EIGEN_PI); + if (positiveRangeRoll && (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 - Matrix3 mbis(AngleAxisx(eabis[0], I) * AngleAxisx(eabis[1], J) * AngleAxisx(eabis[2], K)); + Matrix3 mbis(AngleAxisType(eabis[0], I) * AngleAxisType(eabis[1], J) * AngleAxisType(eabis[2], K)); VERIFY_IS_APPROX(m, mbis); - /* 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_IS_APPROX_OR_LESS_THAN(eabis[0], Scalar(EIGEN_PI)); - VERIFY_IS_APPROX_OR_LESS_THAN(-Scalar(EIGEN_PI), eabis[1]); - VERIFY_IS_APPROX_OR_LESS_THAN(eabis[1], Scalar(EIGEN_PI)); - VERIFY_IS_APPROX_OR_LESS_THAN(-Scalar(EIGEN_PI), eabis[2]); - VERIFY_IS_APPROX_OR_LESS_THAN(eabis[2], Scalar(EIGEN_PI)); + + // Tests that are only relevant for no possitive range + if (!(positiveRangeHeading || positiveRangePitch || positiveRangeRoll)) + { + /* 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))); + } + + // Quaternions + QuaternionType q(e); + eabis = EulerAnglesType(q, positiveRangeHeading, positiveRangePitch, positiveRangeRoll).coeffs(); + 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); } template void check_all_var(const Matrix& ea) { - verify_euler(ea); - verify_euler(ea); - verify_euler(ea); - verify_euler(ea); - - verify_euler(ea); - verify_euler(ea); - verify_euler(ea); - verify_euler(ea); - - verify_euler(ea); - verify_euler(ea); - verify_euler(ea); - verify_euler(ea); + verify_euler(ea); + verify_euler(ea); + verify_euler(ea); + verify_euler(ea); + + verify_euler(ea); + verify_euler(ea); + verify_euler(ea); + verify_euler(ea); + + verify_euler(ea); + verify_euler(ea); + verify_euler(ea); + verify_euler(ea); } template void eulerangles() @@ -85,11 +158,11 @@ template void eulerangles() typedef Matrix Vector3; typedef Array Array3; typedef Quaternion Quaternionx; - typedef AngleAxis AngleAxisx; + typedef AngleAxis AngleAxisType; Scalar a = internal::random(-Scalar(EIGEN_PI), Scalar(EIGEN_PI)); Quaternionx q1; - q1 = AngleAxisx(a, Vector3::Random().normalized()); + q1 = AngleAxisType(a, Vector3::Random().normalized()); Matrix3 m; m = q1; -- cgit v1.2.3 From c006ecace15602e8c4b9a543af108a1dadc080db Mon Sep 17 00:00:00 2001 From: Tal Hadad Date: Sun, 20 Dec 2015 20:07:06 +0200 Subject: Fix comments --- unsupported/Eigen/src/EulerAngles/EulerAngles.h | 6 +----- 1 file changed, 1 insertion(+), 5 deletions(-) diff --git a/unsupported/Eigen/src/EulerAngles/EulerAngles.h b/unsupported/Eigen/src/EulerAngles/EulerAngles.h index 3362d9c3e..d21f52491 100644 --- a/unsupported/Eigen/src/EulerAngles/EulerAngles.h +++ b/unsupported/Eigen/src/EulerAngles/EulerAngles.h @@ -84,16 +84,12 @@ namespace Eigen fromRotation(rot, positiveRangeHeading, positiveRangePitch, positiveRangeRoll); } - // TODO: Support assignment from euler to euler - Scalar angle(int i) const { return m_angles.coeff(i); } Scalar& angle(int i) { return m_angles.coeffRef(i); } const Vector3& coeffs() const { return m_angles; } Vector3& coeffs() { return m_angles; } - // TODO: Add set/get functions - Scalar h() const { return m_angles[0]; } Scalar& h() { return m_angles[0]; } @@ -201,7 +197,7 @@ namespace Eigen return fromRotation(mat); } - // TODO: Assign and construct from another EulerAngle (with different system) + // TODO: Assign and construct from another EulerAngles (with different system) /** Set \c *this from a rotation. */ -- cgit v1.2.3 From 2aaaf22623cdd92a772fc7e0f6523ae2c423e5de Mon Sep 17 00:00:00 2001 From: Tal Hadad Date: Thu, 2 Jun 2016 22:12:57 +0300 Subject: Fix Gael reports (except documention) - "Scalar angle(int) const" should be "const Vector& angles() const" - then method "coeffs" could be removed. - avoid one letter names like h, p, r -> use alpha(), beta(), gamma() ;) - about the "fromRotation" methods: - replace the ones which are not static by operator= (as in Quaternion) - the others are actually static methods: use a capital F: FromRotation - method "invert" should be removed. - use a macro to define both float and double EulerAnglesXYZ* typedefs - AddConstIf -> not used - no needs for NegateIfXor, compilers are extremely good at optimizing away branches based on compile time constants: if(IsHeadingOpposite-=IsEven) res.alpha() = -res.alpha(); --- unsupported/Eigen/EulerAngles | 18 ++- unsupported/Eigen/src/EulerAngles/EulerAngles.h | 168 ++++++++++-------------- unsupported/Eigen/src/EulerAngles/EulerSystem.h | 153 +++++++-------------- 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 + * \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, 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 QuaternionType; typedef AngleAxis AngleAxisType; - static Vector3 HeadingAxisVector() { - return internal::NegativeIf::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::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::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 inline EulerAngles(const MatrixBase& m) { *this = m; } @@ -64,11 +67,11 @@ namespace Eigen template inline EulerAngles( const MatrixBase& 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 @@ -77,35 +80,24 @@ namespace Eigen template inline EulerAngles( const RotationBase& 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 - EulerAngles& fromRotation(const MatrixBase& 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& m) + static EulerAngles FromRotation(const MatrixBase& m) { - System::eulerAngles(*this, m); - return *this; - } - - template - EulerAngles& fromRotation( - const MatrixBase& m, - bool positiveRangeHeading, - bool positiveRangePitch, - bool positiveRangeRoll) - { - System::eulerAngles(*this, m, positiveRangeHeading, positiveRangePitch, positiveRangeRoll); - return *this; - } - - template - EulerAngles& fromRotation(const RotationBase& rot) - { - return fromRotation(rot.toRotationMatrix()); + EulerAngles e; + System::CalcEulerAngles(e, m); + return e; } template< - bool PositiveRangeHeading, - bool PositiveRangePitch, - bool PositiveRangeRoll, + bool PositiveRangeAlpha, + bool PositiveRangeBeta, + bool PositiveRangeGamma, typename Derived> - EulerAngles& fromRotation(const RotationBase& rot) + static EulerAngles& FromRotation(const RotationBase& rot) { - return fromRotation(rot.toRotationMatrix()); - } - - template - EulerAngles& fromRotation( - const RotationBase& rot, - bool positiveRangeHeading, - bool positiveRangePitch, - bool positiveRangeRoll) - { - return fromRotation(rot.toRotationMatrix(), positiveRangeHeading, positiveRangePitch, positiveRangeRoll); + return FromRotation(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 - EulerAngles& operator=(const MatrixBase& mat) { - return fromRotation(mat); + EulerAngles& operator=(const MatrixBase& m) { + System::CalcEulerAngles(*this, m); + return *this; } // TODO: Assign and construct from another EulerAngles (with different system) @@ -203,7 +163,8 @@ namespace Eigen */ template EulerAngles& operator=(const RotationBase& 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 EulerAnglesXYZd; - typedef EulerAngles EulerAnglesXYXd; - typedef EulerAngles EulerAnglesXZYd; - typedef EulerAngles EulerAnglesXZXd; - - typedef EulerAngles EulerAnglesYZXd; - typedef EulerAngles EulerAnglesYZYd; - typedef EulerAngles EulerAnglesYXZd; - typedef EulerAngles EulerAnglesYXYd; - - typedef EulerAngles EulerAnglesZXYd; - typedef EulerAngles EulerAnglesZXZd; - typedef EulerAngles EulerAnglesZYXd; - typedef EulerAngles EulerAnglesZYZd; +#define EIGEN_EULER_ANGLES_SINGLE_TYPEDEF(SYSTEM, SCALAR_TYPE, SCALAR_POSTFIX) \ + typedef EulerAngles 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 - struct NegativeIf - { - template - static T run(const T& t) - { - return -t; - } - }; - - template <> - struct NegativeIf - { - template - static T run(const T& t) - { - return t; - } - }; - - template - struct NegateIf - { - template - static void run(T& t) - { - t = -t; - } - }; - - template <> - struct NegateIf - { - template - static void run(T&) - { - // no op - } - }; - - template - struct NegateIfXor : NegateIf {}; - - template - struct AddConstIf - { - template - static void run(T& t) - { - t += T(value); - } - }; - - template - struct AddConstIf - { - template - static void run(T&) - { - // no op - } - }; template struct IsValidAxis @@ -108,36 +45,36 @@ namespace Eigen EULER_Z = 3 }; - template + template 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::value, - PitchAxisAbs = internal::Abs::value, - RollAxisAbs = internal::Abs::value, + AlphaAxisAbs = internal::Abs::value, + BetaAxisAbs = internal::Abs::value, + GammaAxisAbs = internal::Abs::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::value && internal::IsValidAxis::value && internal::IsValidAxis::value) ? 1 : 0, + IsValid = ((unsigned)AlphaAxisAbs != (unsigned)BetaAxisAbs && + (unsigned)BetaAxisAbs != (unsigned)GammaAxisAbs && + internal::IsValidAxis::value && internal::IsValidAxis::value && internal::IsValidAxis::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 - static void eulerAngles_imp(Matrix::Scalar, 3, 1>& res, const MatrixBase& mat, internal::true_type /*isTaitBryan*/) + static void CalcEulerAngles_imp(Matrix::Scalar, 3, 1>& res, const MatrixBase& mat, internal::true_type /*isTaitBryan*/) { using std::atan2; using std::sin; @@ -176,7 +114,7 @@ namespace Eigen } template - static void eulerAngles_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; @@ -216,50 +154,55 @@ namespace Eigen public: template - static void eulerAngles( + static void CalcEulerAngles( EulerAngles& res, const typename EulerAngles::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& res, const typename EulerAngles::Matrix3& mat) { - eulerAngles(res, mat, PositiveRangeHeading, PositiveRangePitch, PositiveRangeRoll); + CalcEulerAngles(res, mat, PositiveRangeAlpha, PositiveRangeBeta, PositiveRangeGamma); } template - static void eulerAngles( + static void CalcEulerAngles( EulerAngles& res, const typename EulerAngles::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::type()); - internal::NegateIfXor::run(res.h()); - internal::NegateIfXor::run(res.p()); - internal::NegateIfXor::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 void verify_euler_ranged(const Matrix& ea, - bool positiveRangeHeading, bool positiveRangePitch, bool positiveRangeRoll) + bool positiveRangeAlpha, bool positiveRangeBeta, bool positiveRangeGamma) { typedef EulerAngles EulerAnglesType; typedef Matrix Matrix3; @@ -24,64 +24,64 @@ void verify_euler_ranged(const Matrix& ea, typedef AngleAxis 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& 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& 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& 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 } -- cgit v1.2.3 From e30133e439c98fa5ec981339fc3d6bc6be7f2e1b Mon Sep 17 00:00:00 2001 From: Tal Hadad Date: Mon, 6 Jun 2016 22:01:40 +0300 Subject: Doc EulerAngles class, and minor fixes. --- unsupported/Eigen/EulerAngles | 2 + unsupported/Eigen/src/EulerAngles/EulerAngles.h | 97 +++++++++++++++++++++---- unsupported/Eigen/src/EulerAngles/EulerSystem.h | 35 +++++---- 3 files changed, 104 insertions(+), 30 deletions(-) diff --git a/unsupported/Eigen/EulerAngles b/unsupported/Eigen/EulerAngles index 08cc74237..5e1d2ccba 100644 --- a/unsupported/Eigen/EulerAngles +++ b/unsupported/Eigen/EulerAngles @@ -22,6 +22,8 @@ namespace Eigen { * \defgroup EulerAngles_Module EulerAngles module * \brief This module provides generic euler angles rotation. * + * See EulerAngles for more information. + * * \code * #include * \endcode diff --git a/unsupported/Eigen/src/EulerAngles/EulerAngles.h b/unsupported/Eigen/src/EulerAngles/EulerAngles.h index 66050a426..4082eb6c4 100644 --- a/unsupported/Eigen/src/EulerAngles/EulerAngles.h +++ b/unsupported/Eigen/src/EulerAngles/EulerAngles.h @@ -21,9 +21,9 @@ namespace Eigen * * \brief Represents a rotation in a 3 dimensional space as three Euler angles * - * \param _Scalar the scalar type, i.e., the type of the angles. + * \sa _Scalar the scalar type, i.e., the type of the angles. * - * \sa cl + * \sa _System the EulerSystem to use, which represents the axes of rotation. */ template class EulerAngles : public RotationBase, 3> @@ -38,16 +38,19 @@ namespace Eigen typedef Quaternion QuaternionType; typedef AngleAxis AngleAxisType; + /** \returns the axis vector of the first (alpha) rotation */ static Vector3 AlphaAxisVector() { const Vector3& u = Vector3::Unit(System::AlphaAxisAbs - 1); return System::IsAlphaOpposite ? -u : u; } + /** \returns the axis vector of the second (beta) rotation */ static Vector3 BetaAxisVector() { const Vector3& u = Vector3::Unit(System::BetaAxisAbs - 1); return System::IsBetaOpposite ? -u : u; } + /** \returns the axis vector of the third (gamma) rotation */ static Vector3 GammaAxisVector() { const Vector3& u = Vector3::Unit(System::GammaAxisAbs - 1); return System::IsGammaOpposite ? -u : u; @@ -57,15 +60,31 @@ namespace Eigen Vector3 m_angles; public: - + /** Default constructor without initialization. */ EulerAngles() {} - inline EulerAngles(Scalar alpha, Scalar beta, Scalar gamma) : m_angles(alpha, beta, gamma) {} + /** Constructs and initialize euler angles(\p alpha, \p beta, \p gamma). */ + EulerAngles(Scalar alpha, Scalar beta, Scalar gamma) : m_angles(alpha, beta, gamma) {} + /** Constructs and initialize euler angles from a 3x3 rotation matrix \p m. + * + * \note All angles will be in the range [-PI, PI]. + */ template - inline EulerAngles(const MatrixBase& m) { *this = m; } + 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 possitive 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 - inline EulerAngles( + EulerAngles( const MatrixBase& m, bool positiveRangeAlpha, bool positiveRangeBeta, @@ -74,11 +93,26 @@ namespace Eigen System::CalcEulerAngles(*this, m, positiveRangeAlpha, positiveRangeBeta, positiveRangeGamma); } + /** Constructs and initialize euler angles from a rotation \p rot. + * + * \note All angles will be in the range [-PI, PI]. + */ template - inline EulerAngles(const RotationBase& rot) { *this = rot; } + EulerAngles(const RotationBase& rot) { *this = rot; } + /** Constructs and initialize euler angles from a rotation \p rot, + * with options to choose for each angle the requested range. + * + * If possitive 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 - inline EulerAngles( + EulerAngles( const RotationBase& rot, bool positiveRangeAlpha, bool positiveRangeBeta, @@ -87,18 +121,29 @@ namespace Eigen System::CalcEulerAngles(*this, rot.toRotationMatrix(), positiveRangeAlpha, positiveRangeBeta, positiveRangeGamma); } + /** \returns The angle values stored in a vector (alpha, beta, gamma). */ const Vector3& angles() const { return m_angles; } + /** \returns A read-write reference to the angle values stored in a vector (alpha, beta, gamma). */ Vector3& angles() { return m_angles; } + /** \returns The value of the first angle. */ Scalar alpha() const { return m_angles[0]; } + /** \returns A read-write reference to the angle of the first angle. */ Scalar& alpha() { return m_angles[0]; } + /** \returns The value of the second angle. */ Scalar beta() const { return m_angles[1]; } + /** \returns A read-write reference to the angle of the second angle. */ Scalar& beta() { return m_angles[1]; } + /** \returns The value of the third angle. */ Scalar gamma() const { return m_angles[2]; } + /** \returns A read-write reference to the angle of the third angle. */ Scalar& gamma() { return m_angles[2]; } + /** \returns The euler angles rotation inverse (which is as same as the negative), + * (-alpha, -beta, -gamma). + */ EulerAngles inverse() const { EulerAngles res; @@ -106,12 +151,24 @@ namespace Eigen return res; } + /** \returns The euler angles rotation negative (which is as same as the inverse), + * (-alpha, -beta, -gamma). + */ EulerAngles operator -() const { return inverse(); } - /** Constructs and \returns an equivalent 3x3 rotation matrix. + /** 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__). + * + * If possitive 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, @@ -125,6 +182,17 @@ namespace Eigen 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 possitive 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]. + */ template< bool PositiveRangeAlpha, bool PositiveRangeBeta, @@ -149,8 +217,7 @@ namespace Eigen //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). - */ + /** Set \c *this from a rotation matrix(i.e. pure orthogonal matrix with determinant of +1). */ template EulerAngles& operator=(const MatrixBase& m) { System::CalcEulerAngles(*this, m); @@ -159,8 +226,7 @@ namespace Eigen // TODO: Assign and construct from another EulerAngles (with different system) - /** Set \c *this from a rotation. - */ + /** Set \c *this from a rotation. */ template EulerAngles& operator=(const RotationBase& rot) { System::CalcEulerAngles(*this, rot.toRotationMatrix()); @@ -169,11 +235,13 @@ namespace Eigen // TODO: Support isApprox function + /** \returns an equivalent 3x3 rotation matrix. */ Matrix3 toRotationMatrix() const { return static_cast(*this).toRotationMatrix(); } + /** \returns an equivalent quaternion */ QuaternionType toQuaternion() const { return @@ -181,7 +249,8 @@ namespace Eigen AngleAxisType(beta(), BetaAxisVector()) * AngleAxisType(gamma(), GammaAxisVector()); } - + + /** Convert the euler angles to quaternion. */ operator QuaternionType() const { return toQuaternion(); diff --git a/unsupported/Eigen/src/EulerAngles/EulerSystem.h b/unsupported/Eigen/src/EulerAngles/EulerSystem.h index f37c62f8e..a5ef56a5e 100644 --- a/unsupported/Eigen/src/EulerAngles/EulerSystem.h +++ b/unsupported/Eigen/src/EulerAngles/EulerSystem.h @@ -103,7 +103,7 @@ namespace Eigen 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))) { - res[0] = (res[0] > Scalar(0)) ? res[0] - Scalar(M_PI) : res[0] + Scalar(M_PI); + res[0] = (res[0] > Scalar(0)) ? res[0] - Scalar(EIGEN_PI) : res[0] + Scalar(EIGEN_PI); res[1] = atan2(-mat(I,K), -c2); } else @@ -126,7 +126,7 @@ namespace Eigen res[0] = atan2(mat(J,I), mat(K,I)); if((IsOdd && res[0]Scalar(0))) { - res[0] = (res[0] > Scalar(0)) ? res[0] - Scalar(M_PI) : res[0] + Scalar(M_PI); + res[0] = (res[0] > Scalar(0)) ? res[0] - Scalar(EIGEN_PI) : res[0] + Scalar(EIGEN_PI); Scalar s2 = Vector2(mat(J,I), mat(K,I)).norm(); res[1] = -atan2(s2, mat(I,I)); } @@ -206,20 +206,23 @@ namespace Eigen } }; - typedef EulerSystem EulerSystemXYZ; - typedef EulerSystem EulerSystemXYX; - typedef EulerSystem EulerSystemXZY; - typedef EulerSystem EulerSystemXZX; - - typedef EulerSystem EulerSystemYZX; - typedef EulerSystem EulerSystemYZY; - typedef EulerSystem EulerSystemYXZ; - typedef EulerSystem EulerSystemYXY; - - typedef EulerSystem EulerSystemZXY; - typedef EulerSystem EulerSystemZXZ; - typedef EulerSystem EulerSystemZYX; - typedef EulerSystem EulerSystemZYZ; +#define EIGEN_EULER_SYSTEM_TYPEDEF(A, B, C) \ + typedef EulerSystem 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 -- cgit v1.2.3 From 06206482d9964835b201c682884360dd0d1e73a5 Mon Sep 17 00:00:00 2001 From: Tal Hadad Date: Sun, 12 Jun 2016 23:40:17 +0300 Subject: More docs, and minor code fixes --- unsupported/Eigen/CMakeLists.txt | 3 +- unsupported/Eigen/EulerAngles | 4 + unsupported/Eigen/src/EulerAngles/CMakeLists.txt | 6 +- unsupported/Eigen/src/EulerAngles/EulerAngles.h | 120 ++++++++++++++++------- unsupported/Eigen/src/EulerAngles/EulerSystem.h | 95 ++++++++++++++++-- 5 files changed, 180 insertions(+), 48 deletions(-) diff --git a/unsupported/Eigen/CMakeLists.txt b/unsupported/Eigen/CMakeLists.txt index 6d0cf4f9d..2fc8db412 100644 --- a/unsupported/Eigen/CMakeLists.txt +++ b/unsupported/Eigen/CMakeLists.txt @@ -4,6 +4,7 @@ set(Eigen_HEADERS ArpackSupport AutoDiff BVH + EulerAngles FFT IterativeSolvers KroneckerProduct @@ -26,4 +27,4 @@ install(FILES ) add_subdirectory(src) -add_subdirectory(CXX11) \ No newline at end of file +add_subdirectory(CXX11) diff --git a/unsupported/Eigen/EulerAngles b/unsupported/Eigen/EulerAngles index 5e1d2ccba..a595b5f2c 100644 --- a/unsupported/Eigen/EulerAngles +++ b/unsupported/Eigen/EulerAngles @@ -22,6 +22,10 @@ namespace Eigen { * \defgroup EulerAngles_Module EulerAngles module * \brief This module provides generic euler angles rotation. * + * Euler angles are a way to represent 3D rotation. + * + * !TODO! More about the purpose of this module and examples. + * * See EulerAngles for more information. * * \code diff --git a/unsupported/Eigen/src/EulerAngles/CMakeLists.txt b/unsupported/Eigen/src/EulerAngles/CMakeLists.txt index 7986afc5e..40af550e8 100644 --- a/unsupported/Eigen/src/EulerAngles/CMakeLists.txt +++ b/unsupported/Eigen/src/EulerAngles/CMakeLists.txt @@ -1,6 +1,6 @@ -FILE(GLOB Eigen_IterativeSolvers_SRCS "*.h") +FILE(GLOB Eigen_EulerAngles_SRCS "*.h") INSTALL(FILES - ${Eigen_IterativeSolvers_SRCS} - DESTINATION ${INCLUDE_INSTALL_DIR}/unsupported/Eigen/src/IterativeSolvers COMPONENT Devel + ${Eigen_EulerAngles_SRCS} + DESTINATION ${INCLUDE_INSTALL_DIR}/unsupported/Eigen/src/EulerAngles COMPONENT Devel ) diff --git a/unsupported/Eigen/src/EulerAngles/EulerAngles.h b/unsupported/Eigen/src/EulerAngles/EulerAngles.h index 4082eb6c4..9d1762ace 100644 --- a/unsupported/Eigen/src/EulerAngles/EulerAngles.h +++ b/unsupported/Eigen/src/EulerAngles/EulerAngles.h @@ -19,11 +19,60 @@ namespace Eigen /** \class EulerAngles * - * \brief Represents a rotation in a 3 dimensional space as three Euler angles + * \brief Represents a rotation in a 3 dimensional space as three Euler angles. * - * \sa _Scalar the scalar type, i.e., the type of the angles. + * Euler rotation is a set of three rotation of three angles over three fixed axes, defined by the EulerSystem given as a template parameter. + * + * Here is how intrinsic Euler angles works: + * - first, rotate the axes system over the alpha axis in angle alpha + * - then, rotate the axes system over the beta axis(which was rotated in the first stage) in angle beta + * - then, rotate the axes system over the gamma axis(which was rotated in the two stages above) in angle gamma * - * \sa _System the EulerSystem to use, which represents the axes of rotation. + * \note This class support only intrinsic Euler angles for simplicity, + * see EulerSystem how to easily overcome it for extrinsic systems. + * + * ### Rotation representation and conversions ### + * + * It has been proved(see Wikipedia link below) that every rotation can be represented + * by Euler angles, but there is no singular representation (e.g. unlike rotation matrices). + * Therefore, you can convert from Eigen rotation and to them + * (including rotation matrices, which is not called "rotations" by Eigen design). + * + * Euler angles usually used for: + * - convenient human representation of rotation, especially in interactive GUI. + * - gimbal systems and robotics + * - efficient encoding(i.e. 3 floats only) of rotation for network protocols. + * + * However, Euler angles are slow comparing to quaternion or matrices, + * because their unnatural math definition, although it's simple for human. + * To overcome this, this class provide easy movement from the math friendly representation + * to the human friendly representation, and vise-versa. + * + * All the user need to do is a safe simple C++ type conversion, + * and this class take care for the math. + * Additionally, some axes related computation is done in compile time. + * + * ### Convenient user typedefs ### + * + * Convenient typedefs for EulerAngles exist for float and double scalar, + * in a form of EulerAngles{A}{B}{C}{scalar}, + * e.g. EulerAnglesXYZd, EulerAnglesZYZf. + * + * !TODO! Add examples + * + * Only for positive axes{+x,+y,+z} euler systems are have convenient typedef. + * If you need negative axes{-x,-y,-z}, it is recommended to create you own typedef with + * a word that represent what you need, e.g. EulerAnglesUTM (!TODO! make it more clear with example code). + * + * ### Additional reading ### + * + * If you're want to get more idea about how Euler system work in Eigen see EulerSystem. + * + * More information about Euler angles: https://en.wikipedia.org/wiki/Euler_angles + * + * \tparam _Scalar the scalar type, i.e., the type of the angles. + * + * \tparam _System the EulerSystem to use, which represents the axes of rotation. */ template class EulerAngles : public RotationBase, 3> @@ -62,17 +111,18 @@ namespace Eigen public: /** Default constructor without initialization. */ EulerAngles() {} - /** Constructs and initialize euler angles(\p alpha, \p beta, \p gamma). */ - EulerAngles(Scalar alpha, Scalar beta, Scalar gamma) : m_angles(alpha, beta, gamma) {} + /** Constructs and initialize Euler angles(\p alpha, \p beta, \p gamma). */ + EulerAngles(const Scalar& alpha, const Scalar& beta, const Scalar& gamma) : + m_angles(alpha, beta, gamma) {} - /** Constructs and initialize euler angles from a 3x3 rotation matrix \p m. + /** Constructs and initialize Euler angles from a 3x3 rotation matrix \p m. * * \note All angles will be in the range [-PI, PI]. */ template EulerAngles(const MatrixBase& m) { *this = m; } - /** Constructs and initialize euler angles from a 3x3 rotation matrix \p m, + /** Constructs and initialize Euler angles from a 3x3 rotation matrix \p m, * with options to choose for each angle the requested range. * * If possitive range is true, then the specified angle will be in the range [0, +2*PI]. @@ -93,14 +143,16 @@ namespace Eigen System::CalcEulerAngles(*this, m, positiveRangeAlpha, positiveRangeBeta, positiveRangeGamma); } - /** Constructs and initialize euler angles from a rotation \p rot. + /** Constructs and initialize Euler angles from a rotation \p rot. * - * \note All angles will be in the range [-PI, PI]. + * \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) */ template EulerAngles(const RotationBase& rot) { *this = rot; } - /** Constructs and initialize euler angles from a rotation \p rot, + /** Constructs and initialize Euler angles from a rotation \p rot, * with options to choose for each angle the requested range. * * If possitive range is true, then the specified angle will be in the range [0, +2*PI]. @@ -141,7 +193,7 @@ namespace Eigen /** \returns A read-write reference to the angle of the third angle. */ Scalar& gamma() { return m_angles[2]; } - /** \returns The euler angles rotation inverse (which is as same as the negative), + /** \returns The Euler angles rotation inverse (which is as same as the negative), * (-alpha, -beta, -gamma). */ EulerAngles inverse() const @@ -151,7 +203,7 @@ namespace Eigen return res; } - /** \returns The euler angles rotation negative (which is as same as the inverse), + /** \returns The Euler angles rotation negative (which is as same as the inverse), * (-alpha, -beta, -gamma). */ EulerAngles operator -() const @@ -159,7 +211,7 @@ namespace Eigen return inverse(); } - /** Constructs and initialize euler angles from a 3x3 rotation matrix \p m, + /** 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__). * * If possitive range is true, then the specified angle will be in the range [0, +2*PI]. @@ -182,7 +234,7 @@ namespace Eigen return e; } - /** Constructs and initialize euler angles from a rotation \p rot, + /** 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 possitive range is true, then the specified angle will be in the range [0, +2*PI]. @@ -241,40 +293,34 @@ namespace Eigen return static_cast(*this).toRotationMatrix(); } - /** \returns an equivalent quaternion */ - QuaternionType toQuaternion() const + /** Convert the Euler angles to quaternion. */ + operator QuaternionType() const { return AngleAxisType(alpha(), AlphaAxisVector()) * - AngleAxisType(beta(), BetaAxisVector()) * + AngleAxisType(beta(), BetaAxisVector()) * AngleAxisType(gamma(), GammaAxisVector()); } - - /** Convert the euler angles to quaternion. */ - operator QuaternionType() const - { - return toQuaternion(); - } }; -#define EIGEN_EULER_ANGLES_SINGLE_TYPEDEF(SYSTEM, SCALAR_TYPE, SCALAR_POSTFIX) \ - typedef EulerAngles SYSTEM##SCALAR_POSTFIX; +#define EIGEN_EULER_ANGLES_SINGLE_TYPEDEF(AXES, SCALAR_TYPE, SCALAR_POSTFIX) \ + typedef EulerAngles EulerSystem##AXES##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(XYZ, SCALAR_TYPE, SCALAR_POSTFIX) \ + EIGEN_EULER_ANGLES_SINGLE_TYPEDEF(XYX, SCALAR_TYPE, SCALAR_POSTFIX) \ + EIGEN_EULER_ANGLES_SINGLE_TYPEDEF(XZY, SCALAR_TYPE, SCALAR_POSTFIX) \ + EIGEN_EULER_ANGLES_SINGLE_TYPEDEF(XZX, 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(YZX, SCALAR_TYPE, SCALAR_POSTFIX) \ + EIGEN_EULER_ANGLES_SINGLE_TYPEDEF(YZY, SCALAR_TYPE, SCALAR_POSTFIX) \ + EIGEN_EULER_ANGLES_SINGLE_TYPEDEF(YXZ, SCALAR_TYPE, SCALAR_POSTFIX) \ + EIGEN_EULER_ANGLES_SINGLE_TYPEDEF(YXY, 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_SINGLE_TYPEDEF(ZXY, SCALAR_TYPE, SCALAR_POSTFIX) \ + EIGEN_EULER_ANGLES_SINGLE_TYPEDEF(ZXZ, SCALAR_TYPE, SCALAR_POSTFIX) \ + EIGEN_EULER_ANGLES_SINGLE_TYPEDEF(ZYX, SCALAR_TYPE, SCALAR_POSTFIX) \ + EIGEN_EULER_ANGLES_SINGLE_TYPEDEF(ZYZ, SCALAR_TYPE, SCALAR_POSTFIX) EIGEN_EULER_ANGLES_TYPEDEFS(float, f) EIGEN_EULER_ANGLES_TYPEDEFS(double, d) diff --git a/unsupported/Eigen/src/EulerAngles/EulerSystem.h b/unsupported/Eigen/src/EulerAngles/EulerSystem.h index a5ef56a5e..ac73b48a0 100644 --- a/unsupported/Eigen/src/EulerAngles/EulerSystem.h +++ b/unsupported/Eigen/src/EulerAngles/EulerSystem.h @@ -38,27 +38,107 @@ namespace Eigen }; } + /** \brief Representation of a fixed signed rotation axis for EulerAngles. + * + * Values here represent: + * - The axis of the rotation: X, Y or Z. + * - The sign (i.e. direction of the rotation along the axis): possitive(+) 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}. + * + * !TODO! Add examples + */ enum EulerAxis { - EULER_X = 1, - EULER_Y = 2, - EULER_Z = 3 + EULER_X = 1, /*!< the X axis */ + EULER_Y = 2, /*!< the Y axis */ + EULER_Z = 3 /*!< the Z axis */ }; - + + /** \class EulerSystem + * + * \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 EulerAngles) + * This meta-class store constantly those signed axes. (see 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 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 an assertion !TODO!. + * 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} + * + * !TODO! Add some example code. + * + * ### 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). + * !TODO! Make it more clear and add some example code. + * + * ### Convenient user typedefs ### + * + * Convenient typedefs for EulerSystem exist (only for positive axes Euler systems), + * in a form of EulerSystem{A}{B}{C}, e.g. EulerSystemXYZd. + * !TODO! Make it more clear + * + * ### 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 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 { + /** The first rotation axis unsigned */ AlphaAxisAbs = internal::Abs::value, + /** The second rotation axis unsigned */ BetaAxisAbs = internal::Abs::value, + /** The third rotation axis unsigned */ GammaAxisAbs = internal::Abs::value, IsAlphaOpposite = (AlphaAxis < 0) ? 1 : 0, @@ -81,7 +161,7 @@ namespace Eigen enum { - // I, J, K are the pivot indexes permutation for the rotation matrix, that match this euler system. + // 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, @@ -150,8 +230,6 @@ namespace Eigen Scalar c1 = cos(res[0]); res[2] = atan2(c1*mat(J,K)-s1*mat(K,K), c1*mat(J,J) - s1 * mat(K,J)); } - - public: template static void CalcEulerAngles( @@ -204,6 +282,9 @@ namespace Eigen if (PositiveRangeGamma && (res.gamma() < 0)) res.gamma() += Scalar(2 * EIGEN_PI); } + + template + friend class Eigen::EulerAngles; }; #define EIGEN_EULER_SYSTEM_TYPEDEF(A, B, C) \ -- cgit v1.2.3 From 6e1c086593075e8f16e914a1c7200bceea9aea7b Mon Sep 17 00:00:00 2001 From: Tal Hadad Date: Mon, 13 Jun 2016 21:55:17 +0300 Subject: Add static assertion --- unsupported/Eigen/src/EulerAngles/EulerAngles.h | 4 ++++ unsupported/Eigen/src/EulerAngles/EulerSystem.h | 29 +++++++++++++++++-------- 2 files changed, 24 insertions(+), 9 deletions(-) diff --git a/unsupported/Eigen/src/EulerAngles/EulerAngles.h b/unsupported/Eigen/src/EulerAngles/EulerAngles.h index 9d1762ace..911ba4d09 100644 --- a/unsupported/Eigen/src/EulerAngles/EulerAngles.h +++ b/unsupported/Eigen/src/EulerAngles/EulerAngles.h @@ -229,6 +229,8 @@ namespace Eigen typename Derived> static EulerAngles FromRotation(const MatrixBase& m) { + EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Derived, 3, 3) + EulerAngles e; System::CalcEulerAngles(e, m); return e; @@ -272,6 +274,8 @@ namespace Eigen /** 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) + System::CalcEulerAngles(*this, m); return *this; } diff --git a/unsupported/Eigen/src/EulerAngles/EulerSystem.h b/unsupported/Eigen/src/EulerAngles/EulerSystem.h index ac73b48a0..c368a8bfa 100644 --- a/unsupported/Eigen/src/EulerAngles/EulerSystem.h +++ b/unsupported/Eigen/src/EulerAngles/EulerSystem.h @@ -38,6 +38,8 @@ namespace Eigen }; } + #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 EulerAngles. * * Values here represent: @@ -83,7 +85,7 @@ namespace Eigen * - right handed or left handed * - counterclockwise or clockwise * - * Notice all axed combination are valid, and would trigger an assertion !TODO!. + * 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} @@ -147,17 +149,26 @@ namespace Eigen IsOdd = ((AlphaAxisAbs)%3 == (BetaAxisAbs - 1)%3) ? 0 : 1, IsEven = IsOdd ? 0 : 1, - - // TODO: Assert this, and sort it in a better way - IsValid = ((unsigned)AlphaAxisAbs != (unsigned)BetaAxisAbs && - (unsigned)BetaAxisAbs != (unsigned)GammaAxisAbs && - internal::IsValidAxis::value && internal::IsValidAxis::value && internal::IsValidAxis::value) ? 1 : 0, - // TODO: After a proper assertation, remove the "IsValid" from this expression - IsTaitBryan = (IsValid && (unsigned)AlphaAxisAbs != (unsigned)GammaAxisAbs) ? 1 : 0 + IsTaitBryan = ((unsigned)AlphaAxisAbs != (unsigned)GammaAxisAbs) ? 1 : 0 }; - + private: + + EIGEN_EULER_ANGLES_CLASS_STATIC_ASSERT(internal::IsValidAxis::value, + ALPHA_AXIS_IS_INVALID); + + EIGEN_EULER_ANGLES_CLASS_STATIC_ASSERT(internal::IsValidAxis::value, + BETA_AXIS_IS_INVALID); + + EIGEN_EULER_ANGLES_CLASS_STATIC_ASSERT(internal::IsValidAxis::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 { -- cgit v1.2.3 From 6edfe8771bcff2fbb3b150daea8bddf83938be09 Mon Sep 17 00:00:00 2001 From: Tal Hadad Date: Mon, 13 Jun 2016 22:03:19 +0300 Subject: Little bit docs --- unsupported/Eigen/src/EulerAngles/EulerSystem.h | 21 +++++++++------------ 1 file changed, 9 insertions(+), 12 deletions(-) diff --git a/unsupported/Eigen/src/EulerAngles/EulerSystem.h b/unsupported/Eigen/src/EulerAngles/EulerSystem.h index c368a8bfa..dbca3e174 100644 --- a/unsupported/Eigen/src/EulerAngles/EulerSystem.h +++ b/unsupported/Eigen/src/EulerAngles/EulerSystem.h @@ -136,21 +136,18 @@ namespace Eigen enum { - /** The first rotation axis unsigned */ - AlphaAxisAbs = internal::Abs::value, - /** The second rotation axis unsigned */ - BetaAxisAbs = internal::Abs::value, - /** The third rotation axis unsigned */ - GammaAxisAbs = internal::Abs::value, + AlphaAxisAbs = internal::Abs::value, /*!< the first rotation axis unsigned */ + BetaAxisAbs = internal::Abs::value, /*!< the second rotation axis unsigned */ + GammaAxisAbs = internal::Abs::value, /*!< the third rotation axis unsigned */ - IsAlphaOpposite = (AlphaAxis < 0) ? 1 : 0, - IsBetaOpposite = (BetaAxis < 0) ? 1 : 0, - IsGammaOpposite = (GammaAxis < 0) ? 1 : 0, + 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, - IsEven = IsOdd ? 0 : 1, + 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 + IsTaitBryan = ((unsigned)AlphaAxisAbs != (unsigned)GammaAxisAbs) ? 1 : 0 /*!< weather the Euler system is tait bryan */ }; private: -- cgit v1.2.3 From 8e198d68352933b0e436ad9dee8799dc13892b73 Mon Sep 17 00:00:00 2001 From: Tal Hadad Date: Sun, 19 Jun 2016 20:42:45 +0300 Subject: Complete docs and add ostream operator for EulerAngles. --- doc/Doxyfile.in | 5 +- unsupported/Eigen/EulerAngles | 7 +-- unsupported/Eigen/src/EulerAngles/EulerAngles.h | 79 +++++++++++++++++++------ unsupported/Eigen/src/EulerAngles/EulerSystem.h | 37 ++++++------ unsupported/doc/examples/EulerAngles.cpp | 46 ++++++++++++++ 5 files changed, 132 insertions(+), 42 deletions(-) create mode 100644 unsupported/doc/examples/EulerAngles.cpp diff --git a/doc/Doxyfile.in b/doc/Doxyfile.in index 0c3673f89..a90d3852a 100644 --- a/doc/Doxyfile.in +++ b/doc/Doxyfile.in @@ -1608,7 +1608,10 @@ EXPAND_AS_DEFINED = EIGEN_MAKE_TYPEDEFS \ EIGEN_MATHFUNC_IMPL \ _EIGEN_GENERIC_PUBLIC_INTERFACE \ EIGEN_ARRAY_DECLARE_GLOBAL_UNARY \ - EIGEN_EMPTY + EIGEN_EMPTY \ + EIGEN_EULER_ANGLES_TYPEDEFS \ + EIGEN_EULER_ANGLES_SINGLE_TYPEDEF \ + EIGEN_EULER_SYSTEM_TYPEDEF # If the SKIP_FUNCTION_MACROS tag is set to YES (the default) then # doxygen's preprocessor will remove all references to function-like macros diff --git a/unsupported/Eigen/EulerAngles b/unsupported/Eigen/EulerAngles index a595b5f2c..521fa3f76 100644 --- a/unsupported/Eigen/EulerAngles +++ b/unsupported/Eigen/EulerAngles @@ -24,14 +24,13 @@ namespace Eigen { * * Euler angles are a way to represent 3D rotation. * - * !TODO! More about the purpose of this module and examples. - * - * See EulerAngles for more information. - * + * In order to use this module in your code, include this header: * \code * #include * \endcode * + * See \ref EulerAngles for more information. + * */ } diff --git a/unsupported/Eigen/src/EulerAngles/EulerAngles.h b/unsupported/Eigen/src/EulerAngles/EulerAngles.h index 911ba4d09..13a0da1ab 100644 --- a/unsupported/Eigen/src/EulerAngles/EulerAngles.h +++ b/unsupported/Eigen/src/EulerAngles/EulerAngles.h @@ -18,6 +18,8 @@ namespace Eigen struct ei_eulerangles_assign_impl;*/ /** \class EulerAngles + * + * \ingroup EulerAngles_Module * * \brief Represents a rotation in a 3 dimensional space as three Euler angles. * @@ -29,7 +31,7 @@ namespace Eigen * - then, rotate the axes system over the gamma axis(which was rotated in the two stages above) in angle gamma * * \note This class support only intrinsic Euler angles for simplicity, - * see EulerSystem how to easily overcome it for extrinsic systems. + * see EulerSystem how to easily overcome this for extrinsic systems. * * ### Rotation representation and conversions ### * @@ -52,17 +54,48 @@ namespace Eigen * and this class take care for the math. * Additionally, some axes related computation is done in compile time. * + * #### Euler angles ranges in conversions #### + * + * When converting some rotation to Euler angles, there are some ways you can guarantee + * the Euler angles ranges. + * + * #### 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, * in a form of EulerAngles{A}{B}{C}{scalar}, - * e.g. EulerAnglesXYZd, EulerAnglesZYZf. + * e.g. \ref EulerAnglesXYZd, \ref EulerAnglesZYZf. * - * !TODO! Add examples - * - * Only for positive axes{+x,+y,+z} euler systems are have convenient typedef. + * Only for positive axes{+x,+y,+z} Euler systems are have convenient typedef. * If you need negative axes{-x,-y,-z}, it is recommended to create you own typedef with - * a word that represent what you need, e.g. EulerAnglesUTM (!TODO! make it more clear with example code). + * a word that represent what you need. + * + * ### Example ### + * + * \include EulerAngles.cpp + * Output: \verbinclude EulerAngles.out * * ### Additional reading ### * @@ -80,12 +113,14 @@ namespace Eigen public: /** the scalar type of the angles */ typedef _Scalar Scalar; + + /** the EulerSystem to use, which represents the axes of rotation. */ typedef _System System; - typedef Matrix Matrix3; - typedef Matrix Vector3; - typedef Quaternion QuaternionType; - typedef AngleAxis AngleAxisType; + typedef Matrix Matrix3; /*!< the equivalent rotation matrix type */ + typedef Matrix Vector3; /*!< the equivalent 3 dimension vector type */ + typedef Quaternion QuaternionType; /*!< the equivalent quaternion type */ + typedef AngleAxis AngleAxisType; /*!< the equivalent angle-axis type */ /** \returns the axis vector of the first (alpha) rotation */ static Vector3 AlphaAxisVector() { @@ -125,7 +160,7 @@ namespace Eigen /** Constructs and initialize Euler angles from a 3x3 rotation matrix \p m, * with options to choose for each angle the requested range. * - * If possitive range is true, then the specified angle will be in the range [0, +2*PI]. + * 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 @@ -146,7 +181,7 @@ namespace Eigen /** 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. + * If rot is an EulerAngles, expected EulerAngles range is __undefined__. * (Use other functions here for enforcing range if this effect is desired) */ template @@ -155,7 +190,7 @@ namespace Eigen /** Constructs and initialize Euler angles from a rotation \p rot, * with options to choose for each angle the requested range. * - * If possitive range is true, then the specified angle will be in the range [0, +2*PI]. + * 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 @@ -214,7 +249,7 @@ namespace Eigen /** 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__). * - * If possitive range is true, then the specified angle will be in the range [0, +2*PI]. + * 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 @@ -232,14 +267,15 @@ namespace Eigen EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Derived, 3, 3) EulerAngles e; - System::CalcEulerAngles(e, m); + 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 possitive range is true, then the specified angle will be in the range [0, +2*PI]. + * 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 @@ -252,7 +288,7 @@ namespace Eigen bool PositiveRangeBeta, bool PositiveRangeGamma, typename Derived> - static EulerAngles& FromRotation(const RotationBase& rot) + static EulerAngles FromRotation(const RotationBase& rot) { return FromRotation(rot.toRotationMatrix()); } @@ -305,10 +341,17 @@ namespace Eigen AngleAxisType(beta(), BetaAxisVector()) * AngleAxisType(gamma(), GammaAxisVector()); } + + friend std::ostream& operator<<(std::ostream& s, const EulerAngles& eulerAngles) + { + s << eulerAngles.angles().transpose(); + return s; + } }; #define EIGEN_EULER_ANGLES_SINGLE_TYPEDEF(AXES, SCALAR_TYPE, SCALAR_POSTFIX) \ - typedef EulerAngles EulerSystem##AXES##SCALAR_POSTFIX; + /** \ingroup EulerAngles_Module */ \ + typedef EulerAngles EulerAngles##AXES##SCALAR_POSTFIX; #define EIGEN_EULER_ANGLES_TYPEDEFS(SCALAR_TYPE, SCALAR_POSTFIX) \ EIGEN_EULER_ANGLES_SINGLE_TYPEDEF(XYZ, SCALAR_TYPE, SCALAR_POSTFIX) \ diff --git a/unsupported/Eigen/src/EulerAngles/EulerSystem.h b/unsupported/Eigen/src/EulerAngles/EulerSystem.h index dbca3e174..82243e643 100644 --- a/unsupported/Eigen/src/EulerAngles/EulerSystem.h +++ b/unsupported/Eigen/src/EulerAngles/EulerSystem.h @@ -40,17 +40,17 @@ namespace Eigen #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 EulerAngles. + /** \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): possitive(+) or negative(-) + * - 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}. - * - * !TODO! Add examples */ enum EulerAxis { @@ -60,6 +60,8 @@ namespace Eigen }; /** \class EulerSystem + * + * \ingroup EulerAngles_Module * * \brief Represents a fixed Euler rotation system. * @@ -69,8 +71,8 @@ namespace Eigen * - 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 EulerAngles) - * This meta-class store constantly those signed axes. (see EulerAxis) + * 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 ### * @@ -78,35 +80,31 @@ 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 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: - * - right handed or left handed - * - counterclockwise or clockwise + * - _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, + * - _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} * - * !TODO! Add some example code. - * * ### 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). - * !TODO! Make it more clear and add some example code. + * 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. EulerSystemXYZd. - * !TODO! Make it more clear + * in a form of EulerSystem{A}{B}{C}, e.g. \ref EulerSystemXYZ. * * ### Additional reading ### * @@ -248,10 +246,10 @@ namespace Eigen } template< - typename Scalar, bool PositiveRangeAlpha, bool PositiveRangeBeta, - bool PositiveRangeGamma> + bool PositiveRangeGamma, + typename Scalar> static void CalcEulerAngles( EulerAngles& res, const typename EulerAngles::Matrix3& mat) @@ -296,6 +294,7 @@ namespace Eigen }; #define EIGEN_EULER_SYSTEM_TYPEDEF(A, B, C) \ + /** \ingroup EulerAngles_Module */ \ typedef EulerSystem EulerSystem##A##B##C; EIGEN_EULER_SYSTEM_TYPEDEF(X,Y,Z) diff --git a/unsupported/doc/examples/EulerAngles.cpp b/unsupported/doc/examples/EulerAngles.cpp new file mode 100644 index 000000000..1ef6aee18 --- /dev/null +++ b/unsupported/doc/examples/EulerAngles.cpp @@ -0,0 +1,46 @@ +#include +#include + +using namespace Eigen; + +int main() +{ + // A common Euler system by many armies around the world, + // where the first one is the azimuth(the angle from the north - + // the same angle that is show in compass) + // and the second one is elevation(the angle from the horizon) + // and the third one is roll(the angle between the horizontal body + // direction and the plane ground surface) + // Keep remembering we're using radian angles here! + typedef EulerSystem<-EULER_Z, EULER_Y, EULER_X> MyArmySystem; + typedef EulerAngles MyArmyAngles; + + MyArmyAngles vehicleAngles( + 3.14/*PI*/ / 2, /* heading to east, notice that this angle is counter-clockwise */ + -0.3, /* going down from a mountain */ + 0.1); /* slightly rolled to the right */ + + // Some Euler angles representation that our plane use. + EulerAnglesZYZd planeAngles(0.78474, 0.5271, -0.513794); + + MyArmyAngles planeAnglesInMyArmyAngles = MyArmyAngles::FromRotation(planeAngles); + + std::cout << "vehicle angles(MyArmy): " << vehicleAngles << std::endl; + std::cout << "plane angles(ZYZ): " << planeAngles << std::endl; + std::cout << "plane angles(MyArmy): " << planeAnglesInMyArmyAngles << std::endl; + + // Now lets rotate the plane a little bit + std::cout << "==========================================================\n"; + std::cout << "rotating plane now!\n"; + std::cout << "==========================================================\n"; + + Quaterniond planeRotated = AngleAxisd(-0.342, Vector3d::UnitY()) * planeAngles; + + planeAngles = planeRotated; + planeAnglesInMyArmyAngles = MyArmyAngles::FromRotation(planeRotated); + + std::cout << "new plane angles(ZYZ): " << planeAngles << std::endl; + std::cout << "new plane angles(MyArmy): " << planeAnglesInMyArmyAngles << std::endl; + + return 0; +} -- cgit v1.2.3 From eeb0d880ee6dac1b28ac820566017f641f45fcf9 Mon Sep 17 00:00:00 2001 From: Igor Babuschkin Date: Fri, 1 Jul 2016 19:08:26 +0100 Subject: Enable efficient Tensor reduction for doubles --- .../Eigen/CXX11/src/Tensor/TensorReductionCuda.h | 67 ++++++++++++++++------ unsupported/test/cxx11_tensor_reduction_cuda.cu | 26 +++++---- 2 files changed, 64 insertions(+), 29 deletions(-) diff --git a/unsupported/Eigen/CXX11/src/Tensor/TensorReductionCuda.h b/unsupported/Eigen/CXX11/src/Tensor/TensorReductionCuda.h index d3894e625..4e2e416e6 100644 --- a/unsupported/Eigen/CXX11/src/Tensor/TensorReductionCuda.h +++ b/unsupported/Eigen/CXX11/src/Tensor/TensorReductionCuda.h @@ -67,6 +67,23 @@ __device__ EIGEN_ALWAYS_INLINE void atomicReduce(T* output, T accum, R& reducer) #endif } +// We extend atomicExch to support extra data types +template +__device__ inline Type atomicExchCustom(Type* address, Type val) { + return atomicExch(address, val); +} + +template <> +__device__ inline double atomicExchCustom(double* address, double val) { + unsigned long long int* address_as_ull = (unsigned long long int*)address; + unsigned long long int old = *address_as_ull; + unsigned long long int assumed; + do { + assumed = old; + old = atomicCAS(address_as_ull, assumed, __double_as_longlong(val)); + } while (assumed != old); + return __longlong_as_double(old); +} #ifdef EIGEN_HAS_CUDA_FP16 template