mirror of
https://gitlab.com/libeigen/eigen.git
synced 2026-04-10 11:34:33 +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
@@ -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);
|
||||
}
|
||||
};
|
||||
|
||||
|
||||
Reference in New Issue
Block a user