1 package org.opentrafficsim.core.geometry;
2
3 import java.awt.geom.Line2D;
4 import java.awt.geom.Path2D;
5 import java.awt.geom.PathIterator;
6 import java.awt.geom.Point2D;
7 import java.io.Serializable;
8 import java.util.ArrayList;
9 import java.util.Arrays;
10 import java.util.List;
11
12 import javax.media.j3d.Bounds;
13 import javax.vecmath.Point3d;
14
15 import org.djunits.unit.LengthUnit;
16 import org.djunits.value.vdouble.scalar.Direction;
17 import org.djunits.value.vdouble.scalar.Length;
18
19 import com.vividsolutions.jts.geom.Coordinate;
20 import com.vividsolutions.jts.geom.CoordinateSequence;
21 import com.vividsolutions.jts.geom.Envelope;
22 import com.vividsolutions.jts.geom.Geometry;
23 import com.vividsolutions.jts.geom.GeometryFactory;
24 import com.vividsolutions.jts.geom.LineString;
25 import com.vividsolutions.jts.linearref.LengthIndexedLine;
26
27 import edu.umd.cs.findbugs.annotations.SuppressFBWarnings;
28 import nl.tudelft.simulation.dsol.animation.Locatable;
29 import nl.tudelft.simulation.language.d3.BoundingBox;
30 import nl.tudelft.simulation.language.d3.DirectedPoint;
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46 public class OTSLine3D implements Locatable, Serializable
47 {
48
49 private static final long serialVersionUID = 20150722L;
50
51
52 private OTSPoint3D[] points;
53
54
55 private double[] lengthIndexedLine = null;
56
57
58 private double length = Double.NaN;
59
60
61 private OTSPoint3D centroid = null;
62
63
64 private Bounds bounds = null;
65
66
67 private OTSPoint3D[] fractionalHelperCenters = null;
68
69
70 private Point2D.Double[] fractionalHelperDirections = null;
71
72
73 private OTSPoint3D firstOffsetIntersection;
74
75
76 private OTSPoint3D lastOffsetIntersection;
77
78
79 private static final double FRAC_PROJ_PRECISION = 1e-6;
80
81
82 private Envelope envelope;
83
84
85
86
87
88
89
90 public OTSLine3D(final OTSPoint3D... points) throws OTSGeometryException
91 {
92 init(points);
93 }
94
95
96
97
98
99
100
101 private void init(final OTSPoint3D... pts) throws OTSGeometryException
102 {
103 if (pts.length < 2)
104 {
105 throw new OTSGeometryException("Degenerate OTSLine3D; has " + pts.length + " point" + (pts.length != 1 ? "s" : ""));
106 }
107 this.lengthIndexedLine = new double[pts.length];
108 this.lengthIndexedLine[0] = 0.0;
109 for (int i = 1; i < pts.length; i++)
110 {
111 if (pts[i - 1].x == pts[i].x && pts[i - 1].y == pts[i].y && pts[i - 1].z == pts[i].z)
112 {
113 throw new OTSGeometryException(
114 "Degenerate OTSLine3D; point " + (i - 1) + " has the same x, y and z as point " + i);
115 }
116 this.lengthIndexedLine[i] = this.lengthIndexedLine[i - 1] + pts[i - 1].distanceSI(pts[i]);
117 }
118 this.points = pts;
119 }
120
121
122 public enum OffsetMethod
123 {
124
125 JTS,
126
127
128 PK;
129 };
130
131
132 public static final OffsetMethod OFFSETMETHOD = OffsetMethod.PK;
133
134
135
136
137
138
139
140 public final OTSLine3D offsetLine(final double offset)
141 {
142 try
143 {
144 switch (OFFSETMETHOD)
145 {
146 case PK:
147 return OTSOffsetLinePK.offsetLine(this, offset);
148
149 case JTS:
150 return OTSBufferingJTS.offsetGeometryOLD(this, offset);
151
152 default:
153 return null;
154 }
155 }
156 catch (OTSGeometryException exception)
157 {
158 exception.printStackTrace();
159 return null;
160 }
161 }
162
163
164
165
166
167
168
169 public final OTSLine3D noiseFilteredLine(final double noiseLevel)
170 {
171 if (this.size() <= 2)
172 {
173 return this;
174 }
175 OTSPoint3D prevPoint = null;
176 List<OTSPoint3D> list = null;
177 for (int index = 0; index < this.size(); index++)
178 {
179 OTSPoint3D currentPoint = this.points[index];
180 if (null != prevPoint && prevPoint.distanceSI(currentPoint) < noiseLevel)
181 {
182 if (null == list)
183 {
184
185 list = new ArrayList<OTSPoint3D>();
186 for (int i = 0; i < index; i++)
187 {
188 list.add(this.points[i]);
189 }
190 }
191 if (index == this.size() - 1)
192 {
193 if (list.size() > 1)
194 {
195
196 list.set(list.size() - 1, currentPoint);
197 }
198 else
199 {
200
201
202 list.add(currentPoint);
203 }
204 }
205 continue;
206 }
207 else if (null != list)
208 {
209 list.add(currentPoint);
210 }
211 prevPoint = currentPoint;
212 }
213 if (null == list)
214 {
215 return this;
216 }
217 try
218 {
219 return new OTSLine3D(list);
220 }
221 catch (OTSGeometryException exception)
222 {
223 System.err.println("CANNOT HAPPEN");
224 exception.printStackTrace();
225 throw new Error(exception);
226 }
227 }
228
229
230
231
232
233
234
235
236
237 public final OTSLine3D noiseFilterRamerDouglasPeuker(final double epsilon, final boolean useHorizontalDistance)
238 {
239 try
240 {
241
242
243 double maxDeviation = 0;
244 int splitIndex = -1;
245 int pointCount = size();
246 OTSLine3D straight = new OTSLine3D(get(0), get(pointCount - 1));
247
248 for (int i = 1; i < pointCount - 1; i++)
249 {
250 OTSPoint3D point = get(i);
251 OTSPoint3D closest =
252 useHorizontalDistance ? point.closestPointOnLine2D(straight) : point.closestPointOnLine(straight);
253 double deviation = useHorizontalDistance ? closest.horizontalDistanceSI(point) : closest.distanceSI(point);
254 if (deviation > maxDeviation)
255 {
256 splitIndex = i;
257 maxDeviation = deviation;
258 }
259 }
260 if (maxDeviation <= epsilon)
261 {
262
263 return straight;
264 }
265
266
267
268 OTSLine3D first = new OTSLine3D(Arrays.copyOfRange(this.points, 0, splitIndex + 1))
269 .noiseFilterRamerDouglasPeuker(epsilon, useHorizontalDistance);
270 OTSLine3D second = new OTSLine3D(Arrays.copyOfRange(this.points, splitIndex, this.points.length))
271 .noiseFilterRamerDouglasPeuker(epsilon, useHorizontalDistance);
272 return concatenate(epsilon, first, second);
273 }
274 catch (OTSGeometryException exception)
275 {
276 exception.printStackTrace();
277 return null;
278 }
279 }
280
281
282
283
284
285
286
287
288
289 public final OTSLine3D offsetLine(final double offsetAtStart, final double offsetAtEnd) throws OTSGeometryException
290 {
291
292
293
294 OTSLine3D offsetLineAtStart = offsetLine(offsetAtStart);
295 if (offsetAtStart == offsetAtEnd)
296 {
297 return offsetLineAtStart;
298 }
299
300 OTSLine3D offsetLineAtEnd = offsetLine(offsetAtEnd);
301
302 Geometry startGeometry = offsetLineAtStart.getLineString();
303 Geometry endGeometry = offsetLineAtEnd.getLineString();
304 LengthIndexedLine first = new LengthIndexedLine(startGeometry);
305 double firstLength = startGeometry.getLength();
306 LengthIndexedLine second = new LengthIndexedLine(endGeometry);
307 double secondLength = endGeometry.getLength();
308 ArrayList<Coordinate> out = new ArrayList<Coordinate>();
309 Coordinate[] firstCoordinates = startGeometry.getCoordinates();
310 Coordinate[] secondCoordinates = endGeometry.getCoordinates();
311 int firstIndex = 0;
312 int secondIndex = 0;
313 Coordinate prevCoordinate = null;
314 final double tooClose = 0.05;
315 while (firstIndex < firstCoordinates.length && secondIndex < secondCoordinates.length)
316 {
317 double firstRatio = firstIndex < firstCoordinates.length ? first.indexOf(firstCoordinates[firstIndex]) / firstLength
318 : Double.MAX_VALUE;
319 double secondRatio = secondIndex < secondCoordinates.length
320 ? second.indexOf(secondCoordinates[secondIndex]) / secondLength : Double.MAX_VALUE;
321 double ratio;
322 if (firstRatio < secondRatio)
323 {
324 ratio = firstRatio;
325 firstIndex++;
326 }
327 else
328 {
329 ratio = secondRatio;
330 secondIndex++;
331 }
332 Coordinate firstCoordinate = first.extractPoint(ratio * firstLength);
333 Coordinate secondCoordinate = second.extractPoint(ratio * secondLength);
334 Coordinate resultCoordinate = new Coordinate((1 - ratio) * firstCoordinate.x + ratio * secondCoordinate.x,
335 (1 - ratio) * firstCoordinate.y + ratio * secondCoordinate.y);
336 if (null == prevCoordinate || resultCoordinate.distance(prevCoordinate) > tooClose)
337 {
338 out.add(resultCoordinate);
339 prevCoordinate = resultCoordinate;
340 }
341 }
342 Coordinate[] resultCoordinates = new Coordinate[out.size()];
343 for (int index = 0; index < out.size(); index++)
344 {
345 resultCoordinates[index] = out.get(index);
346 }
347 return new OTSLine3D(resultCoordinates);
348 }
349
350
351
352
353
354
355
356
357
358
359 public final OTSLine3D offsetLine(final double[] relativeFractions, final double[] offsets) throws OTSGeometryException
360 {
361 OTSLine3D[] offsetLine = new OTSLine3D[relativeFractions.length];
362 for (int i = 0; i < offsets.length; i++)
363 {
364 offsetLine[i] = offsetLine(offsets[i]);
365
366
367 }
368
369 ArrayList<Coordinate> out = new ArrayList<Coordinate>();
370 Coordinate prevCoordinate = null;
371 final double tooClose = 0.05;
372 for (int i = 0; i < offsets.length - 1; i++)
373 {
374 Geometry startGeometry =
375 offsetLine[i].extractFractional(relativeFractions[i], relativeFractions[i + 1]).getLineString();
376 Geometry endGeometry =
377 offsetLine[i + 1].extractFractional(relativeFractions[i], relativeFractions[i + 1]).getLineString();
378 LengthIndexedLine first = new LengthIndexedLine(startGeometry);
379 double firstLength = startGeometry.getLength();
380 LengthIndexedLine second = new LengthIndexedLine(endGeometry);
381 double secondLength = endGeometry.getLength();
382 Coordinate[] firstCoordinates = startGeometry.getCoordinates();
383 Coordinate[] secondCoordinates = endGeometry.getCoordinates();
384 int firstIndex = 0;
385 int secondIndex = 0;
386 while (firstIndex < firstCoordinates.length && secondIndex < secondCoordinates.length)
387 {
388 double firstRatio = firstIndex < firstCoordinates.length
389 ? first.indexOf(firstCoordinates[firstIndex]) / firstLength : Double.MAX_VALUE;
390 double secondRatio = secondIndex < secondCoordinates.length
391 ? second.indexOf(secondCoordinates[secondIndex]) / secondLength : Double.MAX_VALUE;
392 double ratio;
393 if (firstRatio < secondRatio)
394 {
395 ratio = firstRatio;
396 firstIndex++;
397 }
398 else
399 {
400 ratio = secondRatio;
401 secondIndex++;
402 }
403 Coordinate firstCoordinate = first.extractPoint(ratio * firstLength);
404 Coordinate secondCoordinate = second.extractPoint(ratio * secondLength);
405 Coordinate resultCoordinate = new Coordinate((1 - ratio) * firstCoordinate.x + ratio * secondCoordinate.x,
406 (1 - ratio) * firstCoordinate.y + ratio * secondCoordinate.y);
407 if (null == prevCoordinate || resultCoordinate.distance(prevCoordinate) > tooClose)
408 {
409 out.add(resultCoordinate);
410 prevCoordinate = resultCoordinate;
411 }
412 }
413 }
414
415 Coordinate[] resultCoordinates = new Coordinate[out.size()];
416 for (int index = 0; index < out.size(); index++)
417 {
418 resultCoordinates[index] = out.get(index);
419 }
420 return new OTSLine3D(resultCoordinates);
421 }
422
423
424
425
426
427
428
429
430 public static OTSLine3D concatenate(final OTSLine3D... lines) throws OTSGeometryException
431 {
432 return concatenate(0.0, lines);
433 }
434
435
436
437
438
439
440
441
442
443 public static OTSLine3D concatenate(final double toleranceSI, final OTSLine3D line1, final OTSLine3D line2)
444 throws OTSGeometryException
445 {
446 if (line1.getLast().distance(line2.getFirst()).si > toleranceSI)
447 {
448 throw new OTSGeometryException("Lines are not connected: " + line1.getLast() + " to " + line2.getFirst()
449 + " distance is " + line1.getLast().distance(line2.getFirst()).si + " > " + toleranceSI);
450 }
451 int size = line1.size() + line2.size() - 1;
452 OTSPoint3D[] points = new OTSPoint3D[size];
453 int nextIndex = 0;
454 for (int j = 0; j < line1.size(); j++)
455 {
456 points[nextIndex++] = line1.get(j);
457 }
458 for (int j = 1; j < line2.size(); j++)
459 {
460 points[nextIndex++] = line2.get(j);
461 }
462 return new OTSLine3D(points);
463 }
464
465
466
467
468
469
470
471
472
473 public static OTSLine3D concatenate(final double toleranceSI, final OTSLine3D... lines) throws OTSGeometryException
474 {
475
476 if (0 == lines.length)
477 {
478 throw new OTSGeometryException("Empty argument list");
479 }
480 else if (1 == lines.length)
481 {
482 return lines[0];
483 }
484 int size = lines[0].size();
485 for (int i = 1; i < lines.length; i++)
486 {
487 if (lines[i - 1].getLast().distance(lines[i].getFirst()).si > toleranceSI)
488 {
489 throw new OTSGeometryException(
490 "Lines are not connected: " + lines[i - 1].getLast() + " to " + lines[i].getFirst() + " distance is "
491 + lines[i - 1].getLast().distance(lines[i].getFirst()).si + " > " + toleranceSI);
492 }
493 size += lines[i].size() - 1;
494 }
495 OTSPoint3D[] points = new OTSPoint3D[size];
496 int nextIndex = 0;
497 for (int i = 0; i < lines.length; i++)
498 {
499 OTSLine3D line = lines[i];
500 for (int j = 0 == i ? 0 : 1; j < line.size(); j++)
501 {
502 points[nextIndex++] = line.get(j);
503 }
504 }
505 return new OTSLine3D(points);
506 }
507
508
509
510
511
512 public final OTSLine3D reverse()
513 {
514 OTSPoint3D[] resultPoints = new OTSPoint3D[size()];
515 int nextIndex = size();
516 for (OTSPoint3D p : getPoints())
517 {
518 resultPoints[--nextIndex] = p;
519 }
520 try
521 {
522 return new OTSLine3D(resultPoints);
523 }
524 catch (OTSGeometryException exception)
525 {
526
527 throw new RuntimeException(exception);
528 }
529 }
530
531
532
533
534
535
536
537
538 public final OTSLine3D extractFractional(final double start, final double end) throws OTSGeometryException
539 {
540 if (start < 0 || start >= end || end > 1)
541 {
542 throw new OTSGeometryException("Bad interval");
543 }
544 getLength();
545 return extract(start * this.length, end * this.length);
546 }
547
548
549
550
551
552
553
554
555
556 public final OTSLine3D extract(final Length start, final Length end) throws OTSGeometryException
557 {
558 return extract(start.si, end.si);
559 }
560
561
562
563
564
565
566
567
568
569 @SuppressFBWarnings("FE_FLOATING_POINT_EQUALITY")
570 public final OTSLine3D extract(final double start, final double end) throws OTSGeometryException
571 {
572 if (Double.isNaN(start) || Double.isNaN(end) || start < 0 || start >= end || end > getLengthSI())
573 {
574 throw new OTSGeometryException(
575 "Bad interval (" + start + ".." + end + "; length of this OTSLine3D is " + this.getLengthSI() + ")");
576 }
577 double cumulativeLength = 0;
578 double nextCumulativeLength = 0;
579 double segmentLength = 0;
580 int index = 0;
581 List<OTSPoint3D> pointList = new ArrayList<>();
582
583 while (start > cumulativeLength)
584 {
585 OTSPoint3D fromPoint = this.points[index];
586 index++;
587 OTSPoint3D toPoint = this.points[index];
588 segmentLength = fromPoint.distanceSI(toPoint);
589 cumulativeLength = nextCumulativeLength;
590 nextCumulativeLength = cumulativeLength + segmentLength;
591 if (nextCumulativeLength >= start)
592 {
593 break;
594 }
595 }
596 if (start == nextCumulativeLength)
597 {
598 pointList.add(this.points[index]);
599 }
600 else
601 {
602 pointList.add(OTSPoint3D.interpolate((start - cumulativeLength) / segmentLength, this.points[index - 1],
603 this.points[index]));
604 if (end > nextCumulativeLength)
605 {
606 pointList.add(this.points[index]);
607 }
608 }
609 while (end > nextCumulativeLength)
610 {
611 OTSPoint3D fromPoint = this.points[index];
612 index++;
613 if (index >= this.points.length)
614 {
615 break;
616 }
617 OTSPoint3D toPoint = this.points[index];
618 segmentLength = fromPoint.distanceSI(toPoint);
619 cumulativeLength = nextCumulativeLength;
620 nextCumulativeLength = cumulativeLength + segmentLength;
621 if (nextCumulativeLength >= end)
622 {
623 break;
624 }
625 pointList.add(toPoint);
626 }
627 if (end == nextCumulativeLength)
628 {
629 pointList.add(this.points[index]);
630 }
631 else
632 {
633 OTSPoint3D point = OTSPoint3D.interpolate((end - cumulativeLength) / segmentLength, this.points[index - 1],
634 this.points[index]);
635
636 if (!point.equals(pointList.get(pointList.size() - 1)))
637 {
638 pointList.add(point);
639 }
640 }
641 try
642 {
643 return new OTSLine3D(pointList);
644 }
645 catch (OTSGeometryException exception)
646 {
647 System.err.println("interval " + start + ".." + end + " too short");
648 throw new OTSGeometryException("interval " + start + ".." + end + "too short");
649 }
650 }
651
652
653
654
655
656
657 private static OTSPoint3D[] coordinatesToOTSPoint3D(final Coordinate[] coordinates)
658 {
659 OTSPoint3D[] result = new OTSPoint3D[coordinates.length];
660 for (int i = 0; i < coordinates.length; i++)
661 {
662 result[i] = new OTSPoint3D(coordinates[i]);
663 }
664 return result;
665 }
666
667
668
669
670
671
672
673 public static OTSLine3D createAndCleanOTSLine3D(final OTSPoint3D... points) throws OTSGeometryException
674 {
675 if (points.length < 2)
676 {
677 throw new OTSGeometryException(
678 "Degenerate OTSLine3D; has " + points.length + " point" + (points.length != 1 ? "s" : ""));
679 }
680 return createAndCleanOTSLine3D(new ArrayList<>(Arrays.asList(points)));
681 }
682
683
684
685
686
687
688
689
690 public static OTSLine3D createAndCleanOTSLine3D(final List<OTSPoint3D> pointList) throws OTSGeometryException
691 {
692
693 int i = 1;
694 while (i < pointList.size())
695 {
696 if (pointList.get(i - 1).equals(pointList.get(i)))
697 {
698 pointList.remove(i);
699 }
700 else
701 {
702 i++;
703 }
704 }
705 return new OTSLine3D(pointList);
706 }
707
708
709
710
711
712
713
714 public OTSLine3D(final Coordinate[] coordinates) throws OTSGeometryException
715 {
716 this(coordinatesToOTSPoint3D(coordinates));
717 }
718
719
720
721
722
723
724
725 public OTSLine3D(final LineString lineString) throws OTSGeometryException
726 {
727 this(lineString.getCoordinates());
728 }
729
730
731
732
733
734
735
736 public OTSLine3D(final Geometry geometry) throws OTSGeometryException
737 {
738 this(geometry.getCoordinates());
739 }
740
741
742
743
744
745
746
747 public OTSLine3D(final List<OTSPoint3D> pointList) throws OTSGeometryException
748 {
749 this(pointList.toArray(new OTSPoint3D[pointList.size()]));
750 }
751
752
753
754
755
756
757
758 public OTSLine3D(final Path2D path) throws OTSGeometryException
759 {
760 List<OTSPoint3D> pl = new ArrayList<>();
761 for (PathIterator pi = path.getPathIterator(null); !pi.isDone(); pi.next())
762 {
763 double[] p = new double[6];
764 int segType = pi.currentSegment(p);
765 if (segType == PathIterator.SEG_MOVETO || segType == PathIterator.SEG_LINETO)
766 {
767 pl.add(new OTSPoint3D(p[0], p[1]));
768 }
769 else if (segType == PathIterator.SEG_CLOSE)
770 {
771 if (!pl.get(0).equals(pl.get(pl.size() - 1)))
772 {
773 pl.add(new OTSPoint3D(pl.get(0).x, pl.get(0).y));
774 }
775 break;
776 }
777 }
778 init(pl.toArray(new OTSPoint3D[pl.size() - 1]));
779 }
780
781
782
783
784
785 public final Coordinate[] getCoordinates()
786 {
787 Coordinate[] result = new Coordinate[size()];
788 for (int i = 0; i < size(); i++)
789 {
790 result[i] = this.points[i].getCoordinate();
791 }
792 return result;
793 }
794
795
796
797
798
799 public final LineString getLineString()
800 {
801 GeometryFactory factory = new GeometryFactory();
802 Coordinate[] coordinates = getCoordinates();
803 CoordinateSequence cs = factory.getCoordinateSequenceFactory().create(coordinates);
804 return new LineString(cs, factory);
805 }
806
807
808
809
810
811 public final int size()
812 {
813 return this.points.length;
814 }
815
816
817
818
819
820 public final OTSPoint3D getFirst()
821 {
822 return this.points[0];
823 }
824
825
826
827
828
829 public final OTSPoint3D getLast()
830 {
831 return this.points[size() - 1];
832 }
833
834
835
836
837
838
839
840 public final OTSPoint3D get(final int i) throws OTSGeometryException
841 {
842 if (i < 0 || i > size() - 1)
843 {
844 throw new OTSGeometryException("OTSLine3D.get(i=" + i + "); i<0 or i>=size(), which is " + size());
845 }
846 return this.points[i];
847 }
848
849
850
851
852
853
854 public final synchronized double getLengthSI()
855 {
856 if (Double.isNaN(this.length))
857 {
858 this.length = 0.0;
859 for (int i = 0; i < size() - 1; i++)
860 {
861 this.length += this.points[i].distanceSI(this.points[i + 1]);
862 }
863 }
864 return this.length;
865 }
866
867
868
869
870
871
872 public final Length getLength()
873 {
874 return new Length(getLengthSI(), LengthUnit.SI);
875 }
876
877
878
879
880
881 public final OTSPoint3D[] getPoints()
882 {
883 return this.points;
884 }
885
886
887
888
889 private void makeLengthIndexedLine()
890 {
891 if (this.lengthIndexedLine == null)
892 {
893 this.lengthIndexedLine = new double[this.points.length];
894 this.lengthIndexedLine[0] = 0.0;
895 for (int i = 1; i < this.points.length; i++)
896 {
897 this.lengthIndexedLine[i] = this.lengthIndexedLine[i - 1] + this.points[i - 1].distanceSI(this.points[i]);
898 }
899 }
900 }
901
902
903
904
905
906
907
908 public final DirectedPoint getLocationExtended(final Length position)
909 {
910 return getLocationExtendedSI(position.getSI());
911 }
912
913
914
915
916
917
918
919 public final DirectedPoint getLocationExtendedSI(final double positionSI)
920 {
921 makeLengthIndexedLine();
922 if (positionSI >= 0.0 && positionSI <= getLengthSI())
923 {
924 try
925 {
926 return getLocationSI(positionSI);
927 }
928 catch (OTSGeometryException exception)
929 {
930
931 }
932 }
933
934
935 if (positionSI < 0.0)
936 {
937 double len = positionSI;
938 double fraction = len / (this.lengthIndexedLine[1] - this.lengthIndexedLine[0]);
939 OTSPoint3D p1 = this.points[0];
940 OTSPoint3D p2 = this.points[1];
941 return new DirectedPoint(p1.x + fraction * (p2.x - p1.x), p1.y + fraction * (p2.y - p1.y),
942 p1.z + fraction * (p2.z - p1.z), 0.0, 0.0, Math.atan2(p2.y - p1.y, p2.x - p1.x));
943 }
944
945
946 int n1 = this.lengthIndexedLine.length - 1;
947 int n2 = this.lengthIndexedLine.length - 2;
948 double len = positionSI - getLengthSI();
949 double fraction = len / (this.lengthIndexedLine[n1] - this.lengthIndexedLine[n2]);
950 OTSPoint3D p1 = this.points[n2];
951 OTSPoint3D p2 = this.points[n1];
952 return new DirectedPoint(p2.x + fraction * (p2.x - p1.x), p2.y + fraction * (p2.y - p1.y),
953 p2.z + fraction * (p2.z - p1.z), 0.0, 0.0, Math.atan2(p2.y - p1.y, p2.x - p1.x));
954 }
955
956
957
958
959
960
961
962 public final DirectedPoint getLocationFraction(final double fraction) throws OTSGeometryException
963 {
964 if (fraction < 0.0 || fraction > 1.0)
965 {
966 throw new OTSGeometryException("getLocationFraction for line: fraction < 0.0 or > 1.0. fraction = " + fraction);
967 }
968 return getLocationSI(fraction * getLengthSI());
969 }
970
971
972
973
974
975
976
977
978 public final DirectedPoint getLocationFraction(final double fraction, final double tolerance) throws OTSGeometryException
979 {
980 if (fraction < -tolerance || fraction > 1.0 + tolerance)
981 {
982 throw new OTSGeometryException(
983 "getLocationFraction for line: fraction < 0.0 - tolerance or > 1.0 + tolerance; fraction = " + fraction);
984 }
985 double f = fraction < 0 ? 0.0 : fraction > 1.0 ? 1.0 : fraction;
986 return getLocationSI(f * getLengthSI());
987 }
988
989
990
991
992
993
994 public final DirectedPoint getLocationFractionExtended(final double fraction)
995 {
996 return getLocationExtendedSI(fraction * getLengthSI());
997 }
998
999
1000
1001
1002
1003
1004
1005 public final DirectedPoint getLocation(final Length position) throws OTSGeometryException
1006 {
1007 return getLocationSI(position.getSI());
1008 }
1009
1010
1011
1012
1013
1014
1015
1016 private int find(final double pos) throws OTSGeometryException
1017 {
1018 if (pos == 0)
1019 {
1020 return 0;
1021 }
1022
1023 int lo = 0;
1024 int hi = this.lengthIndexedLine.length - 1;
1025 while (lo <= hi)
1026 {
1027 if (hi == lo)
1028 {
1029 return lo;
1030 }
1031 int mid = lo + (hi - lo) / 2;
1032 if (pos < this.lengthIndexedLine[mid])
1033 {
1034 hi = mid - 1;
1035 }
1036 else if (pos > this.lengthIndexedLine[mid + 1])
1037 {
1038 lo = mid + 1;
1039 }
1040 else
1041 {
1042 return mid;
1043 }
1044 }
1045 throw new OTSGeometryException(
1046 "Could not find position " + pos + " on line with length indexes: " + this.lengthIndexedLine);
1047 }
1048
1049
1050
1051
1052
1053
1054
1055 public final DirectedPoint getLocationSI(final double positionSI) throws OTSGeometryException
1056 {
1057 makeLengthIndexedLine();
1058 if (positionSI < 0.0 || positionSI > getLengthSI())
1059 {
1060 throw new OTSGeometryException("getLocationSI for line: position < 0.0 or > line length. Position = " + positionSI
1061 + " m. Length = " + getLengthSI() + " m.");
1062 }
1063
1064
1065 if (positionSI == 0.0)
1066 {
1067 OTSPoint3D p1 = this.points[0];
1068 OTSPoint3D p2 = this.points[1];
1069 return new DirectedPoint(p1.x, p1.y, p1.z, 0.0, 0.0, Math.atan2(p2.y - p1.y, p2.x - p1.x));
1070 }
1071 if (positionSI == getLengthSI())
1072 {
1073 OTSPoint3D p1 = this.points[this.points.length - 2];
1074 OTSPoint3D p2 = this.points[this.points.length - 1];
1075 return new DirectedPoint(p2.x, p2.y, p2.z, 0.0, 0.0, Math.atan2(p2.y - p1.y, p2.x - p1.x));
1076 }
1077
1078
1079 int index = find(positionSI);
1080 double remainder = positionSI - this.lengthIndexedLine[index];
1081 double fraction = remainder / (this.lengthIndexedLine[index + 1] - this.lengthIndexedLine[index]);
1082 OTSPoint3D p1 = this.points[index];
1083 OTSPoint3D p2 = this.points[index + 1];
1084 return new DirectedPoint(p1.x + fraction * (p2.x - p1.x), p1.y + fraction * (p2.y - p1.y),
1085 p1.z + fraction * (p2.z - p1.z), 0.0, 0.0, Math.atan2(p2.y - p1.y, p2.x - p1.x));
1086 }
1087
1088
1089
1090
1091
1092
1093
1094 public final OTSLine3D truncate(final double lengthSI) throws OTSGeometryException
1095 {
1096 makeLengthIndexedLine();
1097 if (lengthSI <= 0.0 || lengthSI > getLengthSI())
1098 {
1099 throw new OTSGeometryException("truncate for line: position <= 0.0 or > line length. Position = " + lengthSI
1100 + " m. Length = " + getLengthSI() + " m.");
1101 }
1102
1103
1104 if (lengthSI == getLengthSI())
1105 {
1106 return new OTSLine3D(getPoints());
1107 }
1108
1109
1110 int index = find(lengthSI);
1111 double remainder = lengthSI - this.lengthIndexedLine[index];
1112 double fraction = remainder / (this.lengthIndexedLine[index + 1] - this.lengthIndexedLine[index]);
1113 OTSPoint3D p1 = this.points[index];
1114 OTSPoint3D p2 = this.points[index + 1];
1115 OTSPoint3D newLastPoint = new OTSPoint3D(p1.x + fraction * (p2.x - p1.x), p1.y + fraction * (p2.y - p1.y),
1116 p1.z + fraction * (p2.z - p1.z));
1117 OTSPoint3D[] coords = new OTSPoint3D[index + 2];
1118 for (int i = 0; i <= index; i++)
1119 {
1120 coords[i] = this.points[i];
1121 }
1122 coords[index + 1] = newLastPoint;
1123 return new OTSLine3D(coords);
1124 }
1125
1126
1127
1128
1129
1130
1131
1132
1133
1134
1135
1136
1137
1138
1139
1140
1141
1142
1143
1144
1145
1146
1147
1148
1149
1150
1151
1152
1153
1154
1155
1156
1157
1158
1159
1160
1161
1162
1163
1164
1165
1166
1167 public final double projectOrthogonal(final double x, final double y)
1168 {
1169
1170
1171 makeLengthIndexedLine();
1172 double minDistance = Double.POSITIVE_INFINITY;
1173 double minSegmentFraction = 0;
1174 int minSegment = -1;
1175
1176
1177 for (int i = 0; i < size() - 1; i++)
1178 {
1179 double dx = this.points[i + 1].x - this.points[i].x;
1180 double dy = this.points[i + 1].y - this.points[i].y;
1181
1182 double px = x - this.points[i].x;
1183 double py = y - this.points[i].y;
1184
1185 double dot1 = px * dx + py * dy;
1186 double f;
1187 double distance;
1188 if (dot1 > 0)
1189 {
1190
1191 px = dx - px;
1192 py = dy - py;
1193
1194 double dot2 = px * dx + py * dy;
1195 if (dot2 > 0)
1196 {
1197
1198 double len2 = dx * dx + dy * dy;
1199 double proj = dot2 * dot2 / len2;
1200 f = dot1 / len2;
1201 distance = px * px + py * py - proj;
1202 }
1203 else
1204 {
1205
1206 f = 1;
1207 distance = px * px + py * py;
1208 }
1209 }
1210 else
1211 {
1212
1213 f = 0;
1214 distance = px * px + py * py;
1215 }
1216
1217 if (distance < minDistance)
1218 {
1219 minDistance = distance;
1220 minSegmentFraction = f;
1221 minSegment = i;
1222 }
1223 }
1224
1225
1226 double segLen = this.lengthIndexedLine[minSegment + 1] - this.lengthIndexedLine[minSegment];
1227 return (this.lengthIndexedLine[minSegment] + segLen * minSegmentFraction) / getLengthSI();
1228
1229 }
1230
1231
1232
1233
1234
1235
1236
1237
1238
1239
1240
1241
1242
1243
1244
1245
1246
1247
1248
1249
1250
1251
1252
1253
1254
1255
1256
1257
1258
1259
1260
1261
1262
1263
1264
1265
1266
1267
1268
1269
1270
1271
1272
1273
1274
1275
1276
1277
1278
1279
1280
1281
1282
1283
1284 public final double projectFractional(final Direction start, final Direction end, final double x, final double y)
1285 {
1286
1287
1288 makeLengthIndexedLine();
1289 double minDistance = Double.POSITIVE_INFINITY;
1290 double minSegmentFraction = 0;
1291 int minSegment = -1;
1292 OTSPoint3D point = new OTSPoint3D(x, y);
1293
1294
1295 determineFractionalHelpers(start, end);
1296
1297
1298 double[] d = new double[this.points.length - 1];
1299 double minD = Double.POSITIVE_INFINITY;
1300 for (int i = 0; i < this.points.length - 1; i++)
1301 {
1302 d[i] = Line2D.ptSegDist(this.points[i].x, this.points[i].y, this.points[i + 1].x, this.points[i + 1].y, x, y);
1303 minD = d[i] < minD ? d[i] : minD;
1304 }
1305
1306
1307 double distance;
1308 for (int i = 0; i < this.points.length - 1; i++)
1309 {
1310
1311 if (d[i] > minD + FRAC_PROJ_PRECISION)
1312 {
1313 continue;
1314 }
1315 OTSPoint3D center = this.fractionalHelperCenters[i];
1316 OTSPoint3D p;
1317 if (center != null)
1318 {
1319
1320 p = OTSPoint3D.intersectionOfLines(center, point, this.points[i], this.points[i + 1]);
1321 if (p == null || (x < center.x + FRAC_PROJ_PRECISION && center.x + FRAC_PROJ_PRECISION < p.x)
1322 || (x > center.x - FRAC_PROJ_PRECISION && center.x - FRAC_PROJ_PRECISION > p.x)
1323 || (y < center.y + FRAC_PROJ_PRECISION && center.y + FRAC_PROJ_PRECISION < p.y)
1324 || (y > center.y - FRAC_PROJ_PRECISION && center.y - FRAC_PROJ_PRECISION > p.y))
1325 {
1326
1327 continue;
1328 }
1329 }
1330 else
1331 {
1332
1333 OTSPoint3D offsetPoint =
1334 new OTSPoint3D(x + this.fractionalHelperDirections[i].x, y + this.fractionalHelperDirections[i].y);
1335 p = OTSPoint3D.intersectionOfLines(point, offsetPoint, this.points[i], this.points[i + 1]);
1336 }
1337 if (p == null || p.x < Math.min(this.points[i].x, this.points[i + 1].x) - FRAC_PROJ_PRECISION
1338 || p.x > Math.max(this.points[i].x, this.points[i + 1].x) + FRAC_PROJ_PRECISION
1339 || p.y < Math.min(this.points[i].y, this.points[i + 1].y) - FRAC_PROJ_PRECISION
1340 || p.y > Math.max(this.points[i].y, this.points[i + 1].y) + FRAC_PROJ_PRECISION)
1341 {
1342
1343
1344 continue;
1345 }
1346
1347 double dx = x - p.x;
1348 double dy = y - p.y;
1349 distance = Math.sqrt(dx * dx + dy * dy);
1350
1351 if (distance < minDistance)
1352 {
1353 dx = p.x - this.points[i].x;
1354 dy = p.y - this.points[i].y;
1355 double dFrac = Math.sqrt(dx * dx + dy * dy);
1356
1357 minDistance = distance;
1358 minSegmentFraction = dFrac / (this.lengthIndexedLine[i + 1] - this.lengthIndexedLine[i]);
1359 minSegment = i;
1360 }
1361 }
1362
1363
1364 if (minSegment == -1)
1365 {
1366
1367
1368
1369
1370
1371 return projectOrthogonal(x, y);
1372 }
1373 double segLen = this.lengthIndexedLine[minSegment + 1] - this.lengthIndexedLine[minSegment];
1374 return (this.lengthIndexedLine[minSegment] + segLen * minSegmentFraction) / getLengthSI();
1375
1376 }
1377
1378
1379
1380
1381
1382
1383
1384 private void determineFractionalHelpers(final Direction start, final Direction end)
1385 {
1386
1387 final int n = this.points.length - 1;
1388
1389
1390 if (this.fractionalHelperCenters == null)
1391 {
1392 this.fractionalHelperCenters = new OTSPoint3D[n];
1393 this.fractionalHelperDirections = new Point2D.Double[n];
1394 if (this.points.length > 2)
1395 {
1396
1397 OTSLine3D prevOfsSeg = unitOffsetSegment(0);
1398 OTSLine3D nextOfsSeg = unitOffsetSegment(1);
1399 OTSPoint3D parStartPoint;
1400 try
1401 {
1402 parStartPoint = OTSPoint3D.intersectionOfLines(prevOfsSeg.get(0), prevOfsSeg.get(1), nextOfsSeg.get(0),
1403 nextOfsSeg.get(1));
1404 if (parStartPoint == null)
1405 {
1406 parStartPoint = new OTSPoint3D((prevOfsSeg.get(1).x + nextOfsSeg.get(0).x) / 2,
1407 (prevOfsSeg.get(1).y + nextOfsSeg.get(0).y) / 2);
1408 }
1409 }
1410 catch (OTSGeometryException oge)
1411 {
1412
1413 throw new RuntimeException(oge);
1414 }
1415
1416 this.firstOffsetIntersection = parStartPoint;
1417
1418 for (int i = 1; i < this.points.length - 2; i++)
1419 {
1420 prevOfsSeg = nextOfsSeg;
1421 nextOfsSeg = unitOffsetSegment(i + 1);
1422 OTSPoint3D parEndPoint;
1423 try
1424 {
1425 parEndPoint = OTSPoint3D.intersectionOfLines(prevOfsSeg.get(0), prevOfsSeg.get(1), nextOfsSeg.get(0),
1426 nextOfsSeg.get(1));
1427 if (parEndPoint == null)
1428 {
1429 parEndPoint = new OTSPoint3D((prevOfsSeg.get(1).x + nextOfsSeg.get(0).x) / 2,
1430 (prevOfsSeg.get(1).y + nextOfsSeg.get(0).y) / 2);
1431 }
1432 }
1433 catch (OTSGeometryException oge)
1434 {
1435
1436 throw new RuntimeException(oge);
1437 }
1438
1439 this.fractionalHelperCenters[i] =
1440 OTSPoint3D.intersectionOfLines(this.points[i], parStartPoint, this.points[i + 1], parEndPoint);
1441 if (this.fractionalHelperCenters[i] == null)
1442 {
1443
1444 this.fractionalHelperDirections[i] =
1445 new Point2D.Double(parStartPoint.x - this.points[i].x, parStartPoint.y - this.points[i].y);
1446 }
1447 parStartPoint = parEndPoint;
1448 }
1449
1450 this.lastOffsetIntersection = parStartPoint;
1451 }
1452 }
1453
1454
1455 double ang = start.si + Math.PI / 2;
1456 OTSPoint3D p1 = new OTSPoint3D(this.points[0].x + Math.cos(ang), this.points[0].y + Math.sin(ang));
1457 ang = end.si + Math.PI / 2;
1458 OTSPoint3D p2 = new OTSPoint3D(this.points[n].x + Math.cos(ang), this.points[n].y + Math.sin(ang));
1459
1460
1461 if (this.points.length > 2)
1462 {
1463 this.fractionalHelperCenters[0] =
1464 OTSPoint3D.intersectionOfLines(this.points[0], p1, this.points[1], this.firstOffsetIntersection);
1465 this.fractionalHelperCenters[n - 1] =
1466 OTSPoint3D.intersectionOfLines(this.points[n - 1], this.lastOffsetIntersection, this.points[n], p2);
1467 if (this.fractionalHelperCenters[n - 1] == null)
1468 {
1469
1470 this.fractionalHelperDirections[n - 1] = new Point2D.Double(p2.x - this.points[n].x, p2.y - this.points[n].y);
1471 }
1472 }
1473 else
1474 {
1475
1476 this.fractionalHelperCenters[0] = OTSPoint3D.intersectionOfLines(this.points[0], p1, this.points[1], p2);
1477 this.fractionalHelperCenters[n - 1] = null;
1478 }
1479 if (this.fractionalHelperCenters[0] == null)
1480 {
1481
1482 this.fractionalHelperDirections[0] = new Point2D.Double(p1.x - this.points[0].x, p1.y - this.points[0].y);
1483 }
1484
1485 }
1486
1487
1488
1489
1490
1491
1492 private OTSLine3D unitOffsetSegment(final int segment)
1493 {
1494 OTSPoint3D from = new OTSPoint3D(this.points[segment].x, this.points[segment].y);
1495 OTSPoint3D to = new OTSPoint3D(this.points[segment + 1].x, this.points[segment + 1].y);
1496 try
1497 {
1498 OTSLine3D line = new OTSLine3D(from, to);
1499 return line.offsetLine(1.0);
1500 }
1501 catch (OTSGeometryException oge)
1502 {
1503
1504 throw new RuntimeException(oge);
1505 }
1506 }
1507
1508
1509
1510
1511
1512 private void calcCentroidBounds()
1513 {
1514 double minX = Double.POSITIVE_INFINITY;
1515 double minY = Double.POSITIVE_INFINITY;
1516 double minZ = Double.POSITIVE_INFINITY;
1517 double maxX = Double.NEGATIVE_INFINITY;
1518 double maxY = Double.NEGATIVE_INFINITY;
1519 double maxZ = Double.NEGATIVE_INFINITY;
1520 for (OTSPoint3D p : this.points)
1521 {
1522 minX = Math.min(minX, p.x);
1523 minY = Math.min(minY, p.y);
1524 minZ = Math.min(minZ, p.z);
1525 maxX = Math.max(maxX, p.x);
1526 maxY = Math.max(maxY, p.y);
1527 maxZ = Math.max(maxZ, p.z);
1528 }
1529 this.centroid = new OTSPoint3D((maxX + minX) / 2, (maxY + minY) / 2, (maxZ + minZ) / 2);
1530 double deltaX = maxX - minX;
1531 double deltaY = maxY - minY;
1532 double deltaZ = maxZ - minZ;
1533
1534 this.bounds = new BoundingBox(new Point3d(-deltaX / 2.0, -deltaY / 2.0, -deltaZ / 2.0),
1535 new Point3d(deltaX / 2, deltaY / 2, deltaZ / 2));
1536 this.envelope = new Envelope(minX, maxX, minY, maxY);
1537 }
1538
1539
1540
1541
1542
1543 public final OTSPoint3D getCentroid()
1544 {
1545 if (this.centroid == null)
1546 {
1547 calcCentroidBounds();
1548 }
1549 return this.centroid;
1550 }
1551
1552
1553
1554
1555
1556 public final Envelope getEnvelope()
1557 {
1558 if (this.envelope == null)
1559 {
1560 calcCentroidBounds();
1561 }
1562 return this.envelope;
1563 }
1564
1565
1566 @Override
1567 @SuppressWarnings("checkstyle:designforextension")
1568 public DirectedPoint getLocation()
1569 {
1570 if (this.centroid == null)
1571 {
1572 calcCentroidBounds();
1573 }
1574 return this.centroid.getDirectedPoint();
1575 }
1576
1577
1578 @Override
1579 @SuppressWarnings("checkstyle:designforextension")
1580 public Bounds getBounds()
1581 {
1582 if (this.bounds == null)
1583 {
1584 calcCentroidBounds();
1585 }
1586 return this.bounds;
1587 }
1588
1589
1590 @Override
1591 @SuppressWarnings("checkstyle:designforextension")
1592 public String toString()
1593 {
1594 return Arrays.toString(this.points);
1595 }
1596
1597
1598 @Override
1599 @SuppressWarnings("checkstyle:designforextension")
1600 public int hashCode()
1601 {
1602 final int prime = 31;
1603 int result = 1;
1604 result = prime * result + Arrays.hashCode(this.points);
1605 return result;
1606 }
1607
1608
1609 @Override
1610 @SuppressWarnings({ "checkstyle:designforextension", "checkstyle:needbraces" })
1611 public boolean equals(final Object obj)
1612 {
1613 if (this == obj)
1614 return true;
1615 if (obj == null)
1616 return false;
1617 if (getClass() != obj.getClass())
1618 return false;
1619 OTSLine3D other = (OTSLine3D) obj;
1620 if (!Arrays.equals(this.points, other.points))
1621 return false;
1622 return true;
1623 }
1624
1625
1626
1627
1628 public final String toExcel()
1629 {
1630 StringBuffer s = new StringBuffer();
1631 for (OTSPoint3D p : this.points)
1632 {
1633 s.append(p.x + "\t" + p.y + "\n");
1634 }
1635 return s.toString();
1636 }
1637
1638
1639
1640
1641
1642 public static void main(final String[] args) throws OTSGeometryException
1643 {
1644 OTSLine3D line = new OTSLine3D(new OTSPoint3D(-263.811, -86.551, 1.180), new OTSPoint3D(-262.945, -84.450, 1.180),
1645 new OTSPoint3D(-261.966, -82.074, 1.180), new OTSPoint3D(-260.890, -79.464, 1.198),
1646 new OTSPoint3D(-259.909, -76.955, 1.198), new OTSPoint3D(-258.911, -74.400, 1.198),
1647 new OTSPoint3D(-257.830, -71.633, 1.234));
1648 System.out.println(line.toExcel());
1649 double[] relativeFractions = new double[] { 0.0, 0.19827228089475762, 0.30549496392494213, 0.5824753163948581,
1650 0.6815307752261827, 0.7903990449840241, 0.8942375145295614, 1.0 };
1651 double[] offsets = new double[] { 2.9779999256134, 4.6029999256134, 3.886839156071996, 2.3664845198627207,
1652 1.7858981925396709, 1.472348149010167, 2.0416709053157285, 2.798692100483229 };
1653 System.out.println(line.offsetLine(relativeFractions, offsets).toExcel());
1654 }
1655 }