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). 30c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath */ 31c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamathtemplate<typename Derived> 32c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamathinline Matrix<typename MatrixBase<Derived>::Scalar,3,1> 33c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan KamathMatrixBase<Derived>::eulerAngles(Index a0, Index a1, Index a2) const 34c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath{ 35c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath /* Implemented from Graphics Gems IV */ 36c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Derived,3,3) 37c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 38c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath Matrix<Scalar,3,1> res; 39c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath typedef Matrix<typename Derived::Scalar,2,1> Vector2; 40c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath const Scalar epsilon = NumTraits<Scalar>::dummy_precision(); 41c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 42c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath const Index odd = ((a0+1)%3 == a1) ? 0 : 1; 43c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath const Index i = a0; 44c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath const Index j = (a0 + 1 + odd)%3; 45c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath const Index k = (a0 + 2 - odd)%3; 46c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 47c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath if (a0==a2) 48c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath { 49c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath Scalar s = Vector2(coeff(j,i) , coeff(k,i)).norm(); 50c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath res[1] = internal::atan2(s, coeff(i,i)); 51c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath if (s > epsilon) 52c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath { 53c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath res[0] = internal::atan2(coeff(j,i), coeff(k,i)); 54c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath res[2] = internal::atan2(coeff(i,j),-coeff(i,k)); 55c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath } 56c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath else 57c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath { 58c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath res[0] = Scalar(0); 59c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath res[2] = (coeff(i,i)>0?1:-1)*internal::atan2(-coeff(k,j), coeff(j,j)); 60c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath } 61c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath } 62c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath else 63c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath { 64c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath Scalar c = Vector2(coeff(i,i) , coeff(i,j)).norm(); 65c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath res[1] = internal::atan2(-coeff(i,k), c); 66c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath if (c > epsilon) 67c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath { 68c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath res[0] = internal::atan2(coeff(j,k), coeff(k,k)); 69c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath res[2] = internal::atan2(coeff(i,j), coeff(i,i)); 70c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath } 71c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath else 72c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath { 73c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath res[0] = Scalar(0); 74c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath res[2] = (coeff(i,k)>0?1:-1)*internal::atan2(-coeff(k,j), coeff(j,j)); 75c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath } 76c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath } 77c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath if (!odd) 78c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath res = -res; 79c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath return res; 80c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath} 81c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 82c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath} // end namespace Eigen 83c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 84c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath#endif // EIGEN_EULERANGLES_H 85