1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17 package org.apache.commons.geometry.euclidean.threed;
18
19 import java.util.function.UnaryOperator;
20
21 import org.apache.commons.geometry.core.GeometryTestUtils;
22 import org.apache.commons.geometry.euclidean.EuclideanTestUtils;
23 import org.apache.commons.geometry.euclidean.EuclideanTestUtils.PermuteCallback3D;
24 import org.apache.commons.geometry.euclidean.threed.rotation.QuaternionRotation;
25 import org.apache.commons.geometry.euclidean.threed.rotation.StandardRotations;
26 import org.apache.commons.numbers.angle.Angle;
27 import org.apache.commons.numbers.core.Precision;
28 import org.junit.jupiter.api.Assertions;
29 import org.junit.jupiter.api.Test;
30
31 class AffineTransformMatrix3DTest {
32
33 private static final double EPS = 1e-12;
34
35 private static final Precision.DoubleEquivalence TEST_PRECISION =
36 Precision.doubleEquivalenceOfEpsilon(EPS);
37
38 @Test
39 void testOf() {
40
41 final double[] arr = {
42 1, 2, 3, 4,
43 5, 6, 7, 8,
44 9, 10, 11, 12
45 };
46
47
48 final AffineTransformMatrix3D transform = AffineTransformMatrix3D.of(arr);
49
50
51 final double[] result = transform.toArray();
52 Assertions.assertNotSame(arr, result);
53 Assertions.assertArrayEquals(arr, result, 0.0);
54 }
55
56 @Test
57 void testOf_invalidDimensions() {
58
59 GeometryTestUtils.assertThrowsWithMessage(() -> AffineTransformMatrix3D.of(1, 2),
60 IllegalArgumentException.class, "Dimension mismatch: 2 != 12");
61 }
62
63 @Test
64 void testFromColumnVectors_threeVectors() {
65
66 final Vector3D u = Vector3D.of(1, 2, 3);
67 final Vector3D v = Vector3D.of(4, 5, 6);
68 final Vector3D w = Vector3D.of(7, 8, 9);
69
70
71 final AffineTransformMatrix3D transform = AffineTransformMatrix3D.fromColumnVectors(u, v, w);
72
73
74 Assertions.assertArrayEquals(new double[] {
75 1, 4, 7, 0,
76 2, 5, 8, 0,
77 3, 6, 9, 0
78 }, transform.toArray(), 0.0);
79 }
80
81 @Test
82 void testFromColumnVectors_fourVectors() {
83
84 final Vector3D u = Vector3D.of(1, 2, 3);
85 final Vector3D v = Vector3D.of(4, 5, 6);
86 final Vector3D w = Vector3D.of(7, 8, 9);
87 final Vector3D t = Vector3D.of(10, 11, 12);
88
89
90 final AffineTransformMatrix3D transform = AffineTransformMatrix3D.fromColumnVectors(u, v, w, t);
91
92
93 Assertions.assertArrayEquals(new double[] {
94 1, 4, 7, 10,
95 2, 5, 8, 11,
96 3, 6, 9, 12
97 }, transform.toArray(), 0.0);
98 }
99
100 @Test
101 void testFrom() {
102
103 Assertions.assertArrayEquals(new double[] {
104 1, 0, 0, 0,
105 0, 1, 0, 0,
106 0, 0, 1, 0
107 }, AffineTransformMatrix3D.from(UnaryOperator.identity()).toArray(), EPS);
108 Assertions.assertArrayEquals(new double[] {
109 1, 0, 0, 2,
110 0, 1, 0, 3,
111 0, 0, 1, -4
112 }, AffineTransformMatrix3D.from(v -> v.add(Vector3D.of(2, 3, -4))).toArray(), EPS);
113 Assertions.assertArrayEquals(new double[] {
114 3, 0, 0, 0,
115 0, 3, 0, 0,
116 0, 0, 3, 0
117 }, AffineTransformMatrix3D.from(v -> v.multiply(3)).toArray(), EPS);
118 Assertions.assertArrayEquals(new double[] {
119 3, 0, 0, 6,
120 0, 3, 0, 9,
121 0, 0, 3, 12
122 }, AffineTransformMatrix3D.from(v -> v.add(Vector3D.of(2, 3, 4)).multiply(3)).toArray(), EPS);
123 }
124
125 @Test
126 void testFrom_invalidFunction() {
127
128 Assertions.assertThrows(IllegalArgumentException.class, () -> AffineTransformMatrix3D.from(v -> v.multiply(0)));
129 }
130
131 @Test
132 void testIdentity() {
133
134 final AffineTransformMatrix3D transform = AffineTransformMatrix3D.identity();
135
136
137 final double[] expected = {
138 1, 0, 0, 0,
139 0, 1, 0, 0,
140 0, 0, 1, 0
141 };
142 Assertions.assertArrayEquals(expected, transform.toArray(), 0.0);
143 }
144
145 @Test
146 void testCreateTranslation_xyz() {
147
148 final AffineTransformMatrix3D transform = AffineTransformMatrix3D.createTranslation(2, 3, 4);
149
150
151 final double[] expected = {
152 1, 0, 0, 2,
153 0, 1, 0, 3,
154 0, 0, 1, 4
155 };
156 Assertions.assertArrayEquals(expected, transform.toArray(), 0.0);
157 }
158
159 @Test
160 void testCreateTranslation_vector() {
161
162 final AffineTransformMatrix3D transform = AffineTransformMatrix3D.createTranslation(Vector3D.of(5, 6, 7));
163
164
165 final double[] expected = {
166 1, 0, 0, 5,
167 0, 1, 0, 6,
168 0, 0, 1, 7
169 };
170 Assertions.assertArrayEquals(expected, transform.toArray(), 0.0);
171 }
172
173 @Test
174 void testCreateScale_xyz() {
175
176 final AffineTransformMatrix3D transform = AffineTransformMatrix3D.createScale(2, 3, 4);
177
178
179 final double[] expected = {
180 2, 0, 0, 0,
181 0, 3, 0, 0,
182 0, 0, 4, 0
183 };
184 Assertions.assertArrayEquals(expected, transform.toArray(), 0.0);
185 }
186
187 @Test
188 void testTranslate_xyz() {
189
190 final AffineTransformMatrix3D a = AffineTransformMatrix3D.of(
191 2, 0, 0, 10,
192 0, 3, 0, 11,
193 0, 0, 4, 12
194 );
195
196
197 final AffineTransformMatrix3D result = a.translate(4, 5, 6);
198
199
200 final double[] expected = {
201 2, 0, 0, 14,
202 0, 3, 0, 16,
203 0, 0, 4, 18
204 };
205 Assertions.assertArrayEquals(expected, result.toArray(), 0.0);
206 }
207
208 @Test
209 void testTranslate_vector() {
210
211 final AffineTransformMatrix3D a = AffineTransformMatrix3D.of(
212 2, 0, 0, 10,
213 0, 3, 0, 11,
214 0, 0, 4, 12
215 );
216
217
218 final AffineTransformMatrix3D result = a.translate(Vector3D.of(7, 8, 9));
219
220
221 final double[] expected = {
222 2, 0, 0, 17,
223 0, 3, 0, 19,
224 0, 0, 4, 21
225 };
226 Assertions.assertArrayEquals(expected, result.toArray(), 0.0);
227 }
228
229 @Test
230 void testCreateScale_vector() {
231
232 final AffineTransformMatrix3D transform = AffineTransformMatrix3D.createScale(Vector3D.of(4, 5, 6));
233
234
235 final double[] expected = {
236 4, 0, 0, 0,
237 0, 5, 0, 0,
238 0, 0, 6, 0
239 };
240 Assertions.assertArrayEquals(expected, transform.toArray(), 0.0);
241 }
242
243 @Test
244 void testCreateScale_singleValue() {
245
246 final AffineTransformMatrix3D transform = AffineTransformMatrix3D.createScale(7);
247
248
249 final double[] expected = {
250 7, 0, 0, 0,
251 0, 7, 0, 0,
252 0, 0, 7, 0
253 };
254 Assertions.assertArrayEquals(expected, transform.toArray(), 0.0);
255 }
256
257 @Test
258 void testScale_xyz() {
259
260 final AffineTransformMatrix3D a = AffineTransformMatrix3D.of(
261 2, 0, 0, 10,
262 0, 3, 0, 11,
263 0, 0, 4, 12
264 );
265
266
267 final AffineTransformMatrix3D result = a.scale(4, 5, 6);
268
269
270 final double[] expected = {
271 8, 0, 0, 40,
272 0, 15, 0, 55,
273 0, 0, 24, 72
274 };
275 Assertions.assertArrayEquals(expected, result.toArray(), 0.0);
276 }
277
278 @Test
279 void testScale_vector() {
280
281 final AffineTransformMatrix3D a = AffineTransformMatrix3D.of(
282 2, 0, 0, 10,
283 0, 3, 0, 11,
284 0, 0, 4, 12
285 );
286
287
288 final AffineTransformMatrix3D result = a.scale(Vector3D.of(7, 8, 9));
289
290
291 final double[] expected = {
292 14, 0, 0, 70,
293 0, 24, 0, 88,
294 0, 0, 36, 108
295 };
296 Assertions.assertArrayEquals(expected, result.toArray(), 0.0);
297 }
298
299 @Test
300 void testScale_singleValue() {
301
302 final AffineTransformMatrix3D a = AffineTransformMatrix3D.of(
303 2, 0, 0, 10,
304 0, 3, 0, 11,
305 0, 0, 4, 12
306 );
307
308
309 final AffineTransformMatrix3D result = a.scale(10);
310
311
312 final double[] expected = {
313 20, 0, 0, 100,
314 0, 30, 0, 110,
315 0, 0, 40, 120
316 };
317 Assertions.assertArrayEquals(expected, result.toArray(), 0.0);
318 }
319
320 @Test
321 void testCreateRotation() {
322
323 final Vector3D center = Vector3D.of(1, 2, 3);
324 final QuaternionRotation rotation = QuaternionRotation.fromAxisAngle(Vector3D.Unit.PLUS_Z, Angle.PI_OVER_TWO);
325
326
327 final AffineTransformMatrix3D result = AffineTransformMatrix3D.createRotation(center, rotation);
328
329
330 final double[] expected = {
331 0, -1, 0, 3,
332 1, 0, 0, 1,
333 0, 0, 1, 0
334 };
335 Assertions.assertArrayEquals(expected, result.toArray(), EPS);
336 }
337
338 @Test
339 void testRotate() {
340
341 final AffineTransformMatrix3D a = AffineTransformMatrix3D.of(
342 1, 2, 3, 4,
343 5, 6, 7, 8,
344 9, 10, 11, 12
345 );
346
347 final QuaternionRotation rotation = QuaternionRotation.fromAxisAngle(Vector3D.Unit.PLUS_Z, Angle.PI_OVER_TWO);
348
349
350 final AffineTransformMatrix3D result = a.rotate(rotation);
351
352
353 final double[] expected = {
354 -5, -6, -7, -8,
355 1, 2, 3, 4,
356 9, 10, 11, 12
357 };
358 Assertions.assertArrayEquals(expected, result.toArray(), EPS);
359 }
360
361 @Test
362 void testRotate_aroundCenter() {
363
364 final AffineTransformMatrix3D a = AffineTransformMatrix3D.of(
365 1, 2, 3, 4,
366 5, 6, 7, 8,
367 9, 10, 11, 12
368 );
369
370 final Vector3D center = Vector3D.of(1, 2, 3);
371 final QuaternionRotation rotation = QuaternionRotation.fromAxisAngle(Vector3D.Unit.PLUS_Z, Angle.PI_OVER_TWO);
372
373
374 final AffineTransformMatrix3D result = a.rotate(center, rotation);
375
376
377 final double[] expected = {
378 -5, -6, -7, -5,
379 1, 2, 3, 5,
380 9, 10, 11, 12
381 };
382 Assertions.assertArrayEquals(expected, result.toArray(), EPS);
383 }
384
385 @Test
386 void testApply_identity() {
387
388 final AffineTransformMatrix3D transform = AffineTransformMatrix3D.identity();
389
390
391 runWithCoordinates((x, y, z) -> {
392 final Vector3D v = Vector3D.of(x, y, z);
393
394 EuclideanTestUtils.assertCoordinatesEqual(v, transform.apply(v), EPS);
395 });
396 }
397
398 @Test
399 void testApply_translate() {
400
401 final Vector3D translation = Vector3D.of(1.1, -Math.PI, 5.5);
402
403 final AffineTransformMatrix3D transform = AffineTransformMatrix3D.identity()
404 .translate(translation);
405
406
407 runWithCoordinates((x, y, z) -> {
408 final Vector3D vec = Vector3D.of(x, y, z);
409
410 final Vector3D expectedVec = vec.add(translation);
411
412 EuclideanTestUtils.assertCoordinatesEqual(expectedVec, transform.apply(vec), EPS);
413 });
414 }
415
416 @Test
417 void testApply_scale() {
418
419 final Vector3D factors = Vector3D.of(2.0, -3.0, 4.0);
420
421 final AffineTransformMatrix3D transform = AffineTransformMatrix3D.identity()
422 .scale(factors);
423
424
425 runWithCoordinates((x, y, z) -> {
426 final Vector3D vec = Vector3D.of(x, y, z);
427
428 final Vector3D expectedVec = Vector3D.of(factors.getX() * x, factors.getY() * y, factors.getZ() * z);
429
430 EuclideanTestUtils.assertCoordinatesEqual(expectedVec, transform.apply(vec), EPS);
431 });
432 }
433
434 @Test
435 void testApply_translateThenScale() {
436
437 final Vector3D translation = Vector3D.of(-2.0, -3.0, -4.0);
438 final Vector3D scale = Vector3D.of(5.0, 6.0, 7.0);
439
440 final AffineTransformMatrix3D transform = AffineTransformMatrix3D.identity()
441 .translate(translation)
442 .scale(scale);
443
444
445 EuclideanTestUtils.assertCoordinatesEqual(Vector3D.of(-5, -12, -21), transform.apply(Vector3D.of(1, 1, 1)), EPS);
446
447 runWithCoordinates((x, y, z) -> {
448 final Vector3D vec = Vector3D.of(x, y, z);
449
450 final Vector3D expectedVec = Vector3D.of(
451 (x + translation.getX()) * scale.getX(),
452 (y + translation.getY()) * scale.getY(),
453 (z + translation.getZ()) * scale.getZ()
454 );
455
456 EuclideanTestUtils.assertCoordinatesEqual(expectedVec, transform.apply(vec), EPS);
457 });
458 }
459
460 @Test
461 void testApply_scaleThenTranslate() {
462
463 final Vector3D scale = Vector3D.of(5.0, 6.0, 7.0);
464 final Vector3D translation = Vector3D.of(-2.0, -3.0, -4.0);
465
466 final AffineTransformMatrix3D transform = AffineTransformMatrix3D.identity()
467 .scale(scale)
468 .translate(translation);
469
470
471 runWithCoordinates((x, y, z) -> {
472 final Vector3D vec = Vector3D.of(x, y, z);
473
474 final Vector3D expectedVec = Vector3D.of(
475 (x * scale.getX()) + translation.getX(),
476 (y * scale.getY()) + translation.getY(),
477 (z * scale.getZ()) + translation.getZ()
478 );
479
480 EuclideanTestUtils.assertCoordinatesEqual(expectedVec, transform.apply(vec), EPS);
481 });
482 }
483
484 @Test
485 void testApply_rotate() {
486
487 final QuaternionRotation rotation = QuaternionRotation.fromAxisAngle(Vector3D.of(1, 1, 1), 2.0 * Math.PI / 3.0);
488
489 final AffineTransformMatrix3D transform = AffineTransformMatrix3D.identity().rotate(rotation);
490
491
492 runWithCoordinates((x, y, z) -> {
493 final Vector3D vec = Vector3D.of(x, y, z);
494
495 final Vector3D expectedVec = StandardRotations.PLUS_DIAGONAL_TWO_THIRDS_PI.apply(vec);
496
497 EuclideanTestUtils.assertCoordinatesEqual(expectedVec, transform.apply(vec), EPS);
498 });
499 }
500
501 @Test
502 void testApply_rotate_aroundCenter() {
503
504 final double scaleFactor = 2;
505 final Vector3D center = Vector3D.of(3, -4, 5);
506 final QuaternionRotation rotation = QuaternionRotation.fromAxisAngle(Vector3D.Unit.PLUS_Z, Angle.PI_OVER_TWO);
507
508 final AffineTransformMatrix3D transform = AffineTransformMatrix3D.identity()
509 .scale(scaleFactor)
510 .rotate(center, rotation);
511
512
513 EuclideanTestUtils.assertCoordinatesEqual(Vector3D.of(3, -3, 2), transform.apply(Vector3D.of(2, -2, 1)), EPS);
514
515 runWithCoordinates((x, y, z) -> {
516 final Vector3D vec = Vector3D.of(x, y, z);
517
518 final Vector3D expectedVec = StandardRotations.PLUS_Z_HALF_PI.apply(vec.multiply(scaleFactor).subtract(center)).add(center);
519
520 EuclideanTestUtils.assertCoordinatesEqual(expectedVec, transform.apply(vec), EPS);
521 });
522 }
523
524 @Test
525 void testApplyXYZ() {
526
527 final double scaleFactor = 2;
528 final Vector3D center = Vector3D.of(3, -4, 5);
529 final QuaternionRotation rotation = QuaternionRotation.fromAxisAngle(Vector3D.of(0.5, 1, 1), 2 * Math.PI / 3);
530
531 final AffineTransformMatrix3D transform = AffineTransformMatrix3D.identity()
532 .scale(scaleFactor)
533 .rotate(center, rotation);
534
535
536 runWithCoordinates((x, y, z) -> {
537 final Vector3D vec = Vector3D.of(x, y, z);
538 final Vector3D expectedVec = rotation.apply(vec.multiply(scaleFactor).subtract(center)).add(center);
539
540 Assertions.assertEquals(expectedVec.getX(), transform.applyX(x, y, z), EPS);
541 Assertions.assertEquals(expectedVec.getY(), transform.applyY(x, y, z), EPS);
542 Assertions.assertEquals(expectedVec.getZ(), transform.applyZ(x, y, z), EPS);
543 });
544 }
545
546 @Test
547 void testApplyVector_identity() {
548
549 final AffineTransformMatrix3D transform = AffineTransformMatrix3D.identity();
550
551
552 runWithCoordinates((x, y, z) -> {
553 final Vector3D v = Vector3D.of(x, y, z);
554
555 EuclideanTestUtils.assertCoordinatesEqual(v, transform.applyVector(v), EPS);
556 });
557 }
558
559 @Test
560 void testApplyVector_translate() {
561
562 final Vector3D translation = Vector3D.of(1.1, -Math.PI, 5.5);
563
564 final AffineTransformMatrix3D transform = AffineTransformMatrix3D.identity()
565 .translate(translation);
566
567
568 runWithCoordinates((x, y, z) -> {
569 final Vector3D vec = Vector3D.of(x, y, z);
570
571 EuclideanTestUtils.assertCoordinatesEqual(vec, transform.applyVector(vec), EPS);
572 });
573 }
574
575 @Test
576 void testApplyVector_scale() {
577
578 final Vector3D factors = Vector3D.of(2.0, -3.0, 4.0);
579
580 final AffineTransformMatrix3D transform = AffineTransformMatrix3D.identity()
581 .scale(factors);
582
583
584 runWithCoordinates((x, y, z) -> {
585 final Vector3D vec = Vector3D.of(x, y, z);
586
587 final Vector3D expectedVec = Vector3D.of(factors.getX() * x, factors.getY() * y, factors.getZ() * z);
588
589 EuclideanTestUtils.assertCoordinatesEqual(expectedVec, transform.applyVector(vec), EPS);
590 });
591 }
592
593 @Test
594 void testApplyVector_representsDisplacement() {
595
596 final Vector3D p1 = Vector3D.of(1, 2, 3);
597
598 final AffineTransformMatrix3D transform = AffineTransformMatrix3D.identity()
599 .scale(1.5)
600 .translate(4, 6, 5)
601 .rotate(QuaternionRotation.fromAxisAngle(Vector3D.Unit.PLUS_Z, Angle.PI_OVER_TWO));
602
603
604 runWithCoordinates((x, y, z) -> {
605 final Vector3D p2 = Vector3D.of(x, y, z);
606 final Vector3D input = p1.subtract(p2);
607
608 final Vector3D expected = transform.apply(p1).subtract(transform.apply(p2));
609
610 EuclideanTestUtils.assertCoordinatesEqual(expected, transform.applyVector(input), EPS);
611 });
612 }
613
614 @Test
615 void testApplyVectorXYZ() {
616
617 final Vector3D p1 = Vector3D.of(1, 2, 3);
618
619 final AffineTransformMatrix3D transform = AffineTransformMatrix3D.identity()
620 .scale(1.5)
621 .translate(4, 6, 5)
622 .rotate(QuaternionRotation.fromAxisAngle(Vector3D.of(0.5, 1, 1), Angle.PI_OVER_TWO));
623
624
625 runWithCoordinates((x, y, z) -> {
626 final Vector3D p2 = p1.add(Vector3D.of(x, y, z));
627
628 final Vector3D expected = transform.apply(p1).vectorTo(transform.apply(p2));
629
630 Assertions.assertEquals(expected.getX(), transform.applyVectorX(x, y, z), EPS);
631 Assertions.assertEquals(expected.getY(), transform.applyVectorY(x, y, z), EPS);
632 Assertions.assertEquals(expected.getZ(), transform.applyVectorZ(x, y, z), EPS);
633 });
634 }
635
636 @Test
637 void testApplyDirection_identity() {
638
639 final AffineTransformMatrix3D transform = AffineTransformMatrix3D.identity();
640
641
642 EuclideanTestUtils.permuteSkipZero(-5, 5, 0.5, (x, y, z) -> {
643 final Vector3D v = Vector3D.of(x, y, z);
644
645 EuclideanTestUtils.assertCoordinatesEqual(v.normalize(), transform.applyDirection(v), EPS);
646 });
647 }
648
649 @Test
650 void testApplyDirection_translate() {
651
652 final Vector3D translation = Vector3D.of(1.1, -Math.PI, 5.5);
653
654 final AffineTransformMatrix3D transform = AffineTransformMatrix3D.identity()
655 .translate(translation);
656
657
658 EuclideanTestUtils.permuteSkipZero(-5, 5, 0.5, (x, y, z) -> {
659 final Vector3D vec = Vector3D.of(x, y, z);
660
661 EuclideanTestUtils.assertCoordinatesEqual(vec.normalize(), transform.applyDirection(vec), EPS);
662 });
663 }
664
665 @Test
666 void testApplyDirection_scale() {
667
668 final Vector3D factors = Vector3D.of(2.0, -3.0, 4.0);
669
670 final AffineTransformMatrix3D transform = AffineTransformMatrix3D.identity()
671 .scale(factors);
672
673
674 EuclideanTestUtils.permuteSkipZero(-5, 5, 0.5, (x, y, z) -> {
675 final Vector3D vec = Vector3D.of(x, y, z);
676
677 final Vector3D expectedVec = Vector3D.of(factors.getX() * x, factors.getY() * y, factors.getZ() * z).normalize();
678
679 EuclideanTestUtils.assertCoordinatesEqual(expectedVec, transform.applyDirection(vec), EPS);
680 });
681 }
682
683 @Test
684 void testApplyDirection_representsNormalizedDisplacement() {
685
686 final Vector3D p1 = Vector3D.of(1, 2, 3);
687
688 final AffineTransformMatrix3D transform = AffineTransformMatrix3D.identity()
689 .scale(1.5)
690 .translate(4, 6, 5)
691 .rotate(QuaternionRotation.fromAxisAngle(Vector3D.Unit.PLUS_Z, Angle.PI_OVER_TWO));
692
693
694 runWithCoordinates((x, y, z) -> {
695 final Vector3D p2 = Vector3D.of(x, y, z);
696 final Vector3D input = p1.subtract(p2);
697
698 final Vector3D expected = transform.apply(p1).subtract(transform.apply(p2)).normalize();
699
700 EuclideanTestUtils.assertCoordinatesEqual(expected, transform.applyDirection(input), EPS);
701 });
702 }
703
704 @Test
705 void testApplyDirection_illegalNorm() {
706
707 Assertions.assertThrows(IllegalArgumentException.class, () -> AffineTransformMatrix3D.createScale(1, 0, 1).applyDirection(Vector3D.Unit.PLUS_Y));
708 Assertions.assertThrows(IllegalArgumentException.class, () -> AffineTransformMatrix3D.createScale(2).applyDirection(Vector3D.ZERO));
709 }
710
711 @Test
712 void testMultiply() {
713
714 final AffineTransformMatrix3D a = AffineTransformMatrix3D.of(
715 1, 2, 3, 4,
716 5, 6, 7, 8,
717 9, 10, 11, 12
718 );
719 final AffineTransformMatrix3D b = AffineTransformMatrix3D.of(
720 13, 14, 15, 16,
721 17, 18, 19, 20,
722 21, 22, 23, 24
723 );
724
725
726 final AffineTransformMatrix3D result = a.multiply(b);
727
728
729 final double[] arr = result.toArray();
730 Assertions.assertArrayEquals(new double[] {
731 110, 116, 122, 132,
732 314, 332, 350, 376,
733 518, 548, 578, 620
734 }, arr, EPS);
735 }
736
737 @Test
738 void testDeterminant() {
739
740 Assertions.assertEquals(1.0, AffineTransformMatrix3D.identity().determinant(), EPS);
741 Assertions.assertEquals(1.0, AffineTransformMatrix3D.of(
742 1, 0, 0, 10,
743 0, 1, 0, 11,
744 0, 0, 1, 12
745 ).determinant(), EPS);
746 Assertions.assertEquals(-1.0, AffineTransformMatrix3D.of(
747 -1, 0, 0, 10,
748 0, 1, 0, 11,
749 0, 0, 1, 12
750 ).determinant(), EPS);
751 Assertions.assertEquals(1.0, AffineTransformMatrix3D.of(
752 -1, 0, 0, 10,
753 0, -1, 0, 11,
754 0, 0, 1, 12
755 ).determinant(), EPS);
756 Assertions.assertEquals(-1.0, AffineTransformMatrix3D.of(
757 -1, 0, 0, 10,
758 0, -1, 0, 11,
759 0, 0, -1, 12
760 ).determinant(), EPS);
761 Assertions.assertEquals(49.0, AffineTransformMatrix3D.of(
762 2, -3, 1, 10,
763 2, 0, -1, 11,
764 1, 4, 5, -12
765 ).determinant(), EPS);
766 Assertions.assertEquals(0.0, AffineTransformMatrix3D.of(
767 1, 2, 3, 0,
768 4, 5, 6, 0,
769 7, 8, 9, 0
770 ).determinant(), EPS);
771 }
772
773 @Test
774 void testPreservesOrientation() {
775
776 Assertions.assertTrue(AffineTransformMatrix3D.identity().preservesOrientation());
777 Assertions.assertTrue(AffineTransformMatrix3D.of(
778 1, 0, 0, 10,
779 0, 1, 0, 11,
780 0, 0, 1, 12
781 ).preservesOrientation());
782 Assertions.assertTrue(AffineTransformMatrix3D.of(
783 2, -3, 1, 10,
784 2, 0, -1, 11,
785 1, 4, 5, -12
786 ).preservesOrientation());
787
788 Assertions.assertFalse(AffineTransformMatrix3D.of(
789 -1, 0, 0, 10,
790 0, 1, 0, 11,
791 0, 0, 1, 12
792 ).preservesOrientation());
793
794 Assertions.assertTrue(AffineTransformMatrix3D.of(
795 -1, 0, 0, 10,
796 0, -1, 0, 11,
797 0, 0, 1, 12
798 ).preservesOrientation());
799
800 Assertions.assertFalse(AffineTransformMatrix3D.of(
801 -1, 0, 0, 10,
802 0, -1, 0, 11,
803 0, 0, -1, 12
804 ).preservesOrientation());
805 Assertions.assertFalse(AffineTransformMatrix3D.of(
806 1, 2, 3, 0,
807 4, 5, 6, 0,
808 7, 8, 9, 0
809 ).preservesOrientation());
810 }
811
812 @Test
813 void testMultiply_combinesTransformOperations() {
814
815 final Vector3D translation1 = Vector3D.of(1, 2, 3);
816 final double scale = 2.0;
817 final Vector3D translation2 = Vector3D.of(4, 5, 6);
818
819 final AffineTransformMatrix3D a = AffineTransformMatrix3D.createTranslation(translation1);
820 final AffineTransformMatrix3D b = AffineTransformMatrix3D.createScale(scale);
821 final AffineTransformMatrix3D c = AffineTransformMatrix3D.identity();
822 final AffineTransformMatrix3D d = AffineTransformMatrix3D.createTranslation(translation2);
823
824
825 final AffineTransformMatrix3D transform = d.multiply(c).multiply(b).multiply(a);
826
827
828 runWithCoordinates((x, y, z) -> {
829 final Vector3D vec = Vector3D.of(x, y, z);
830
831 final Vector3D expectedVec = vec
832 .add(translation1)
833 .multiply(scale)
834 .add(translation2);
835
836 EuclideanTestUtils.assertCoordinatesEqual(expectedVec, transform.apply(vec), EPS);
837 });
838 }
839
840 @Test
841 void testPremultiply() {
842
843 final AffineTransformMatrix3D a = AffineTransformMatrix3D.of(
844 1, 2, 3, 4,
845 5, 6, 7, 8,
846 9, 10, 11, 12
847 );
848 final AffineTransformMatrix3D b = AffineTransformMatrix3D.of(
849 13, 14, 15, 16,
850 17, 18, 19, 20,
851 21, 22, 23, 24
852 );
853
854
855 final AffineTransformMatrix3D result = b.premultiply(a);
856
857
858 final double[] arr = result.toArray();
859 Assertions.assertArrayEquals(new double[] {
860 110, 116, 122, 132,
861 314, 332, 350, 376,
862 518, 548, 578, 620
863 }, arr, EPS);
864 }
865
866 @Test
867 void testPremultiply_combinesTransformOperations() {
868
869 final Vector3D translation1 = Vector3D.of(1, 2, 3);
870 final double scale = 2.0;
871 final Vector3D translation2 = Vector3D.of(4, 5, 6);
872
873 final AffineTransformMatrix3D a = AffineTransformMatrix3D.createTranslation(translation1);
874 final AffineTransformMatrix3D b = AffineTransformMatrix3D.createScale(scale);
875 final AffineTransformMatrix3D c = AffineTransformMatrix3D.identity();
876 final AffineTransformMatrix3D d = AffineTransformMatrix3D.createTranslation(translation2);
877
878
879 final AffineTransformMatrix3D transform = a.premultiply(b).premultiply(c).premultiply(d);
880
881
882 runWithCoordinates((x, y, z) -> {
883 final Vector3D vec = Vector3D.of(x, y, z);
884
885 final Vector3D expectedVec = vec
886 .add(translation1)
887 .multiply(scale)
888 .add(translation2);
889
890 EuclideanTestUtils.assertCoordinatesEqual(expectedVec, transform.apply(vec), EPS);
891 });
892 }
893
894 @Test
895 void testInverse_identity() {
896
897 final AffineTransformMatrix3D inverse = AffineTransformMatrix3D.identity().inverse();
898
899
900 final double[] expected = {
901 1, 0, 0, 0,
902 0, 1, 0, 0,
903 0, 0, 1, 0
904 };
905 Assertions.assertArrayEquals(expected, inverse.toArray(), 0.0);
906 }
907
908 @Test
909 void testInverse_multiplyByInverse_producesIdentity() {
910
911 final AffineTransformMatrix3D a = AffineTransformMatrix3D.of(
912 1, 3, 7, 8,
913 2, 4, 9, 12,
914 5, 6, 10, 11
915 );
916
917 final AffineTransformMatrix3D inv = a.inverse();
918
919
920 final AffineTransformMatrix3D result = inv.multiply(a);
921
922
923 final double[] expected = {
924 1, 0, 0, 0,
925 0, 1, 0, 0,
926 0, 0, 1, 0
927 };
928 Assertions.assertArrayEquals(expected, result.toArray(), EPS);
929 }
930
931 @Test
932 void testInverse_translate() {
933
934 final AffineTransformMatrix3D transform = AffineTransformMatrix3D.createTranslation(1, -2, 4);
935
936
937 final AffineTransformMatrix3D inverse = transform.inverse();
938
939
940 final double[] expected = {
941 1, 0, 0, -1,
942 0, 1, 0, 2,
943 0, 0, 1, -4
944 };
945 Assertions.assertArrayEquals(expected, inverse.toArray(), 0.0);
946 }
947
948 @Test
949 void testInverse_scale() {
950
951 final AffineTransformMatrix3D transform = AffineTransformMatrix3D.createScale(10, -2, 4);
952
953
954 final AffineTransformMatrix3D inverse = transform.inverse();
955
956
957 final double[] expected = {
958 0.1, 0, 0, 0,
959 0, -0.5, 0, 0,
960 0, 0, 0.25, 0
961 };
962 Assertions.assertArrayEquals(expected, inverse.toArray(), 0.0);
963 }
964
965 @Test
966 void testInverse_rotate() {
967
968 final Vector3D center = Vector3D.of(1, 2, 3);
969 final QuaternionRotation rotation = QuaternionRotation.fromAxisAngle(Vector3D.Unit.PLUS_Z, Angle.PI_OVER_TWO);
970
971 final AffineTransformMatrix3D transform = AffineTransformMatrix3D.createRotation(center, rotation);
972
973
974 final AffineTransformMatrix3D inverse = transform.inverse();
975
976
977 final double[] expected = {
978 0, 1, 0, -1,
979 -1, 0, 0, 3,
980 0, 0, 1, 0
981 };
982 Assertions.assertArrayEquals(expected, inverse.toArray(), EPS);
983 }
984
985 @Test
986 void testInverse_undoesOriginalTransform() {
987
988 final Vector3D v1 = Vector3D.ZERO;
989 final Vector3D v2 = Vector3D.Unit.PLUS_X;
990 final Vector3D v3 = Vector3D.of(1, 1, 1);
991 final Vector3D v4 = Vector3D.of(-2, 3, 4);
992
993 final Vector3D center = Vector3D.of(1, 2, 3);
994 final QuaternionRotation rotation = QuaternionRotation.fromAxisAngle(Vector3D.of(1, 2, 3), 0.25);
995
996
997 runWithCoordinates((x, y, z) -> {
998 final AffineTransformMatrix3D transform = AffineTransformMatrix3D
999 .createTranslation(x, y, z)
1000 .scale(2, 3, 4)
1001 .rotate(center, rotation)
1002 .translate(x / 3, y / 3, z / 3);
1003
1004 final AffineTransformMatrix3D inverse = transform.inverse();
1005
1006 EuclideanTestUtils.assertCoordinatesEqual(v1, inverse.apply(transform.apply(v1)), EPS);
1007 EuclideanTestUtils.assertCoordinatesEqual(v2, inverse.apply(transform.apply(v2)), EPS);
1008 EuclideanTestUtils.assertCoordinatesEqual(v3, inverse.apply(transform.apply(v3)), EPS);
1009 EuclideanTestUtils.assertCoordinatesEqual(v4, inverse.apply(transform.apply(v4)), EPS);
1010 });
1011 }
1012
1013 @Test
1014 void testInverse_nonInvertible() {
1015
1016 GeometryTestUtils.assertThrowsWithMessage(() -> AffineTransformMatrix3D.of(
1017 0, 0, 0, 0,
1018 0, 0, 0, 0,
1019 0, 0, 0, 0).inverse(), IllegalStateException.class, "Matrix is not invertible; matrix determinant is 0.0");
1020
1021 GeometryTestUtils.assertThrowsWithMessage(() -> AffineTransformMatrix3D.of(
1022 1, 0, 0, 0,
1023 0, 1, 0, 0,
1024 0, 0, Double.NaN, 0).inverse(), IllegalStateException.class, "Matrix is not invertible; matrix determinant is NaN");
1025
1026 GeometryTestUtils.assertThrowsWithMessage(() -> AffineTransformMatrix3D.of(
1027 1, 0, 0, 0,
1028 0, Double.NEGATIVE_INFINITY, 0, 0,
1029 0, 0, 1, 0).inverse(), IllegalStateException.class, "Matrix is not invertible; matrix determinant is NaN");
1030
1031 GeometryTestUtils.assertThrowsWithMessage(() -> AffineTransformMatrix3D.of(
1032 Double.POSITIVE_INFINITY, 0, 0, 0,
1033 0, 1, 0, 0,
1034 0, 0, 1, 0).inverse(), IllegalStateException.class, "Matrix is not invertible; matrix determinant is NaN");
1035
1036 GeometryTestUtils.assertThrowsWithMessage(() -> AffineTransformMatrix3D.of(
1037 1, 0, 0, Double.NaN,
1038 0, 1, 0, 0,
1039 0, 0, 1, 0).inverse(), IllegalStateException.class, "Matrix is not invertible; invalid matrix element: NaN");
1040
1041 GeometryTestUtils.assertThrowsWithMessage(() -> AffineTransformMatrix3D.of(
1042 1, 0, 0, 0,
1043 0, 1, 0, Double.POSITIVE_INFINITY,
1044 0, 0, 1, 0).inverse(), IllegalStateException.class, "Matrix is not invertible; invalid matrix element: Infinity");
1045
1046 GeometryTestUtils.assertThrowsWithMessage(() -> AffineTransformMatrix3D.of(
1047 1, 0, 0, 0,
1048 0, 1, 0, 0,
1049 0, 0, 1, Double.NEGATIVE_INFINITY).inverse(), IllegalStateException.class, "Matrix is not invertible; invalid matrix element: -Infinity");
1050 }
1051
1052 @Test
1053 void testLinear() {
1054
1055 final AffineTransformMatrix3D mat = AffineTransformMatrix3D.of(
1056 2, 3, 4, 5,
1057 6, 7, 8, 9,
1058 10, 11, 12, 13);
1059
1060
1061 final AffineTransformMatrix3D result = mat.linear();
1062
1063
1064 final double[] expected = {
1065 2, 3, 4, 0,
1066 6, 7, 8, 0,
1067 10, 11, 12, 0
1068 };
1069 Assertions.assertArrayEquals(expected, result.toArray(), 0.0);
1070 }
1071
1072 @Test
1073 void testLinearTranspose() {
1074
1075 final AffineTransformMatrix3D mat = AffineTransformMatrix3D.of(
1076 2, 3, 4, 5,
1077 6, 7, 8, 9,
1078 10, 11, 12, 13);
1079
1080
1081 final AffineTransformMatrix3D result = mat.linearTranspose();
1082
1083
1084 final double[] expected = {
1085 2, 6, 10, 0,
1086 3, 7, 11, 0,
1087 4, 8, 12, 0
1088 };
1089 Assertions.assertArrayEquals(expected, result.toArray(), 0.0);
1090 }
1091
1092 @Test
1093 void testNormalTransform() {
1094
1095 checkNormalTransform(AffineTransformMatrix3D.identity());
1096
1097 checkNormalTransform(AffineTransformMatrix3D.createTranslation(2, 3, 4));
1098 checkNormalTransform(AffineTransformMatrix3D.createTranslation(-3, -4, -5));
1099
1100 checkNormalTransform(AffineTransformMatrix3D.createScale(2, 5, 0.5));
1101 checkNormalTransform(AffineTransformMatrix3D.createScale(-3, 4, 2));
1102 checkNormalTransform(AffineTransformMatrix3D.createScale(-0.1, -0.5, 0.8));
1103 checkNormalTransform(AffineTransformMatrix3D.createScale(-2, -5, -8));
1104
1105 final QuaternionRotation rotA = QuaternionRotation.fromAxisAngle(Vector3D.of(2, 3, 4), 0.75 * Math.PI);
1106 final QuaternionRotation rotB = QuaternionRotation.fromAxisAngle(Vector3D.of(-1, 1, -1), 1.75 * Math.PI);
1107
1108 checkNormalTransform(AffineTransformMatrix3D.createRotation(Vector3D.of(1, 1, 1), rotA));
1109 checkNormalTransform(AffineTransformMatrix3D.createRotation(Vector3D.of(-1, -1, -1), rotB));
1110
1111 checkNormalTransform(AffineTransformMatrix3D.createTranslation(2, 3, 4)
1112 .scale(7, 5, 4)
1113 .rotate(rotA));
1114 checkNormalTransform(AffineTransformMatrix3D.createRotation(Vector3D.ZERO, rotB)
1115 .translate(7, 5, 4)
1116 .rotate(rotA)
1117 .scale(2, 3, 0.5));
1118 }
1119
1120 private void checkNormalTransform(final AffineTransformMatrix3D transform) {
1121 final AffineTransformMatrix3D normalTransform = transform.normalTransform();
1122
1123 final Vector3D p1 = Vector3D.of(-0.25, 0.75, 0.5);
1124 final Vector3D p2 = Vector3D.of(0.5, -0.75, 0.25);
1125
1126 final Vector3D t1 = transform.apply(p1);
1127 final Vector3D t2 = transform.apply(p2);
1128
1129 EuclideanTestUtils.permute(-10, 10, 1, (x, y, z) -> {
1130 final Vector3D p3 = Vector3D.of(x, y, z);
1131 final Vector3D n = Planes.fromPoints(p1, p2, p3, TEST_PRECISION).getNormal();
1132
1133 final Vector3D t3 = transform.apply(p3);
1134
1135 final Plane tPlane = transform.preservesOrientation() ?
1136 Planes.fromPoints(t1, t2, t3, TEST_PRECISION) :
1137 Planes.fromPoints(t1, t3, t2, TEST_PRECISION);
1138 final Vector3D expected = tPlane.getNormal();
1139
1140 final Vector3D actual = normalTransform.apply(n).normalize();
1141
1142 EuclideanTestUtils.assertCoordinatesEqual(expected, actual, EPS);
1143 });
1144 }
1145
1146 @Test
1147 void testNormalTransform_nonInvertible() {
1148
1149 Assertions.assertThrows(IllegalStateException.class, () -> AffineTransformMatrix3D.createScale(0).normalTransform());
1150 }
1151
1152 @Test
1153 void testHashCode() {
1154
1155 final double[] values = {
1156 1, 2, 3, 4,
1157 5, 6, 7, 8,
1158 9, 10, 11, 12
1159 };
1160
1161
1162 final int orig = AffineTransformMatrix3D.of(values).hashCode();
1163 final int same = AffineTransformMatrix3D.of(values).hashCode();
1164
1165 Assertions.assertEquals(orig, same);
1166
1167 double[] temp;
1168 for (int i = 0; i < values.length; ++i) {
1169 temp = values.clone();
1170 temp[i] = 0;
1171
1172 final int modified = AffineTransformMatrix3D.of(temp).hashCode();
1173
1174 Assertions.assertNotEquals(orig, modified);
1175 }
1176 }
1177
1178 @Test
1179 void testEquals() {
1180
1181 final double[] values = {
1182 1, 2, 3, 4,
1183 5, 6, 7, 8,
1184 9, 10, 11, 12
1185 };
1186
1187 final AffineTransformMatrix3D a = AffineTransformMatrix3D.of(values);
1188
1189
1190 GeometryTestUtils.assertSimpleEqualsCases(a);
1191
1192 double[] temp;
1193 for (int i = 0; i < values.length; ++i) {
1194 temp = values.clone();
1195 temp[i] = 0;
1196
1197 final AffineTransformMatrix3D modified = AffineTransformMatrix3D.of(temp);
1198
1199 Assertions.assertNotEquals(a, modified);
1200 }
1201 }
1202
1203 @Test
1204 void testEqualsAndHashCode_signedZeroConsistency() {
1205
1206 final double[] arrWithPosZero = {
1207 1.0, 0.0, 0.0, 0.0,
1208 0.0, 1.0, 0.0, 0.0,
1209 0.0, 0.0, 1.0, 0.0,
1210 };
1211 final double[] arrWithNegZero = {
1212 1.0, 0.0, 0.0, 0.0,
1213 0.0, 1.0, 0.0, 0.0,
1214 0.0, 0.0, 1.0, -0.0,
1215 };
1216 final AffineTransformMatrix3D a = AffineTransformMatrix3D.of(arrWithPosZero);
1217 final AffineTransformMatrix3D b = AffineTransformMatrix3D.of(arrWithNegZero);
1218 final AffineTransformMatrix3D c = AffineTransformMatrix3D.of(arrWithPosZero);
1219 final AffineTransformMatrix3D d = AffineTransformMatrix3D.of(arrWithNegZero);
1220
1221
1222 Assertions.assertFalse(a.equals(b));
1223 Assertions.assertNotEquals(a.hashCode(), b.hashCode());
1224
1225 Assertions.assertTrue(a.equals(c));
1226 Assertions.assertEquals(a.hashCode(), c.hashCode());
1227
1228 Assertions.assertTrue(b.equals(d));
1229 Assertions.assertEquals(b.hashCode(), d.hashCode());
1230 }
1231
1232 @Test
1233 void testToString() {
1234
1235 final AffineTransformMatrix3D a = AffineTransformMatrix3D.of(
1236 1, 2, 3, 4,
1237 5, 6, 7, 8,
1238 9, 10, 11, 12
1239 );
1240
1241
1242 final String result = a.toString();
1243
1244
1245 Assertions.assertEquals(
1246 "[ 1.0, 2.0, 3.0, 4.0; " +
1247 "5.0, 6.0, 7.0, 8.0; " +
1248 "9.0, 10.0, 11.0, 12.0 ]", result);
1249 }
1250
1251
1252
1253
1254
1255 private static void runWithCoordinates(final PermuteCallback3D test) {
1256 EuclideanTestUtils.permute(-1e-2, 1e-2, 5e-3, test);
1257 EuclideanTestUtils.permute(-1e2, 1e2, 5, test);
1258 }
1259 }