EulerAngles.h
Go to the documentation of this file.
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 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   /* Implemented from Graphics Gems IV */
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 } // end namespace Eigen
00098 
00099 #endif // EIGEN_EULERANGLES_H