This bugzilla service is closed. All entries have been migrated to https://gitlab.com/libeigen/eigen
View | Details | Raw Unified | Return to bug 609
Collapse All | Expand All

(-)a/Eigen/src/Geometry/EulerAngles.h (-19 / +39 lines)
Lines 22-85 namespace Eigen { Link Here
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<typename Derived>
35
template<typename Derived>
32
inline Matrix<typename MatrixBase<Derived>::Scalar,3,1>
36
inline Matrix<typename MatrixBase<Derived>::Scalar,3,1>
33
MatrixBase<Derived>::eulerAngles(Index a0, Index a1, Index a2) const
37
MatrixBase<Derived>::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<Scalar,3,1> res;
45
  Matrix<Scalar,3,1> res;
40
  typedef Matrix<typename Derived::Scalar,2,1> Vector2;
46
  typedef Matrix<typename Derived::Scalar,2,1> Vector2;
41
  const Scalar epsilon = NumTraits<Scalar>::dummy_precision();
47
  const Scalar epsilon = NumTraits<Scalar>::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
(-)a/test/geo_eulerangles.cpp (-12 / +46 lines)
Lines 1-38 Link Here
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 <gael.guennebaud@inria.fr>
4
// Copyright (C) 2008-2012 Gael Guennebaud <gael.guennebaud@inria.fr>
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 <Eigen/Geometry>
11
#include <Eigen/Geometry>
12
#include <Eigen/LU>
12
#include <Eigen/LU>
13
#include <Eigen/SVD>
13
#include <Eigen/SVD>
14
14
15
template<typename Scalar> void eulerangles(void)
15
template<typename Scalar> void check_all_var(const Matrix<Scalar,3,1>& ea)
16
{
16
{
17
  typedef Matrix<Scalar,3,3> Matrix3;
17
  typedef Matrix<Scalar,3,3> Matrix3;
18
  typedef Matrix<Scalar,3,1> Vector3;
18
  typedef Matrix<Scalar,3,1> Vector3;
19
  typedef Quaternion<Scalar> Quaternionx;
20
  typedef AngleAxis<Scalar> AngleAxisx;
19
  typedef AngleAxis<Scalar> AngleAxisx;
21
20
  
22
  Scalar a = internal::random<Scalar>(-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);
Lines 40-54 template<typename Scalar> void eulerangl Link Here
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<typename Scalar> void eulerangles(void)
45
{
46
  typedef Matrix<Scalar,3,3> Matrix3;
47
  typedef Matrix<Scalar,3,1> Vector3;
48
  typedef Array<Scalar,3,1> Array3;
49
  typedef Quaternion<Scalar> Quaternionx;
50
  typedef AngleAxis<Scalar> AngleAxisx;
51
52
  Scalar a = internal::random<Scalar>(-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<Scalar>(0,M_PI);
67
  check_all_var(ea);
68
  
69
  ea[0] = ea[1] = internal::random<Scalar>(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<float>() );
85
    CALL_SUBTEST_1( eulerangles<float>() );
52
    CALL_SUBTEST_2( eulerangles<double>() );
86
    CALL_SUBTEST_2( eulerangles<double>() );
53
  }
87
  }
54
}
88
}

Return to bug 609