10 #ifndef EIGEN_EULERANGLES_H
11 #define EIGEN_EULERANGLES_H
13 #include "./InternalHeaderCheck.h"
37 template<
typename Derived>
38 EIGEN_DEVICE_FUNC
inline Matrix<typename MatrixBase<Derived>::Scalar,3,1>
41 EIGEN_USING_STD(atan2)
45 EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Derived,3,3)
50 const Index odd = ((a0+1)%3 == a1) ? 0 : 1;
52 const Index j = (a0 + 1 + odd)%3;
53 const Index k = (a0 + 2 - odd)%3;
57 res[0] = atan2(coeff(j,i), coeff(k,i));
58 if((odd && res[0]<
Scalar(0)) || ((!odd) && res[0]>
Scalar(0)))
61 res[0] -=
Scalar(EIGEN_PI);
64 res[0] +=
Scalar(EIGEN_PI);
67 res[1] = -atan2(s2, coeff(i,i));
72 res[1] = atan2(s2, coeff(i,i));
87 res[2] = atan2(c1*coeff(j,k)-s1*coeff(k,k), c1*coeff(j,j) - s1 * coeff(k,j));
91 res[0] = atan2(coeff(j,k), coeff(k,k));
93 if((odd && res[0]<
Scalar(0)) || ((!odd) && res[0]>
Scalar(0))) {
95 res[0] -=
Scalar(EIGEN_PI);
98 res[0] +=
Scalar(EIGEN_PI);
100 res[1] = atan2(-coeff(i,k), -c2);
103 res[1] = atan2(-coeff(i,k), c2);
106 res[2] = atan2(s1*coeff(k,i)-c1*coeff(j,i), c1*coeff(j,j) - s1 * coeff(k,j));
internal::traits< Derived >::Scalar Scalar
Definition: DenseBase.h:61
Matrix< Scalar, 3, 1 > eulerAngles(Index a0, Index a1, Index a2) const
Definition: EulerAngles.h:39
Matrix< Type, 2, 1 > Vector2
[c++11] 2×1 vector of type Type.
Definition: Matrix.h:533
Namespace containing all symbols from the Eigen library.
Definition: Core:139
const Eigen::CwiseUnaryOp< Eigen::internal::scalar_cos_op< typename Derived::Scalar >, const Derived > cos(const Eigen::ArrayBase< Derived > &x)
const Eigen::CwiseUnaryOp< Eigen::internal::scalar_sin_op< typename Derived::Scalar >, const Derived > sin(const Eigen::ArrayBase< Derived > &x)
Eigen::Index Index
The interface type of indices.
Definition: EigenBase.h:41