Michael Ernst Peter / Eigen

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

src/Geometry/EulerAngles.h@0:13a5d365ba16, 2016-10-13 (annotated)

Committer:
ykuroda
Date:
Thu Oct 13 04:07:23 2016 +0000
Revision:
0:13a5d365ba16
First commint, Eigne Matrix Class Library

Who changed what in which revision?

UserRevisionLine numberNew contents of line
ykuroda 0:13a5d365ba16 1 // This file is part of Eigen, a lightweight C++ template library
ykuroda 0:13a5d365ba16 2 // for linear algebra.
ykuroda 0:13a5d365ba16 3 //
ykuroda 0:13a5d365ba16 4 // Copyright (C) 2008 Gael Guennebaud <gael.guennebaud@inria.fr>
ykuroda 0:13a5d365ba16 5 //
ykuroda 0:13a5d365ba16 6 // This Source Code Form is subject to the terms of the Mozilla
ykuroda 0:13a5d365ba16 7 // Public License v. 2.0. If a copy of the MPL was not distributed
ykuroda 0:13a5d365ba16 8 // with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
ykuroda 0:13a5d365ba16 9
ykuroda 0:13a5d365ba16 10 #ifndef EIGEN_EULERANGLES_H
ykuroda 0:13a5d365ba16 11 #define EIGEN_EULERANGLES_H
ykuroda 0:13a5d365ba16 12
ykuroda 0:13a5d365ba16 13 namespace Eigen {
ykuroda 0:13a5d365ba16 14
ykuroda 0:13a5d365ba16 15 /** \geometry_module \ingroup Geometry_Module
ykuroda 0:13a5d365ba16 16 *
ykuroda 0:13a5d365ba16 17 *
ykuroda 0:13a5d365ba16 18 * \returns the Euler-angles of the rotation matrix \c *this using the convention defined by the triplet (\a a0,\a a1,\a a2)
ykuroda 0:13a5d365ba16 19 *
ykuroda 0:13a5d365ba16 20 * Each of the three parameters \a a0,\a a1,\a a2 represents the respective rotation axis as an integer in {0,1,2}.
ykuroda 0:13a5d365ba16 21 * For instance, in:
ykuroda 0:13a5d365ba16 22 * \code Vector3f ea = mat.eulerAngles(2, 0, 2); \endcode
ykuroda 0:13a5d365ba16 23 * "2" represents the z axis and "0" the x axis, etc. The returned angles are such that
ykuroda 0:13a5d365ba16 24 * we have the following equality:
ykuroda 0:13a5d365ba16 25 * \code
ykuroda 0:13a5d365ba16 26 * mat == AngleAxisf(ea[0], Vector3f::UnitZ())
ykuroda 0:13a5d365ba16 27 * * AngleAxisf(ea[1], Vector3f::UnitX())
ykuroda 0:13a5d365ba16 28 * * AngleAxisf(ea[2], Vector3f::UnitZ()); \endcode
ykuroda 0:13a5d365ba16 29 * This corresponds to the right-multiply conventions (with right hand side frames).
ykuroda 0:13a5d365ba16 30 *
ykuroda 0:13a5d365ba16 31 * The returned angles are in the ranges [0:pi]x[-pi:pi]x[-pi:pi].
ykuroda 0:13a5d365ba16 32 *
ykuroda 0:13a5d365ba16 33 * \sa class AngleAxis
ykuroda 0:13a5d365ba16 34 */
ykuroda 0:13a5d365ba16 35 template<typename Derived>
ykuroda 0:13a5d365ba16 36 inline Matrix<typename MatrixBase<Derived>::Scalar,3,1>
ykuroda 0:13a5d365ba16 37 MatrixBase<Derived>::eulerAngles(Index a0, Index a1, Index a2) const
ykuroda 0:13a5d365ba16 38 {
ykuroda 0:13a5d365ba16 39 using std::atan2;
ykuroda 0:13a5d365ba16 40 using std::sin;
ykuroda 0:13a5d365ba16 41 using std::cos;
ykuroda 0:13a5d365ba16 42 /* Implemented from Graphics Gems IV */
ykuroda 0:13a5d365ba16 43 EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Derived,3,3)
ykuroda 0:13a5d365ba16 44
ykuroda 0:13a5d365ba16 45 Matrix<Scalar,3,1> res;
ykuroda 0:13a5d365ba16 46 typedef Matrix<typename Derived::Scalar,2,1> Vector2;
ykuroda 0:13a5d365ba16 47
ykuroda 0:13a5d365ba16 48 const Index odd = ((a0+1)%3 == a1) ? 0 : 1;
ykuroda 0:13a5d365ba16 49 const Index i = a0;
ykuroda 0:13a5d365ba16 50 const Index j = (a0 + 1 + odd)%3;
ykuroda 0:13a5d365ba16 51 const Index k = (a0 + 2 - odd)%3;
ykuroda 0:13a5d365ba16 52
ykuroda 0:13a5d365ba16 53 if (a0==a2)
ykuroda 0:13a5d365ba16 54 {
ykuroda 0:13a5d365ba16 55 res[0] = atan2(coeff(j,i), coeff(k,i));
ykuroda 0:13a5d365ba16 56 if((odd && res[0]<Scalar(0)) || ((!odd) && res[0]>Scalar(0)))
ykuroda 0:13a5d365ba16 57 {
ykuroda 0:13a5d365ba16 58 res[0] = (res[0] > Scalar(0)) ? res[0] - Scalar(M_PI) : res[0] + Scalar(M_PI);
ykuroda 0:13a5d365ba16 59 Scalar s2 = Vector2(coeff(j,i), coeff(k,i)).norm();
ykuroda 0:13a5d365ba16 60 res[1] = -atan2(s2, coeff(i,i));
ykuroda 0:13a5d365ba16 61 }
ykuroda 0:13a5d365ba16 62 else
ykuroda 0:13a5d365ba16 63 {
ykuroda 0:13a5d365ba16 64 Scalar s2 = Vector2(coeff(j,i), coeff(k,i)).norm();
ykuroda 0:13a5d365ba16 65 res[1] = atan2(s2, coeff(i,i));
ykuroda 0:13a5d365ba16 66 }
ykuroda 0:13a5d365ba16 67
ykuroda 0:13a5d365ba16 68 // With a=(0,1,0), we have i=0; j=1; k=2, and after computing the first two angles,
ykuroda 0:13a5d365ba16 69 // we can compute their respective rotation, and apply its inverse to M. Since the result must
ykuroda 0:13a5d365ba16 70 // be a rotation around x, we have:
ykuroda 0:13a5d365ba16 71 //
ykuroda 0:13a5d365ba16 72 // c2 s1.s2 c1.s2 1 0 0
ykuroda 0:13a5d365ba16 73 // 0 c1 -s1 * M = 0 c3 s3
ykuroda 0:13a5d365ba16 74 // -s2 s1.c2 c1.c2 0 -s3 c3
ykuroda 0:13a5d365ba16 75 //
ykuroda 0:13a5d365ba16 76 // Thus: m11.c1 - m21.s1 = c3 & m12.c1 - m22.s1 = s3
ykuroda 0:13a5d365ba16 77
ykuroda 0:13a5d365ba16 78 Scalar s1 = sin(res[0]);
ykuroda 0:13a5d365ba16 79 Scalar c1 = cos(res[0]);
ykuroda 0:13a5d365ba16 80 res[2] = atan2(c1*coeff(j,k)-s1*coeff(k,k), c1*coeff(j,j) - s1 * coeff(k,j));
ykuroda 0:13a5d365ba16 81 }
ykuroda 0:13a5d365ba16 82 else
ykuroda 0:13a5d365ba16 83 {
ykuroda 0:13a5d365ba16 84 res[0] = atan2(coeff(j,k), coeff(k,k));
ykuroda 0:13a5d365ba16 85 Scalar c2 = Vector2(coeff(i,i), coeff(i,j)).norm();
ykuroda 0:13a5d365ba16 86 if((odd && res[0]<Scalar(0)) || ((!odd) && res[0]>Scalar(0))) {
ykuroda 0:13a5d365ba16 87 res[0] = (res[0] > Scalar(0)) ? res[0] - Scalar(M_PI) : res[0] + Scalar(M_PI);
ykuroda 0:13a5d365ba16 88 res[1] = atan2(-coeff(i,k), -c2);
ykuroda 0:13a5d365ba16 89 }
ykuroda 0:13a5d365ba16 90 else
ykuroda 0:13a5d365ba16 91 res[1] = atan2(-coeff(i,k), c2);
ykuroda 0:13a5d365ba16 92 Scalar s1 = sin(res[0]);
ykuroda 0:13a5d365ba16 93 Scalar c1 = cos(res[0]);
ykuroda 0:13a5d365ba16 94 res[2] = atan2(s1*coeff(k,i)-c1*coeff(j,i), c1*coeff(j,j) - s1 * coeff(k,j));
ykuroda 0:13a5d365ba16 95 }
ykuroda 0:13a5d365ba16 96 if (!odd)
ykuroda 0:13a5d365ba16 97 res = -res;
ykuroda 0:13a5d365ba16 98
ykuroda 0:13a5d365ba16 99 return res;
ykuroda 0:13a5d365ba16 100 }
ykuroda 0:13a5d365ba16 101
ykuroda 0:13a5d365ba16 102 } // end namespace Eigen
ykuroda 0:13a5d365ba16 103
ykuroda 0:13a5d365ba16 104 #endif // EIGEN_EULERANGLES_H