1 // This file is part of Eigen, a lightweight C++ template library
2 // for linear algebra.
3 //
4 // Copyright (C) 2015 Tal Hadad <tal_hd@hotmail.com>
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
12 #include <unsupported/Eigen/EulerAngles>
13
14 using namespace Eigen;
15
16 template<typename EulerSystem, typename Scalar>
verify_euler_ranged(const Matrix<Scalar,3,1> & ea,bool positiveRangeAlpha,bool positiveRangeBeta,bool positiveRangeGamma)17 void verify_euler_ranged(const Matrix<Scalar,3,1>& ea,
18 bool positiveRangeAlpha, bool positiveRangeBeta, bool positiveRangeGamma)
19 {
20 typedef EulerAngles<Scalar, EulerSystem> EulerAnglesType;
21 typedef Matrix<Scalar,3,3> Matrix3;
22 typedef Matrix<Scalar,3,1> Vector3;
23 typedef Quaternion<Scalar> QuaternionType;
24 typedef AngleAxis<Scalar> AngleAxisType;
25 using std::abs;
26
27 Scalar alphaRangeStart, alphaRangeEnd;
28 Scalar betaRangeStart, betaRangeEnd;
29 Scalar gammaRangeStart, gammaRangeEnd;
30
31 if (positiveRangeAlpha)
32 {
33 alphaRangeStart = Scalar(0);
34 alphaRangeEnd = Scalar(2 * EIGEN_PI);
35 }
36 else
37 {
38 alphaRangeStart = -Scalar(EIGEN_PI);
39 alphaRangeEnd = Scalar(EIGEN_PI);
40 }
41
42 if (positiveRangeBeta)
43 {
44 betaRangeStart = Scalar(0);
45 betaRangeEnd = Scalar(2 * EIGEN_PI);
46 }
47 else
48 {
49 betaRangeStart = -Scalar(EIGEN_PI);
50 betaRangeEnd = Scalar(EIGEN_PI);
51 }
52
53 if (positiveRangeGamma)
54 {
55 gammaRangeStart = Scalar(0);
56 gammaRangeEnd = Scalar(2 * EIGEN_PI);
57 }
58 else
59 {
60 gammaRangeStart = -Scalar(EIGEN_PI);
61 gammaRangeEnd = Scalar(EIGEN_PI);
62 }
63
64 const int i = EulerSystem::AlphaAxisAbs - 1;
65 const int j = EulerSystem::BetaAxisAbs - 1;
66 const int k = EulerSystem::GammaAxisAbs - 1;
67
68 const int iFactor = EulerSystem::IsAlphaOpposite ? -1 : 1;
69 const int jFactor = EulerSystem::IsBetaOpposite ? -1 : 1;
70 const int kFactor = EulerSystem::IsGammaOpposite ? -1 : 1;
71
72 const Vector3 I = EulerAnglesType::AlphaAxisVector();
73 const Vector3 J = EulerAnglesType::BetaAxisVector();
74 const Vector3 K = EulerAnglesType::GammaAxisVector();
75
76 EulerAnglesType e(ea[0], ea[1], ea[2]);
77
78 Matrix3 m(e);
79 Vector3 eabis = EulerAnglesType(m, positiveRangeAlpha, positiveRangeBeta, positiveRangeGamma).angles();
80
81 // Check that eabis in range
82 VERIFY(alphaRangeStart <= eabis[0] && eabis[0] <= alphaRangeEnd);
83 VERIFY(betaRangeStart <= eabis[1] && eabis[1] <= betaRangeEnd);
84 VERIFY(gammaRangeStart <= eabis[2] && eabis[2] <= gammaRangeEnd);
85
86 Vector3 eabis2 = m.eulerAngles(i, j, k);
87
88 // Invert the relevant axes
89 eabis2[0] *= iFactor;
90 eabis2[1] *= jFactor;
91 eabis2[2] *= kFactor;
92
93 // Saturate the angles to the correct range
94 if (positiveRangeAlpha && (eabis2[0] < 0))
95 eabis2[0] += Scalar(2 * EIGEN_PI);
96 if (positiveRangeBeta && (eabis2[1] < 0))
97 eabis2[1] += Scalar(2 * EIGEN_PI);
98 if (positiveRangeGamma && (eabis2[2] < 0))
99 eabis2[2] += Scalar(2 * EIGEN_PI);
100
101 VERIFY_IS_APPROX(eabis, eabis2);// Verify that our estimation is the same as m.eulerAngles() is
102
103 Matrix3 mbis(AngleAxisType(eabis[0], I) * AngleAxisType(eabis[1], J) * AngleAxisType(eabis[2], K));
104 VERIFY_IS_APPROX(m, mbis);
105
106 // Tests that are only relevant for no possitive range
107 if (!(positiveRangeAlpha || positiveRangeBeta || positiveRangeGamma))
108 {
109 /* If I==K, and ea[1]==0, then there no unique solution. */
110 /* The remark apply in the case where I!=K, and |ea[1]| is close to pi/2. */
111 if( (i!=k || ea[1]!=0) && (i==k || !internal::isApprox(abs(ea[1]),Scalar(EIGEN_PI/2),test_precision<Scalar>())) )
112 VERIFY((ea-eabis).norm() <= test_precision<Scalar>());
113
114 // approx_or_less_than does not work for 0
115 VERIFY(0 < eabis[0] || test_isMuchSmallerThan(eabis[0], Scalar(1)));
116 }
117
118 // Quaternions
119 QuaternionType q(e);
120 eabis = EulerAnglesType(q, positiveRangeAlpha, positiveRangeBeta, positiveRangeGamma).angles();
121 VERIFY_IS_APPROX(eabis, eabis2);// Verify that the euler angles are still the same
122 }
123
124 template<typename EulerSystem, typename Scalar>
verify_euler(const Matrix<Scalar,3,1> & ea)125 void verify_euler(const Matrix<Scalar,3,1>& ea)
126 {
127 verify_euler_ranged<EulerSystem>(ea, false, false, false);
128 verify_euler_ranged<EulerSystem>(ea, false, false, true);
129 verify_euler_ranged<EulerSystem>(ea, false, true, false);
130 verify_euler_ranged<EulerSystem>(ea, false, true, true);
131 verify_euler_ranged<EulerSystem>(ea, true, false, false);
132 verify_euler_ranged<EulerSystem>(ea, true, false, true);
133 verify_euler_ranged<EulerSystem>(ea, true, true, false);
134 verify_euler_ranged<EulerSystem>(ea, true, true, true);
135 }
136
check_all_var(const Matrix<Scalar,3,1> & ea)137 template<typename Scalar> void check_all_var(const Matrix<Scalar,3,1>& ea)
138 {
139 verify_euler<EulerSystemXYZ>(ea);
140 verify_euler<EulerSystemXYX>(ea);
141 verify_euler<EulerSystemXZY>(ea);
142 verify_euler<EulerSystemXZX>(ea);
143
144 verify_euler<EulerSystemYZX>(ea);
145 verify_euler<EulerSystemYZY>(ea);
146 verify_euler<EulerSystemYXZ>(ea);
147 verify_euler<EulerSystemYXY>(ea);
148
149 verify_euler<EulerSystemZXY>(ea);
150 verify_euler<EulerSystemZXZ>(ea);
151 verify_euler<EulerSystemZYX>(ea);
152 verify_euler<EulerSystemZYZ>(ea);
153 }
154
eulerangles()155 template<typename Scalar> void eulerangles()
156 {
157 typedef Matrix<Scalar,3,3> Matrix3;
158 typedef Matrix<Scalar,3,1> Vector3;
159 typedef Array<Scalar,3,1> Array3;
160 typedef Quaternion<Scalar> Quaternionx;
161 typedef AngleAxis<Scalar> AngleAxisType;
162
163 Scalar a = internal::random<Scalar>(-Scalar(EIGEN_PI), Scalar(EIGEN_PI));
164 Quaternionx q1;
165 q1 = AngleAxisType(a, Vector3::Random().normalized());
166 Matrix3 m;
167 m = q1;
168
169 Vector3 ea = m.eulerAngles(0,1,2);
170 check_all_var(ea);
171 ea = m.eulerAngles(0,1,0);
172 check_all_var(ea);
173
174 // Check with purely random Quaternion:
175 q1.coeffs() = Quaternionx::Coefficients::Random().normalized();
176 m = q1;
177 ea = m.eulerAngles(0,1,2);
178 check_all_var(ea);
179 ea = m.eulerAngles(0,1,0);
180 check_all_var(ea);
181
182 // Check with random angles in range [0:pi]x[-pi:pi]x[-pi:pi].
183 ea = (Array3::Random() + Array3(1,0,0))*Scalar(EIGEN_PI)*Array3(0.5,1,1);
184 check_all_var(ea);
185
186 ea[2] = ea[0] = internal::random<Scalar>(0,Scalar(EIGEN_PI));
187 check_all_var(ea);
188
189 ea[0] = ea[1] = internal::random<Scalar>(0,Scalar(EIGEN_PI));
190 check_all_var(ea);
191
192 ea[1] = 0;
193 check_all_var(ea);
194
195 ea.head(2).setZero();
196 check_all_var(ea);
197
198 ea.setZero();
199 check_all_var(ea);
200 }
201
test_EulerAngles()202 void test_EulerAngles()
203 {
204 for(int i = 0; i < g_repeat; i++) {
205 CALL_SUBTEST_1( eulerangles<float>() );
206 CALL_SUBTEST_2( eulerangles<double>() );
207 }
208 }
209