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.network.NetworkException;
22
23 import com.vividsolutions.jts.geom.Coordinate;
24 import com.vividsolutions.jts.geom.Geometry;
25 import com.vividsolutions.jts.geom.GeometryFactory;
26 import com.vividsolutions.jts.geom.LineString;
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 private 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
541
542
543
544 @Test
545 public final void reverseTest() throws OTSGeometryException
546 {
547 OTSPoint3D p0 = new OTSPoint3D(1.1, 2.2, 3.3);
548 OTSPoint3D p1 = new OTSPoint3D(2.1, 2.2, 3.3);
549 OTSPoint3D p2 = new OTSPoint3D(3.1, 2.2, 3.3);
550 OTSPoint3D p3 = new OTSPoint3D(4.1, 2.2, 3.3);
551 OTSPoint3D p4 = new OTSPoint3D(5.1, 2.2, 3.3);
552 OTSPoint3D p5 = new OTSPoint3D(6.1, 2.2, 3.3);
553
554 OTSLine3D l01 = new OTSLine3D(p0, p1);
555 OTSLine3D r = l01.reverse();
556 assertEquals("result has size 2", 2, r.size());
557 assertEquals("point 0 is p1", p1, r.get(0));
558 assertEquals("point 1 is p0", p0, r.get(1));
559
560 OTSLine3D l05 = new OTSLine3D(p0, p1, p2, p3, p4, p5);
561 r = l05.reverse();
562 assertEquals("result has size 6", 6, r.size());
563 assertEquals("point 0 is p5", p5, r.get(0));
564 assertEquals("point 1 is p4", p4, r.get(1));
565 assertEquals("point 2 is p3", p3, r.get(2));
566 assertEquals("point 3 is p2", p2, r.get(3));
567 assertEquals("point 4 is p1", p1, r.get(4));
568 assertEquals("point 5 is p0", p0, r.get(5));
569
570 }
571
572
573
574
575
576 @SuppressWarnings("checkstyle:methodlength")
577 @Test
578 public final void extractTest() throws OTSGeometryException
579 {
580 OTSPoint3D p0 = new OTSPoint3D(1, 2, 3);
581 OTSPoint3D p1 = new OTSPoint3D(2, 3, 4);
582 OTSPoint3D p1a = new OTSPoint3D(2.01, 3.01, 4.01);
583 OTSPoint3D p1b = new OTSPoint3D(2.02, 3.02, 4.02);
584 OTSPoint3D p1c = new OTSPoint3D(2.03, 3.03, 4.03);
585 OTSPoint3D p2 = new OTSPoint3D(12, 13, 14);
586
587 OTSLine3D l = new OTSLine3D(p0, p1);
588 OTSLine3D e = l.extractFractional(0, 1);
589 assertEquals("size of extraction is 2", 2, e.size());
590 assertEquals("point 0 is p0", p0, e.get(0));
591 assertEquals("point 1 is p1", p1, e.get(1));
592 try
593 {
594 l.extractFractional(-0.1, 1);
595 fail("negative start should have thrown an exception");
596 }
597 catch (OTSGeometryException exception)
598 {
599
600 }
601 try
602 {
603 l.extractFractional(Double.NaN, 1);
604 fail("NaN start should have thrown an exception");
605 }
606 catch (OTSGeometryException exception)
607 {
608
609 }
610 try
611 {
612 l.extractFractional(0, 1.1);
613 fail("end > 1 should have thrown an exception");
614 }
615 catch (OTSGeometryException exception)
616 {
617
618 }
619 try
620 {
621 l.extractFractional(0, Double.NaN);
622 fail("NaN end should have thrown an exception");
623 }
624 catch (OTSGeometryException exception)
625 {
626
627 }
628 try
629 {
630 l.extractFractional(0.6, 0.4);
631 fail("start > end should have thrown an exception");
632 }
633 catch (OTSGeometryException exception)
634 {
635
636 }
637 try
638 {
639 l.extract(-0.1, 1);
640 fail("negative start should have thrown an exception");
641 }
642 catch (OTSGeometryException exception)
643 {
644
645 }
646 try
647 {
648 l.extract(Double.NaN, 1);
649 fail("NaN start should have thrown an exception");
650 }
651 catch (OTSGeometryException exception)
652 {
653
654 }
655 try
656 {
657 l.extract(0, l.getLengthSI() + 0.1);
658 fail("end > length should have thrown an exception");
659 }
660 catch (OTSGeometryException exception)
661 {
662
663 }
664 try
665 {
666 l.extract(0, Double.NaN);
667 fail("NaN end should have thrown an exception");
668 }
669 catch (OTSGeometryException exception)
670 {
671
672 }
673 try
674 {
675 l.extract(0.6, 0.4);
676 fail("start > end should have thrown an exception");
677 }
678 catch (OTSGeometryException exception)
679 {
680
681 }
682
683 for (int i = 0; i < 10; i++)
684 {
685 for (int j = i + 1; j < 10; j++)
686 {
687 double start = i * l.getLengthSI() / 10;
688 double end = j * l.getLengthSI() / 10;
689
690 for (OTSLine3D extractedLine : new OTSLine3D[] { l.extract(start, end),
691 l.extract(new Length(start, LengthUnit.SI), new Length(end, LengthUnit.SI)),
692 l.extractFractional(1.0 * i / 10, 1.0 * j / 10) })
693 {
694 assertEquals("size of extract is 2", 2, extractedLine.size());
695 assertEquals("x of 0", p0.x + (p1.x - p0.x) * i / 10, extractedLine.get(0).x, 0.0001);
696 assertEquals("y of 0", p0.y + (p1.y - p0.y) * i / 10, extractedLine.get(0).y, 0.0001);
697 assertEquals("z of 0", p0.z + (p1.z - p0.z) * i / 10, extractedLine.get(0).z, 0.0001);
698 assertEquals("x of 1", p0.x + (p1.x - p0.x) * j / 10, extractedLine.get(1).x, 0.0001);
699 assertEquals("y of 1", p0.y + (p1.y - p0.y) * j / 10, extractedLine.get(1).y, 0.0001);
700 assertEquals("z of 1", p0.z + (p1.z - p0.z) * j / 10, extractedLine.get(1).z, 0.0001);
701 }
702 }
703 }
704
705 for (OTSLine3D line : new OTSLine3D[] { new OTSLine3D(p0, p1, p2), new OTSLine3D(p0, p1, p1a, p1b, p1c, p2) })
706 {
707 for (int i = 0; i < 110; i++)
708 {
709 if (10 == i)
710 {
711 continue;
712 }
713 for (int j = i + 1; j < 110; j++)
714 {
715 if (10 == j)
716 {
717 continue;
718 }
719 double start = i * line.getLengthSI() / 110;
720 double end = j * line.getLengthSI() / 110;
721
722
723
724 for (OTSLine3D extractedLine : new OTSLine3D[] { line.extract(start, end),
725 line.extract(new Length(start, LengthUnit.SI), new Length(end, LengthUnit.SI)),
726 line.extractFractional(1.0 * i / 110, 1.0 * j / 110) })
727 {
728 int expectedSize = i < 10 && j > 10 ? line.size() : 2;
729 assertEquals("size is " + expectedSize, expectedSize, extractedLine.size());
730 if (i < 10)
731 {
732 assertEquals("x of 0", p0.x + (p1.x - p0.x) * i / 10, extractedLine.get(0).x, 0.0001);
733 assertEquals("y of 0", p0.y + (p1.y - p0.y) * i / 10, extractedLine.get(0).y, 0.0001);
734 assertEquals("z of 0", p0.z + (p1.z - p0.z) * i / 10, extractedLine.get(0).z, 0.0001);
735 }
736 else
737 {
738 assertEquals("x of 0", p1.x + (p2.x - p1.x) * (i - 10) / 100, extractedLine.get(0).x, 0.0001);
739 assertEquals("y of 0", p1.y + (p2.y - p1.y) * (i - 10) / 100, extractedLine.get(0).y, 0.0001);
740 assertEquals("z of 0", p1.z + (p2.z - p1.z) * (i - 10) / 100, extractedLine.get(0).z, 0.0001);
741 }
742 if (j < 10)
743 {
744 assertEquals("x of 1", p0.x + (p1.x - p0.x) * j / 10, extractedLine.get(1).x, 0.0001);
745 assertEquals("y of 1", p0.y + (p1.y - p0.y) * j / 10, extractedLine.get(1).y, 0.0001);
746 assertEquals("z of 1", p0.z + (p1.z - p0.z) * j / 10, extractedLine.get(1).z, 0.0001);
747 }
748 else
749 {
750 assertEquals("x of last", p1.x + (p2.x - p1.x) * (j - 10) / 100, extractedLine.getLast().x, 0.0001);
751 assertEquals("y of last", p1.y + (p2.y - p1.y) * (j - 10) / 100, extractedLine.getLast().y, 0.0001);
752 assertEquals("z of last", p1.z + (p2.z - p1.z) * (j - 10) / 100, extractedLine.getLast().z, 0.0001);
753 }
754 if (extractedLine.size() > 2)
755 {
756 assertEquals("x of mid", p1.x, extractedLine.get(1).x, 0.0001);
757 assertEquals("y of mid", p1.y, extractedLine.get(1).y, 0.0001);
758 assertEquals("z of mid", p1.z, extractedLine.get(1).z, 0.0001);
759 }
760 }
761 }
762 }
763 }
764 }
765
766
767
768
769
770 @Test
771 public final void offsetLineTest() throws OTSGeometryException
772 {
773 OTSPoint3D from = new OTSPoint3D(1, 2, 3);
774 OTSPoint3D to = new OTSPoint3D(4, 3, 2);
775 OTSLine3D line = new OTSLine3D(from, to);
776 double lineLengthHorizontal = new OTSPoint3D(from.x, from.y).distanceSI(new OTSPoint3D(to.x, to.y));
777 for (int step = -5; step <= 5; step++)
778 {
779 OTSLine3D offsetLine = line.offsetLine(step);
780 assertEquals("Offset line of a single straight segment has two points", 2, offsetLine.size());
781 assertEquals("Distance between start points should be equal to offset", Math.abs(step),
782 offsetLine.getFirst().horizontalDistanceSI(line.getFirst()), 0.0001);
783 assertEquals("Distance between end points should be equal to offset", Math.abs(step),
784 offsetLine.getLast().horizontalDistanceSI(line.getLast()), 0.0001);
785
786
787
788 assertEquals("Length of offset line of straight segment should equal length of reference line",
789 lineLengthHorizontal, offsetLine.getLengthSI(), 0.001);
790 }
791 OTSPoint3D via = new OTSPoint3D(4, 3, 3);
792 line = new OTSLine3D(from, via, to);
793 for (int step = -5; step <= 5; step++)
794 {
795 OTSLine3D offsetLine = line.offsetLine(step);
796
797
798
799 assertTrue("Offset line has > 2 points", 2 <= offsetLine.size());
800 assertEquals("Distance between start points should be equal to offset", Math.abs(step),
801 offsetLine.getFirst().horizontalDistanceSI(line.getFirst()), 0.0001);
802 assertEquals("Distance between end points should be equal to offset", Math.abs(step),
803 offsetLine.getLast().horizontalDistanceSI(line.getLast()), 0.0001);
804 }
805 }
806
807
808
809
810
811 @Test
812 public final void testFilter() throws OTSGeometryException
813 {
814 OTSPoint3D from = new OTSPoint3D(1, 2, 3);
815 OTSPoint3D to = new OTSPoint3D(4, 3, 2);
816 for (int steps = 0; steps < 10; steps++)
817 {
818 List<OTSPoint3D> points = new ArrayList<>(2 + steps);
819 points.add(from);
820 for (int i = 0; i < steps; i++)
821 {
822 points.add(OTSPoint3D.interpolate(1.0 * (i + 1) / (steps + 2), from, to));
823 }
824 points.add(to);
825 OTSLine3D line = new OTSLine3D(points);
826
827 double segmentLength = line.getFirst().distanceSI(line.get(1));
828 OTSLine3D filteredLine = line.noiseFilteredLine(segmentLength * 0.9);
829 assertEquals("filtering with a filter that is smaller than any segment should return the original", line.size(),
830 filteredLine.size());
831 filteredLine = line.noiseFilteredLine(segmentLength * 1.1);
832 int expectedSize = 2 + steps / 2;
833 assertEquals("filtering with a filter slightly larger than each segment should return a line with " + expectedSize
834 + " points", expectedSize, filteredLine.size());
835 filteredLine = line.noiseFilteredLine(segmentLength * 2.1);
836
837 expectedSize = 2 + (steps - 1) / 3;
838 assertEquals("filtering with a filter slightly larger than twice the length of each segment should return a "
839 + "line with " + expectedSize + " points", expectedSize, filteredLine.size());
840 }
841 }
842
843
844
845
846
847 @Test
848 public final void testFractionalProjection() throws OTSGeometryException
849 {
850 Direction zeroDir = Direction.ZERO;
851
852 OTSLine3D line = new OTSLine3D(new OTSPoint3D(0, 0), new OTSPoint3D(1, 1), new OTSPoint3D(2, 0), new OTSPoint3D(3, 1),
853 new OTSPoint3D(4, 0));
854 double fraction;
855 fraction = line.projectFractional(zeroDir, zeroDir, 1.5, -5.0);
856 checkGetLocation(line, fraction, new OTSPoint3D(1.5, .5, 0), Math.atan2(-1, 1));
857 fraction = line.projectFractional(zeroDir, zeroDir, 1.5, 5.0);
858 checkGetLocation(line, fraction, new OTSPoint3D(1.5, .5, 0), Math.atan2(-1, 1));
859 fraction = line.projectFractional(zeroDir, zeroDir, 2.5, -5.0);
860 checkGetLocation(line, fraction, new OTSPoint3D(2.5, .5, 0), Math.atan2(1, 1));
861 fraction = line.projectFractional(zeroDir, zeroDir, 2.5, 5.0);
862 checkGetLocation(line, fraction, new OTSPoint3D(2.5, .5, 0), Math.atan2(1, 1));
863
864 line = new OTSLine3D(new OTSPoint3D(0, 0), new OTSPoint3D(2, 2), new OTSPoint3D(4, 4), new OTSPoint3D(6, 6));
865 fraction = line.projectFractional(zeroDir, zeroDir, 2, 4);
866 checkGetLocation(line, fraction, new OTSPoint3D(3, 3, 0), Math.atan2(1, 1));
867 fraction = line.projectFractional(zeroDir, zeroDir, 4, 2);
868 checkGetLocation(line, fraction, new OTSPoint3D(3, 3, 0), Math.atan2(1, 1));
869
870 line = new OTSLine3D(new OTSPoint3D(-2, -2), new OTSPoint3D(2, -2), new OTSPoint3D(2, 2), new OTSPoint3D(-2, 2));
871 for (double f = 0; f < 0; f += .1)
872 {
873 fraction = line.projectFractional(zeroDir, zeroDir, 1, -1 + f * 2);
874 checkGetLocation(line, fraction, new OTSPoint3D(2, -2 + f * 4, 0), Math.atan2(1, 0));
875 }
876
877 double[] e = new double[] { 1e-3, 1e-6, 1e-9, 1e-12, 1e-16, 1e-32 };
878 double[] d = new double[] { 1e0, 1e1, 1e2, 1e3, 1e4, 1e5, 1e6, 1e9, 1e12 };
879 for (int i = 0; i < e.length; i++)
880 {
881 line = new OTSLine3D(new OTSPoint3D(e[i], 0), new OTSPoint3D(2 + e[i], 2), new OTSPoint3D(4, 4),
882 new OTSPoint3D(6, 6 - e[i]), new OTSPoint3D(8, 8 - e[i]));
883 for (int j = 0; j < d.length; j++)
884 {
885 fraction = line.projectFractional(zeroDir, zeroDir, 4 - d[j], 4 + d[j]);
886 if (Math.abs(fraction - 0.5) > 0.001)
887 {
888 line.projectFractional(zeroDir, zeroDir, 4 - d[j], 4 + d[j]);
889 }
890 if (e[i] >= 1e-6)
891 {
892 assertTrue("Projection of point on outside of very slight bend was wrong with e=" + e[i] + " and d=" + d[j],
893 Math.abs(fraction - 0.5) < 0.001);
894 }
895 else
896 {
897 assertTrue("Projection of point on outside of very slight bend was wrong with e=" + e[i] + " and d=" + d[j],
898 fraction >= 0.0 && fraction <= 1.0);
899 }
900 }
901 }
902
903 Direction start = new Direction(Math.PI / 2, DirectionUnit.NORTH_RADIAN);
904 Direction end = new Direction(-Math.PI / 2, DirectionUnit.NORTH_RADIAN);
905 for (double radius : new double[] { 1e16, 1e12, 1e9, 1e6, 1e3, 1, 0.1, 1e-3, 1e-6, 1e-9, 1e-12 })
906 {
907 for (int n : new int[] { 9, 10, 901, 1000 })
908 {
909 List<OTSPoint3D> list = new ArrayList<>();
910 for (double r = 0; r <= Math.PI; r += Math.PI / n)
911 {
912 list.add(new OTSPoint3D(Math.cos(r) * radius, Math.sin(r) * radius));
913 }
914 line = new OTSLine3D(list);
915 for (double x : new double[] { 0, 1e-3, 1e-6, 1e-9, 1e-12 })
916 {
917 for (double y : new double[] { 0, 1e-3, 1e-6, 1e-9, 1e-12 })
918 {
919 double f = line.projectFractional(start, end, x, y);
920 assertTrue("Fractional projection on circle is not between 0.0 and 1.0.", f >= 0.0 && f <= 1.0);
921 }
922 }
923 }
924 }
925
926 Random random = new Random(0);
927 for (int n = 0; n < 10; n++)
928 {
929
930 List<OTSPoint3D> list = new ArrayList<>();
931 double prevX = 0;
932 for (int i = 0; i < 100; i++)
933 {
934 double x = prevX + random.nextDouble() - 0.4;
935 prevX = x;
936 list.add(new OTSPoint3D(x, random.nextDouble()));
937
938 }
939 line = new OTSLine3D(list);
940 for (double x = -2; x < 12; x += 0.01)
941 {
942 for (double y = -1; y <= 2; y += 0.1)
943 {
944 double f = line.projectFractional(zeroDir, zeroDir, x, y);
945 assertTrue("Fractional projection on circle is not between 0.0 and 1.0.", f >= 0.0 && f <= 1.0);
946 }
947 }
948 }
949
950 line = new OTSLine3D(new OTSPoint3D(0, 0), new OTSPoint3D(1, 1));
951 fraction = line.projectFractional(zeroDir, zeroDir, .5, 1);
952 assertTrue("Projection on line with single segment is not correct.", Math.abs(fraction - 0.5) < 0.001);
953
954
955
956
957
958
959
960
961
962
963
964
965
966
967
968
969
970
971
972
973
974 }
975
976
977
978
979
980
981
982
983
984
985 @Test
986 public final void testFind() throws OTSGeometryException, NoSuchMethodException, SecurityException, IllegalAccessException,
987 IllegalArgumentException, InvocationTargetException
988 {
989
990 List<OTSPoint3D> points = new ArrayList<>();
991 for (int i = 0; i < 20; i++)
992 {
993 points.add(new OTSPoint3D(Math.pow(2, i) - 1, 10, 20));
994 }
995 OTSLine3D line = new OTSLine3D(points);
996 double end = points.get(points.size() - 1).x;
997 Method findMethod = line.getClass().getDeclaredMethod("find", double.class);
998 findMethod.setAccessible(true);
999 for (int i = 0; i < end; i++)
1000 {
1001 double pos = i + 0.5;
1002 int index = (int) findMethod.invoke(line, pos);
1003 assertTrue("segment starts before pos", line.get(index).x <= pos);
1004 assertTrue("next segment starts after pos", line.get(index + 1).x >= pos);
1005 }
1006 assertEquals("pos 0 returns index 0", 0, (int) findMethod.invoke(line, 0.0));
1007 }
1008 }