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