1 package org.opentrafficsim.core.geometry;
2
3 import static org.junit.Assert.assertEquals;
4 import static org.junit.Assert.assertFalse;
5 import static org.junit.Assert.assertTrue;
6 import static org.junit.Assert.fail;
7
8 import java.awt.geom.Path2D;
9 import java.awt.geom.PathIterator;
10 import java.lang.reflect.InvocationTargetException;
11 import java.lang.reflect.Method;
12 import java.util.ArrayList;
13 import java.util.List;
14 import java.util.Random;
15
16 import org.djunits.unit.DirectionUnit;
17 import org.djunits.unit.LengthUnit;
18 import org.djunits.value.vdouble.scalar.Direction;
19 import org.djunits.value.vdouble.scalar.Length;
20 import org.junit.Test;
21 import org.locationtech.jts.geom.Coordinate;
22 import org.locationtech.jts.geom.Geometry;
23 import org.locationtech.jts.geom.GeometryFactory;
24 import org.locationtech.jts.geom.LineString;
25 import org.opentrafficsim.core.geometry.OTSLine3D.FractionalFallback;
26 import org.opentrafficsim.core.network.NetworkException;
27
28 import nl.tudelft.simulation.language.d3.DirectedPoint;
29
30
31
32
33
34
35
36
37
38 public class OTSLine3DTest
39 {
40
41
42
43
44
45 @Test
46 public final void constructorsTest() throws OTSGeometryException, NetworkException
47 {
48 double[] values = {-999, 0, 99, 9999};
49 OTSPoint3D[] points = new OTSPoint3D[0];
50 try
51 {
52 runConstructors(points);
53 fail("Should have thrown a NetworkException");
54 }
55 catch (OTSGeometryException exception)
56 {
57
58 }
59 for (double x0 : values)
60 {
61 for (double y0 : values)
62 {
63 for (double z0 : values)
64 {
65 points = new OTSPoint3D[1];
66 points[0] = new OTSPoint3D(x0, y0, z0);
67 try
68 {
69 runConstructors(points);
70 fail("Should have thrown a NetworkException");
71 }
72 catch (OTSGeometryException exception)
73 {
74
75 }
76 for (double x1 : values)
77 {
78 for (double y1 : values)
79 {
80 for (double z1 : values)
81 {
82 points = new OTSPoint3D[2];
83 points[0] = new OTSPoint3D(x0, y0, z0);
84 points[1] = new OTSPoint3D(x1, y1, z1);
85 if (0 == points[0].distance(points[1]).si)
86 {
87 try
88 {
89 runConstructors(points);
90 fail("Should have thrown a NetworkException");
91 }
92 catch (OTSGeometryException exception)
93 {
94
95 }
96 }
97 else
98 {
99 runConstructors(points);
100 for (double x2 : values)
101 {
102 for (double y2 : values)
103 {
104 for (double z2 : values)
105 {
106 points = new OTSPoint3D[3];
107 points[0] = new OTSPoint3D(x0, y0, z0);
108 points[1] = new OTSPoint3D(x1, y1, z1);
109 points[2] = new OTSPoint3D(x2, y2, z2);
110 if (0 == points[1].distance(points[2]).si)
111 {
112 try
113 {
114 runConstructors(points);
115 fail("Should have thrown a NetworkException");
116 }
117 catch (OTSGeometryException exception)
118 {
119
120 }
121 }
122 else
123 {
124 runConstructors(points);
125 }
126 }
127 }
128 }
129 }
130 }
131 }
132 }
133 }
134 }
135 }
136 }
137
138
139
140
141
142
143
144 private void runConstructors(final OTSPoint3D[] points) throws OTSGeometryException, NetworkException
145 {
146 verifyPoints(new OTSLine3D(points), points);
147 Coordinate[] coordinates = new Coordinate[points.length];
148 for (int i = 0; i < points.length; i++)
149 {
150 coordinates[i] = new Coordinate(points[i].x, points[i].y, points[i].z);
151 }
152 verifyPoints(new OTSLine3D(coordinates), points);
153 GeometryFactory gm = new GeometryFactory();
154 LineString lineString = gm.createLineString(coordinates);
155 verifyPoints(new OTSLine3D(lineString), points);
156 verifyPoints(new OTSLine3D((Geometry) lineString), points);
157 List<OTSPoint3D> list = new ArrayList<>();
158 for (int i = 0; i < points.length; i++)
159 {
160 list.add(points[i]);
161 }
162 OTSLine3D line = new OTSLine3D(list);
163 verifyPoints(line, points);
164
165 verifyPoints(new OTSLine3D(line.getCoordinates()), points);
166
167 verifyPoints(new OTSLine3D(line.getLineString()), points);
168
169 verifyPoints(new OTSLine3D(line.getPoints()), points);
170 double length = 0;
171 for (int i = 1; i < points.length; i++)
172 {
173 length += Math.sqrt(Math.pow(points[i].x - points[i - 1].x, 2) + Math.pow(points[i].y - points[i - 1].y, 2)
174 + Math.pow(points[i].z - points[i - 1].z, 2));
175 }
176 assertEquals("length", length, line.getLength().si, 10 * Math.ulp(length));
177 assertEquals("length", length, line.getLength().si, 10 * Math.ulp(length));
178 assertEquals("length", length, line.getLengthSI(), 10 * Math.ulp(length));
179 assertEquals("length", length, line.getLengthSI(), 10 * Math.ulp(length));
180
181 int horizontalMoves = 0;
182 Path2D path = new Path2D.Double();
183 path.moveTo(points[0].x, points[0].y);
184
185 for (int i = 1; i < points.length; i++)
186 {
187 if (points[i].x != points[i - 1].x || points[i].y != points[i - 1].y)
188 {
189 path.lineTo(points[i].x, points[i].y);
190
191 horizontalMoves++;
192 }
193 }
194 try
195 {
196 line = new OTSLine3D(path);
197 if (0 == horizontalMoves)
198 {
199 fail("Construction of OTSLine3D from path with degenerate projection should have failed");
200 }
201
202 assertEquals("number of points should match", horizontalMoves + 1, line.size());
203 int indexInLine = 0;
204 for (int i = 0; i < points.length; i++)
205 {
206 if (i > 0 && (points[i].x != points[i - 1].x || points[i].y != points[i - 1].y))
207 {
208 indexInLine++;
209 }
210 assertEquals("x in line", points[i].x, line.get(indexInLine).x, 0.001);
211 assertEquals("y in line", points[i].y, line.get(indexInLine).y, 0.001);
212 }
213 }
214 catch (OTSGeometryException e)
215 {
216 if (0 != horizontalMoves)
217 {
218 fail("Construction of OTSLine3D from path with non-degenerate projection should not have failed");
219 }
220 }
221 }
222
223
224
225
226
227 public final void printPath2D(final Path2D path)
228 {
229 PathIterator pi = path.getPathIterator(null);
230 double[] p = new double[6];
231 while (!pi.isDone())
232 {
233 int segType = pi.currentSegment(p);
234 if (segType == PathIterator.SEG_MOVETO)
235 {
236 System.out.print(" move to " + new OTSPoint3D(p[0], p[1]));
237 }
238 if (segType == PathIterator.SEG_LINETO)
239 {
240 System.out.print(" line to " + new OTSPoint3D(p[0], p[1]));
241 }
242 else if (segType == PathIterator.SEG_CLOSE)
243 {
244 System.out.print(" close");
245 }
246 pi.next();
247 }
248 System.out.println("");
249 }
250
251
252
253
254
255
256
257 private void verifyPoints(final OTSLine3D line, final OTSPoint3D[] points) throws OTSGeometryException
258 {
259 assertEquals("Line should have same number of points as point array", line.size(), points.length);
260 for (int i = 0; i < points.length; i++)
261 {
262 assertEquals("x of point i should match", points[i].x, line.get(i).x, Math.ulp(points[i].x));
263 assertEquals("y of point i should match", points[i].y, line.get(i).y, Math.ulp(points[i].y));
264 assertEquals("z of point i should match", points[i].z, line.get(i).z, Math.ulp(points[i].z));
265 }
266 }
267
268
269
270
271
272 @Test
273 public final void exceptionTest() throws OTSGeometryException
274 {
275 OTSLine3D line = new OTSLine3D(new OTSPoint3D[] {new OTSPoint3D(1, 2, 3), new OTSPoint3D(4, 5, 6)});
276 try
277 {
278 line.get(-1);
279 fail("Should have thrown an exception");
280 }
281 catch (OTSGeometryException oe)
282 {
283
284 }
285 try
286 {
287 line.get(2);
288 fail("Should have thrown an exception");
289 }
290 catch (OTSGeometryException oe)
291 {
292
293 }
294 }
295
296
297
298
299
300 @Test
301 public final void locationExtendedTest() throws OTSGeometryException
302 {
303 OTSPoint3D p0 = new OTSPoint3D(10, 20, 30);
304 OTSPoint3D p1 = new OTSPoint3D(40, 50, 60);
305 OTSPoint3D p2 = new OTSPoint3D(90, 80, 70);
306 OTSLine3D l = new OTSLine3D(new OTSPoint3D[] {p0, p1, p2});
307 checkGetLocation(l, -10, null, Math.atan2(p1.y - p0.y, p1.x - p0.x));
308 checkGetLocation(l, -0.0001, p0, Math.atan2(p1.y - p0.y, p1.x - p0.x));
309 checkGetLocation(l, 0, p0, Math.atan2(p1.y - p0.y, p1.x - p0.x));
310 checkGetLocation(l, 0.0001, p0, Math.atan2(p1.y - p0.y, p1.x - p0.x));
311 checkGetLocation(l, 0.9999, p2, Math.atan2(p2.y - p1.y, p2.x - p1.x));
312 checkGetLocation(l, 1, p2, Math.atan2(p2.y - p1.y, p2.x - p1.x));
313 checkGetLocation(l, 1.0001, p2, Math.atan2(p2.y - p1.y, p2.x - p1.x));
314 checkGetLocation(l, 10, null, Math.atan2(p2.y - p1.y, p2.x - p1.x));
315 }
316
317
318
319
320
321
322
323
324
325 private void checkGetLocation(final OTSLine3D line, final double fraction, final OTSPoint3D expectedPoint,
326 final double expectedZRotation) throws OTSGeometryException
327 {
328 double length = line.getLengthSI();
329 checkDirectedPoint(line.getLocationExtendedSI(fraction * length), expectedPoint, expectedZRotation);
330 Length typedLength = new Length(fraction * length, LengthUnit.METER);
331 checkDirectedPoint(line.getLocationExtended(typedLength), expectedPoint, expectedZRotation);
332 if (fraction < 0 || fraction > 1)
333 {
334 try
335 {
336 line.getLocationSI(fraction * length);
337 fail("getLocation should have thrown a OTSGeometryException");
338 }
339 catch (OTSGeometryException ne)
340 {
341
342 }
343 try
344 {
345 line.getLocation(typedLength);
346 fail("getLocation should have thrown a OTSGeometryException");
347 }
348 catch (OTSGeometryException ne)
349 {
350
351 }
352 try
353 {
354 line.getLocationFraction(fraction);
355 fail("getLocation should have thrown a OTSGeometryException");
356 }
357 catch (OTSGeometryException ne)
358 {
359
360 }
361 }
362 else
363 {
364 checkDirectedPoint(line.getLocationSI(fraction * length), expectedPoint, expectedZRotation);
365 checkDirectedPoint(line.getLocation(typedLength), expectedPoint, expectedZRotation);
366 checkDirectedPoint(line.getLocationFraction(fraction), expectedPoint, expectedZRotation);
367 }
368
369 }
370
371
372
373
374
375
376
377 private void checkDirectedPoint(final DirectedPoint dp, final OTSPoint3D expectedPoint, final double expectedZRotation)
378 {
379
380 if (null != expectedPoint)
381 {
382 OTSPoint3D p = new OTSPoint3D(dp);
383 assertEquals("locationExtendedSI(0) returns approximately expected point", 0, expectedPoint.distanceSI(p), 0.1);
384 }
385 assertEquals("z-rotation at 0", expectedZRotation, dp.getRotZ(), 0.001);
386 }
387
388
389
390
391
392 @Test
393 public final void locationTest() throws OTSGeometryException
394 {
395 OTSPoint3D p0 = new OTSPoint3D(10, 20, 60);
396 OTSPoint3D p1 = new OTSPoint3D(40, 50, 60);
397 OTSPoint3D p2 = new OTSPoint3D(90, 70, 90);
398 OTSLine3D l = new OTSLine3D(new OTSPoint3D[] {p0, p1, p2});
399 DirectedPoint dp = l.getLocation();
400 assertEquals("centroid x", 50, dp.x, 0.001);
401 assertEquals("centroid y", 45, dp.y, 0.001);
402 assertEquals("centroid z", 75, dp.z, 0.001);
403 l = new OTSLine3D(new OTSPoint3D[] {p1, p0, p2});
404 dp = l.getLocation();
405 assertEquals("centroid x", 50, dp.x, 0.001);
406 assertEquals("centroid y", 45, dp.y, 0.001);
407 assertEquals("centroid z", 75, dp.z, 0.001);
408 l = new OTSLine3D(new OTSPoint3D[] {p0, p1});
409 dp = l.getLocation();
410 assertEquals("centroid x", 25, dp.x, 0.001);
411 assertEquals("centroid y", 35, dp.y, 0.001);
412 assertEquals("centroid z", 60, dp.z, 0.001);
413 }
414
415
416
417
418
419 @Test
420 public final void cleanTest() throws OTSGeometryException
421 {
422 OTSPoint3D[] tooShort = new OTSPoint3D[] {};
423 try
424 {
425 OTSLine3D.createAndCleanOTSLine3D(tooShort);
426 fail("Array with no points should have thrown an exception");
427 }
428 catch (OTSGeometryException ne)
429 {
430
431 }
432 tooShort = new OTSPoint3D[] {new OTSPoint3D(1, 2, 3)};
433 try
434 {
435 OTSLine3D.createAndCleanOTSLine3D(tooShort);
436 fail("Array with no points should have thrown an exception");
437 }
438 catch (OTSGeometryException ne)
439 {
440
441 }
442 OTSPoint3D p0 = new OTSPoint3D(1, 2, 3);
443 OTSPoint3D p1 = new OTSPoint3D(4, 5, 6);
444 OTSPoint3D[] points = new OTSPoint3D[] {p0, p1};
445 OTSLine3D result = OTSLine3D.createAndCleanOTSLine3D(points);
446 assertTrue("first point is p0", p0.equals(result.get(0)));
447 assertTrue("second point is p1", p1.equals(result.get(1)));
448 OTSPoint3D p1Same = new OTSPoint3D(4, 5, 6);
449 result = OTSLine3D.createAndCleanOTSLine3D(new OTSPoint3D[] {p0, p0, p0, p0, p1Same, p0, p1, p1, p1Same, p1, p1});
450 assertEquals("result should contain 4 points", 4, result.size());
451 assertTrue("first point is p0", p0.equals(result.get(0)));
452 assertTrue("second point is p1", p1.equals(result.get(1)));
453 assertTrue("third point is p0", p0.equals(result.get(0)));
454 assertTrue("last point is p1", p1.equals(result.get(1)));
455 }
456
457
458
459
460
461 @Test
462 public final void equalsTest() throws OTSGeometryException
463 {
464 OTSPoint3D p0 = new OTSPoint3D(1.1, 2.2, 3.3);
465 OTSPoint3D p1 = new OTSPoint3D(2.1, 2.2, 3.3);
466 OTSPoint3D p2 = new OTSPoint3D(3.1, 2.2, 3.3);
467
468 OTSLine3D line = new OTSLine3D(new OTSPoint3D[] {p0, p1, p2});
469 assertTrue("OTSLine3D is equal to itself", line.equals(line));
470 assertFalse("OTSLine3D is not equal to null", line.equals(null));
471 assertFalse("OTSLine3D is not equals to some other kind of Object", line.equals(new String("hello")));
472 OTSLine3D line2 = new OTSLine3D(new OTSPoint3D[] {p0, p1, p2});
473 assertTrue("OTSLine3D is equal ot other OTSLine3D that has the exact same list of OTSPoint3D", line.equals(line2));
474 OTSPoint3D p2Same = new OTSPoint3D(3.1, 2.2, 3.3);
475 line2 = new OTSLine3D(new OTSPoint3D[] {p0, p1, p2Same});
476 assertTrue("OTSLine3D is equal ot other OTSLine3D that has the exact same list of OTSPoint3D; even if some of "
477 + "those point are different instances with the same coordinates", line.equals(line2));
478 OTSPoint3D p2NotSame = new OTSPoint3D(3.1, 2.2, 3.35);
479 line2 = new OTSLine3D(new OTSPoint3D[] {p0, p1, p2NotSame});
480 assertFalse("OTSLine3D is not equal ot other OTSLine3D that differs in one coordinate", line.equals(line2));
481 line2 = new OTSLine3D(new OTSPoint3D[] {p0, p1, p2, p2NotSame});
482 assertFalse("OTSLine3D is not equal ot other OTSLine3D that has more points (but is identical up to the common length)",
483 line.equals(line2));
484 assertFalse(
485 "OTSLine3D is not equal ot other OTSLine3D that has fewer points (but is identical up to the common length)",
486 line2.equals(line));
487 }
488
489
490
491
492
493 @Test
494 public final void concatenateTest() throws OTSGeometryException
495 {
496 OTSPoint3D p0 = new OTSPoint3D(1.1, 2.2, 3.3);
497 OTSPoint3D p1 = new OTSPoint3D(2.1, 2.2, 3.3);
498 OTSPoint3D p2 = new OTSPoint3D(3.1, 2.2, 3.3);
499 OTSPoint3D p3 = new OTSPoint3D(4.1, 2.2, 3.3);
500 OTSPoint3D p4 = new OTSPoint3D(5.1, 2.2, 3.3);
501 OTSPoint3D p5 = new OTSPoint3D(6.1, 2.2, 3.3);
502
503 OTSLine3D l0 = new OTSLine3D(p0, p1, p2);
504 OTSLine3D l1 = new OTSLine3D(p2, p3);
505 OTSLine3D l2 = new OTSLine3D(p3, p4, p5);
506 OTSLine3D ll = OTSLine3D.concatenate(l0, l1, l2);
507 assertEquals("size is 6", 6, ll.size());
508 assertEquals("point 0 is p0", p0, ll.get(0));
509 assertEquals("point 1 is p1", p1, ll.get(1));
510 assertEquals("point 2 is p2", p2, ll.get(2));
511 assertEquals("point 3 is p3", p3, ll.get(3));
512 assertEquals("point 4 is p4", p4, ll.get(4));
513 assertEquals("point 5 is p5", p5, ll.get(5));
514
515 ll = OTSLine3D.concatenate(l1);
516 assertEquals("size is 2", 2, ll.size());
517 assertEquals("point 0 is p2", p2, ll.get(0));
518 assertEquals("point 1 is p3", p3, ll.get(1));
519
520 try
521 {
522 OTSLine3D.concatenate(l0, l2);
523 fail("Gap should have throw an exception");
524 }
525 catch (OTSGeometryException e)
526 {
527
528 }
529 try
530 {
531 OTSLine3D.concatenate();
532 fail("concatenate of empty list should have thrown an exception");
533 }
534 catch (OTSGeometryException e)
535 {
536
537 }
538
539
540 OTSLine3D thirdLine = new OTSLine3D(p4, p5);
541 for (double tolerance : new double[] {0.1, 0.01, 0.001, 0.0001, 0.00001})
542 {
543 for (double actualError : new double[] {tolerance * 0.9, tolerance * 1.1})
544 {
545 int maxDirection = 10;
546 for (int direction = 0; direction < maxDirection; direction++)
547 {
548 double dx = actualError * Math.cos(Math.PI * 2 * direction / maxDirection);
549 double dy = actualError * Math.sin(Math.PI * 2 * direction / maxDirection);
550 OTSLine3D otherLine = new OTSLine3D(new OTSPoint3D(p2.x + dx, p2.y + dy, p2.z), p3, p4);
551 if (actualError < tolerance)
552 {
553 try
554 {
555 OTSLine3D.concatenate(tolerance, l0, otherLine);
556 }
557 catch (OTSGeometryException oge)
558 {
559 OTSLine3D.concatenate(tolerance, l0, otherLine);
560 fail("concatenation with error " + actualError + " and tolerance " + tolerance
561 + " should not have failed");
562 }
563 try
564 {
565 OTSLine3D.concatenate(tolerance, l0, otherLine, thirdLine);
566 }
567 catch (OTSGeometryException oge)
568 {
569 fail("concatenation with error " + actualError + " and tolerance " + tolerance
570 + " should not have failed");
571 }
572 }
573 else
574 {
575 try
576 {
577 OTSLine3D.concatenate(tolerance, l0, otherLine);
578 }
579 catch (OTSGeometryException oge)
580 {
581
582 }
583 try
584 {
585 OTSLine3D.concatenate(tolerance, l0, otherLine, thirdLine);
586 }
587 catch (OTSGeometryException oge)
588 {
589
590 }
591 }
592 }
593 }
594 }
595 }
596
597
598
599
600
601 @Test
602 public final void noiseFilterRamerDouglasPeuckerTest() throws OTSGeometryException
603 {
604 OTSPoint3D start = new OTSPoint3D(1, 2, 3);
605 int maxDirection = 20;
606 double length = 100;
607 for (boolean useHorizontalDistance : new boolean[] {true, false})
608 {
609 for (int direction = 0; direction < maxDirection; direction++)
610 {
611 double angle = Math.PI * 2 * direction / maxDirection;
612 double dx = length * Math.cos(angle);
613 double dy = length * Math.sin(angle);
614 OTSPoint3D end = new OTSPoint3D(start.x + dx, start.y + dy, start.z);
615 OTSLine3D straightLine = new OTSLine3D(start, end);
616 int intermediatePointCount = 5;
617 for (double tolerance : new double[] {0.1, 0.01, 0.001})
618 {
619 double error = tolerance * 0.9;
620 List<OTSPoint3D> pointsOnTestLine = new ArrayList<>();
621 pointsOnTestLine.add(start);
622 for (int intermediatePoint = 0; intermediatePoint < intermediatePointCount; intermediatePoint++)
623 {
624 double iAngle = Math.PI * 2 * intermediatePoint / intermediatePointCount;
625 double idx = error * Math.cos(iAngle);
626 double idy = error * Math.sin(iAngle);
627 double idz = useHorizontalDistance ? (intermediatePoint % 2 * 2 - 1) * 10 : 0;
628 DirectedPoint exactPoint =
629 straightLine.getLocationFraction((intermediatePoint + 0.5) / intermediatePointCount);
630 OTSPoint3D additionalPoint = new OTSPoint3D(exactPoint.x + idx, exactPoint.y + idy, exactPoint.z + idz);
631 pointsOnTestLine.add(additionalPoint);
632 }
633 pointsOnTestLine.add(end);
634 OTSLine3D testLine = new OTSLine3D(pointsOnTestLine);
635 OTSLine3D filteredLine = testLine.noiseFilterRamerDouglasPeuker(tolerance, useHorizontalDistance);
636 assertEquals("RamerDouglasPeuker filter should have removed all intermediate points", 2,
637 filteredLine.size());
638
639
640 OTSPoint3D newStart = new OTSPoint3D(start.x + 10 * tolerance * dy / length,
641 start.y - 10 * tolerance * dx / length, start.z);
642 pointsOnTestLine.add(0, newStart);
643
644
645 testLine = new OTSLine3D(pointsOnTestLine);
646 filteredLine = testLine.noiseFilterRamerDouglasPeuker(tolerance, useHorizontalDistance);
647
648
649
650
651 assertEquals("RamerDouglasPeuker filter should have left three points", 3, filteredLine.size());
652 pointsOnTestLine.remove(0);
653 OTSPoint3D newEnd =
654 new OTSPoint3D(end.x + 10 * tolerance * dy / length, end.y - 10 * tolerance * dx / length, end.z);
655 pointsOnTestLine.add(newEnd);
656 testLine = new OTSLine3D(pointsOnTestLine);
657 filteredLine = testLine.noiseFilterRamerDouglasPeuker(tolerance, useHorizontalDistance);
658
659
660
661
662 assertEquals("RamerDouglasPeuker filter should have left three points", 3, filteredLine.size());
663 }
664 }
665 }
666 }
667
668
669
670
671
672 @Test
673 public final void reverseTest() throws OTSGeometryException
674 {
675 OTSPoint3D p0 = new OTSPoint3D(1.1, 2.2, 3.3);
676 OTSPoint3D p1 = new OTSPoint3D(2.1, 2.2, 3.3);
677 OTSPoint3D p2 = new OTSPoint3D(3.1, 2.2, 3.3);
678 OTSPoint3D p3 = new OTSPoint3D(4.1, 2.2, 3.3);
679 OTSPoint3D p4 = new OTSPoint3D(5.1, 2.2, 3.3);
680 OTSPoint3D p5 = new OTSPoint3D(6.1, 2.2, 3.3);
681
682 OTSLine3D l01 = new OTSLine3D(p0, p1);
683 OTSLine3D r = l01.reverse();
684 assertEquals("result has size 2", 2, r.size());
685 assertEquals("point 0 is p1", p1, r.get(0));
686 assertEquals("point 1 is p0", p0, r.get(1));
687
688 OTSLine3D l05 = new OTSLine3D(p0, p1, p2, p3, p4, p5);
689 r = l05.reverse();
690 assertEquals("result has size 6", 6, r.size());
691 assertEquals("point 0 is p5", p5, r.get(0));
692 assertEquals("point 1 is p4", p4, r.get(1));
693 assertEquals("point 2 is p3", p3, r.get(2));
694 assertEquals("point 3 is p2", p2, r.get(3));
695 assertEquals("point 4 is p1", p1, r.get(4));
696 assertEquals("point 5 is p0", p0, r.get(5));
697
698 }
699
700
701
702
703
704 @SuppressWarnings("checkstyle:methodlength")
705 @Test
706 public final void extractTest() throws OTSGeometryException
707 {
708 OTSPoint3D p0 = new OTSPoint3D(1, 2, 3);
709 OTSPoint3D p1 = new OTSPoint3D(2, 3, 4);
710 OTSPoint3D p1a = new OTSPoint3D(2.01, 3.01, 4.01);
711 OTSPoint3D p1b = new OTSPoint3D(2.02, 3.02, 4.02);
712 OTSPoint3D p1c = new OTSPoint3D(2.03, 3.03, 4.03);
713 OTSPoint3D p2 = new OTSPoint3D(12, 13, 14);
714
715 OTSLine3D l = new OTSLine3D(p0, p1);
716 OTSLine3D e = l.extractFractional(0, 1);
717 assertEquals("size of extraction is 2", 2, e.size());
718 assertEquals("point 0 is p0", p0, e.get(0));
719 assertEquals("point 1 is p1", p1, e.get(1));
720 try
721 {
722 l.extractFractional(-0.1, 1);
723 fail("negative start should have thrown an exception");
724 }
725 catch (OTSGeometryException exception)
726 {
727
728 }
729 try
730 {
731 l.extractFractional(Double.NaN, 1);
732 fail("NaN start should have thrown an exception");
733 }
734 catch (OTSGeometryException exception)
735 {
736
737 }
738 try
739 {
740 l.extractFractional(0, 1.1);
741 fail("end > 1 should have thrown an exception");
742 }
743 catch (OTSGeometryException exception)
744 {
745
746 }
747 try
748 {
749 l.extractFractional(0, Double.NaN);
750 fail("NaN end should have thrown an exception");
751 }
752 catch (OTSGeometryException exception)
753 {
754
755 }
756 try
757 {
758 l.extractFractional(0.6, 0.4);
759 fail("start > end should have thrown an exception");
760 }
761 catch (OTSGeometryException exception)
762 {
763
764 }
765 try
766 {
767 l.extract(-0.1, 1);
768 fail("negative start should have thrown an exception");
769 }
770 catch (OTSGeometryException exception)
771 {
772
773 }
774 try
775 {
776 l.extract(Double.NaN, 1);
777 fail("NaN start should have thrown an exception");
778 }
779 catch (OTSGeometryException exception)
780 {
781
782 }
783 try
784 {
785 l.extract(0, l.getLengthSI() + 0.1);
786 fail("end > length should have thrown an exception");
787 }
788 catch (OTSGeometryException exception)
789 {
790
791 }
792 try
793 {
794 l.extract(0, Double.NaN);
795 fail("NaN end should have thrown an exception");
796 }
797 catch (OTSGeometryException exception)
798 {
799
800 }
801 try
802 {
803 l.extract(0.6, 0.4);
804 fail("start > end should have thrown an exception");
805 }
806 catch (OTSGeometryException exception)
807 {
808
809 }
810
811 for (int i = 0; i < 10; i++)
812 {
813 for (int j = i + 1; j < 10; j++)
814 {
815 double start = i * l.getLengthSI() / 10;
816 double end = j * l.getLengthSI() / 10;
817
818 for (OTSLine3D extractedLine : new OTSLine3D[] {l.extract(start, end),
819 l.extract(new Length(start, LengthUnit.SI), new Length(end, LengthUnit.SI)),
820 l.extractFractional(1.0 * i / 10, 1.0 * j / 10)})
821 {
822 assertEquals("size of extract is 2", 2, extractedLine.size());
823 assertEquals("x of 0", p0.x + (p1.x - p0.x) * i / 10, extractedLine.get(0).x, 0.0001);
824 assertEquals("y of 0", p0.y + (p1.y - p0.y) * i / 10, extractedLine.get(0).y, 0.0001);
825 assertEquals("z of 0", p0.z + (p1.z - p0.z) * i / 10, extractedLine.get(0).z, 0.0001);
826 assertEquals("x of 1", p0.x + (p1.x - p0.x) * j / 10, extractedLine.get(1).x, 0.0001);
827 assertEquals("y of 1", p0.y + (p1.y - p0.y) * j / 10, extractedLine.get(1).y, 0.0001);
828 assertEquals("z of 1", p0.z + (p1.z - p0.z) * j / 10, extractedLine.get(1).z, 0.0001);
829 }
830 }
831 }
832
833 for (OTSLine3D line : new OTSLine3D[] {new OTSLine3D(p0, p1, p2), new OTSLine3D(p0, p1, p1a, p1b, p1c, p2)})
834 {
835 for (int i = 0; i < 110; i++)
836 {
837 if (10 == i)
838 {
839 continue;
840 }
841 for (int j = i + 1; j < 110; j++)
842 {
843 if (10 == j)
844 {
845 continue;
846 }
847 double start = i * line.getLengthSI() / 110;
848 double end = j * line.getLengthSI() / 110;
849
850
851
852 for (OTSLine3D extractedLine : new OTSLine3D[] {line.extract(start, end),
853 line.extract(new Length(start, LengthUnit.SI), new Length(end, LengthUnit.SI)),
854 line.extractFractional(1.0 * i / 110, 1.0 * j / 110)})
855 {
856 int expectedSize = i < 10 && j > 10 ? line.size() : 2;
857 assertEquals("size is " + expectedSize, expectedSize, extractedLine.size());
858 if (i < 10)
859 {
860 assertEquals("x of 0", p0.x + (p1.x - p0.x) * i / 10, extractedLine.get(0).x, 0.0001);
861 assertEquals("y of 0", p0.y + (p1.y - p0.y) * i / 10, extractedLine.get(0).y, 0.0001);
862 assertEquals("z of 0", p0.z + (p1.z - p0.z) * i / 10, extractedLine.get(0).z, 0.0001);
863 }
864 else
865 {
866 assertEquals("x of 0", p1.x + (p2.x - p1.x) * (i - 10) / 100, extractedLine.get(0).x, 0.0001);
867 assertEquals("y of 0", p1.y + (p2.y - p1.y) * (i - 10) / 100, extractedLine.get(0).y, 0.0001);
868 assertEquals("z of 0", p1.z + (p2.z - p1.z) * (i - 10) / 100, extractedLine.get(0).z, 0.0001);
869 }
870 if (j < 10)
871 {
872 assertEquals("x of 1", p0.x + (p1.x - p0.x) * j / 10, extractedLine.get(1).x, 0.0001);
873 assertEquals("y of 1", p0.y + (p1.y - p0.y) * j / 10, extractedLine.get(1).y, 0.0001);
874 assertEquals("z of 1", p0.z + (p1.z - p0.z) * j / 10, extractedLine.get(1).z, 0.0001);
875 }
876 else
877 {
878 assertEquals("x of last", p1.x + (p2.x - p1.x) * (j - 10) / 100, extractedLine.getLast().x, 0.0001);
879 assertEquals("y of last", p1.y + (p2.y - p1.y) * (j - 10) / 100, extractedLine.getLast().y, 0.0001);
880 assertEquals("z of last", p1.z + (p2.z - p1.z) * (j - 10) / 100, extractedLine.getLast().z, 0.0001);
881 }
882 if (extractedLine.size() > 2)
883 {
884 assertEquals("x of mid", p1.x, extractedLine.get(1).x, 0.0001);
885 assertEquals("y of mid", p1.y, extractedLine.get(1).y, 0.0001);
886 assertEquals("z of mid", p1.z, extractedLine.get(1).z, 0.0001);
887 }
888 }
889 }
890 }
891 }
892 }
893
894
895
896
897
898 @Test
899 public final void offsetLineTest() throws OTSGeometryException
900 {
901 OTSPoint3D from = new OTSPoint3D(1, 2, 3);
902 OTSPoint3D to = new OTSPoint3D(4, 3, 2);
903 OTSLine3D line = new OTSLine3D(from, to);
904 double lineLengthHorizontal = new OTSPoint3D(from.x, from.y).distanceSI(new OTSPoint3D(to.x, to.y));
905 for (int step = -5; step <= 5; step++)
906 {
907 OTSLine3D offsetLine = line.offsetLine(step);
908 assertEquals("Offset line of a single straight segment has two points", 2, offsetLine.size());
909 assertEquals("Distance between start points should be equal to offset", Math.abs(step),
910 offsetLine.getFirst().horizontalDistanceSI(line.getFirst()), 0.0001);
911 assertEquals("Distance between end points should be equal to offset", Math.abs(step),
912 offsetLine.getLast().horizontalDistanceSI(line.getLast()), 0.0001);
913
914
915
916 assertEquals("Length of offset line of straight segment should equal length of reference line",
917 lineLengthHorizontal, offsetLine.getLengthSI(), 0.001);
918 }
919 OTSPoint3D via = new OTSPoint3D(4, 3, 3);
920 line = new OTSLine3D(from, via, to);
921 for (int step = -5; step <= 5; step++)
922 {
923 OTSLine3D offsetLine = line.offsetLine(step);
924
925
926
927 assertTrue("Offset line has > 2 points", 2 <= offsetLine.size());
928 assertEquals("Distance between start points should be equal to offset", Math.abs(step),
929 offsetLine.getFirst().horizontalDistanceSI(line.getFirst()), 0.0001);
930 assertEquals("Distance between end points should be equal to offset", Math.abs(step),
931 offsetLine.getLast().horizontalDistanceSI(line.getLast()), 0.0001);
932 }
933 }
934
935
936
937
938
939 @Test
940 public final void testFilter() throws OTSGeometryException
941 {
942 OTSPoint3D from = new OTSPoint3D(1, 2, 3);
943 OTSPoint3D to = new OTSPoint3D(4, 3, 2);
944 for (int steps = 0; steps < 10; steps++)
945 {
946 List<OTSPoint3D> points = new ArrayList<>(2 + steps);
947 points.add(from);
948 for (int i = 0; i < steps; i++)
949 {
950 points.add(OTSPoint3D.interpolate(1.0 * (i + 1) / (steps + 2), from, to));
951 }
952 points.add(to);
953 OTSLine3D line = new OTSLine3D(points);
954
955 double segmentLength = line.getFirst().distanceSI(line.get(1));
956 OTSLine3D filteredLine = line.noiseFilteredLine(segmentLength * 0.9);
957 assertEquals("filtering with a filter that is smaller than any segment should return the original", line.size(),
958 filteredLine.size());
959 filteredLine = line.noiseFilteredLine(segmentLength * 1.1);
960 int expectedSize = 2 + steps / 2;
961 assertEquals("filtering with a filter slightly larger than each segment should return a line with " + expectedSize
962 + " points", expectedSize, filteredLine.size());
963 filteredLine = line.noiseFilteredLine(segmentLength * 2.1);
964
965 expectedSize = 2 + (steps - 1) / 3;
966 assertEquals("filtering with a filter slightly larger than twice the length of each segment should return a "
967 + "line with " + expectedSize + " points", expectedSize, filteredLine.size());
968 }
969 }
970
971
972
973
974
975
976 @Test
977 public final void testFractionalProjection() throws OTSGeometryException
978 {
979 Direction zeroDir = Direction.ZERO;
980
981 OTSLine3D line = new OTSLine3D(new OTSPoint3D(0, 0), new OTSPoint3D(1, 1), new OTSPoint3D(2, 0), new OTSPoint3D(3, 1),
982 new OTSPoint3D(4, 0));
983 double fraction;
984 fraction = line.projectFractional(zeroDir, zeroDir, 1.5, -5.0, FractionalFallback.ORTHOGONAL);
985 checkGetLocation(line, fraction, new OTSPoint3D(1.5, .5, 0), Math.atan2(-1, 1));
986 fraction = line.projectFractional(zeroDir, zeroDir, 1.5, 5.0, FractionalFallback.ORTHOGONAL);
987 checkGetLocation(line, fraction, new OTSPoint3D(1.5, .5, 0), Math.atan2(-1, 1));
988 fraction = line.projectFractional(zeroDir, zeroDir, 2.5, -5.0, FractionalFallback.ORTHOGONAL);
989 checkGetLocation(line, fraction, new OTSPoint3D(2.5, .5, 0), Math.atan2(1, 1));
990 fraction = line.projectFractional(zeroDir, zeroDir, 2.5, 5.0, FractionalFallback.ORTHOGONAL);
991 checkGetLocation(line, fraction, new OTSPoint3D(2.5, .5, 0), Math.atan2(1, 1));
992
993 line = new OTSLine3D(new OTSPoint3D(0, 0), new OTSPoint3D(2, 2), new OTSPoint3D(4, 4), new OTSPoint3D(6, 6));
994 fraction = line.projectFractional(zeroDir, zeroDir, 2, 4, FractionalFallback.ORTHOGONAL);
995 checkGetLocation(line, fraction, new OTSPoint3D(3, 3, 0), Math.atan2(1, 1));
996 fraction = line.projectFractional(zeroDir, zeroDir, 4, 2, FractionalFallback.ORTHOGONAL);
997 checkGetLocation(line, fraction, new OTSPoint3D(3, 3, 0), Math.atan2(1, 1));
998
999 line = new OTSLine3D(new OTSPoint3D(-2, -2), new OTSPoint3D(2, -2), new OTSPoint3D(2, 2), new OTSPoint3D(-2, 2));
1000 for (double f = 0; f < 0; f += .1)
1001 {
1002 fraction = line.projectFractional(zeroDir, zeroDir, 1, -1 + f * 2, FractionalFallback.ORTHOGONAL);
1003
1004
1005
1006 checkGetLocation(line, fraction, new OTSPoint3D(2, -2 + f * 4, 0), Math.atan2(1, 0));
1007 }
1008
1009 double[] e = new double[] {1e-3, 1e-6, 1e-9, 1e-12, 1e-16, 1e-32};
1010 double[] d = new double[] {1e0, 1e1, 1e2, 1e3, 1e4, 1e5, 1e6, 1e9, 1e12};
1011 for (int i = 0; i < e.length; i++)
1012 {
1013 line = new OTSLine3D(new OTSPoint3D(e[i], 0), new OTSPoint3D(2 + e[i], 2), new OTSPoint3D(4, 4),
1014 new OTSPoint3D(6, 6 - e[i]), new OTSPoint3D(8, 8 - e[i]));
1015 for (int j = 0; j < d.length; j++)
1016 {
1017
1018 fraction = line.projectFractional(zeroDir, zeroDir, 4 - d[j], 4 + d[j], FractionalFallback.ENDPOINT);
1019 if (Math.abs(fraction - 0.5) > 0.001)
1020 {
1021 line.projectFractional(zeroDir, zeroDir, 4 - d[j], 4 + d[j], FractionalFallback.ENDPOINT);
1022 }
1023 if (e[i] >= 1e-3)
1024 {
1025 assertTrue("Projection of point on outside of very slight bend was wrong with e=" + e[i] + " and d=" + d[j],
1026 Math.abs(fraction - 0.5) < 0.001);
1027 }
1028 else
1029 {
1030 assertTrue("Projection of point on outside of very slight bend was wrong with e=" + e[i] + " and d=" + d[j],
1031 fraction >= 0.0 && fraction <= 1.0);
1032 }
1033 }
1034 }
1035
1036 Direction start = new Direction(Math.PI / 2, DirectionUnit.EAST_RADIAN);
1037 Direction end = new Direction(-Math.PI / 2, DirectionUnit.EAST_RADIAN);
1038 for (double radius : new double[] {1e16, 1e12, 1e9, 1e6, 1e3, 1, 0.1, 1e-3, 1e-6, 1e-9, 1e-12})
1039 {
1040 for (int n : new int[] {9, 10, 901, 1000})
1041 {
1042 List<OTSPoint3D> list = new ArrayList<>();
1043 for (double r = 0; r <= Math.PI; r += Math.PI / n)
1044 {
1045 list.add(new OTSPoint3D(Math.cos(r) * radius, Math.sin(r) * radius));
1046 }
1047 line = new OTSLine3D(list);
1048 for (double x : new double[] {0, 1e-3, 1e-6, 1e-9, 1e-12})
1049 {
1050 for (double y : new double[] {0, 1e-3, 1e-6, 1e-9, 1e-12})
1051 {
1052 double f = line.projectFractional(start, end, x, y, FractionalFallback.ORTHOGONAL);
1053 assertTrue("Fractional projection on circle is not between 0.0 and 1.0.", f >= 0.0 && f <= 1.0);
1054 }
1055 }
1056 }
1057 }
1058
1059 Random random = new Random(0);
1060 for (int n = 0; n < 10; n++)
1061 {
1062
1063 List<OTSPoint3D> list = new ArrayList<>();
1064 double prevX = 0;
1065 for (int i = 0; i < 100; i++)
1066 {
1067 double x = prevX + random.nextDouble() - 0.4;
1068 prevX = x;
1069 list.add(new OTSPoint3D(x, random.nextDouble()));
1070
1071 }
1072 line = new OTSLine3D(list);
1073 for (double x = -2; x < 12; x += 0.01)
1074 {
1075 for (double y = -1; y <= 2; y += 0.1)
1076 {
1077 double f = line.projectFractional(zeroDir, zeroDir, x, y, FractionalFallback.ORTHOGONAL);
1078 assertTrue("Fractional projection on circle is not between 0.0 and 1.0.", f >= 0.0 && f <= 1.0);
1079 }
1080 }
1081 }
1082
1083 line = new OTSLine3D(new OTSPoint3D(0, 0), new OTSPoint3D(1, 1));
1084 fraction = line.projectFractional(zeroDir, zeroDir, .5, 1, FractionalFallback.ORTHOGONAL);
1085 assertTrue("Projection on line with single segment is not correct.", Math.abs(fraction - 0.5) < 0.001);
1086
1087
1088
1089
1090
1091
1092
1093
1094
1095
1096
1097
1098
1099
1100
1101
1102
1103
1104
1105
1106
1107 }
1108
1109
1110
1111
1112
1113
1114
1115
1116
1117
1118 @Test
1119 public final void testFind() throws OTSGeometryException, NoSuchMethodException, SecurityException, IllegalAccessException,
1120 IllegalArgumentException, InvocationTargetException
1121 {
1122
1123 List<OTSPoint3D> points = new ArrayList<>();
1124 for (int i = 0; i < 20; i++)
1125 {
1126 points.add(new OTSPoint3D(Math.pow(2, i) - 1, 10, 20));
1127 }
1128 OTSLine3D line = new OTSLine3D(points);
1129 double end = points.get(points.size() - 1).x;
1130 Method findMethod = line.getClass().getDeclaredMethod("find", double.class);
1131 findMethod.setAccessible(true);
1132 for (int i = 0; i < end; i++)
1133 {
1134 double pos = i + 0.5;
1135 int index = (int) findMethod.invoke(line, pos);
1136 assertTrue("segment starts before pos", line.get(index).x <= pos);
1137 assertTrue("next segment starts after pos", line.get(index + 1).x >= pos);
1138 }
1139 assertEquals("pos 0 returns index 0", 0, (int) findMethod.invoke(line, 0.0));
1140 }
1141
1142 }