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 // This Source Code Form is subject to the terms of the Mozilla
00007 // Public License v. 2.0. If a copy of the MPL was not distributed
00008 // with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
00009 
00010 #ifndef EIGEN_EULERANGLES_H
00011 #define EIGEN_EULERANGLES_H
00012 
00013 namespace Eigen { 
00014 
00031 template<typename Derived>
00032 inline Matrix<typename MatrixBase<Derived>::Scalar,3,1>
00033 MatrixBase<Derived>::eulerAngles(Index a0, Index a1, Index a2) const
00034 {
00035   /* Implemented from Graphics Gems IV */
00036   EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Derived,3,3)
00037 
00038   Matrix<Scalar,3,1> res;
00039   typedef Matrix<typename Derived::Scalar,2,1> Vector2;
00040   const Scalar epsilon = NumTraits<Scalar>::dummy_precision();
00041 
00042   const Index odd = ((a0+1)%3 == a1) ? 0 : 1;
00043   const Index i = a0;
00044   const Index j = (a0 + 1 + odd)%3;
00045   const Index k = (a0 + 2 - odd)%3;
00046 
00047   if (a0==a2)
00048   {
00049     Scalar s = Vector2(coeff(j,i) , coeff(k,i)).norm();
00050     res[1] = internal::atan2(s, coeff(i,i));
00051     if (s > epsilon)
00052     {
00053       res[0] = internal::atan2(coeff(j,i), coeff(k,i));
00054       res[2] = internal::atan2(coeff(i,j),-coeff(i,k));
00055     }
00056     else
00057     {
00058       res[0] = Scalar(0);
00059       res[2] = (coeff(i,i)>0?1:-1)*internal::atan2(-coeff(k,j), coeff(j,j));
00060     }
00061   }
00062   else
00063   {
00064     Scalar c = Vector2(coeff(i,i) , coeff(i,j)).norm();
00065     res[1] = internal::atan2(-coeff(i,k), c);
00066     if (c > epsilon)
00067     {
00068       res[0] = internal::atan2(coeff(j,k), coeff(k,k));
00069       res[2] = internal::atan2(coeff(i,j), coeff(i,i));
00070     }
00071     else
00072     {
00073       res[0] = Scalar(0);
00074       res[2] = (coeff(i,k)>0?1:-1)*internal::atan2(-coeff(k,j), coeff(j,j));
00075     }
00076   }
00077   if (!odd)
00078     res = -res;
00079   return res;
00080 }
00081 
00082 } // end namespace Eigen
00083 
00084 #endif // EIGEN_EULERANGLES_H


win_eigen
Author(s): Daniel Stonier
autogenerated on Mon Oct 6 2014 12:24:24