This bugzilla service is closed. All entries have been migrated to https://gitlab.com/libeigen/eigen
 Lines 22-85 namespace Eigen { (-)a/Eigen/src/Geometry/EulerAngles.h (-19 / +39 lines) 22 ` * \code Vector3f ea = mat.eulerAngles(2, 0, 2); \endcode` 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` 23 ` * "2" represents the z axis and "0" the x axis, etc. The returned angles are such that` 24 ` * we have the following equality:` 24 ` * we have the following equality:` 25 ` * \code` 25 ` * \code` 26 ` * mat == AngleAxisf(ea[0], Vector3f::UnitZ())` 26 ` * mat == AngleAxisf(ea[0], Vector3f::UnitZ())` 27 ` * * AngleAxisf(ea[1], Vector3f::UnitX())` 27 ` * * AngleAxisf(ea[1], Vector3f::UnitX())` 28 ` * * AngleAxisf(ea[2], Vector3f::UnitZ()); \endcode` 28 ` * * AngleAxisf(ea[2], Vector3f::UnitZ()); \endcode` 29 ` * This corresponds to the right-multiply conventions (with right hand side frames).` 29 ` * This corresponds to the right-multiply conventions (with right hand side frames).` 30 ` * ` 31 ` * The returned angles are in the ranges [0:pi]x[0:pi]x[-pi:pi].` 32 ` * ` 33 ` * \sa class AngleAxis` 30 ` */` 34 ` */` 31 `template` 35 `template` 32 `inline Matrix::Scalar,3,1>` 36 `inline Matrix::Scalar,3,1>` 33 `MatrixBase::eulerAngles(Index a0, Index a1, Index a2) const` 37 `MatrixBase::eulerAngles(Index a0, Index a1, Index a2) const` 34 `{` 38 `{` 35 ` using std::atan2;` 39 ` using std::atan2;` 40 ` using std::sin;` 41 ` using std::cos;` 36 ` /* Implemented from Graphics Gems IV */` 42 ` /* Implemented from Graphics Gems IV */` 37 ` EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Derived,3,3)` 43 ` EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Derived,3,3)` 38 44 39 ` Matrix res;` 45 ` Matrix res;` 40 ` typedef Matrix Vector2;` 46 ` typedef Matrix Vector2;` 41 ` const Scalar epsilon = NumTraits::dummy_precision();` 47 ` const Scalar epsilon = NumTraits::dummy_precision();` 42 48 43 ` const Index odd = ((a0+1)%3 == a1) ? 0 : 1;` 49 ` const Index odd = ((a0+1)%3 == a1) ? 0 : 1;` 44 ` const Index i = a0;` 50 ` const Index i = a0;` 45 ` const Index j = (a0 + 1 + odd)%3;` 51 ` const Index j = (a0 + 1 + odd)%3;` 46 ` const Index k = (a0 + 2 - odd)%3;` 52 ` const Index k = (a0 + 2 - odd)%3;` 47 53 ` ` 48 ` if (a0==a2)` 54 ` if (a0==a2)` 49 ` {` 55 ` {` 50 ` Scalar s = Vector2(coeff(j,i) , coeff(k,i)).norm();` 56 ` res[0] = atan2(coeff(j,i), coeff(k,i));` 51 ` res[1] = atan2(s, coeff(i,i));` 57 ` if((odd && res[0]<0) || ((!odd) && res[0]>0))` 52 ` if (s > epsilon)` 53 ` {` 58 ` {` 54 ` res[0] = atan2(coeff(j,i), coeff(k,i));` 59 ` res[0] = (res[0] > 0) ? res[0] - M_PI : res[0] + M_PI;` 55 ` res[2] = atan2(coeff(i,j),-coeff(i,k));` 60 ` Scalar s2 = Vector2(coeff(j,i), coeff(k,i)).norm();` 61 ` res[1] = -atan2(s2, coeff(i,i));` 56 ` }` 62 ` }` 57 ` else` 63 ` else` 58 ` {` 64 ` {` 59 ` res[0] = Scalar(0);` 65 ` Scalar s2 = Vector2(coeff(j,i), coeff(k,i)).norm();` 60 ` res[2] = (coeff(i,i)>0?1:-1)*atan2(-coeff(k,j), coeff(j,j));` 66 ` res[1] = atan2(s2, coeff(i,i));` 61 ` }` 67 ` }` 62 ` }` 68 ` ` 69 ` // With a=(0,1,0), we have i=0; j=1; k=2, and after computing the first two angles,` 70 ` // we can compute their respective rotation, and apply its inverse to M. Since the result must` 71 ` // be a rotation around x, we have:` 72 ` //` 73 ` // c2 s1.s2 c1.s2 1 0 0 ` 74 ` // 0 c1 -s1 * M = 0 c3 s3` 75 ` // -s2 s1.c2 c1.c2 0 -s3 c3` 76 ` //` 77 ` // Thus: m11.c1 - m21.s1 = c3 & m12.c1 - m22.s1 = s3` 78 ` ` 79 ` Scalar s1 = sin(res[0]);` 80 ` Scalar c1 = cos(res[0]);` 81 ` res[2] = atan2(c1*coeff(j,k)-s1*coeff(k,k), c1*coeff(j,j) - s1 * coeff(k,j));` 82 ` } ` 63 ` else` 83 ` else` 64 ` {` 84 ` {` 65 ` Scalar c = Vector2(coeff(i,i) , coeff(i,j)).norm();` 85 ` res[0] = atan2(coeff(j,k), coeff(k,k));` 66 ` res[1] = atan2(-coeff(i,k), c);` 86 ` Scalar c2 = Vector2(coeff(i,i), coeff(i,j)).norm();` 67 ` if (c > epsilon)` 87 ` if((odd && res[0]<0) || ((!odd) && res[0]>0)) {` 68 ` {` 88 ` res[0] = (res[0] > 0) ? res[0] - M_PI : res[0] + M_PI;` 69 ` res[0] = atan2(coeff(j,k), coeff(k,k));` 89 ` res[1] = atan2(-coeff(i,k), -c2);` 70 ` res[2] = atan2(coeff(i,j), coeff(i,i));` 71 ` }` 90 ` }` 72 ` else` 91 ` else` 73 ` {` 92 ` res[1] = atan2(-coeff(i,k), c2);` 74 ` res[0] = Scalar(0);` 93 ` Scalar s1 = sin(res[0]);` 75 ` res[2] = (coeff(i,k)>0?1:-1)*atan2(-coeff(k,j), coeff(j,j));` 94 ` Scalar c1 = cos(res[0]);` 76 ` }` 95 ` res[2] = atan2(s1*coeff(k,i)-c1*coeff(j,i), c1*coeff(j,j) - s1 * coeff(k,j));` 77 ` }` 96 ` }` 78 ` if (!odd)` 97 ` if (!odd)` 79 ` res = -res;` 98 ` res = -res;` 99 ` ` 80 ` return res;` 100 ` return res;` 81 `}` 101 `}` 82 102 83 `} // end namespace Eigen` 103 `} // end namespace Eigen` 84 104 85 `#endif // EIGEN_EULERANGLES_H` 105 `#endif // EIGEN_EULERANGLES_H`
 Lines 1-38 Lines 40-54 template void eulerangl (-)a/test/geo_eulerangles.cpp (-12 / +46 lines) 1 `// This file is part of Eigen, a lightweight C++ template library` 1 `// This file is part of Eigen, a lightweight C++ template library` 2 `// for linear algebra.` 2 `// for linear algebra.` 3 `//` 3 `//` 4 `// Copyright (C) 2008-2009 Gael Guennebaud ` 4 `// Copyright (C) 2008-2012 Gael Guennebaud ` 5 `//` 5 `//` 6 `// This Source Code Form is subject to the terms of the Mozilla` 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` 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/.` 8 `// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.` 9 9 10 `#include "main.h"` 10 `#include "main.h"` 11 `#include ` 11 `#include ` 12 `#include ` 12 `#include ` 13 `#include ` 13 `#include ` 14 14 15 `template void eulerangles(void)` 15 `template void check_all_var(const Matrix& ea)` 16 `{` 16 `{` 17 ` typedef Matrix Matrix3;` 17 ` typedef Matrix Matrix3;` 18 ` typedef Matrix Vector3;` 18 ` typedef Matrix Vector3;` 19 ` typedef Quaternion Quaternionx;` 20 ` typedef AngleAxis AngleAxisx;` 19 ` typedef AngleAxis AngleAxisx;` 21 20 ` ` 22 ` Scalar a = internal::random(-Scalar(M_PI), Scalar(M_PI));` 23 ` Quaternionx q1;` 24 ` q1 = AngleAxisx(a, Vector3::Random().normalized());` 25 ` Matrix3 m;` 26 ` m = q1;` 27 28 ` #define VERIFY_EULER(I,J,K, X,Y,Z) { \` 21 ` #define VERIFY_EULER(I,J,K, X,Y,Z) { \` 29 ` Vector3 ea = m.eulerAngles(I,J,K); \` 22 ` Matrix3 m(AngleAxisx(ea[0], Vector3::Unit##X()) * AngleAxisx(ea[1], Vector3::Unit##Y()) * AngleAxisx(ea[2], Vector3::Unit##Z())); \` 30 ` VERIFY_IS_APPROX(m, Matrix3(AngleAxisx(ea[0], Vector3::Unit##X()) * AngleAxisx(ea[1], Vector3::Unit##Y()) * AngleAxisx(ea[2], Vector3::Unit##Z()))); \` 23 ` Vector3 eabis = m.eulerAngles(I,J,K); \` 24 ` Matrix3 mbis(AngleAxisx(eabis[0], Vector3::Unit##X()) * AngleAxisx(eabis[1], Vector3::Unit##Y()) * AngleAxisx(eabis[2], Vector3::Unit##Z())); \` 25 ` VERIFY_IS_APPROX(m, mbis); \` 26 ` if(I!=K || ea[1]!=0) VERIFY_IS_APPROX(ea, eabis); \` 31 ` }` 27 ` }` 32 ` VERIFY_EULER(0,1,2, X,Y,Z);` 28 ` VERIFY_EULER(0,1,2, X,Y,Z);` 33 ` VERIFY_EULER(0,1,0, X,Y,X);` 29 ` VERIFY_EULER(0,1,0, X,Y,X);` 34 ` VERIFY_EULER(0,2,1, X,Z,Y);` 30 ` VERIFY_EULER(0,2,1, X,Z,Y);` 35 ` VERIFY_EULER(0,2,0, X,Z,X);` 31 ` VERIFY_EULER(0,2,0, X,Z,X);` 36 32 37 ` VERIFY_EULER(1,2,0, Y,Z,X);` 33 ` VERIFY_EULER(1,2,0, Y,Z,X);` 38 ` VERIFY_EULER(1,2,1, Y,Z,Y);` 34 ` VERIFY_EULER(1,2,1, Y,Z,Y);` 40 ` VERIFY_EULER(1,0,1, Y,X,Y);` 36 ` VERIFY_EULER(1,0,1, Y,X,Y);` 41 37 42 ` VERIFY_EULER(2,0,1, Z,X,Y);` 38 ` VERIFY_EULER(2,0,1, Z,X,Y);` 43 ` VERIFY_EULER(2,0,2, Z,X,Z);` 39 ` VERIFY_EULER(2,0,2, Z,X,Z);` 44 ` VERIFY_EULER(2,1,0, Z,Y,X);` 40 ` VERIFY_EULER(2,1,0, Z,Y,X);` 45 ` VERIFY_EULER(2,1,2, Z,Y,Z);` 41 ` VERIFY_EULER(2,1,2, Z,Y,Z);` 46 `}` 42 `}` 47 43 44 `template void eulerangles(void)` 45 `{` 46 ` typedef Matrix Matrix3;` 47 ` typedef Matrix Vector3;` 48 ` typedef Array Array3;` 49 ` typedef Quaternion Quaternionx;` 50 ` typedef AngleAxis AngleAxisx;` 51 52 ` Scalar a = internal::random(-Scalar(M_PI), Scalar(M_PI));` 53 ` Quaternionx q1;` 54 ` q1 = AngleAxisx(a, Vector3::Random().normalized());` 55 ` Matrix3 m;` 56 ` m = q1;` 57 ` ` 58 ` Vector3 ea = m.eulerAngles(0,1,2);` 59 ` check_all_var(ea);` 60 ` ea = m.eulerAngles(0,1,0);` 61 ` check_all_var(ea);` 62 ` ` 63 ` ea = (Array3::Random() + Array3(1,1,0))*M_PI*Array3(0.5,0.5,1);` 64 ` check_all_var(ea);` 65 ` ` 66 ` ea[2] = ea[0] = internal::random(0,M_PI);` 67 ` check_all_var(ea);` 68 ` ` 69 ` ea[0] = ea[1] = internal::random(0,M_PI);` 70 ` check_all_var(ea);` 71 ` ` 72 ` ea[1] = 0;` 73 ` check_all_var(ea);` 74 ` ` 75 ` ea.head(2).setZero();` 76 ` check_all_var(ea);` 77 ` ` 78 ` ea.setZero();` 79 ` check_all_var(ea);` 80 `}` 81 48 `void test_geo_eulerangles()` 82 `void test_geo_eulerangles()` 49 `{` 83 `{` 50 ` for(int i = 0; i < g_repeat; i++) {` 84 ` for(int i = 0; i < g_repeat; i++) {` 51 ` CALL_SUBTEST_1( eulerangles() );` 85 ` CALL_SUBTEST_1( eulerangles() );` 52 ` CALL_SUBTEST_2( eulerangles() );` 86 ` CALL_SUBTEST_2( eulerangles() );` 53 ` }` 87 ` }` 54 `}` 88 `}`