EulerAngles.h
Go to the documentation of this file.
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@inria.fr>
5 //
6 // Eigen is free software; you can redistribute it and/or
7 // modify it under the terms of the GNU Lesser General Public
8 // License as published by the Free Software Foundation; either
9 // version 3 of the License, or (at your option) any later version.
10 //
11 // Alternatively, you can redistribute it and/or
12 // modify it under the terms of the GNU General Public License as
13 // published by the Free Software Foundation; either version 2 of
14 // the License, or (at your option) any later version.
15 //
16 // Eigen is distributed in the hope that it will be useful, but WITHOUT ANY
17 // WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
18 // FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License or the
19 // GNU General Public License for more details.
20 //
21 // You should have received a copy of the GNU Lesser General Public
22 // License and a copy of the GNU General Public License along with
23 // Eigen. If not, see <http://www.gnu.org/licenses/>.
24 
25 #ifndef EIGEN_EULERANGLES_H
26 #define EIGEN_EULERANGLES_H
27 
28 namespace Eigen {
29 
46 template<typename Derived>
47 inline Matrix<typename MatrixBase<Derived>::Scalar,3,1>
49 {
50  /* Implemented from Graphics Gems IV */
52 
56 
57  const Index odd = ((a0+1)%3 == a1) ? 0 : 1;
58  const Index i = a0;
59  const Index j = (a0 + 1 + odd)%3;
60  const Index k = (a0 + 2 - odd)%3;
61 
62  if (a0==a2)
63  {
64  Scalar s = Vector2(coeff(j,i) , coeff(k,i)).norm();
65  res[1] = internal::atan2(s, coeff(i,i));
66  if (s > epsilon)
67  {
68  res[0] = internal::atan2(coeff(j,i), coeff(k,i));
69  res[2] = internal::atan2(coeff(i,j),-coeff(i,k));
70  }
71  else
72  {
73  res[0] = Scalar(0);
74  res[2] = (coeff(i,i)>0?1:-1)*internal::atan2(-coeff(k,j), coeff(j,j));
75  }
76  }
77  else
78  {
79  Scalar c = Vector2(coeff(i,i) , coeff(i,j)).norm();
80  res[1] = internal::atan2(-coeff(i,k), c);
81  if (c > epsilon)
82  {
83  res[0] = internal::atan2(coeff(j,k), coeff(k,k));
84  res[2] = internal::atan2(coeff(i,j), coeff(i,i));
85  }
86  else
87  {
88  res[0] = Scalar(0);
89  res[2] = (coeff(i,k)>0?1:-1)*internal::atan2(-coeff(k,j), coeff(j,j));
90  }
91  }
92  if (!odd)
93  res = -res;
94  return res;
95 }
96 
97 } // end namespace Eigen
98 
99 #endif // EIGEN_EULERANGLES_H