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 toStringTest()
503 {
504 assertTrue("toString returns something descriptive", new Transform3d().toString().startsWith("Transform3d "));
505 }
506
507
508
509
510
511 public static void main(final String[] args)
512 {
513 Point3d unitVector = new Point3d(1, 0, 0);
514 double rotX = Math.toRadians(-55);
515 double rotY = Math.toRadians(-65);
516 double rotZ = Math.toRadians(-175);
517 Transform3d transform = new Transform3d();
518 transform.rotZ(rotZ);
519 System.out.println(transform.transform(unitVector));
520 transform.rotY(rotY);
521 System.out.println(transform.transform(unitVector));
522 transform.rotX(rotX);
523 Point3d rotated = transform.transform(unitVector);
524 System.out.println(rotated);
525 System.out.println("dirZ: " + Math.toDegrees(Math.atan2(rotated.y, rotated.x)));
526 System.out.println(
527 "dirY: " + Math.toDegrees(Math.atan2(-rotated.z, Math.sqrt(rotated.x * rotated.x + rotated.y * rotated.y)))
528 + " == " + Math.toDegrees(Math.atan2(-rotated.z, Math.hypot(rotated.x, rotated.y))));
529
530 }
531
532 }