1// This file is part of Eigen, a lightweight C++ template library
2// for linear algebra.
3//
4// Copyright (C) 2008-2012 Gael Guennebaud <gael.guennebaud@inria.fr>
5//
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
8// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
9
10#include "main.h"
11#include <Eigen/Geometry>
12#include <Eigen/LU>
13#include <Eigen/SVD>
14
15
16template<typename Scalar>
17void verify_euler(const Matrix<Scalar,3,1>& ea, int i, int j, int k)
18{
19  typedef Matrix<Scalar,3,3> Matrix3;
20  typedef Matrix<Scalar,3,1> Vector3;
21  typedef AngleAxis<Scalar> AngleAxisx;
22  using std::abs;
23  Matrix3 m(AngleAxisx(ea[0], Vector3::Unit(i)) * AngleAxisx(ea[1], Vector3::Unit(j)) * AngleAxisx(ea[2], Vector3::Unit(k)));
24  Vector3 eabis = m.eulerAngles(i, j, k);
25  Matrix3 mbis(AngleAxisx(eabis[0], Vector3::Unit(i)) * AngleAxisx(eabis[1], Vector3::Unit(j)) * AngleAxisx(eabis[2], Vector3::Unit(k)));
26  VERIFY_IS_APPROX(m,  mbis);
27  /* If I==K, and ea[1]==0, then there no unique solution. */
28  /* The remark apply in the case where I!=K, and |ea[1]| is close to pi/2. */
29  if( (i!=k || ea[1]!=0) && (i==k || !internal::isApprox(abs(ea[1]),Scalar(M_PI/2),test_precision<Scalar>())) )
30    VERIFY((ea-eabis).norm() <= test_precision<Scalar>());
31
32  // approx_or_less_than does not work for 0
33  VERIFY(0 < eabis[0] || test_isMuchSmallerThan(eabis[0], Scalar(1)));
34  VERIFY_IS_APPROX_OR_LESS_THAN(eabis[0], Scalar(M_PI));
35  VERIFY_IS_APPROX_OR_LESS_THAN(-Scalar(M_PI), eabis[1]);
36  VERIFY_IS_APPROX_OR_LESS_THAN(eabis[1], Scalar(M_PI));
37  VERIFY_IS_APPROX_OR_LESS_THAN(-Scalar(M_PI), eabis[2]);
38  VERIFY_IS_APPROX_OR_LESS_THAN(eabis[2], Scalar(M_PI));
39}
40
41template<typename Scalar> void check_all_var(const Matrix<Scalar,3,1>& ea)
42{
43  verify_euler(ea, 0,1,2);
44  verify_euler(ea, 0,1,0);
45  verify_euler(ea, 0,2,1);
46  verify_euler(ea, 0,2,0);
47
48  verify_euler(ea, 1,2,0);
49  verify_euler(ea, 1,2,1);
50  verify_euler(ea, 1,0,2);
51  verify_euler(ea, 1,0,1);
52
53  verify_euler(ea, 2,0,1);
54  verify_euler(ea, 2,0,2);
55  verify_euler(ea, 2,1,0);
56  verify_euler(ea, 2,1,2);
57}
58
59template<typename Scalar> void eulerangles()
60{
61  typedef Matrix<Scalar,3,3> Matrix3;
62  typedef Matrix<Scalar,3,1> Vector3;
63  typedef Array<Scalar,3,1> Array3;
64  typedef Quaternion<Scalar> Quaternionx;
65  typedef AngleAxis<Scalar> AngleAxisx;
66
67  Scalar a = internal::random<Scalar>(-Scalar(M_PI), Scalar(M_PI));
68  Quaternionx q1;
69  q1 = AngleAxisx(a, Vector3::Random().normalized());
70  Matrix3 m;
71  m = q1;
72
73  Vector3 ea = m.eulerAngles(0,1,2);
74  check_all_var(ea);
75  ea = m.eulerAngles(0,1,0);
76  check_all_var(ea);
77
78  // Check with purely random Quaternion:
79  q1.coeffs() = Quaternionx::Coefficients::Random().normalized();
80  m = q1;
81  ea = m.eulerAngles(0,1,2);
82  check_all_var(ea);
83  ea = m.eulerAngles(0,1,0);
84  check_all_var(ea);
85
86  // Check with random angles in range [0:pi]x[-pi:pi]x[-pi:pi].
87  ea = (Array3::Random() + Array3(1,0,0))*Scalar(M_PI)*Array3(0.5,1,1);
88  check_all_var(ea);
89
90  ea[2] = ea[0] = internal::random<Scalar>(0,Scalar(M_PI));
91  check_all_var(ea);
92
93  ea[0] = ea[1] = internal::random<Scalar>(0,Scalar(M_PI));
94  check_all_var(ea);
95
96  ea[1] = 0;
97  check_all_var(ea);
98
99  ea.head(2).setZero();
100  check_all_var(ea);
101
102  ea.setZero();
103  check_all_var(ea);
104}
105
106void test_geo_eulerangles()
107{
108  for(int i = 0; i < g_repeat; i++) {
109    CALL_SUBTEST_1( eulerangles<float>() );
110    CALL_SUBTEST_2( eulerangles<double>() );
111  }
112}
113