Doc EulerAngles class, and minor fixes.

This commit is contained in:
Tal Hadad 2016-06-06 22:01:40 +03:00
parent 52e4cbf539
commit e30133e439
3 changed files with 104 additions and 30 deletions

View File

@ -22,6 +22,8 @@ namespace Eigen {
* \defgroup EulerAngles_Module EulerAngles module * \defgroup EulerAngles_Module EulerAngles module
* \brief This module provides generic euler angles rotation. * \brief This module provides generic euler angles rotation.
* *
* See EulerAngles for more information.
*
* \code * \code
* #include <unsupported/Eigen/EulerAngles> * #include <unsupported/Eigen/EulerAngles>
* \endcode * \endcode

View File

@ -21,9 +21,9 @@ namespace Eigen
* *
* \brief Represents a rotation in a 3 dimensional space as three Euler angles * \brief Represents a rotation in a 3 dimensional space as three Euler angles
* *
* \param _Scalar the scalar type, i.e., the type of the angles. * \sa _Scalar the scalar type, i.e., the type of the angles.
* *
* \sa cl * \sa _System the EulerSystem to use, which represents the axes of rotation.
*/ */
template <typename _Scalar, class _System> template <typename _Scalar, class _System>
class EulerAngles : public RotationBase<EulerAngles<_Scalar, _System>, 3> class EulerAngles : public RotationBase<EulerAngles<_Scalar, _System>, 3>
@ -38,16 +38,19 @@ namespace Eigen
typedef Quaternion<Scalar> QuaternionType; typedef Quaternion<Scalar> QuaternionType;
typedef AngleAxis<Scalar> AngleAxisType; typedef AngleAxis<Scalar> AngleAxisType;
/** \returns the axis vector of the first (alpha) rotation */
static Vector3 AlphaAxisVector() { static Vector3 AlphaAxisVector() {
const Vector3& u = Vector3::Unit(System::AlphaAxisAbs - 1); const Vector3& u = Vector3::Unit(System::AlphaAxisAbs - 1);
return System::IsAlphaOpposite ? -u : u; return System::IsAlphaOpposite ? -u : u;
} }
/** \returns the axis vector of the second (beta) rotation */
static Vector3 BetaAxisVector() { static Vector3 BetaAxisVector() {
const Vector3& u = Vector3::Unit(System::BetaAxisAbs - 1); const Vector3& u = Vector3::Unit(System::BetaAxisAbs - 1);
return System::IsBetaOpposite ? -u : u; return System::IsBetaOpposite ? -u : u;
} }
/** \returns the axis vector of the third (gamma) rotation */
static Vector3 GammaAxisVector() { static Vector3 GammaAxisVector() {
const Vector3& u = Vector3::Unit(System::GammaAxisAbs - 1); const Vector3& u = Vector3::Unit(System::GammaAxisAbs - 1);
return System::IsGammaOpposite ? -u : u; return System::IsGammaOpposite ? -u : u;
@ -57,15 +60,31 @@ namespace Eigen
Vector3 m_angles; Vector3 m_angles;
public: public:
/** Default constructor without initialization. */
EulerAngles() {} EulerAngles() {}
inline EulerAngles(Scalar alpha, Scalar beta, Scalar gamma) : m_angles(alpha, beta, gamma) {} /** Constructs and initialize euler angles(\p alpha, \p beta, \p gamma). */
EulerAngles(Scalar alpha, Scalar beta, Scalar gamma) : m_angles(alpha, beta, gamma) {}
/** Constructs and initialize euler angles from a 3x3 rotation matrix \p m.
*
* \note All angles will be in the range [-PI, PI].
*/
template<typename Derived> template<typename Derived>
inline EulerAngles(const MatrixBase<Derived>& m) { *this = m; } 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 possitive range is true, then the specified angle will be in the range [0, +2*PI].
* Otherwise, the specified angle will be in the range [-PI, +PI].
*
* \param m The 3x3 rotation matrix to convert
* \param positiveRangeAlpha If true, alpha will be in [0, 2*PI]. Otherwise, in [-PI, +PI].
* \param positiveRangeBeta If true, beta will be in [0, 2*PI]. Otherwise, in [-PI, +PI].
* \param positiveRangeGamma If true, gamma will be in [0, 2*PI]. Otherwise, in [-PI, +PI].
*/
template<typename Derived> template<typename Derived>
inline EulerAngles( EulerAngles(
const MatrixBase<Derived>& m, const MatrixBase<Derived>& m,
bool positiveRangeAlpha, bool positiveRangeAlpha,
bool positiveRangeBeta, bool positiveRangeBeta,
@ -74,11 +93,26 @@ namespace Eigen
System::CalcEulerAngles(*this, m, positiveRangeAlpha, positiveRangeBeta, 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].
*/
template<typename Derived> template<typename Derived>
inline EulerAngles(const RotationBase<Derived, 3>& rot) { *this = rot; } 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 possitive range is true, then the specified angle will be in the range [0, +2*PI].
* Otherwise, the specified angle will be in the range [-PI, +PI].
*
* \param rot The 3x3 rotation matrix to convert
* \param positiveRangeAlpha If true, alpha will be in [0, 2*PI]. Otherwise, in [-PI, +PI].
* \param positiveRangeBeta If true, beta will be in [0, 2*PI]. Otherwise, in [-PI, +PI].
* \param positiveRangeGamma If true, gamma will be in [0, 2*PI]. Otherwise, in [-PI, +PI].
*/
template<typename Derived> template<typename Derived>
inline EulerAngles( EulerAngles(
const RotationBase<Derived, 3>& rot, const RotationBase<Derived, 3>& rot,
bool positiveRangeAlpha, bool positiveRangeAlpha,
bool positiveRangeBeta, bool positiveRangeBeta,
@ -87,18 +121,29 @@ namespace Eigen
System::CalcEulerAngles(*this, rot.toRotationMatrix(), positiveRangeAlpha, positiveRangeBeta, 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; } 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; } Vector3& angles() { return m_angles; }
/** \returns The value of the first angle. */
Scalar alpha() const { return m_angles[0]; } 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]; } Scalar& alpha() { return m_angles[0]; }
/** \returns The value of the second angle. */
Scalar beta() const { return m_angles[1]; } 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]; } Scalar& beta() { return m_angles[1]; }
/** \returns The value of the third angle. */
Scalar gamma() const { return m_angles[2]; } 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]; } 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 inverse() const
{ {
EulerAngles res; EulerAngles res;
@ -106,12 +151,24 @@ namespace Eigen
return res; return res;
} }
/** \returns The euler angles rotation negative (which is as same as the inverse),
* (-alpha, -beta, -gamma).
*/
EulerAngles operator -() const EulerAngles operator -() const
{ {
return inverse(); return inverse();
} }
/** Constructs and \returns an equivalent 3x3 rotation matrix. /** Constructs and initialize euler angles from a 3x3 rotation matrix \p m,
* with options to choose for each angle the requested range (__only in compile time__).
*
* If possitive range is true, then the specified angle will be in the range [0, +2*PI].
* Otherwise, the specified angle will be in the range [-PI, +PI].
*
* \param m The 3x3 rotation matrix to convert
* \tparam positiveRangeAlpha If true, alpha will be in [0, 2*PI]. Otherwise, in [-PI, +PI].
* \tparam positiveRangeBeta If true, beta will be in [0, 2*PI]. Otherwise, in [-PI, +PI].
* \tparam positiveRangeGamma If true, gamma will be in [0, 2*PI]. Otherwise, in [-PI, +PI].
*/ */
template< template<
bool PositiveRangeAlpha, bool PositiveRangeAlpha,
@ -125,6 +182,17 @@ namespace Eigen
return e; return e;
} }
/** Constructs and initialize euler angles from a rotation \p rot,
* with options to choose for each angle the requested range (__only in compile time__).
*
* If possitive range is true, then the specified angle will be in the range [0, +2*PI].
* Otherwise, the specified angle will be in the range [-PI, +PI].
*
* \param rot The 3x3 rotation matrix to convert
* \tparam positiveRangeAlpha If true, alpha will be in [0, 2*PI]. Otherwise, in [-PI, +PI].
* \tparam positiveRangeBeta If true, beta will be in [0, 2*PI]. Otherwise, in [-PI, +PI].
* \tparam positiveRangeGamma If true, gamma will be in [0, 2*PI]. Otherwise, in [-PI, +PI].
*/
template< template<
bool PositiveRangeAlpha, bool PositiveRangeAlpha,
bool PositiveRangeBeta, bool PositiveRangeBeta,
@ -149,8 +217,7 @@ namespace Eigen
//m_angles[2] = std::atan2(2*(q.w()*q.x() + q.y()*q.z()), (1 - 2*(q.x()*q.x() + y2))); //m_angles[2] = std::atan2(2*(q.w()*q.x() + q.y()*q.z()), (1 - 2*(q.x()*q.x() + y2)));
}*/ }*/
/** Set \c *this from a rotation matrix(i.e. pure orthogonal matrix with determinent of +1). /** Set \c *this from a rotation matrix(i.e. pure orthogonal matrix with determinant of +1). */
*/
template<typename Derived> template<typename Derived>
EulerAngles& operator=(const MatrixBase<Derived>& m) { EulerAngles& operator=(const MatrixBase<Derived>& m) {
System::CalcEulerAngles(*this, m); System::CalcEulerAngles(*this, m);
@ -159,8 +226,7 @@ namespace Eigen
// TODO: Assign and construct from another EulerAngles (with different system) // TODO: Assign and construct from another EulerAngles (with different system)
/** Set \c *this from a rotation. /** Set \c *this from a rotation. */
*/
template<typename Derived> template<typename Derived>
EulerAngles& operator=(const RotationBase<Derived, 3>& rot) { EulerAngles& operator=(const RotationBase<Derived, 3>& rot) {
System::CalcEulerAngles(*this, rot.toRotationMatrix()); System::CalcEulerAngles(*this, rot.toRotationMatrix());
@ -169,11 +235,13 @@ namespace Eigen
// TODO: Support isApprox function // TODO: Support isApprox function
/** \returns an equivalent 3x3 rotation matrix. */
Matrix3 toRotationMatrix() const Matrix3 toRotationMatrix() const
{ {
return static_cast<QuaternionType>(*this).toRotationMatrix(); return static_cast<QuaternionType>(*this).toRotationMatrix();
} }
/** \returns an equivalent quaternion */
QuaternionType toQuaternion() const QuaternionType toQuaternion() const
{ {
return return
@ -182,6 +250,7 @@ namespace Eigen
AngleAxisType(gamma(), GammaAxisVector()); AngleAxisType(gamma(), GammaAxisVector());
} }
/** Convert the euler angles to quaternion. */
operator QuaternionType() const operator QuaternionType() const
{ {
return toQuaternion(); return toQuaternion();

View File

@ -103,7 +103,7 @@ namespace Eigen
res[0] = atan2(mat(J,K), mat(K,K)); res[0] = atan2(mat(J,K), mat(K,K));
Scalar c2 = Vector2(mat(I,I), mat(I,J)).norm(); Scalar c2 = Vector2(mat(I,I), mat(I,J)).norm();
if((IsOdd && res[0]<Scalar(0)) || ((!IsOdd) && res[0]>Scalar(0))) { if((IsOdd && res[0]<Scalar(0)) || ((!IsOdd) && res[0]>Scalar(0))) {
res[0] = (res[0] > Scalar(0)) ? res[0] - Scalar(M_PI) : res[0] + Scalar(M_PI); res[0] = (res[0] > Scalar(0)) ? res[0] - Scalar(EIGEN_PI) : res[0] + Scalar(EIGEN_PI);
res[1] = atan2(-mat(I,K), -c2); res[1] = atan2(-mat(I,K), -c2);
} }
else else
@ -126,7 +126,7 @@ namespace Eigen
res[0] = atan2(mat(J,I), mat(K,I)); res[0] = atan2(mat(J,I), mat(K,I));
if((IsOdd && res[0]<Scalar(0)) || ((!IsOdd) && res[0]>Scalar(0))) if((IsOdd && res[0]<Scalar(0)) || ((!IsOdd) && res[0]>Scalar(0)))
{ {
res[0] = (res[0] > Scalar(0)) ? res[0] - Scalar(M_PI) : res[0] + Scalar(M_PI); res[0] = (res[0] > Scalar(0)) ? res[0] - Scalar(EIGEN_PI) : res[0] + Scalar(EIGEN_PI);
Scalar s2 = Vector2(mat(J,I), mat(K,I)).norm(); Scalar s2 = Vector2(mat(J,I), mat(K,I)).norm();
res[1] = -atan2(s2, mat(I,I)); res[1] = -atan2(s2, mat(I,I));
} }
@ -206,20 +206,23 @@ namespace Eigen
} }
}; };
typedef EulerSystem<EULER_X, EULER_Y, EULER_Z> EulerSystemXYZ; #define EIGEN_EULER_SYSTEM_TYPEDEF(A, B, C) \
typedef EulerSystem<EULER_X, EULER_Y, EULER_X> EulerSystemXYX; typedef EulerSystem<EULER_##A, EULER_##B, EULER_##C> EulerSystem##A##B##C;
typedef EulerSystem<EULER_X, EULER_Z, EULER_Y> EulerSystemXZY;
typedef EulerSystem<EULER_X, EULER_Z, EULER_X> EulerSystemXZX;
typedef EulerSystem<EULER_Y, EULER_Z, EULER_X> EulerSystemYZX; EIGEN_EULER_SYSTEM_TYPEDEF(X,Y,Z)
typedef EulerSystem<EULER_Y, EULER_Z, EULER_Y> EulerSystemYZY; EIGEN_EULER_SYSTEM_TYPEDEF(X,Y,X)
typedef EulerSystem<EULER_Y, EULER_X, EULER_Z> EulerSystemYXZ; EIGEN_EULER_SYSTEM_TYPEDEF(X,Z,Y)
typedef EulerSystem<EULER_Y, EULER_X, EULER_Y> EulerSystemYXY; EIGEN_EULER_SYSTEM_TYPEDEF(X,Z,X)
typedef EulerSystem<EULER_Z, EULER_X, EULER_Y> EulerSystemZXY; EIGEN_EULER_SYSTEM_TYPEDEF(Y,Z,X)
typedef EulerSystem<EULER_Z, EULER_X, EULER_Z> EulerSystemZXZ; EIGEN_EULER_SYSTEM_TYPEDEF(Y,Z,Y)
typedef EulerSystem<EULER_Z, EULER_Y, EULER_X> EulerSystemZYX; EIGEN_EULER_SYSTEM_TYPEDEF(Y,X,Z)
typedef EulerSystem<EULER_Z, EULER_Y, EULER_Z> EulerSystemZYZ; 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 #endif // EIGEN_EULERSYSTEM_H