1 package org.djutils.draw;
2
3 import static org.junit.jupiter.api.Assertions.assertEquals;
4 import static org.junit.jupiter.api.Assertions.assertFalse;
5 import static org.junit.jupiter.api.Assertions.assertNotEquals;
6 import static org.junit.jupiter.api.Assertions.assertTrue;
7 import static org.junit.jupiter.api.Assertions.fail;
8
9 import java.lang.reflect.Field;
10 import java.util.Arrays;
11
12 import org.djutils.draw.bounds.Bounds3d;
13 import org.djutils.draw.point.Point3d;
14 import org.junit.jupiter.api.Test;
15
16
17
18
19
20
21
22
23
24
25
26 public class Transform3dTest
27 {
28
29
30
31 @Test
32 public void testMatrixMultiplication()
33 {
34 double[] mA = new double[] {5, 7, 9, 10, 2, 3, 3, 8, 8, 10, 2, 3, 3, 3, 4, 8};
35 double[] mB = new double[] {3, 10, 12, 18, 12, 1, 4, 9, 9, 10, 12, 2, 3, 12, 4, 10};
36 double[] mAmB = Transform3d.mulMatMat(mA, mB);
37 double[] expected = new double[] {210, 267, 236, 271, 93, 149, 104, 149, 171, 146, 172, 268, 105, 169, 128, 169};
38 for (int i = 0; i < 16; i++)
39 {
40 if (mAmB[i] != expected[i])
41 {
42 fail(String.format("difference MA x MB at %d: expected %f, was: %f", i, expected[i], mAmB[i]));
43 }
44 }
45
46 double[] m = new double[] {1, 0, 2, 0, 0, 3, 0, 4, 0, 0, 5, 0, 6, 0, 0, 7};
47 double[] v = new double[] {2, 5, 1, 8};
48 double[] mv = Transform3d.mulMatVec(m, v);
49 double[] ev = new double[] {4, 47, 5, 68};
50 for (int i = 0; i < 4; i++)
51 {
52 if (mv[i] != ev[i])
53 {
54 fail(String.format("difference M x V at %d: expected %f, was: %f", i, ev[i], mv[i]));
55 }
56 }
57
58 v = new double[] {1, 2, 3};
59 mv = Transform3d.mulMatVec3(m, v);
60 ev = new double[] {7, 10, 15};
61 for (int i = 0; i < 3; i++)
62 {
63 if (mv[i] != ev[i])
64 {
65 fail(String.format("difference M x V3 at %d: expected %f, was: %f", i, ev[i], mv[i]));
66 }
67 }
68 }
69
70
71
72
73 @Test
74 public void testConstructor()
75 {
76
77 Transform3d t = new Transform3d();
78 assertEquals(16, t.getMat().length, "matrix contians 16 values");
79 for (int row = 0; row < 4; row++)
80 {
81 for (int col = 0; col < 4; col++)
82 {
83 int e = row == col ? 1 : 0;
84 assertEquals(e, t.getMat()[4 * row + col], 0, "Value in identity matrix matches");
85 }
86 }
87 }
88
89
90
91
92 @Test
93 public void testTranslateScaleRotateShearAndReflect()
94 {
95 Transform3d t;
96
97 double[] values = new double[] {-100000, -100, -3, -1, -0.1, 0, 0.1, 1, 3, 100, 100000};
98 for (double dx : values)
99 {
100 for (double dy : values)
101 {
102 for (double dz : values)
103 {
104
105 t = new Transform3d();
106 t.translate(dx, dy, dz);
107 for (double px : values)
108 {
109 for (double py : values)
110 {
111 for (double pz : values)
112 {
113 Point3d p = t.transform(new Point3d(px, py, pz));
114 assertEquals(px + dx, p.x, 0.001, "translated x matches");
115 assertEquals(py + dy, p.y, 0.001, "translated y matches");
116 assertEquals(pz + dz, p.z, 0.001, "translated z matches");
117 double[] result = t.transform(new double[] {px, py, pz});
118 assertEquals(px + dx, result[0], 0.001, "translated x matches");
119 assertEquals(py + dy, result[1], 0.001, "translated y matches");
120 assertEquals(pz + dz, result[2], 0.001, "translated z matches");
121 }
122 }
123 }
124
125 t = new Transform3d();
126 t.translate(new Point3d(dx, dy, dz));
127 for (double px : values)
128 {
129 for (double py : values)
130 {
131 for (double pz : values)
132 {
133 Point3d p = t.transform(new Point3d(px, py, pz));
134 assertEquals(px + dx, p.x, 0.001, "translated x matches");
135 assertEquals(py + dy, p.y, 0.001, "translated y matches");
136 assertEquals(pz + dz, p.z, 0.001, "translated z matches");
137 double[] result = t.transform(new double[] {px, py, pz});
138 assertEquals(px + dx, result[0], 0.001, "translated x matches");
139 assertEquals(py + dy, result[1], 0.001, "translated y matches");
140 assertEquals(pz + dz, result[2], 0.001, "translated z matches");
141 }
142 }
143 }
144
145 t = new Transform3d();
146 t.scale(dx, dy, dz);
147 for (double px : values)
148 {
149 for (double py : values)
150 {
151 for (double pz : values)
152 {
153 Point3d p = t.transform(new Point3d(px, py, pz));
154 assertEquals(px * dx, p.x, 0.001, "scaled x matches");
155 assertEquals(py * dy, p.y, 0.001, "scaled y matches");
156 assertEquals(pz * dz, p.z, 0.001, "scaled z matches");
157 double[] result = t.transform(new double[] {px, py, pz});
158 assertEquals(px * dx, result[0], 0.001, "scaled x matches");
159 assertEquals(py * dy, result[1], 0.001, "scaled y matches");
160 assertEquals(pz * dz, result[2], 0.001, "scaled z matches");
161 }
162 }
163 }
164
165 t = new Transform3d();
166 t.shearXY(dx, dy);
167 for (double px : values)
168 {
169 for (double py : values)
170 {
171 for (double pz : values)
172 {
173 Point3d p = t.transform(new Point3d(px, py, pz));
174 assertEquals(px + pz * dx, p.x, 0.001, "sheared x matches");
175 assertEquals(py + pz * dy, p.y, 0.001, "sheared y matches");
176 assertEquals(pz, p.z, 0.001, "sheared z matches");
177 double[] result = t.transform(new double[] {px, py, pz});
178 assertEquals(px + pz * dx, result[0], 0.001, "sheared x matches");
179 assertEquals(py + pz * dy, result[1], 0.001, "sheared y matches");
180 assertEquals(pz, result[2], 0.001, "sheared z matches");
181 }
182 }
183 }
184
185 t = new Transform3d();
186 t.shearXZ(dx, dz);
187 for (double px : values)
188 {
189 for (double py : values)
190 {
191 for (double pz : values)
192 {
193 Point3d p = t.transform(new Point3d(px, py, pz));
194 assertEquals(px + py * dx, p.x, 0.001, "sheared x matches");
195 assertEquals(py, p.y, 0.001, "sheared y matches");
196 assertEquals(pz + py * dz, p.z, 0.001, "sheared z matches");
197 double[] result = t.transform(new double[] {px, py, pz});
198 assertEquals(px + py * dx, result[0], 0.001, "sheared x matches");
199 assertEquals(py, result[1], 0.001, "sheared y matches");
200 assertEquals(pz + py * dz, result[2], 0.001, "sheared z matches");
201 }
202 }
203 }
204
205 t = new Transform3d();
206 t.shearYZ(dy, dz);
207 for (double px : values)
208 {
209 for (double py : values)
210 {
211 for (double pz : values)
212 {
213 Point3d p = t.transform(new Point3d(px, py, pz));
214 assertEquals(px, p.x, 0.001, "sheared x matches");
215 assertEquals(py + px * dy, p.y, 0.001, "sheared y matches");
216 assertEquals(pz + px * dz, p.z, 0.001, "sheared z matches");
217 double[] result = t.transform(new double[] {px, py, pz});
218 assertEquals(px, result[0], 0.001, "sheared x matches");
219 assertEquals(py + px * dy, result[1], 0.001, "sheared y matches");
220 assertEquals(pz + px * dz, result[2], 0.001, "sheared z matches");
221 }
222 }
223 }
224 }
225
226 t = new Transform3d();
227 t.rotZ(dx);
228 double sine = Math.sin(dx);
229 double cosine = Math.cos(dx);
230 for (double px : values)
231 {
232 for (double py : values)
233 {
234 for (double pz : values)
235 {
236 Point3d p = t.transform(new Point3d(px, py, pz));
237 assertEquals(px * cosine - py * sine, p.x, 0.001, "rotated x matches");
238 assertEquals(py * cosine + px * sine, p.y, 0.001, "rotated y matches");
239 assertEquals(pz, p.z, 0.001, "rotated z matches");
240 double[] result = t.transform(new double[] {px, py, pz});
241 assertEquals(px * cosine - py * sine, result[0], 0.001, "rotated x matches");
242 assertEquals(py * cosine + px * sine, result[1], 0.001, "rotated z matches");
243 assertEquals(pz, result[2], 0.001, "rotated z matches");
244 }
245 }
246 }
247
248 t = new Transform3d();
249 t.rotX(dx);
250 sine = Math.sin(dx);
251 cosine = Math.cos(dx);
252 for (double px : values)
253 {
254 for (double py : values)
255 {
256 for (double pz : values)
257 {
258 Point3d p = t.transform(new Point3d(px, py, pz));
259 assertEquals(px, p.x, 0.001, "rotated x matches");
260 assertEquals(py * cosine - pz * sine, p.y, 0.001, "rotated y matches");
261 assertEquals(pz * cosine + py * sine, p.z, 0.001, "rotated z matches");
262 double[] result = t.transform(new double[] {px, py, pz});
263 assertEquals(px, result[0], 0.001, "rotated x matches");
264 assertEquals(py * cosine - pz * sine, result[1], 0.001, "rotated z matches");
265 assertEquals(pz * cosine + py * sine, result[2], 0.001, "rotated z matches");
266 }
267 }
268 }
269
270 t = new Transform3d();
271 t.rotY(dx);
272 sine = Math.sin(dx);
273 cosine = Math.cos(dx);
274 for (double px : values)
275 {
276 for (double py : values)
277 {
278 for (double pz : values)
279 {
280 Point3d p = t.transform(new Point3d(px, py, pz));
281 assertEquals(px * cosine + pz * sine, p.x, 0.001, "rotated x matches");
282 assertEquals(py, p.y, 0.001, "rotated y matches");
283 assertEquals(pz * cosine - px * sine, p.z, 0.001, "rotated z matches");
284 double[] result = t.transform(new double[] {px, py, pz});
285 assertEquals(px * cosine + pz * sine, result[0], 0.001, "rotated x matches");
286 assertEquals(py, result[1], 0.001, "rotated z matches");
287 assertEquals(pz * cosine - px * sine, result[2], 0.001, "rotated z matches");
288 }
289 }
290 }
291 }
292 }
293
294 t = new Transform3d();
295 t.reflectX();
296 for (double px : values)
297 {
298 for (double py : values)
299 {
300 for (double pz : values)
301 {
302 Point3d p = t.transform(new Point3d(px, py, pz));
303 assertEquals(-px, p.x, 0.001, "x-reflected x matches");
304 assertEquals(py, p.y, 0.001, "x-reflected y matches");
305 assertEquals(pz, p.z, 0.001, "x-reflected z matches");
306 double[] result = t.transform(new double[] {px, py, pz});
307 assertEquals(-px, result[0], 0.001, "x-reflected x matches");
308 assertEquals(py, result[1], 0.001, "x-reflected y matches");
309 assertEquals(pz, result[2], 0.001, "x-reflected z matches");
310 }
311 }
312 }
313
314 t = new Transform3d();
315 t.reflectY();
316 for (double px : values)
317 {
318 for (double py : values)
319 {
320 for (double pz : values)
321 {
322 Point3d p = t.transform(new Point3d(px, py, pz));
323 assertEquals(px, p.x, 0.001, "y-reflected x matches");
324 assertEquals(-py, p.y, 0.001, "y-reflected y matches");
325 assertEquals(pz, p.z, 0.001, "y-reflected z matches");
326 double[] result = t.transform(new double[] {px, py, pz});
327 assertEquals(px, result[0], 0.001, "y-reflected x matches");
328 assertEquals(-py, result[1], 0.001, "y-reflected y matches");
329 assertEquals(pz, result[2], 0.001, "y-reflected z matches");
330 }
331 }
332 }
333
334 t = new Transform3d();
335 t.reflectZ();
336 for (double px : values)
337 {
338 for (double py : values)
339 {
340 for (double pz : values)
341 {
342 Point3d p = t.transform(new Point3d(px, py, pz));
343 assertEquals(px, p.x, 0.001, "z-reflected x matches");
344 assertEquals(py, p.y, 0.001, "z-reflected y matches");
345 assertEquals(-pz, p.z, 0.001, "z-reflected z matches");
346 double[] result = t.transform(new double[] {px, py, pz});
347 assertEquals(px, result[0], 0.001, "z-reflected x matches");
348 assertEquals(py, result[1], 0.001, "z-reflected y matches");
349 assertEquals(-pz, result[2], 0.001, "z-reflected z matches");
350 }
351 }
352 }
353 }
354
355
356
357
358 @Test
359 public void transformTest()
360 {
361 Transform3d reflectionX = new Transform3d().reflectX();
362 Transform3d reflectionY = new Transform3d().reflectY();
363 Transform3d reflectionZ = new Transform3d().reflectZ();
364
365 double[] values = new double[] {-30, 0, 0.07, 25};
366 for (double translateX : values)
367 {
368 for (double translateY : values)
369 {
370 for (double translateZ : values)
371 {
372 Transform3d translation = new Transform3d().translate(translateX, translateY, translateZ);
373 for (double scaleX : values)
374 {
375 for (double scaleY : values)
376 {
377 for (double scaleZ : values)
378 {
379 Transform3d scaling = new Transform3d().scale(scaleX, scaleY, scaleZ);
380 for (double angle : new double[] {-2, 0, 0.5})
381 {
382 Transform3d rotationX = new Transform3d().rotX(angle);
383 Transform3d rotationY = new Transform3d().rotY(angle);
384 Transform3d rotationZ = new Transform3d().rotZ(angle);
385 for (double shearA : values)
386 {
387 for (double shearB : values)
388 {
389 Transform3d t = new Transform3d().translate(translateX, translateY, translateZ)
390 .scale(scaleX, scaleY, scaleZ).rotZ(angle).shearXY(shearA, shearB);
391 Transform3d shearXY = new Transform3d().shearXY(shearA, shearB);
392 Transform3d tReflectX =
393 new Transform3d().reflectX().translate(translateX, translateY, translateZ)
394 .scale(scaleX, scaleY, scaleZ).rotY(angle).shearYZ(shearA, shearB);
395 Transform3d shearYZ = new Transform3d().shearYZ(shearA, shearB);
396 Transform3d tReflectY =
397 new Transform3d().reflectY().translate(translateX, translateY, translateZ)
398 .scale(scaleX, scaleY, scaleZ).rotZ(angle).shearXZ(shearA, shearB);
399 Transform3d shearXZ = new Transform3d().shearXZ(shearA, shearB);
400 Transform3d tReflectZ =
401 new Transform3d().reflectZ().translate(translateX, translateY, translateZ)
402 .scale(scaleX, scaleY, scaleZ).rotX(angle).shearXY(shearA, shearB);
403 for (double px : values)
404 {
405 for (double py : values)
406 {
407 for (double pz : values)
408 {
409 Point3d p = new Point3d(px, py, pz);
410 Point3d tp = t.transform(p);
411 Point3d chainP = translation.transform(
412 scaling.transform(rotationZ.transform(shearXY.transform(p))));
413 assertEquals(chainP.x, tp.x, 0.0000001, "X");
414 assertEquals(chainP.y, tp.y, 0.0000001, "Y");
415 assertEquals(chainP.z, tp.z, 0.0000001, "Z");
416 tp = tReflectX.transform(p);
417 Point3d chainPReflectX = reflectionX.transform(translation.transform(
418 scaling.transform(rotationY.transform(shearYZ.transform(p)))));
419 assertEquals(chainPReflectX.x, tp.x, 0.0000001, "RX X");
420 assertEquals(chainPReflectX.y, tp.y, 0.0000001, "RX Y");
421 assertEquals(chainPReflectX.z, tp.z, 0.0000001, "RX Z");
422 tp = tReflectY.transform(p);
423 Point3d chainPReflectY = reflectionY.transform(translation.transform(
424 scaling.transform(rotationZ.transform(shearXZ.transform(p)))));
425 assertEquals(chainPReflectY.x, tp.x, 0.0000001, "RY X");
426 assertEquals(chainPReflectY.y, tp.y, 0.0000001, "RY Y");
427 assertEquals(chainPReflectY.z, tp.z, 0.0000001, "RY Z");
428 tp = tReflectZ.transform(p);
429 Point3d chainPReflectZ = reflectionZ.transform(translation.transform(
430 scaling.transform(rotationX.transform(shearXY.transform(p)))));
431 assertEquals(chainPReflectZ.x, tp.x, 0.0000001, "RZ X");
432 assertEquals(chainPReflectZ.y, tp.y, 0.0000001, "RZ Y");
433 assertEquals(chainPReflectZ.z, tp.z, 0.0000001, "RZ Z");
434 }
435 }
436 }
437 }
438 }
439 }
440 }
441 }
442 }
443 }
444 }
445 }
446 }
447
448
449
450
451 @Test
452 public void transformBounds3dTest()
453 {
454 double[] values = new double[] {-100, 0.1, 0, 0.1, 100};
455 double[] sizes = new double[] {0, 10, 100};
456 Transform3d t = new Transform3d().rotX(0.4).rotZ(0.8).rotY(-1.2).reflectX().scale(0.5, 1.5, 2.5).shearXY(2, 3)
457 .translate(123, 456, 789);
458
459 for (double x : values)
460 {
461 for (double y : values)
462 {
463 for (double z : values)
464 {
465 for (double xSize : sizes)
466 {
467 for (double ySize : sizes)
468 {
469 for (double zSize : sizes)
470 {
471 Bounds3d bb = new Bounds3d(x, x + xSize, y, y + ySize, z, z + zSize);
472 Point3d[] points = new Point3d[] {new Point3d(x, y, z), new Point3d(x + xSize, y, z),
473 new Point3d(x, y + ySize, z), new Point3d(x + xSize, y + ySize, z),
474 new Point3d(x, y, z + zSize), new Point3d(x + xSize, y, z + zSize),
475 new Point3d(x, y + ySize, z + zSize), new Point3d(x + xSize, y + ySize, z + zSize)};
476 Point3d[] transformedPoints = new Point3d[8];
477 for (int i = 0; i < points.length; i++)
478 {
479 transformedPoints[i] = t.transform(points[i]);
480 }
481 Bounds3d expected = new Bounds3d(Arrays.stream(transformedPoints).iterator());
482 Bounds3d got = t.transform(bb);
483 if (!got.equals(expected))
484 {
485 System.err.println("oops");
486 t.transform(bb);
487 }
488 assertEquals(expected.getMinX(), got.getMinX(), 0.0001, "bb minX");
489 assertEquals(expected.getMaxX(), got.getMaxX(), 0.0001, "bb maxX");
490 assertEquals(expected.getMinY(), got.getMinY(), 0.0001, "bb minY");
491 assertEquals(expected.getMaxY(), got.getMaxY(), 0.0001, "bb maxY");
492 assertEquals(expected.getMinZ(), got.getMinZ(), 0.0001, "bb minZ");
493 assertEquals(expected.getMaxZ(), got.getMaxZ(), 0.0001, "bb maxZ");
494 }
495 }
496 }
497 }
498 }
499 }
500 }
501
502
503
504
505 @Test
506 public void testBoundingBox3d()
507 {
508 Bounds3d bounds = new Bounds3d(-4, 4, -4, 4, -4, 4);
509
510
511 Transform3d transform = new Transform3d();
512 Bounds3d b = transform.transform(bounds);
513 testBounds3d(b, -4, 4, -4, 4, -4, 4);
514
515
516 transform = new Transform3d();
517 transform.translate(20, 10, 0);
518 b = transform.transform(bounds);
519 testBounds3d(b, 20 - 4, 20 + 4, 10 - 4, 10 + 4, -4, 4);
520
521
522 transform = new Transform3d();
523 transform.translate(-20, -10, -30);
524 b = transform.transform(bounds);
525 testBounds3d(b, -20 - 4, -20 + 4, -10 - 4, -10 + 4, -30 - 4, -30 + 4);
526
527
528 transform = new Transform3d();
529 transform.rotZ(Math.toRadians(90.0));
530 b = transform.transform(bounds);
531 testBounds3d(b, -4, 4, -4, 4, -4, 4);
532
533
534 transform = new Transform3d();
535 transform.rotZ(Math.toRadians(45.0));
536 double d = 4.0 * Math.sqrt(2.0);
537 b = transform.transform(bounds);
538 testBounds3d(b, -d, d, -d, d, -4, 4);
539
540
541
542
543 transform = new Transform3d();
544 transform.translate(10, 20, 0);
545 transform.rotZ(Math.toRadians(45.0));
546 b = transform.transform(bounds);
547 testBounds3d(b, 10 - d, 10 + d, 20 - d, 20 + d, -4, 4);
548 }
549
550
551
552
553
554
555
556
557
558
559
560 private void testBounds3d(final Bounds3d b, final double minX, final double maxX, final double minY, final double maxY,
561 final double minZ, final double maxZ)
562 {
563 assertEquals(minX, b.getMinX(), 0.001);
564 assertEquals(maxX, b.getMaxX(), 0.001);
565 assertEquals(minY, b.getMinY(), 0.001);
566 assertEquals(maxY, b.getMaxY(), 0.001);
567 assertEquals(minZ, b.getMinZ(), 0.001);
568 assertEquals(maxZ, b.getMaxZ(), 0.001);
569 }
570
571
572
573
574 @Test
575 public void toStringTest()
576 {
577 assertTrue(new Transform3d().toString().startsWith("Transform3d "), "toString returns something descriptive");
578 }
579
580
581
582
583
584 public static void main(final String[] args)
585 {
586 Point3d unitVector = new Point3d(1, 0, 0);
587 double rotX = Math.toRadians(-55);
588 double rotY = Math.toRadians(-65);
589 double rotZ = Math.toRadians(-175);
590 Transform3d transform = new Transform3d();
591 transform.rotZ(rotZ);
592 System.out.println(transform.transform(unitVector));
593 transform.rotY(rotY);
594 System.out.println(transform.transform(unitVector));
595 transform.rotX(rotX);
596 Point3d rotated = transform.transform(unitVector);
597 System.out.println(rotated);
598 System.out.println("dirZ: " + Math.toDegrees(Math.atan2(rotated.y, rotated.x)));
599 System.out.println(
600 "dirY: " + Math.toDegrees(Math.atan2(-rotated.z, Math.sqrt(rotated.x * rotated.x + rotated.y * rotated.y)))
601 + " == " + Math.toDegrees(Math.atan2(-rotated.z, Math.hypot(rotated.x, rotated.y))));
602
603 }
604
605
606
607
608
609
610
611
612 @Test
613 @SuppressWarnings({"unlikely-arg-type"})
614 public void testHashCodeAndEquals()
615 throws NoSuchFieldException, SecurityException, IllegalArgumentException, IllegalAccessException
616 {
617
618
619 Transform3d reference = new Transform3d();
620 assertEquals(reference, new Transform3d(), "Two different instances with same matrix do test equal");
621 assertEquals(reference.hashCode(), new Transform3d().hashCode(),
622 "Two different instances with same matrix have same hash code");
623 for (int index = 0; index < 16; index++)
624 {
625
626 for (double alteration : new double[] {-100, -10, -Math.PI, -0.1, 0.3, Math.E, 123})
627 {
628 Transform3d other = new Transform3d();
629 Field matrix = other.getClass().getDeclaredField("mat");
630 matrix.setAccessible(true);
631 double[] matrixValues = (double[]) matrix.get(other);
632 matrixValues[index] = alteration;
633 assertNotEquals(reference, other, "Modified transform should not be equals");
634 assertNotEquals(reference.hashCode(), other.hashCode(), "HashCode should be different "
635 + "(or it does not take all elements of the internal array into account");
636 }
637 }
638 assertTrue(reference.equals(reference), "equal to itself");
639 assertFalse(reference.equals(null), "not equal to null");
640 assertFalse(reference.equals("nope"), "not equal to some other object");
641 }
642
643 }