Debian Codegraph @

src/ImathTest/testExtractEuler.cpp

raw
1 //
2 // SPDX-License-Identifier: BSD-3-Clause
3 // Copyright Contributors to the OpenEXR Project.
4 //
5
6 #ifdef NDEBUG
7 # undef NDEBUG
8 #endif
9
10 #include <ImathEuler.h>
11 #include <ImathFun.h>
12 #include <ImathMatrixAlgo.h>
13 #include <ImathRandom.h>
14 #include <assert.h>
15 #include <iostream>
16 #include "testExtractEuler.h"
17
18 using namespace std;
19 using namespace IMATH_INTERNAL_NAMESPACE;
20
21 namespace
22 {
23
24 float
25 rad (float deg)
26 {
27 return deg * float(M_PI / 180);
28 }
29
30 M44f
31 matrixEulerMatrix_1 (const M44f& M, Eulerf::Order order)
32 {
33 V3f f;
34
35 if (order == Eulerf::XYZ)
36 extractEulerXYZ (M, f);
37 else
38 extractEulerZYX (M, f);
39
40 return Eulerf (f, order).toMatrix44();
41 }
42
43 M44f
44 matrixEulerMatrix_2 (const M44f& M, Eulerf::Order order)
45 {
46 Eulerf f (order);
47 f.extract (M);
48 return f.toMatrix44();
49 }
50
51 void
52 testMatrix (const M44f M,
53 M44f (*matrixEulerMatrix) (const M44f&, Eulerf::Order),
54 Eulerf::Order order)
55 {
56 //
57 // Extract Euler angles from M, and convert the
58 // Euler angles back to a matrix, N.
59 //
60
61 M44f N = matrixEulerMatrix (M, order);
62
63 //
64 // Verify that the entries in M and N do not
65 // differ too much.
66 //
67
68 M44f D (M - N);
69
70 for (int j = 0; j < 3; ++j)
71 {
72 for (int k = 0; k < 3; ++k)
73 {
74 if (abs (D[j][k]) > 0.000002)
75 {
76 cout << "unexpectedly large matrix to "
77 "euler angles conversion error: "
78 << D[j][k] << endl;
79
80 cout << j << " " << k << endl;
81
82 cout << "M\n" << M << endl;
83 cout << "N\n" << N << endl;
84 cout << "D\n" << D << endl;
85
86 assert (false);
87 }
88 }
89 }
90 }
91
92 void
93 testRandomAngles (M44f (*matrixEulerMatrix) (const M44f&, Eulerf::Order), Eulerf::Order order)
94 {
95 Rand48 r (0);
96
97 for (int i = 0; i < 100000; ++i)
98 {
99 //
100 // Create a rotation matrix, M
101 //
102
103 Eulerf e (
104 rad (float(r.nextf (-180, 180))),
105 rad (float(r.nextf (-180, 180))),
106 rad (float(r.nextf (-180, 180))),
107 Eulerf::XYZ);
108
109 M44f M (e.toMatrix44());
110
111 //
112 // Add a small random error to the elements of M
113 //
114
115 for (int j = 0; j < 3; ++j)
116 for (int k = 0; k < 3; ++k)
117 M[j][k] += float(r.nextf (-1e-7, 1e-7));
118
119 //
120 // Extract Euler angles from M, convert the Euler angles
121 // back to a matrix, N, and verify that the entries in M
122 // and N do not differ too much.
123 //
124
125 testMatrix (M, matrixEulerMatrix, order);
126 }
127 }
128
129 void
130 testAngles (V3f angles, M44f (*matrixEulerMatrix) (const M44f&, Eulerf::Order), Eulerf::Order order)
131 {
132 Eulerf e (rad (angles.x), rad (angles.y), rad (angles.z), order);
133
134 M44f M (e.toMatrix44());
135
136 //
137 // With rounding errors from e.toMatrix.
138 //
139
140 testMatrix (M, matrixEulerMatrix, order);
141
142 //
143 // Without rounding errors (assuming that
144 // all angles are multiples of 90 degrees).
145 //
146
147 for (int i = 0; i < 3; ++i)
148 for (int j = 0; j < 3; ++j)
149 if (M[i][j] < -0.5)
150 M[i][j] = -1;
151 else if (M[i][j] > 0.5)
152 M[i][j] = 1;
153 else
154 M[i][j] = 0;
155
156 testMatrix (M, matrixEulerMatrix, order);
157 }
158
159 void
160 test (M44f (*matrixEulerMatrix) (const M44f&, Eulerf::Order), Eulerf::Order order)
161 {
162 cout << "order = " << setbase (16) << int (order) << setbase (10) << endl;
163
164 // cout << "random angles" << endl;
165
166 testRandomAngles (matrixEulerMatrix, order);
167
168 // cout << "special angles" << endl;
169
170 for (int i = 0; i < 360; i += 90)
171 for (int j = 0; j < 360; j += 90)
172 for (int k = 0; k < 360; k += 90)
173 testAngles (V3f (float(i), float(j), float(k)), matrixEulerMatrix, order);
174 }
175
176 void
177 testRandomAngles33()
178 {
179 Rand48 r (0);
180
181 float eps = 8.0f * std::numeric_limits<float>::epsilon ();
182
183 for (int i = 0; i < 100000; ++i)
184 {
185 float angle = rad (float(r.nextf (-180, 180)));
186
187 M33f M;
188 M.setRotation (angle);
189
190 float angleEx;
191 extractEuler (M, angleEx);
192
193 assert (IMATH_INTERNAL_NAMESPACE::equal (angle, angleEx, eps));
194 }
195 }
196
197 } // namespace
198
199 void
200 testExtractEuler()
201 {
202 cout << "Testing extraction of rotation angle from 3x3 matrices" << endl;
203 testRandomAngles33();
204
205 cout << "Testing extraction of Euler angles from matrices" << endl;
206
207 cout << "extractEulerXYZ()" << endl;
208 test (matrixEulerMatrix_1, Eulerf::XYZ);
209
210 cout << "extractEulerZYX()" << endl;
211 test (matrixEulerMatrix_1, Eulerf::ZYX);
212
213 cout << "Eulerf::extract()" << endl;
214 test (matrixEulerMatrix_2, Eulerf::XYZ);
215 test (matrixEulerMatrix_2, Eulerf::XZY);
216 test (matrixEulerMatrix_2, Eulerf::YZX);
217 test (matrixEulerMatrix_2, Eulerf::YXZ);
218 test (matrixEulerMatrix_2, Eulerf::ZXY);
219 test (matrixEulerMatrix_2, Eulerf::ZYX);
220
221 test (matrixEulerMatrix_2, Eulerf::XZX);
222 test (matrixEulerMatrix_2, Eulerf::XYX);
223 test (matrixEulerMatrix_2, Eulerf::YXY);
224 test (matrixEulerMatrix_2, Eulerf::YZY);
225 test (matrixEulerMatrix_2, Eulerf::ZYZ);
226 test (matrixEulerMatrix_2, Eulerf::ZXZ);
227
228 test (matrixEulerMatrix_2, Eulerf::XYZr);
229 test (matrixEulerMatrix_2, Eulerf::XZYr);
230 test (matrixEulerMatrix_2, Eulerf::YZXr);
231 test (matrixEulerMatrix_2, Eulerf::YXZr);
232 test (matrixEulerMatrix_2, Eulerf::ZXYr);
233 test (matrixEulerMatrix_2, Eulerf::ZYXr);
234
235 test (matrixEulerMatrix_2, Eulerf::XZXr);
236 test (matrixEulerMatrix_2, Eulerf::XYXr);
237 test (matrixEulerMatrix_2, Eulerf::YXYr);
238 test (matrixEulerMatrix_2, Eulerf::YZYr);
239 test (matrixEulerMatrix_2, Eulerf::ZYZr);
240 test (matrixEulerMatrix_2, Eulerf::ZXZr);
241
242 cout << "ok\n" << endl;
243 }