1 // This file is part of Eigen, a lightweight C++ template library 2 // for linear algebra. 3 // 4 // Copyright (C) 2008 Gael Guennebaud <gael.guennebaud (at) inria.fr> 5 // 6 // This Source Code Form is subject to the terms of the Mozilla 7 // Public License v. 2.0. If a copy of the MPL was not distributed 8 // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. 9 10 #ifndef EIGEN_EULERANGLES_H 11 #define EIGEN_EULERANGLES_H 12 13 namespace Eigen { 14 15 /** \geometry_module \ingroup Geometry_Module 16 * 17 * 18 * \returns the Euler-angles of the rotation matrix \c *this using the convention defined by the triplet (\a a0,\a a1,\a a2) 19 * 20 * Each of the three parameters \a a0,\a a1,\a a2 represents the respective rotation axis as an integer in {0,1,2}. 21 * For instance, in: 22 * \code Vector3f ea = mat.eulerAngles(2, 0, 2); \endcode 23 * "2" represents the z axis and "0" the x axis, etc. The returned angles are such that 24 * we have the following equality: 25 * \code 26 * mat == AngleAxisf(ea[0], Vector3f::UnitZ()) 27 * * AngleAxisf(ea[1], Vector3f::UnitX()) 28 * * AngleAxisf(ea[2], Vector3f::UnitZ()); \endcode 29 * This corresponds to the right-multiply conventions (with right hand side frames). 30 */ 31 template<typename Derived> 32 inline Matrix<typename MatrixBase<Derived>::Scalar,3,1> 33 MatrixBase<Derived>::eulerAngles(Index a0, Index a1, Index a2) const 34 { 35 /* Implemented from Graphics Gems IV */ 36 EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Derived,3,3) 37 38 Matrix<Scalar,3,1> res; 39 typedef Matrix<typename Derived::Scalar,2,1> Vector2; 40 const Scalar epsilon = NumTraits<Scalar>::dummy_precision(); 41 42 const Index odd = ((a0+1)%3 == a1) ? 0 : 1; 43 const Index i = a0; 44 const Index j = (a0 + 1 + odd)%3; 45 const Index k = (a0 + 2 - odd)%3; 46 47 if (a0==a2) 48 { 49 Scalar s = Vector2(coeff(j,i) , coeff(k,i)).norm(); 50 res[1] = internal::atan2(s, coeff(i,i)); 51 if (s > epsilon) 52 { 53 res[0] = internal::atan2(coeff(j,i), coeff(k,i)); 54 res[2] = internal::atan2(coeff(i,j),-coeff(i,k)); 55 } 56 else 57 { 58 res[0] = Scalar(0); 59 res[2] = (coeff(i,i)>0?1:-1)*internal::atan2(-coeff(k,j), coeff(j,j)); 60 } 61 } 62 else 63 { 64 Scalar c = Vector2(coeff(i,i) , coeff(i,j)).norm(); 65 res[1] = internal::atan2(-coeff(i,k), c); 66 if (c > epsilon) 67 { 68 res[0] = internal::atan2(coeff(j,k), coeff(k,k)); 69 res[2] = internal::atan2(coeff(i,j), coeff(i,i)); 70 } 71 else 72 { 73 res[0] = Scalar(0); 74 res[2] = (coeff(i,k)>0?1:-1)*internal::atan2(-coeff(k,j), coeff(j,j)); 75 } 76 } 77 if (!odd) 78 res = -res; 79 return res; 80 } 81 82 } // end namespace Eigen 83 84 #endif // EIGEN_EULERANGLES_H 85
