Allow to construct EulerAngles from 3D vector directly.

Using assignment template struct to distinguish between 3D vector and 3D rotation matrix.
This commit is contained in:
Tal Hadad 2016-11-18 15:01:06 +02:00
parent 15eca2432a
commit 76b2a3e6e7
2 changed files with 57 additions and 18 deletions

View File

@ -12,11 +12,6 @@
namespace Eigen namespace Eigen
{ {
/*template<typename Other,
int OtherRows=Other::RowsAtCompileTime,
int OtherCols=Other::ColsAtCompileTime>
struct ei_eulerangles_assign_impl;*/
/** \class EulerAngles /** \class EulerAngles
* *
* \ingroup EulerAngles_Module * \ingroup EulerAngles_Module
@ -66,7 +61,7 @@ namespace Eigen
* When converting some rotation to EulerAngles, the rules for ranges are as follow: * When converting some rotation to EulerAngles, the rules for ranges are as follow:
* - If the rotation we converting from is an EulerAngles * - If the rotation we converting from is an EulerAngles
* (even when it represented as RotationBase explicitly), angles ranges are __undefined__. * (even when it represented as RotationBase explicitly), angles ranges are __undefined__.
* - otherwise, Alpha and Gamma angles will be in the range [-PI, PI].<BR> * - otherwise, alpha and gamma angles will be in the range [-PI, PI].<BR>
* As for Beta angle: * As for Beta angle:
* - If the system is Tait-Bryan, the beta angle will be in the range [-PI/2, PI/2]. * - If the system is Tait-Bryan, the beta angle will be in the range [-PI/2, PI/2].
* - otherwise: * - otherwise:
@ -143,27 +138,34 @@ namespace Eigen
public: public:
/** Default constructor without initialization. */ /** Default constructor without initialization. */
EulerAngles() {} EulerAngles() {}
/** Constructs and initialize Euler angles(\p alpha, \p beta, \p gamma). */ /** Constructs and initialize an EulerAngles (\p alpha, \p beta, \p gamma). */
EulerAngles(const Scalar& alpha, const Scalar& beta, const Scalar& gamma) : EulerAngles(const Scalar& alpha, const Scalar& beta, const Scalar& gamma) :
m_angles(alpha, beta, gamma) {} m_angles(alpha, beta, gamma) {}
/** Constructs and initialize Euler angles from a 3x3 rotation matrix \p m. // TODO: Test this constructor
/** Constructs and initialize an EulerAngles from the array data {alpha, beta, gamma} */
explicit EulerAngles(const Scalar* data) : m_angles(data) {}
/** Constructs and initializes an EulerAngles from either:
* - a 3x3 rotation matrix expression(i.e. pure orthogonal matrix with determinant of +1),
* - a 3D vector expression representing Euler angles.
* *
* \note Alpha and Gamma angles will be in the range [-PI, PI].<BR> * \note If \p other is a 3x3 rotation matrix, the angles range rules will be as follow:<BR>
* Alpha and gamma angles will be in the range [-PI, PI].<BR>
* As for Beta angle: * As for Beta angle:
* - If the system is Tait-Bryan, the beta angle will be in the range [-PI/2, PI/2]. * - If the system is Tait-Bryan, the beta angle will be in the range [-PI/2, PI/2].
* - otherwise: * - otherwise:
* - If the beta axis is positive, the beta angle will be in the range [0, PI] * - If the beta axis is positive, the beta angle will be in the range [0, PI]
* - If the beta axis is negative, the beta angle will be in the range [-PI, 0] * - If the beta axis is negative, the beta angle will be in the range [-PI, 0]
*/ */
template<typename Derived> template<typename Derived>
EulerAngles(const MatrixBase<Derived>& m) { System::CalcEulerAngles(*this, m); } explicit EulerAngles(const MatrixBase<Derived>& other) { *this = other; }
/** Constructs and initialize Euler angles from a rotation \p rot. /** Constructs and initialize Euler angles from a rotation \p rot.
* *
* \note If \p rot is an EulerAngles (even when it represented as RotationBase explicitly), * \note If \p rot is an EulerAngles (even when it represented as RotationBase explicitly),
* angles ranges are __undefined__. * angles ranges are __undefined__.
* Otherwise, Alpha and Gamma angles will be in the range [-PI, PI].<BR> * Otherwise, alpha and gamma angles will be in the range [-PI, PI].<BR>
* As for Beta angle: * As for Beta angle:
* - If the system is Tait-Bryan, the beta angle will be in the range [-PI/2, PI/2]. * - If the system is Tait-Bryan, the beta angle will be in the range [-PI/2, PI/2].
* - otherwise: * - otherwise:
@ -225,16 +227,20 @@ namespace Eigen
return inverse(); return inverse();
} }
/** Set \c *this from a rotation matrix(i.e. pure orthogonal matrix with determinant of +1). /** Set \c *this from either:
* - a 3x3 rotation matrix expression(i.e. pure orthogonal matrix with determinant of +1),
* - a 3D vector expression representing Euler angles.
* *
* See EulerAngles(const MatrixBase<Derived, 3>&) for more information about * See EulerAngles(const MatrixBase<Derived, 3>&) for more information about
* angles ranges output. * angles ranges output.
*/ */
template<typename Derived> template<class Derived>
EulerAngles& operator=(const MatrixBase<Derived>& m) { EulerAngles& operator=(const MatrixBase<Derived>& other)
EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Derived, 3, 3) {
EIGEN_STATIC_ASSERT((internal::is_same<Scalar, typename Derived::Scalar>::value),
YOU_MIXED_DIFFERENT_NUMERIC_TYPES__YOU_NEED_TO_USE_THE_CAST_METHOD_OF_MATRIXBASE_TO_CAST_NUMERIC_TYPES_EXPLICITLY)
System::CalcEulerAngles(*this, m); internal::eulerangles_assign_impl<System, Derived>::run(*this, other.derived());
return *this; return *this;
} }
@ -321,8 +327,29 @@ EIGEN_EULER_ANGLES_TYPEDEFS(double, d)
{ {
typedef _Scalar Scalar; typedef _Scalar Scalar;
}; };
// set from a rotation matrix
template<class System, class Other>
struct eulerangles_assign_impl<System,Other,3,3>
{
typedef typename Other::Scalar Scalar;
static void run(EulerAngles<Scalar, System>& e, const Other& m)
{
System::CalcEulerAngles(e, m);
}
};
// set from a vector of Euler angles
template<class System, class Other>
struct eulerangles_assign_impl<System,Other,4,1>
{
typedef typename Other::Scalar Scalar;
static void run(EulerAngles<Scalar, System>& e, const Other& vec)
{
e.angles() = vec;
}
};
} }
} }
#endif // EIGEN_EULERANGLESCLASS_H #endif // EIGEN_EULERANGLESCLASS_H

View File

@ -36,6 +36,12 @@ namespace Eigen
{ {
enum { value = Axis != 0 && Abs<Axis>::value <= 3 }; enum { value = Axis != 0 && Abs<Axis>::value <= 3 };
}; };
template<typename System,
typename Other,
int OtherRows=Other::RowsAtCompileTime,
int OtherCols=Other::ColsAtCompileTime>
struct eulerangles_assign_impl;
} }
#define EIGEN_EULER_ANGLES_CLASS_STATIC_ASSERT(COND,MSG) typedef char static_assertion_##MSG[(COND)?1:-1] #define EIGEN_EULER_ANGLES_CLASS_STATIC_ASSERT(COND,MSG) typedef char static_assertion_##MSG[(COND)?1:-1]
@ -269,6 +275,12 @@ namespace Eigen
template <typename _Scalar, class _System> template <typename _Scalar, class _System>
friend class Eigen::EulerAngles; friend class Eigen::EulerAngles;
template<typename System,
typename Other,
int OtherRows,
int OtherCols>
friend struct internal::eulerangles_assign_impl;
}; };
#define EIGEN_EULER_SYSTEM_TYPEDEF(A, B, C) \ #define EIGEN_EULER_SYSTEM_TYPEDEF(A, B, C) \