diff options
author | Gael Guennebaud <g.gael@free.fr> | 2016-08-30 10:01:53 +0200 |
---|---|---|
committer | Gael Guennebaud <g.gael@free.fr> | 2016-08-30 10:01:53 +0200 |
commit | 1f84f0d33ad01df50a1f73e62d3d436552c00f99 (patch) | |
tree | 2089a1bae9604e182a72c048fe121a9987bb6bfc | |
parent | 68e803a26ea37b8895f0bd45f7bfaa47c375d890 (diff) | |
parent | 8e198d68352933b0e436ad9dee8799dc13892b73 (diff) |
merge EulerAngles module
-rw-r--r-- | doc/Doxyfile.in | 5 | ||||
-rw-r--r-- | unsupported/Eigen/CMakeLists.txt | 3 | ||||
-rw-r--r-- | unsupported/Eigen/EulerAngles | 43 | ||||
-rw-r--r-- | unsupported/Eigen/src/EulerAngles/CMakeLists.txt | 6 | ||||
-rw-r--r-- | unsupported/Eigen/src/EulerAngles/EulerAngles.h | 386 | ||||
-rw-r--r-- | unsupported/Eigen/src/EulerAngles/EulerSystem.h | 316 | ||||
-rw-r--r-- | unsupported/doc/examples/EulerAngles.cpp | 46 | ||||
-rw-r--r-- | unsupported/test/CMakeLists.txt | 2 | ||||
-rw-r--r-- | unsupported/test/EulerAngles.cpp | 208 |
9 files changed, 1013 insertions, 2 deletions
diff --git a/doc/Doxyfile.in b/doc/Doxyfile.in index 058c88b97..6f8d6bc01 100644 --- a/doc/Doxyfile.in +++ b/doc/Doxyfile.in @@ -1609,7 +1609,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/CMakeLists.txt b/unsupported/Eigen/CMakeLists.txt index 67f3981dc..631a06014 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 @@ -28,4 +29,4 @@ install(FILES install(DIRECTORY src DESTINATION ${INCLUDE_INSTALL_DIR}/unsupported/Eigen COMPONENT Devel FILES_MATCHING PATTERN "*.h") -add_subdirectory(CXX11)
\ No newline at end of file +add_subdirectory(CXX11) diff --git a/unsupported/Eigen/EulerAngles b/unsupported/Eigen/EulerAngles new file mode 100644 index 000000000..521fa3f76 --- /dev/null +++ b/unsupported/Eigen/EulerAngles @@ -0,0 +1,43 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// Copyright (C) 2015 Tal Hadad <tal_hd@hotmail.com> +// +// This Source Code Form is subject to the terms of the Mozilla +// Public License v. 2.0. If a copy of the MPL was not distributed +// with this file, You can obtain one at http://mozilla.org/MPL/2.0/. + +#ifndef EIGEN_EULERANGLES_MODULE_H +#define EIGEN_EULERANGLES_MODULE_H + + +#include "Eigen/Core" +#include "Eigen/Geometry" + +#include "Eigen/src/Core/util/DisableStupidWarnings.h" + +namespace Eigen { + +/** + * \defgroup EulerAngles_Module EulerAngles module + * \brief This module provides generic euler angles rotation. + * + * Euler angles are a way to represent 3D rotation. + * + * In order to use this module in your code, include this header: + * \code + * #include <unsupported/Eigen/EulerAngles> + * \endcode + * + * See \ref EulerAngles for more information. + * + */ + +} + +#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/EulerAngles/CMakeLists.txt b/unsupported/Eigen/src/EulerAngles/CMakeLists.txt new file mode 100644 index 000000000..40af550e8 --- /dev/null +++ b/unsupported/Eigen/src/EulerAngles/CMakeLists.txt @@ -0,0 +1,6 @@ +FILE(GLOB Eigen_EulerAngles_SRCS "*.h") + +INSTALL(FILES + ${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 new file mode 100644 index 000000000..13a0da1ab --- /dev/null +++ b/unsupported/Eigen/src/EulerAngles/EulerAngles.h @@ -0,0 +1,386 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// Copyright (C) 2015 Tal Hadad <tal_hd@hotmail.com> +// +// This Source Code Form is subject to the terms of the Mozilla +// Public License v. 2.0. If a copy of the MPL was not distributed +// with this file, You can obtain one at http://mozilla.org/MPL/2.0/. + +#ifndef EIGEN_EULERANGLESCLASS_H// TODO: Fix previous "EIGEN_EULERANGLES_H" definition? +#define EIGEN_EULERANGLESCLASS_H + +namespace Eigen +{ + /*template<typename Other, + int OtherRows=Other::RowsAtCompileTime, + int OtherCols=Other::ColsAtCompileTime> + struct ei_eulerangles_assign_impl;*/ + + /** \class EulerAngles + * + * \ingroup EulerAngles_Module + * + * \brief Represents a rotation in a 3 dimensional space as three Euler 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 + * + * \note This class support only intrinsic Euler angles for simplicity, + * see EulerSystem how to easily overcome this 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. + * + * #### 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<Derived>&) + * \sa EulerAngles(const RotationBase<Derived, 3>&) + * + * #### 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<Derived>&, bool, bool, bool) + * \sa EulerAngles(const RotationBase<Derived, 3>&, 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. \ref EulerAnglesXYZd, \ref EulerAnglesZYZf. + * + * 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. + * + * ### Example ### + * + * \include EulerAngles.cpp + * Output: \verbinclude EulerAngles.out + * + * ### 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 <typename _Scalar, class _System> + class EulerAngles : public RotationBase<EulerAngles<_Scalar, _System>, 3> + { + 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<Scalar,3,3> Matrix3; /*!< the equivalent rotation matrix type */ + typedef Matrix<Scalar,3,1> Vector3; /*!< the equivalent 3 dimension vector type */ + typedef Quaternion<Scalar> QuaternionType; /*!< the equivalent quaternion type */ + typedef AngleAxis<Scalar> AngleAxisType; /*!< the equivalent angle-axis type */ + + /** \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; + } + + private: + Vector3 m_angles; + + public: + /** Default constructor without initialization. */ + EulerAngles() {} + /** 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. + * + * \note All angles will be in the range [-PI, PI]. + */ + template<typename Derived> + EulerAngles(const MatrixBase<Derived>& m) { *this = m; } + + /** Constructs and initialize Euler angles from a 3x3 rotation matrix \p m, + * with options to choose for each angle the requested range. + * + * If positive range is true, then the specified angle will be in the range [0, +2*PI]. + * Otherwise, the specified angle will be in the range [-PI, +PI]. + * + * \param m The 3x3 rotation matrix to convert + * \param positiveRangeAlpha If true, alpha will be in [0, 2*PI]. Otherwise, in [-PI, +PI]. + * \param positiveRangeBeta If true, beta will be in [0, 2*PI]. Otherwise, in [-PI, +PI]. + * \param positiveRangeGamma If true, gamma will be in [0, 2*PI]. Otherwise, in [-PI, +PI]. + */ + template<typename Derived> + EulerAngles( + const MatrixBase<Derived>& m, + bool positiveRangeAlpha, + bool positiveRangeBeta, + bool positiveRangeGamma) { + + 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], 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<typename Derived> + EulerAngles(const RotationBase<Derived, 3>& rot) { *this = rot; } + + /** Constructs and initialize Euler angles from a rotation \p rot, + * with options to choose for each angle the requested range. + * + * If positive range is true, then the specified angle will be in the range [0, +2*PI]. + * Otherwise, the specified angle will be in the range [-PI, +PI]. + * + * \param rot The 3x3 rotation matrix to convert + * \param positiveRangeAlpha If true, alpha will be in [0, 2*PI]. Otherwise, in [-PI, +PI]. + * \param positiveRangeBeta If true, beta will be in [0, 2*PI]. Otherwise, in [-PI, +PI]. + * \param positiveRangeGamma If true, gamma will be in [0, 2*PI]. Otherwise, in [-PI, +PI]. + */ + template<typename Derived> + EulerAngles( + const RotationBase<Derived, 3>& rot, + bool positiveRangeAlpha, + bool positiveRangeBeta, + bool positiveRangeGamma) { + + 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; + res.m_angles = -m_angles; + 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 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 positive range is true, then the specified angle will be in the range [0, +2*PI]. + * Otherwise, the specified angle will be in the range [-PI, +PI]. + * + * \param m The 3x3 rotation matrix to convert + * \tparam positiveRangeAlpha If true, alpha will be in [0, 2*PI]. Otherwise, in [-PI, +PI]. + * \tparam positiveRangeBeta If true, beta will be in [0, 2*PI]. Otherwise, in [-PI, +PI]. + * \tparam positiveRangeGamma If true, gamma will be in [0, 2*PI]. Otherwise, in [-PI, +PI]. + */ + template< + bool PositiveRangeAlpha, + bool PositiveRangeBeta, + bool PositiveRangeGamma, + typename Derived> + static EulerAngles FromRotation(const MatrixBase<Derived>& m) + { + EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Derived, 3, 3) + + EulerAngles e; + System::template CalcEulerAngles< + PositiveRangeAlpha, PositiveRangeBeta, PositiveRangeGamma, _Scalar>(e, m); + return e; + } + + /** Constructs and initialize Euler angles from a rotation \p rot, + * with options to choose for each angle the requested range (__only in compile time__). + * + * If positive range is true, then the specified angle will be in the range [0, +2*PI]. + * Otherwise, the specified angle will be in the range [-PI, +PI]. + * + * \param rot The 3x3 rotation matrix to convert + * \tparam positiveRangeAlpha If true, alpha will be in [0, 2*PI]. Otherwise, in [-PI, +PI]. + * \tparam positiveRangeBeta If true, beta will be in [0, 2*PI]. Otherwise, in [-PI, +PI]. + * \tparam positiveRangeGamma If true, gamma will be in [0, 2*PI]. Otherwise, in [-PI, +PI]. + */ + template< + bool PositiveRangeAlpha, + bool PositiveRangeBeta, + bool PositiveRangeGamma, + typename Derived> + static EulerAngles FromRotation(const RotationBase<Derived, 3>& rot) + { + return FromRotation<PositiveRangeAlpha, PositiveRangeBeta, PositiveRangeGamma>(rot.toRotationMatrix()); + } + + /*EulerAngles& fromQuaternion(const QuaternionType& q) + { + // TODO: Implement it in a faster way for quaternions + // According to http://www.euclideanspace.com/maths/geometry/rotations/conversions/quaternionToEuler/ + // we can compute only the needed matrix cells and then convert to euler angles. (see ZYX example below) + // Currently we compute all matrix cells from quaternion. + + // Special case only for ZYX + //Scalar y2 = q.y() * q.y(); + //m_angles[0] = std::atan2(2*(q.w()*q.z() + q.x()*q.y()), (1 - 2*(y2 + q.z()*q.z()))); + //m_angles[1] = std::asin( 2*(q.w()*q.y() - q.z()*q.x())); + //m_angles[2] = std::atan2(2*(q.w()*q.x() + q.y()*q.z()), (1 - 2*(q.x()*q.x() + y2))); + }*/ + + /** Set \c *this from a rotation matrix(i.e. pure orthogonal matrix with determinant of +1). */ + template<typename Derived> + EulerAngles& operator=(const MatrixBase<Derived>& m) { + EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Derived, 3, 3) + + System::CalcEulerAngles(*this, m); + return *this; + } + + // TODO: Assign and construct from another EulerAngles (with different system) + + /** Set \c *this from a rotation. */ + template<typename Derived> + EulerAngles& operator=(const RotationBase<Derived, 3>& rot) { + System::CalcEulerAngles(*this, rot.toRotationMatrix()); + return *this; + } + + // TODO: Support isApprox function + + /** \returns an equivalent 3x3 rotation matrix. */ + Matrix3 toRotationMatrix() const + { + return static_cast<QuaternionType>(*this).toRotationMatrix(); + } + + /** Convert the Euler angles to quaternion. */ + operator QuaternionType() const + { + return + AngleAxisType(alpha(), AlphaAxisVector()) * + AngleAxisType(beta(), BetaAxisVector()) * + AngleAxisType(gamma(), GammaAxisVector()); + } + + friend std::ostream& operator<<(std::ostream& s, const EulerAngles<Scalar, System>& eulerAngles) + { + s << eulerAngles.angles().transpose(); + return s; + } + }; + +#define EIGEN_EULER_ANGLES_SINGLE_TYPEDEF(AXES, SCALAR_TYPE, SCALAR_POSTFIX) \ + /** \ingroup EulerAngles_Module */ \ + typedef EulerAngles<SCALAR_TYPE, EulerSystem##AXES> EulerAngles##AXES##SCALAR_POSTFIX; + +#define EIGEN_EULER_ANGLES_TYPEDEFS(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(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(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) + + namespace internal + { + template<typename _Scalar, class _System> + struct traits<EulerAngles<_Scalar, _System> > + { + 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..82243e643 --- /dev/null +++ b/unsupported/Eigen/src/EulerAngles/EulerSystem.h @@ -0,0 +1,316 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// Copyright (C) 2015 Tal Hadad <tal_hd@hotmail.com> +// +// This Source Code Form is subject to the terms of the Mozilla +// Public License v. 2.0. If a copy of the MPL was not distributed +// with this file, You can obtain one at http://mozilla.org/MPL/2.0/. + +#ifndef EIGEN_EULERSYSTEM_H +#define EIGEN_EULERSYSTEM_H + +namespace Eigen +{ + // Forward declerations + template <typename _Scalar, class _System> + class EulerAngles; + + namespace internal + { + // TODO: Check if already exists on the rest API + template <int Num, bool IsPositive = (Num > 0)> + struct Abs + { + enum { value = Num }; + }; + + template <int Num> + struct Abs<Num, false> + { + enum { value = -Num }; + }; + + template <int Axis> + struct IsValidAxis + { + enum { value = Axis != 0 && Abs<Axis>::value <= 3 }; + }; + } + + #define EIGEN_EULER_ANGLES_CLASS_STATIC_ASSERT(COND,MSG) typedef char static_assertion_##MSG[(COND)?1:-1] + + /** \brief Representation of a fixed signed rotation axis for EulerSystem. + * + * \ingroup EulerAngles_Module + * + * Values here represent: + * - The axis of the rotation: X, Y or Z. + * - The sign (i.e. direction of the rotation along the axis): positive(+) or negative(-) + * + * Therefore, this could express all the axes {+X,+Y,+Z,-X,-Y,-Z} + * + * For positive axis, use +EULER_{axis}, and for negative axis use -EULER_{axis}. + */ + enum EulerAxis + { + EULER_X = 1, /*!< the X axis */ + EULER_Y = 2, /*!< the Y axis */ + EULER_Z = 3 /*!< the Z axis */ + }; + + /** \class EulerSystem + * + * \ingroup EulerAngles_Module + * + * \brief Represents a fixed Euler rotation system. + * + * This meta-class goal is to represent the Euler system in compilation time, for EulerAngles. + * + * You can use this class to get two things: + * - Build an Euler system, and then pass it as a template parameter to EulerAngles. + * - Query some compile time data about an Euler system. (e.g. Whether it's tait bryan) + * + * Euler rotation is a set of three rotation on fixed axes. (see \ref EulerAngles) + * This meta-class store constantly those signed axes. (see \ref EulerAxis) + * + * ### Types of Euler systems ### + * + * All and only valid 3 dimension Euler rotation over standard + * signed axes{+X,+Y,+Z,-X,-Y,-Z} are supported: + * - all axes X, Y, Z in each valid order (see below what order is valid) + * - rotation over the axis is supported both over the positive and negative directions. + * - both tait bryan and proper/classic Euler angles (i.e. the opposite). + * + * Since EulerSystem support both positive and negative directions, + * you may call this rotation distinction in other names: + * - _right handed_ or _left handed_ + * - _counterclockwise_ or _clockwise_ + * + * Notice all axed combination are valid, and would trigger a static assertion. + * Same unsigned axes can't be neighbors, e.g. {X,X,Y} is invalid. + * This yield two and only two classes: + * - _tait bryan_ - all unsigned axes are distinct, e.g. {X,Y,Z} + * - _proper/classic Euler angles_ - The first and the third unsigned axes is equal, + * and the second is different, e.g. {X,Y,X} + * + * ### Intrinsic vs extrinsic Euler systems ### + * + * Only intrinsic Euler systems are supported for simplicity. + * If you want to use extrinsic Euler systems, + * just use the equal intrinsic opposite order for axes and angles. + * I.e axes (A,B,C) becomes (C,B,A), and angles (a,b,c) becomes (c,b,a). + * + * ### Convenient user typedefs ### + * + * Convenient typedefs for EulerSystem exist (only for positive axes Euler systems), + * in a form of EulerSystem{A}{B}{C}, e.g. \ref EulerSystemXYZ. + * + * ### Additional reading ### + * + * More information about Euler angles: https://en.wikipedia.org/wiki/Euler_angles + * + * \tparam _AlphaAxis the first fixed EulerAxis + * + * \tparam _AlphaAxis the second fixed EulerAxis + * + * \tparam _AlphaAxis the third fixed EulerAxis + */ + template <int _AlphaAxis, int _BetaAxis, int _GammaAxis> + class EulerSystem + { + public: + // It's defined this way and not as enum, because I think + // that enum is not guerantee to support negative numbers + + /** The first rotation axis */ + static const int AlphaAxis = _AlphaAxis; + + /** The second rotation axis */ + static const int BetaAxis = _BetaAxis; + + /** The third rotation axis */ + static const int GammaAxis = _GammaAxis; + + enum + { + AlphaAxisAbs = internal::Abs<AlphaAxis>::value, /*!< the first rotation axis unsigned */ + BetaAxisAbs = internal::Abs<BetaAxis>::value, /*!< the second rotation axis unsigned */ + GammaAxisAbs = internal::Abs<GammaAxis>::value, /*!< the third rotation axis unsigned */ + + IsAlphaOpposite = (AlphaAxis < 0) ? 1 : 0, /*!< weather alpha axis is negative */ + IsBetaOpposite = (BetaAxis < 0) ? 1 : 0, /*!< weather beta axis is negative */ + IsGammaOpposite = (GammaAxis < 0) ? 1 : 0, /*!< weather gamma axis is negative */ + + IsOdd = ((AlphaAxisAbs)%3 == (BetaAxisAbs - 1)%3) ? 0 : 1, /*!< weather the Euler system is odd */ + IsEven = IsOdd ? 0 : 1, /*!< weather the Euler system is even */ + + IsTaitBryan = ((unsigned)AlphaAxisAbs != (unsigned)GammaAxisAbs) ? 1 : 0 /*!< weather the Euler system is tait bryan */ + }; + + private: + + EIGEN_EULER_ANGLES_CLASS_STATIC_ASSERT(internal::IsValidAxis<AlphaAxis>::value, + ALPHA_AXIS_IS_INVALID); + + EIGEN_EULER_ANGLES_CLASS_STATIC_ASSERT(internal::IsValidAxis<BetaAxis>::value, + BETA_AXIS_IS_INVALID); + + EIGEN_EULER_ANGLES_CLASS_STATIC_ASSERT(internal::IsValidAxis<GammaAxis>::value, + GAMMA_AXIS_IS_INVALID); + + EIGEN_EULER_ANGLES_CLASS_STATIC_ASSERT((unsigned)AlphaAxisAbs != (unsigned)BetaAxisAbs, + ALPHA_AXIS_CANT_BE_EQUAL_TO_BETA_AXIS); + + EIGEN_EULER_ANGLES_CLASS_STATIC_ASSERT((unsigned)BetaAxisAbs != (unsigned)GammaAxisAbs, + BETA_AXIS_CANT_BE_EQUAL_TO_GAMMA_AXIS); + + enum + { + // I, J, K are the pivot indexes permutation for the rotation matrix, that match this Euler system. + // They are used in this class converters. + // They are always different from each other, and their possible values are: 0, 1, or 2. + I = AlphaAxisAbs - 1, + J = (AlphaAxisAbs - 1 + 1 + IsOdd)%3, + K = (AlphaAxisAbs - 1 + 2 - IsOdd)%3 + }; + + // TODO: Get @mat parameter in form that avoids double evaluation. + template <typename Derived> + static void CalcEulerAngles_imp(Matrix<typename MatrixBase<Derived>::Scalar, 3, 1>& res, const MatrixBase<Derived>& mat, internal::true_type /*isTaitBryan*/) + { + using std::atan2; + using std::sin; + using std::cos; + + typedef typename Derived::Scalar Scalar; + typedef Matrix<Scalar,2,1> Vector2; + + res[0] = atan2(mat(J,K), mat(K,K)); + Scalar c2 = Vector2(mat(I,I), mat(I,J)).norm(); + if((IsOdd && res[0]<Scalar(0)) || ((!IsOdd) && res[0]>Scalar(0))) { + res[0] = (res[0] > Scalar(0)) ? res[0] - Scalar(EIGEN_PI) : res[0] + Scalar(EIGEN_PI); + res[1] = atan2(-mat(I,K), -c2); + } + else + res[1] = atan2(-mat(I,K), c2); + Scalar s1 = sin(res[0]); + Scalar c1 = cos(res[0]); + res[2] = atan2(s1*mat(K,I)-c1*mat(J,I), c1*mat(J,J) - s1 * mat(K,J)); + } + + template <typename Derived> + static void CalcEulerAngles_imp(Matrix<typename MatrixBase<Derived>::Scalar,3,1>& res, const MatrixBase<Derived>& mat, internal::false_type /*isTaitBryan*/) + { + using std::atan2; + using std::sin; + using std::cos; + + typedef typename Derived::Scalar Scalar; + typedef Matrix<Scalar,2,1> Vector2; + + res[0] = atan2(mat(J,I), mat(K,I)); + if((IsOdd && res[0]<Scalar(0)) || ((!IsOdd) && res[0]>Scalar(0))) + { + 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)); + } + else + { + Scalar s2 = Vector2(mat(J,I), mat(K,I)).norm(); + res[1] = atan2(s2, mat(I,I)); + } + + // With a=(0,1,0), we have i=0; j=1; k=2, and after computing the first two angles, + // we can compute their respective rotation, and apply its inverse to M. Since the result must + // be a rotation around x, we have: + // + // c2 s1.s2 c1.s2 1 0 0 + // 0 c1 -s1 * M = 0 c3 s3 + // -s2 s1.c2 c1.c2 0 -s3 c3 + // + // Thus: m11.c1 - m21.s1 = c3 & m12.c1 - m22.s1 = s3 + + Scalar s1 = sin(res[0]); + Scalar c1 = cos(res[0]); + res[2] = atan2(c1*mat(J,K)-s1*mat(K,K), c1*mat(J,J) - s1 * mat(K,J)); + } + + template<typename Scalar> + static void CalcEulerAngles( + EulerAngles<Scalar, EulerSystem>& res, + const typename EulerAngles<Scalar, EulerSystem>::Matrix3& mat) + { + CalcEulerAngles(res, mat, false, false, false); + } + + template< + bool PositiveRangeAlpha, + bool PositiveRangeBeta, + bool PositiveRangeGamma, + typename Scalar> + static void CalcEulerAngles( + EulerAngles<Scalar, EulerSystem>& res, + const typename EulerAngles<Scalar, EulerSystem>::Matrix3& mat) + { + CalcEulerAngles(res, mat, PositiveRangeAlpha, PositiveRangeBeta, PositiveRangeGamma); + } + + template<typename Scalar> + static void CalcEulerAngles( + EulerAngles<Scalar, EulerSystem>& res, + const typename EulerAngles<Scalar, EulerSystem>::Matrix3& mat, + bool PositiveRangeAlpha, + bool PositiveRangeBeta, + bool PositiveRangeGamma) + { + CalcEulerAngles_imp( + res.angles(), mat, + typename internal::conditional<IsTaitBryan, internal::true_type, internal::false_type>::type()); + + if (IsAlphaOpposite == IsOdd) + res.alpha() = -res.alpha(); + + if (IsBetaOpposite == IsOdd) + res.beta() = -res.beta(); + + if (IsGammaOpposite == IsOdd) + res.gamma() = -res.gamma(); + + // Saturate results to the requested range + if (PositiveRangeAlpha && (res.alpha() < 0)) + res.alpha() += Scalar(2 * EIGEN_PI); + + if (PositiveRangeBeta && (res.beta() < 0)) + res.beta() += Scalar(2 * EIGEN_PI); + + if (PositiveRangeGamma && (res.gamma() < 0)) + res.gamma() += Scalar(2 * EIGEN_PI); + } + + template <typename _Scalar, class _System> + friend class Eigen::EulerAngles; + }; + +#define EIGEN_EULER_SYSTEM_TYPEDEF(A, B, C) \ + /** \ingroup EulerAngles_Module */ \ + typedef EulerSystem<EULER_##A, EULER_##B, EULER_##C> EulerSystem##A##B##C; + + EIGEN_EULER_SYSTEM_TYPEDEF(X,Y,Z) + EIGEN_EULER_SYSTEM_TYPEDEF(X,Y,X) + EIGEN_EULER_SYSTEM_TYPEDEF(X,Z,Y) + EIGEN_EULER_SYSTEM_TYPEDEF(X,Z,X) + + EIGEN_EULER_SYSTEM_TYPEDEF(Y,Z,X) + EIGEN_EULER_SYSTEM_TYPEDEF(Y,Z,Y) + EIGEN_EULER_SYSTEM_TYPEDEF(Y,X,Z) + EIGEN_EULER_SYSTEM_TYPEDEF(Y,X,Y) + + EIGEN_EULER_SYSTEM_TYPEDEF(Z,X,Y) + EIGEN_EULER_SYSTEM_TYPEDEF(Z,X,Z) + EIGEN_EULER_SYSTEM_TYPEDEF(Z,Y,X) + EIGEN_EULER_SYSTEM_TYPEDEF(Z,Y,Z) +} + +#endif // EIGEN_EULERSYSTEM_H 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 <unsupported/Eigen/EulerAngles> +#include <iostream> + +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<double, MyArmySystem> 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<true, false, false>(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<true, false, false>(planeRotated); + + std::cout << "new plane angles(ZYZ): " << planeAngles << std::endl; + std::cout << "new plane angles(MyArmy): " << planeAnglesInMyArmyAngles << std::endl; + + return 0; +} diff --git a/unsupported/test/CMakeLists.txt b/unsupported/test/CMakeLists.txt index 6188b421a..c0b321617 100644 --- a/unsupported/test/CMakeLists.txt +++ b/unsupported/test/CMakeLists.txt @@ -59,6 +59,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 AND EIGEN_COMPILER_SUPPORT_CXX11) diff --git a/unsupported/test/EulerAngles.cpp b/unsupported/test/EulerAngles.cpp new file mode 100644 index 000000000..a8cb52864 --- /dev/null +++ b/unsupported/test/EulerAngles.cpp @@ -0,0 +1,208 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// Copyright (C) 2015 Tal Hadad <tal_hd@hotmail.com> +// +// This Source Code Form is subject to the terms of the Mozilla +// Public License v. 2.0. If a copy of the MPL was not distributed +// with this file, You can obtain one at http://mozilla.org/MPL/2.0/. + +#include "main.h" + +#include <unsupported/Eigen/EulerAngles> + +using namespace Eigen; + +template<typename EulerSystem, typename Scalar> +void verify_euler_ranged(const Matrix<Scalar,3,1>& ea, + bool positiveRangeAlpha, bool positiveRangeBeta, bool positiveRangeGamma) +{ + typedef EulerAngles<Scalar, EulerSystem> EulerAnglesType; + typedef Matrix<Scalar,3,3> Matrix3; + typedef Matrix<Scalar,3,1> Vector3; + typedef Quaternion<Scalar> QuaternionType; + typedef AngleAxis<Scalar> AngleAxisType; + using std::abs; + + Scalar alphaRangeStart, alphaRangeEnd; + Scalar betaRangeStart, betaRangeEnd; + Scalar gammaRangeStart, gammaRangeEnd; + + if (positiveRangeAlpha) + { + alphaRangeStart = Scalar(0); + alphaRangeEnd = Scalar(2 * EIGEN_PI); + } + else + { + alphaRangeStart = -Scalar(EIGEN_PI); + alphaRangeEnd = Scalar(EIGEN_PI); + } + + if (positiveRangeBeta) + { + betaRangeStart = Scalar(0); + betaRangeEnd = Scalar(2 * EIGEN_PI); + } + else + { + betaRangeStart = -Scalar(EIGEN_PI); + betaRangeEnd = Scalar(EIGEN_PI); + } + + if (positiveRangeGamma) + { + gammaRangeStart = Scalar(0); + gammaRangeEnd = Scalar(2 * EIGEN_PI); + } + else + { + gammaRangeStart = -Scalar(EIGEN_PI); + gammaRangeEnd = Scalar(EIGEN_PI); + } + + const int i = EulerSystem::AlphaAxisAbs - 1; + const int j = EulerSystem::BetaAxisAbs - 1; + const int k = EulerSystem::GammaAxisAbs - 1; + + const int iFactor = EulerSystem::IsAlphaOpposite ? -1 : 1; + const int jFactor = EulerSystem::IsBetaOpposite ? -1 : 1; + const int kFactor = EulerSystem::IsGammaOpposite ? -1 : 1; + + const Vector3 I = EulerAnglesType::AlphaAxisVector(); + const Vector3 J = EulerAnglesType::BetaAxisVector(); + const Vector3 K = EulerAnglesType::GammaAxisVector(); + + EulerAnglesType e(ea[0], ea[1], ea[2]); + + Matrix3 m(e); + Vector3 eabis = EulerAnglesType(m, positiveRangeAlpha, positiveRangeBeta, positiveRangeGamma).angles(); + + // Check that eabis in range + VERIFY(alphaRangeStart <= eabis[0] && eabis[0] <= alphaRangeEnd); + VERIFY(betaRangeStart <= eabis[1] && eabis[1] <= betaRangeEnd); + VERIFY(gammaRangeStart <= eabis[2] && eabis[2] <= gammaRangeEnd); + + Vector3 eabis2 = m.eulerAngles(i, j, k); + + // Invert the relevant axes + eabis2[0] *= iFactor; + eabis2[1] *= jFactor; + eabis2[2] *= kFactor; + + // Saturate the angles to the correct range + if (positiveRangeAlpha && (eabis2[0] < 0)) + eabis2[0] += Scalar(2 * EIGEN_PI); + if (positiveRangeBeta && (eabis2[1] < 0)) + eabis2[1] += Scalar(2 * EIGEN_PI); + if (positiveRangeGamma && (eabis2[2] < 0)) + eabis2[2] += Scalar(2 * EIGEN_PI); + + VERIFY_IS_APPROX(eabis, eabis2);// Verify that our estimation is the same as m.eulerAngles() is + + Matrix3 mbis(AngleAxisType(eabis[0], I) * AngleAxisType(eabis[1], J) * AngleAxisType(eabis[2], K)); + VERIFY_IS_APPROX(m, mbis); + + // Tests that are only relevant for no possitive range + if (!(positiveRangeAlpha || positiveRangeBeta || positiveRangeGamma)) + { + /* 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<Scalar>())) ) + VERIFY((ea-eabis).norm() <= test_precision<Scalar>()); + + // 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, positiveRangeAlpha, positiveRangeBeta, positiveRangeGamma).angles(); + VERIFY_IS_APPROX(eabis, eabis2);// Verify that the euler angles are still the same +} + +template<typename EulerSystem, typename Scalar> +void verify_euler(const Matrix<Scalar,3,1>& ea) +{ + verify_euler_ranged<EulerSystem>(ea, false, false, false); + verify_euler_ranged<EulerSystem>(ea, false, false, true); + verify_euler_ranged<EulerSystem>(ea, false, true, false); + verify_euler_ranged<EulerSystem>(ea, false, true, true); + verify_euler_ranged<EulerSystem>(ea, true, false, false); + verify_euler_ranged<EulerSystem>(ea, true, false, true); + verify_euler_ranged<EulerSystem>(ea, true, true, false); + verify_euler_ranged<EulerSystem>(ea, true, true, true); +} + +template<typename Scalar> void check_all_var(const Matrix<Scalar,3,1>& ea) +{ + verify_euler<EulerSystemXYZ>(ea); + verify_euler<EulerSystemXYX>(ea); + verify_euler<EulerSystemXZY>(ea); + verify_euler<EulerSystemXZX>(ea); + + verify_euler<EulerSystemYZX>(ea); + verify_euler<EulerSystemYZY>(ea); + verify_euler<EulerSystemYXZ>(ea); + verify_euler<EulerSystemYXY>(ea); + + verify_euler<EulerSystemZXY>(ea); + verify_euler<EulerSystemZXZ>(ea); + verify_euler<EulerSystemZYX>(ea); + verify_euler<EulerSystemZYZ>(ea); +} + +template<typename Scalar> void eulerangles() +{ + typedef Matrix<Scalar,3,3> Matrix3; + typedef Matrix<Scalar,3,1> Vector3; + typedef Array<Scalar,3,1> Array3; + typedef Quaternion<Scalar> Quaternionx; + typedef AngleAxis<Scalar> AngleAxisType; + + Scalar a = internal::random<Scalar>(-Scalar(EIGEN_PI), Scalar(EIGEN_PI)); + Quaternionx q1; + q1 = AngleAxisType(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<Scalar>(0,Scalar(EIGEN_PI)); + check_all_var(ea); + + ea[0] = ea[1] = internal::random<Scalar>(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<float>() ); + CALL_SUBTEST_2( eulerangles<double>() ); + } +} |