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 
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


libicr
Author(s): Robert Krug
autogenerated on Mon Jan 6 2014 11:32:40