Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025 #ifndef EIGEN_EULERANGLES_H
00026 #define EIGEN_EULERANGLES_H
00027
00028 namespace Eigen {
00029
00046 template<typename Derived>
00047 inline Matrix<typename MatrixBase<Derived>::Scalar,3,1>
00048 MatrixBase<Derived>::eulerAngles(Index a0, Index a1, Index a2) const
00049 {
00050
00051 EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Derived,3,3)
00052
00053 Matrix<Scalar,3,1> res;
00054 typedef Matrix<typename Derived::Scalar,2,1> Vector2;
00055 const Scalar epsilon = NumTraits<Scalar>::dummy_precision();
00056
00057 const Index odd = ((a0+1)%3 == a1) ? 0 : 1;
00058 const Index i = a0;
00059 const Index j = (a0 + 1 + odd)%3;
00060 const Index k = (a0 + 2 - odd)%3;
00061
00062 if (a0==a2)
00063 {
00064 Scalar s = Vector2(coeff(j,i) , coeff(k,i)).norm();
00065 res[1] = internal::atan2(s, coeff(i,i));
00066 if (s > epsilon)
00067 {
00068 res[0] = internal::atan2(coeff(j,i), coeff(k,i));
00069 res[2] = internal::atan2(coeff(i,j),-coeff(i,k));
00070 }
00071 else
00072 {
00073 res[0] = Scalar(0);
00074 res[2] = (coeff(i,i)>0?1:-1)*internal::atan2(-coeff(k,j), coeff(j,j));
00075 }
00076 }
00077 else
00078 {
00079 Scalar c = Vector2(coeff(i,i) , coeff(i,j)).norm();
00080 res[1] = internal::atan2(-coeff(i,k), c);
00081 if (c > epsilon)
00082 {
00083 res[0] = internal::atan2(coeff(j,k), coeff(k,k));
00084 res[2] = internal::atan2(coeff(i,j), coeff(i,i));
00085 }
00086 else
00087 {
00088 res[0] = Scalar(0);
00089 res[2] = (coeff(i,k)>0?1:-1)*internal::atan2(-coeff(k,j), coeff(j,j));
00090 }
00091 }
00092 if (!odd)
00093 res = -res;
00094 return res;
00095 }
00096
00097 }
00098
00099 #endif // EIGEN_EULERANGLES_H