mirror of
https://gitlab.com/libeigen/eigen.git
synced 2024-12-15 07:10:37 +08:00
Fix Gael reports (except documention)
- "Scalar angle(int) const" should be "const Vector& angles() const" - then method "coeffs" could be removed. - avoid one letter names like h, p, r -> use alpha(), beta(), gamma() ;) - about the "fromRotation" methods: - replace the ones which are not static by operator= (as in Quaternion) - the others are actually static methods: use a capital F: FromRotation - method "invert" should be removed. - use a macro to define both float and double EulerAnglesXYZ* typedefs - AddConstIf -> not used - no needs for NegateIfXor, compilers are extremely good at optimizing away branches based on compile time constants: if(IsHeadingOpposite-=IsEven) res.alpha() = -res.alpha();
This commit is contained in:
Tal Hadad
parent
c006ecace1
commit
2aaaf22623
@ -16,13 +16,19 @@
|
||||
|
||||
#include "Eigen/src/Core/util/DisableStupidWarnings.h"
|
||||
|
||||
namespace Eigen {
|
||||
|
||||
/**
|
||||
* \defgroup EulerAngles_Module EulerAngles module
|
||||
*
|
||||
*
|
||||
*
|
||||
*
|
||||
*/
|
||||
* \defgroup EulerAngles_Module EulerAngles module
|
||||
* \brief This module provides generic euler angles rotation.
|
||||
*
|
||||
* \code
|
||||
* #include <unsupported/Eigen/EulerAngles>
|
||||
* \endcode
|
||||
*
|
||||
*/
|
||||
|
||||
}
|
||||
|
||||
#include "src/EulerAngles/EulerSystem.h"
|
||||
#include "src/EulerAngles/EulerAngles.h"
|
||||
|
||||
@ -29,7 +29,7 @@ namespace Eigen
|
||||
class EulerAngles : public RotationBase<EulerAngles<_Scalar, _System>, 3>
|
||||
{
|
||||
public:
|
||||
/** the scalar type of the coefficients */
|
||||
/** the scalar type of the angles */
|
||||
typedef _Scalar Scalar;
|
||||
typedef _System System;
|
||||
|
||||
@ -38,16 +38,19 @@ namespace Eigen
|
||||
typedef Quaternion<Scalar> QuaternionType;
|
||||
typedef AngleAxis<Scalar> AngleAxisType;
|
||||
|
||||
static Vector3 HeadingAxisVector() {
|
||||
return internal::NegativeIf<System::IsHeadingOpposite>::run(Vector3::Unit(System::HeadingAxisAbs - 1));
|
||||
static Vector3 AlphaAxisVector() {
|
||||
const Vector3& u = Vector3::Unit(System::AlphaAxisAbs - 1);
|
||||
return System::IsAlphaOpposite ? -u : u;
|
||||
}
|
||||
|
||||
static Vector3 PitchAxisVector() {
|
||||
return internal::NegativeIf<System::IsPitchOpposite>::run(Vector3::Unit(System::PitchAxisAbs - 1));
|
||||
static Vector3 BetaAxisVector() {
|
||||
const Vector3& u = Vector3::Unit(System::BetaAxisAbs - 1);
|
||||
return System::IsBetaOpposite ? -u : u;
|
||||
}
|
||||
|
||||
static Vector3 RollAxisVector() {
|
||||
return internal::NegativeIf<System::IsRollOpposite>::run(Vector3::Unit(System::RollAxisAbs - 1));
|
||||
static Vector3 GammaAxisVector() {
|
||||
const Vector3& u = Vector3::Unit(System::GammaAxisAbs - 1);
|
||||
return System::IsGammaOpposite ? -u : u;
|
||||
}
|
||||
|
||||
private:
|
||||
@ -56,7 +59,7 @@ namespace Eigen
|
||||
public:
|
||||
|
||||
EulerAngles() {}
|
||||
inline EulerAngles(Scalar a0, Scalar a1, Scalar a2) : m_angles(a0, a1, a2) {}
|
||||
inline EulerAngles(Scalar alpha, Scalar beta, Scalar gamma) : m_angles(alpha, beta, gamma) {}
|
||||
|
||||
template<typename Derived>
|
||||
inline EulerAngles(const MatrixBase<Derived>& m) { *this = m; }
|
||||
@ -64,11 +67,11 @@ namespace Eigen
|
||||
template<typename Derived>
|
||||
inline EulerAngles(
|
||||
const MatrixBase<Derived>& m,
|
||||
bool positiveRangeHeading,
|
||||
bool positiveRangePitch,
|
||||
bool positiveRangeRoll) {
|
||||
bool positiveRangeAlpha,
|
||||
bool positiveRangeBeta,
|
||||
bool positiveRangeGamma) {
|
||||
|
||||
fromRotation(m, positiveRangeHeading, positiveRangePitch, positiveRangeRoll);
|
||||
System::CalcEulerAngles(*this, m, positiveRangeAlpha, positiveRangeBeta, positiveRangeGamma);
|
||||
}
|
||||
|
||||
template<typename Derived>
|
||||
@ -77,35 +80,24 @@ namespace Eigen
|
||||
template<typename Derived>
|
||||
inline EulerAngles(
|
||||
const RotationBase<Derived, 3>& rot,
|
||||
bool positiveRangeHeading,
|
||||
bool positiveRangePitch,
|
||||
bool positiveRangeRoll) {
|
||||
bool positiveRangeAlpha,
|
||||
bool positiveRangeBeta,
|
||||
bool positiveRangeGamma) {
|
||||
|
||||
fromRotation(rot, positiveRangeHeading, positiveRangePitch, positiveRangeRoll);
|
||||
System::CalcEulerAngles(*this, rot.toRotationMatrix(), positiveRangeAlpha, positiveRangeBeta, positiveRangeGamma);
|
||||
}
|
||||
|
||||
Scalar angle(int i) const { return m_angles.coeff(i); }
|
||||
Scalar& angle(int i) { return m_angles.coeffRef(i); }
|
||||
const Vector3& angles() const { return m_angles; }
|
||||
Vector3& angles() { return m_angles; }
|
||||
|
||||
const Vector3& coeffs() const { return m_angles; }
|
||||
Vector3& coeffs() { return m_angles; }
|
||||
Scalar alpha() const { return m_angles[0]; }
|
||||
Scalar& alpha() { return m_angles[0]; }
|
||||
|
||||
Scalar h() const { return m_angles[0]; }
|
||||
Scalar& h() { return m_angles[0]; }
|
||||
Scalar beta() const { return m_angles[1]; }
|
||||
Scalar& beta() { return m_angles[1]; }
|
||||
|
||||
Scalar p() const { return m_angles[1]; }
|
||||
Scalar& p() { return m_angles[1]; }
|
||||
|
||||
Scalar r() const { return m_angles[2]; }
|
||||
Scalar& r() { return m_angles[2]; }
|
||||
|
||||
EulerAngles invert() const
|
||||
{
|
||||
//m_angles = -m_angles;// I want to do this but there could be an aliasing issue!
|
||||
m_angles *= -1;
|
||||
|
||||
return *this;
|
||||
}
|
||||
Scalar gamma() const { return m_angles[2]; }
|
||||
Scalar& gamma() { return m_angles[2]; }
|
||||
|
||||
EulerAngles inverse() const
|
||||
{
|
||||
@ -121,59 +113,26 @@ namespace Eigen
|
||||
|
||||
/** Constructs and \returns an equivalent 3x3 rotation matrix.
|
||||
*/
|
||||
template<typename Derived>
|
||||
EulerAngles& fromRotation(const MatrixBase<Derived>& m)
|
||||
template<
|
||||
bool PositiveRangeAlpha,
|
||||
bool PositiveRangeBeta,
|
||||
bool PositiveRangeGamma,
|
||||
typename Derived>
|
||||
static EulerAngles FromRotation(const MatrixBase<Derived>& m)
|
||||
{
|
||||
System::eulerAngles(*this, m);
|
||||
return *this;
|
||||
EulerAngles e;
|
||||
System::CalcEulerAngles<PositiveRangeAlpha, PositiveRangeBeta, PositiveRangeGamma>(e, m);
|
||||
return e;
|
||||
}
|
||||
|
||||
template<
|
||||
bool PositiveRangeHeading,
|
||||
bool PositiveRangePitch,
|
||||
bool PositiveRangeRoll,
|
||||
bool PositiveRangeAlpha,
|
||||
bool PositiveRangeBeta,
|
||||
bool PositiveRangeGamma,
|
||||
typename Derived>
|
||||
EulerAngles& fromRotation(const MatrixBase<Derived>& m)
|
||||
static EulerAngles& FromRotation(const RotationBase<Derived, 3>& rot)
|
||||
{
|
||||
System::eulerAngles<PositiveRangeHeading, PositiveRangePitch, PositiveRangeRoll>(*this, m);
|
||||
return *this;
|
||||
}
|
||||
|
||||
template<typename Derived>
|
||||
EulerAngles& fromRotation(
|
||||
const MatrixBase<Derived>& m,
|
||||
bool positiveRangeHeading,
|
||||
bool positiveRangePitch,
|
||||
bool positiveRangeRoll)
|
||||
{
|
||||
System::eulerAngles(*this, m, positiveRangeHeading, positiveRangePitch, positiveRangeRoll);
|
||||
return *this;
|
||||
}
|
||||
|
||||
template<typename Derived>
|
||||
EulerAngles& fromRotation(const RotationBase<Derived, 3>& rot)
|
||||
{
|
||||
return fromRotation(rot.toRotationMatrix());
|
||||
}
|
||||
|
||||
template<
|
||||
bool PositiveRangeHeading,
|
||||
bool PositiveRangePitch,
|
||||
bool PositiveRangeRoll,
|
||||
typename Derived>
|
||||
EulerAngles& fromRotation(const RotationBase<Derived, 3>& rot)
|
||||
{
|
||||
return fromRotation<PositiveRangeHeading, PositiveRangePitch, PositiveRangeRoll>(rot.toRotationMatrix());
|
||||
}
|
||||
|
||||
template<typename Derived>
|
||||
EulerAngles& fromRotation(
|
||||
const RotationBase<Derived, 3>& rot,
|
||||
bool positiveRangeHeading,
|
||||
bool positiveRangePitch,
|
||||
bool positiveRangeRoll)
|
||||
{
|
||||
return fromRotation(rot.toRotationMatrix(), positiveRangeHeading, positiveRangePitch, positiveRangeRoll);
|
||||
return FromRotation<PositiveRangeAlpha, PositiveRangeBeta, PositiveRangeGamma>(rot.toRotationMatrix());
|
||||
}
|
||||
|
||||
/*EulerAngles& fromQuaternion(const QuaternionType& q)
|
||||
@ -193,8 +152,9 @@ namespace Eigen
|
||||
/** Set \c *this from a rotation matrix(i.e. pure orthogonal matrix with determinent of +1).
|
||||
*/
|
||||
template<typename Derived>
|
||||
EulerAngles& operator=(const MatrixBase<Derived>& mat) {
|
||||
return fromRotation(mat);
|
||||
EulerAngles& operator=(const MatrixBase<Derived>& m) {
|
||||
System::CalcEulerAngles(*this, m);
|
||||
return *this;
|
||||
}
|
||||
|
||||
// TODO: Assign and construct from another EulerAngles (with different system)
|
||||
@ -203,7 +163,8 @@ namespace Eigen
|
||||
*/
|
||||
template<typename Derived>
|
||||
EulerAngles& operator=(const RotationBase<Derived, 3>& rot) {
|
||||
return fromRotation(rot.toRotationMatrix());
|
||||
System::CalcEulerAngles(*this, rot.toRotationMatrix());
|
||||
return *this;
|
||||
}
|
||||
|
||||
// TODO: Support isApprox function
|
||||
@ -216,9 +177,9 @@ namespace Eigen
|
||||
QuaternionType toQuaternion() const
|
||||
{
|
||||
return
|
||||
AngleAxisType(h(), HeadingAxisVector()) *
|
||||
AngleAxisType(p(), PitchAxisVector()) *
|
||||
AngleAxisType(r(), RollAxisVector());
|
||||
AngleAxisType(alpha(), AlphaAxisVector()) *
|
||||
AngleAxisType(beta(), BetaAxisVector()) *
|
||||
AngleAxisType(gamma(), GammaAxisVector());
|
||||
}
|
||||
|
||||
operator QuaternionType() const
|
||||
@ -227,20 +188,27 @@ namespace Eigen
|
||||
}
|
||||
};
|
||||
|
||||
typedef EulerAngles<double, EulerSystemXYZ> EulerAnglesXYZd;
|
||||
typedef EulerAngles<double, EulerSystemXYX> EulerAnglesXYXd;
|
||||
typedef EulerAngles<double, EulerSystemXZY> EulerAnglesXZYd;
|
||||
typedef EulerAngles<double, EulerSystemXZX> EulerAnglesXZXd;
|
||||
#define EIGEN_EULER_ANGLES_SINGLE_TYPEDEF(SYSTEM, SCALAR_TYPE, SCALAR_POSTFIX) \
|
||||
typedef EulerAngles<SCALAR_TYPE, SYSTEM> SYSTEM##SCALAR_POSTFIX;
|
||||
|
||||
typedef EulerAngles<double, EulerSystemYZX> EulerAnglesYZXd;
|
||||
typedef EulerAngles<double, EulerSystemYZY> EulerAnglesYZYd;
|
||||
typedef EulerAngles<double, EulerSystemYXZ> EulerAnglesYXZd;
|
||||
typedef EulerAngles<double, EulerSystemYXY> EulerAnglesYXYd;
|
||||
#define EIGEN_EULER_ANGLES_TYPEDEFS(SCALAR_TYPE, SCALAR_POSTFIX) \
|
||||
EIGEN_EULER_ANGLES_SINGLE_TYPEDEF(EulerSystemXYZ, SCALAR_TYPE, SCALAR_POSTFIX) \
|
||||
EIGEN_EULER_ANGLES_SINGLE_TYPEDEF(EulerSystemXYX, SCALAR_TYPE, SCALAR_POSTFIX) \
|
||||
EIGEN_EULER_ANGLES_SINGLE_TYPEDEF(EulerSystemXZY, SCALAR_TYPE, SCALAR_POSTFIX) \
|
||||
EIGEN_EULER_ANGLES_SINGLE_TYPEDEF(EulerSystemXZX, SCALAR_TYPE, SCALAR_POSTFIX) \
|
||||
\
|
||||
EIGEN_EULER_ANGLES_SINGLE_TYPEDEF(EulerSystemYZX, SCALAR_TYPE, SCALAR_POSTFIX) \
|
||||
EIGEN_EULER_ANGLES_SINGLE_TYPEDEF(EulerSystemYZY, SCALAR_TYPE, SCALAR_POSTFIX) \
|
||||
EIGEN_EULER_ANGLES_SINGLE_TYPEDEF(EulerSystemYXZ, SCALAR_TYPE, SCALAR_POSTFIX) \
|
||||
EIGEN_EULER_ANGLES_SINGLE_TYPEDEF(EulerSystemYXY, SCALAR_TYPE, SCALAR_POSTFIX) \
|
||||
\
|
||||
EIGEN_EULER_ANGLES_SINGLE_TYPEDEF(EulerSystemZXY, SCALAR_TYPE, SCALAR_POSTFIX) \
|
||||
EIGEN_EULER_ANGLES_SINGLE_TYPEDEF(EulerSystemZXZ, SCALAR_TYPE, SCALAR_POSTFIX) \
|
||||
EIGEN_EULER_ANGLES_SINGLE_TYPEDEF(EulerSystemZYX, SCALAR_TYPE, SCALAR_POSTFIX) \
|
||||
EIGEN_EULER_ANGLES_SINGLE_TYPEDEF(EulerSystemZYZ, SCALAR_TYPE, SCALAR_POSTFIX)
|
||||
|
||||
typedef EulerAngles<double, EulerSystemZXY> EulerAnglesZXYd;
|
||||
typedef EulerAngles<double, EulerSystemZXZ> EulerAnglesZXZd;
|
||||
typedef EulerAngles<double, EulerSystemZYX> EulerAnglesZYXd;
|
||||
typedef EulerAngles<double, EulerSystemZYZ> EulerAnglesZYZd;
|
||||
EIGEN_EULER_ANGLES_TYPEDEFS(float, f)
|
||||
EIGEN_EULER_ANGLES_TYPEDEFS(double, d)
|
||||
|
||||
namespace internal
|
||||
{
|
||||
|
||||
@ -30,69 +30,6 @@ namespace Eigen
|
||||
{
|
||||
enum { value = -Num };
|
||||
};
|
||||
|
||||
template <bool Cond>
|
||||
struct NegativeIf
|
||||
{
|
||||
template <typename T>
|
||||
static T run(const T& t)
|
||||
{
|
||||
return -t;
|
||||
}
|
||||
};
|
||||
|
||||
template <>
|
||||
struct NegativeIf<false>
|
||||
{
|
||||
template <typename T>
|
||||
static T run(const T& t)
|
||||
{
|
||||
return t;
|
||||
}
|
||||
};
|
||||
|
||||
template <bool Cond>
|
||||
struct NegateIf
|
||||
{
|
||||
template <typename T>
|
||||
static void run(T& t)
|
||||
{
|
||||
t = -t;
|
||||
}
|
||||
};
|
||||
|
||||
template <>
|
||||
struct NegateIf<false>
|
||||
{
|
||||
template <typename T>
|
||||
static void run(T&)
|
||||
{
|
||||
// no op
|
||||
}
|
||||
};
|
||||
|
||||
template <bool Cond1, bool Cond2>
|
||||
struct NegateIfXor : NegateIf<Cond1 != Cond2> {};
|
||||
|
||||
template <typename Type, Type value, bool Cond>
|
||||
struct AddConstIf
|
||||
{
|
||||
template <typename T>
|
||||
static void run(T& t)
|
||||
{
|
||||
t += T(value);
|
||||
}
|
||||
};
|
||||
|
||||
template <typename Type, Type value>
|
||||
struct AddConstIf<Type, value, false>
|
||||
{
|
||||
template <typename T>
|
||||
static void run(T&)
|
||||
{
|
||||
// no op
|
||||
}
|
||||
};
|
||||
|
||||
template <int Axis>
|
||||
struct IsValidAxis
|
||||
@ -108,36 +45,36 @@ namespace Eigen
|
||||
EULER_Z = 3
|
||||
};
|
||||
|
||||
template <int _HeadingAxis, int _PitchAxis, int _RollAxis>
|
||||
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
|
||||
static const int HeadingAxis = _HeadingAxis;
|
||||
static const int PitchAxis = _PitchAxis;
|
||||
static const int RollAxis = _RollAxis;
|
||||
static const int AlphaAxis = _AlphaAxis;
|
||||
static const int BetaAxis = _BetaAxis;
|
||||
static const int GammaAxis = _GammaAxis;
|
||||
|
||||
enum
|
||||
{
|
||||
HeadingAxisAbs = internal::Abs<HeadingAxis>::value,
|
||||
PitchAxisAbs = internal::Abs<PitchAxis>::value,
|
||||
RollAxisAbs = internal::Abs<RollAxis>::value,
|
||||
AlphaAxisAbs = internal::Abs<AlphaAxis>::value,
|
||||
BetaAxisAbs = internal::Abs<BetaAxis>::value,
|
||||
GammaAxisAbs = internal::Abs<GammaAxis>::value,
|
||||
|
||||
IsHeadingOpposite = (HeadingAxis < 0) ? 1 : 0,
|
||||
IsPitchOpposite = (PitchAxis < 0) ? 1 : 0,
|
||||
IsRollOpposite = (RollAxis < 0) ? 1 : 0,
|
||||
IsAlphaOpposite = (AlphaAxis < 0) ? 1 : 0,
|
||||
IsBetaOpposite = (BetaAxis < 0) ? 1 : 0,
|
||||
IsGammaOpposite = (GammaAxis < 0) ? 1 : 0,
|
||||
|
||||
IsOdd = ((HeadingAxisAbs)%3 == (PitchAxisAbs - 1)%3) ? 0 : 1,
|
||||
IsOdd = ((AlphaAxisAbs)%3 == (BetaAxisAbs - 1)%3) ? 0 : 1,
|
||||
IsEven = IsOdd ? 0 : 1,
|
||||
|
||||
// TODO: Assert this, and sort it in a better way
|
||||
IsValid = ((unsigned)HeadingAxisAbs != (unsigned)PitchAxisAbs &&
|
||||
(unsigned)PitchAxisAbs != (unsigned)RollAxisAbs &&
|
||||
internal::IsValidAxis<HeadingAxis>::value && internal::IsValidAxis<PitchAxis>::value && internal::IsValidAxis<RollAxis>::value) ? 1 : 0,
|
||||
IsValid = ((unsigned)AlphaAxisAbs != (unsigned)BetaAxisAbs &&
|
||||
(unsigned)BetaAxisAbs != (unsigned)GammaAxisAbs &&
|
||||
internal::IsValidAxis<AlphaAxis>::value && internal::IsValidAxis<BetaAxis>::value && internal::IsValidAxis<GammaAxis>::value) ? 1 : 0,
|
||||
|
||||
// TODO: After a proper assertation, remove the "IsValid" from this expression
|
||||
IsTaitBryan = (IsValid && (unsigned)HeadingAxisAbs != (unsigned)RollAxisAbs) ? 1 : 0
|
||||
IsTaitBryan = (IsValid && (unsigned)AlphaAxisAbs != (unsigned)GammaAxisAbs) ? 1 : 0
|
||||
};
|
||||
|
||||
private:
|
||||
@ -147,13 +84,14 @@ namespace Eigen
|
||||
// I, J, K are the pivot indexes permutation for the rotation matrix, that match this euler system.
|
||||
// They are used in this class converters.
|
||||
// They are always different from each other, and their possible values are: 0, 1, or 2.
|
||||
I = HeadingAxisAbs - 1,
|
||||
J = (HeadingAxisAbs - 1 + 1 + IsOdd)%3,
|
||||
K = (HeadingAxisAbs - 1 + 2 - IsOdd)%3
|
||||
I = AlphaAxisAbs - 1,
|
||||
J = (AlphaAxisAbs - 1 + 1 + IsOdd)%3,
|
||||
K = (AlphaAxisAbs - 1 + 2 - IsOdd)%3
|
||||
};
|
||||
|
||||
// TODO: Get @mat parameter in form that avoids double evaluation.
|
||||
template <typename Derived>
|
||||
static void eulerAngles_imp(Matrix<typename MatrixBase<Derived>::Scalar, 3, 1>& res, const MatrixBase<Derived>& mat, internal::true_type /*isTaitBryan*/)
|
||||
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;
|
||||
@ -176,7 +114,7 @@ namespace Eigen
|
||||
}
|
||||
|
||||
template <typename Derived>
|
||||
static void eulerAngles_imp(Matrix<typename MatrixBase<Derived>::Scalar,3,1>& res, const MatrixBase<Derived>& mat, internal::false_type /*isTaitBryan*/)
|
||||
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;
|
||||
@ -216,50 +154,55 @@ namespace Eigen
|
||||
public:
|
||||
|
||||
template<typename Scalar>
|
||||
static void eulerAngles(
|
||||
static void CalcEulerAngles(
|
||||
EulerAngles<Scalar, EulerSystem>& res,
|
||||
const typename EulerAngles<Scalar, EulerSystem>::Matrix3& mat)
|
||||
{
|
||||
eulerAngles(res, mat, false, false, false);
|
||||
CalcEulerAngles(res, mat, false, false, false);
|
||||
}
|
||||
|
||||
template<
|
||||
typename Scalar,
|
||||
bool PositiveRangeHeading,
|
||||
bool PositiveRangePitch,
|
||||
bool PositiveRangeRoll>
|
||||
static void eulerAngles(
|
||||
bool PositiveRangeAlpha,
|
||||
bool PositiveRangeBeta,
|
||||
bool PositiveRangeGamma>
|
||||
static void CalcEulerAngles(
|
||||
EulerAngles<Scalar, EulerSystem>& res,
|
||||
const typename EulerAngles<Scalar, EulerSystem>::Matrix3& mat)
|
||||
{
|
||||
eulerAngles(res, mat, PositiveRangeHeading, PositiveRangePitch, PositiveRangeRoll);
|
||||
CalcEulerAngles(res, mat, PositiveRangeAlpha, PositiveRangeBeta, PositiveRangeGamma);
|
||||
}
|
||||
|
||||
template<typename Scalar>
|
||||
static void eulerAngles(
|
||||
static void CalcEulerAngles(
|
||||
EulerAngles<Scalar, EulerSystem>& res,
|
||||
const typename EulerAngles<Scalar, EulerSystem>::Matrix3& mat,
|
||||
bool positiveRangeHeading,
|
||||
bool positiveRangePitch,
|
||||
bool positiveRangeRoll)
|
||||
bool PositiveRangeAlpha,
|
||||
bool PositiveRangeBeta,
|
||||
bool PositiveRangeGamma)
|
||||
{
|
||||
eulerAngles_imp(
|
||||
res.coeffs(), mat,
|
||||
CalcEulerAngles_imp(
|
||||
res.angles(), mat,
|
||||
typename internal::conditional<IsTaitBryan, internal::true_type, internal::false_type>::type());
|
||||
|
||||
internal::NegateIfXor<IsHeadingOpposite, IsEven>::run(res.h());
|
||||
internal::NegateIfXor<IsPitchOpposite, IsEven>::run(res.p());
|
||||
internal::NegateIfXor<IsRollOpposite, IsEven>::run(res.r());
|
||||
if (IsAlphaOpposite == IsOdd)
|
||||
res.alpha() = -res.alpha();
|
||||
|
||||
if (IsBetaOpposite == IsOdd)
|
||||
res.beta() = -res.beta();
|
||||
|
||||
if (IsGammaOpposite == IsOdd)
|
||||
res.gamma() = -res.gamma();
|
||||
|
||||
// Saturate results to the requested range
|
||||
if (positiveRangeHeading && (res.h() < 0))
|
||||
res.h() += Scalar(2 * EIGEN_PI);
|
||||
if (PositiveRangeAlpha && (res.alpha() < 0))
|
||||
res.alpha() += Scalar(2 * EIGEN_PI);
|
||||
|
||||
if (positiveRangePitch && (res.p() < 0))
|
||||
res.p() += Scalar(2 * EIGEN_PI);
|
||||
if (PositiveRangeBeta && (res.beta() < 0))
|
||||
res.beta() += Scalar(2 * EIGEN_PI);
|
||||
|
||||
if (positiveRangeRoll && (res.r() < 0))
|
||||
res.r() += Scalar(2 * EIGEN_PI);
|
||||
if (PositiveRangeGamma && (res.gamma() < 0))
|
||||
res.gamma() += Scalar(2 * EIGEN_PI);
|
||||
}
|
||||
};
|
||||
|
||||
|
||||
@ -15,7 +15,7 @@ using namespace Eigen;
|
||||
|
||||
template<typename EulerSystem, typename Scalar>
|
||||
void verify_euler_ranged(const Matrix<Scalar,3,1>& ea,
|
||||
bool positiveRangeHeading, bool positiveRangePitch, bool positiveRangeRoll)
|
||||
bool positiveRangeAlpha, bool positiveRangeBeta, bool positiveRangeGamma)
|
||||
{
|
||||
typedef EulerAngles<Scalar, EulerSystem> EulerAnglesType;
|
||||
typedef Matrix<Scalar,3,3> Matrix3;
|
||||
@ -24,64 +24,64 @@ void verify_euler_ranged(const Matrix<Scalar,3,1>& ea,
|
||||
typedef AngleAxis<Scalar> AngleAxisType;
|
||||
using std::abs;
|
||||
|
||||
Scalar headingRangeStart, headingRangeEnd;
|
||||
Scalar pitchRangeStart, pitchRangeEnd;
|
||||
Scalar rollRangeStart, rollRangeEnd;
|
||||
Scalar alphaRangeStart, alphaRangeEnd;
|
||||
Scalar betaRangeStart, betaRangeEnd;
|
||||
Scalar gammaRangeStart, gammaRangeEnd;
|
||||
|
||||
if (positiveRangeHeading)
|
||||
if (positiveRangeAlpha)
|
||||
{
|
||||
headingRangeStart = Scalar(0);
|
||||
headingRangeEnd = Scalar(2 * EIGEN_PI);
|
||||
alphaRangeStart = Scalar(0);
|
||||
alphaRangeEnd = Scalar(2 * EIGEN_PI);
|
||||
}
|
||||
else
|
||||
{
|
||||
headingRangeStart = -Scalar(EIGEN_PI);
|
||||
headingRangeEnd = Scalar(EIGEN_PI);
|
||||
alphaRangeStart = -Scalar(EIGEN_PI);
|
||||
alphaRangeEnd = Scalar(EIGEN_PI);
|
||||
}
|
||||
|
||||
if (positiveRangePitch)
|
||||
if (positiveRangeBeta)
|
||||
{
|
||||
pitchRangeStart = Scalar(0);
|
||||
pitchRangeEnd = Scalar(2 * EIGEN_PI);
|
||||
betaRangeStart = Scalar(0);
|
||||
betaRangeEnd = Scalar(2 * EIGEN_PI);
|
||||
}
|
||||
else
|
||||
{
|
||||
pitchRangeStart = -Scalar(EIGEN_PI);
|
||||
pitchRangeEnd = Scalar(EIGEN_PI);
|
||||
betaRangeStart = -Scalar(EIGEN_PI);
|
||||
betaRangeEnd = Scalar(EIGEN_PI);
|
||||
}
|
||||
|
||||
if (positiveRangeRoll)
|
||||
if (positiveRangeGamma)
|
||||
{
|
||||
rollRangeStart = Scalar(0);
|
||||
rollRangeEnd = Scalar(2 * EIGEN_PI);
|
||||
gammaRangeStart = Scalar(0);
|
||||
gammaRangeEnd = Scalar(2 * EIGEN_PI);
|
||||
}
|
||||
else
|
||||
{
|
||||
rollRangeStart = -Scalar(EIGEN_PI);
|
||||
rollRangeEnd = Scalar(EIGEN_PI);
|
||||
gammaRangeStart = -Scalar(EIGEN_PI);
|
||||
gammaRangeEnd = Scalar(EIGEN_PI);
|
||||
}
|
||||
|
||||
const int i = EulerSystem::HeadingAxisAbs - 1;
|
||||
const int j = EulerSystem::PitchAxisAbs - 1;
|
||||
const int k = EulerSystem::RollAxisAbs - 1;
|
||||
const int i = EulerSystem::AlphaAxisAbs - 1;
|
||||
const int j = EulerSystem::BetaAxisAbs - 1;
|
||||
const int k = EulerSystem::GammaAxisAbs - 1;
|
||||
|
||||
const int iFactor = EulerSystem::IsHeadingOpposite ? -1 : 1;
|
||||
const int jFactor = EulerSystem::IsPitchOpposite ? -1 : 1;
|
||||
const int kFactor = EulerSystem::IsRollOpposite ? -1 : 1;
|
||||
const int iFactor = EulerSystem::IsAlphaOpposite ? -1 : 1;
|
||||
const int jFactor = EulerSystem::IsBetaOpposite ? -1 : 1;
|
||||
const int kFactor = EulerSystem::IsGammaOpposite ? -1 : 1;
|
||||
|
||||
const Vector3 I = EulerAnglesType::HeadingAxisVector();
|
||||
const Vector3 J = EulerAnglesType::PitchAxisVector();
|
||||
const Vector3 K = EulerAnglesType::RollAxisVector();
|
||||
const Vector3 I = EulerAnglesType::AlphaAxisVector();
|
||||
const Vector3 J = EulerAnglesType::BetaAxisVector();
|
||||
const Vector3 K = EulerAnglesType::GammaAxisVector();
|
||||
|
||||
EulerAnglesType e(ea[0], ea[1], ea[2]);
|
||||
|
||||
Matrix3 m(e);
|
||||
Vector3 eabis = EulerAnglesType(m, positiveRangeHeading, positiveRangePitch, positiveRangeRoll).coeffs();
|
||||
Vector3 eabis = EulerAnglesType(m, positiveRangeAlpha, positiveRangeBeta, positiveRangeGamma).angles();
|
||||
|
||||
// Check that eabis in range
|
||||
VERIFY(headingRangeStart <= eabis[0] && eabis[0] <= headingRangeEnd);
|
||||
VERIFY(pitchRangeStart <= eabis[1] && eabis[1] <= pitchRangeEnd);
|
||||
VERIFY(rollRangeStart <= eabis[2] && eabis[2] <= rollRangeEnd);
|
||||
VERIFY(alphaRangeStart <= eabis[0] && eabis[0] <= alphaRangeEnd);
|
||||
VERIFY(betaRangeStart <= eabis[1] && eabis[1] <= betaRangeEnd);
|
||||
VERIFY(gammaRangeStart <= eabis[2] && eabis[2] <= gammaRangeEnd);
|
||||
|
||||
Vector3 eabis2 = m.eulerAngles(i, j, k);
|
||||
|
||||
@ -91,11 +91,11 @@ void verify_euler_ranged(const Matrix<Scalar,3,1>& ea,
|
||||
eabis2[2] *= kFactor;
|
||||
|
||||
// Saturate the angles to the correct range
|
||||
if (positiveRangeHeading && (eabis2[0] < 0))
|
||||
if (positiveRangeAlpha && (eabis2[0] < 0))
|
||||
eabis2[0] += Scalar(2 * EIGEN_PI);
|
||||
if (positiveRangePitch && (eabis2[1] < 0))
|
||||
if (positiveRangeBeta && (eabis2[1] < 0))
|
||||
eabis2[1] += Scalar(2 * EIGEN_PI);
|
||||
if (positiveRangeRoll && (eabis2[2] < 0))
|
||||
if (positiveRangeGamma && (eabis2[2] < 0))
|
||||
eabis2[2] += Scalar(2 * EIGEN_PI);
|
||||
|
||||
VERIFY_IS_APPROX(eabis, eabis2);// Verify that our estimation is the same as m.eulerAngles() is
|
||||
@ -104,7 +104,7 @@ void verify_euler_ranged(const Matrix<Scalar,3,1>& ea,
|
||||
VERIFY_IS_APPROX(m, mbis);
|
||||
|
||||
// Tests that are only relevant for no possitive range
|
||||
if (!(positiveRangeHeading || positiveRangePitch || positiveRangeRoll))
|
||||
if (!(positiveRangeAlpha || positiveRangeBeta || positiveRangeGamma))
|
||||
{
|
||||
/* If I==K, and ea[1]==0, then there no unique solution. */
|
||||
/* The remark apply in the case where I!=K, and |ea[1]| is close to pi/2. */
|
||||
@ -117,7 +117,7 @@ void verify_euler_ranged(const Matrix<Scalar,3,1>& ea,
|
||||
|
||||
// Quaternions
|
||||
QuaternionType q(e);
|
||||
eabis = EulerAnglesType(q, positiveRangeHeading, positiveRangePitch, positiveRangeRoll).coeffs();
|
||||
eabis = EulerAnglesType(q, positiveRangeAlpha, positiveRangeBeta, positiveRangeGamma).angles();
|
||||
VERIFY_IS_APPROX(eabis, eabis2);// Verify that the euler angles are still the same
|
||||
}
|
||||
|
||||
|
||||
Loading…
Reference in New Issue
Block a user