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