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