1c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath// This file is part of Eigen, a lightweight C++ template library 2c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath// for linear algebra. 3c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath// 4c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath// Copyright (C) 2008 Gael Guennebaud <gael.guennebaud@inria.fr> 5c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath// 6c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath// This Source Code Form is subject to the terms of the Mozilla 7c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath// Public License v. 2.0. If a copy of the MPL was not distributed 8c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath// with this file, You can obtain one at http://mozilla.org/MPL/2.0/. 9c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 10c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath#ifndef EIGEN_EULERANGLES_H 11c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath#define EIGEN_EULERANGLES_H 12c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 13c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamathnamespace Eigen { 14c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 15c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath/** \geometry_module \ingroup Geometry_Module 16c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath * 17c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath * 18c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath * \returns the Euler-angles of the rotation matrix \c *this using the convention defined by the triplet (\a a0,\a a1,\a a2) 19c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath * 20c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath * Each of the three parameters \a a0,\a a1,\a a2 represents the respective rotation axis as an integer in {0,1,2}. 21c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath * For instance, in: 22c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath * \code Vector3f ea = mat.eulerAngles(2, 0, 2); \endcode 23c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath * "2" represents the z axis and "0" the x axis, etc. The returned angles are such that 24c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath * we have the following equality: 25c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath * \code 26c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath * mat == AngleAxisf(ea[0], Vector3f::UnitZ()) 27c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath * * AngleAxisf(ea[1], Vector3f::UnitX()) 28c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath * * AngleAxisf(ea[2], Vector3f::UnitZ()); \endcode 29c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath * This corresponds to the right-multiply conventions (with right hand side frames). 307faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez * 317faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez * The returned angles are in the ranges [0:pi]x[-pi:pi]x[-pi:pi]. 327faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez * 337faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez * \sa class AngleAxis 34c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath */ 35c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamathtemplate<typename Derived> 36c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamathinline Matrix<typename MatrixBase<Derived>::Scalar,3,1> 37c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan KamathMatrixBase<Derived>::eulerAngles(Index a0, Index a1, Index a2) const 38c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath{ 397faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez using std::atan2; 407faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez using std::sin; 417faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez using std::cos; 42c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath /* Implemented from Graphics Gems IV */ 43c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Derived,3,3) 44c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 45c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath Matrix<Scalar,3,1> res; 46c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath typedef Matrix<typename Derived::Scalar,2,1> Vector2; 47c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 48c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath const Index odd = ((a0+1)%3 == a1) ? 0 : 1; 49c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath const Index i = a0; 50c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath const Index j = (a0 + 1 + odd)%3; 51c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath const Index k = (a0 + 2 - odd)%3; 527faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez 53c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath if (a0==a2) 54c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath { 557faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez res[0] = atan2(coeff(j,i), coeff(k,i)); 567faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez if((odd && res[0]<Scalar(0)) || ((!odd) && res[0]>Scalar(0))) 57c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath { 587faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez res[0] = (res[0] > Scalar(0)) ? res[0] - Scalar(M_PI) : res[0] + Scalar(M_PI); 597faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez Scalar s2 = Vector2(coeff(j,i), coeff(k,i)).norm(); 607faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez res[1] = -atan2(s2, coeff(i,i)); 61c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath } 62c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath else 63c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath { 647faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez Scalar s2 = Vector2(coeff(j,i), coeff(k,i)).norm(); 657faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez res[1] = atan2(s2, coeff(i,i)); 66c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath } 677faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez 687faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez // With a=(0,1,0), we have i=0; j=1; k=2, and after computing the first two angles, 697faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez // we can compute their respective rotation, and apply its inverse to M. Since the result must 707faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez // be a rotation around x, we have: 717faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez // 727faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez // c2 s1.s2 c1.s2 1 0 0 737faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez // 0 c1 -s1 * M = 0 c3 s3 747faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez // -s2 s1.c2 c1.c2 0 -s3 c3 757faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez // 767faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez // Thus: m11.c1 - m21.s1 = c3 & m12.c1 - m22.s1 = s3 777faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez 787faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez Scalar s1 = sin(res[0]); 797faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez Scalar c1 = cos(res[0]); 807faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez res[2] = atan2(c1*coeff(j,k)-s1*coeff(k,k), c1*coeff(j,j) - s1 * coeff(k,j)); 817faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez } 82c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath else 83c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath { 847faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez res[0] = atan2(coeff(j,k), coeff(k,k)); 857faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez Scalar c2 = Vector2(coeff(i,i), coeff(i,j)).norm(); 867faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez if((odd && res[0]<Scalar(0)) || ((!odd) && res[0]>Scalar(0))) { 877faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez res[0] = (res[0] > Scalar(0)) ? res[0] - Scalar(M_PI) : res[0] + Scalar(M_PI); 887faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez res[1] = atan2(-coeff(i,k), -c2); 89c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath } 90c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath else 917faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez res[1] = atan2(-coeff(i,k), c2); 927faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez Scalar s1 = sin(res[0]); 937faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez Scalar c1 = cos(res[0]); 947faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez res[2] = atan2(s1*coeff(k,i)-c1*coeff(j,i), c1*coeff(j,j) - s1 * coeff(k,j)); 95c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath } 96c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath if (!odd) 97c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath res = -res; 987faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez 99c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath return res; 100c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath} 101c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 102c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath} // end namespace Eigen 103c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 104c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath#endif // EIGEN_EULERANGLES_H 105