387 lines
16 KiB
C++
387 lines
16 KiB
C++
// 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
|