devtools/eigen
1
0
Fork 0
mirror of https://gitlab.com/libeigen/eigen.git synced 2026-04-10 11:34:33 +08:00
Code Issues Packages Projects Releases Wiki Activity

Apply clang-format

Browse Source
This commit is contained in:
Tobias Wood
2023-11-29 11:12:48 +00:00
parent 9ea520fc45
commit f38e16c193
534 changed files with 103368 additions and 116934 deletions
Download Patch File Download Diff File Expand all files Collapse all files

602
unsupported/Eigen/src/EulerAngles/EulerAngles.h
View File

@@ -7,314 +7,306 @@
// 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?
#ifndef EIGEN_EULERANGLESCLASS_H // TODO: Fix previous "EIGEN_EULERANGLES_H" definition?
#define EIGEN_EULERANGLESCLASS_H
// IWYU pragma: private
#include "./InternalHeaderCheck.h"
namespace Eigen
{
/** \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 single 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 ####
* Rotations representation as EulerAngles are not single (unlike matrices),
* and even have infinite EulerAngles representations.<BR>
* For example, add or subtract 2*PI from either angle of EulerAngles
* and you'll get the same rotation.
* This is the general reason for infinite representation,
* but it's not the only general reason for not having a single representation.
*
* When converting rotation to EulerAngles, this class convert it to specific ranges
* When converting some rotation to EulerAngles, the rules for ranges are as follow:
* - If the rotation we converting from is an EulerAngles
* (even when it represented as RotationBase explicitly), angles ranges are __undefined__.
* - otherwise, alpha and gamma angles will be in the range [-PI, PI].<BR>
* As for Beta angle:
* - If the system is Tait-Bryan, the beta angle will be in the range [-PI/2, PI/2].
* - otherwise:
* - If the beta axis is positive, the beta angle will be in the range [0, PI]
* - If the beta axis is negative, the beta angle will be in the range [-PI, 0]
*
* \sa EulerAngles(const MatrixBase<Derived>&)
* \sa EulerAngles(const RotationBase<Derived, 3>&)
*
* ### 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>
namespace Eigen {
/** \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 single 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 ####
* Rotations representation as EulerAngles are not single (unlike matrices),
* and even have infinite EulerAngles representations.<BR>
* For example, add or subtract 2*PI from either angle of EulerAngles
* and you'll get the same rotation.
* This is the general reason for infinite representation,
* but it's not the only general reason for not having a single representation.
*
* When converting rotation to EulerAngles, this class convert it to specific ranges
* When converting some rotation to EulerAngles, the rules for ranges are as follow:
* - If the rotation we converting from is an EulerAngles
* (even when it represented as RotationBase explicitly), angles ranges are __undefined__.
* - otherwise, alpha and gamma angles will be in the range [-PI, PI].<BR>
* As for Beta angle:
* - If the system is Tait-Bryan, the beta angle will be in the range [-PI/2, PI/2].
* - otherwise:
* - If the beta axis is positive, the beta angle will be in the range [0, PI]
* - If the beta axis is negative, the beta angle will be in the range [-PI, 0]
*
* \sa EulerAngles(const MatrixBase<Derived>&)
* \sa EulerAngles(const RotationBase<Derived, 3>&)
*
* ### 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:
typedef RotationBase<EulerAngles<Scalar_, _System>, 3> Base;
/** the scalar type of the angles */
typedef Scalar_ Scalar;
typedef typename NumTraits<Scalar>::Real RealScalar;
/** 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 an EulerAngles (\p alpha, \p beta, \p gamma). */
EulerAngles(const Scalar& alpha, const Scalar& beta, const Scalar& gamma) : m_angles(alpha, beta, gamma) {}
// TODO: Test this constructor
/** Constructs and initialize an EulerAngles from the array data {alpha, beta, gamma} */
explicit EulerAngles(const Scalar* data) : m_angles(data) {}
/** Constructs and initializes an EulerAngles from either:
* - a 3x3 rotation matrix expression(i.e. pure orthogonal matrix with determinant of +1),
* - a 3D vector expression representing Euler angles.
*
* \note If \p other is a 3x3 rotation matrix, the angles range rules will be as follow:<BR>
* Alpha and gamma angles will be in the range [-PI, PI].<BR>
* As for Beta angle:
* - If the system is Tait-Bryan, the beta angle will be in the range [-PI/2, PI/2].
* - otherwise:
* - If the beta axis is positive, the beta angle will be in the range [0, PI]
* - If the beta axis is negative, the beta angle will be in the range [-PI, 0]
*/
template <typename Derived>
explicit EulerAngles(const MatrixBase<Derived>& other) {
*this = other;
}
/** Constructs and initialize Euler angles from a rotation \p rot.
*
* \note If \p rot is an EulerAngles (even when it represented as RotationBase explicitly),
* angles ranges are __undefined__.
* Otherwise, alpha and gamma angles will be in the range [-PI, PI].<BR>
* As for Beta angle:
* - If the system is Tait-Bryan, the beta angle will be in the range [-PI/2, PI/2].
* - otherwise:
* - If the beta axis is positive, the beta angle will be in the range [0, PI]
* - If the beta axis is negative, the beta angle will be in the range [-PI, 0]
*/
template <typename Derived>
EulerAngles(const RotationBase<Derived, 3>& rot) {
System::CalcEulerAngles(*this, rot.toRotationMatrix());
}
/*EulerAngles(const QuaternionType& q)
{
public:
typedef RotationBase<EulerAngles<Scalar_, _System>, 3> Base;
/** the scalar type of the angles */
typedef Scalar_ Scalar;
typedef typename NumTraits<Scalar>::Real RealScalar;
/** 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;
}
// 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.
private:
Vector3 m_angles;
// 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)));
}*/
public:
/** Default constructor without initialization. */
EulerAngles() {}
/** Constructs and initialize an EulerAngles (\p alpha, \p beta, \p gamma). */
EulerAngles(const Scalar& alpha, const Scalar& beta, const Scalar& gamma) :
m_angles(alpha, beta, gamma) {}
// TODO: Test this constructor
/** Constructs and initialize an EulerAngles from the array data {alpha, beta, gamma} */
explicit EulerAngles(const Scalar* data) : m_angles(data) {}
/** Constructs and initializes an EulerAngles from either:
* - a 3x3 rotation matrix expression(i.e. pure orthogonal matrix with determinant of +1),
* - a 3D vector expression representing Euler angles.
*
* \note If \p other is a 3x3 rotation matrix, the angles range rules will be as follow:<BR>
* Alpha and gamma angles will be in the range [-PI, PI].<BR>
* As for Beta angle:
* - If the system is Tait-Bryan, the beta angle will be in the range [-PI/2, PI/2].
* - otherwise:
* - If the beta axis is positive, the beta angle will be in the range [0, PI]
* - If the beta axis is negative, the beta angle will be in the range [-PI, 0]
*/
template<typename Derived>
explicit EulerAngles(const MatrixBase<Derived>& other) { *this = other; }
/** Constructs and initialize Euler angles from a rotation \p rot.
*
* \note If \p rot is an EulerAngles (even when it represented as RotationBase explicitly),
* angles ranges are __undefined__.
* Otherwise, alpha and gamma angles will be in the range [-PI, PI].<BR>
* As for Beta angle:
* - If the system is Tait-Bryan, the beta angle will be in the range [-PI/2, PI/2].
* - otherwise:
* - If the beta axis is positive, the beta angle will be in the range [0, PI]
* - If the beta axis is negative, the beta angle will be in the range [-PI, 0]
*/
template<typename Derived>
EulerAngles(const RotationBase<Derived, 3>& rot) { System::CalcEulerAngles(*this, rot.toRotationMatrix()); }
/*EulerAngles(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.
/** \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; }
// 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)));
}*/
/** \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 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 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 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 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 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 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 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 negative (which is as same as the inverse),
* (-alpha, -beta, -gamma).
*/
EulerAngles operator-() const { return inverse(); }
/** \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;
}
/** Set \c *this from either:
* - a 3x3 rotation matrix expression(i.e. pure orthogonal matrix with determinant of +1),
* - a 3D vector expression representing Euler angles.
*
* See EulerAngles(const MatrixBase<Derived, 3>&) for more information about
* angles ranges output.
*/
template <class Derived>
EulerAngles& operator=(const MatrixBase<Derived>& other) {
EIGEN_STATIC_ASSERT(
(internal::is_same<Scalar, typename Derived::Scalar>::value),
YOU_MIXED_DIFFERENT_NUMERIC_TYPES__YOU_NEED_TO_USE_THE_CAST_METHOD_OF_MATRIXBASE_TO_CAST_NUMERIC_TYPES_EXPLICITLY)
/** \returns The Euler angles rotation negative (which is as same as the inverse),
* (-alpha, -beta, -gamma).
*/
EulerAngles operator -() const
{
return inverse();
}
/** Set \c *this from either:
* - a 3x3 rotation matrix expression(i.e. pure orthogonal matrix with determinant of +1),
* - a 3D vector expression representing Euler angles.
*
* See EulerAngles(const MatrixBase<Derived, 3>&) for more information about
* angles ranges output.
*/
template<class Derived>
EulerAngles& operator=(const MatrixBase<Derived>& other)
{
EIGEN_STATIC_ASSERT((internal::is_same<Scalar, typename Derived::Scalar>::value),
YOU_MIXED_DIFFERENT_NUMERIC_TYPES__YOU_NEED_TO_USE_THE_CAST_METHOD_OF_MATRIXBASE_TO_CAST_NUMERIC_TYPES_EXPLICITLY)
internal::eulerangles_assign_impl<System, Derived>::run(*this, other.derived());
return *this;
}
internal::eulerangles_assign_impl<System, Derived>::run(*this, other.derived());
return *this;
}
// TODO: Assign and construct from another EulerAngles (with different system)
/** Set \c *this from a rotation.
*
* See EulerAngles(const RotationBase<Derived, 3>&) for more information about
* angles ranges output.
*/
template<typename Derived>
EulerAngles& operator=(const RotationBase<Derived, 3>& rot) {
System::CalcEulerAngles(*this, rot.toRotationMatrix());
return *this;
}
/** \returns \c true if \c *this is approximately equal to \a other, within the precision
* determined by \a prec.
*
* \sa MatrixBase::isApprox() */
bool isApprox(const EulerAngles& other,
const RealScalar& prec = NumTraits<Scalar>::dummy_precision()) const
{ return angles().isApprox(other.angles(), prec); }
// TODO: Assign and construct from another EulerAngles (with different system)
/** \returns an equivalent 3x3 rotation matrix. */
Matrix3 toRotationMatrix() const
{
// TODO: Calc it faster
return static_cast<QuaternionType>(*this).toRotationMatrix();
}
/** Set \c *this from a rotation.
*
* See EulerAngles(const RotationBase<Derived, 3>&) for more information about
* angles ranges output.
*/
template <typename Derived>
EulerAngles& operator=(const RotationBase<Derived, 3>& rot) {
System::CalcEulerAngles(*this, rot.toRotationMatrix());
return *this;
}
/** 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;
}
/** \returns \c *this with scalar type casted to \a NewScalarType */
template <typename NewScalarType>
EulerAngles<NewScalarType, System> cast() const
{
EulerAngles<NewScalarType, System> e;
e.angles() = angles().template cast<NewScalarType>();
return e;
}
};
/** \returns \c true if \c *this is approximately equal to \a other, within the precision
* determined by \a prec.
*
* \sa MatrixBase::isApprox() */
bool isApprox(const EulerAngles& other, const RealScalar& prec = NumTraits<Scalar>::dummy_precision()) const {
return angles().isApprox(other.angles(), prec);
}
/** \returns an equivalent 3x3 rotation matrix. */
Matrix3 toRotationMatrix() const {
// TODO: Calc it faster
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;
}
/** \returns \c *this with scalar type casted to \a NewScalarType */
template <typename NewScalarType>
EulerAngles<NewScalarType, System> cast() const {
EulerAngles<NewScalarType, System> e;
e.angles() = angles().template cast<NewScalarType>();
return e;
}
};
#define EIGEN_EULER_ANGLES_SINGLE_TYPEDEF(AXES, SCALAR_TYPE, SCALAR_POSTFIX) \
/** \ingroup EulerAngles_Module */ \
/** \ingroup EulerAngles_Module */ \
typedef EulerAngles<SCALAR_TYPE, EulerSystem##AXES> EulerAngles##AXES##SCALAR_POSTFIX;
#define EIGEN_EULER_ANGLES_TYPEDEFS(SCALAR_TYPE, 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) \
@@ -323,36 +315,26 @@ namespace Eigen
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;
};
// set from a rotation matrix
template<class System, class Other>
struct eulerangles_assign_impl<System,Other,3,3>
{
typedef typename Other::Scalar Scalar;
static void run(EulerAngles<Scalar, System>& e, const Other& m)
{
System::CalcEulerAngles(e, m);
}
};
// set from a vector of Euler angles
template<class System, class Other>
struct eulerangles_assign_impl<System,Other,3,1>
{
typedef typename Other::Scalar Scalar;
static void run(EulerAngles<Scalar, System>& e, const Other& vec)
{
e.angles() = vec;
}
};
}
}
namespace internal {
template <typename Scalar_, class _System>
struct traits<EulerAngles<Scalar_, _System> > {
typedef Scalar_ Scalar;
};
#endif // EIGEN_EULERANGLESCLASS_H
// set from a rotation matrix
template <class System, class Other>
struct eulerangles_assign_impl<System, Other, 3, 3> {
typedef typename Other::Scalar Scalar;
static void run(EulerAngles<Scalar, System>& e, const Other& m) { System::CalcEulerAngles(e, m); }
};
// set from a vector of Euler angles
template <class System, class Other>
struct eulerangles_assign_impl<System, Other, 3, 1> {
typedef typename Other::Scalar Scalar;
static void run(EulerAngles<Scalar, System>& e, const Other& vec) { e.angles() = vec; }
};
} // namespace internal
} // namespace Eigen
#endif // EIGEN_EULERANGLESCLASS_H

511
unsupported/Eigen/src/EulerAngles/EulerSystem.h
View File

@@ -13,296 +13,271 @@
// IWYU pragma: private
#include "./InternalHeaderCheck.h"
namespace Eigen
{
// Forward declarations
template <typename Scalar_, class _System>
class EulerAngles;
namespace internal
{
// TODO: Add this trait to the Eigen internal API?
template <int Num, bool IsPositive = (Num > 0)>
struct Abs
{
enum { value = Num };
};
template <int Num>
struct Abs<Num, false>
{
enum { value = -Num };
};
namespace Eigen {
// Forward declarations
template <typename Scalar_, class _System>
class EulerAngles;
template <int Axis>
struct IsValidAxis
{
enum { value = Axis != 0 && Abs<Axis>::value <= 3 };
};
template<typename System,
typename Other,
int OtherRows=Other::RowsAtCompileTime,
int OtherCols=Other::ColsAtCompileTime>
struct eulerangles_assign_impl;
}
#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 */
namespace internal {
// TODO: Add this trait to the Eigen internal 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 };
};
template <typename System, typename Other, int OtherRows = Other::RowsAtCompileTime,
int OtherCols = Other::ColsAtCompileTime>
struct eulerangles_assign_impl;
} // namespace internal
#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 _BetaAxis the second fixed EulerAxis
*
* \tparam _GammaAxis 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 constexpr int AlphaAxis = _AlphaAxis;
/** The second rotation axis */
static constexpr int BetaAxis = _BetaAxis;
/** The third rotation axis */
static constexpr 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, /*!< whether alpha axis is negative */
IsBetaOpposite = (BetaAxis < 0) ? 1 : 0, /*!< whether beta axis is negative */
IsGammaOpposite = (GammaAxis < 0) ? 1 : 0, /*!< whether gamma axis is negative */
// Parity is even if alpha axis X is followed by beta axis Y, or Y is followed
// by Z, or Z is followed by X; otherwise it is odd.
IsOdd = ((AlphaAxisAbs) % 3 == (BetaAxisAbs - 1) % 3) ? 0 : 1, /*!< whether the Euler system is odd */
IsEven = IsOdd ? 0 : 1, /*!< whether the Euler system is even */
IsTaitBryan =
((unsigned)AlphaAxisAbs != (unsigned)GammaAxisAbs) ? 1 : 0 /*!< whether the Euler system is Tait-Bryan */
};
/** \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 _BetaAxis the second fixed EulerAxis
*
* \tparam _GammaAxis 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 constexpr int AlphaAxis = _AlphaAxis;
/** The second rotation axis */
static constexpr int BetaAxis = _BetaAxis;
/** The third rotation axis */
static constexpr 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, /*!< whether alpha axis is negative */
IsBetaOpposite = (BetaAxis < 0) ? 1 : 0, /*!< whether beta axis is negative */
IsGammaOpposite = (GammaAxis < 0) ? 1 : 0, /*!< whether gamma axis is negative */
private:
EIGEN_EULER_ANGLES_CLASS_STATIC_ASSERT(internal::IsValidAxis<AlphaAxis>::value, ALPHA_AXIS_IS_INVALID);
// Parity is even if alpha axis X is followed by beta axis Y, or Y is followed
// by Z, or Z is followed by X; otherwise it is odd.
IsOdd = ((AlphaAxisAbs)%3 == (BetaAxisAbs - 1)%3) ? 0 : 1, /*!< whether the Euler system is odd */
IsEven = IsOdd ? 0 : 1, /*!< whether the Euler system is even */
EIGEN_EULER_ANGLES_CLASS_STATIC_ASSERT(internal::IsValidAxis<BetaAxis>::value, BETA_AXIS_IS_INVALID);
IsTaitBryan = ((unsigned)AlphaAxisAbs != (unsigned)GammaAxisAbs) ? 1 : 0 /*!< whether 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);
EIGEN_EULER_ANGLES_CLASS_STATIC_ASSERT(internal::IsValidAxis<GammaAxis>::value, GAMMA_AXIS_IS_INVALID);
static const int
// I, J, K are the pivot indexes permutation for the rotation matrix, that match this Euler system.
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);
static const int
// 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::sqrt;
typedef typename Derived::Scalar Scalar;
J_ = (AlphaAxisAbs - 1 + 1 + IsOdd) % 3, K_ = (AlphaAxisAbs - 1 + 2 - IsOdd) % 3;
const Scalar plusMinus = IsEven? 1 : -1;
const Scalar minusPlus = IsOdd? 1 : -1;
// 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::sqrt;
const Scalar Rsum = sqrt((mat(I_,I_) * mat(I_,I_) + mat(I_,J_) * mat(I_,J_) + mat(J_,K_) * mat(J_,K_) + mat(K_,K_) * mat(K_,K_))/2);
res[1] = atan2(plusMinus * mat(I_,K_), Rsum);
typedef typename Derived::Scalar Scalar;
// There is a singularity when cos(beta) == 0
if(Rsum > 4 * NumTraits<Scalar>::epsilon()) {// cos(beta) != 0
res[0] = atan2(minusPlus * mat(J_, K_), mat(K_, K_));
res[2] = atan2(minusPlus * mat(I_, J_), mat(I_, I_));
}
else if(plusMinus * mat(I_, K_) > 0) {// cos(beta) == 0 and sin(beta) == 1
Scalar spos = mat(J_, I_) + plusMinus * mat(K_, J_); // 2*sin(alpha + plusMinus * gamma
Scalar cpos = mat(J_, J_) + minusPlus * mat(K_, I_); // 2*cos(alpha + plusMinus * gamma)
Scalar alphaPlusMinusGamma = atan2(spos, cpos);
res[0] = alphaPlusMinusGamma;
res[2] = 0;
}
else {// cos(beta) == 0 and sin(beta) == -1
Scalar sneg = plusMinus * (mat(K_, J_) + minusPlus * mat(J_, I_)); // 2*sin(alpha + minusPlus*gamma)
Scalar cneg = mat(J_, J_) + plusMinus * mat(K_, I_); // 2*cos(alpha + minusPlus*gamma)
Scalar alphaMinusPlusBeta = atan2(sneg, cneg);
res[0] = alphaMinusPlusBeta;
res[2] = 0;
}
const Scalar plusMinus = IsEven ? 1 : -1;
const Scalar minusPlus = IsOdd ? 1 : -1;
const Scalar Rsum = sqrt((mat(I_, I_) * mat(I_, I_) + mat(I_, J_) * mat(I_, J_) + mat(J_, K_) * mat(J_, K_) +
mat(K_, K_) * mat(K_, K_)) /
2);
res[1] = atan2(plusMinus * mat(I_, K_), Rsum);
// There is a singularity when cos(beta) == 0
if (Rsum > 4 * NumTraits<Scalar>::epsilon()) { // cos(beta) != 0
res[0] = atan2(minusPlus * mat(J_, K_), mat(K_, K_));
res[2] = atan2(minusPlus * mat(I_, J_), mat(I_, I_));
} else if (plusMinus * mat(I_, K_) > 0) { // cos(beta) == 0 and sin(beta) == 1
Scalar spos = mat(J_, I_) + plusMinus * mat(K_, J_); // 2*sin(alpha + plusMinus * gamma
Scalar cpos = mat(J_, J_) + minusPlus * mat(K_, I_); // 2*cos(alpha + plusMinus * gamma)
Scalar alphaPlusMinusGamma = atan2(spos, cpos);
res[0] = alphaPlusMinusGamma;
res[2] = 0;
} else { // cos(beta) == 0 and sin(beta) == -1
Scalar sneg = plusMinus * (mat(K_, J_) + minusPlus * mat(J_, I_)); // 2*sin(alpha + minusPlus*gamma)
Scalar cneg = mat(J_, J_) + plusMinus * mat(K_, I_); // 2*cos(alpha + minusPlus*gamma)
Scalar alphaMinusPlusBeta = atan2(sneg, cneg);
res[0] = alphaMinusPlusBeta;
res[2] = 0;
}
}
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::sqrt;
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::sqrt;
typedef typename Derived::Scalar Scalar;
typedef typename Derived::Scalar Scalar;
const Scalar plusMinus = IsEven? 1 : -1;
const Scalar minusPlus = IsOdd? 1 : -1;
const Scalar plusMinus = IsEven ? 1 : -1;
const Scalar minusPlus = IsOdd ? 1 : -1;
const Scalar Rsum = sqrt((mat(I_, J_) * mat(I_, J_) + mat(I_, K_) * mat(I_, K_) + mat(J_, I_) * mat(J_, I_) + mat(K_, I_) * mat(K_, I_)) / 2);
const Scalar Rsum = sqrt((mat(I_, J_) * mat(I_, J_) + mat(I_, K_) * mat(I_, K_) + mat(J_, I_) * mat(J_, I_) +
mat(K_, I_) * mat(K_, I_)) /
2);
res[1] = atan2(Rsum, mat(I_, I_));
res[1] = atan2(Rsum, mat(I_, I_));
// There is a singularity when sin(beta) == 0
if(Rsum > 4 * NumTraits<Scalar>::epsilon()) {// sin(beta) != 0
res[0] = atan2(mat(J_, I_), minusPlus * mat(K_, I_));
res[2] = atan2(mat(I_, J_), plusMinus * mat(I_, K_));
}
else if(mat(I_, I_) > 0) {// sin(beta) == 0 and cos(beta) == 1
Scalar spos = plusMinus * mat(K_, J_) + minusPlus * mat(J_, K_); // 2*sin(alpha + gamma)
Scalar cpos = mat(J_, J_) + mat(K_, K_); // 2*cos(alpha + gamma)
res[0] = atan2(spos, cpos);
res[2] = 0;
}
else {// sin(beta) == 0 and cos(beta) == -1
Scalar sneg = plusMinus * mat(K_, J_) + plusMinus * mat(J_, K_); // 2*sin(alpha - gamma)
Scalar cneg = mat(J_, J_) - mat(K_, K_); // 2*cos(alpha - gamma)
res[0] = atan2(sneg, cneg);
res[2] = 0;
}
// There is a singularity when sin(beta) == 0
if (Rsum > 4 * NumTraits<Scalar>::epsilon()) { // sin(beta) != 0
res[0] = atan2(mat(J_, I_), minusPlus * mat(K_, I_));
res[2] = atan2(mat(I_, J_), plusMinus * mat(I_, K_));
} else if (mat(I_, I_) > 0) { // sin(beta) == 0 and cos(beta) == 1
Scalar spos = plusMinus * mat(K_, J_) + minusPlus * mat(J_, K_); // 2*sin(alpha + gamma)
Scalar cpos = mat(J_, J_) + mat(K_, K_); // 2*cos(alpha + gamma)
res[0] = atan2(spos, cpos);
res[2] = 0;
} else { // sin(beta) == 0 and cos(beta) == -1
Scalar sneg = plusMinus * mat(K_, J_) + plusMinus * mat(J_, K_); // 2*sin(alpha - gamma)
Scalar cneg = mat(J_, J_) - mat(K_, K_); // 2*cos(alpha - gamma)
res[0] = atan2(sneg, cneg);
res[2] = 0;
}
template<typename Scalar>
static void CalcEulerAngles(
EulerAngles<Scalar, EulerSystem>& res,
const typename EulerAngles<Scalar, EulerSystem>::Matrix3& mat)
{
CalcEulerAngles_imp(
res.angles(), mat,
std::conditional_t<IsTaitBryan, internal::true_type, internal::false_type>());
}
if (IsAlphaOpposite)
res.alpha() = -res.alpha();
if (IsBetaOpposite)
res.beta() = -res.beta();
if (IsGammaOpposite)
res.gamma() = -res.gamma();
}
template <typename Scalar_, class _System>
friend class Eigen::EulerAngles;
template<typename System,
typename Other,
int OtherRows,
int OtherCols>
friend struct internal::eulerangles_assign_impl;
};
template <typename Scalar>
static void CalcEulerAngles(EulerAngles<Scalar, EulerSystem>& res,
const typename EulerAngles<Scalar, EulerSystem>::Matrix3& mat) {
CalcEulerAngles_imp(res.angles(), mat,
std::conditional_t<IsTaitBryan, internal::true_type, internal::false_type>());
if (IsAlphaOpposite) res.alpha() = -res.alpha();
if (IsBetaOpposite) res.beta() = -res.beta();
if (IsGammaOpposite) res.gamma() = -res.gamma();
}
template <typename Scalar_, class _System>
friend class Eigen::EulerAngles;
template <typename System, typename Other, int OtherRows, int OtherCols>
friend struct internal::eulerangles_assign_impl;
};
#define EIGEN_EULER_SYSTEM_TYPEDEF(A, B, C) \
/** \ingroup EulerAngles_Module */ \
/** \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
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)
} // namespace Eigen
#endif // EIGEN_EULERSYSTEM_H