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 import java.util.Locale;
12
13 import javax.media.j3d.Bounds;
14 import javax.vecmath.Point3d;
15
16 import org.djunits.unit.DirectionUnit;
17 import org.djunits.unit.LengthUnit;
18 import org.djunits.value.vdouble.scalar.Direction;
19 import org.djunits.value.vdouble.scalar.Length;
20
21 import com.vividsolutions.jts.geom.Coordinate;
22 import com.vividsolutions.jts.geom.CoordinateSequence;
23 import com.vividsolutions.jts.geom.Envelope;
24 import com.vividsolutions.jts.geom.Geometry;
25 import com.vividsolutions.jts.geom.GeometryFactory;
26 import com.vividsolutions.jts.geom.LineString;
27 import com.vividsolutions.jts.linearref.LengthIndexedLine;
28
29 import edu.umd.cs.findbugs.annotations.SuppressFBWarnings;
30 import nl.tudelft.simulation.dsol.animation.Locatable;
31 import nl.tudelft.simulation.language.d3.BoundingBox;
32 import nl.tudelft.simulation.language.d3.DirectedPoint;
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48 public class OTSLine3D implements Locatable, Serializable
49 {
50
51 private static final long serialVersionUID = 20150722L;
52
53
54 private OTSPoint3D[] points;
55
56
57 private double[] lengthIndexedLine = null;
58
59
60 private double length = Double.NaN;
61
62
63 private OTSPoint3D centroid = null;
64
65
66 private Bounds bounds = null;
67
68
69 private OTSPoint3D[] fractionalHelperCenters = null;
70
71
72 private Point2D.Double[] fractionalHelperDirections = null;
73
74
75 private OTSPoint3D firstOffsetIntersection;
76
77
78 private OTSPoint3D lastOffsetIntersection;
79
80
81 private static final double FRAC_PROJ_PRECISION = 2e-5 ;
82
83
84 private Envelope envelope;
85
86
87
88
89
90
91
92 public OTSLine3D(final OTSPoint3D... points) throws OTSGeometryException
93 {
94 init(points);
95 }
96
97
98
99
100
101
102
103 private void init(final OTSPoint3D... pts) throws OTSGeometryException
104 {
105 if (pts.length < 2)
106 {
107 throw new OTSGeometryException("Degenerate OTSLine3D; has " + pts.length + " point" + (pts.length != 1 ? "s" : ""));
108 }
109 this.lengthIndexedLine = new double[pts.length];
110 this.lengthIndexedLine[0] = 0.0;
111 for (int i = 1; i < pts.length; i++)
112 {
113 if (pts[i - 1].x == pts[i].x && pts[i - 1].y == pts[i].y && pts[i - 1].z == pts[i].z)
114 {
115 throw new OTSGeometryException(
116 "Degenerate OTSLine3D; point " + (i - 1) + " has the same x, y and z as point " + i);
117 }
118 this.lengthIndexedLine[i] = this.lengthIndexedLine[i - 1] + pts[i - 1].distanceSI(pts[i]);
119 }
120 this.points = pts;
121 }
122
123
124 public enum OffsetMethod
125 {
126
127 JTS,
128
129
130 PK;
131 };
132
133
134 public static final OffsetMethod OFFSETMETHOD = OffsetMethod.PK;
135
136
137
138
139
140
141
142 public final OTSLine3D offsetLine(final double offset)
143 {
144 try
145 {
146 switch (OFFSETMETHOD)
147 {
148 case PK:
149 return OTSOffsetLinePK.offsetLine(this, offset);
150
151 case JTS:
152 return OTSBufferingJTS.offsetGeometryOLD(this, offset);
153
154 default:
155 return null;
156 }
157 }
158 catch (OTSGeometryException exception)
159 {
160 exception.printStackTrace();
161 return null;
162 }
163 }
164
165
166
167
168
169
170
171 public final OTSLine3D noiseFilteredLine(final double noiseLevel)
172 {
173 if (this.size() <= 2)
174 {
175 return this;
176 }
177 OTSPoint3D prevPoint = null;
178 List<OTSPoint3D> list = null;
179 for (int index = 0; index < this.size(); index++)
180 {
181 OTSPoint3D currentPoint = this.points[index];
182 if (null != prevPoint && prevPoint.distanceSI(currentPoint) < noiseLevel)
183 {
184 if (null == list)
185 {
186
187 list = new ArrayList<>();
188 for (int i = 0; i < index; i++)
189 {
190 list.add(this.points[i]);
191 }
192 }
193 if (index == this.size() - 1)
194 {
195 if (list.size() > 1)
196 {
197
198 list.set(list.size() - 1, currentPoint);
199 }
200 else
201 {
202
203
204 list.add(currentPoint);
205 }
206 }
207 continue;
208 }
209 else if (null != list)
210 {
211 list.add(currentPoint);
212 }
213 prevPoint = currentPoint;
214 }
215 if (null == list)
216 {
217 return this;
218 }
219 try
220 {
221 return new OTSLine3D(list);
222 }
223 catch (OTSGeometryException exception)
224 {
225 System.err.println("CANNOT HAPPEN");
226 exception.printStackTrace();
227 throw new Error(exception);
228 }
229 }
230
231
232
233
234
235
236
237
238
239 public final OTSLine3D noiseFilterRamerDouglasPeuker(final double epsilon, final boolean useHorizontalDistance)
240 {
241
242 try
243 {
244
245
246 double maxDeviation = 0;
247 int splitIndex = -1;
248 int pointCount = size();
249 OTSLine3D straight = new OTSLine3D(get(0), get(pointCount - 1));
250
251 for (int i = 1; i < pointCount - 1; i++)
252 {
253 OTSPoint3D point = get(i);
254 OTSPoint3D closest =
255 useHorizontalDistance ? point.closestPointOnLine2D(straight) : point.closestPointOnLine(straight);
256 double deviation = useHorizontalDistance ? closest.horizontalDistanceSI(point) : closest.distanceSI(point);
257 if (deviation > maxDeviation)
258 {
259 splitIndex = i;
260 maxDeviation = deviation;
261 }
262 }
263 if (maxDeviation <= epsilon)
264 {
265
266 return straight;
267 }
268
269
270
271 OTSLine3D first = new OTSLine3D(Arrays.copyOfRange(this.points, 0, splitIndex + 1))
272 .noiseFilterRamerDouglasPeuker(epsilon, useHorizontalDistance);
273 OTSLine3D second = new OTSLine3D(Arrays.copyOfRange(this.points, splitIndex, this.points.length))
274 .noiseFilterRamerDouglasPeuker(epsilon, useHorizontalDistance);
275 return concatenate(epsilon, first, second);
276 }
277 catch (OTSGeometryException exception)
278 {
279 exception.printStackTrace();
280 return null;
281 }
282 }
283
284
285
286
287
288
289
290
291
292 public final OTSLine3D offsetLine(final double offsetAtStart, final double offsetAtEnd) throws OTSGeometryException
293 {
294
295
296
297 OTSLine3D offsetLineAtStart = offsetLine(offsetAtStart);
298 if (offsetAtStart == offsetAtEnd)
299 {
300 return offsetLineAtStart;
301 }
302
303 OTSLine3D offsetLineAtEnd = offsetLine(offsetAtEnd);
304
305 Geometry startGeometry = offsetLineAtStart.getLineString();
306 Geometry endGeometry = offsetLineAtEnd.getLineString();
307 LengthIndexedLine first = new LengthIndexedLine(startGeometry);
308 double firstLength = startGeometry.getLength();
309 LengthIndexedLine second = new LengthIndexedLine(endGeometry);
310 double secondLength = endGeometry.getLength();
311 ArrayList<Coordinate> out = new ArrayList<>();
312 Coordinate[] firstCoordinates = startGeometry.getCoordinates();
313 Coordinate[] secondCoordinates = endGeometry.getCoordinates();
314 int firstIndex = 0;
315 int secondIndex = 0;
316 Coordinate prevCoordinate = null;
317 final double tooClose = 0.05;
318 while (firstIndex < firstCoordinates.length && secondIndex < secondCoordinates.length)
319 {
320 double firstRatio = firstIndex < firstCoordinates.length ? first.indexOf(firstCoordinates[firstIndex]) / firstLength
321 : Double.MAX_VALUE;
322 double secondRatio = secondIndex < secondCoordinates.length
323 ? second.indexOf(secondCoordinates[secondIndex]) / secondLength : Double.MAX_VALUE;
324 double ratio;
325 if (firstRatio < secondRatio)
326 {
327 ratio = firstRatio;
328 firstIndex++;
329 }
330 else
331 {
332 ratio = secondRatio;
333 secondIndex++;
334 }
335 Coordinate firstCoordinate = first.extractPoint(ratio * firstLength);
336 Coordinate secondCoordinate = second.extractPoint(ratio * secondLength);
337 Coordinate resultCoordinate = new Coordinate((1 - ratio) * firstCoordinate.x + ratio * secondCoordinate.x,
338 (1 - ratio) * firstCoordinate.y + ratio * secondCoordinate.y);
339 if (null == prevCoordinate || resultCoordinate.distance(prevCoordinate) > tooClose)
340 {
341 out.add(resultCoordinate);
342 prevCoordinate = resultCoordinate;
343 }
344 }
345 Coordinate[] resultCoordinates = new Coordinate[out.size()];
346 for (int index = 0; index < out.size(); index++)
347 {
348 resultCoordinates[index] = out.get(index);
349 }
350 return new OTSLine3D(resultCoordinates);
351 }
352
353
354
355
356
357
358
359
360
361
362 public final OTSLine3D offsetLine(final double[] relativeFractions, final double[] offsets) throws OTSGeometryException
363 {
364 OTSLine3D[] offsetLine = new OTSLine3D[relativeFractions.length];
365 for (int i = 0; i < offsets.length; i++)
366 {
367 offsetLine[i] = offsetLine(offsets[i]);
368
369
370 }
371
372 ArrayList<Coordinate> out = new ArrayList<>();
373 Coordinate prevCoordinate = null;
374 final double tooClose = 0.05;
375 for (int i = 0; i < offsets.length - 1; i++)
376 {
377 Geometry startGeometry =
378 offsetLine[i].extractFractional(relativeFractions[i], relativeFractions[i + 1]).getLineString();
379 Geometry endGeometry =
380 offsetLine[i + 1].extractFractional(relativeFractions[i], relativeFractions[i + 1]).getLineString();
381 LengthIndexedLine first = new LengthIndexedLine(startGeometry);
382 double firstLength = startGeometry.getLength();
383 LengthIndexedLine second = new LengthIndexedLine(endGeometry);
384 double secondLength = endGeometry.getLength();
385 Coordinate[] firstCoordinates = startGeometry.getCoordinates();
386 Coordinate[] secondCoordinates = endGeometry.getCoordinates();
387 int firstIndex = 0;
388 int secondIndex = 0;
389 while (firstIndex < firstCoordinates.length && secondIndex < secondCoordinates.length)
390 {
391 double firstRatio = firstIndex < firstCoordinates.length
392 ? first.indexOf(firstCoordinates[firstIndex]) / firstLength : Double.MAX_VALUE;
393 double secondRatio = secondIndex < secondCoordinates.length
394 ? second.indexOf(secondCoordinates[secondIndex]) / secondLength : Double.MAX_VALUE;
395 double ratio;
396 if (firstRatio < secondRatio)
397 {
398 ratio = firstRatio;
399 firstIndex++;
400 }
401 else
402 {
403 ratio = secondRatio;
404 secondIndex++;
405 }
406 Coordinate firstCoordinate = first.extractPoint(ratio * firstLength);
407 Coordinate secondCoordinate = second.extractPoint(ratio * secondLength);
408 Coordinate resultCoordinate = new Coordinate((1 - ratio) * firstCoordinate.x + ratio * secondCoordinate.x,
409 (1 - ratio) * firstCoordinate.y + ratio * secondCoordinate.y);
410 if (null == prevCoordinate || resultCoordinate.distance(prevCoordinate) > tooClose)
411 {
412 out.add(resultCoordinate);
413 prevCoordinate = resultCoordinate;
414 }
415 }
416 }
417
418 Coordinate[] resultCoordinates = new Coordinate[out.size()];
419 for (int index = 0; index < out.size(); index++)
420 {
421 resultCoordinates[index] = out.get(index);
422 }
423 return new OTSLine3D(resultCoordinates);
424 }
425
426
427
428
429
430
431
432
433 public static OTSLine3D concatenate(final OTSLine3D... lines) throws OTSGeometryException
434 {
435 return concatenate(0.0, lines);
436 }
437
438
439
440
441
442
443
444
445
446 public static OTSLine3D concatenate(final double toleranceSI, final OTSLine3D line1, final OTSLine3D line2)
447 throws OTSGeometryException
448 {
449 if (line1.getLast().distance(line2.getFirst()).si > toleranceSI)
450 {
451 throw new OTSGeometryException("Lines are not connected: " + line1.getLast() + " to " + line2.getFirst()
452 + " distance is " + line1.getLast().distance(line2.getFirst()).si + " > " + toleranceSI);
453 }
454 int size = line1.size() + line2.size() - 1;
455 OTSPoint3D[] points = new OTSPoint3D[size];
456 int nextIndex = 0;
457 for (int j = 0; j < line1.size(); j++)
458 {
459 points[nextIndex++] = line1.get(j);
460 }
461 for (int j = 1; j < line2.size(); j++)
462 {
463 points[nextIndex++] = line2.get(j);
464 }
465 return new OTSLine3D(points);
466 }
467
468
469
470
471
472
473
474
475
476 public static OTSLine3D concatenate(final double toleranceSI, final OTSLine3D... lines) throws OTSGeometryException
477 {
478
479 if (0 == lines.length)
480 {
481 throw new OTSGeometryException("Empty argument list");
482 }
483 else if (1 == lines.length)
484 {
485 return lines[0];
486 }
487 int size = lines[0].size();
488 for (int i = 1; i < lines.length; i++)
489 {
490 if (lines[i - 1].getLast().distance(lines[i].getFirst()).si > toleranceSI)
491 {
492 throw new OTSGeometryException(
493 "Lines are not connected: " + lines[i - 1].getLast() + " to " + lines[i].getFirst() + " distance is "
494 + lines[i - 1].getLast().distance(lines[i].getFirst()).si + " > " + toleranceSI);
495 }
496 size += lines[i].size() - 1;
497 }
498 OTSPoint3D[] points = new OTSPoint3D[size];
499 int nextIndex = 0;
500 for (int i = 0; i < lines.length; i++)
501 {
502 OTSLine3D line = lines[i];
503 for (int j = 0 == i ? 0 : 1; j < line.size(); j++)
504 {
505 points[nextIndex++] = line.get(j);
506 }
507 }
508 return new OTSLine3D(points);
509 }
510
511
512
513
514
515 public final OTSLine3D reverse()
516 {
517 OTSPoint3D[] resultPoints = new OTSPoint3D[size()];
518 int nextIndex = size();
519 for (OTSPoint3D p : getPoints())
520 {
521 resultPoints[--nextIndex] = p;
522 }
523 try
524 {
525 return new OTSLine3D(resultPoints);
526 }
527 catch (OTSGeometryException exception)
528 {
529
530 throw new RuntimeException(exception);
531 }
532 }
533
534
535
536
537
538
539
540
541 public final OTSLine3D extractFractional(final double start, final double end) throws OTSGeometryException
542 {
543 if (start < 0 || start >= end || end > 1)
544 {
545 throw new OTSGeometryException("Bad interval");
546 }
547 getLength();
548 return extract(start * this.length, end * this.length);
549 }
550
551
552
553
554
555
556
557
558
559 public final OTSLine3D extract(final Length start, final Length end) throws OTSGeometryException
560 {
561 return extract(start.si, end.si);
562 }
563
564
565
566
567
568
569
570
571
572 @SuppressFBWarnings("FE_FLOATING_POINT_EQUALITY")
573 public final OTSLine3D extract(final double start, final double end) throws OTSGeometryException
574 {
575 if (Double.isNaN(start) || Double.isNaN(end) || start < 0 || start >= end || end > getLengthSI())
576 {
577 throw new OTSGeometryException(
578 "Bad interval (" + start + ".." + end + "; length of this OTSLine3D is " + this.getLengthSI() + ")");
579 }
580 double cumulativeLength = 0;
581 double nextCumulativeLength = 0;
582 double segmentLength = 0;
583 int index = 0;
584 List<OTSPoint3D> pointList = new ArrayList<>();
585
586 while (start > cumulativeLength)
587 {
588 OTSPoint3D fromPoint = this.points[index];
589 index++;
590 OTSPoint3D toPoint = this.points[index];
591 segmentLength = fromPoint.distanceSI(toPoint);
592 cumulativeLength = nextCumulativeLength;
593 nextCumulativeLength = cumulativeLength + segmentLength;
594 if (nextCumulativeLength >= start)
595 {
596 break;
597 }
598 }
599 if (start == nextCumulativeLength)
600 {
601 pointList.add(this.points[index]);
602 }
603 else
604 {
605 pointList.add(OTSPoint3D.interpolate((start - cumulativeLength) / segmentLength, this.points[index - 1],
606 this.points[index]));
607 if (end > nextCumulativeLength)
608 {
609 pointList.add(this.points[index]);
610 }
611 }
612 while (end > nextCumulativeLength)
613 {
614 OTSPoint3D fromPoint = this.points[index];
615 index++;
616 if (index >= this.points.length)
617 {
618 break;
619 }
620 OTSPoint3D toPoint = this.points[index];
621 segmentLength = fromPoint.distanceSI(toPoint);
622 cumulativeLength = nextCumulativeLength;
623 nextCumulativeLength = cumulativeLength + segmentLength;
624 if (nextCumulativeLength >= end)
625 {
626 break;
627 }
628 pointList.add(toPoint);
629 }
630 if (end == nextCumulativeLength)
631 {
632 pointList.add(this.points[index]);
633 }
634 else
635 {
636 OTSPoint3D point = OTSPoint3D.interpolate((end - cumulativeLength) / segmentLength, this.points[index - 1],
637 this.points[index]);
638
639 if (!point.equals(pointList.get(pointList.size() - 1)))
640 {
641 pointList.add(point);
642 }
643 }
644 try
645 {
646 return new OTSLine3D(pointList);
647 }
648 catch (@SuppressWarnings("unused") OTSGeometryException exception)
649 {
650 System.err.println("interval " + start + ".." + end + " too short");
651 throw new OTSGeometryException("interval " + start + ".." + end + "too short");
652 }
653 }
654
655
656
657
658
659
660 private static OTSPoint3D[] coordinatesToOTSPoint3D(final Coordinate[] coordinates)
661 {
662 OTSPoint3D[] result = new OTSPoint3D[coordinates.length];
663 for (int i = 0; i < coordinates.length; i++)
664 {
665 result[i] = new OTSPoint3D(coordinates[i]);
666 }
667 return result;
668 }
669
670
671
672
673
674
675
676 public static OTSLine3D createAndCleanOTSLine3D(final OTSPoint3D... points) throws OTSGeometryException
677 {
678 if (points.length < 2)
679 {
680 throw new OTSGeometryException(
681 "Degenerate OTSLine3D; has " + points.length + " point" + (points.length != 1 ? "s" : ""));
682 }
683 return createAndCleanOTSLine3D(new ArrayList<>(Arrays.asList(points)));
684 }
685
686
687
688
689
690
691
692
693 public static OTSLine3D createAndCleanOTSLine3D(final List<OTSPoint3D> pointList) throws OTSGeometryException
694 {
695
696 int i = 1;
697 while (i < pointList.size())
698 {
699 if (pointList.get(i - 1).equals(pointList.get(i)))
700 {
701 pointList.remove(i);
702 }
703 else
704 {
705 i++;
706 }
707 }
708 return new OTSLine3D(pointList);
709 }
710
711
712
713
714
715
716
717 public OTSLine3D(final Coordinate[] coordinates) throws OTSGeometryException
718 {
719 this(coordinatesToOTSPoint3D(coordinates));
720 }
721
722
723
724
725
726
727
728 public OTSLine3D(final LineString lineString) throws OTSGeometryException
729 {
730 this(lineString.getCoordinates());
731 }
732
733
734
735
736
737
738
739 public OTSLine3D(final Geometry geometry) throws OTSGeometryException
740 {
741 this(geometry.getCoordinates());
742 }
743
744
745
746
747
748
749
750 public OTSLine3D(final List<OTSPoint3D> pointList) throws OTSGeometryException
751 {
752 this(pointList.toArray(new OTSPoint3D[pointList.size()]));
753 }
754
755
756
757
758
759
760
761 public OTSLine3D(final Path2D path) throws OTSGeometryException
762 {
763 List<OTSPoint3D> pl = new ArrayList<>();
764 for (PathIterator pi = path.getPathIterator(null); !pi.isDone(); pi.next())
765 {
766 double[] p = new double[6];
767 int segType = pi.currentSegment(p);
768 if (segType == PathIterator.SEG_MOVETO || segType == PathIterator.SEG_LINETO)
769 {
770 pl.add(new OTSPoint3D(p[0], p[1]));
771 }
772 else if (segType == PathIterator.SEG_CLOSE)
773 {
774 if (!pl.get(0).equals(pl.get(pl.size() - 1)))
775 {
776 pl.add(new OTSPoint3D(pl.get(0).x, pl.get(0).y));
777 }
778 break;
779 }
780 }
781 init(pl.toArray(new OTSPoint3D[pl.size() - 1]));
782 }
783
784
785
786
787
788 public final Coordinate[] getCoordinates()
789 {
790 Coordinate[] result = new Coordinate[size()];
791 for (int i = 0; i < size(); i++)
792 {
793 result[i] = this.points[i].getCoordinate();
794 }
795 return result;
796 }
797
798
799
800
801
802 public final LineString getLineString()
803 {
804 GeometryFactory factory = new GeometryFactory();
805 Coordinate[] coordinates = getCoordinates();
806 CoordinateSequence cs = factory.getCoordinateSequenceFactory().create(coordinates);
807 return new LineString(cs, factory);
808 }
809
810
811
812
813
814 public final int size()
815 {
816 return this.points.length;
817 }
818
819
820
821
822
823 public final OTSPoint3D getFirst()
824 {
825 return this.points[0];
826 }
827
828
829
830
831
832 public final OTSPoint3D getLast()
833 {
834 return this.points[size() - 1];
835 }
836
837
838
839
840
841
842
843 public final OTSPoint3D get(final int i) throws OTSGeometryException
844 {
845 if (i < 0 || i > size() - 1)
846 {
847 throw new OTSGeometryException("OTSLine3D.get(i=" + i + "); i<0 or i>=size(), which is " + size());
848 }
849 return this.points[i];
850 }
851
852
853
854
855
856
857 public final synchronized double getLengthSI()
858 {
859 if (Double.isNaN(this.length))
860 {
861 this.length = 0.0;
862 for (int i = 0; i < size() - 1; i++)
863 {
864 this.length += this.points[i].distanceSI(this.points[i + 1]);
865 }
866 }
867 return this.length;
868 }
869
870
871
872
873
874
875 public final Length getLength()
876 {
877 return new Length(getLengthSI(), LengthUnit.SI);
878 }
879
880
881
882
883
884 public final OTSPoint3D[] getPoints()
885 {
886 return this.points;
887 }
888
889
890
891
892 private void makeLengthIndexedLine()
893 {
894 if (this.lengthIndexedLine == null)
895 {
896 this.lengthIndexedLine = new double[this.points.length];
897 this.lengthIndexedLine[0] = 0.0;
898 for (int i = 1; i < this.points.length; i++)
899 {
900 this.lengthIndexedLine[i] = this.lengthIndexedLine[i - 1] + this.points[i - 1].distanceSI(this.points[i]);
901 }
902 }
903 }
904
905
906
907
908
909
910
911 public final DirectedPoint getLocationExtended(final Length position)
912 {
913 return getLocationExtendedSI(position.getSI());
914 }
915
916
917
918
919
920
921
922 public final DirectedPoint getLocationExtendedSI(final double positionSI)
923 {
924 makeLengthIndexedLine();
925 if (positionSI >= 0.0 && positionSI <= getLengthSI())
926 {
927 try
928 {
929 return getLocationSI(positionSI);
930 }
931 catch (@SuppressWarnings("unused") OTSGeometryException exception)
932 {
933
934 }
935 }
936
937
938 if (positionSI < 0.0)
939 {
940 double len = positionSI;
941 double fraction = len / (this.lengthIndexedLine[1] - this.lengthIndexedLine[0]);
942 OTSPoint3D p1 = this.points[0];
943 OTSPoint3D p2 = this.points[1];
944 return new DirectedPoint(p1.x + fraction * (p2.x - p1.x), p1.y + fraction * (p2.y - p1.y),
945 p1.z + fraction * (p2.z - p1.z), 0.0, 0.0, Math.atan2(p2.y - p1.y, p2.x - p1.x));
946 }
947
948
949 int n1 = this.lengthIndexedLine.length - 1;
950 int n2 = this.lengthIndexedLine.length - 2;
951 double len = positionSI - getLengthSI();
952 double fraction = len / (this.lengthIndexedLine[n1] - this.lengthIndexedLine[n2]);
953 OTSPoint3D p1 = this.points[n2];
954 OTSPoint3D p2 = this.points[n1];
955 return new DirectedPoint(p2.x + fraction * (p2.x - p1.x), p2.y + fraction * (p2.y - p1.y),
956 p2.z + fraction * (p2.z - p1.z), 0.0, 0.0, Math.atan2(p2.y - p1.y, p2.x - p1.x));
957 }
958
959
960
961
962
963
964
965 public final DirectedPoint getLocationFraction(final double fraction) throws OTSGeometryException
966 {
967 if (fraction < 0.0 || fraction > 1.0)
968 {
969 throw new OTSGeometryException("getLocationFraction for line: fraction < 0.0 or > 1.0. fraction = " + fraction);
970 }
971 return getLocationSI(fraction * getLengthSI());
972 }
973
974
975
976
977
978
979
980
981 public final DirectedPoint getLocationFraction(final double fraction, final double tolerance) throws OTSGeometryException
982 {
983 if (fraction < -tolerance || fraction > 1.0 + tolerance)
984 {
985 throw new OTSGeometryException(
986 "getLocationFraction for line: fraction < 0.0 - tolerance or > 1.0 + tolerance; fraction = " + fraction);
987 }
988 double f = fraction < 0 ? 0.0 : fraction > 1.0 ? 1.0 : fraction;
989 return getLocationSI(f * getLengthSI());
990 }
991
992
993
994
995
996
997 public final DirectedPoint getLocationFractionExtended(final double fraction)
998 {
999 return getLocationExtendedSI(fraction * getLengthSI());
1000 }
1001
1002
1003
1004
1005
1006
1007
1008 public final DirectedPoint getLocation(final Length position) throws OTSGeometryException
1009 {
1010 return getLocationSI(position.getSI());
1011 }
1012
1013
1014
1015
1016
1017
1018
1019 private int find(final double pos) throws OTSGeometryException
1020 {
1021 if (pos == 0)
1022 {
1023 return 0;
1024 }
1025
1026 int lo = 0;
1027 int hi = this.lengthIndexedLine.length - 1;
1028 while (lo <= hi)
1029 {
1030 if (hi == lo)
1031 {
1032 return lo;
1033 }
1034 int mid = lo + (hi - lo) / 2;
1035 if (pos < this.lengthIndexedLine[mid])
1036 {
1037 hi = mid - 1;
1038 }
1039 else if (pos > this.lengthIndexedLine[mid + 1])
1040 {
1041 lo = mid + 1;
1042 }
1043 else
1044 {
1045 return mid;
1046 }
1047 }
1048 throw new OTSGeometryException(
1049 "Could not find position " + pos + " on line with length indexes: " + Arrays.toString(this.lengthIndexedLine));
1050 }
1051
1052
1053
1054
1055
1056
1057
1058 public final DirectedPoint getLocationSI(final double positionSI) throws OTSGeometryException
1059 {
1060 makeLengthIndexedLine();
1061 if (positionSI < 0.0 || positionSI > getLengthSI())
1062 {
1063 throw new OTSGeometryException("getLocationSI for line: position < 0.0 or > line length. Position = " + positionSI
1064 + " m. Length = " + getLengthSI() + " m.");
1065 }
1066
1067
1068 if (positionSI == 0.0)
1069 {
1070 OTSPoint3D p1 = this.points[0];
1071 OTSPoint3D p2 = this.points[1];
1072 return new DirectedPoint(p1.x, p1.y, p1.z, 0.0, 0.0, Math.atan2(p2.y - p1.y, p2.x - p1.x));
1073 }
1074 if (positionSI == getLengthSI())
1075 {
1076 OTSPoint3D p1 = this.points[this.points.length - 2];
1077 OTSPoint3D p2 = this.points[this.points.length - 1];
1078 return new DirectedPoint(p2.x, p2.y, p2.z, 0.0, 0.0, Math.atan2(p2.y - p1.y, p2.x - p1.x));
1079 }
1080
1081
1082 int index = find(positionSI);
1083 double remainder = positionSI - this.lengthIndexedLine[index];
1084 double fraction = remainder / (this.lengthIndexedLine[index + 1] - this.lengthIndexedLine[index]);
1085 OTSPoint3D p1 = this.points[index];
1086 OTSPoint3D p2 = this.points[index + 1];
1087 return new DirectedPoint(p1.x + fraction * (p2.x - p1.x), p1.y + fraction * (p2.y - p1.y),
1088 p1.z + fraction * (p2.z - p1.z), 0.0, 0.0, Math.atan2(p2.y - p1.y, p2.x - p1.x));
1089 }
1090
1091
1092
1093
1094
1095
1096
1097 public final OTSLine3D truncate(final double lengthSI) throws OTSGeometryException
1098 {
1099 makeLengthIndexedLine();
1100 if (lengthSI <= 0.0 || lengthSI > getLengthSI())
1101 {
1102 throw new OTSGeometryException("truncate for line: position <= 0.0 or > line length. Position = " + lengthSI
1103 + " m. Length = " + getLengthSI() + " m.");
1104 }
1105
1106
1107 if (lengthSI == getLengthSI())
1108 {
1109 return new OTSLine3D(getPoints());
1110 }
1111
1112
1113 int index = find(lengthSI);
1114 double remainder = lengthSI - this.lengthIndexedLine[index];
1115 double fraction = remainder / (this.lengthIndexedLine[index + 1] - this.lengthIndexedLine[index]);
1116 OTSPoint3D p1 = this.points[index];
1117 OTSPoint3D p2 = this.points[index + 1];
1118 OTSPoint3D newLastPoint = new OTSPoint3D(p1.x + fraction * (p2.x - p1.x), p1.y + fraction * (p2.y - p1.y),
1119 p1.z + fraction * (p2.z - p1.z));
1120 OTSPoint3D[] coords = new OTSPoint3D[index + 2];
1121 for (int i = 0; i <= index; i++)
1122 {
1123 coords[i] = this.points[i];
1124 }
1125 coords[index + 1] = newLastPoint;
1126 return new OTSLine3D(coords);
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
1168
1169
1170 public final double projectOrthogonal(final double x, final double y)
1171 {
1172
1173
1174 makeLengthIndexedLine();
1175 double minDistance = Double.POSITIVE_INFINITY;
1176 double minSegmentFraction = 0;
1177 int minSegment = -1;
1178
1179
1180 for (int i = 0; i < size() - 1; i++)
1181 {
1182 double dx = this.points[i + 1].x - this.points[i].x;
1183 double dy = this.points[i + 1].y - this.points[i].y;
1184
1185 double px = x - this.points[i].x;
1186 double py = y - this.points[i].y;
1187
1188 double dot1 = px * dx + py * dy;
1189 double f;
1190 double distance;
1191 if (dot1 > 0)
1192 {
1193
1194 px = dx - px;
1195 py = dy - py;
1196
1197 double dot2 = px * dx + py * dy;
1198 if (dot2 > 0)
1199 {
1200
1201 double len2 = dx * dx + dy * dy;
1202 double proj = dot2 * dot2 / len2;
1203 f = dot1 / len2;
1204 distance = px * px + py * py - proj;
1205 }
1206 else
1207 {
1208
1209 f = 1;
1210 distance = px * px + py * py;
1211 }
1212 }
1213 else
1214 {
1215
1216 f = 0;
1217 distance = px * px + py * py;
1218 }
1219
1220 if (distance < minDistance)
1221 {
1222 minDistance = distance;
1223 minSegmentFraction = f;
1224 minSegment = i;
1225 }
1226 }
1227
1228
1229 double segLen = this.lengthIndexedLine[minSegment + 1] - this.lengthIndexedLine[minSegment];
1230 return (this.lengthIndexedLine[minSegment] + segLen * minSegmentFraction) / getLengthSI();
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
1285
1286
1287
1288
1289 public final double projectFractional(final Direction start, final Direction end, final double x, final double y,
1290 final FractionalFallback fallback)
1291 {
1292
1293
1294 makeLengthIndexedLine();
1295 double minDistance = Double.POSITIVE_INFINITY;
1296 double minSegmentFraction = 0;
1297 int minSegment = -1;
1298 OTSPoint3D point = new OTSPoint3D(x, y);
1299
1300
1301 determineFractionalHelpers(start, end);
1302
1303
1304 double[] d = new double[this.points.length - 1];
1305 double minD = Double.POSITIVE_INFINITY;
1306 for (int i = 0; i < this.points.length - 1; i++)
1307 {
1308 d[i] = Line2D.ptSegDist(this.points[i].x, this.points[i].y, this.points[i + 1].x, this.points[i + 1].y, x, y);
1309 minD = d[i] < minD ? d[i] : minD;
1310 }
1311
1312
1313 double distance;
1314 for (int i = 0; i < this.points.length - 1; i++)
1315 {
1316
1317 if (d[i] > minD + FRAC_PROJ_PRECISION)
1318 {
1319 continue;
1320 }
1321 OTSPoint3D center = this.fractionalHelperCenters[i];
1322 OTSPoint3D p;
1323 if (center != null)
1324 {
1325
1326 p = OTSPoint3D.intersectionOfLines(center, point, this.points[i], this.points[i + 1]);
1327 if (p == null || (x < center.x + FRAC_PROJ_PRECISION && center.x + FRAC_PROJ_PRECISION < p.x)
1328 || (x > center.x - FRAC_PROJ_PRECISION && center.x - FRAC_PROJ_PRECISION > p.x)
1329 || (y < center.y + FRAC_PROJ_PRECISION && center.y + FRAC_PROJ_PRECISION < p.y)
1330 || (y > center.y - FRAC_PROJ_PRECISION && center.y - FRAC_PROJ_PRECISION > p.y))
1331 {
1332
1333 continue;
1334 }
1335 }
1336 else
1337 {
1338
1339 OTSPoint3D offsetPoint =
1340 new OTSPoint3D(x + this.fractionalHelperDirections[i].x, y + this.fractionalHelperDirections[i].y);
1341 p = OTSPoint3D.intersectionOfLines(point, offsetPoint, this.points[i], this.points[i + 1]);
1342 }
1343 double segLength = this.points[i].distance(this.points[i + 1]).si + FRAC_PROJ_PRECISION;
1344 if (p == null || this.points[i].distance(p).si > segLength || this.points[i + 1].distance(p).si > segLength)
1345 {
1346
1347
1348 continue;
1349 }
1350
1351 double dx = x - p.x;
1352 double dy = y - p.y;
1353 distance = Math.sqrt(dx * dx + dy * dy);
1354
1355 if (distance < minDistance)
1356 {
1357 dx = p.x - this.points[i].x;
1358 dy = p.y - this.points[i].y;
1359 double dFrac = Math.sqrt(dx * dx + dy * dy);
1360
1361 minDistance = distance;
1362 minSegmentFraction = dFrac / (this.lengthIndexedLine[i + 1] - this.lengthIndexedLine[i]);
1363 minSegment = i;
1364 }
1365 }
1366
1367
1368 if (minSegment == -1)
1369
1370 {
1371
1372
1373
1374
1375
1376
1377 return fallback.getFraction(this, x, y);
1378 }
1379
1380 double segLen = this.lengthIndexedLine[minSegment + 1] - this.lengthIndexedLine[minSegment];
1381 return (this.lengthIndexedLine[minSegment] + segLen * minSegmentFraction) /
1382
1383 getLengthSI();
1384
1385 }
1386
1387
1388
1389
1390
1391
1392
1393
1394
1395
1396
1397
1398
1399 public enum FractionalFallback
1400 {
1401
1402 ORTHOGONAL
1403 {
1404 @Override
1405 double getFraction(final OTSLine3D line, final double x, final double y)
1406 {
1407 return line.projectOrthogonal(x, y);
1408 }
1409 },
1410
1411
1412 ENDPOINT
1413 {
1414 @Override
1415 double getFraction(final OTSLine3D line, final double x, final double y)
1416 {
1417 OTSPoint3D point = new OTSPoint3D(x, y);
1418 double dStart = point.distanceSI(line.getFirst());
1419 double dEnd = point.distanceSI(line.getLast());
1420 if (dStart < dEnd)
1421 {
1422 return -dStart / line.getLengthSI();
1423 }
1424 else
1425 {
1426 return (dEnd + line.getLengthSI()) / line.getLengthSI();
1427 }
1428 }
1429 },
1430
1431
1432 NaN
1433 {
1434 @Override
1435 double getFraction(final OTSLine3D line, final double x, final double y)
1436 {
1437 return Double.NaN;
1438 }
1439 };
1440
1441
1442
1443
1444
1445
1446
1447
1448 abstract double getFraction(OTSLine3D line, double x, double y);
1449
1450 }
1451
1452
1453
1454
1455
1456
1457
1458 private void determineFractionalHelpers(final Direction start, final Direction end)
1459 {
1460
1461 final int n = this.points.length - 1;
1462
1463
1464 if (this.fractionalHelperCenters == null)
1465 {
1466 this.fractionalHelperCenters = new OTSPoint3D[n];
1467 this.fractionalHelperDirections = new Point2D.Double[n];
1468 if (this.points.length > 2)
1469 {
1470
1471 OTSLine3D prevOfsSeg = unitOffsetSegment(0);
1472 OTSLine3D nextOfsSeg = unitOffsetSegment(1);
1473 OTSPoint3D parStartPoint;
1474 try
1475 {
1476 parStartPoint = OTSPoint3D.intersectionOfLines(prevOfsSeg.get(0), prevOfsSeg.get(1), nextOfsSeg.get(0),
1477 nextOfsSeg.get(1));
1478 if (parStartPoint == null || prevOfsSeg.get(1).distanceSI(nextOfsSeg.get(0)) < Math
1479 .min(prevOfsSeg.get(1).distanceSI(parStartPoint), nextOfsSeg.get(0).distanceSI(parStartPoint)))
1480 {
1481 parStartPoint = new OTSPoint3D((prevOfsSeg.get(1).x + nextOfsSeg.get(0).x) / 2,
1482 (prevOfsSeg.get(1).y + nextOfsSeg.get(0).y) / 2);
1483 }
1484 }
1485 catch (OTSGeometryException oge)
1486 {
1487
1488 throw new RuntimeException(oge);
1489 }
1490
1491 this.firstOffsetIntersection = parStartPoint;
1492
1493 for (int i = 1; i < this.points.length - 2; i++)
1494 {
1495 prevOfsSeg = nextOfsSeg;
1496 nextOfsSeg = unitOffsetSegment(i + 1);
1497 OTSPoint3D parEndPoint;
1498 try
1499 {
1500 parEndPoint = OTSPoint3D.intersectionOfLines(prevOfsSeg.get(0), prevOfsSeg.get(1), nextOfsSeg.get(0),
1501 nextOfsSeg.get(1));
1502 if (parEndPoint == null || prevOfsSeg.get(1).distanceSI(nextOfsSeg.get(0)) < Math
1503 .min(prevOfsSeg.get(1).distanceSI(parEndPoint), nextOfsSeg.get(0).distanceSI(parEndPoint)))
1504 {
1505 parEndPoint = new OTSPoint3D((prevOfsSeg.get(1).x + nextOfsSeg.get(0).x) / 2,
1506 (prevOfsSeg.get(1).y + nextOfsSeg.get(0).y) / 2);
1507 }
1508 }
1509 catch (OTSGeometryException oge)
1510 {
1511
1512 throw new RuntimeException(oge);
1513 }
1514
1515 this.fractionalHelperCenters[i] =
1516 OTSPoint3D.intersectionOfLines(this.points[i], parStartPoint, this.points[i + 1], parEndPoint);
1517 if (this.fractionalHelperCenters[i] == null)
1518 {
1519
1520 this.fractionalHelperDirections[i] =
1521 new Point2D.Double(parStartPoint.x - this.points[i].x, parStartPoint.y - this.points[i].y);
1522 }
1523 parStartPoint = parEndPoint;
1524 }
1525
1526 this.lastOffsetIntersection = parStartPoint;
1527 }
1528 }
1529
1530
1531 double ang = (start == null ? Math.atan2(this.points[1].y - this.points[0].y, this.points[1].x - this.points[0].x)
1532 : start.getInUnit(DirectionUnit.BASE)) + Math.PI / 2;
1533 OTSPoint3D p1 = new OTSPoint3D(this.points[0].x + Math.cos(ang), this.points[0].y + Math.sin(ang));
1534 ang = (end == null ? Math.atan2(this.points[n].y - this.points[n - 1].y, this.points[n].x - this.points[n - 1].x)
1535 : end.getInUnit(DirectionUnit.BASE)) + Math.PI / 2;
1536 OTSPoint3D p2 = new OTSPoint3D(this.points[n].x + Math.cos(ang), this.points[n].y + Math.sin(ang));
1537
1538
1539 if (this.points.length > 2)
1540 {
1541 this.fractionalHelperCenters[0] =
1542 OTSPoint3D.intersectionOfLines(this.points[0], p1, this.points[1], this.firstOffsetIntersection);
1543 this.fractionalHelperCenters[n - 1] =
1544 OTSPoint3D.intersectionOfLines(this.points[n - 1], this.lastOffsetIntersection, this.points[n], p2);
1545 if (this.fractionalHelperCenters[n - 1] == null)
1546 {
1547
1548 this.fractionalHelperDirections[n - 1] = new Point2D.Double(p2.x - this.points[n].x, p2.y - this.points[n].y);
1549 }
1550 }
1551 else
1552 {
1553
1554 this.fractionalHelperCenters[0] = OTSPoint3D.intersectionOfLines(this.points[0], p1, this.points[1], p2);
1555 this.fractionalHelperCenters[n - 1] = null;
1556 }
1557 if (this.fractionalHelperCenters[0] == null)
1558 {
1559
1560 this.fractionalHelperDirections[0] = new Point2D.Double(p1.x - this.points[0].x, p1.y - this.points[0].y);
1561 }
1562
1563 }
1564
1565
1566
1567
1568
1569
1570 private OTSLine3D unitOffsetSegment(final int segment)
1571 {
1572
1573
1574
1575
1576
1577
1578
1579
1580
1581
1582
1583
1584
1585
1586
1587
1588
1589
1590
1591
1592
1593
1594
1595 OTSPoint3D from = new OTSPoint3D(this.points[segment].x, this.points[segment].y);
1596 OTSPoint3D to = new OTSPoint3D(this.points[segment + 1].x, this.points[segment + 1].y);
1597 try
1598 {
1599 OTSLine3D line = new OTSLine3D(from, to);
1600 return line.offsetLine(1.0);
1601 }
1602 catch (OTSGeometryException oge)
1603 {
1604
1605 throw new RuntimeException(oge);
1606 }
1607 }
1608
1609
1610
1611
1612
1613 private void calcCentroidBounds()
1614 {
1615 double minX = Double.POSITIVE_INFINITY;
1616 double minY = Double.POSITIVE_INFINITY;
1617 double minZ = Double.POSITIVE_INFINITY;
1618 double maxX = Double.NEGATIVE_INFINITY;
1619 double maxY = Double.NEGATIVE_INFINITY;
1620 double maxZ = Double.NEGATIVE_INFINITY;
1621 for (OTSPoint3D p : this.points)
1622 {
1623 minX = Math.min(minX, p.x);
1624 minY = Math.min(minY, p.y);
1625 minZ = Math.min(minZ, p.z);
1626 maxX = Math.max(maxX, p.x);
1627 maxY = Math.max(maxY, p.y);
1628 maxZ = Math.max(maxZ, p.z);
1629 }
1630 this.centroid = new OTSPoint3D((maxX + minX) / 2, (maxY + minY) / 2, (maxZ + minZ) / 2);
1631 double deltaX = maxX - minX;
1632 double deltaY = maxY - minY;
1633 double deltaZ = maxZ - minZ;
1634
1635 this.bounds = new BoundingBox(new Point3d(-deltaX / 2.0, -deltaY / 2.0, -deltaZ / 2.0),
1636 new Point3d(deltaX / 2, deltaY / 2, deltaZ / 2));
1637 this.envelope = new Envelope(minX, maxX, minY, maxY);
1638 }
1639
1640
1641
1642
1643
1644 public final OTSPoint3D getCentroid()
1645 {
1646 if (this.centroid == null)
1647 {
1648 calcCentroidBounds();
1649 }
1650 return this.centroid;
1651 }
1652
1653
1654
1655
1656
1657 public final Envelope getEnvelope()
1658 {
1659 if (this.envelope == null)
1660 {
1661 calcCentroidBounds();
1662 }
1663 return this.envelope;
1664 }
1665
1666
1667 @Override
1668 @SuppressWarnings("checkstyle:designforextension")
1669 public DirectedPoint getLocation()
1670 {
1671 if (this.centroid == null)
1672 {
1673 calcCentroidBounds();
1674 }
1675 return this.centroid.getDirectedPoint();
1676 }
1677
1678
1679 @Override
1680 @SuppressWarnings("checkstyle:designforextension")
1681 public Bounds getBounds()
1682 {
1683 if (this.bounds == null)
1684 {
1685 calcCentroidBounds();
1686 }
1687 return this.bounds;
1688 }
1689
1690
1691 @Override
1692 @SuppressWarnings("checkstyle:designforextension")
1693 public String toString()
1694 {
1695 return Arrays.toString(this.points);
1696 }
1697
1698
1699 @Override
1700 @SuppressWarnings("checkstyle:designforextension")
1701 public int hashCode()
1702 {
1703 final int prime = 31;
1704 int result = 1;
1705 result = prime * result + Arrays.hashCode(this.points);
1706 return result;
1707 }
1708
1709
1710 @Override
1711 @SuppressWarnings({ "checkstyle:designforextension", "checkstyle:needbraces" })
1712 public boolean equals(final Object obj)
1713 {
1714 if (this == obj)
1715 return true;
1716 if (obj == null)
1717 return false;
1718 if (getClass() != obj.getClass())
1719 return false;
1720 OTSLine3D other = (OTSLine3D) obj;
1721 if (!Arrays.equals(this.points, other.points))
1722 return false;
1723 return true;
1724 }
1725
1726
1727
1728
1729 public final String toExcel()
1730 {
1731 StringBuffer s = new StringBuffer();
1732 for (OTSPoint3D p : this.points)
1733 {
1734 s.append(p.x + "\t" + p.y + "\n");
1735 }
1736 return s.toString();
1737 }
1738
1739
1740
1741
1742 public final String toPlot()
1743 {
1744 StringBuffer result = new StringBuffer();
1745 for (OTSPoint3D p : this.points)
1746 {
1747 result.append(String.format(Locale.US, "%s%.3f,%.3f", 0 == result.length() ? "M" : " L", p.x, p.y));
1748 }
1749 result.append("\n");
1750 return result.toString();
1751 }
1752
1753
1754
1755
1756
1757 public static void main(final String[] args) throws OTSGeometryException
1758 {
1759
1760
1761
1762
1763
1764
1765
1766
1767
1768
1769
1770 List<OTSPoint3D> list = new ArrayList<>();
1771 boolean laneOn933 = true;
1772 if (!laneOn933)
1773 {
1774 double x = 0;
1775 double y = 0;
1776 double dx = 0.000001;
1777 double dy = 0.05;
1778 double ddx = 1.5;
1779 for (int i = 0; i < 32; i++)
1780 {
1781 list.add(new OTSPoint3D(x, y));
1782 x += dx;
1783 dx *= ddx;
1784 y += dy;
1785 }
1786 }
1787 else
1788 {
1789 String lineStr = "@0 426333.939, 4581372.345@" + "1 426333.92109750526, 4581372.491581111@"
1790 + "2 426333.9016207722, 4581372.6364820665@" + "3 426333.8806181711, 4581372.7797264075@"
1791 + "4 426333.8581377007, 4581372.921337651@" + "5 426333.8342269785, 4581373.061339286@"
1792 + "6 426333.80893323367, 4581373.199754763@" + "7 426333.78230329906, 4581373.336607476@"
1793 + "8 426333.75438360614, 4581373.471920755@" + "9 426333.7252201801, 4581373.605717849@"
1794 + "10 426333.69485863775, 4581373.738021923@" + "11 426333.6633441839, 4581373.868856039@"
1795 + "12 426333.6307216125, 4581373.998243135@" + "13 426333.5970353065, 4581374.1262060385@"
1796 + "14 426333.56232923956, 4581374.252767426@" + "15 426333.54571270826, 4581374.331102062@"
1797 + "16 426333.53121128445, 4581374.399777128@" + "17 426333.51761287224, 4581374.46141805@"
1798 + "18 426333.5035609495, 4581374.524905452@" + "19 426333.4885681211, 4581374.590110448@"
1799 + "20 426333.4750534529, 4581374.648530791@" + "21 426333.4586325006, 4581374.71720738@"
1800 + "22 426333.44573716016, 4581374.770680802@" + "23 426333.4278589452, 4581374.84273674@"
1801 + "24 426333.41565935884, 4581374.891382747@" + "25 426333.39629928104, 4581374.966726161@"
1802 + "26 426333.3640042249, 4581375.089202983@" + "27 426333.3310233974, 4581375.210194213@"
1803 + "28 426333.2974053264, 4581375.329726505@" + "29 426333.26319745823, 4581375.44782613@"
1804 + "30 426333.2284461768, 4581375.564518943@" + "31 426333.1931968143, 4581375.679830365@"
1805 + "32 426333.15749366966, 4581375.793785359@" + "33 426333.12138002727, 4581375.9064084105@"
1806 + "34 426333.0848981781, 4581376.017723508@" + "35 426333.0526068902, 4581376.127395174@"
1807 + "36 426333.0222216131, 4581376.235573194@" + "37 426333.00835773064, 4581376.284013769@"
1808 + "38 426332.9916265083, 4581376.342442355@" + "39 426332.9771780217, 4581376.392075247@"
1809 + "40 426332.96085931134, 4581376.448026933@" + "41 426332.9448449097, 4581376.5021694945@"
1810 + "42 426332.9299564511, 4581376.552350422@" + "43 426332.9123899684, 4581376.610862428@"
1811 + "44 426332.87985284685, 4581376.718179138@" + "45 426332.8472718188, 4581376.824143872@"
1812 + "46 426332.81468381727, 4581376.92878003@" + "47 426332.78212446393, 4581377.032110168@"
1813 + "48 426332.7496281178, 4581377.134155947@" + "49 426332.71722788643, 4581377.234938197@"
1814 + "50 426332.68495568086, 4581377.3344768565@" + "51 426332.6528422234, 4581377.432791035@"
1815 + "52 426332.6209170973, 4581377.529898969@" + "53 426332.59026768577, 4581377.622609458@"
1816 + "54 426332.5618311538, 4581377.708242513@" + "55 426332.5292456913, 4581377.813700842@"
1817 + "56 426332.5007497582, 4581377.905735847@" + "57 426332.4725916431, 4581377.996633883@"
1818 + "58 426332.4447947076, 4581378.086409748@" + "59 426332.41739884845, 4581378.175020202@"
1819 + "60 426332.3904224847, 4581378.262486783@" + "61 426332.37513187295, 4581378.312218361@"
1820 + "62 426332.3474726438, 4581378.402429141@" + "63 426332.3203478011, 4581378.491354613@"
1821 + "64 426332.2937555201, 4581378.579078223@" + "65 426332.26771504263, 4581378.665610338@"
1822 + "66 426332.24224462465, 4581378.750960108@" + "67 426332.21736132156, 4581378.835136287@"
1823 + "68 426332.1930813682, 4581378.918146061@" + "69 426332.1694196611, 4581378.999996922@"
1824 + "70 426332.1468078785, 4581379.079234334@" + "71 426332.1253935003, 4581379.155326921@"
1825 + "72 426332.10456227185, 4581379.230438552@" + "73 426332.08413377195, 4581379.301777359@"
1826 + "74 426332.0575671712, 4581379.393246921@" + "75 426332.037751917, 4581379.463051603@"
1827 + "76 426332.01541074895, 4581379.543672992@" + "77 426331.9954696024, 4581379.617241848@"
1828 + "78 426331.9764488572, 4581379.689794578@" + "79 426331.9581173997, 4581379.761214821@"
1829 + "80 426331.9407607595, 4581379.831643043@" + "81 426331.92459788476, 4581379.898797621@"
1830 + "82 426331.89349001576, 4581380.036207511@" + "83 426331.8662295119, 4581380.167554456@"
1831 + "84 426331.84239882755, 4581380.294825263@" + "85 426331.8220095046, 4581380.41813201@"
1832 + "86 426331.80506772455, 4581380.537631294@" + "87 426331.79158302536, 4581380.653536015@"
1833 + "88 426331.78158027114, 4581380.766126917@" + "89 426331.7754554946, 4581380.838605414@"
1834 + "90 426331.76793314604, 4581380.909291444@" + "91 426331.7605002508, 4581381.016285149@"
1835 + "92 426331.75725734304, 4581381.119549306@" + "93 426331.75814653496, 4581381.219559045@"
1836 + "94 426331.76316353114, 4581381.316908372@" + "95 426331.7723867522, 4581381.412305131@"
1837 + "96 426331.7860053539, 4581381.506554079@" + "97 426331.80434182915, 4581381.600527881@"
1838 + "98 426331.82733581704, 4581381.692992337@" + "99 426331.8531803791, 4581381.777938947@"
1839 + "100 426331.884024255, 4581381.864352291@" + "101 426331.92063241004, 4581381.953224321@"
1840 + "102 426331.96390912175, 4581382.045434713@" + "103 426331.9901409878, 4581382.095566823@"
1841 + "104 426332.0148562894, 4581382.141714169@" + "105 426332.05172826024, 4581382.204388889@"
1842 + "106 426332.12722889386, 4581382.323121141@" + "107 426332.1628785428, 4581382.375872464@"
1843 + "108 426332.22007742553, 4581382.462661629@" + "109 426332.26023980865, 4581382.523784153@"
1844 + "110 426332.3033344728, 4581382.586422447@" + "111 426332.34946240357, 4581382.650580184@"
1845 + "112 426332.3987196004, 4581382.716255575@" + "113 426332.4511967281, 4581382.783441929@"
1846 + "114 426332.50697922776, 4581382.852128648@" + "115 426332.56614731904, 4581382.922301916@"
1847 + "116 426332.628776037, 4581382.993945288@" + "117 426332.6949354622, 4581383.067040358@"
1848 + "118 426332.76469110255, 4581383.141567508@" + "119 426332.8381037568, 4581383.217505949@"
1849 + "120 426332.91523022414, 4581383.294834619@" + "121 426332.9961233405, 4581383.373532268@"
1850 + "122 426333.0808322224, 4581383.453577724@" + "123 426333.1693585424, 4581383.534909724@"
1851 + "124 426333.26164044754, 4581383.61741792@" + "125 426333.3650128907, 4581383.707446191@";
1852 int fromIndex = 0;
1853 while (true)
1854 {
1855 int at1 = lineStr.indexOf('@', fromIndex);
1856 fromIndex = at1 + 1;
1857 int at2 = lineStr.indexOf('@', fromIndex);
1858 if (at2 < 0)
1859 {
1860 break;
1861 }
1862 fromIndex = at2;
1863
1864 String subStr = lineStr.substring(at1 + 5, at2);
1865 int comma = subStr.indexOf(',');
1866 double x = Double.valueOf(subStr.substring(0, comma));
1867 double y = Double.valueOf(subStr.substring(comma + 1));
1868
1869 list.add(new OTSPoint3D(x, y, 0.0));
1870
1871 }
1872 }
1873 OTSLine3D line = new OTSLine3D(list);
1874
1875 line.projectFractional(null, null, 1.0, 0.5, FractionalFallback.NaN);
1876
1877
1878 OTSPoint3D[] array = line.fractionalHelperCenters;
1879 for (int i = 0; i < array.length; i++)
1880 {
1881 if (array[i] == null)
1882 {
1883 array[i] = new OTSPoint3D(Double.NaN, Double.NaN);
1884 }
1885 }
1886 OTSLine3D helpers = new OTSLine3D(line.fractionalHelperCenters);
1887
1888
1889 StringBuilder str = new StringBuilder();
1890 str.append("line = [");
1891 String sep = "";
1892 for (OTSPoint3D p : line.getPoints())
1893 {
1894 str.append(String.format(Locale.US, "%s %.8f, %.8f", sep, p.x, p.y));
1895 sep = ",";
1896 }
1897 str.append("];\n");
1898
1899 str.append("helpers = [");
1900 sep = "";
1901 for (OTSPoint3D p : helpers.getPoints())
1902 {
1903 str.append(String.format(Locale.US, "%s %.8f, %.8f", sep, p.x, p.y));
1904 sep = ",";
1905 }
1906 str.append("];\n");
1907
1908 System.out.print(str);
1909 }
1910
1911 }