Initial fork of unsupported module EulerAngles.

This commit is contained in:
Tal Hadad 2015-09-27 16:51:24 +03:00
parent d16797cfc0
commit 5e0a178df2
7 changed files with 453 additions and 0 deletions

View File

@ -0,0 +1,32 @@
// 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_EULERANGLES_MODULE_H
#define EIGEN_EULERANGLES_MODULE_H
#include "Eigen/Core"
#include "Eigen/Geometry"
#include "Eigen/src/Core/util/DisableStupidWarnings.h"
/**
* \defgroup EulerAngles_Module EulerAngles module
*
*
*
*
*/
#include "src/EulerAngles/EulerSystem.h"
#include "src/EulerAngles/EulerAngles.h"
#include "Eigen/src/Core/util/ReenableStupidWarnings.h"
#endif // EIGEN_EULERANGLES_MODULE_H

View File

@ -12,3 +12,4 @@ ADD_SUBDIRECTORY(Skyline)
ADD_SUBDIRECTORY(SparseExtra) ADD_SUBDIRECTORY(SparseExtra)
ADD_SUBDIRECTORY(KroneckerProduct) ADD_SUBDIRECTORY(KroneckerProduct)
ADD_SUBDIRECTORY(Splines) ADD_SUBDIRECTORY(Splines)
ADD_SUBDIRECTORY(EulerAngles)

View File

@ -0,0 +1,6 @@
FILE(GLOB Eigen_IterativeSolvers_SRCS "*.h")
INSTALL(FILES
${Eigen_IterativeSolvers_SRCS}
DESTINATION ${INCLUDE_INSTALL_DIR}/unsupported/Eigen/src/IterativeSolvers COMPONENT Devel
)

View File

@ -0,0 +1,183 @@
// 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_EULERANGLESCLASS_H// TODO: Fix previous "EIGEN_EULERANGLES_H" definition?
#define EIGEN_EULERANGLESCLASS_H
namespace Eigen
{
/*template<typename Other,
int OtherRows=Other::RowsAtCompileTime,
int OtherCols=Other::ColsAtCompileTime>
struct ei_eulerangles_assign_impl;*/
/** \class EulerAngles
*
* \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 cl
*/
template <typename _Scalar, class _System>
class EulerAngles : public RotationBase<EulerAngles<_Scalar, _System>, 3>
{
public:
/** the scalar type of the coefficients */
typedef _Scalar Scalar;
typedef _System System;
typedef Matrix<Scalar,3,3> Matrix3;
typedef Matrix<Scalar,3,1> Vector3;
typedef Quaternion<Scalar> QuaternionType;
typedef AngleAxis<Scalar> AngleAxisType;
protected:
Vector3 m_angles;
public:
EulerAngles() {}
inline EulerAngles(Scalar a0, Scalar a1, Scalar a2) : m_angles(a0, a1, a2) {}
inline EulerAngles(Vector3 angles) : m_angles(angles) {}
inline EulerAngles(const QuaternionType& q) { *this = q; }
inline EulerAngles(const AngleAxisType& aa) { *this = aa; }
template<typename Derived>
inline EulerAngles(const MatrixBase<Derived>& m) { *this = m; }
// TODO: Support assignment from euler to euler
Scalar angle(int i) const { return m_angles.coeff(i); }
Scalar& angle(int i) { return m_angles.coeffRef(i); }
const Vector3& coeffs() const { return m_angles; }
Vector3& coeffs() { return m_angles; }
// TODO: Add set/get functions
Scalar h() const { return m_angles[0]; }
Scalar& h() { return m_angles[0]; }
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;
}
EulerAngles inverse() const
{
EulerAngles res;
res.m_angles = -m_angles;
return res;
}
EulerAngles operator -() const
{
return inverse();
}
/** Constructs and \returns an equivalent 3x3 rotation matrix.
*/
template<typename Derived>
// TODO: Add booleans which let the user control desired output angles range( (-PI, PI) or [0, 2*PI) )
EulerAngles& fromRotationMatrix(const MatrixBase<Derived>& m)
{
System::eulerAngles(*this, m);
return *this;
}
/** 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 fromRotationMatrix(mat);
}
// TODO: Assign and construct from another EulerAngle (with different system)
/** Set \c *this from a quaternion.
* The axis is normalized.
*/
EulerAngles& operator=(const QuaternionType& q){
// TODO: Implement it in a better way
// 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.
// Currently we compute all matrix cells from quaternion.
fromRotationMatrix(q.toRotationMatrix());
// 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)));
*/
return *this;
}
/** Set \c *this from AngleAxis \a ea.
*/
EulerAngles& operator=(const AngleAxisType& ea)
{
// TODO: Implement it in a better way
return *this = ea.toRotationMatrix();
}
// TODO: Fix this function, and make it generic
Matrix3 toRotationMatrix(void) const
{
return static_cast<QuaternionType>(*this).toRotationMatrix();
}
operator QuaternionType() const
{
return
AngleAxisType((System::IsHeadingOpposite ? -1 : 1) * h(), Vector3::Unit(System::HeadingAxisAbs - 1)) *
AngleAxisType((System::IsPitchOpposite ? -1 : 1) * p(), Vector3::Unit(System::PitchAxisAbs - 1)) *
AngleAxisType((System::IsRollOpposite ? -1 : 1) * r(), Vector3::Unit(System::RollAxisAbs - 1));
}
};
typedef EulerAngles<double, EulerSystemXYZ> EulerAnglesXYZd;
typedef EulerAngles<double, EulerSystemXYX> EulerAnglesXYXd;
typedef EulerAngles<double, EulerSystemXZY> EulerAnglesXZYd;
typedef EulerAngles<double, EulerSystemXZX> EulerAnglesXZXd;
typedef EulerAngles<double, EulerSystemYZX> EulerAnglesYZXd;
typedef EulerAngles<double, EulerSystemYZY> EulerAnglesYZYd;
typedef EulerAngles<double, EulerSystemYXZ> EulerAnglesYXZd;
typedef EulerAngles<double, EulerSystemYXY> EulerAnglesYXYd;
typedef EulerAngles<double, EulerSystemZXY> EulerAnglesZXYd;
typedef EulerAngles<double, EulerSystemZXZ> EulerAnglesZXZd;
typedef EulerAngles<double, EulerSystemZYX> EulerAnglesZYXd;
typedef EulerAngles<double, EulerSystemZYZ> EulerAnglesZYZd;
namespace internal
{
template<typename _Scalar, class _System>
struct traits<EulerAngles<_Scalar, _System> >
{
typedef _Scalar Scalar;
};
}
}
#endif // EIGEN_EULERANGLESCLASS_H

View File

@ -0,0 +1,211 @@
// 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 NegateIf
{
template <typename T>
static void run(T& t)
{
t = -t;
}
};
template <>
struct NegateIf<false>
{
template <typename T>
static void run(T& 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

View File

@ -45,6 +45,8 @@ ei_add_test(alignedvector3)
ei_add_test(FFT) ei_add_test(FFT)
ei_add_test(EulerAngles)
find_package(MPFR 2.3.0) find_package(MPFR 2.3.0)
find_package(GMP) find_package(GMP)
if(MPFR_FOUND) if(MPFR_FOUND)

View File

@ -0,0 +1,18 @@
// 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/.
#include "main.h"
#include <unsupported/Eigen/EulerAngles>
void test_EulerAngles()
{
//CALL_SUBTEST( test_return_by_value(32) );
}