00001 // This file is part of Eigen, a lightweight C++ template library 00002 // for linear algebra. 00003 // 00004 // Copyright (C) 2008 Gael Guennebaud <gael.guennebaud@inria.fr> 00005 // 00006 // Eigen is free software; you can redistribute it and/or 00007 // modify it under the terms of the GNU Lesser General Public 00008 // License as published by the Free Software Foundation; either 00009 // version 3 of the License, or (at your option) any later version. 00010 // 00011 // Alternatively, you can redistribute it and/or 00012 // modify it under the terms of the GNU General Public License as 00013 // published by the Free Software Foundation; either version 2 of 00014 // the License, or (at your option) any later version. 00015 // 00016 // Eigen is distributed in the hope that it will be useful, but WITHOUT ANY 00017 // WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS 00018 // FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License or the 00019 // GNU General Public License for more details. 00020 // 00021 // You should have received a copy of the GNU Lesser General Public 00022 // License and a copy of the GNU General Public License along with 00023 // Eigen. If not, see <http://www.gnu.org/licenses/>. 00024 00025 #ifndef EIGEN_EULERANGLES_H 00026 #define EIGEN_EULERANGLES_H 00027 00028 /** \geometry_module \ingroup Geometry_Module 00029 * 00030 * 00031 * \returns the Euler-angles of the rotation matrix \c *this using the convention defined by the triplet (\a a0,\a a1,\a a2) 00032 * 00033 * Each of the three parameters \a a0,\a a1,\a a2 represents the respective rotation axis as an integer in {0,1,2}. 00034 * For instance, in: 00035 * \code Vector3f ea = mat.eulerAngles(2, 0, 2); \endcode 00036 * "2" represents the z axis and "0" the x axis, etc. The returned angles are such that 00037 * we have the following equality: 00038 * \code 00039 * mat == AngleAxisf(ea[0], Vector3f::UnitZ()) 00040 * * AngleAxisf(ea[1], Vector3f::UnitX()) 00041 * * AngleAxisf(ea[2], Vector3f::UnitZ()); \endcode 00042 * This corresponds to the right-multiply conventions (with right hand side frames). 00043 */ 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 /* Implemented from Graphics Gems IV */ 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
Page generated by Doxygen 1.7.2 for MRPT 0.9.4 SVN: at Mon Jan 10 22:30:30 UTC 2011 |