| 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 | // This Source Code Form is subject to the terms of the Mozilla |
|---|
| 7 | // Public License v. 2.0. If a copy of the MPL was not distributed |
|---|
| 8 | // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. |
|---|
| 9 | |
|---|
| 10 | #ifndef EIGEN_EULERANGLES_H |
|---|
| 11 | #define EIGEN_EULERANGLES_H |
|---|
| 12 | |
|---|
| 13 | namespace Eigen { |
|---|
| 14 | |
|---|
| 15 | /** \geometry_module \ingroup Geometry_Module |
|---|
| 16 | * |
|---|
| 17 | * |
|---|
| 18 | * \returns the Euler-angles of the rotation matrix \c *this using the convention defined by the triplet (\a a0,\a a1,\a a2) |
|---|
| 19 | * |
|---|
| 20 | * Each of the three parameters \a a0,\a a1,\a a2 represents the respective rotation axis as an integer in {0,1,2}. |
|---|
| 21 | * For instance, in: |
|---|
| 22 | * \code Vector3f ea = mat.eulerAngles(2, 0, 2); \endcode |
|---|
| 23 | * "2" represents the z axis and "0" the x axis, etc. The returned angles are such that |
|---|
| 24 | * we have the following equality: |
|---|
| 25 | * \code |
|---|
| 26 | * mat == AngleAxisf(ea[0], Vector3f::UnitZ()) |
|---|
| 27 | * * AngleAxisf(ea[1], Vector3f::UnitX()) |
|---|
| 28 | * * AngleAxisf(ea[2], Vector3f::UnitZ()); \endcode |
|---|
| 29 | * This corresponds to the right-multiply conventions (with right hand side frames). |
|---|
| 30 | */ |
|---|
| 31 | template<typename Derived> |
|---|
| 32 | inline Matrix<typename MatrixBase<Derived>::Scalar,3,1> |
|---|
| 33 | MatrixBase<Derived>::eulerAngles(Index a0, Index a1, Index a2) const |
|---|
| 34 | { |
|---|
| 35 | /* Implemented from Graphics Gems IV */ |
|---|
| 36 | EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Derived,3,3) |
|---|
| 37 | |
|---|
| 38 | Matrix<Scalar,3,1> res; |
|---|
| 39 | typedef Matrix<typename Derived::Scalar,2,1> Vector2; |
|---|
| 40 | const Scalar epsilon = NumTraits<Scalar>::dummy_precision(); |
|---|
| 41 | |
|---|
| 42 | const Index odd = ((a0+1)%3 == a1) ? 0 : 1; |
|---|
| 43 | const Index i = a0; |
|---|
| 44 | const Index j = (a0 + 1 + odd)%3; |
|---|
| 45 | const Index k = (a0 + 2 - odd)%3; |
|---|
| 46 | |
|---|
| 47 | if (a0==a2) |
|---|
| 48 | { |
|---|
| 49 | Scalar s = Vector2(coeff(j,i) , coeff(k,i)).norm(); |
|---|
| 50 | res[1] = internal::atan2(s, coeff(i,i)); |
|---|
| 51 | if (s > epsilon) |
|---|
| 52 | { |
|---|
| 53 | res[0] = internal::atan2(coeff(j,i), coeff(k,i)); |
|---|
| 54 | res[2] = internal::atan2(coeff(i,j),-coeff(i,k)); |
|---|
| 55 | } |
|---|
| 56 | else |
|---|
| 57 | { |
|---|
| 58 | res[0] = Scalar(0); |
|---|
| 59 | res[2] = (coeff(i,i)>0?1:-1)*internal::atan2(-coeff(k,j), coeff(j,j)); |
|---|
| 60 | } |
|---|
| 61 | } |
|---|
| 62 | else |
|---|
| 63 | { |
|---|
| 64 | Scalar c = Vector2(coeff(i,i) , coeff(i,j)).norm(); |
|---|
| 65 | res[1] = internal::atan2(-coeff(i,k), c); |
|---|
| 66 | if (c > epsilon) |
|---|
| 67 | { |
|---|
| 68 | res[0] = internal::atan2(coeff(j,k), coeff(k,k)); |
|---|
| 69 | res[2] = internal::atan2(coeff(i,j), coeff(i,i)); |
|---|
| 70 | } |
|---|
| 71 | else |
|---|
| 72 | { |
|---|
| 73 | res[0] = Scalar(0); |
|---|
| 74 | res[2] = (coeff(i,k)>0?1:-1)*internal::atan2(-coeff(k,j), coeff(j,j)); |
|---|
| 75 | } |
|---|
| 76 | } |
|---|
| 77 | if (!odd) |
|---|
| 78 | res = -res; |
|---|
| 79 | return res; |
|---|
| 80 | } |
|---|
| 81 | |
|---|
| 82 | } // end namespace Eigen |
|---|
| 83 | |
|---|
| 84 | #endif // EIGEN_EULERANGLES_H |
|---|
