1 package org.djutils.draw.line;
2
3 import java.awt.geom.Path2D;
4 import java.awt.geom.PathIterator;
5 import java.util.ArrayList;
6 import java.util.Arrays;
7 import java.util.Iterator;
8 import java.util.List;
9 import java.util.Locale;
10 import java.util.NoSuchElementException;
11 import java.util.function.Function;
12
13 import org.djutils.draw.DrawRuntimeException;
14 import org.djutils.draw.Drawable2d;
15 import org.djutils.draw.bounds.Bounds2d;
16 import org.djutils.draw.point.Point2d;
17 import org.djutils.exceptions.Throw;
18 import org.djutils.logger.CategoryLogger;
19
20
21
22
23
24
25
26
27
28
29 public class PolyLine2d implements Drawable2d, PolyLine<PolyLine2d, Point2d, Ray2d, LineSegment2d>
30 {
31
32 private static final long serialVersionUID = 20200911L;
33
34
35 private final double[] x;
36
37
38 private final double[] y;
39
40
41 private final double[] lengthIndexedLine;
42
43
44 private final double length;
45
46
47 private final Bounds2d bounds;
48
49
50 private final double startHeading;
51
52
53
54
55
56
57
58
59
60
61 PolyLine2d(final boolean copyNeeded, final double[] x, final double[] y) throws NullPointerException, DrawRuntimeException
62 {
63 Throw.whenNull(x, "x array may not be null");
64 Throw.whenNull(y, "y array may not be null");
65 Throw.when(x.length != y.length, DrawRuntimeException.class, "x and y arrays must have same length");
66 Throw.when(x.length < 2, DrawRuntimeException.class, "Need at least two points");
67 this.x = copyNeeded ? Arrays.copyOf(x, x.length) : x;
68 this.y = copyNeeded ? Arrays.copyOf(y, y.length) : y;
69 double minX = x[0];
70 double minY = y[0];
71 double maxX = x[0];
72 double maxY = y[0];
73 this.lengthIndexedLine = new double[x.length];
74 this.lengthIndexedLine[0] = 0.0;
75 for (int i = 1; i < x.length; i++)
76 {
77 minX = Math.min(minX, x[i]);
78 minY = Math.min(minY, y[i]);
79 maxX = Math.max(maxX, x[i]);
80 maxY = Math.max(maxY, y[i]);
81 if (x[i - 1] == x[i] && y[i - 1] == y[i])
82 {
83 throw new DrawRuntimeException(
84 "Degenerate PolyLine2d; point " + (i - 1) + " has the same x and y as point " + i);
85 }
86 this.lengthIndexedLine[i] = this.lengthIndexedLine[i - 1] + Math.hypot(x[i] - x[i - 1], y[i] - y[i - 1]);
87 }
88 this.length = this.lengthIndexedLine[this.lengthIndexedLine.length - 1];
89 this.bounds = new Bounds2d(minX, maxX, minY, maxY);
90 this.startHeading = Double.NaN;
91 }
92
93
94
95
96
97
98
99
100 public PolyLine2d(final double x, final double y, final double heading) throws DrawRuntimeException
101 {
102 Throw.when(Double.isNaN(x), DrawRuntimeException.class, "x may not be NaN");
103 Throw.when(Double.isNaN(y), DrawRuntimeException.class, "y may not be NaN");
104 Throw.when(Double.isNaN(heading), DrawRuntimeException.class, "heading may not be NaN");
105 Throw.when(Double.isInfinite(heading), DrawRuntimeException.class, "heading must be finite");
106 this.x = new double[] { x };
107 this.y = new double[] { y };
108 this.startHeading = heading;
109 this.length = 0;
110 this.bounds = new Bounds2d(x, x, y, y);
111 this.lengthIndexedLine = new double[] { 0.0 };
112 }
113
114
115
116
117
118
119
120
121 public PolyLine2d(final Point2d p, final double heading) throws NullPointerException, DrawRuntimeException
122 {
123 this(Throw.whenNull(p, "p may not be null").x, p.y, heading);
124 }
125
126
127
128
129
130
131
132 public PolyLine2d(final Ray2d r) throws NullPointerException, DrawRuntimeException
133 {
134 this(Throw.whenNull(r, "r may not be NaN").x, r.y, r.phi);
135 }
136
137
138
139
140
141
142
143
144
145 public PolyLine2d(final double[] x, final double[] y) throws NullPointerException, DrawRuntimeException
146 {
147 this(true, x, y);
148 }
149
150
151
152
153
154
155
156
157 public PolyLine2d(final Point2d[] points) throws NullPointerException, DrawRuntimeException
158 {
159 this(false, makeArray(Throw.whenNull(points, "points may not be null"), p -> p.x), makeArray(points, p -> p.y));
160 }
161
162
163
164
165
166
167
168 protected static double[] makeArray(final Point2d[] points, final Function<Point2d, Double> getter)
169 {
170 double[] array = new double[points.length];
171 for (int index = 0; index < points.length; index++)
172 {
173 array[index] = getter.apply(points[index]);
174 }
175 return array;
176 }
177
178
179
180
181
182
183
184
185
186
187 public PolyLine2d(final Point2d point1, final Point2d point2, final Point2d... otherPoints)
188 throws NullPointerException, DrawRuntimeException
189 {
190 this(spliceArray(Throw.whenNull(point1, "point1 may not be null"), Throw.whenNull(point2, "point2 may not be null"),
191 otherPoints));
192 }
193
194
195
196
197
198
199
200
201
202 private static Point2d[] spliceArray(final Point2d point1, final Point2d point2, final Point2d... otherPoints)
203 {
204 Point2d[] result = new Point2d[2 + (otherPoints == null ? 0 : otherPoints.length)];
205 result[0] = point1;
206 result[1] = point2;
207 if (otherPoints != null)
208 {
209 for (int i = 0; i < otherPoints.length; i++)
210 {
211 result[i + 2] = otherPoints[i];
212 }
213 }
214 return result;
215 }
216
217
218
219
220
221
222
223 public PolyLine2d(final Iterator<Point2d> iterator) throws NullPointerException, DrawRuntimeException
224 {
225 this(iteratorToList(Throw.whenNull(iterator, "iterator cannot be null")));
226 }
227
228
229
230
231
232
233
234
235 public PolyLine2d(final List<Point2d> pointList) throws DrawRuntimeException, NullPointerException
236 {
237 this(pointList.toArray(new Point2d[Throw.whenNull(pointList, "pointList may not be null").size()]));
238 }
239
240
241
242
243
244
245
246
247 public PolyLine2d(final Path2D path) throws DrawRuntimeException, NullPointerException
248 {
249 this(path2DtoArray(Throw.whenNull(path, "path may not be null")));
250 }
251
252
253
254
255
256
257
258 private static Point2d[] path2DtoArray(final Path2D path) throws DrawRuntimeException
259 {
260 List<Point2d> result = new ArrayList<>();
261 for (PathIterator pi = path.getPathIterator(null); !pi.isDone(); pi.next())
262 {
263 double[] p = new double[6];
264 int segType = pi.currentSegment(p);
265 if (segType == PathIterator.SEG_MOVETO || segType == PathIterator.SEG_LINETO)
266 {
267 result.add(new Point2d(p[0], p[1]));
268 }
269 else if (segType == PathIterator.SEG_CLOSE)
270 {
271 if (!result.get(0).equals(result.get(result.size() - 1)))
272 {
273 result.add(result.get(0));
274 }
275 break;
276 }
277 else
278 {
279 throw new DrawRuntimeException("path2DtoArray only handles SEG_MOVETO, SEG_LINETO and SEG_CLOSE");
280 }
281 }
282 return result.toArray(new Point2d[result.size() - 1]);
283 }
284
285
286
287
288
289
290 protected static List<Point2d> iteratorToList(final Iterator<Point2d> iterator)
291 {
292 List<Point2d> result = new ArrayList<>();
293 iterator.forEachRemaining(result::add);
294 return result;
295 }
296
297
298
299
300
301
302
303 public PolyLine2d(final boolean filterDuplicates, final Point2d... points) throws DrawRuntimeException
304 {
305 this(PolyLine2d.cleanPoints(filterDuplicates, Arrays.stream(points).iterator()));
306 }
307
308
309
310
311
312
313
314
315 public PolyLine2d(final boolean filterDuplicates, final List<Point2d> pointList) throws DrawRuntimeException
316 {
317 this(PolyLine2d.cleanPoints(filterDuplicates, pointList.iterator()));
318 }
319
320
321
322
323
324
325
326 static Iterator<Point2d> cleanPoints(final boolean filter, final Iterator<Point2d> iterator)
327 {
328 Throw.whenNull(iterator, "Iterator may not be null");
329 Throw.when(!iterator.hasNext(), DrawRuntimeException.class, "Iterator has no points to return");
330 if (!filter)
331 {
332 return iterator;
333 }
334 return new Iterator<Point2d>()
335 {
336 private Point2d currentPoint = iterator.next();
337
338 @Override
339 public boolean hasNext()
340 {
341 return this.currentPoint != null;
342 }
343
344 @Override
345 public Point2d next()
346 {
347 Throw.when(this.currentPoint == null, NoSuchElementException.class, "Out of input");
348 Point2d result = this.currentPoint;
349 this.currentPoint = null;
350 while (iterator.hasNext())
351 {
352 this.currentPoint = iterator.next();
353 if (result.x != this.currentPoint.x || result.y != this.currentPoint.y)
354 {
355 break;
356 }
357 this.currentPoint = null;
358 }
359 return result;
360 }
361 };
362 }
363
364
365 @Override
366 public PolyLine2d instantiate(final List<Point2d> pointList) throws NullPointerException, DrawRuntimeException
367 {
368 return new PolyLine2d(pointList);
369 }
370
371
372 @Override
373 public int size()
374 {
375 return this.x.length;
376 }
377
378
379 @Override
380 public final Point2d get(final int i) throws IndexOutOfBoundsException
381 {
382 return new Point2d(this.x[i], this.y[i]);
383 }
384
385
386 @Override
387 public final double getX(final int i) throws IndexOutOfBoundsException
388 {
389 return this.x[i];
390 }
391
392
393 @Override
394 public final double getY(final int i) throws IndexOutOfBoundsException
395 {
396 return this.y[i];
397 }
398
399
400 @Override
401 public LineSegment2d getSegment(final int index)
402 {
403 Throw.when(index < 0 || index >= this.x.length - 1, DrawRuntimeException.class, "index must be in range 0..size() - 1");
404 return new LineSegment2d(this.x[index], this.y[index], this.x[index + 1], this.y[index + 1]);
405 }
406
407
408 @Override
409 public final double lengthAtIndex(final int index)
410 {
411 return this.lengthIndexedLine[index];
412 }
413
414
415 @Override
416 public double getLength()
417 {
418 return this.length;
419 }
420
421
422 @Override
423 public Iterator<Point2d> getPoints()
424 {
425 return new Iterator<Point2d>()
426 {
427 private int nextIndex = 0;
428
429
430 @Override
431 public boolean hasNext()
432 {
433 return this.nextIndex < size();
434 }
435
436
437 @Override
438 public Point2d next()
439 {
440 return get(this.nextIndex++);
441 }
442 };
443 }
444
445
446 @Override
447 public Bounds2d getBounds()
448 {
449 return this.bounds;
450 }
451
452
453 @Override
454 public final PolyLine2d noiseFilteredLine(final double noiseLevel)
455 {
456 if (this.size() <= 2)
457 {
458 return this;
459 }
460 Point2d prevPoint = null;
461 List<Point2d> list = new ArrayList<>();
462 for (int index = 0; index < this.size(); index++)
463 {
464 Point2d currentPoint = get(index);
465 if (null != prevPoint && prevPoint.distance(currentPoint) < noiseLevel)
466 {
467 if (index == this.size() - 1)
468 {
469 if (list.size() > 1)
470 {
471
472 list.set(list.size() - 1, currentPoint);
473 }
474 else
475 {
476
477
478 list.add(currentPoint);
479 }
480 }
481 continue;
482 }
483 list.add(currentPoint);
484 prevPoint = currentPoint;
485 }
486 if (list.size() == this.x.length)
487 {
488 return this;
489 }
490 if (list.size() == 2 && list.get(0).equals(list.get(1)))
491 {
492
493 list.add(1, get(1));
494 }
495 try
496 {
497 return new PolyLine2d(list);
498 }
499 catch (DrawRuntimeException exception)
500 {
501
502 CategoryLogger.always().error(exception);
503 throw new Error(exception);
504 }
505 }
506
507
508
509
510
511
512
513
514 public static PolyLine2d concatenate(final PolyLine2d... lines) throws DrawRuntimeException
515 {
516 return concatenate(0.0, lines);
517 }
518
519
520
521
522
523
524
525
526
527 public static PolyLine2d concatenate(final double tolerance, final PolyLine2d line1, final PolyLine2d line2)
528 throws DrawRuntimeException
529 {
530 if (line1.getLast().distance(line2.getFirst()) > tolerance)
531 {
532 throw new DrawRuntimeException("Lines are not connected: " + line1.getLast() + " to " + line2.getFirst()
533 + " distance is " + line1.getLast().distance(line2.getFirst()) + " > " + tolerance);
534 }
535 int size = line1.size() + line2.size() - 1;
536 Point2d[] points = new Point2d[size];
537 int nextIndex = 0;
538 for (int j = 0; j < line1.size(); j++)
539 {
540 points[nextIndex++] = line1.get(j);
541 }
542 for (int j = 1; j < line2.size(); j++)
543 {
544 points[nextIndex++] = line2.get(j);
545 }
546 return new PolyLine2d(points);
547 }
548
549
550
551
552
553
554
555
556
557 public static PolyLine2d concatenate(final double tolerance, final PolyLine2d... lines) throws DrawRuntimeException
558 {
559 if (0 == lines.length)
560 {
561 throw new DrawRuntimeException("Empty argument list");
562 }
563 else if (1 == lines.length)
564 {
565 return lines[0];
566 }
567 int size = lines[0].size();
568 for (int i = 1; i < lines.length; i++)
569 {
570 if (lines[i - 1].getLast().distance(lines[i].getFirst()) > tolerance)
571 {
572 throw new DrawRuntimeException(
573 "Lines are not connected: " + lines[i - 1].getLast() + " to " + lines[i].getFirst() + " distance is "
574 + lines[i - 1].getLast().distance(lines[i].getFirst()) + " > " + tolerance);
575 }
576 size += lines[i].size() - 1;
577 }
578 Point2d[] points = new Point2d[size];
579 int nextIndex = 0;
580 for (int i = 0; i < lines.length; i++)
581 {
582 PolyLine2d line = lines[i];
583 for (int j = 0 == i ? 0 : 1; j < line.size(); j++)
584 {
585 points[nextIndex++] = line.get(j);
586 }
587 }
588 return new PolyLine2d(points);
589 }
590
591
592 @Override
593 public final Ray2d getLocationExtended(final double position)
594 {
595 if (position >= 0.0 && position <= getLength())
596 {
597 try
598 {
599 return getLocation(position);
600 }
601 catch (DrawRuntimeException exception)
602 {
603
604 CategoryLogger.always().error(exception);
605 throw new Error(exception);
606 }
607 }
608
609
610 if (position < 0.0)
611 {
612 double fraction = position / (this.lengthIndexedLine[1] - this.lengthIndexedLine[0]);
613 return new Ray2d(this.x[0] + fraction * (this.x[1] - this.x[0]), this.y[0] + fraction * (this.y[1] - this.y[0]),
614 this.x[1], this.y[1]);
615 }
616
617
618
619 int n1 = this.x.length - 1;
620 int n2 = this.x.length - 2;
621 double len = position - getLength();
622 double fraction = len / (this.lengthIndexedLine[n1] - this.lengthIndexedLine[n2]);
623 while (Double.isInfinite(fraction))
624 {
625
626 if (--n2 < 0)
627 {
628 CategoryLogger.always().error("lengthIndexedLine of {} is invalid", this);
629 return new Ray2d(this.x[n1], this.y[n1], 0.0);
630 }
631 fraction = len / (this.lengthIndexedLine[n1] - this.lengthIndexedLine[n2]);
632 }
633 return new Ray2d(this.x[n1] + fraction * (this.x[n1] - this.x[n2]), this.y[n1] + fraction * (this.y[n1] - this.y[n2]),
634 Math.atan2(this.y[n1] - this.y[n2], this.x[n1] - this.x[n2]));
635 }
636
637
638 @Override
639 public final Ray2d getLocation(final double position) throws DrawRuntimeException
640 {
641 Throw.when(Double.isNaN(position), DrawRuntimeException.class, "position may not be NaN");
642 Throw.when(position < 0.0 || position > getLength(), DrawRuntimeException.class,
643 "getLocation for line: position < 0.0 or > line length. Position = " + position + "; length = " + getLength());
644
645 if (position == 0.0)
646 {
647 if (this.lengthIndexedLine.length == 1)
648 {
649 return new Ray2d(this.x[0], this.y[0], this.startHeading);
650 }
651 return new Ray2d(this.x[0], this.y[0], this.x[1], this.y[1]);
652 }
653 if (position == getLength())
654 {
655 return new Ray2d(this.x[this.x.length - 1], this.y[this.x.length - 1],
656 2 * this.x[this.x.length - 1] - this.x[this.x.length - 2],
657 2 * this.y[this.x.length - 1] - this.y[this.x.length - 2]);
658 }
659
660 int index = find(position);
661 double remainder = position - this.lengthIndexedLine[index];
662 double fraction = remainder / (this.lengthIndexedLine[index + 1] - this.lengthIndexedLine[index]);
663
664
665
666
667
668
669
670 return new Ray2d(this.x[index] + fraction * (this.x[index + 1] - this.x[index]),
671 this.y[index] + fraction * (this.y[index + 1] - this.y[index]), 2 * this.x[index + 1] - this.x[index],
672 2 * this.y[index + 1] - this.y[index]);
673 }
674
675
676
677
678
679
680
681
682
683 private double projectOrthogonalFractional(final Point2d point, final Boolean limitHandling)
684 {
685 Throw.whenNull(point, "point may not be null");
686 double result = Double.NaN;
687 if (this.lengthIndexedLine.length == 1)
688 {
689
690 if (null != limitHandling && limitHandling)
691 {
692 return 0.0;
693 }
694 result = getLocation(0.0).projectOrthogonalFractionalExtended(point);
695 if (null == limitHandling)
696 {
697 return result == 0.0 ? 0.0 : Double.NaN;
698 }
699
700 if (result == 0.0)
701 {
702 return 0.0;
703 }
704 return result > 0 ? Double.POSITIVE_INFINITY : Double.NEGATIVE_INFINITY;
705 }
706 double bestDistance = Double.POSITIVE_INFINITY;
707 double bestDistanceExtended = Double.POSITIVE_INFINITY;
708 for (int index = 1; index < this.size(); index++)
709 {
710 double fraction = point.fractionalPositionOnLine(this.x[index - 1], this.y[index - 1], this.x[index], this.y[index],
711 false, false);
712 double distance = Math.hypot(point.x - (this.x[index - 1] + fraction * (this.x[index] - this.x[index - 1])),
713 point.y - (this.y[index - 1] + fraction * (this.y[index] - this.y[index - 1])));
714 if (distance < bestDistanceExtended && (fraction >= 0.0 && fraction <= 1.0 || (fraction < 0.0 && index == 1)
715 || fraction > 1.0 && index == this.size() - 1))
716 {
717 bestDistanceExtended = distance;
718 }
719 if (distance < bestDistance && (fraction >= 0.0 || index == 1 && limitHandling != null && !limitHandling)
720 && (fraction <= 1.0 || index == this.size() - 1 && limitHandling != null && !limitHandling))
721 {
722 bestDistance = distance;
723 result = lengthAtIndex(index - 1) + fraction * (lengthAtIndex(index) - lengthAtIndex(index - 1));
724 }
725 else if (fraction < 0.0 && limitHandling != null && limitHandling)
726 {
727 distance = Math.hypot(point.x - this.x[index - 1], point.y - this.y[index - 1]);
728 if (distance < bestDistance)
729 {
730 bestDistance = distance;
731 result = lengthAtIndex(index - 1);
732 }
733 }
734 else if (index == this.size() - 1 && limitHandling != null && limitHandling)
735 {
736 distance = Math.hypot(point.x - this.x[index], point.y - this.y[index]);
737 if (distance < bestDistance)
738 {
739 bestDistance = distance;
740 result = lengthAtIndex(index);
741 }
742 }
743 }
744 if (bestDistance > bestDistanceExtended && (limitHandling == null || !limitHandling))
745 {
746 return Double.NaN;
747 }
748 return result / getLength();
749 }
750
751
752 @Override
753 public Point2d closestPointOnPolyLine(final Point2d point)
754 {
755 try
756 {
757 return getLocation(projectOrthogonalFractional(point, true) * getLength());
758 }
759 catch (DrawRuntimeException exception)
760 {
761
762 CategoryLogger.always().error(exception);
763 throw new Error(exception);
764 }
765 }
766
767
768
769
770
771
772
773
774
775 private Point2d projectOrthogonal(final Point2d point, final Boolean limitHandling)
776 {
777 Throw.whenNull(point, "point may not be null");
778 if (this.lengthIndexedLine.length == 1)
779 {
780
781 Point2d result = this.getLocation(0.0).projectOrthogonalExtended(point);
782 if (null == limitHandling)
783 {
784 return result.x != this.x[0] || result.y != this.y[0] ? null : get(0);
785 }
786
787 return result;
788 }
789 double fraction = projectOrthogonalFractional(point, limitHandling);
790 if (Double.isNaN(fraction))
791 {
792 return null;
793 }
794 return getLocationExtended(fraction * getLength());
795 }
796
797
798 @Override
799 public Point2d projectOrthogonal(final Point2d point) throws NullPointerException
800 {
801 return projectOrthogonal(point, null);
802 }
803
804
805 @Override
806 public Point2d projectOrthogonalExtended(final Point2d point) throws NullPointerException
807 {
808 return projectOrthogonal(point, false);
809 }
810
811
812 @Override
813 public final double projectOrthogonalFractional(final Point2d point) throws NullPointerException
814 {
815 return projectOrthogonalFractional(point, null);
816 }
817
818
819 @Override
820 public double projectOrthogonalFractionalExtended(final Point2d point) throws NullPointerException
821 {
822 return projectOrthogonalFractional(point, false);
823 }
824
825
826 @Override
827 public PolyLine2d extract(final double start, final double end) throws DrawRuntimeException
828 {
829 if (Double.isNaN(start) || Double.isNaN(end) || start < 0 || start >= end || end > getLength())
830 {
831 throw new DrawRuntimeException(
832 "Bad interval (" + start + ".." + end + "; length of this PolyLine2d is " + this.getLength() + ")");
833 }
834 double cumulativeLength = 0;
835 double nextCumulativeLength = 0;
836 double segmentLength = 0;
837 int index = 0;
838 List<Point2d> pointList = new ArrayList<>();
839 while (start > cumulativeLength)
840 {
841 Point2d fromPoint = get(index);
842 index++;
843 Point2d toPoint = get(index);
844 segmentLength = fromPoint.distance(toPoint);
845 cumulativeLength = nextCumulativeLength;
846 nextCumulativeLength = cumulativeLength + segmentLength;
847 if (nextCumulativeLength >= start)
848 {
849 break;
850 }
851 }
852 if (start == nextCumulativeLength)
853 {
854 pointList.add(get(index));
855 }
856 else
857 {
858 pointList.add(get(index - 1).interpolate(get(index), (start - cumulativeLength) / segmentLength));
859 if (end > nextCumulativeLength)
860 {
861 pointList.add(get(index));
862 }
863 }
864 while (end > nextCumulativeLength)
865 {
866 Point2d fromPoint = get(index);
867 index++;
868 if (index >= size())
869 {
870 break;
871 }
872 Point2d toPoint = get(index);
873 segmentLength = fromPoint.distance(toPoint);
874 cumulativeLength = nextCumulativeLength;
875 nextCumulativeLength = cumulativeLength + segmentLength;
876 if (nextCumulativeLength >= end)
877 {
878 break;
879 }
880 pointList.add(toPoint);
881 }
882 if (end == nextCumulativeLength)
883 {
884 pointList.add(get(index));
885 }
886 else if (index < this.x.length)
887 {
888 Point2d point = get(index - 1).interpolate(get(index), (end - cumulativeLength) / segmentLength);
889
890 if (!point.equals(pointList.get(pointList.size() - 1)))
891 {
892 pointList.add(point);
893 }
894 }
895
896 try
897 {
898 return instantiate(pointList);
899 }
900 catch (DrawRuntimeException exception)
901 {
902 CategoryLogger.always().error(exception, "interval " + start + ".." + end + " too short");
903 throw new DrawRuntimeException("interval " + start + ".." + end + "too short");
904 }
905 }
906
907
908 @Override
909 public PolyLine2d truncate(final double position) throws DrawRuntimeException
910 {
911 if (position <= 0.0 || position > getLength())
912 {
913 throw new DrawRuntimeException("truncate for line: position <= 0.0 or > line length. Position = " + position
914 + ". Length = " + getLength() + " m.");
915 }
916
917
918 if (position == getLength())
919 {
920 return this;
921 }
922
923
924 int index = find(position);
925 double remainder = position - lengthAtIndex(index);
926 double fraction = remainder / (lengthAtIndex(index + 1) - lengthAtIndex(index));
927 Point2d p1 = get(index);
928 Point2d lastPoint;
929 if (0.0 == fraction)
930 {
931 lastPoint = p1;
932 }
933 else
934 {
935 Point2d p2 = get(index + 1);
936 lastPoint = p1.interpolate(p2, fraction);
937 index++;
938 }
939 double[] truncatedX = new double[index + 1];
940 double[] truncatedY = new double[index + 1];
941 for (int i = 0; i < index; i++)
942 {
943 truncatedX[i] = this.x[i];
944 truncatedY[i] = this.y[i];
945 }
946 truncatedX[index] = lastPoint.x;
947 truncatedY[index] = lastPoint.y;
948 return new PolyLine2d(truncatedX, truncatedY);
949 }
950
951
952 @Override
953 @SuppressWarnings("checkstyle:methodlength")
954 public PolyLine2d offsetLine(final double offset, final double circlePrecision, final double offsetMinimumFilterValue,
955 final double offsetMaximumFilterValue, final double offsetFilterRatio, final double minimumOffset)
956 throws IllegalArgumentException
957 {
958 Throw.when(Double.isNaN(offset), IllegalArgumentException.class, "Offset may not be NaN");
959 Throw.when(Double.isNaN(circlePrecision) || circlePrecision <= 0, IllegalArgumentException.class,
960 "bad circlePrecision");
961 Throw.when(Double.isNaN(offsetMinimumFilterValue) || offsetMinimumFilterValue <= 0, IllegalArgumentException.class,
962 "bad offsetMinimumFilterValue");
963 Throw.when(Double.isNaN(offsetMaximumFilterValue) || offsetMaximumFilterValue <= 0, IllegalArgumentException.class,
964 "bad offsetMaximumFilterValue");
965 Throw.when(Double.isNaN(offsetFilterRatio) || offsetFilterRatio <= 0, IllegalArgumentException.class,
966 "bad offsetFilterRatio");
967 Throw.when(Double.isNaN(minimumOffset) || minimumOffset <= 0, IllegalArgumentException.class, "bad minimumOffset");
968 Throw.when(offsetMinimumFilterValue >= offsetMaximumFilterValue, IllegalArgumentException.class,
969 "bad offset filter values; minimum must be less than maximum");
970 double bufferOffset = Math.abs(offset);
971 if (bufferOffset < minimumOffset)
972 {
973 return this;
974 }
975
976 PolyLine2d filteredReferenceLine = noiseFilteredLine(
977 Math.max(offsetMinimumFilterValue, Math.min(bufferOffset / offsetFilterRatio, offsetMaximumFilterValue)));
978 List<Point2d> tempPoints = new ArrayList<>();
979
980 Point2d prevPoint = filteredReferenceLine.get(0);
981 Double prevAngle = null;
982 for (int index = 0; index < filteredReferenceLine.size() - 1; index++)
983 {
984 Point2d nextPoint = filteredReferenceLine.get(index + 1);
985 double angle = Math.atan2(nextPoint.y - prevPoint.y, nextPoint.x - prevPoint.x);
986 Point2d segmentFrom = new Point2d(prevPoint.x - Math.sin(angle) * offset, prevPoint.y + Math.cos(angle) * offset);
987 Point2d segmentTo = new Point2d(nextPoint.x - Math.sin(angle) * offset, nextPoint.y + Math.cos(angle) * offset);
988 boolean addSegment = true;
989 if (index > 0)
990 {
991 double deltaAngle = angle - prevAngle;
992 if (Math.abs(deltaAngle) > Math.PI)
993 {
994 deltaAngle -= Math.signum(deltaAngle) * 2 * Math.PI;
995 }
996 if (deltaAngle * offset <= 0)
997 {
998
999
1000
1001 int numSegments = 1;
1002 if (Math.abs(deltaAngle) > Math.PI / 2)
1003 {
1004 numSegments = 2;
1005 }
1006 while (true)
1007 {
1008 double maxError = bufferOffset * (1 - Math.abs(Math.cos(deltaAngle / numSegments / 2)));
1009 if (maxError < circlePrecision)
1010 {
1011 break;
1012 }
1013 numSegments *= 2;
1014 }
1015 Point2d prevArcPoint = tempPoints.get(tempPoints.size() - 1);
1016
1017 for (int additionalPoint = 1; additionalPoint < numSegments; additionalPoint++)
1018 {
1019 double intermediateAngle =
1020 (additionalPoint * angle + (numSegments - additionalPoint) * prevAngle) / numSegments;
1021 if (prevAngle * angle < 0 && Math.abs(prevAngle) > Math.PI / 2 && Math.abs(angle) > Math.PI / 2)
1022 {
1023 intermediateAngle += Math.PI;
1024 }
1025 Point2d intermediatePoint = new Point2d(prevPoint.x - Math.sin(intermediateAngle) * offset,
1026 prevPoint.y + Math.cos(intermediateAngle) * offset);
1027
1028 Point2d prevSegFrom = null;
1029 int stopAt = tempPoints.size();
1030 for (int i = 0; i < stopAt; i++)
1031 {
1032 Point2d prevSegTo = tempPoints.get(i);
1033 if (null != prevSegFrom)
1034 {
1035 Point2d prevSegIntersection = Point2d.intersectionOfLineSegments(prevArcPoint,
1036 intermediatePoint, prevSegFrom, prevSegTo);
1037 if (null != prevSegIntersection && prevSegIntersection.distance(prevArcPoint) > circlePrecision
1038 && prevSegIntersection.distance(prevSegFrom) > circlePrecision
1039 && prevSegIntersection.distance(prevSegTo) > circlePrecision)
1040 {
1041 tempPoints.add(prevSegIntersection);
1042
1043 }
1044 }
1045 prevSegFrom = prevSegTo;
1046 }
1047 Point2d nextSegmentIntersection =
1048 Point2d.intersectionOfLineSegments(prevSegFrom, intermediatePoint, segmentFrom, segmentTo);
1049 if (null != nextSegmentIntersection)
1050 {
1051 tempPoints.add(nextSegmentIntersection);
1052
1053 }
1054 tempPoints.add(intermediatePoint);
1055
1056 prevArcPoint = intermediatePoint;
1057 }
1058 }
1059
1060
1061 Point2d pPoint = null;
1062 int currentSize = tempPoints.size();
1063 for (int i = 0; i < currentSize ; i++)
1064 {
1065 Point2d p = tempPoints.get(i);
1066 if (null != pPoint)
1067 {
1068 double pAngle = Math.atan2(p.y - pPoint.y, p.x - pPoint.x);
1069 double angleDifference = angle - pAngle;
1070 if (Math.abs(angleDifference) > Math.PI)
1071 {
1072 angleDifference -= Math.signum(angleDifference) * 2 * Math.PI;
1073 }
1074 if (Math.abs(angleDifference) > 0)
1075 {
1076 Point2d intersection = Point2d.intersectionOfLineSegments(pPoint, p, segmentFrom, segmentTo);
1077 if (null != intersection)
1078 {
1079 if (tempPoints.size() - 1 == i)
1080 {
1081 tempPoints.remove(tempPoints.size() - 1);
1082 segmentFrom = intersection;
1083 }
1084 else
1085 {
1086 tempPoints.add(intersection);
1087 }
1088 }
1089 }
1090 else
1091 {
1092
1093 if (i == tempPoints.size() - 1)
1094 {
1095 tempPoints.remove(tempPoints.size() - 1);
1096 segmentFrom = tempPoints.get(tempPoints.size() - 1);
1097 tempPoints.remove(tempPoints.size() - 1);
1098 }
1099 }
1100 }
1101 pPoint = p;
1102 }
1103 }
1104 if (addSegment)
1105 {
1106 tempPoints.add(segmentFrom);
1107 tempPoints.add(segmentTo);
1108 prevPoint = nextPoint;
1109 prevAngle = angle;
1110 }
1111 }
1112
1113 for (int index = 1; index < tempPoints.size() - 1; index++)
1114 {
1115 Point2d checkPoint = tempPoints.get(index);
1116 prevPoint = null;
1117 boolean tooClose = false;
1118 boolean somewhereAtCorrectDistance = false;
1119 for (int i = 0; i < filteredReferenceLine.size(); i++)
1120 {
1121 Point2d p = filteredReferenceLine.get(i);
1122 if (null != prevPoint)
1123 {
1124 Point2d closestPoint = checkPoint.closestPointOnSegment(prevPoint, p);
1125 double distance = closestPoint.distance(checkPoint);
1126 if (distance < bufferOffset - circlePrecision)
1127 {
1128 tooClose = true;
1129 break;
1130 }
1131 else if (distance < bufferOffset + minimumOffset)
1132 {
1133 somewhereAtCorrectDistance = true;
1134 }
1135 }
1136 prevPoint = p;
1137 }
1138 if (tooClose || !somewhereAtCorrectDistance)
1139 {
1140 tempPoints.remove(index);
1141 index--;
1142 }
1143 }
1144 try
1145 {
1146 return new PolyLine2d(true, tempPoints);
1147 }
1148 catch (DrawRuntimeException exception)
1149 {
1150 exception.printStackTrace();
1151 }
1152 return null;
1153 }
1154
1155
1156 @Override
1157 public PolyLine2d offsetLine(final double offsetAtStart, final double offsetAtEnd, final double circlePrecision,
1158 final double offsetMinimumFilterValue, final double offsetMaximumFilterValue, final double offsetFilterRatio,
1159 final double minimumOffset) throws IllegalArgumentException, DrawRuntimeException
1160 {
1161 if (offsetAtStart == offsetAtEnd)
1162 {
1163 return offsetLine(offsetAtStart, circlePrecision, offsetMinimumFilterValue, offsetMaximumFilterValue,
1164 offsetFilterRatio, minimumOffset);
1165 }
1166 PolyLine2d atStart = offsetLine(offsetAtStart, circlePrecision, offsetMinimumFilterValue, offsetMaximumFilterValue,
1167 offsetFilterRatio, minimumOffset);
1168 PolyLine2d atEnd = offsetLine(offsetAtEnd, circlePrecision, offsetMinimumFilterValue, offsetMaximumFilterValue,
1169 offsetFilterRatio, minimumOffset);
1170 return atStart.transitionLine(atEnd, new TransitionFunction()
1171 {
1172 @Override
1173 public double function(final double fraction)
1174 {
1175 return fraction;
1176 }
1177 });
1178 }
1179
1180
1181 @Override
1182 public PolyLine2d transitionLine(final PolyLine2d endLine, final TransitionFunction transition) throws DrawRuntimeException
1183 {
1184 Throw.whenNull(endLine, "endLine may not be null");
1185 Throw.whenNull(transition, "transition may not be null");
1186 List<Point2d> pointList = new ArrayList<>();
1187 int indexInStart = 0;
1188 int indexInEnd = 0;
1189 while (indexInStart < this.size() && indexInEnd < endLine.size())
1190 {
1191 double fractionInStart = lengthAtIndex(indexInStart) / getLength();
1192 double fractionInEnd = endLine.lengthAtIndex(indexInEnd) / endLine.getLength();
1193 if (fractionInStart < fractionInEnd)
1194 {
1195 pointList.add(get(indexInStart).interpolate(endLine.getLocation(fractionInStart * endLine.getLength()),
1196 transition.function(fractionInStart)));
1197 indexInStart++;
1198 }
1199 else if (fractionInStart > fractionInEnd)
1200 {
1201 pointList.add(this.getLocation(fractionInEnd * getLength()).interpolate(endLine.get(indexInEnd),
1202 transition.function(fractionInEnd)));
1203 indexInEnd++;
1204 }
1205 else
1206 {
1207 pointList.add(this.get(indexInStart).interpolate(endLine.getLocation(fractionInEnd * endLine.getLength()),
1208 transition.function(fractionInStart)));
1209 indexInStart++;
1210 indexInEnd++;
1211 }
1212 }
1213 return new PolyLine2d(true, pointList);
1214 }
1215
1216
1217
1218
1219
1220
1221
1222
1223
1224
1225
1226
1227 public double projectRay(final Ray2d ray) throws NullPointerException
1228 {
1229 Throw.whenNull(ray, "ray may not be null");
1230 double bestDistance = Double.POSITIVE_INFINITY;
1231 double positionAtBestDistance = Double.NaN;
1232
1233
1234 double perpendicularX = ray.x - Math.sin(ray.phi);
1235 double perpendicularY = ray.y + Math.cos(ray.phi);
1236 for (int index = 1; index < this.x.length; index++)
1237 {
1238 Point2d intersection = Point2d.intersectionOfLines(ray.x, ray.y, perpendicularX, perpendicularY, false, false,
1239 this.x[index - 1], this.y[index - 1], this.x[index], this.y[index], true, true);
1240 if (intersection != null)
1241 {
1242 double thisDistance = intersection.distance(ray);
1243 if (thisDistance < bestDistance)
1244 {
1245 double distanceToPrevPoint =
1246 Math.hypot(intersection.x - this.x[index - 1], intersection.y - this.y[index - 1]);
1247 positionAtBestDistance = lengthAtIndex(index - 1) + distanceToPrevPoint;
1248 bestDistance = thisDistance;
1249 }
1250 }
1251 }
1252 return positionAtBestDistance;
1253 }
1254
1255
1256
1257
1258
1259 public Path2D toPath2D()
1260 {
1261 Path2D.Double result = new Path2D.Double();
1262 result.moveTo(this.x[0], this.y[0]);
1263 for (int i = 1; i < this.x.length; i++)
1264 {
1265 result.lineTo(this.x[i], this.y[i]);
1266 }
1267 return result;
1268 }
1269
1270
1271 @Override
1272 public String toString()
1273 {
1274 return toString("%f", false);
1275 }
1276
1277
1278 @Override
1279 public String toString(final String doubleFormat, final boolean doNotIncludeClassName)
1280 {
1281 StringBuilder result = new StringBuilder();
1282 if (!doNotIncludeClassName)
1283 {
1284 result.append("PolyLine2d ");
1285 }
1286 String format = String.format("%%sx=%1$s, y=%1$s", doubleFormat);
1287 for (int index = 0; index < this.x.length; index++)
1288 {
1289 result.append(String.format(Locale.US, format, index == 0 ? "[" : ", ", this.x[index], this.y[index]));
1290 }
1291 if (this.lengthIndexedLine.length == 1)
1292 {
1293 format = String.format(", startHeading=%1$s", doubleFormat);
1294 result.append(String.format(Locale.US, format, this.startHeading));
1295 }
1296 result.append("]");
1297 return result.toString();
1298 }
1299
1300
1301 @Override
1302 public String toExcel()
1303 {
1304 StringBuffer s = new StringBuffer();
1305 for (int i = 0; i < this.x.length; i++)
1306 {
1307 s.append(this.x[i] + "\t" + this.y[i] + "\n");
1308 }
1309 return s.toString();
1310 }
1311
1312
1313
1314
1315
1316 public String toPlot()
1317 {
1318 StringBuffer result = new StringBuffer();
1319 for (int i = 0; i < this.x.length; i++)
1320 {
1321 result.append(String.format(Locale.US, "%s%.3f,%.3f", 0 == result.length() ? "M" : " L", this.x[i], this.y[i]));
1322 }
1323 result.append("\n");
1324 return result.toString();
1325 }
1326
1327
1328 @Override
1329 public int hashCode()
1330 {
1331 final int prime = 31;
1332 int result = 1;
1333 long temp;
1334 temp = Double.doubleToLongBits(this.startHeading);
1335 result = prime * result + (int) (temp ^ (temp >>> 32));
1336 result = prime * result + Arrays.hashCode(this.x);
1337 result = prime * result + Arrays.hashCode(this.y);
1338 return result;
1339 }
1340
1341
1342 @SuppressWarnings("checkstyle:needbraces")
1343 @Override
1344 public boolean equals(final Object obj)
1345 {
1346 if (this == obj)
1347 return true;
1348 if (obj == null)
1349 return false;
1350 if (getClass() != obj.getClass())
1351 return false;
1352 PolyLine2d other = (PolyLine2d) obj;
1353 if (Double.doubleToLongBits(this.startHeading) != Double.doubleToLongBits(other.startHeading))
1354 return false;
1355 if (!Arrays.equals(this.x, other.x))
1356 return false;
1357 if (!Arrays.equals(this.y, other.y))
1358 return false;
1359 return true;
1360 }
1361
1362 }