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
00044 template<typename Derived>
00045 inline Matrix<typename MatrixBase<Derived>::Scalar,3,1>
00046 MatrixBase<Derived>::eulerAngles(Index a0, Index a1, Index a2) const
00047 {
00048
00049 EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Derived,3,3)
00050
00051 Matrix<Scalar,3,1> res;
00052 typedef Matrix<typename Derived::Scalar,2,1> Vector2;
00053 const Scalar epsilon = NumTraits<Scalar>::dummy_precision();
00054
00055 const Index odd = ((a0+1)%3 == a1) ? 0 : 1;
00056 const Index i = a0;
00057 const Index j = (a0 + 1 + odd)%3;
00058 const Index k = (a0 + 2 - odd)%3;
00059
00060 if (a0==a2)
00061 {
00062 Scalar s = Vector2(coeff(j,i) , coeff(k,i)).norm();
00063 res[1] = internal::atan2(s, coeff(i,i));
00064 if (s > epsilon)
00065 {
00066 res[0] = internal::atan2(coeff(j,i), coeff(k,i));
00067 res[2] = internal::atan2(coeff(i,j),-coeff(i,k));
00068 }
00069 else
00070 {
00071 res[0] = Scalar(0);
00072 res[2] = (coeff(i,i)>0?1:-1)*internal::atan2(-coeff(k,j), coeff(j,j));
00073 }
00074 }
00075 else
00076 {
00077 Scalar c = Vector2(coeff(i,i) , coeff(i,j)).norm();
00078 res[1] = internal::atan2(-coeff(i,k), c);
00079 if (c > epsilon)
00080 {
00081 res[0] = internal::atan2(coeff(j,k), coeff(k,k));
00082 res[2] = internal::atan2(coeff(i,j), coeff(i,i));
00083 }
00084 else
00085 {
00086 res[0] = Scalar(0);
00087 res[2] = (coeff(i,k)>0?1:-1)*internal::atan2(-coeff(k,j), coeff(j,j));
00088 }
00089 }
00090 if (!odd)
00091 res = -res;
00092 return res;
00093 }
00094
00095
00096 #endif // EIGEN_EULERANGLES_H