mirror of
https://gitlab.com/libeigen/eigen.git
synced 2025-07-31 01:03:38 +08:00
232 lines
7.0 KiB
C++
232 lines
7.0 KiB
C++
// This file is part of Eigen, a lightweight C++ template library
|
|
// for linear algebra.
|
|
//
|
|
// Copyright (C) 2015 Tal Hadad <tal_hd@hotmail.com>
|
|
//
|
|
// This Source Code Form is subject to the terms of the Mozilla
|
|
// Public License v. 2.0. If a copy of the MPL was not distributed
|
|
// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
|
|
|
|
#ifndef EIGEN_EULERSYSTEM_H
|
|
#define EIGEN_EULERSYSTEM_H
|
|
|
|
namespace Eigen
|
|
{
|
|
// Forward declerations
|
|
template <typename _Scalar, class _System>
|
|
class EulerAngles;
|
|
|
|
namespace internal
|
|
{
|
|
// TODO: Check if already exists on the rest API
|
|
template <int Num, bool IsPossitive = (Num > 0)>
|
|
struct Abs
|
|
{
|
|
enum { value = Num };
|
|
};
|
|
|
|
template <int Num>
|
|
struct Abs<Num, false>
|
|
{
|
|
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 <int Axis>
|
|
struct IsValidAxis
|
|
{
|
|
enum { value = Axis != 0 && Abs<Axis>::value <= 3 };
|
|
};
|
|
}
|
|
|
|
enum EulerAxis
|
|
{
|
|
EULER_X = 1,
|
|
EULER_Y = 2,
|
|
EULER_Z = 3
|
|
};
|
|
|
|
template <int _HeadingAxis, int _PitchAxis, int _RollAxis>
|
|
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;
|
|
|
|
enum
|
|
{
|
|
HeadingAxisAbs = internal::Abs<HeadingAxis>::value,
|
|
PitchAxisAbs = internal::Abs<PitchAxis>::value,
|
|
RollAxisAbs = internal::Abs<RollAxis>::value,
|
|
|
|
IsHeadingOpposite = (HeadingAxis < 0) ? 1 : 0,
|
|
IsPitchOpposite = (PitchAxis < 0) ? 1 : 0,
|
|
IsRollOpposite = (RollAxis < 0) ? 1 : 0,
|
|
|
|
IsOdd = ((HeadingAxisAbs)%3 == (PitchAxisAbs - 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,
|
|
|
|
// TODO: After a proper assertation, remove the "IsValid" from this expression
|
|
IsTaitBryan = (IsValid && (unsigned)HeadingAxisAbs != (unsigned)RollAxisAbs) ? 1 : 0
|
|
};
|
|
|
|
private:
|
|
|
|
enum
|
|
{
|
|
// 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
|
|
};
|
|
|
|
template <typename Derived>
|
|
static void eulerAngles_imp(Matrix<typename MatrixBase<Derived>::Scalar, 3, 1>& res, const MatrixBase<Derived>& mat, internal::true_type /*isTaitBryan*/)
|
|
{
|
|
using std::atan2;
|
|
using std::sin;
|
|
using std::cos;
|
|
|
|
typedef typename Derived::Scalar Scalar;
|
|
typedef Matrix<Scalar,2,1> Vector2;
|
|
|
|
res[0] = atan2(mat(J,K), mat(K,K));
|
|
Scalar c2 = Vector2(mat(I,I), mat(I,J)).norm();
|
|
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[1] = atan2(-mat(I,K), -c2);
|
|
}
|
|
else
|
|
res[1] = atan2(-mat(I,K), c2);
|
|
Scalar s1 = sin(res[0]);
|
|
Scalar c1 = cos(res[0]);
|
|
res[2] = atan2(s1*mat(K,I)-c1*mat(J,I), c1*mat(J,J) - s1 * mat(K,J));
|
|
}
|
|
|
|
template <typename Derived>
|
|
static void eulerAngles_imp(Matrix<typename MatrixBase<Derived>::Scalar,3,1>& res, const MatrixBase<Derived>& mat, internal::false_type /*isTaitBryan*/)
|
|
{
|
|
using std::atan2;
|
|
using std::sin;
|
|
using std::cos;
|
|
|
|
typedef typename Derived::Scalar Scalar;
|
|
typedef Matrix<Scalar,2,1> Vector2;
|
|
|
|
res[0] = atan2(mat(J,I), mat(K,I));
|
|
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);
|
|
Scalar s2 = Vector2(mat(J,I), mat(K,I)).norm();
|
|
res[1] = -atan2(s2, mat(I,I));
|
|
}
|
|
else
|
|
{
|
|
Scalar s2 = Vector2(mat(J,I), mat(K,I)).norm();
|
|
res[1] = atan2(s2, mat(I,I));
|
|
}
|
|
|
|
// With a=(0,1,0), we have i=0; j=1; k=2, and after computing the first two angles,
|
|
// we can compute their respective rotation, and apply its inverse to M. Since the result must
|
|
// be a rotation around x, we have:
|
|
//
|
|
// c2 s1.s2 c1.s2 1 0 0
|
|
// 0 c1 -s1 * M = 0 c3 s3
|
|
// -s2 s1.c2 c1.c2 0 -s3 c3
|
|
//
|
|
// Thus: m11.c1 - m21.s1 = c3 & m12.c1 - m22.s1 = s3
|
|
|
|
Scalar s1 = sin(res[0]);
|
|
Scalar c1 = cos(res[0]);
|
|
res[2] = atan2(c1*mat(J,K)-s1*mat(K,K), c1*mat(J,J) - s1 * mat(K,J));
|
|
}
|
|
|
|
public:
|
|
|
|
// TODO: Support headingAxisVector(), ..
|
|
|
|
template<typename Scalar>
|
|
static void eulerAngles(EulerAngles<Scalar, EulerSystem>& res, const typename EulerAngles<Scalar, EulerSystem>::Matrix3& mat)
|
|
{
|
|
eulerAngles_imp(
|
|
res.coeffs(), 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());
|
|
}
|
|
};
|
|
|
|
typedef EulerSystem<EULER_X, EULER_Y, EULER_Z> EulerSystemXYZ;
|
|
typedef EulerSystem<EULER_X, EULER_Y, EULER_X> EulerSystemXYX;
|
|
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;
|
|
typedef EulerSystem<EULER_Y, EULER_Z, EULER_Y> EulerSystemYZY;
|
|
typedef EulerSystem<EULER_Y, EULER_X, EULER_Z> EulerSystemYXZ;
|
|
typedef EulerSystem<EULER_Y, EULER_X, EULER_Y> EulerSystemYXY;
|
|
|
|
typedef EulerSystem<EULER_Z, EULER_X, EULER_Y> EulerSystemZXY;
|
|
typedef EulerSystem<EULER_Z, EULER_X, EULER_Z> EulerSystemZXZ;
|
|
typedef EulerSystem<EULER_Z, EULER_Y, EULER_X> EulerSystemZYX;
|
|
typedef EulerSystem<EULER_Z, EULER_Y, EULER_Z> EulerSystemZYZ;
|
|
}
|
|
|
|
#endif // EIGEN_EULERSYSTEM_H
|