1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17 package org.apache.commons.geometry.euclidean.threed.rotation;
18
19 import java.util.List;
20 import java.util.function.DoubleFunction;
21 import java.util.function.UnaryOperator;
22 import java.util.stream.Collectors;
23 import java.util.stream.Stream;
24
25 import org.apache.commons.geometry.core.GeometryTestUtils;
26 import org.apache.commons.geometry.core.internal.SimpleTupleFormat;
27 import org.apache.commons.geometry.euclidean.EuclideanTestUtils;
28 import org.apache.commons.geometry.euclidean.threed.AffineTransformMatrix3D;
29 import org.apache.commons.geometry.euclidean.threed.Vector3D;
30 import org.apache.commons.numbers.angle.Angle;
31 import org.apache.commons.numbers.core.Precision;
32 import org.apache.commons.numbers.quaternion.Quaternion;
33 import org.apache.commons.rng.UniformRandomProvider;
34 import org.apache.commons.rng.simple.RandomSource;
35 import org.junit.jupiter.api.Assertions;
36 import org.junit.jupiter.api.Test;
37
38 class QuaternionRotationTest {
39
40 private static final double EPS = 1e-12;
41
42
43 private static final Vector3D PLUS_X_DIR = Vector3D.of(2, 0, 0);
44 private static final Vector3D MINUS_X_DIR = Vector3D.of(-2, 0, 0);
45
46 private static final Vector3D PLUS_Y_DIR = Vector3D.of(0, 3, 0);
47 private static final Vector3D MINUS_Y_DIR = Vector3D.of(0, -3, 0);
48
49 private static final Vector3D PLUS_Z_DIR = Vector3D.of(0, 0, 4);
50 private static final Vector3D MINUS_Z_DIR = Vector3D.of(0, 0, -4);
51
52 private static final Vector3D PLUS_DIAGONAL = Vector3D.of(1, 1, 1);
53 private static final Vector3D MINUS_DIAGONAL = Vector3D.of(-1, -1, -1);
54
55 private static final double TWO_THIRDS_PI = 2.0 * Math.PI / 3.0;
56 private static final double MINUS_TWO_THIRDS_PI = -TWO_THIRDS_PI;
57
58 @Test
59 void testOf_quaternion() {
60
61 checkQuaternion(QuaternionRotation.of(Quaternion.of(1, 0, 0, 0)), 1, 0, 0, 0);
62 checkQuaternion(QuaternionRotation.of(Quaternion.of(-1, 0, 0, 0)), 1, 0, 0, 0);
63 checkQuaternion(QuaternionRotation.of(Quaternion.of(0, 1, 0, 0)), 0, 1, 0, 0);
64 checkQuaternion(QuaternionRotation.of(Quaternion.of(0, 0, 1, 0)), 0, 0, 1, 0);
65 checkQuaternion(QuaternionRotation.of(Quaternion.of(0, 0, 0, 1)), 0, 0, 0, 1);
66
67 checkQuaternion(QuaternionRotation.of(Quaternion.of(1, 1, 1, 1)), 0.5, 0.5, 0.5, 0.5);
68 checkQuaternion(QuaternionRotation.of(Quaternion.of(-1, -1, -1, -1)), 0.5, 0.5, 0.5, 0.5);
69 }
70
71 @Test
72 void testOf_quaternion_illegalNorm() {
73
74 Assertions.assertThrows(IllegalStateException.class, () -> QuaternionRotation.of(Quaternion.of(0, 0, 0, 0)));
75 Assertions.assertThrows(IllegalStateException.class, () -> QuaternionRotation.of(Quaternion.of(1, 1, 1, Double.NaN)));
76 Assertions.assertThrows(IllegalStateException.class, () -> QuaternionRotation.of(Quaternion.of(1, 1, Double.POSITIVE_INFINITY, 1)));
77 Assertions.assertThrows(IllegalStateException.class, () -> QuaternionRotation.of(Quaternion.of(1, Double.NEGATIVE_INFINITY, 1, 1)));
78 Assertions.assertThrows(IllegalStateException.class, () -> QuaternionRotation.of(Quaternion.of(Double.NaN, 1, 1, 1)));
79 }
80
81 @Test
82 void testOf_components() {
83
84 checkQuaternion(QuaternionRotation.of(1, 0, 0, 0), 1, 0, 0, 0);
85 checkQuaternion(QuaternionRotation.of(-1, 0, 0, 0), 1, 0, 0, 0);
86 checkQuaternion(QuaternionRotation.of(0, 1, 0, 0), 0, 1, 0, 0);
87 checkQuaternion(QuaternionRotation.of(0, 0, 1, 0), 0, 0, 1, 0);
88 checkQuaternion(QuaternionRotation.of(0, 0, 0, 1), 0, 0, 0, 1);
89
90 checkQuaternion(QuaternionRotation.of(1, 1, 1, 1), 0.5, 0.5, 0.5, 0.5);
91 checkQuaternion(QuaternionRotation.of(-1, -1, -1, -1), 0.5, 0.5, 0.5, 0.5);
92 }
93
94 @Test
95 void testOf_components_illegalNorm() {
96
97 Assertions.assertThrows(IllegalStateException.class, () -> QuaternionRotation.of(0, 0, 0, 0));
98 Assertions.assertThrows(IllegalStateException.class, () -> QuaternionRotation.of(1, 1, 1, Double.NaN));
99 Assertions.assertThrows(IllegalStateException.class, () -> QuaternionRotation.of(1, 1, Double.POSITIVE_INFINITY, 1));
100 Assertions.assertThrows(IllegalStateException.class, () -> QuaternionRotation.of(1, Double.NEGATIVE_INFINITY, 1, 1));
101 Assertions.assertThrows(IllegalStateException.class, () -> QuaternionRotation.of(Double.NaN, 1, 1, 1));
102 }
103
104 @Test
105 void testIdentity() {
106
107 final QuaternionRotation q = QuaternionRotation.identity();
108
109
110 assertRotationEquals(StandardRotations.IDENTITY, q);
111 }
112
113 @Test
114 void testIdentity_axis() {
115
116 final QuaternionRotation q = QuaternionRotation.identity();
117
118
119 EuclideanTestUtils.assertCoordinatesEqual(Vector3D.Unit.PLUS_X, q.getAxis(), EPS);
120 }
121
122 @Test
123 void testGetAxis() {
124
125 checkVector(QuaternionRotation.of(0, 1, 0, 0).getAxis(), 1, 0, 0);
126 checkVector(QuaternionRotation.of(0, -1, 0, 0).getAxis(), -1, 0, 0);
127
128 checkVector(QuaternionRotation.of(0, 0, 1, 0).getAxis(), 0, 1, 0);
129 checkVector(QuaternionRotation.of(0, 0, -1, 0).getAxis(), 0, -1, 0);
130
131 checkVector(QuaternionRotation.of(0, 0, 0, 1).getAxis(), 0, 0, 1);
132 checkVector(QuaternionRotation.of(0, 0, 0, -1).getAxis(), 0, 0, -1);
133 }
134
135 @Test
136 void testGetAxis_noAxis() {
137
138 final QuaternionRotation rot = QuaternionRotation.of(1, 0, 0, 0);
139
140
141 EuclideanTestUtils.assertCoordinatesEqual(Vector3D.Unit.PLUS_X, rot.getAxis(), EPS);
142 }
143
144 @Test
145 void testGetAxis_matchesAxisAngleConstruction() {
146 EuclideanTestUtils.permuteSkipZero(-5, 5, 1, (x, y, z) -> {
147
148 final Vector3D vec = Vector3D.of(x, y, z);
149 final Vector3D norm = vec.normalize();
150
151
152
153
154 EuclideanTestUtils.assertCoordinatesEqual(norm,
155 QuaternionRotation.fromAxisAngle(vec, Angle.PI_OVER_TWO).getAxis(), EPS);
156
157
158 EuclideanTestUtils.assertCoordinatesEqual(norm,
159 QuaternionRotation.fromAxisAngle(vec.negate(), -Angle.PI_OVER_TWO).getAxis(), EPS);
160 });
161 }
162
163 @Test
164 void testGetAngle() {
165
166 Assertions.assertEquals(0.0, QuaternionRotation.of(1, 0, 0, 0).getAngle(), EPS);
167 Assertions.assertEquals(0.0, QuaternionRotation.of(-1, 0, 0, 0).getAngle(), EPS);
168
169 Assertions.assertEquals(Angle.PI_OVER_TWO, QuaternionRotation.of(1, 0, 0, 1).getAngle(), EPS);
170 Assertions.assertEquals(Angle.PI_OVER_TWO, QuaternionRotation.of(-1, 0, 0, -1).getAngle(), EPS);
171
172 Assertions.assertEquals(Math.PI * 2.0 / 3.0, QuaternionRotation.of(1, 1, 1, 1).getAngle(), EPS);
173
174 Assertions.assertEquals(Math.PI, QuaternionRotation.of(0, 0, 0, 1).getAngle(), EPS);
175 }
176
177 @Test
178 void testGetAngle_matchesAxisAngleConstruction() {
179 for (double theta = -2 * Math.PI; theta <= 2 * Math.PI; theta += 0.1) {
180
181 final QuaternionRotation rot = QuaternionRotation.fromAxisAngle(PLUS_DIAGONAL, theta);
182
183
184 final double angle = rot.getAngle();
185
186
187
188 Assertions.assertTrue(angle >= 0.0);
189 Assertions.assertTrue(angle <= Math.PI);
190
191 double expected = Angle.Rad.WITHIN_MINUS_PI_AND_PI.applyAsDouble(theta);
192 if (PLUS_DIAGONAL.dot(rot.getAxis()) < 0) {
193
194 expected *= -1;
195 }
196
197 Assertions.assertEquals(expected, angle, EPS);
198 }
199 }
200
201 @Test
202 void testFromAxisAngle_apply() {
203
204
205
206 assertRotationEquals(StandardRotations.IDENTITY, QuaternionRotation.fromAxisAngle(PLUS_X_DIR, 0.0));
207
208 assertRotationEquals(StandardRotations.PLUS_X_HALF_PI, QuaternionRotation.fromAxisAngle(PLUS_X_DIR, Angle.PI_OVER_TWO));
209 assertRotationEquals(StandardRotations.PLUS_X_HALF_PI, QuaternionRotation.fromAxisAngle(MINUS_X_DIR, -Angle.PI_OVER_TWO));
210
211 assertRotationEquals(StandardRotations.MINUS_X_HALF_PI, QuaternionRotation.fromAxisAngle(MINUS_X_DIR, Angle.PI_OVER_TWO));
212 assertRotationEquals(StandardRotations.MINUS_X_HALF_PI, QuaternionRotation.fromAxisAngle(PLUS_X_DIR, -Angle.PI_OVER_TWO));
213
214 assertRotationEquals(StandardRotations.X_PI, QuaternionRotation.fromAxisAngle(PLUS_X_DIR, Math.PI));
215 assertRotationEquals(StandardRotations.X_PI, QuaternionRotation.fromAxisAngle(MINUS_X_DIR, Math.PI));
216
217
218 assertRotationEquals(StandardRotations.IDENTITY, QuaternionRotation.fromAxisAngle(PLUS_Y_DIR, 0.0));
219
220 assertRotationEquals(StandardRotations.PLUS_Y_HALF_PI, QuaternionRotation.fromAxisAngle(PLUS_Y_DIR, Angle.PI_OVER_TWO));
221 assertRotationEquals(StandardRotations.PLUS_Y_HALF_PI, QuaternionRotation.fromAxisAngle(MINUS_Y_DIR, -Angle.PI_OVER_TWO));
222
223 assertRotationEquals(StandardRotations.MINUS_Y_HALF_PI, QuaternionRotation.fromAxisAngle(MINUS_Y_DIR, Angle.PI_OVER_TWO));
224 assertRotationEquals(StandardRotations.MINUS_Y_HALF_PI, QuaternionRotation.fromAxisAngle(PLUS_Y_DIR, -Angle.PI_OVER_TWO));
225
226 assertRotationEquals(StandardRotations.Y_PI, QuaternionRotation.fromAxisAngle(PLUS_Y_DIR, Math.PI));
227 assertRotationEquals(StandardRotations.Y_PI, QuaternionRotation.fromAxisAngle(MINUS_Y_DIR, Math.PI));
228
229
230 assertRotationEquals(StandardRotations.IDENTITY, QuaternionRotation.fromAxisAngle(PLUS_Z_DIR, 0.0));
231
232 assertRotationEquals(StandardRotations.PLUS_Z_HALF_PI, QuaternionRotation.fromAxisAngle(PLUS_Z_DIR, Angle.PI_OVER_TWO));
233 assertRotationEquals(StandardRotations.PLUS_Z_HALF_PI, QuaternionRotation.fromAxisAngle(MINUS_Z_DIR, -Angle.PI_OVER_TWO));
234
235 assertRotationEquals(StandardRotations.MINUS_Z_HALF_PI, QuaternionRotation.fromAxisAngle(MINUS_Z_DIR, Angle.PI_OVER_TWO));
236 assertRotationEquals(StandardRotations.MINUS_Z_HALF_PI, QuaternionRotation.fromAxisAngle(PLUS_Z_DIR, -Angle.PI_OVER_TWO));
237
238 assertRotationEquals(StandardRotations.Z_PI, QuaternionRotation.fromAxisAngle(PLUS_Z_DIR, Math.PI));
239 assertRotationEquals(StandardRotations.Z_PI, QuaternionRotation.fromAxisAngle(MINUS_Z_DIR, Math.PI));
240
241
242 assertRotationEquals(StandardRotations.PLUS_DIAGONAL_TWO_THIRDS_PI, QuaternionRotation.fromAxisAngle(PLUS_DIAGONAL, TWO_THIRDS_PI));
243 assertRotationEquals(StandardRotations.PLUS_DIAGONAL_TWO_THIRDS_PI, QuaternionRotation.fromAxisAngle(MINUS_DIAGONAL, MINUS_TWO_THIRDS_PI));
244
245 assertRotationEquals(StandardRotations.MINUS_DIAGONAL_TWO_THIRDS_PI, QuaternionRotation.fromAxisAngle(MINUS_DIAGONAL, TWO_THIRDS_PI));
246 assertRotationEquals(StandardRotations.MINUS_DIAGONAL_TWO_THIRDS_PI, QuaternionRotation.fromAxisAngle(PLUS_DIAGONAL, MINUS_TWO_THIRDS_PI));
247 }
248
249 @Test
250 void testFromAxisAngle_invalidAxisNorm() {
251
252 Assertions.assertThrows(IllegalArgumentException.class, () -> QuaternionRotation.fromAxisAngle(Vector3D.ZERO, Angle.PI_OVER_TWO));
253 Assertions.assertThrows(IllegalArgumentException.class, () -> QuaternionRotation.fromAxisAngle(Vector3D.NaN, Angle.PI_OVER_TWO));
254 Assertions.assertThrows(IllegalArgumentException.class, () -> QuaternionRotation.fromAxisAngle(Vector3D.POSITIVE_INFINITY, Angle.PI_OVER_TWO));
255 Assertions.assertThrows(IllegalArgumentException.class, () -> QuaternionRotation.fromAxisAngle(Vector3D.NEGATIVE_INFINITY, Angle.PI_OVER_TWO));
256 }
257
258 @Test
259 void testFromAxisAngle_invalidAngle() {
260
261 GeometryTestUtils.assertThrowsWithMessage(() -> QuaternionRotation.fromAxisAngle(Vector3D.Unit.PLUS_X, Double.NaN),
262 IllegalArgumentException.class, "Invalid angle: NaN");
263 GeometryTestUtils.assertThrowsWithMessage(() -> QuaternionRotation.fromAxisAngle(Vector3D.Unit.PLUS_X, Double.POSITIVE_INFINITY),
264 IllegalArgumentException.class, "Invalid angle: Infinity");
265 GeometryTestUtils.assertThrowsWithMessage(() -> QuaternionRotation.fromAxisAngle(Vector3D.Unit.PLUS_X, Double.NEGATIVE_INFINITY),
266 IllegalArgumentException.class, "Invalid angle: -Infinity");
267 }
268
269 @Test
270 void testApplyVector() {
271
272 final QuaternionRotation q = QuaternionRotation.fromAxisAngle(Vector3D.of(1, 1, 1), Angle.PI_OVER_TWO);
273
274 EuclideanTestUtils.permute(-2, 2, 0.2, (x, y, z) -> {
275 final Vector3D input = Vector3D.of(x, y, z);
276
277
278 final Vector3D pt = q.apply(input);
279 final Vector3D vec = q.applyVector(input);
280
281 EuclideanTestUtils.assertCoordinatesEqual(pt, vec, EPS);
282 });
283 }
284
285 @Test
286 void testInverse() {
287
288 final QuaternionRotation rot = QuaternionRotation.of(0.5, 0.5, 0.5, 0.5);
289
290
291 final QuaternionRotation neg = rot.inverse();
292
293
294 Assertions.assertEquals(-0.5, neg.getQuaternion().getX(), EPS);
295 Assertions.assertEquals(-0.5, neg.getQuaternion().getY(), EPS);
296 Assertions.assertEquals(-0.5, neg.getQuaternion().getZ(), EPS);
297 Assertions.assertEquals(0.5, neg.getQuaternion().getW(), EPS);
298 }
299
300 @Test
301 void testInverse_apply() {
302
303
304
305 assertRotationEquals(StandardRotations.IDENTITY, QuaternionRotation.fromAxisAngle(PLUS_X_DIR, 0.0).inverse());
306
307 assertRotationEquals(StandardRotations.PLUS_X_HALF_PI, QuaternionRotation.fromAxisAngle(PLUS_X_DIR, -Angle.PI_OVER_TWO).inverse());
308 assertRotationEquals(StandardRotations.PLUS_X_HALF_PI, QuaternionRotation.fromAxisAngle(MINUS_X_DIR, Angle.PI_OVER_TWO).inverse());
309
310 assertRotationEquals(StandardRotations.MINUS_X_HALF_PI, QuaternionRotation.fromAxisAngle(MINUS_X_DIR, -Angle.PI_OVER_TWO).inverse());
311 assertRotationEquals(StandardRotations.MINUS_X_HALF_PI, QuaternionRotation.fromAxisAngle(PLUS_X_DIR, Angle.PI_OVER_TWO).inverse());
312
313 assertRotationEquals(StandardRotations.X_PI, QuaternionRotation.fromAxisAngle(PLUS_X_DIR, Math.PI).inverse());
314 assertRotationEquals(StandardRotations.X_PI, QuaternionRotation.fromAxisAngle(MINUS_X_DIR, Math.PI).inverse());
315
316
317 assertRotationEquals(StandardRotations.IDENTITY, QuaternionRotation.fromAxisAngle(PLUS_Y_DIR, 0.0).inverse());
318
319 assertRotationEquals(StandardRotations.PLUS_Y_HALF_PI, QuaternionRotation.fromAxisAngle(PLUS_Y_DIR, -Angle.PI_OVER_TWO).inverse());
320 assertRotationEquals(StandardRotations.PLUS_Y_HALF_PI, QuaternionRotation.fromAxisAngle(MINUS_Y_DIR, Angle.PI_OVER_TWO).inverse());
321
322 assertRotationEquals(StandardRotations.MINUS_Y_HALF_PI, QuaternionRotation.fromAxisAngle(MINUS_Y_DIR, -Angle.PI_OVER_TWO).inverse());
323 assertRotationEquals(StandardRotations.MINUS_Y_HALF_PI, QuaternionRotation.fromAxisAngle(PLUS_Y_DIR, Angle.PI_OVER_TWO).inverse());
324
325 assertRotationEquals(StandardRotations.Y_PI, QuaternionRotation.fromAxisAngle(PLUS_Y_DIR, Math.PI).inverse());
326 assertRotationEquals(StandardRotations.Y_PI, QuaternionRotation.fromAxisAngle(MINUS_Y_DIR, Math.PI).inverse());
327
328
329 assertRotationEquals(StandardRotations.IDENTITY, QuaternionRotation.fromAxisAngle(PLUS_Z_DIR, 0.0).inverse());
330
331 assertRotationEquals(StandardRotations.PLUS_Z_HALF_PI, QuaternionRotation.fromAxisAngle(PLUS_Z_DIR, -Angle.PI_OVER_TWO).inverse());
332 assertRotationEquals(StandardRotations.PLUS_Z_HALF_PI, QuaternionRotation.fromAxisAngle(MINUS_Z_DIR, Angle.PI_OVER_TWO).inverse());
333
334 assertRotationEquals(StandardRotations.MINUS_Z_HALF_PI, QuaternionRotation.fromAxisAngle(MINUS_Z_DIR, -Angle.PI_OVER_TWO).inverse());
335 assertRotationEquals(StandardRotations.MINUS_Z_HALF_PI, QuaternionRotation.fromAxisAngle(PLUS_Z_DIR, Angle.PI_OVER_TWO).inverse());
336
337 assertRotationEquals(StandardRotations.Z_PI, QuaternionRotation.fromAxisAngle(PLUS_Z_DIR, Math.PI).inverse());
338 assertRotationEquals(StandardRotations.Z_PI, QuaternionRotation.fromAxisAngle(MINUS_Z_DIR, Math.PI).inverse());
339
340
341 assertRotationEquals(StandardRotations.PLUS_DIAGONAL_TWO_THIRDS_PI, QuaternionRotation.fromAxisAngle(PLUS_DIAGONAL, MINUS_TWO_THIRDS_PI).inverse());
342 assertRotationEquals(StandardRotations.PLUS_DIAGONAL_TWO_THIRDS_PI, QuaternionRotation.fromAxisAngle(MINUS_DIAGONAL, TWO_THIRDS_PI).inverse());
343
344 assertRotationEquals(StandardRotations.MINUS_DIAGONAL_TWO_THIRDS_PI, QuaternionRotation.fromAxisAngle(MINUS_DIAGONAL, MINUS_TWO_THIRDS_PI).inverse());
345 assertRotationEquals(StandardRotations.MINUS_DIAGONAL_TWO_THIRDS_PI, QuaternionRotation.fromAxisAngle(PLUS_DIAGONAL, TWO_THIRDS_PI).inverse());
346 }
347
348 @Test
349 void testInverse_undoesOriginalRotation() {
350 EuclideanTestUtils.permuteSkipZero(-5, 5, 1, (x, y, z) -> {
351
352 final Vector3D vec = Vector3D.of(x, y, z);
353
354 final QuaternionRotation rot = QuaternionRotation.fromAxisAngle(vec, 0.75 * Math.PI);
355 final QuaternionRotation neg = rot.inverse();
356
357
358 EuclideanTestUtils.assertCoordinatesEqual(PLUS_DIAGONAL, neg.apply(rot.apply(PLUS_DIAGONAL)), EPS);
359 EuclideanTestUtils.assertCoordinatesEqual(PLUS_DIAGONAL, rot.apply(neg.apply(PLUS_DIAGONAL)), EPS);
360 });
361 }
362
363 @Test
364 void testMultiply_sameAxis_simple() {
365
366 final QuaternionRotation q1 = QuaternionRotation.fromAxisAngle(Vector3D.Unit.PLUS_X, 0.1 * Math.PI);
367 final QuaternionRotation q2 = QuaternionRotation.fromAxisAngle(Vector3D.Unit.PLUS_X, 0.4 * Math.PI);
368
369
370 final QuaternionRotation result = q1.multiply(q2);
371
372
373 EuclideanTestUtils.assertCoordinatesEqual(Vector3D.Unit.PLUS_X, result.getAxis(), EPS);
374 Assertions.assertEquals(Angle.PI_OVER_TWO, result.getAngle(), EPS);
375
376 assertRotationEquals(StandardRotations.PLUS_X_HALF_PI, result);
377 }
378
379 @Test
380 void testMultiply_sameAxis_multiple() {
381
382 final double oneThird = 1.0 / 3.0;
383 final QuaternionRotation q1 = QuaternionRotation.fromAxisAngle(PLUS_DIAGONAL, 0.1 * Math.PI);
384 final QuaternionRotation q2 = QuaternionRotation.fromAxisAngle(PLUS_DIAGONAL, oneThird * Math.PI);
385 final QuaternionRotation q3 = QuaternionRotation.fromAxisAngle(MINUS_DIAGONAL, 0.4 * Math.PI);
386 final QuaternionRotation q4 = QuaternionRotation.fromAxisAngle(PLUS_DIAGONAL, 0.3 * Math.PI);
387 final QuaternionRotation q5 = QuaternionRotation.fromAxisAngle(MINUS_DIAGONAL, -oneThird * Math.PI);
388
389
390 final QuaternionRotation result = q1.multiply(q2).multiply(q3).multiply(q4).multiply(q5);
391
392
393 EuclideanTestUtils.assertCoordinatesEqual(PLUS_DIAGONAL.normalize(), result.getAxis(), EPS);
394 Assertions.assertEquals(2.0 * Math.PI / 3.0, result.getAngle(), EPS);
395
396 assertRotationEquals(StandardRotations.PLUS_DIAGONAL_TWO_THIRDS_PI, result);
397 }
398
399 @Test
400 void testMultiply_differentAxes() {
401
402 final QuaternionRotation q1 = QuaternionRotation.fromAxisAngle(Vector3D.Unit.PLUS_X, Angle.PI_OVER_TWO);
403 final QuaternionRotation q2 = QuaternionRotation.fromAxisAngle(Vector3D.Unit.PLUS_Y, Angle.PI_OVER_TWO);
404
405
406 final QuaternionRotation result = q1.multiply(q2);
407
408
409 EuclideanTestUtils.assertCoordinatesEqual(PLUS_DIAGONAL.normalize(), result.getAxis(), EPS);
410 Assertions.assertEquals(2.0 * Math.PI / 3.0, result.getAngle(), EPS);
411
412 assertRotationEquals(StandardRotations.PLUS_DIAGONAL_TWO_THIRDS_PI, result);
413
414 assertRotationEquals(v -> {
415 final Vector3D temp = StandardRotations.PLUS_Y_HALF_PI.apply(v);
416 return StandardRotations.PLUS_X_HALF_PI.apply(temp);
417 }, result);
418 }
419
420 @Test
421 void testMultiply_orderOfOperations() {
422
423 final QuaternionRotation q1 = QuaternionRotation.fromAxisAngle(Vector3D.Unit.PLUS_X, Angle.PI_OVER_TWO);
424 final QuaternionRotation q2 = QuaternionRotation.fromAxisAngle(Vector3D.Unit.PLUS_Y, Math.PI);
425 final QuaternionRotation q3 = QuaternionRotation.fromAxisAngle(Vector3D.Unit.MINUS_Z, Angle.PI_OVER_TWO);
426
427
428 final QuaternionRotation result = q3.multiply(q2).multiply(q1);
429
430
431 assertRotationEquals(v -> {
432 Vector3D temp = StandardRotations.PLUS_X_HALF_PI.apply(v);
433 temp = StandardRotations.Y_PI.apply(temp);
434 return StandardRotations.MINUS_Z_HALF_PI.apply(temp);
435 }, result);
436 }
437
438 @Test
439 void testMultiply_numericalStability() {
440
441 final int slices = 1024;
442 final double delta = (8.0 * Math.PI / 3.0) / slices;
443
444 QuaternionRotation q = QuaternionRotation.identity();
445
446 final UniformRandomProvider rand = RandomSource.create(RandomSource.JDK, 2L);
447
448
449 for (int i = 0; i < slices; ++i) {
450 final double angle = rand.nextDouble();
451 final QuaternionRotation forward = QuaternionRotation.fromAxisAngle(PLUS_DIAGONAL, angle);
452 final QuaternionRotation backward = QuaternionRotation.fromAxisAngle(PLUS_DIAGONAL, delta - angle);
453
454 q = q.multiply(forward).multiply(backward);
455 }
456
457
458 Assertions.assertTrue(q.getQuaternion().getW() > 0);
459 Assertions.assertEquals(1.0, q.getQuaternion().norm(), EPS);
460
461 assertRotationEquals(StandardRotations.PLUS_DIAGONAL_TWO_THIRDS_PI, q);
462 }
463
464 @Test
465 void testPremultiply_sameAxis_simple() {
466
467 final QuaternionRotation q1 = QuaternionRotation.fromAxisAngle(Vector3D.Unit.PLUS_X, 0.1 * Math.PI);
468 final QuaternionRotation q2 = QuaternionRotation.fromAxisAngle(Vector3D.Unit.PLUS_X, 0.4 * Math.PI);
469
470
471 final QuaternionRotation result = q1.premultiply(q2);
472
473
474 EuclideanTestUtils.assertCoordinatesEqual(Vector3D.Unit.PLUS_X, result.getAxis(), EPS);
475 Assertions.assertEquals(Angle.PI_OVER_TWO, result.getAngle(), EPS);
476
477 assertRotationEquals(StandardRotations.PLUS_X_HALF_PI, result);
478 }
479
480 @Test
481 void testPremultiply_sameAxis_multiple() {
482
483 final double oneThird = 1.0 / 3.0;
484 final QuaternionRotation q1 = QuaternionRotation.fromAxisAngle(PLUS_DIAGONAL, 0.1 * Math.PI);
485 final QuaternionRotation q2 = QuaternionRotation.fromAxisAngle(PLUS_DIAGONAL, oneThird * Math.PI);
486 final QuaternionRotation q3 = QuaternionRotation.fromAxisAngle(MINUS_DIAGONAL, 0.4 * Math.PI);
487 final QuaternionRotation q4 = QuaternionRotation.fromAxisAngle(PLUS_DIAGONAL, 0.3 * Math.PI);
488 final QuaternionRotation q5 = QuaternionRotation.fromAxisAngle(MINUS_DIAGONAL, -oneThird * Math.PI);
489
490
491 final QuaternionRotation result = q1.premultiply(q2).premultiply(q3).premultiply(q4).premultiply(q5);
492
493
494 EuclideanTestUtils.assertCoordinatesEqual(PLUS_DIAGONAL.normalize(), result.getAxis(), EPS);
495 Assertions.assertEquals(2.0 * Math.PI / 3.0, result.getAngle(), EPS);
496
497 assertRotationEquals(StandardRotations.PLUS_DIAGONAL_TWO_THIRDS_PI, result);
498 }
499
500 @Test
501 void testPremultiply_differentAxes() {
502
503 final QuaternionRotation q1 = QuaternionRotation.fromAxisAngle(Vector3D.Unit.PLUS_X, Angle.PI_OVER_TWO);
504 final QuaternionRotation q2 = QuaternionRotation.fromAxisAngle(Vector3D.Unit.PLUS_Y, Angle.PI_OVER_TWO);
505
506
507 final QuaternionRotation result = q2.premultiply(q1);
508
509
510 EuclideanTestUtils.assertCoordinatesEqual(PLUS_DIAGONAL.normalize(), result.getAxis(), EPS);
511 Assertions.assertEquals(2.0 * Math.PI / 3.0, result.getAngle(), EPS);
512
513 assertRotationEquals(StandardRotations.PLUS_DIAGONAL_TWO_THIRDS_PI, result);
514
515 assertRotationEquals(v -> {
516 final Vector3D temp = StandardRotations.PLUS_Y_HALF_PI.apply(v);
517 return StandardRotations.PLUS_X_HALF_PI.apply(temp);
518 }, result);
519 }
520
521 @Test
522 void testPremultiply_orderOfOperations() {
523
524 final QuaternionRotation q1 = QuaternionRotation.fromAxisAngle(Vector3D.Unit.PLUS_X, Angle.PI_OVER_TWO);
525 final QuaternionRotation q2 = QuaternionRotation.fromAxisAngle(Vector3D.Unit.PLUS_Y, Math.PI);
526 final QuaternionRotation q3 = QuaternionRotation.fromAxisAngle(Vector3D.Unit.MINUS_Z, Angle.PI_OVER_TWO);
527
528
529 final QuaternionRotation result = q1.premultiply(q2).premultiply(q3);
530
531
532 assertRotationEquals(v -> {
533 Vector3D temp = StandardRotations.PLUS_X_HALF_PI.apply(v);
534 temp = StandardRotations.Y_PI.apply(temp);
535 return StandardRotations.MINUS_Z_HALF_PI.apply(temp);
536 }, result);
537 }
538
539 @Test
540 void testSlerp_simple() {
541
542 final QuaternionRotation q0 = QuaternionRotation.fromAxisAngle(Vector3D.Unit.PLUS_Z, 0.0);
543 final QuaternionRotation q1 = QuaternionRotation.fromAxisAngle(Vector3D.Unit.PLUS_Z, Math.PI);
544 final DoubleFunction<QuaternionRotation> fn = q0.slerp(q1);
545 final Vector3D v = Vector3D.of(2, 0, 1);
546
547 final double sqrt2 = Math.sqrt(2);
548
549
550 checkVector(fn.apply(0).apply(v), 2, 0, 1);
551 checkVector(fn.apply(0.25).apply(v), sqrt2, sqrt2, 1);
552 checkVector(fn.apply(0.5).apply(v), 0, 2, 1);
553 checkVector(fn.apply(0.75).apply(v), -sqrt2, sqrt2, 1);
554 checkVector(fn.apply(1).apply(v), -2, 0, 1);
555 }
556
557 @Test
558 void testSlerp_multipleCombinations() {
559
560 final QuaternionRotation[] rotations = {
561 QuaternionRotation.fromAxisAngle(Vector3D.Unit.PLUS_X, 0.0),
562 QuaternionRotation.fromAxisAngle(Vector3D.Unit.PLUS_X, Angle.PI_OVER_TWO),
563 QuaternionRotation.fromAxisAngle(Vector3D.Unit.PLUS_X, Math.PI),
564
565 QuaternionRotation.fromAxisAngle(Vector3D.Unit.MINUS_X, 0.0),
566 QuaternionRotation.fromAxisAngle(Vector3D.Unit.MINUS_X, Angle.PI_OVER_TWO),
567 QuaternionRotation.fromAxisAngle(Vector3D.Unit.MINUS_X, Math.PI),
568
569 QuaternionRotation.fromAxisAngle(Vector3D.Unit.PLUS_Y, 0.0),
570 QuaternionRotation.fromAxisAngle(Vector3D.Unit.PLUS_Y, Angle.PI_OVER_TWO),
571 QuaternionRotation.fromAxisAngle(Vector3D.Unit.PLUS_Y, Math.PI),
572
573 QuaternionRotation.fromAxisAngle(Vector3D.Unit.MINUS_Y, 0.0),
574 QuaternionRotation.fromAxisAngle(Vector3D.Unit.MINUS_Y, Angle.PI_OVER_TWO),
575 QuaternionRotation.fromAxisAngle(Vector3D.Unit.MINUS_Y, Math.PI),
576
577 QuaternionRotation.fromAxisAngle(Vector3D.Unit.PLUS_Z, 0.0),
578 QuaternionRotation.fromAxisAngle(Vector3D.Unit.PLUS_Z, Angle.PI_OVER_TWO),
579 QuaternionRotation.fromAxisAngle(Vector3D.Unit.PLUS_Z, Math.PI),
580
581 QuaternionRotation.fromAxisAngle(Vector3D.Unit.MINUS_Z, 0.0),
582 QuaternionRotation.fromAxisAngle(Vector3D.Unit.MINUS_Z, Angle.PI_OVER_TWO),
583 QuaternionRotation.fromAxisAngle(Vector3D.Unit.MINUS_Z, Math.PI),
584 };
585
586
587
588 for (final QuaternionRotation quaternionRotation : rotations) {
589 for (final QuaternionRotation rotation : rotations) {
590 checkSlerpCombination(quaternionRotation, rotation);
591 }
592 }
593 }
594
595 private void checkSlerpCombination(final QuaternionRotation start, final QuaternionRotation end) {
596 final DoubleFunction<QuaternionRotation> slerp = start.slerp(end);
597 final Vector3D vec = Vector3D.of(1, 1, 1).normalize();
598
599 final Vector3D startVec = start.apply(vec);
600 final Vector3D endVec = end.apply(vec);
601
602
603 EuclideanTestUtils.assertCoordinatesEqual(startVec, slerp.apply(0).apply(vec), EPS);
604 EuclideanTestUtils.assertCoordinatesEqual(endVec, slerp.apply(1).apply(vec), EPS);
605
606
607 double prevAngle = -1;
608 final int numSteps = 100;
609 final double delta = 1d / numSteps;
610 for (int step = 0; step <= numSteps; step++) {
611 final double t = step * delta;
612 final QuaternionRotation result = slerp.apply(t);
613
614 final Vector3D slerpVec = result.apply(vec);
615 Assertions.assertEquals(1, slerpVec.norm(), EPS);
616
617
618 final double angle = slerpVec.angle(startVec);
619 Assertions.assertTrue(Precision.compareTo(angle, prevAngle, EPS) >= 0, "Expected slerp angle to continuously increase; previous angle was " +
620 prevAngle + " and new angle is " + angle);
621
622 prevAngle = angle;
623 }
624 }
625
626 @Test
627 void testSlerp_followsShortestPath() {
628
629 final QuaternionRotation q1 = QuaternionRotation.fromAxisAngle(Vector3D.Unit.PLUS_Z, 0.75 * Math.PI);
630 final QuaternionRotation q2 = QuaternionRotation.fromAxisAngle(Vector3D.Unit.PLUS_Z, -0.75 * Math.PI);
631
632
633 final QuaternionRotation result = q1.slerp(q2).apply(0.5);
634
635
636
637
638 EuclideanTestUtils.assertCoordinatesEqual(Vector3D.Unit.MINUS_X, result.apply(Vector3D.Unit.PLUS_X), EPS);
639
640 EuclideanTestUtils.assertCoordinatesEqual(Vector3D.Unit.PLUS_Z, result.getAxis(), EPS);
641 Assertions.assertEquals(Math.PI, result.getAngle(), EPS);
642 }
643
644 @Test
645 void testSlerp_inputQuaternionsHaveMinusOneDotProduct() {
646
647 final QuaternionRotation q1 = QuaternionRotation.of(1, 0, 0, 1);
648 final QuaternionRotation q2 = QuaternionRotation.of(-1, 0, 0, -1);
649
650
651 final QuaternionRotation result = q1.slerp(q2).apply(0.5);
652
653
654 EuclideanTestUtils.assertCoordinatesEqual(Vector3D.Unit.PLUS_Y, result.apply(Vector3D.Unit.PLUS_X), EPS);
655
656 Assertions.assertEquals(Angle.PI_OVER_TWO, result.getAngle(), EPS);
657 EuclideanTestUtils.assertCoordinatesEqual(Vector3D.Unit.PLUS_Z, result.getAxis(), EPS);
658 }
659
660 @Test
661 void testSlerp_outputQuaternionIsNormalizedForAllT() {
662
663 final QuaternionRotation q1 = QuaternionRotation.fromAxisAngle(Vector3D.Unit.PLUS_Z, 0.25 * Math.PI);
664 final QuaternionRotation q2 = QuaternionRotation.fromAxisAngle(Vector3D.Unit.PLUS_Z, 0.75 * Math.PI);
665
666 final int numSteps = 200;
667 final double delta = 1d / numSteps;
668 for (int step = 0; step <= numSteps; step++) {
669 final double t = -10 + step * delta;
670
671
672 final QuaternionRotation result = q1.slerp(q2).apply(t);
673
674
675 Assertions.assertEquals(1.0, result.getQuaternion().norm(), EPS);
676 }
677 }
678
679 @Test
680 void testSlerp_tOutsideOfZeroToOne_apply() {
681
682 final Vector3D vec = Vector3D.Unit.PLUS_X;
683
684 final QuaternionRotation q1 = QuaternionRotation.fromAxisAngle(Vector3D.Unit.PLUS_Z, 0.25 * Math.PI);
685 final QuaternionRotation q2 = QuaternionRotation.fromAxisAngle(Vector3D.Unit.PLUS_Z, 0.75 * Math.PI);
686
687
688 final DoubleFunction<QuaternionRotation> slerp12 = q1.slerp(q2);
689 EuclideanTestUtils.assertCoordinatesEqual(Vector3D.Unit.PLUS_X, slerp12.apply(-4.5).apply(vec), EPS);
690 EuclideanTestUtils.assertCoordinatesEqual(Vector3D.Unit.PLUS_X, slerp12.apply(-0.5).apply(vec), EPS);
691 EuclideanTestUtils.assertCoordinatesEqual(Vector3D.Unit.MINUS_X, slerp12.apply(1.5).apply(vec), EPS);
692 EuclideanTestUtils.assertCoordinatesEqual(Vector3D.Unit.MINUS_X, slerp12.apply(5.5).apply(vec), EPS);
693
694 final DoubleFunction<QuaternionRotation> slerp21 = q2.slerp(q1);
695 EuclideanTestUtils.assertCoordinatesEqual(Vector3D.Unit.MINUS_X, slerp21.apply(-4.5).apply(vec), EPS);
696 EuclideanTestUtils.assertCoordinatesEqual(Vector3D.Unit.MINUS_X, slerp21.apply(-0.5).apply(vec), EPS);
697 EuclideanTestUtils.assertCoordinatesEqual(Vector3D.Unit.PLUS_X, slerp21.apply(1.5).apply(vec), EPS);
698 EuclideanTestUtils.assertCoordinatesEqual(Vector3D.Unit.PLUS_X, slerp21.apply(5.5).apply(vec), EPS);
699 }
700
701 @Test
702 void testToMatrix() {
703
704
705 assertTransformEquals(StandardRotations.IDENTITY, QuaternionRotation.fromAxisAngle(PLUS_X_DIR, 0.0).toMatrix());
706
707 assertTransformEquals(StandardRotations.PLUS_X_HALF_PI, QuaternionRotation.fromAxisAngle(PLUS_X_DIR, Angle.PI_OVER_TWO).toMatrix());
708 assertTransformEquals(StandardRotations.PLUS_X_HALF_PI, QuaternionRotation.fromAxisAngle(MINUS_X_DIR, -Angle.PI_OVER_TWO).toMatrix());
709
710 assertTransformEquals(StandardRotations.MINUS_X_HALF_PI, QuaternionRotation.fromAxisAngle(MINUS_X_DIR, Angle.PI_OVER_TWO).toMatrix());
711 assertTransformEquals(StandardRotations.MINUS_X_HALF_PI, QuaternionRotation.fromAxisAngle(PLUS_X_DIR, -Angle.PI_OVER_TWO).toMatrix());
712
713 assertTransformEquals(StandardRotations.X_PI, QuaternionRotation.fromAxisAngle(PLUS_X_DIR, Math.PI).toMatrix());
714 assertTransformEquals(StandardRotations.X_PI, QuaternionRotation.fromAxisAngle(MINUS_X_DIR, Math.PI).toMatrix());
715
716
717 assertTransformEquals(StandardRotations.IDENTITY, QuaternionRotation.fromAxisAngle(PLUS_Y_DIR, 0.0).toMatrix());
718
719 assertTransformEquals(StandardRotations.PLUS_Y_HALF_PI, QuaternionRotation.fromAxisAngle(PLUS_Y_DIR, Angle.PI_OVER_TWO).toMatrix());
720 assertTransformEquals(StandardRotations.PLUS_Y_HALF_PI, QuaternionRotation.fromAxisAngle(MINUS_Y_DIR, -Angle.PI_OVER_TWO).toMatrix());
721
722 assertTransformEquals(StandardRotations.MINUS_Y_HALF_PI, QuaternionRotation.fromAxisAngle(MINUS_Y_DIR, Angle.PI_OVER_TWO).toMatrix());
723 assertTransformEquals(StandardRotations.MINUS_Y_HALF_PI, QuaternionRotation.fromAxisAngle(PLUS_Y_DIR, -Angle.PI_OVER_TWO).toMatrix());
724
725 assertTransformEquals(StandardRotations.Y_PI, QuaternionRotation.fromAxisAngle(PLUS_Y_DIR, Math.PI).toMatrix());
726 assertTransformEquals(StandardRotations.Y_PI, QuaternionRotation.fromAxisAngle(MINUS_Y_DIR, Math.PI).toMatrix());
727
728
729 assertTransformEquals(StandardRotations.IDENTITY, QuaternionRotation.fromAxisAngle(PLUS_Z_DIR, 0.0).toMatrix());
730
731 assertTransformEquals(StandardRotations.PLUS_Z_HALF_PI, QuaternionRotation.fromAxisAngle(PLUS_Z_DIR, Angle.PI_OVER_TWO).toMatrix());
732 assertTransformEquals(StandardRotations.PLUS_Z_HALF_PI, QuaternionRotation.fromAxisAngle(MINUS_Z_DIR, -Angle.PI_OVER_TWO).toMatrix());
733
734 assertTransformEquals(StandardRotations.MINUS_Z_HALF_PI, QuaternionRotation.fromAxisAngle(MINUS_Z_DIR, Angle.PI_OVER_TWO).toMatrix());
735 assertTransformEquals(StandardRotations.MINUS_Z_HALF_PI, QuaternionRotation.fromAxisAngle(PLUS_Z_DIR, -Angle.PI_OVER_TWO).toMatrix());
736
737 assertTransformEquals(StandardRotations.Z_PI, QuaternionRotation.fromAxisAngle(PLUS_Z_DIR, Math.PI).toMatrix());
738 assertTransformEquals(StandardRotations.Z_PI, QuaternionRotation.fromAxisAngle(MINUS_Z_DIR, Math.PI).toMatrix());
739
740
741 assertTransformEquals(StandardRotations.PLUS_DIAGONAL_TWO_THIRDS_PI, QuaternionRotation.fromAxisAngle(PLUS_DIAGONAL, TWO_THIRDS_PI).toMatrix());
742 assertTransformEquals(StandardRotations.PLUS_DIAGONAL_TWO_THIRDS_PI, QuaternionRotation.fromAxisAngle(MINUS_DIAGONAL, MINUS_TWO_THIRDS_PI).toMatrix());
743
744 assertTransformEquals(StandardRotations.MINUS_DIAGONAL_TWO_THIRDS_PI, QuaternionRotation.fromAxisAngle(MINUS_DIAGONAL, TWO_THIRDS_PI).toMatrix());
745 assertTransformEquals(StandardRotations.MINUS_DIAGONAL_TWO_THIRDS_PI, QuaternionRotation.fromAxisAngle(PLUS_DIAGONAL, MINUS_TWO_THIRDS_PI).toMatrix());
746 }
747
748 @Test
749 void testAxisAngleSequenceConversion_relative() {
750 for (final AxisSequence axes : AxisSequence.values()) {
751 checkAxisAngleSequenceToQuaternionRoundtrip(AxisReferenceFrame.RELATIVE, axes);
752 checkQuaternionToAxisAngleSequenceRoundtrip(AxisReferenceFrame.RELATIVE, axes);
753 }
754 }
755
756 @Test
757 void testAxisAngleSequenceConversion_absolute() {
758 for (final AxisSequence axes : AxisSequence.values()) {
759 checkAxisAngleSequenceToQuaternionRoundtrip(AxisReferenceFrame.ABSOLUTE, axes);
760 checkQuaternionToAxisAngleSequenceRoundtrip(AxisReferenceFrame.ABSOLUTE, axes);
761 }
762 }
763
764 private void checkAxisAngleSequenceToQuaternionRoundtrip(final AxisReferenceFrame frame, final AxisSequence axes) {
765 final double step = 0.3;
766 final double angle2Start = axes.getType() == AxisSequenceType.EULER ? 0.0 + 0.1 : -Angle.PI_OVER_TWO + 0.1;
767 final double angle2Stop = angle2Start + Math.PI;
768
769 for (double angle1 = 0.0; angle1 <= Angle.TWO_PI; angle1 += step) {
770 for (double angle2 = angle2Start; angle2 < angle2Stop; angle2 += step) {
771 for (double angle3 = 0.0; angle3 <= Angle.TWO_PI; angle3 += 0.3) {
772
773 final AxisAngleSequence angles = new AxisAngleSequence(frame, axes, angle1, angle2, angle3);
774
775
776 final QuaternionRotation q = QuaternionRotation.fromAxisAngleSequence(angles);
777 final AxisAngleSequence result = q.toAxisAngleSequence(frame, axes);
778
779
780 Assertions.assertEquals(frame, result.getReferenceFrame());
781 Assertions.assertEquals(axes, result.getAxisSequence());
782
783 assertRadiansEquals(angle1, result.getAngle1());
784 assertRadiansEquals(angle2, result.getAngle2());
785 assertRadiansEquals(angle3, result.getAngle3());
786 }
787 }
788 }
789 }
790
791 private void checkQuaternionToAxisAngleSequenceRoundtrip(final AxisReferenceFrame frame, final AxisSequence axes) {
792 final double step = 0.1;
793
794 EuclideanTestUtils.permuteSkipZero(-1, 1, 0.5, (x, y, z) -> {
795 final Vector3D axis = Vector3D.of(x, y, z);
796
797 for (double angle = -Angle.TWO_PI; angle <= Angle.TWO_PI; angle += step) {
798
799 final QuaternionRotation q = QuaternionRotation.fromAxisAngle(axis, angle);
800
801
802 final AxisAngleSequence seq = q.toAxisAngleSequence(frame, axes);
803 final QuaternionRotation result = QuaternionRotation.fromAxisAngleSequence(seq);
804
805
806 checkQuaternion(result, q.getQuaternion().getW(), q.getQuaternion().getX(), q.getQuaternion().getY(), q.getQuaternion().getZ());
807 }
808 });
809 }
810
811 @Test
812 void testAxisAngleSequenceConversion_relative_eulerSingularities() {
813
814 final double[] eulerSingularities = {
815 0.0,
816 Math.PI
817 };
818
819 final double angle1 = 0.1;
820 final double angle2 = 0.3;
821
822 final AxisReferenceFrame frame = AxisReferenceFrame.RELATIVE;
823
824 for (final AxisSequence axes : getAxes(AxisSequenceType.EULER)) {
825 for (final double singularityAngle : eulerSingularities) {
826
827 final AxisAngleSequence inputSeq = new AxisAngleSequence(frame, axes, angle1, singularityAngle, angle2);
828 final QuaternionRotation inputQuat = QuaternionRotation.fromAxisAngleSequence(inputSeq);
829
830
831 final AxisAngleSequence resultSeq = inputQuat.toAxisAngleSequence(frame, axes);
832 final QuaternionRotation resultQuat = QuaternionRotation.fromAxisAngleSequence(resultSeq);
833
834
835 Assertions.assertEquals(frame, resultSeq.getReferenceFrame());
836 Assertions.assertEquals(axes, resultSeq.getAxisSequence());
837
838 assertRadiansEquals(singularityAngle, resultSeq.getAngle2());
839 assertRadiansEquals(0.0, resultSeq.getAngle3());
840
841 checkQuaternion(resultQuat, inputQuat.getQuaternion().getW(), inputQuat.getQuaternion().getX(), inputQuat.getQuaternion().getY(), inputQuat.getQuaternion().getZ());
842 }
843 }
844 }
845
846 @Test
847 void testAxisAngleSequenceConversion_absolute_eulerSingularities() {
848
849 final double[] eulerSingularities = {
850 0.0,
851 Math.PI
852 };
853
854 final double angle1 = 0.1;
855 final double angle2 = 0.3;
856
857 final AxisReferenceFrame frame = AxisReferenceFrame.ABSOLUTE;
858
859 for (final AxisSequence axes : getAxes(AxisSequenceType.EULER)) {
860 for (final double singularityAngle : eulerSingularities) {
861
862 final AxisAngleSequence inputSeq = new AxisAngleSequence(frame, axes, angle1, singularityAngle, angle2);
863 final QuaternionRotation inputQuat = QuaternionRotation.fromAxisAngleSequence(inputSeq);
864
865
866 final AxisAngleSequence resultSeq = inputQuat.toAxisAngleSequence(frame, axes);
867 final QuaternionRotation resultQuat = QuaternionRotation.fromAxisAngleSequence(resultSeq);
868
869
870 Assertions.assertEquals(frame, resultSeq.getReferenceFrame());
871 Assertions.assertEquals(axes, resultSeq.getAxisSequence());
872
873 assertRadiansEquals(0.0, resultSeq.getAngle1());
874 assertRadiansEquals(singularityAngle, resultSeq.getAngle2());
875
876 checkQuaternion(resultQuat, inputQuat.getQuaternion().getW(), inputQuat.getQuaternion().getX(), inputQuat.getQuaternion().getY(), inputQuat.getQuaternion().getZ());
877 }
878 }
879 }
880
881 @Test
882 void testAxisAngleSequenceConversion_relative_taitBryanSingularities() {
883
884 final double[] taitBryanSingularities = {
885 -Angle.PI_OVER_TWO,
886 Angle.PI_OVER_TWO
887 };
888
889 final double angle1 = 0.1;
890 final double angle2 = 0.3;
891
892 final AxisReferenceFrame frame = AxisReferenceFrame.RELATIVE;
893
894 for (final AxisSequence axes : getAxes(AxisSequenceType.TAIT_BRYAN)) {
895 for (final double singularityAngle : taitBryanSingularities) {
896
897 final AxisAngleSequence inputSeq = new AxisAngleSequence(frame, axes, angle1, singularityAngle, angle2);
898 final QuaternionRotation inputQuat = QuaternionRotation.fromAxisAngleSequence(inputSeq);
899
900
901 final AxisAngleSequence resultSeq = inputQuat.toAxisAngleSequence(frame, axes);
902 final QuaternionRotation resultQuat = QuaternionRotation.fromAxisAngleSequence(resultSeq);
903
904
905 Assertions.assertEquals(frame, resultSeq.getReferenceFrame());
906 Assertions.assertEquals(axes, resultSeq.getAxisSequence());
907
908 assertRadiansEquals(singularityAngle, resultSeq.getAngle2());
909 assertRadiansEquals(0.0, resultSeq.getAngle3());
910
911 checkQuaternion(resultQuat, inputQuat.getQuaternion().getW(), inputQuat.getQuaternion().getX(), inputQuat.getQuaternion().getY(), inputQuat.getQuaternion().getZ());
912 }
913 }
914 }
915
916 @Test
917 void testAxisAngleSequenceConversion_absolute_taitBryanSingularities() {
918
919 final double[] taitBryanSingularities = {
920 -Angle.PI_OVER_TWO,
921 Angle.PI_OVER_TWO
922 };
923
924 final double angle1 = 0.1;
925 final double angle2 = 0.3;
926
927 final AxisReferenceFrame frame = AxisReferenceFrame.ABSOLUTE;
928
929 for (final AxisSequence axes : getAxes(AxisSequenceType.TAIT_BRYAN)) {
930 for (final double singularityAngle : taitBryanSingularities) {
931
932 final AxisAngleSequence inputSeq = new AxisAngleSequence(frame, axes, angle1, singularityAngle, angle2);
933 final QuaternionRotation inputQuat = QuaternionRotation.fromAxisAngleSequence(inputSeq);
934
935
936 final AxisAngleSequence resultSeq = inputQuat.toAxisAngleSequence(frame, axes);
937 final QuaternionRotation resultQuat = QuaternionRotation.fromAxisAngleSequence(resultSeq);
938
939
940 Assertions.assertEquals(frame, resultSeq.getReferenceFrame());
941 Assertions.assertEquals(axes, resultSeq.getAxisSequence());
942
943 assertRadiansEquals(0.0, resultSeq.getAngle1());
944 assertRadiansEquals(singularityAngle, resultSeq.getAngle2());
945
946 checkQuaternion(resultQuat, inputQuat.getQuaternion().getW(), inputQuat.getQuaternion().getX(), inputQuat.getQuaternion().getY(), inputQuat.getQuaternion().getZ());
947 }
948 }
949 }
950
951 private List<AxisSequence> getAxes(final AxisSequenceType type) {
952 return Stream.of(AxisSequence.values())
953 .filter(a -> type.equals(a.getType()))
954 .collect(Collectors.toList());
955 }
956
957 @Test
958 void testToAxisAngleSequence_invalidArgs() {
959
960 final QuaternionRotation q = QuaternionRotation.identity();
961
962
963 Assertions.assertThrows(IllegalArgumentException.class, () -> q.toAxisAngleSequence(null, AxisSequence.XYZ));
964 Assertions.assertThrows(IllegalArgumentException.class, () -> q.toAxisAngleSequence(AxisReferenceFrame.ABSOLUTE, null));
965 }
966
967 @Test
968 void testToRelativeAxisAngleSequence() {
969
970 final QuaternionRotation q = QuaternionRotation.fromAxisAngle(PLUS_DIAGONAL, TWO_THIRDS_PI);
971
972
973 final AxisAngleSequence seq = q.toRelativeAxisAngleSequence(AxisSequence.YZX);
974
975
976 Assertions.assertEquals(AxisReferenceFrame.RELATIVE, seq.getReferenceFrame());
977 Assertions.assertEquals(AxisSequence.YZX, seq.getAxisSequence());
978 Assertions.assertEquals(Angle.PI_OVER_TWO, seq.getAngle1(), EPS);
979 Assertions.assertEquals(Angle.PI_OVER_TWO, seq.getAngle2(), EPS);
980 Assertions.assertEquals(0, seq.getAngle3(), EPS);
981 }
982
983 @Test
984 void testToAbsoluteAxisAngleSequence() {
985
986 final QuaternionRotation q = QuaternionRotation.fromAxisAngle(PLUS_DIAGONAL, TWO_THIRDS_PI);
987
988
989 final AxisAngleSequence seq = q.toAbsoluteAxisAngleSequence(AxisSequence.YZX);
990
991
992 Assertions.assertEquals(AxisReferenceFrame.ABSOLUTE, seq.getReferenceFrame());
993 Assertions.assertEquals(AxisSequence.YZX, seq.getAxisSequence());
994 Assertions.assertEquals(Angle.PI_OVER_TWO, seq.getAngle1(), EPS);
995 Assertions.assertEquals(0, seq.getAngle2(), EPS);
996 Assertions.assertEquals(Angle.PI_OVER_TWO, seq.getAngle3(), EPS);
997 }
998
999 @Test
1000 void testHashCode() {
1001
1002 final double delta = 100 * Precision.EPSILON;
1003 final QuaternionRotation q1 = QuaternionRotation.of(1, 2, 3, 4);
1004 final QuaternionRotation q2 = QuaternionRotation.of(1, 2, 3, 4);
1005
1006
1007 Assertions.assertEquals(q1.hashCode(), q2.hashCode());
1008
1009 Assertions.assertNotEquals(q1.hashCode(), QuaternionRotation.of(1 + delta, 2, 3, 4).hashCode());
1010 Assertions.assertNotEquals(q1.hashCode(), QuaternionRotation.of(1, 2 + delta, 3, 4).hashCode());
1011 Assertions.assertNotEquals(q1.hashCode(), QuaternionRotation.of(1, 2, 3 + delta, 4).hashCode());
1012 Assertions.assertNotEquals(q1.hashCode(), QuaternionRotation.of(1, 2, 3, 4 + delta).hashCode());
1013 }
1014
1015 @Test
1016 void testEquals() {
1017
1018 final double delta = 100 * Precision.EPSILON;
1019 final QuaternionRotation q1 = QuaternionRotation.of(1, 2, 3, 4);
1020 final QuaternionRotation q2 = QuaternionRotation.of(1, 2, 3, 4);
1021
1022
1023 GeometryTestUtils.assertSimpleEqualsCases(q1);
1024 Assertions.assertEquals(q1, q2);
1025
1026 Assertions.assertNotEquals(q1, QuaternionRotation.of(-1, -2, -3, 4));
1027 Assertions.assertNotEquals(q1, QuaternionRotation.of(1, 2, 3, -4));
1028
1029 Assertions.assertNotEquals(q1, QuaternionRotation.of(1 + delta, 2, 3, 4));
1030 Assertions.assertNotEquals(q1, QuaternionRotation.of(1, 2 + delta, 3, 4));
1031 Assertions.assertNotEquals(q1, QuaternionRotation.of(1, 2, 3 + delta, 4));
1032 Assertions.assertNotEquals(q1, QuaternionRotation.of(1, 2, 3, 4 + delta));
1033 }
1034
1035 @Test
1036 void testToString() {
1037
1038 final QuaternionRotation q = QuaternionRotation.of(1, 2, 3, 4);
1039 final Quaternion qField = q.getQuaternion();
1040
1041
1042 Assertions.assertEquals(qField.toString(), q.toString());
1043 }
1044
1045 @Test
1046 void testCreateVectorRotation_simple() {
1047
1048 final Vector3D u1 = Vector3D.Unit.PLUS_X;
1049 final Vector3D u2 = Vector3D.Unit.PLUS_Y;
1050
1051
1052 final QuaternionRotation q = QuaternionRotation.createVectorRotation(u1, u2);
1053
1054
1055 final double val = Math.sqrt(2) * 0.5;
1056
1057 checkQuaternion(q, val, 0, 0, val);
1058
1059 EuclideanTestUtils.assertCoordinatesEqual(Vector3D.Unit.PLUS_Z, q.getAxis(), EPS);
1060 Assertions.assertEquals(Angle.PI_OVER_TWO, q.getAngle(), EPS);
1061
1062 EuclideanTestUtils.assertCoordinatesEqual(u2, q.apply(u1), EPS);
1063 EuclideanTestUtils.assertCoordinatesEqual(u1, q.inverse().apply(u2), EPS);
1064 }
1065
1066 @Test
1067 void testCreateVectorRotation_identity() {
1068
1069 final Vector3D u1 = Vector3D.of(0, 2, 0);
1070
1071
1072 final QuaternionRotation q = QuaternionRotation.createVectorRotation(u1, u1);
1073
1074
1075 checkQuaternion(q, 1, 0, 0, 0);
1076
1077 EuclideanTestUtils.assertCoordinatesEqual(Vector3D.Unit.PLUS_X, q.getAxis(), EPS);
1078 Assertions.assertEquals(0.0, q.getAngle(), EPS);
1079
1080 EuclideanTestUtils.assertCoordinatesEqual(Vector3D.of(0, 2, 0), q.apply(u1), EPS);
1081 EuclideanTestUtils.assertCoordinatesEqual(Vector3D.of(0, 2, 0), q.inverse().apply(u1), EPS);
1082 }
1083
1084 @Test
1085 void testCreateVectorRotation_parallel() {
1086
1087 final Vector3D u1 = Vector3D.of(0, 2, 0);
1088 final Vector3D u2 = Vector3D.of(0, 3, 0);
1089
1090
1091 final QuaternionRotation q = QuaternionRotation.createVectorRotation(u1, u2);
1092
1093
1094 checkQuaternion(q, 1, 0, 0, 0);
1095
1096 EuclideanTestUtils.assertCoordinatesEqual(Vector3D.Unit.PLUS_X, q.getAxis(), EPS);
1097 Assertions.assertEquals(0.0, q.getAngle(), EPS);
1098
1099 EuclideanTestUtils.assertCoordinatesEqual(Vector3D.of(0, 2, 0), q.apply(u1), EPS);
1100 EuclideanTestUtils.assertCoordinatesEqual(Vector3D.of(0, 3, 0), q.inverse().apply(u2), EPS);
1101 }
1102
1103 @Test
1104 void testCreateVectorRotation_antiparallel() {
1105
1106 final Vector3D u1 = Vector3D.of(0, 2, 0);
1107 final Vector3D u2 = Vector3D.of(0, -3, 0);
1108
1109
1110 final QuaternionRotation q = QuaternionRotation.createVectorRotation(u1, u2);
1111
1112
1113 final Vector3D axis = q.getAxis();
1114 Assertions.assertEquals(0.0, axis.dot(u1), EPS);
1115 Assertions.assertEquals(0.0, axis.dot(u2), EPS);
1116 Assertions.assertEquals(Math.PI, q.getAngle(), EPS);
1117
1118 EuclideanTestUtils.assertCoordinatesEqual(Vector3D.of(0, -2, 0), q.apply(u1), EPS);
1119 EuclideanTestUtils.assertCoordinatesEqual(Vector3D.of(0, 3, 0), q.inverse().apply(u2), EPS);
1120 }
1121
1122 @Test
1123 void testCreateVectorRotation_permute() {
1124 EuclideanTestUtils.permuteSkipZero(-5, 5, 0.1, (x, y, z) -> {
1125
1126 final Vector3D u1 = Vector3D.of(x, y, z);
1127 final Vector3D u2 = PLUS_DIAGONAL;
1128
1129
1130 final QuaternionRotation q = QuaternionRotation.createVectorRotation(u1, u2);
1131
1132
1133 Assertions.assertEquals(0.0, q.apply(u1).angle(u2), EPS);
1134 Assertions.assertEquals(0.0, q.inverse().apply(u2).angle(u1), EPS);
1135
1136 final double angle = q.getAngle();
1137 Assertions.assertTrue(angle >= 0.0);
1138 Assertions.assertTrue(angle <= Math.PI);
1139 });
1140 }
1141
1142 @Test
1143 void testCreateVectorRotation_invalidArgs() {
1144
1145 Assertions.assertThrows(IllegalArgumentException.class, () -> QuaternionRotation.createVectorRotation(Vector3D.ZERO, Vector3D.Unit.PLUS_X));
1146 Assertions.assertThrows(IllegalArgumentException.class, () -> QuaternionRotation.createVectorRotation(Vector3D.Unit.PLUS_X, Vector3D.ZERO));
1147 Assertions.assertThrows(IllegalArgumentException.class, () -> QuaternionRotation.createVectorRotation(Vector3D.NaN, Vector3D.Unit.PLUS_X));
1148 Assertions.assertThrows(IllegalArgumentException.class, () -> QuaternionRotation.createVectorRotation(Vector3D.Unit.PLUS_X, Vector3D.POSITIVE_INFINITY));
1149 Assertions.assertThrows(IllegalArgumentException.class, () -> QuaternionRotation.createVectorRotation(Vector3D.Unit.PLUS_X, Vector3D.NEGATIVE_INFINITY));
1150 }
1151
1152 @Test
1153 void testCreateBasisRotation_simple() {
1154
1155 final Vector3D u1 = Vector3D.Unit.PLUS_X;
1156 final Vector3D u2 = Vector3D.Unit.PLUS_Y;
1157
1158 final Vector3D v1 = Vector3D.Unit.PLUS_Y;
1159 final Vector3D v2 = Vector3D.Unit.MINUS_X;
1160
1161
1162 final QuaternionRotation q = QuaternionRotation.createBasisRotation(u1, u2, v1, v2);
1163
1164
1165 final QuaternionRotation qInv = q.inverse();
1166
1167 EuclideanTestUtils.assertCoordinatesEqual(v1, q.apply(u1), EPS);
1168 EuclideanTestUtils.assertCoordinatesEqual(v2, q.apply(u2), EPS);
1169
1170 EuclideanTestUtils.assertCoordinatesEqual(u1, qInv.apply(v1), EPS);
1171 EuclideanTestUtils.assertCoordinatesEqual(u2, qInv.apply(v2), EPS);
1172
1173 assertRotationEquals(StandardRotations.PLUS_Z_HALF_PI, q);
1174 }
1175
1176 @Test
1177 void testCreateBasisRotation_diagonalAxis() {
1178
1179 final Vector3D u1 = Vector3D.Unit.PLUS_X;
1180 final Vector3D u2 = Vector3D.Unit.PLUS_Y;
1181
1182 final Vector3D v1 = Vector3D.Unit.PLUS_Y;
1183 final Vector3D v2 = Vector3D.Unit.PLUS_Z;
1184
1185
1186 final QuaternionRotation q = QuaternionRotation.createBasisRotation(u1, u2, v1, v2);
1187
1188
1189 final QuaternionRotation qInv = q.inverse();
1190
1191 EuclideanTestUtils.assertCoordinatesEqual(v1, q.apply(u1), EPS);
1192 EuclideanTestUtils.assertCoordinatesEqual(v2, q.apply(u2), EPS);
1193
1194 EuclideanTestUtils.assertCoordinatesEqual(u1, qInv.apply(v1), EPS);
1195 EuclideanTestUtils.assertCoordinatesEqual(u2, qInv.apply(v2), EPS);
1196
1197 assertRotationEquals(StandardRotations.PLUS_DIAGONAL_TWO_THIRDS_PI, q);
1198 assertRotationEquals(StandardRotations.MINUS_DIAGONAL_TWO_THIRDS_PI, q.inverse());
1199 }
1200
1201 @Test
1202 void testCreateBasisRotation_identity() {
1203
1204 final Vector3D u1 = Vector3D.Unit.PLUS_X;
1205 final Vector3D u2 = Vector3D.Unit.PLUS_Y;
1206
1207
1208 final QuaternionRotation q = QuaternionRotation.createBasisRotation(u1, u2, u1, u2);
1209
1210
1211 final QuaternionRotation qInv = q.inverse();
1212
1213 EuclideanTestUtils.assertCoordinatesEqual(u1, q.apply(u1), EPS);
1214 EuclideanTestUtils.assertCoordinatesEqual(u2, q.apply(u2), EPS);
1215
1216 EuclideanTestUtils.assertCoordinatesEqual(u1, qInv.apply(u1), EPS);
1217 EuclideanTestUtils.assertCoordinatesEqual(u2, qInv.apply(u2), EPS);
1218
1219 assertRotationEquals(StandardRotations.IDENTITY, q);
1220 }
1221
1222 @Test
1223 void testCreateBasisRotation_equivalentBases() {
1224
1225 final Vector3D u1 = Vector3D.of(2, 0, 0);
1226 final Vector3D u2 = Vector3D.of(0, 3, 0);
1227
1228 final Vector3D v1 = Vector3D.of(4, 0, 0);
1229 final Vector3D v2 = Vector3D.of(0, 5, 0);
1230
1231
1232 final QuaternionRotation q = QuaternionRotation.createBasisRotation(u1, u2, v1, v2);
1233
1234
1235 final QuaternionRotation qInv = q.inverse();
1236
1237 EuclideanTestUtils.assertCoordinatesEqual(u1, q.apply(u1), EPS);
1238 EuclideanTestUtils.assertCoordinatesEqual(u2, q.apply(u2), EPS);
1239
1240 EuclideanTestUtils.assertCoordinatesEqual(v1, qInv.apply(v1), EPS);
1241 EuclideanTestUtils.assertCoordinatesEqual(v2, qInv.apply(v2), EPS);
1242
1243 assertRotationEquals(StandardRotations.IDENTITY, q);
1244 }
1245
1246 @Test
1247 void testCreateBasisRotation_nonOrthogonalVectors() {
1248
1249 final Vector3D u1 = Vector3D.of(2, 0, 0);
1250 final Vector3D u2 = Vector3D.of(1, 0.5, 0);
1251
1252 final Vector3D v1 = Vector3D.of(0, 1.5, 0);
1253 final Vector3D v2 = Vector3D.of(-1, 1.5, 0);
1254
1255
1256 final QuaternionRotation q = QuaternionRotation.createBasisRotation(u1, u2, v1, v2);
1257
1258
1259 final QuaternionRotation qInv = q.inverse();
1260
1261 EuclideanTestUtils.assertCoordinatesEqual(Vector3D.of(0, 2, 0), q.apply(u1), EPS);
1262 EuclideanTestUtils.assertCoordinatesEqual(Vector3D.of(-0.5, 1, 0), q.apply(u2), EPS);
1263
1264 EuclideanTestUtils.assertCoordinatesEqual(Vector3D.of(1.5, 0, 0), qInv.apply(v1), EPS);
1265 EuclideanTestUtils.assertCoordinatesEqual(Vector3D.of(1.5, 1, 0), qInv.apply(v2), EPS);
1266
1267 assertRotationEquals(StandardRotations.PLUS_Z_HALF_PI, q);
1268 }
1269
1270 @Test
1271 void testCreateBasisRotation_permute() {
1272
1273 final Vector3D u1 = Vector3D.of(1, 2, 3);
1274 final Vector3D u2 = Vector3D.of(0, 4, 0);
1275
1276 final Vector3D u1Dir = u1.normalize();
1277 final Vector3D u2Dir = u1Dir.orthogonal(u2);
1278
1279 EuclideanTestUtils.permuteSkipZero(-5, 5, 0.2, (x, y, z) -> {
1280 final Vector3D v1 = Vector3D.of(x, y, z);
1281 final Vector3D v2 = v1.orthogonal();
1282
1283 final Vector3D v1Dir = v1.normalize();
1284 final Vector3D v2Dir = v2.normalize();
1285
1286
1287 final QuaternionRotation q = QuaternionRotation.createBasisRotation(u1, u2, v1, v2);
1288 final QuaternionRotation qInv = q.inverse();
1289
1290
1291 EuclideanTestUtils.assertCoordinatesEqual(v1Dir, q.apply(u1Dir), EPS);
1292 EuclideanTestUtils.assertCoordinatesEqual(v2Dir, q.apply(u2Dir), EPS);
1293
1294 EuclideanTestUtils.assertCoordinatesEqual(u1Dir, qInv.apply(v1Dir), EPS);
1295 EuclideanTestUtils.assertCoordinatesEqual(u2Dir, qInv.apply(v2Dir), EPS);
1296
1297 final double angle = q.getAngle();
1298 Assertions.assertTrue(angle >= 0.0);
1299 Assertions.assertTrue(angle <= Math.PI);
1300
1301 final Vector3D transformedX = q.apply(Vector3D.Unit.PLUS_X);
1302 final Vector3D transformedY = q.apply(Vector3D.Unit.PLUS_Y);
1303 final Vector3D transformedZ = q.apply(Vector3D.Unit.PLUS_Z);
1304
1305 Assertions.assertEquals(1.0, transformedX.norm(), EPS);
1306 Assertions.assertEquals(1.0, transformedY.norm(), EPS);
1307 Assertions.assertEquals(1.0, transformedZ.norm(), EPS);
1308
1309 Assertions.assertEquals(0.0, transformedX.dot(transformedY), EPS);
1310 Assertions.assertEquals(0.0, transformedX.dot(transformedZ), EPS);
1311 Assertions.assertEquals(0.0, transformedY.dot(transformedZ), EPS);
1312
1313 EuclideanTestUtils.assertCoordinatesEqual(transformedZ.normalize(),
1314 transformedX.normalize().cross(transformedY.normalize()), EPS);
1315
1316 Assertions.assertEquals(1.0, q.getQuaternion().norm(), EPS);
1317 });
1318 }
1319
1320 @Test
1321 void testCreateBasisRotation_invalidArgs() {
1322
1323 Assertions.assertThrows(IllegalArgumentException.class, () -> QuaternionRotation.createBasisRotation(
1324 Vector3D.ZERO, Vector3D.Unit.PLUS_Y, Vector3D.Unit.PLUS_Y, Vector3D.Unit.MINUS_X));
1325 Assertions.assertThrows(IllegalArgumentException.class, () -> QuaternionRotation.createBasisRotation(
1326 Vector3D.Unit.PLUS_X, Vector3D.NaN, Vector3D.Unit.PLUS_Y, Vector3D.Unit.MINUS_X));
1327 Assertions.assertThrows(IllegalArgumentException.class, () -> QuaternionRotation.createBasisRotation(
1328 Vector3D.Unit.PLUS_X, Vector3D.Unit.PLUS_Y, Vector3D.POSITIVE_INFINITY, Vector3D.Unit.MINUS_X));
1329 Assertions.assertThrows(IllegalArgumentException.class, () -> QuaternionRotation.createBasisRotation(
1330 Vector3D.Unit.PLUS_X, Vector3D.Unit.PLUS_Y, Vector3D.Unit.PLUS_Y, Vector3D.NEGATIVE_INFINITY));
1331 Assertions.assertThrows(IllegalArgumentException.class, () -> QuaternionRotation.createBasisRotation(
1332 Vector3D.Unit.PLUS_X, Vector3D.Unit.PLUS_X, Vector3D.Unit.PLUS_Y, Vector3D.Unit.MINUS_X));
1333 Assertions.assertThrows(IllegalArgumentException.class, () -> QuaternionRotation.createBasisRotation(
1334 Vector3D.Unit.PLUS_X, Vector3D.Unit.PLUS_Y, Vector3D.Unit.PLUS_Y, Vector3D.Unit.MINUS_Y));
1335 }
1336
1337 @Test
1338 void testFromEulerAngles_identity() {
1339 for (final AxisSequence axes : AxisSequence.values()) {
1340
1341
1342 assertRotationEquals(StandardRotations.IDENTITY,
1343 QuaternionRotation.fromAxisAngleSequence(AxisAngleSequence.createRelative(axes, 0, 0, 0)));
1344 assertRotationEquals(StandardRotations.IDENTITY,
1345 QuaternionRotation.fromAxisAngleSequence(AxisAngleSequence.createRelative(axes, Angle.TWO_PI, Angle.TWO_PI, Angle.TWO_PI)));
1346
1347 assertRotationEquals(StandardRotations.IDENTITY,
1348 QuaternionRotation.fromAxisAngleSequence(AxisAngleSequence.createAbsolute(axes, 0, 0, 0)));
1349 assertRotationEquals(StandardRotations.IDENTITY,
1350 QuaternionRotation.fromAxisAngleSequence(AxisAngleSequence.createAbsolute(axes, Angle.TWO_PI, Angle.TWO_PI, Angle.TWO_PI)));
1351 }
1352 }
1353
1354 @Test
1355 void testFromEulerAngles_relative() {
1356
1357
1358
1359
1360 checkFromAxisAngleSequenceRelative(StandardRotations.PLUS_X_HALF_PI, AxisSequence.XYZ, Angle.PI_OVER_TWO, 0, 0);
1361 checkFromAxisAngleSequenceRelative(StandardRotations.PLUS_Y_HALF_PI, AxisSequence.XYZ, 0, Angle.PI_OVER_TWO, 0);
1362 checkFromAxisAngleSequenceRelative(StandardRotations.PLUS_Z_HALF_PI, AxisSequence.XYZ, 0, 0, Angle.PI_OVER_TWO);
1363 checkFromAxisAngleSequenceRelative(StandardRotations.PLUS_DIAGONAL_TWO_THIRDS_PI, AxisSequence.XYZ, Angle.PI_OVER_TWO, Angle.PI_OVER_TWO, 0);
1364
1365
1366 checkFromAxisAngleSequenceRelative(StandardRotations.PLUS_X_HALF_PI, AxisSequence.XZY, Angle.PI_OVER_TWO, 0, 0);
1367 checkFromAxisAngleSequenceRelative(StandardRotations.PLUS_Y_HALF_PI, AxisSequence.XZY, 0, 0, Angle.PI_OVER_TWO);
1368 checkFromAxisAngleSequenceRelative(StandardRotations.PLUS_Z_HALF_PI, AxisSequence.XZY, 0, Angle.PI_OVER_TWO, 0);
1369 checkFromAxisAngleSequenceRelative(StandardRotations.PLUS_DIAGONAL_TWO_THIRDS_PI, AxisSequence.XZY, Angle.PI_OVER_TWO, 0, Angle.PI_OVER_TWO);
1370
1371
1372 checkFromAxisAngleSequenceRelative(StandardRotations.PLUS_X_HALF_PI, AxisSequence.YXZ, 0, Angle.PI_OVER_TWO, 0);
1373 checkFromAxisAngleSequenceRelative(StandardRotations.PLUS_Y_HALF_PI, AxisSequence.YXZ, Angle.PI_OVER_TWO, 0, 0);
1374 checkFromAxisAngleSequenceRelative(StandardRotations.PLUS_Z_HALF_PI, AxisSequence.YXZ, 0, 0, Angle.PI_OVER_TWO);
1375 checkFromAxisAngleSequenceRelative(StandardRotations.PLUS_DIAGONAL_TWO_THIRDS_PI, AxisSequence.YXZ, Angle.PI_OVER_TWO, 0, Angle.PI_OVER_TWO);
1376
1377
1378 checkFromAxisAngleSequenceRelative(StandardRotations.PLUS_X_HALF_PI, AxisSequence.YZX, 0, 0, Angle.PI_OVER_TWO);
1379 checkFromAxisAngleSequenceRelative(StandardRotations.PLUS_Y_HALF_PI, AxisSequence.YZX, Angle.PI_OVER_TWO, 0, 0);
1380 checkFromAxisAngleSequenceRelative(StandardRotations.PLUS_Z_HALF_PI, AxisSequence.YZX, 0, Angle.PI_OVER_TWO, 0);
1381 checkFromAxisAngleSequenceRelative(StandardRotations.PLUS_DIAGONAL_TWO_THIRDS_PI, AxisSequence.YZX, Angle.PI_OVER_TWO, Angle.PI_OVER_TWO, 0);
1382
1383
1384 checkFromAxisAngleSequenceRelative(StandardRotations.PLUS_X_HALF_PI, AxisSequence.YZX, 0, 0, Angle.PI_OVER_TWO);
1385 checkFromAxisAngleSequenceRelative(StandardRotations.PLUS_Y_HALF_PI, AxisSequence.YZX, Angle.PI_OVER_TWO, 0, 0);
1386 checkFromAxisAngleSequenceRelative(StandardRotations.PLUS_Z_HALF_PI, AxisSequence.YZX, 0, Angle.PI_OVER_TWO, 0);
1387 checkFromAxisAngleSequenceRelative(StandardRotations.PLUS_DIAGONAL_TWO_THIRDS_PI, AxisSequence.YZX, Angle.PI_OVER_TWO, Angle.PI_OVER_TWO, 0);
1388
1389
1390 checkFromAxisAngleSequenceRelative(StandardRotations.PLUS_X_HALF_PI, AxisSequence.ZYX, 0, 0, Angle.PI_OVER_TWO);
1391 checkFromAxisAngleSequenceRelative(StandardRotations.PLUS_Y_HALF_PI, AxisSequence.ZYX, 0, Angle.PI_OVER_TWO, 0);
1392 checkFromAxisAngleSequenceRelative(StandardRotations.PLUS_Z_HALF_PI, AxisSequence.ZYX, Angle.PI_OVER_TWO, 0, 0);
1393 checkFromAxisAngleSequenceRelative(StandardRotations.PLUS_DIAGONAL_TWO_THIRDS_PI, AxisSequence.ZYX, Angle.PI_OVER_TWO, 0, Angle.PI_OVER_TWO);
1394
1395
1396 checkFromAxisAngleSequenceRelative(StandardRotations.PLUS_X_HALF_PI, AxisSequence.XYX, Angle.PI_OVER_TWO, 0, 0);
1397 checkFromAxisAngleSequenceRelative(StandardRotations.PLUS_Y_HALF_PI, AxisSequence.XYX, 0, Angle.PI_OVER_TWO, 0);
1398 checkFromAxisAngleSequenceRelative(StandardRotations.PLUS_Z_HALF_PI, AxisSequence.XYX, Angle.PI_OVER_TWO, Angle.PI_OVER_TWO, -Angle.PI_OVER_TWO);
1399 checkFromAxisAngleSequenceRelative(StandardRotations.PLUS_DIAGONAL_TWO_THIRDS_PI, AxisSequence.XYX, Angle.PI_OVER_TWO, Angle.PI_OVER_TWO, 0);
1400
1401
1402 checkFromAxisAngleSequenceRelative(StandardRotations.PLUS_X_HALF_PI, AxisSequence.XZX, Angle.PI_OVER_TWO, 0, 0);
1403 checkFromAxisAngleSequenceRelative(StandardRotations.PLUS_Y_HALF_PI, AxisSequence.XZX, -Angle.PI_OVER_TWO, Angle.PI_OVER_TWO, Angle.PI_OVER_TWO);
1404 checkFromAxisAngleSequenceRelative(StandardRotations.PLUS_Z_HALF_PI, AxisSequence.XZX, 0, Angle.PI_OVER_TWO, 0);
1405 checkFromAxisAngleSequenceRelative(StandardRotations.PLUS_DIAGONAL_TWO_THIRDS_PI, AxisSequence.XZX, 0, Angle.PI_OVER_TWO, Angle.PI_OVER_TWO);
1406
1407
1408 checkFromAxisAngleSequenceRelative(StandardRotations.PLUS_X_HALF_PI, AxisSequence.YXY, 0, Angle.PI_OVER_TWO, 0);
1409 checkFromAxisAngleSequenceRelative(StandardRotations.PLUS_Y_HALF_PI, AxisSequence.YXY, Angle.PI_OVER_TWO, 0, 0);
1410 checkFromAxisAngleSequenceRelative(StandardRotations.PLUS_Z_HALF_PI, AxisSequence.YXY, -Angle.PI_OVER_TWO, Angle.PI_OVER_TWO, Angle.PI_OVER_TWO);
1411 checkFromAxisAngleSequenceRelative(StandardRotations.PLUS_DIAGONAL_TWO_THIRDS_PI, AxisSequence.YXY, 0, Angle.PI_OVER_TWO, Angle.PI_OVER_TWO);
1412
1413
1414 checkFromAxisAngleSequenceRelative(StandardRotations.PLUS_X_HALF_PI, AxisSequence.YZY, -Angle.PI_OVER_TWO, -Angle.PI_OVER_TWO, Angle.PI_OVER_TWO);
1415 checkFromAxisAngleSequenceRelative(StandardRotations.PLUS_Y_HALF_PI, AxisSequence.YZY, Angle.PI_OVER_TWO, 0, 0);
1416 checkFromAxisAngleSequenceRelative(StandardRotations.PLUS_Z_HALF_PI, AxisSequence.YZY, 0, Angle.PI_OVER_TWO, 0);
1417 checkFromAxisAngleSequenceRelative(StandardRotations.PLUS_DIAGONAL_TWO_THIRDS_PI, AxisSequence.YZY, Angle.PI_OVER_TWO, Angle.PI_OVER_TWO, 0);
1418
1419
1420 checkFromAxisAngleSequenceRelative(StandardRotations.PLUS_X_HALF_PI, AxisSequence.ZXZ, 0, Angle.PI_OVER_TWO, 0);
1421 checkFromAxisAngleSequenceRelative(StandardRotations.PLUS_Y_HALF_PI, AxisSequence.ZXZ, Angle.PI_OVER_TWO, Angle.PI_OVER_TWO, -Angle.PI_OVER_TWO);
1422 checkFromAxisAngleSequenceRelative(StandardRotations.PLUS_Z_HALF_PI, AxisSequence.ZXZ, Angle.PI_OVER_TWO, 0, 0);
1423 checkFromAxisAngleSequenceRelative(StandardRotations.PLUS_DIAGONAL_TWO_THIRDS_PI, AxisSequence.ZXZ, Angle.PI_OVER_TWO, Angle.PI_OVER_TWO, 0);
1424
1425
1426 checkFromAxisAngleSequenceRelative(StandardRotations.PLUS_X_HALF_PI, AxisSequence.ZYZ, Angle.PI_OVER_TWO, -Angle.PI_OVER_TWO, -Angle.PI_OVER_TWO);
1427 checkFromAxisAngleSequenceRelative(StandardRotations.PLUS_Y_HALF_PI, AxisSequence.ZYZ, 0, Angle.PI_OVER_TWO, 0);
1428 checkFromAxisAngleSequenceRelative(StandardRotations.PLUS_Z_HALF_PI, AxisSequence.ZYZ, Angle.PI_OVER_TWO, 0, 0);
1429 checkFromAxisAngleSequenceRelative(StandardRotations.PLUS_DIAGONAL_TWO_THIRDS_PI, AxisSequence.ZYZ, 0, Angle.PI_OVER_TWO, Angle.PI_OVER_TWO);
1430 }
1431
1432
1433
1434
1435
1436
1437
1438
1439
1440 private void checkFromAxisAngleSequenceRelative(final UnaryOperator<Vector3D> rotation, final AxisSequence axes, final double angle1, final double angle2, final double angle3) {
1441 final AxisAngleSequence angles = AxisAngleSequence.createRelative(axes, angle1, angle2, angle3);
1442
1443 assertRotationEquals(rotation, QuaternionRotation.fromAxisAngleSequence(angles));
1444 }
1445
1446 @Test
1447 void testFromEulerAngles_absolute() {
1448
1449
1450
1451
1452 checkFromAxisAngleSequenceAbsolute(StandardRotations.PLUS_X_HALF_PI, AxisSequence.XYZ, Angle.PI_OVER_TWO, 0, 0);
1453 checkFromAxisAngleSequenceAbsolute(StandardRotations.PLUS_Y_HALF_PI, AxisSequence.XYZ, 0, Angle.PI_OVER_TWO, 0);
1454 checkFromAxisAngleSequenceAbsolute(StandardRotations.PLUS_Z_HALF_PI, AxisSequence.XYZ, 0, 0, Angle.PI_OVER_TWO);
1455 checkFromAxisAngleSequenceAbsolute(StandardRotations.PLUS_DIAGONAL_TWO_THIRDS_PI, AxisSequence.XYZ, Angle.PI_OVER_TWO, 0, Angle.PI_OVER_TWO);
1456
1457
1458 checkFromAxisAngleSequenceAbsolute(StandardRotations.PLUS_X_HALF_PI, AxisSequence.XZY, Angle.PI_OVER_TWO, 0, 0);
1459 checkFromAxisAngleSequenceAbsolute(StandardRotations.PLUS_Y_HALF_PI, AxisSequence.XZY, 0, 0, Angle.PI_OVER_TWO);
1460 checkFromAxisAngleSequenceAbsolute(StandardRotations.PLUS_Z_HALF_PI, AxisSequence.XZY, 0, Angle.PI_OVER_TWO, 0);
1461 checkFromAxisAngleSequenceAbsolute(StandardRotations.PLUS_DIAGONAL_TWO_THIRDS_PI, AxisSequence.XZY, Angle.PI_OVER_TWO, Angle.PI_OVER_TWO, 0);
1462
1463
1464 checkFromAxisAngleSequenceAbsolute(StandardRotations.PLUS_X_HALF_PI, AxisSequence.YXZ, 0, Angle.PI_OVER_TWO, 0);
1465 checkFromAxisAngleSequenceAbsolute(StandardRotations.PLUS_Y_HALF_PI, AxisSequence.YXZ, Angle.PI_OVER_TWO, 0, 0);
1466 checkFromAxisAngleSequenceAbsolute(StandardRotations.PLUS_Z_HALF_PI, AxisSequence.YXZ, 0, 0, Angle.PI_OVER_TWO);
1467 checkFromAxisAngleSequenceAbsolute(StandardRotations.PLUS_DIAGONAL_TWO_THIRDS_PI, AxisSequence.YXZ, Angle.PI_OVER_TWO, Angle.PI_OVER_TWO, 0);
1468
1469
1470 checkFromAxisAngleSequenceAbsolute(StandardRotations.PLUS_X_HALF_PI, AxisSequence.YZX, 0, 0, Angle.PI_OVER_TWO);
1471 checkFromAxisAngleSequenceAbsolute(StandardRotations.PLUS_Y_HALF_PI, AxisSequence.YZX, Angle.PI_OVER_TWO, 0, 0);
1472 checkFromAxisAngleSequenceAbsolute(StandardRotations.PLUS_Z_HALF_PI, AxisSequence.YZX, 0, Angle.PI_OVER_TWO, 0);
1473 checkFromAxisAngleSequenceAbsolute(StandardRotations.PLUS_DIAGONAL_TWO_THIRDS_PI, AxisSequence.YZX, Angle.PI_OVER_TWO, 0, Angle.PI_OVER_TWO);
1474
1475
1476 checkFromAxisAngleSequenceAbsolute(StandardRotations.PLUS_X_HALF_PI, AxisSequence.YZX, 0, 0, Angle.PI_OVER_TWO);
1477 checkFromAxisAngleSequenceAbsolute(StandardRotations.PLUS_Y_HALF_PI, AxisSequence.YZX, Angle.PI_OVER_TWO, 0, 0);
1478 checkFromAxisAngleSequenceAbsolute(StandardRotations.PLUS_Z_HALF_PI, AxisSequence.YZX, 0, Angle.PI_OVER_TWO, 0);
1479 checkFromAxisAngleSequenceAbsolute(StandardRotations.PLUS_DIAGONAL_TWO_THIRDS_PI, AxisSequence.YZX, Angle.PI_OVER_TWO, 0, Angle.PI_OVER_TWO);
1480
1481
1482 checkFromAxisAngleSequenceAbsolute(StandardRotations.PLUS_X_HALF_PI, AxisSequence.ZYX, 0, 0, Angle.PI_OVER_TWO);
1483 checkFromAxisAngleSequenceAbsolute(StandardRotations.PLUS_Y_HALF_PI, AxisSequence.ZYX, 0, Angle.PI_OVER_TWO, 0);
1484 checkFromAxisAngleSequenceAbsolute(StandardRotations.PLUS_Z_HALF_PI, AxisSequence.ZYX, Angle.PI_OVER_TWO, 0, 0);
1485 checkFromAxisAngleSequenceAbsolute(StandardRotations.PLUS_DIAGONAL_TWO_THIRDS_PI, AxisSequence.ZYX, Angle.PI_OVER_TWO, Angle.PI_OVER_TWO, 0);
1486
1487
1488 checkFromAxisAngleSequenceAbsolute(StandardRotations.PLUS_X_HALF_PI, AxisSequence.XYX, Angle.PI_OVER_TWO, 0, 0);
1489 checkFromAxisAngleSequenceAbsolute(StandardRotations.PLUS_Y_HALF_PI, AxisSequence.XYX, 0, Angle.PI_OVER_TWO, 0);
1490 checkFromAxisAngleSequenceAbsolute(StandardRotations.PLUS_Z_HALF_PI, AxisSequence.XYX, Angle.PI_OVER_TWO, -Angle.PI_OVER_TWO, -Angle.PI_OVER_TWO);
1491 checkFromAxisAngleSequenceAbsolute(StandardRotations.PLUS_DIAGONAL_TWO_THIRDS_PI, AxisSequence.XYX, 0, Angle.PI_OVER_TWO, Angle.PI_OVER_TWO);
1492
1493
1494 checkFromAxisAngleSequenceAbsolute(StandardRotations.PLUS_X_HALF_PI, AxisSequence.XZX, Angle.PI_OVER_TWO, 0, 0);
1495 checkFromAxisAngleSequenceAbsolute(StandardRotations.PLUS_Y_HALF_PI, AxisSequence.XZX, -Angle.PI_OVER_TWO, -Angle.PI_OVER_TWO, Angle.PI_OVER_TWO);
1496 checkFromAxisAngleSequenceAbsolute(StandardRotations.PLUS_Z_HALF_PI, AxisSequence.XZX, 0, Angle.PI_OVER_TWO, 0);
1497 checkFromAxisAngleSequenceAbsolute(StandardRotations.PLUS_DIAGONAL_TWO_THIRDS_PI, AxisSequence.XZX, Angle.PI_OVER_TWO, Angle.PI_OVER_TWO, 0);
1498
1499
1500 checkFromAxisAngleSequenceAbsolute(StandardRotations.PLUS_X_HALF_PI, AxisSequence.YXY, 0, Angle.PI_OVER_TWO, 0);
1501 checkFromAxisAngleSequenceAbsolute(StandardRotations.PLUS_Y_HALF_PI, AxisSequence.YXY, Angle.PI_OVER_TWO, 0, 0);
1502 checkFromAxisAngleSequenceAbsolute(StandardRotations.PLUS_Z_HALF_PI, AxisSequence.YXY, -Angle.PI_OVER_TWO, -Angle.PI_OVER_TWO, Angle.PI_OVER_TWO);
1503 checkFromAxisAngleSequenceAbsolute(StandardRotations.PLUS_DIAGONAL_TWO_THIRDS_PI, AxisSequence.YXY, Angle.PI_OVER_TWO, Angle.PI_OVER_TWO, 0);
1504
1505
1506 checkFromAxisAngleSequenceAbsolute(StandardRotations.PLUS_X_HALF_PI, AxisSequence.YZY, -Angle.PI_OVER_TWO, Angle.PI_OVER_TWO, Angle.PI_OVER_TWO);
1507 checkFromAxisAngleSequenceAbsolute(StandardRotations.PLUS_Y_HALF_PI, AxisSequence.YZY, Angle.PI_OVER_TWO, 0, 0);
1508 checkFromAxisAngleSequenceAbsolute(StandardRotations.PLUS_Z_HALF_PI, AxisSequence.YZY, 0, Angle.PI_OVER_TWO, 0);
1509 checkFromAxisAngleSequenceAbsolute(StandardRotations.PLUS_DIAGONAL_TWO_THIRDS_PI, AxisSequence.YZY, 0, Angle.PI_OVER_TWO, Angle.PI_OVER_TWO);
1510
1511
1512 checkFromAxisAngleSequenceAbsolute(StandardRotations.PLUS_X_HALF_PI, AxisSequence.ZXZ, 0, Angle.PI_OVER_TWO, 0);
1513 checkFromAxisAngleSequenceAbsolute(StandardRotations.PLUS_Y_HALF_PI, AxisSequence.ZXZ, -Angle.PI_OVER_TWO, Angle.PI_OVER_TWO, Angle.PI_OVER_TWO);
1514 checkFromAxisAngleSequenceAbsolute(StandardRotations.PLUS_Z_HALF_PI, AxisSequence.ZXZ, Angle.PI_OVER_TWO, 0, 0);
1515 checkFromAxisAngleSequenceAbsolute(StandardRotations.PLUS_DIAGONAL_TWO_THIRDS_PI, AxisSequence.ZXZ, 0, Angle.PI_OVER_TWO, Angle.PI_OVER_TWO);
1516
1517
1518 checkFromAxisAngleSequenceAbsolute(StandardRotations.PLUS_X_HALF_PI, AxisSequence.ZYZ, Angle.PI_OVER_TWO, Angle.PI_OVER_TWO, -Angle.PI_OVER_TWO);
1519 checkFromAxisAngleSequenceAbsolute(StandardRotations.PLUS_Y_HALF_PI, AxisSequence.ZYZ, 0, Angle.PI_OVER_TWO, 0);
1520 checkFromAxisAngleSequenceAbsolute(StandardRotations.PLUS_Z_HALF_PI, AxisSequence.ZYZ, Angle.PI_OVER_TWO, 0, 0);
1521 checkFromAxisAngleSequenceAbsolute(StandardRotations.PLUS_DIAGONAL_TWO_THIRDS_PI, AxisSequence.ZYZ, Angle.PI_OVER_TWO, Angle.PI_OVER_TWO, 0);
1522 }
1523
1524
1525
1526
1527
1528
1529
1530
1531
1532 private void checkFromAxisAngleSequenceAbsolute(final UnaryOperator<Vector3D> rotation, final AxisSequence axes, final double angle1, final double angle2, final double angle3) {
1533 final AxisAngleSequence angles = AxisAngleSequence.createAbsolute(axes, angle1, angle2, angle3);
1534
1535 assertRotationEquals(rotation, QuaternionRotation.fromAxisAngleSequence(angles));
1536 }
1537
1538 private static void checkQuaternion(final QuaternionRotation qrot, final double w, final double x, final double y, final double z) {
1539 final String msg = "Expected" +
1540 " quaternion to equal " + SimpleTupleFormat.getDefault().format(w, x, y, z) + " but was " + qrot;
1541
1542 Assertions.assertEquals(w, qrot.getQuaternion().getW(), EPS, msg);
1543 Assertions.assertEquals(x, qrot.getQuaternion().getX(), EPS, msg);
1544 Assertions.assertEquals(y, qrot.getQuaternion().getY(), EPS, msg);
1545 Assertions.assertEquals(z, qrot.getQuaternion().getZ(), EPS, msg);
1546
1547 final Quaternion q = qrot.getQuaternion();
1548 Assertions.assertEquals(w, q.getW(), EPS, msg);
1549 Assertions.assertEquals(x, q.getX(), EPS, msg);
1550 Assertions.assertEquals(y, q.getY(), EPS, msg);
1551 Assertions.assertEquals(z, q.getZ(), EPS, msg);
1552
1553 Assertions.assertTrue(qrot.preservesOrientation());
1554 }
1555
1556 private static void checkVector(final Vector3D v, final double x, final double y, final double z) {
1557 final String msg = "Expected vector to equal " + SimpleTupleFormat.getDefault().format(x, y, z) + " but was " + v;
1558
1559 Assertions.assertEquals(x, v.getX(), EPS, msg);
1560 Assertions.assertEquals(y, v.getY(), EPS, msg);
1561 Assertions.assertEquals(z, v.getZ(), EPS, msg);
1562 }
1563
1564
1565
1566
1567
1568 private static void assertRadiansEquals(final double expected, final double actual) {
1569 final double diff = Angle.Rad.WITHIN_MINUS_PI_AND_PI.applyAsDouble(expected - actual);
1570 final String msg = "Expected " + actual + " radians to be equivalent to " + expected + " radians; difference is " + diff;
1571
1572 Assertions.assertTrue(Math.abs(diff) < 1e-6, msg);
1573 }
1574
1575
1576
1577
1578
1579
1580 private static void assertRotationEquals(final UnaryOperator<Vector3D> expected, final QuaternionRotation rotation) {
1581 assertFnEquals(expected, rotation);
1582 }
1583
1584
1585
1586
1587
1588
1589 private static void assertTransformEquals(final UnaryOperator<Vector3D> expected, final AffineTransformMatrix3D transform) {
1590 assertFnEquals(expected, transform);
1591 }
1592
1593
1594
1595
1596
1597
1598 private static void assertFnEquals(final UnaryOperator<Vector3D> expectedFn, final UnaryOperator<Vector3D> actualFn) {
1599 EuclideanTestUtils.permute(-2, 2, 0.25, (x, y, z) -> {
1600 final Vector3D input = Vector3D.of(x, y, z);
1601
1602 final Vector3D expected = expectedFn.apply(input);
1603 final Vector3D actual = actualFn.apply(input);
1604
1605 final String msg = "Expected vector " + input + " to be transformed to " + expected + " but was " + actual;
1606
1607 Assertions.assertEquals(expected.getX(), actual.getX(), EPS, msg);
1608 Assertions.assertEquals(expected.getY(), actual.getY(), EPS, msg);
1609 Assertions.assertEquals(expected.getZ(), actual.getZ(), EPS, msg);
1610 });
1611 }
1612 }