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