View Javadoc
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  import java.util.stream.Collectors;
13  import java.util.stream.DoubleStream;
14  
15  import javax.media.j3d.Bounds;
16  import javax.vecmath.Point3d;
17  
18  import org.djunits.unit.DirectionUnit;
19  import org.djunits.value.vdouble.scalar.Direction;
20  import org.djunits.value.vdouble.scalar.Length;
21  import org.djutils.exceptions.Throw;
22  import org.djutils.logger.CategoryLogger;
23  import org.locationtech.jts.geom.Coordinate;
24  import org.locationtech.jts.geom.CoordinateSequence;
25  import org.locationtech.jts.geom.Envelope;
26  import org.locationtech.jts.geom.Geometry;
27  import org.locationtech.jts.geom.GeometryFactory;
28  import org.locationtech.jts.geom.LineString;
29  import org.locationtech.jts.linearref.LengthIndexedLine;
30  
31  import nl.tudelft.simulation.dsol.animation.Locatable;
32  import nl.tudelft.simulation.language.d3.BoundingBox;
33  import nl.tudelft.simulation.language.d3.DirectedPoint;
34  
35  /**
36   * Line with OTSPoint3D points, a cached length indexed line, a cached length, and a cached centroid (all calculated on first
37   * use).
38   * <p>
39   * Copyright (c) 2013-2020 Delft University of Technology, PO Box 5, 2600 AA, Delft, the Netherlands. All rights reserved. <br>
40   * BSD-style license. See <a href="http://opentrafficsim.org/docs/license.html">OpenTrafficSim License</a>.
41   * <p>
42   * $LastChangedDate: 2015-07-16 10:20:53 +0200 (Thu, 16 Jul 2015) $, @version $Revision: 1124 $, by $Author: pknoppers $,
43   * initial version Jul 22, 2015 <br>
44   * @author <a href="http://www.tbm.tudelft.nl/averbraeck">Alexander Verbraeck</a>
45   * @author <a href="http://www.tudelft.nl/pknoppers">Peter Knoppers</a>
46   * @author <a href="http://www.citg.tudelft.nl">Guus Tamminga</a>
47   * @author <a href="http://www.transport.citg.tudelft.nl">Wouter Schakel</a>
48   */
49  public class OTSLine3D implements Locatable, Serializable
50  {
51      /** */
52      private static final long serialVersionUID = 20150722L;
53  
54      /** The points of the line. */
55      private OTSPoint3D[] points;
56  
57      /** The cumulative length of the line at point 'i'. */
58      private double[] lengthIndexedLine = null;
59  
60      /** The cached length; will be calculated at time of construction. */
61      private Length length;
62  
63      /** The cached centroid; will be calculated when needed for the first time. */
64      private OTSPoint3D centroid = null;
65  
66      /** The cached bounds; will be calculated when needed for the first time. */
67      private Bounds bounds = null;
68  
69      /** The cached helper points for fractional projection; will be calculated when needed for the first time. */
70      private OTSPoint3D[] fractionalHelperCenters = null;
71  
72      /** The cached helper directions for fractional projection; will be calculated when needed for the first time. */
73      private Point2D.Double[] fractionalHelperDirections = null;
74  
75      /** Intersection of unit offset lines of first two segments. */
76      private OTSPoint3D firstOffsetIntersection;
77  
78      /** Intersection of unit offset lines of last two segments. */
79      private OTSPoint3D lastOffsetIntersection;
80  
81      /** Precision for fractional projection algorithm. */
82      private static final double FRAC_PROJ_PRECISION = 2e-5 /* PK too fine 1e-6 */;
83  
84      /** Radius at each vertex. */
85      private Length[] vertexRadii;
86  
87      /** Bounding of this OTSLine3D. */
88      private Envelope envelope;
89  
90      /**
91       * Construct a new OTSLine3D.
92       * @param points OTSPoint3D...; the array of points to construct this OTSLine3D from.
93       * @throws OTSGeometryException when the provided points do not constitute a valid line (too few points or identical
94       *             adjacent points)
95       */
96      public OTSLine3D(final OTSPoint3D... points) throws OTSGeometryException
97      {
98          init(points);
99      }
100 
101     /**
102      * Construct a new OTSLine3D, and immediately make the length-indexed line.
103      * @param pts OTSPoint3D...; the array of points to construct this OTSLine3D from.
104      * @throws OTSGeometryException when the provided points do not constitute a valid line (too few points or identical
105      *             adjacent points)
106      */
107     private void init(final OTSPoint3D... pts) throws OTSGeometryException
108     {
109         if (pts.length < 2)
110         {
111             throw new OTSGeometryException("Degenerate OTSLine3D; has " + pts.length + " point" + (pts.length != 1 ? "s" : ""));
112         }
113         this.lengthIndexedLine = new double[pts.length];
114         this.lengthIndexedLine[0] = 0.0;
115         for (int i = 1; i < pts.length; i++)
116         {
117             if (pts[i - 1].x == pts[i].x && pts[i - 1].y == pts[i].y && pts[i - 1].z == pts[i].z)
118             {
119                 throw new OTSGeometryException(
120                         "Degenerate OTSLine3D; point " + (i - 1) + " has the same x, y and z as point " + i);
121             }
122             this.lengthIndexedLine[i] = this.lengthIndexedLine[i - 1] + pts[i - 1].distanceSI(pts[i]);
123         }
124         this.points = pts;
125         this.length = Length.instantiateSI(this.lengthIndexedLine[this.lengthIndexedLine.length - 1]);
126     }
127 
128     /** Which offsetLine method to use... */
129     public enum OffsetMethod
130     {
131         /** Via JTS buffer. */
132         JTS,
133 
134         /** Peter Knoppers. */
135         PK;
136     };
137 
138     /** Which offset line method to use... */
139     public static final OffsetMethod OFFSETMETHOD = OffsetMethod.PK;
140 
141     /**
142      * Construct parallel line.<br>
143      * TODO Let the Z-component of the result follow the Z-values of the reference line.
144      * @param offset double; offset distance from the reference line; positive is LEFT, negative is RIGHT
145      * @return OTSLine3D; the line that has the specified offset from the reference line
146      */
147     public final OTSLine3D offsetLine(final double offset)
148     {
149         try
150         {
151             switch (OFFSETMETHOD)
152             {
153                 case PK:
154                     return OTSOffsetLinePK.offsetLine(this, offset);
155 
156                 case JTS:
157                     return OTSBufferingJTS.offsetLine(this, offset);
158 
159                 default:
160                     return null;
161             }
162         }
163         catch (OTSGeometryException exception)
164         {
165             CategoryLogger.always().error(exception);
166             return null;
167         }
168     }
169 
170     /**
171      * Construct a line that is equal to this line except for segments that are shorter than the <cite>noiseLevel</cite>. The
172      * result is guaranteed to start with the first point of this line and end with the last point of this line.
173      * @param noiseLevel double; the minimum segment length that is <b>not</b> removed
174      * @return OTSLine3D; the filtered line
175      */
176     public final OTSLine3D noiseFilteredLine(final double noiseLevel)
177     {
178         if (this.size() <= 2)
179         {
180             return this; // Except for some cached fields; an OTSLine3D is immutable; so safe to return
181         }
182         OTSPoint3D prevPoint = null;
183         List<OTSPoint3D> list = null;
184         for (int index = 0; index < this.size(); index++)
185         {
186             OTSPoint3D currentPoint = this.points[index];
187             if (null != prevPoint && prevPoint.distanceSI(currentPoint) < noiseLevel)
188             {
189                 if (null == list)
190                 {
191                     // Found something to filter; copy this up to (and including) prevPoint
192                     list = new ArrayList<>();
193                     for (int i = 0; i < index; i++)
194                     {
195                         list.add(this.points[i]);
196                     }
197                 }
198                 if (index == this.size() - 1)
199                 {
200                     if (list.size() > 1)
201                     {
202                         // Replace the last point of the result by the last point of this OTSLine3D
203                         list.set(list.size() - 1, currentPoint);
204                     }
205                     else
206                     {
207                         // Append the last point of this even though it is close to the first point than the noise value to
208                         // comply with the requirement that first and last point of this are ALWAYS included in the result.
209                         list.add(currentPoint);
210                     }
211                 }
212                 continue; // Do not replace prevPoint by currentPoint
213             }
214             else if (null != list)
215             {
216                 list.add(currentPoint);
217             }
218             prevPoint = currentPoint;
219         }
220         if (null == list)
221         {
222             return this;
223         }
224         if (list.size() == 2 && list.get(0).equals(list.get(1)))
225         {
226             // CategoryLogger.always().debug("Fixing up degenerate noiseFilteredLine by inserting an intermediate point");
227             // Find something to insert along the way
228             for (int index = 1; index < this.size() - 1; index++)
229             {
230                 if (!this.points[index].equals(list.get(0)))
231                 {
232                     list.add(1, this.points[index]);
233                     break;
234                 }
235             }
236         }
237         try
238         {
239             return new OTSLine3D(list);
240         }
241         catch (OTSGeometryException exception)
242         {
243             CategoryLogger.always().error(exception);
244             throw new Error(exception);
245         }
246     }
247 
248     /**
249      * Clean up a list of points that describe a polyLine by removing points that lie within epsilon distance of a more
250      * straightened version of the line. <br>
251      * TODO Test this code (currently untested).
252      * @param epsilon double; maximal deviation
253      * @param useHorizontalDistance boolean; if true; the horizontal distance is used; if false; the 3D distance is used
254      * @return OTSLine3D; a new OTSLine3D containing all the remaining points
255      */
256     public final OTSLine3D noiseFilterRamerDouglasPeucker(final double epsilon, final boolean useHorizontalDistance)
257     {
258         try
259         {
260             // Apply the Ramer-Douglas-Peucker algorithm to the buffered points.
261             // Adapted from https://en.wikipedia.org/wiki/Ramer%E2%80%93Douglas%E2%80%93Peucker_algorithm
262             double maxDeviation = 0;
263             int splitIndex = -1;
264             int pointCount = size();
265             OTSLine3DOTSLine3D.html#OTSLine3D">OTSLine3D straight = new OTSLine3D(get(0), get(pointCount - 1));
266             // Find the point with largest deviation from the straight line from start point to end point
267             for (int i = 1; i < pointCount - 1; i++)
268             {
269                 OTSPoint3D point = get(i);
270                 OTSPoint3D closest =
271                         useHorizontalDistance ? point.closestPointOnLine2D(straight) : point.closestPointOnLine(straight);
272                 double deviation = useHorizontalDistance ? closest.horizontalDistanceSI(point) : closest.distanceSI(point);
273                 if (deviation > maxDeviation)
274                 {
275                     splitIndex = i;
276                     maxDeviation = deviation;
277                 }
278             }
279             if (maxDeviation <= epsilon)
280             {
281                 // All intermediate points can be dropped. Return a new list containing only the first and last point.
282                 return straight;
283             }
284             // The largest deviation is larger than epsilon.
285             // Split the polyLine at the point with the maximum deviation. Process each sub list recursively and concatenate the
286             // results
287             OTSLine3Dry/OTSLine3D.html#OTSLine3D">OTSLine3D first = new OTSLine3D(Arrays.copyOfRange(this.points, 0, splitIndex + 1))
288                     .noiseFilterRamerDouglasPeucker(epsilon, useHorizontalDistance);
289             OTSLine3Dy/OTSLine3D.html#OTSLine3D">OTSLine3D second = new OTSLine3D(Arrays.copyOfRange(this.points, splitIndex, this.points.length))
290                     .noiseFilterRamerDouglasPeucker(epsilon, useHorizontalDistance);
291             return concatenate(epsilon, first, second);
292         }
293         catch (OTSGeometryException exception)
294         {
295             CategoryLogger.always().error(exception); // Peter thinks this cannot happen ...
296             return null;
297         }
298     }
299 
300     /**
301      * Create a line at linearly varying offset from this line. The offset may change linearly from its initial value at the
302      * start of the reference line to its final offset value at the end of the reference line.
303      * @param offsetAtStart double; offset at the start of the reference line (positive value is Left, negative value is Right)
304      * @param offsetAtEnd double; offset at the end of the reference line (positive value is Left, negative value is Right)
305      * @return Geometry; the Geometry of the line at linearly changing offset of the reference line
306      * @throws OTSGeometryException when this method fails to create the offset line
307      */
308     public final OTSLine3D offsetLine(final double offsetAtStart, final double offsetAtEnd) throws OTSGeometryException
309     {
310         // CategoryLogger.trace(Cat.CORE, OTSGeometry.printCoordinates("#referenceLine: \nc1,0,0\n# offset at start is "
311         // + offsetAtStart + " at end is " + offsetAtEnd + "\n#", referenceLine, "\n "));
312 
313         OTSLine3D offsetLineAtStart = offsetLine(offsetAtStart);
314         if (offsetAtStart == offsetAtEnd)
315         {
316             return offsetLineAtStart; // offset does not change
317         }
318         // CategoryLogger.trace(Cat.CORE, OTSGeometry.printCoordinates("#offset line at start: \nc0,0,0\n#",
319         // offsetLineAtStart, "\n "));
320         OTSLine3D offsetLineAtEnd = offsetLine(offsetAtEnd);
321         // CategoryLogger.trace(Cat.CORE, OTSGeometry.printCoordinates("#offset line at end: \nc0.7,0.7,0.7\n#",
322         // offsetLineAtEnd, "\n "));
323         Geometry startGeometry = offsetLineAtStart.getLineString();
324         Geometry endGeometry = offsetLineAtEnd.getLineString();
325         LengthIndexedLine first = new LengthIndexedLine(startGeometry);
326         double firstLength = startGeometry.getLength();
327         LengthIndexedLine second = new LengthIndexedLine(endGeometry);
328         double secondLength = endGeometry.getLength();
329         ArrayList<Coordinate> out = new ArrayList<>();
330         Coordinate[] firstCoordinates = startGeometry.getCoordinates();
331         Coordinate[] secondCoordinates = endGeometry.getCoordinates();
332         int firstIndex = 0;
333         int secondIndex = 0;
334         Coordinate prevCoordinate = null;
335         final double tooClose = 0.05; // 5 cm
336         while (firstIndex < firstCoordinates.length && secondIndex < secondCoordinates.length)
337         {
338             double firstRatio = firstIndex < firstCoordinates.length ? first.indexOf(firstCoordinates[firstIndex]) / firstLength
339                     : Double.MAX_VALUE;
340             double secondRatio = secondIndex < secondCoordinates.length
341                     ? second.indexOf(secondCoordinates[secondIndex]) / secondLength : Double.MAX_VALUE;
342             double ratio;
343             if (firstRatio < secondRatio)
344             {
345                 ratio = firstRatio;
346                 firstIndex++;
347             }
348             else
349             {
350                 ratio = secondRatio;
351                 secondIndex++;
352             }
353             Coordinate firstCoordinate = first.extractPoint(ratio * firstLength);
354             Coordinate secondCoordinate = second.extractPoint(ratio * secondLength);
355             Coordinate resultCoordinate = new Coordinate((1 - ratio) * firstCoordinate.x + ratio * secondCoordinate.x,
356                     (1 - ratio) * firstCoordinate.y + ratio * secondCoordinate.y);
357             if (null == prevCoordinate || resultCoordinate.distance(prevCoordinate) > tooClose)
358             {
359                 out.add(resultCoordinate);
360                 prevCoordinate = resultCoordinate;
361             }
362         }
363         Coordinate[] resultCoordinates = new Coordinate[out.size()];
364         for (int index = 0; index < out.size(); index++)
365         {
366             resultCoordinates[index] = out.get(index);
367         }
368         return new OTSLine3D(resultCoordinates);
369     }
370 
371     /**
372      * Create a line at linearly varying offset from this line. The offset may change linearly from its initial value at the
373      * start of the reference line via a number of intermediate offsets at intermediate positions to its final offset value at
374      * the end of the reference line.
375      * @param relativeFractions double[]; positional fractions for which the offsets have to be generated
376      * @param offsets double[]; offsets at the relative positions (positive value is Left, negative value is Right)
377      * @return Geometry; the Geometry of the line at linearly changing offset of the reference line
378      * @throws OTSGeometryException when this method fails to create the offset line
379      */
380     public final OTSLine3D offsetLine(final double[] relativeFractions, final double[] offsets) throws OTSGeometryException
381     {
382         Throw.whenNull(relativeFractions, "relativeFraction may not be null");
383         Throw.whenNull(offsets, "offsets may not be null");
384         Throw.when(relativeFractions.length < 2, OTSGeometryException.class, "size of relativeFractions must be >= 2");
385         Throw.when(relativeFractions.length != offsets.length, OTSGeometryException.class,
386                 "size of relativeFractions must be equal to size of offsets");
387         Throw.when(relativeFractions[0] < 0, OTSGeometryException.class, "relativeFractions may not start before 0");
388         Throw.when(relativeFractions[relativeFractions.length - 1] > 1, OTSGeometryException.class,
389                 "relativeFractions may not end beyond 1");
390         List<Double> fractionsList = DoubleStream.of(relativeFractions).boxed().collect(Collectors.toList());
391         List<Double> offsetsList = DoubleStream.of(offsets).boxed().collect(Collectors.toList());
392         if (relativeFractions[0] != 0)
393         {
394             fractionsList.add(0, 0.0);
395             offsetsList.add(0, 0.0);
396         }
397         if (relativeFractions[relativeFractions.length - 1] < 1.0)
398         {
399             fractionsList.add(1.0);
400             offsetsList.add(0.0);
401         }
402         OTSLine3Dine3D.html#OTSLine3D">OTSLine3D[] offsetLine = new OTSLine3D[fractionsList.size()];
403         for (int i = 0; i < fractionsList.size(); i++)
404         {
405             offsetLine[i] = offsetLine(offsetsList.get(i));
406             System.out.println("# offset is " + offsetsList.get(i));
407             System.out.println(offsetLine[i].toPlot());
408         }
409         List<Coordinate> out = new ArrayList<>();
410         Coordinate prevCoordinate = null;
411         final double tooClose = 0.05; // 5 cm
412         // TODO make tooClose a parameter of this method.
413         for (int i = 0; i < offsetsList.size() - 1; i++)
414         {
415             Throw.when(fractionsList.get(i + 1) <= fractionsList.get(i), OTSGeometryException.class,
416                     "fractions must be in ascending order");
417             Geometry startGeometry =
418                     offsetLine[i].extractFractional(fractionsList.get(i), fractionsList.get(i + 1)).getLineString();
419             Geometry endGeometry =
420                     offsetLine[i + 1].extractFractional(fractionsList.get(i), fractionsList.get(i + 1)).getLineString();
421             LengthIndexedLine first = new LengthIndexedLine(startGeometry);
422             double firstLength = startGeometry.getLength();
423             LengthIndexedLine second = new LengthIndexedLine(endGeometry);
424             double secondLength = endGeometry.getLength();
425             Coordinate[] firstCoordinates = startGeometry.getCoordinates();
426             Coordinate[] secondCoordinates = endGeometry.getCoordinates();
427             int firstIndex = 0;
428             int secondIndex = 0;
429             while (firstIndex < firstCoordinates.length && secondIndex < secondCoordinates.length)
430             {
431                 double firstRatio = firstIndex < firstCoordinates.length
432                         ? first.indexOf(firstCoordinates[firstIndex]) / firstLength : Double.MAX_VALUE;
433                 double secondRatio = secondIndex < secondCoordinates.length
434                         ? second.indexOf(secondCoordinates[secondIndex]) / secondLength : Double.MAX_VALUE;
435                 double ratio;
436                 if (firstRatio < secondRatio)
437                 {
438                     ratio = firstRatio;
439                     firstIndex++;
440                 }
441                 else
442                 {
443                     ratio = secondRatio;
444                     secondIndex++;
445                 }
446                 Coordinate firstCoordinate = first.extractPoint(ratio * firstLength);
447                 Coordinate secondCoordinate = second.extractPoint(ratio * secondLength);
448                 Coordinate resultCoordinate = new Coordinate((1 - ratio) * firstCoordinate.x + ratio * secondCoordinate.x,
449                         (1 - ratio) * firstCoordinate.y + ratio * secondCoordinate.y);
450                 if (null == prevCoordinate || resultCoordinate.distance(prevCoordinate) > tooClose)
451                 {
452                     out.add(resultCoordinate);
453                     prevCoordinate = resultCoordinate;
454                 }
455             }
456         }
457         Coordinate[] resultCoordinates = new Coordinate[out.size()];
458         for (int index = 0; index < out.size(); index++)
459         {
460             resultCoordinates[index] = out.get(index);
461         }
462         return new OTSLine3D(resultCoordinates);
463     }
464 
465     /**
466      * Concatenate several OTSLine3D instances.
467      * @param lines OTSLine3D...; OTSLine3D... one or more OTSLine3D. The last point of the first
468      *            &lt;strong&gt;must&lt;/strong&gt; match the first of the second, etc.
469      * @return OTSLine3D
470      * @throws OTSGeometryException if zero lines are given, or when there is a gap between consecutive lines
471      */
472     public static OTSLine3D concatenate(final OTSLine3D... lines) throws OTSGeometryException
473     {
474         return concatenate(0.0, lines);
475     }
476 
477     /**
478      * Concatenate two OTSLine3D instances. This method is separate for efficiency reasons.
479      * @param toleranceSI double; the tolerance between the end point of a line and the first point of the next line
480      * @param line1 OTSLine3D; first line
481      * @param line2 OTSLine3D; second line
482      * @return OTSLine3D
483      * @throws OTSGeometryException if zero lines are given, or when there is a gap between consecutive lines
484      */
485     public static OTSLine3D concatenate(finalOTSLine3Dy/OTSLine3D.html#OTSLine3D">OTSLine3Dclass="jxr_keyword">double toleranceSI, final OTSLine3Dy/OTSLine3D.html#OTSLine3D">OTSLine3D line1, final OTSLine3D line2)
486             throws OTSGeometryException
487     {
488         if (line1.getLast().distance(line2.getFirst()).si > toleranceSI)
489         {
490             throw new OTSGeometryException("Lines are not connected: " + line1.getLast() + " to " + line2.getFirst()
491                     + " distance is " + line1.getLast().distance(line2.getFirst()).si + " > " + toleranceSI);
492         }
493         int size = line1.size() + line2.size() - 1;
494         OTSPoint3DTSPoint3D.html#OTSPoint3D">OTSPoint3D[] points = new OTSPoint3D[size];
495         int nextIndex = 0;
496         for (int j = 0; j < line1.size(); j++)
497         {
498             points[nextIndex++] = line1.get(j);
499         }
500         for (int j = 1; j < line2.size(); j++)
501         {
502             points[nextIndex++] = line2.get(j);
503         }
504         return new OTSLine3D(points);
505     }
506 
507     /**
508      * Concatenate several OTSLine3D instances.
509      * @param toleranceSI double; the tolerance between the end point of a line and the first point of the next line
510      * @param lines OTSLine3D...; OTSLine3D... one or more OTSLine3D. The last point of the first
511      *            &lt;strong&gt;must&lt;/strong&gt; match the first of the second, etc.
512      * @return OTSLine3D
513      * @throws OTSGeometryException if zero lines are given, or when there is a gap between consecutive lines
514      */
515     public static OTSLine3D concatenate(final double toleranceSI, final OTSLine3D... lines) throws OTSGeometryException
516     {
517         // CategoryLogger.trace(Cat.CORE, "Concatenating " + lines.length + " lines.");
518         if (0 == lines.length)
519         {
520             throw new OTSGeometryException("Empty argument list");
521         }
522         else if (1 == lines.length)
523         {
524             return lines[0];
525         }
526         int size = lines[0].size();
527         for (int i = 1; i < lines.length; i++)
528         {
529             if (lines[i - 1].getLast().distance(lines[i].getFirst()).si > toleranceSI)
530             {
531                 throw new OTSGeometryException(
532                         "Lines are not connected: " + lines[i - 1].getLast() + " to " + lines[i].getFirst() + " distance is "
533                                 + lines[i - 1].getLast().distance(lines[i].getFirst()).si + " > " + toleranceSI);
534             }
535             size += lines[i].size() - 1;
536         }
537         OTSPoint3DTSPoint3D.html#OTSPoint3D">OTSPoint3D[] points = new OTSPoint3D[size];
538         int nextIndex = 0;
539         for (int i = 0; i < lines.length; i++)
540         {
541             OTSLine3D line = lines[i];
542             for (int j = 0 == i ? 0 : 1; j < line.size(); j++)
543             {
544                 points[nextIndex++] = line.get(j);
545             }
546         }
547         return new OTSLine3D(points);
548     }
549 
550     /**
551      * Construct a new OTSLine3D with all points of this OTSLine3D in reverse order.
552      * @return OTSLine3D; the new OTSLine3D
553      */
554     public final OTSLine3D reverse()
555     {
556         OTSPoint3Dt3D.html#OTSPoint3D">OTSPoint3D[] resultPoints = new OTSPoint3D[size()];
557         int nextIndex = size();
558         for (OTSPoint3D p : getPoints())
559         {
560             resultPoints[--nextIndex] = p;
561         }
562         try
563         {
564             return new OTSLine3D(resultPoints);
565         }
566         catch (OTSGeometryException exception)
567         {
568             // Cannot happen
569             throw new RuntimeException(exception);
570         }
571     }
572 
573     /**
574      * Construct a new OTSLine3D covering the indicated fraction of this OTSLine3D.
575      * @param start double; starting point, valid range [0..<cite>end</cite>)
576      * @param end double; ending point, valid range (<cite>start</cite>..1]
577      * @return OTSLine3D; the new OTSLine3D
578      * @throws OTSGeometryException when start &gt;= end, or start &lt; 0, or end &gt; 1
579      */
580     public final OTSLine3D extractFractional(final double start, final double end) throws OTSGeometryException
581     {
582         if (start < 0 || start >= end || end > 1)
583         {
584             throw new OTSGeometryException(
585                     "Bad interval (start=" + start + ", end=" + end + ", this is " + this.toString() + ")");
586         }
587         return extract(start * this.length.si, end * this.length.si);
588     }
589 
590     /**
591      * Create a new OTSLine3D that covers a sub-section of this OTSLine3D.
592      * @param start Length; the length along this OTSLine3D where the sub-section starts, valid range [0..<cite>end</cite>)
593      * @param end Length; length along this OTSLine3D where the sub-section ends, valid range
594      *            (<cite>start</cite>..<cite>length</cite> (length is the length of this OTSLine3D)
595      * @return OTSLine3D; the selected sub-section
596      * @throws OTSGeometryException when start &gt;= end, or start &lt; 0, or end &gt; length
597      */
598     public final OTSLine3D extract(final Length start, final Length end) throws OTSGeometryException
599     {
600         return extract(start.si, end.si);
601     }
602 
603     /**
604      * Create a new OTSLine3D that covers a sub-section of this OTSLine3D.
605      * @param start double; length along this OTSLine3D where the sub-section starts, valid range [0..<cite>end</cite>)
606      * @param end double; length along this OTSLine3D where the sub-section ends, valid range
607      *            (<cite>start</cite>..<cite>length</cite> (length is the length of this OTSLine3D)
608      * @return OTSLine3D; the selected sub-section
609      * @throws OTSGeometryException when start &gt;= end, or start &lt; 0, or end &gt; length
610      */
611     public final OTSLine3D extract(final double start, final double end) throws OTSGeometryException
612     {
613         if (Double.isNaN(start) || Double.isNaN(end) || start < 0 || start >= end || end > getLengthSI())
614         {
615             throw new OTSGeometryException(
616                     "Bad interval (" + start + ".." + end + "; length of this OTSLine3D is " + this.getLengthSI() + ")");
617         }
618         double cumulativeLength = 0;
619         double nextCumulativeLength = 0;
620         double segmentLength = 0;
621         int index = 0;
622         List<OTSPoint3D> pointList = new ArrayList<>();
623         // CategoryLogger.trace(Cat.CORE, "interval " + start + ".." + end);
624         while (start > cumulativeLength)
625         {
626             OTSPoint3D fromPoint = this.points[index];
627             index++;
628             OTSPoint3D toPoint = this.points[index];
629             segmentLength = fromPoint.distanceSI(toPoint);
630             cumulativeLength = nextCumulativeLength;
631             nextCumulativeLength = cumulativeLength + segmentLength;
632             if (nextCumulativeLength >= start)
633             {
634                 break;
635             }
636         }
637         if (start == nextCumulativeLength)
638         {
639             pointList.add(this.points[index]);
640         }
641         else
642         {
643             pointList.add(OTSPoint3D.interpolate((start - cumulativeLength) / segmentLength, this.points[index - 1],
644                     this.points[index]));
645             if (end > nextCumulativeLength)
646             {
647                 pointList.add(this.points[index]);
648             }
649         }
650         while (end > nextCumulativeLength)
651         {
652             OTSPoint3D fromPoint = this.points[index];
653             index++;
654             if (index >= this.points.length)
655             {
656                 break; // rounding error
657             }
658             OTSPoint3D toPoint = this.points[index];
659             segmentLength = fromPoint.distanceSI(toPoint);
660             cumulativeLength = nextCumulativeLength;
661             nextCumulativeLength = cumulativeLength + segmentLength;
662             if (nextCumulativeLength >= end)
663             {
664                 break;
665             }
666             pointList.add(toPoint);
667         }
668         if (end == nextCumulativeLength)
669         {
670             pointList.add(this.points[index]);
671         }
672         else
673         {
674             OTSPoint3D point = OTSPoint3D.interpolate((end - cumulativeLength) / segmentLength, this.points[index - 1],
675                     this.points[index]);
676             // can be the same due to rounding
677             if (!point.equals(pointList.get(pointList.size() - 1)))
678             {
679                 pointList.add(point);
680             }
681         }
682         try
683         {
684             return new OTSLine3D(pointList);
685         }
686         catch (OTSGeometryException exception)
687         {
688             CategoryLogger.always().error(exception, "interval " + start + ".." + end + " too short");
689             throw new OTSGeometryException("interval " + start + ".." + end + "too short");
690         }
691     }
692 
693     /**
694      * Build an array of OTSPoint3D from an array of Coordinate.
695      * @param coordinates Coordinate[]; the coordinates
696      * @return OTSPoint3D[]
697      */
698     private static OTSPoint3D[] coordinatesToOTSPoint3D(final Coordinate[] coordinates)
699     {
700         OTSPoint3DTSPoint3D.html#OTSPoint3D">OTSPoint3D[] result = new OTSPoint3D[coordinates.length];
701         for (int i = 0; i < coordinates.length; i++)
702         {
703             result[i] = new OTSPoint3D(coordinates[i]);
704         }
705         return result;
706     }
707 
708     /**
709      * Create an OTSLine3D, while cleaning repeating successive points.
710      * @param points OTSPoint3D...; the coordinates of the line as OTSPoint3D
711      * @return the line
712      * @throws OTSGeometryException when number of points &lt; 2
713      */
714     public static OTSLine3D createAndCleanOTSLine3D(final OTSPoint3D... points) throws OTSGeometryException
715     {
716         if (points.length < 2)
717         {
718             throw new OTSGeometryException(
719                     "Degenerate OTSLine3D; has " + points.length + " point" + (points.length != 1 ? "s" : ""));
720         }
721         return createAndCleanOTSLine3D(new ArrayList<>(Arrays.asList(points)));
722     }
723 
724     /**
725      * Create an OTSLine3D, while cleaning repeating successive points.
726      * @param pointList List&lt;OTSPoint3D&gt;; list of the coordinates of the line as OTSPoint3D; any duplicate points in this
727      *            list are removed (this method may modify the provided list)
728      * @return OTSLine3D; the line
729      * @throws OTSGeometryException when number of non-equal points &lt; 2
730      */
731     public static OTSLine3D createAndCleanOTSLine3D(final List<OTSPoint3D> pointList) throws OTSGeometryException
732     {
733         // clean successive equal points
734         int i = 1;
735         while (i < pointList.size())
736         {
737             if (pointList.get(i - 1).equals(pointList.get(i)))
738             {
739                 pointList.remove(i);
740             }
741             else
742             {
743                 i++;
744             }
745         }
746         return new OTSLine3D(pointList);
747     }
748 
749     /**
750      * Construct a new OTSLine3D from an array of Coordinate.
751      * @param coordinates Coordinate[]; the array of coordinates to construct this OTSLine3D from
752      * @throws OTSGeometryException when the provided points do not constitute a valid line (too few points or identical
753      *             adjacent points)
754      */
755     public OTSLine3D(final Coordinate[] coordinates) throws OTSGeometryException
756     {
757         this(coordinatesToOTSPoint3D(coordinates));
758     }
759 
760     /**
761      * Construct a new OTSLine3D from a LineString.
762      * @param lineString LineString; the lineString to construct this OTSLine3D from.
763      * @throws OTSGeometryException when the provided LineString does not constitute a valid line (too few points or identical
764      *             adjacent points)
765      */
766     public OTSLine3D(final LineString lineString) throws OTSGeometryException
767     {
768         this(lineString.getCoordinates());
769     }
770 
771     /**
772      * Construct a new OTSLine3D from a Geometry.
773      * @param geometry Geometry; the geometry to construct this OTSLine3D from
774      * @throws OTSGeometryException when the provided Geometry do not constitute a valid line (too few points or identical
775      *             adjacent points)
776      */
777     public OTSLine3D(final Geometry geometry) throws OTSGeometryException
778     {
779         this(geometry.getCoordinates());
780     }
781 
782     /**
783      * Construct a new OTSLine3D from a List&lt;OTSPoint3D&gt;.
784      * @param pointList List&lt;OTSPoint3D&gt;; the list of points to construct this OTSLine3D from.
785      * @throws OTSGeometryException when the provided points do not constitute a valid line (too few points or identical
786      *             adjacent points)
787      */
788     public OTSLine3D(final List<OTSPoint3D> pointList) throws OTSGeometryException
789     {
790         this(pointList.toArray(new OTSPoint3D[pointList.size()]));
791     }
792 
793     /**
794      * Construct a new OTSShape (closed shape) from a Path2D.
795      * @param path Path2D; the Path2D to construct this OTSLine3D from.
796      * @throws OTSGeometryException when the provided points do not constitute a valid line (too few points or identical
797      *             adjacent points)
798      */
799     public OTSLine3D(final Path2D path) throws OTSGeometryException
800     {
801         List<OTSPoint3D> pl = new ArrayList<>();
802         for (PathIterator pi = path.getPathIterator(null); !pi.isDone(); pi.next())
803         {
804             double[] p = new double[6];
805             int segType = pi.currentSegment(p);
806             if (segType == PathIterator.SEG_MOVETO || segType == PathIterator.SEG_LINETO)
807             {
808                 pl.add(new OTSPoint3D(p[0], p[1]));
809             }
810             else if (segType == PathIterator.SEG_CLOSE)
811             {
812                 if (!pl.get(0).equals(pl.get(pl.size() - 1)))
813                 {
814                     pl.add(new OTSPoint3D(pl.get(0).x, pl.get(0).y));
815                 }
816                 break;
817             }
818         }
819         init(pl.toArray(new OTSPoint3D[pl.size() - 1]));
820     }
821 
822     /**
823      * Construct a Coordinate array and fill it with the points of this OTSLine3D.
824      * @return an array of Coordinates corresponding to this OTSLine
825      */
826     public final Coordinate[] getCoordinates()
827     {
828         Coordinate[] result = new Coordinate[size()];
829         for (int i = 0; i < size(); i++)
830         {
831             result[i] = this.points[i].getCoordinate();
832         }
833         return result;
834     }
835 
836     /**
837      * Construct a LineString from this OTSLine3D.
838      * @return a LineString corresponding to this OTSLine
839      */
840     public final LineString getLineString()
841     {
842         GeometryFactory factory = new GeometryFactory();
843         Coordinate[] coordinates = getCoordinates();
844         CoordinateSequence cs = factory.getCoordinateSequenceFactory().create(coordinates);
845         return new LineString(cs, factory);
846     }
847 
848     /**
849      * Return the number of points in this OTSLine3D.
850      * @return the number of points on the line
851      */
852     public final int size()
853     {
854         return this.points.length;
855     }
856 
857     /**
858      * Return the first point of this OTSLine3D.
859      * @return the first point on the line
860      */
861     public final OTSPoint3D getFirst()
862     {
863         return this.points[0];
864     }
865 
866     /**
867      * Return the last point of this OTSLine3D.
868      * @return the last point on the line
869      */
870     public final OTSPoint3D getLast()
871     {
872         return this.points[size() - 1];
873     }
874 
875     /**
876      * Return one point of this OTSLine3D.
877      * @param i int; the index of the point to retrieve
878      * @return OTSPoint3d; the i-th point of the line
879      * @throws OTSGeometryException when i &lt; 0 or i &gt; the number of points
880      */
881     public final OTSPoint3D get(final int i) throws OTSGeometryException
882     {
883         if (i < 0 || i > size() - 1)
884         {
885             throw new OTSGeometryException("OTSLine3D.get(i=" + i + "); i<0 or i>=size(), which is " + size());
886         }
887         return this.points[i];
888     }
889 
890     /**
891      * Return the length of this OTSLine3D as a double value in SI units. (Assumes that the coordinates of the points
892      * constituting this line are expressed in meters.)
893      * @return the length of the line in SI units
894      */
895     public final double getLengthSI()
896     {
897         return this.length.si;
898     }
899 
900     /**
901      * Return the length of this OTSLine3D in meters. (Assuming that the coordinates of the points constituting this line are
902      * expressed in meters.)
903      * @return the length of the line
904      */
905     public final Length getLength()
906     {
907         return this.length;
908     }
909 
910     /**
911      * Return an array of OTSPoint3D that represents this OTSLine3D. <strong>Do not modify the result.</strong>
912      * @return the points of this line
913      */
914     public final OTSPoint3D[] getPoints()
915     {
916         return this.points;
917     }
918 
919     /**
920      * Make the length indexed line if it does not exist yet, and cache it.
921      */
922     private synchronized void makeLengthIndexedLine()
923     {
924         if (this.lengthIndexedLine == null)
925         {
926             this.lengthIndexedLine = new double[this.points.length];
927             this.lengthIndexedLine[0] = 0.0;
928             for (int i = 1; i < this.points.length; i++)
929             {
930                 this.lengthIndexedLine[i] = this.lengthIndexedLine[i - 1] + this.points[i - 1].distanceSI(this.points[i]);
931             }
932         }
933     }
934 
935     /**
936      * Get the location at a position on the line, with its direction. Position can be below 0 or more than the line length. In
937      * that case, the position will be extrapolated in the direction of the line at its start or end.
938      * @param position Length; the position on the line for which to calculate the point on, before, of after the line
939      * @return a directed point
940      */
941     public final DirectedPoint getLocationExtended(final Length position)
942     {
943         return getLocationExtendedSI(position.getSI());
944     }
945 
946     /**
947      * Get the location at a position on the line, with its direction. Position can be below 0 or more than the line length. In
948      * that case, the position will be extrapolated in the direction of the line at its start or end.
949      * @param positionSI double; the position on the line for which to calculate the point on, before, of after the line, in SI
950      *            units
951      * @return a directed point
952      */
953     public final synchronized DirectedPoint getLocationExtendedSI(final double positionSI)
954     {
955         makeLengthIndexedLine();
956         if (positionSI >= 0.0 && positionSI <= getLengthSI())
957         {
958             try
959             {
960                 return getLocationSI(positionSI);
961             }
962             catch (OTSGeometryException exception)
963             {
964                 // cannot happen
965             }
966         }
967 
968         // position before start point -- extrapolate
969         if (positionSI < 0.0)
970         {
971             double len = positionSI;
972             double fraction = len / (this.lengthIndexedLine[1] - this.lengthIndexedLine[0]);
973             OTSPoint3D p1 = this.points[0];
974             OTSPoint3D p2 = this.points[1];
975             return new DirectedPoint(p1.x + fraction * (p2.x - p1.x), p1.y + fraction * (p2.y - p1.y),
976                     p1.z + fraction * (p2.z - p1.z), 0.0, 0.0, Math.atan2(p2.y - p1.y, p2.x - p1.x));
977         }
978 
979         // position beyond end point -- extrapolate
980         int n1 = this.lengthIndexedLine.length - 1;
981         int n2 = this.lengthIndexedLine.length - 2;
982         double len = positionSI - getLengthSI();
983         double fraction = len / (this.lengthIndexedLine[n1] - this.lengthIndexedLine[n2]);
984         OTSPoint3D p1 = this.points[n2];
985         OTSPoint3D p2 = this.points[n1];
986         return new DirectedPoint(p2.x + fraction * (p2.x - p1.x), p2.y + fraction * (p2.y - p1.y),
987                 p2.z + fraction * (p2.z - p1.z), 0.0, 0.0, Math.atan2(p2.y - p1.y, p2.x - p1.x));
988     }
989 
990     /**
991      * Get the location at a fraction of the line, with its direction. Fraction should be between 0.0 and 1.0.
992      * @param fraction double; the fraction for which to calculate the point on the line
993      * @return a directed point
994      * @throws OTSGeometryException when fraction less than 0.0 or more than 1.0.
995      */
996     public final DirectedPoint getLocationFraction(final double fraction) throws OTSGeometryException
997     {
998         if (fraction < 0.0 || fraction > 1.0)
999         {
1000             throw new OTSGeometryException("getLocationFraction for line: fraction < 0.0 or > 1.0. fraction = " + fraction);
1001         }
1002         return getLocationSI(fraction * getLengthSI());
1003     }
1004 
1005     /**
1006      * Get the location at a fraction of the line, with its direction. Fraction should be between 0.0 and 1.0.
1007      * @param fraction double; the fraction for which to calculate the point on the line
1008      * @param tolerance double; the delta from 0.0 and 1.0 that will be forgiven
1009      * @return a directed point
1010      * @throws OTSGeometryException when fraction less than 0.0 or more than 1.0.
1011      */
1012     public final DirectedPoint getLocationFraction(final double fraction, final double tolerance) throws OTSGeometryException
1013     {
1014         if (fraction < -tolerance || fraction > 1.0 + tolerance)
1015         {
1016             throw new OTSGeometryException(
1017                     "getLocationFraction for line: fraction < 0.0 - tolerance or > 1.0 + tolerance; fraction = " + fraction);
1018         }
1019         double f = fraction < 0 ? 0.0 : fraction > 1.0 ? 1.0 : fraction;
1020         return getLocationSI(f * getLengthSI());
1021     }
1022 
1023     /**
1024      * Get the location at a fraction of the line (or outside the line), with its direction.
1025      * @param fraction double; the fraction for which to calculate the point on the line
1026      * @return a directed point
1027      */
1028     public final DirectedPoint getLocationFractionExtended(final double fraction)
1029     {
1030         return getLocationExtendedSI(fraction * getLengthSI());
1031     }
1032 
1033     /**
1034      * Get the location at a position on the line, with its direction. Position should be between 0.0 and line length.
1035      * @param position Length; the position on the line for which to calculate the point on the line
1036      * @return a directed point
1037      * @throws OTSGeometryException when position less than 0.0 or more than line length.
1038      */
1039     public final DirectedPoint getLocation(final Length position) throws OTSGeometryException
1040     {
1041         return getLocationSI(position.getSI());
1042     }
1043 
1044     /**
1045      * Binary search for a position on the line.
1046      * @param pos double; the position to look for.
1047      * @return the index below the position; the position is between points[index] and points[index+1]
1048      * @throws OTSGeometryException when index could not be found
1049      */
1050     private int find(final double pos) throws OTSGeometryException
1051     {
1052         if (pos == 0)
1053         {
1054             return 0;
1055         }
1056 
1057         int lo = 0;
1058         int hi = this.lengthIndexedLine.length - 1;
1059         while (lo <= hi)
1060         {
1061             if (hi == lo)
1062             {
1063                 return lo;
1064             }
1065             int mid = lo + (hi - lo) / 2;
1066             if (pos < this.lengthIndexedLine[mid])
1067             {
1068                 hi = mid - 1;
1069             }
1070             else if (pos > this.lengthIndexedLine[mid + 1])
1071             {
1072                 lo = mid + 1;
1073             }
1074             else
1075             {
1076                 return mid;
1077             }
1078         }
1079         throw new OTSGeometryException(
1080                 "Could not find position " + pos + " on line with length indexes: " + Arrays.toString(this.lengthIndexedLine));
1081     }
1082 
1083     /**
1084      * Get the location at a position on the line, with its direction. Position should be between 0.0 and line length.
1085      * @param positionSI double; the position on the line for which to calculate the point on the line
1086      * @return a directed point
1087      * @throws OTSGeometryException when position less than 0.0 or more than line length.
1088      */
1089     public final synchronized DirectedPoint getLocationSI(final double positionSI) throws OTSGeometryException
1090     {
1091         makeLengthIndexedLine();
1092         if (positionSI < 0.0 || positionSI > getLengthSI())
1093         {
1094             throw new OTSGeometryException("getLocationSI for line: position < 0.0 or > line length. Position = " + positionSI
1095                     + " m. Length = " + getLengthSI() + " m.");
1096         }
1097 
1098         // handle special cases: position == 0.0, or position == length
1099         if (positionSI == 0.0)
1100         {
1101             OTSPoint3D p1 = this.points[0];
1102             OTSPoint3D p2 = this.points[1];
1103             return new DirectedPoint(p1.x, p1.y, p1.z, 0.0, 0.0, Math.atan2(p2.y - p1.y, p2.x - p1.x));
1104         }
1105         if (positionSI == getLengthSI())
1106         {
1107             OTSPoint3D p1 = this.points[this.points.length - 2];
1108             OTSPoint3D p2 = this.points[this.points.length - 1];
1109             return new DirectedPoint(p2.x, p2.y, p2.z, 0.0, 0.0, Math.atan2(p2.y - p1.y, p2.x - p1.x));
1110         }
1111 
1112         // find the index of the line segment, use binary search
1113         int index = find(positionSI);
1114         double remainder = positionSI - 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         return new DirectedPoint(p1.x + fraction * (p2.x - p1.x), p1.y + fraction * (p2.y - p1.y),
1119                 p1.z + fraction * (p2.z - p1.z), 0.0, 0.0, Math.atan2(p2.y - p1.y, p2.x - p1.x));
1120     }
1121 
1122     /**
1123      * Truncate a line at the given length (less than the length of the line, and larger than zero) and return a new line.
1124      * @param lengthSI double; the location where to truncate the line
1125      * @return a new OTSLine3D truncated at the exact position where line.getLength() == lengthSI
1126      * @throws OTSGeometryException when position less than 0.0 or more than line length.
1127      */
1128     public final synchronized OTSLine3D truncate(final double lengthSI) throws OTSGeometryException
1129     {
1130         makeLengthIndexedLine();
1131         if (lengthSI <= 0.0 || lengthSI > getLengthSI())
1132         {
1133             throw new OTSGeometryException("truncate for line: position <= 0.0 or > line length. Position = " + lengthSI
1134                     + " m. Length = " + getLengthSI() + " m.");
1135         }
1136 
1137         // handle special case: position == length
1138         if (lengthSI == getLengthSI())
1139         {
1140             return new OTSLine3D(getPoints());
1141         }
1142 
1143         // find the index of the line segment
1144         int index = find(lengthSI);
1145         double remainder = lengthSI - this.lengthIndexedLine[index];
1146         double fraction = remainder / (this.lengthIndexedLine[index + 1] - this.lengthIndexedLine[index]);
1147         OTSPoint3D p1 = this.points[index];
1148         OTSPoint3D lastPoint;
1149         if (0.0 == fraction)
1150         {
1151             index--;
1152             lastPoint = p1;
1153         }
1154         else
1155         {
1156             OTSPoint3D p2 = this.points[index + 1];
1157             lastPoint = new OTSPoint3D(p1.x + fraction * (p2.x - p1.x), p1.y + fraction * (p2.y - p1.y),
1158                     p1.z + fraction * (p2.z - p1.z));
1159 
1160         }
1161         OTSPoint3DTSPoint3D.html#OTSPoint3D">OTSPoint3D[] coords = new OTSPoint3D[index + 2];
1162         for (int i = 0; i <= index; i++)
1163         {
1164             coords[i] = this.points[i];
1165         }
1166         coords[index + 1] = lastPoint;
1167         return new OTSLine3D(coords);
1168     }
1169 
1170     /*-
1171      * TODO finish this method if it is needed; remove otherwise.
1172      * Calculate the first point on this line that intersects somewhere with the provided line, or NaN if no intersection was
1173      * found.
1174      * @param line the line to test the intersection with
1175      * @return the fraction of the first intersection point
1176      *
1177     public final double firstIntersectionFraction(final OTSLine3D line)
1178     {
1179         List<Line2D.Double> segs = new ArrayList<>();
1180         for (int j = 1; j < line.getPoints().length; j++)
1181         {
1182             Line2D.Double seg =
1183                 new Line2D.Double(this.points[j - 1].x, this.points[j - 1].y, this.points[j].x, this.points[j].y);
1184             segs.add(seg);
1185     
1186         }
1187         for (int i = 1; i < this.points.length; i++)
1188         {
1189             Line2D.Double thisSeg =
1190                 new Line2D.Double(this.points[i - 1].x, this.points[i - 1].y, this.points[i].x, this.points[i].y);
1191             for (Line2D.Double seg : segs)
1192             {
1193                 if (thisSeg.intersectsLine(seg))
1194                 {
1195                     // Point2D.Double intersectionPoint = thisSeg.
1196                     
1197                 }
1198             }
1199         }
1200         return Double.NaN;
1201     }
1202      */
1203 
1204     /**
1205      * Returns the fractional position along this line of the orthogonal projection of point (x, y) on this line. If the point
1206      * is not orthogonal to the closest line segment, the nearest point is selected.
1207      * @param x double; x-coordinate of point to project
1208      * @param y double; y-coordinate of point to project
1209      * @return fractional position along this line of the orthogonal projection on this line of a point
1210      */
1211     public final synchronized double projectOrthogonal(final double x, final double y)
1212     {
1213 
1214         // prepare
1215         makeLengthIndexedLine();
1216         double minDistance = Double.POSITIVE_INFINITY;
1217         double minSegmentFraction = 0;
1218         int minSegment = -1;
1219 
1220         // code based on Line2D.ptSegDistSq(...)
1221         for (int i = 0; i < size() - 1; i++)
1222         {
1223             double dx = this.points[i + 1].x - this.points[i].x;
1224             double dy = this.points[i + 1].y - this.points[i].y;
1225             // vector relative to (x(i), y(i))
1226             double px = x - this.points[i].x;
1227             double py = y - this.points[i].y;
1228             // dot product
1229             double dot1 = px * dx + py * dy;
1230             double f;
1231             double distance;
1232             if (dot1 > 0)
1233             {
1234                 // vector relative to (x(i+1), y(i+1))
1235                 px = dx - px;
1236                 py = dy - py;
1237                 // dot product
1238                 double dot2 = px * dx + py * dy;
1239                 if (dot2 > 0)
1240                 {
1241                     // projection on line segment
1242                     double len2 = dx * dx + dy * dy;
1243                     double proj = dot2 * dot2 / len2;
1244                     f = dot1 / len2;
1245                     distance = px * px + py * py - proj;
1246                 }
1247                 else
1248                 {
1249                     // dot<=0 projection 'after' line segment
1250                     f = 1;
1251                     distance = px * px + py * py;
1252                 }
1253             }
1254             else
1255             {
1256                 // dot<=0 projection 'before' line segment
1257                 f = 0;
1258                 distance = px * px + py * py;
1259             }
1260             // check if closer than previous
1261             if (distance < minDistance)
1262             {
1263                 minDistance = distance;
1264                 minSegmentFraction = f;
1265                 minSegment = i;
1266             }
1267         }
1268 
1269         // return
1270         double segLen = this.lengthIndexedLine[minSegment + 1] - this.lengthIndexedLine[minSegment];
1271         return (this.lengthIndexedLine[minSegment] + segLen * minSegmentFraction) / getLengthSI();
1272 
1273     }
1274 
1275     /**
1276      * Returns the fractional projection of a point to a line. The projection works by taking slices in space per line segment
1277      * as shown below. A point is always projected to the nearest segment, but not necessarily to the closest point on that
1278      * segment. The slices in space are analogous to a Voronoi diagram, but for the line segments instead of points. If
1279      * fractional projection fails, the orthogonal projection is returned.<br>
1280      * <br>
1281      * The point 'A' is projected to point 'B' on the 3rd segment of line 'C-D'. The line from 'A' to 'B' extends towards point
1282      * 'E', which is the intersection of lines 'E-F' and 'E-G'. Line 'E-F' cuts the first bend of the 3rd segment (at point 'H')
1283      * in half, while the line 'E-G' cuts the second bend of the 3rd segment (at point 'I') in half.
1284      * 
1285      * <pre>
1286      *            ____________________________     G                   .
1287      * .         |                            |    .                 .
1288      *   .       |  . . . .  helper lines     |    .               .
1289      *     .     |  _.._.._  projection line  |   I.             .
1290      *       .   |____________________________|  _.'._         .       L
1291      *        F.                              _.'  .  '-.    .
1292      *          ..                       B _.'     .     '-.
1293      *           . .                    _.\        .     .  D
1294      *            .  .               _.'   :       .   .
1295      *     J       .   .          _.'      \       . .
1296      *             ..    .     _.'          :      .                M
1297      *            .  .     ..-'             \      .
1298      *           .    .    /H.               A     .
1299      *          .      .  /    .                   .
1300      *        C _________/       .                 .
1301      *        .          .         .               .
1302      *   K   .            .          .             .
1303      *      .              .           .           .
1304      *     .                .            .         .           N
1305      *    .                  .             .       .
1306      *   .                    .              .     .
1307      *  .                      .               .   .
1308      * .                        .                . .
1309      *                           .                 .E
1310      *                            .                  .
1311      *                             .                   .
1312      *                              .                    .
1313      * </pre>
1314      * 
1315      * Fractional projection may fail in three cases.
1316      * <ol>
1317      * <li>Numerical difficulties at slight bend, orthogonal projection returns the correct point.</li>
1318      * <li>Fractional projection is possible only to segments that aren't the nearest segment(s).</li>
1319      * <li>Fractional projection is possible for no segment.</li>
1320      * </ol>
1321      * In the latter two cases the projection is undefined and a orthogonal projection is returned if
1322      * {@code orthoFallback = true}, or {@code NaN} if {@code orthoFallback = false}.
1323      * @param start Direction; direction in first point
1324      * @param end Direction; direction in last point
1325      * @param x double; x-coordinate of point to project
1326      * @param y double; y-coordinate of point to project
1327      * @param fallback FractionalFallback; fallback method for when fractional projection fails
1328      * @return fractional position along this line of the fractional projection on that line of a point
1329      */
1330     public final synchronized double projectFractional(final Direction start, final Direction end, final double x,
1331             final double y, final FractionalFallback fallback)
1332     {
1333 
1334         // prepare
1335         makeLengthIndexedLine();
1336         double minDistance = Double.POSITIVE_INFINITY;
1337         double minSegmentFraction = 0;
1338         int minSegment = -1;
1339         OTSPoint3Dy/OTSPoint3D.html#OTSPoint3D">OTSPoint3D point = new OTSPoint3D(x, y);
1340 
1341         // determine helpers (centers and directions)
1342         determineFractionalHelpers(start, end);
1343 
1344         // get distance of point to each segment
1345         double[] d = new double[this.points.length - 1];
1346         double minD = Double.POSITIVE_INFINITY;
1347         for (int i = 0; i < this.points.length - 1; i++)
1348         {
1349             d[i] = Line2D.ptSegDist(this.points[i].x, this.points[i].y, this.points[i + 1].x, this.points[i + 1].y, x, y);
1350             minD = d[i] < minD ? d[i] : minD;
1351         }
1352 
1353         // loop over segments for projection
1354         double distance;
1355         for (int i = 0; i < this.points.length - 1; i++)
1356         {
1357             // skip if not the closest segment, note that often two segments are equally close in their shared end point
1358             if (d[i] > minD + FRAC_PROJ_PRECISION)
1359             {
1360                 continue;
1361             }
1362             OTSPoint3D center = this.fractionalHelperCenters[i];
1363             OTSPoint3D p;
1364             if (center != null)
1365             {
1366                 // get intersection of line "center - (x, y)" and the segment
1367                 p = OTSPoint3D.intersectionOfLines(center, point, this.points[i], this.points[i + 1]);
1368                 if (p == null || (x < center.x + FRAC_PROJ_PRECISION && center.x + FRAC_PROJ_PRECISION < p.x)
1369                         || (x > center.x - FRAC_PROJ_PRECISION && center.x - FRAC_PROJ_PRECISION > p.x)
1370                         || (y < center.y + FRAC_PROJ_PRECISION && center.y + FRAC_PROJ_PRECISION < p.y)
1371                         || (y > center.y - FRAC_PROJ_PRECISION && center.y - FRAC_PROJ_PRECISION > p.y))
1372                 {
1373                     // projected point may not be 'beyond' segment center (i.e. center may not be between (x, y) and (p.x, p.y)
1374                     continue;
1375                 }
1376             }
1377             else
1378             {
1379                 // parallel helper lines, project along direction
1380                 OTSPoint3D offsetPoint =
1381                         new OTSPoint3D(x + this.fractionalHelperDirections[i].x, y + this.fractionalHelperDirections[i].y);
1382                 p = OTSPoint3D.intersectionOfLines(point, offsetPoint, this.points[i], this.points[i + 1]);
1383             }
1384             double segLength = this.points[i].distance(this.points[i + 1]).si + FRAC_PROJ_PRECISION;
1385             if (p == null || this.points[i].distance(p).si > segLength || this.points[i + 1].distance(p).si > segLength)
1386             {
1387                 // intersection must be on the segment
1388                 // in case of p == null, the length of the fractional helper direction falls away due to precision
1389                 continue;
1390             }
1391             // distance from (x, y) to intersection on segment
1392             double dx = x - p.x;
1393             double dy = y - p.y;
1394             distance = Math.sqrt(dx * dx + dy * dy);
1395             // distance from start of segment to point on segment
1396             if (distance < minDistance)
1397             {
1398                 dx = p.x - this.points[i].x;
1399                 dy = p.y - this.points[i].y;
1400                 double dFrac = Math.sqrt(dx * dx + dy * dy);
1401                 // fraction to point on segment
1402                 minDistance = distance;
1403                 minSegmentFraction = dFrac / (this.lengthIndexedLine[i + 1] - this.lengthIndexedLine[i]);
1404                 minSegment = i;
1405             }
1406         }
1407 
1408         // return
1409         if (minSegment == -1)
1410 
1411         {
1412             /*
1413              * If fractional projection fails (x, y) is either outside of the applicable area for fractional projection, or is
1414              * inside an area where numerical difficulties arise (i.e. far away outside of very slight bend which is considered
1415              * parallel).
1416              */
1417             // CategoryLogger.info(Cat.CORE, "projectFractional failed to project " + point + " on " + this
1418             // + "; using fallback approach");
1419             return fallback.getFraction(this, x, y);
1420         }
1421 
1422         double segLen = this.lengthIndexedLine[minSegment + 1] - this.lengthIndexedLine[minSegment];
1423         return (this.lengthIndexedLine[minSegment] + segLen * minSegmentFraction) / getLengthSI();
1424 
1425     }
1426 
1427     /**
1428      * Fallback method for when fractional projection fails as the point is beyond the line or from numerical limitations.
1429      * <p>
1430      * Copyright (c) 2013-2020 Delft University of Technology, PO Box 5, 2600 AA, Delft, the Netherlands. All rights reserved.
1431      * <br>
1432      * BSD-style license. See <a href="http://opentrafficsim.org/node/13">OpenTrafficSim License</a>.
1433      * <p>
1434      * @version $Revision$, $LastChangedDate$, by $Author$, initial version 18 apr. 2018 <br>
1435      * @author <a href="http://www.tbm.tudelft.nl/averbraeck">Alexander Verbraeck</a>
1436      * @author <a href="http://www.tudelft.nl/pknoppers">Peter Knoppers</a>
1437      * @author <a href="http://www.transport.citg.tudelft.nl">Wouter Schakel</a>
1438      */
1439     public enum FractionalFallback
1440     {
1441         /** Orthogonal projection. */
1442         ORTHOGONAL
1443         {
1444             @Override
1445             double getFraction(final OTSLine3D line, final double x, final double y)
1446             {
1447                 return line.projectOrthogonal(x, y);
1448             }
1449         },
1450 
1451         /** Distance to nearest end point. */
1452         ENDPOINT
1453         {
1454             @Override
1455             double getFraction(final OTSLine3D line, final double x, final double y)
1456             {
1457                 OTSPoint3Dy/OTSPoint3D.html#OTSPoint3D">OTSPoint3D point = new OTSPoint3D(x, y);
1458                 double dStart = point.distanceSI(line.getFirst());
1459                 double dEnd = point.distanceSI(line.getLast());
1460                 if (dStart < dEnd)
1461                 {
1462                     return -dStart / line.getLengthSI();
1463                 }
1464                 else
1465                 {
1466                     return (dEnd + line.getLengthSI()) / line.getLengthSI();
1467                 }
1468             }
1469         },
1470 
1471         /** NaN value. */
1472         NaN
1473         {
1474             @Override
1475             double getFraction(final OTSLine3D line, final double x, final double y)
1476             {
1477                 return Double.NaN;
1478             }
1479         };
1480 
1481         /**
1482          * Returns fraction for when fractional projection fails as the point is beyond the line or from numerical limitations.
1483          * @param line OTSLine3D; line
1484          * @param x double; x coordinate of point
1485          * @param y double; y coordinate of point
1486          * @return double; fraction for when fractional projection fails
1487          */
1488         abstract double getFraction(OTSLine3D line, double x, double y);
1489 
1490     }
1491 
1492     /**
1493      * Determines all helpers (points and/or directions) for fractional projection and stores fixed information in properties
1494      * while returning the first and last center points (.
1495      * @param start Direction; direction in first point
1496      * @param end Direction; direction in last point
1497      */
1498     private synchronized void determineFractionalHelpers(final Direction start, final Direction end)
1499     {
1500 
1501         final int n = this.points.length - 1;
1502 
1503         // calculate fixed helpers if not done yet
1504         if (this.fractionalHelperCenters == null)
1505         {
1506             this.fractionalHelperCenters = new OTSPoint3D[n];
1507             this.fractionalHelperDirections = new Point2D.Double[n];
1508             if (this.points.length > 2)
1509             {
1510                 // intersection of parallel lines of first and second segment
1511                 OTSLine3D prevOfsSeg = unitOffsetSegment(0);
1512                 OTSLine3D nextOfsSeg = unitOffsetSegment(1);
1513                 OTSPoint3D parStartPoint;
1514                 try
1515                 {
1516                     parStartPoint = OTSPoint3D.intersectionOfLines(prevOfsSeg.get(0), prevOfsSeg.get(1), nextOfsSeg.get(0),
1517                             nextOfsSeg.get(1));
1518                     if (parStartPoint == null || prevOfsSeg.get(1).distanceSI(nextOfsSeg.get(0)) < Math
1519                             .min(prevOfsSeg.get(1).distanceSI(parStartPoint), nextOfsSeg.get(0).distanceSI(parStartPoint)))
1520                     {
1521                         parStartPoint = new OTSPoint3D((prevOfsSeg.get(1).x + nextOfsSeg.get(0).x) / 2,
1522                                 (prevOfsSeg.get(1).y + nextOfsSeg.get(0).y) / 2);
1523                     }
1524                 }
1525                 catch (OTSGeometryException oge)
1526                 {
1527                     // cannot happen as only the first and second point (which are always present) are requested
1528                     throw new RuntimeException(oge);
1529                 }
1530                 // remember the intersection of the first two unit offset segments
1531                 this.firstOffsetIntersection = parStartPoint;
1532                 // loop segments
1533                 for (int i = 1; i < this.points.length - 2; i++)
1534                 {
1535                     prevOfsSeg = nextOfsSeg;
1536                     nextOfsSeg = unitOffsetSegment(i + 1);
1537                     OTSPoint3D parEndPoint;
1538                     try
1539                     {
1540                         parEndPoint = OTSPoint3D.intersectionOfLines(prevOfsSeg.get(0), prevOfsSeg.get(1), nextOfsSeg.get(0),
1541                                 nextOfsSeg.get(1));
1542                         if (parEndPoint == null || prevOfsSeg.get(1).distanceSI(nextOfsSeg.get(0)) < Math
1543                                 .min(prevOfsSeg.get(1).distanceSI(parEndPoint), nextOfsSeg.get(0).distanceSI(parEndPoint)))
1544                         {
1545                             parEndPoint = new OTSPoint3D((prevOfsSeg.get(1).x + nextOfsSeg.get(0).x) / 2,
1546                                     (prevOfsSeg.get(1).y + nextOfsSeg.get(0).y) / 2);
1547                         }
1548                     }
1549                     catch (OTSGeometryException oge)
1550                     {
1551                         // cannot happen as only the first and second point (which are always present) are requested
1552                         throw new RuntimeException(oge);
1553                     }
1554                     // center = intersections of helper lines
1555                     this.fractionalHelperCenters[i] =
1556                             OTSPoint3D.intersectionOfLines(this.points[i], parStartPoint, this.points[i + 1], parEndPoint);
1557                     if (this.fractionalHelperCenters[i] == null)
1558                     {
1559                         // parallel helper lines, parallel segments or /\/ cause parallel helper lines, use direction
1560                         this.fractionalHelperDirections[i] =
1561                                 new Point2D.Double(parStartPoint.x - this.points[i].x, parStartPoint.y - this.points[i].y);
1562                     }
1563                     parStartPoint = parEndPoint;
1564                 }
1565                 // remember the intersection of the last two unit offset segments
1566                 this.lastOffsetIntersection = parStartPoint;
1567             }
1568         }
1569 
1570         // use directions at start and end to get unit offset points to the left at a distance of 1
1571         double ang = (start == null ? Math.atan2(this.points[1].y - this.points[0].y, this.points[1].x - this.points[0].x)
1572                 : start.getInUnit(DirectionUnit.DEFAULT)) + Math.PI / 2; // start.si + Math.PI / 2;
1573         OTSPoint3Detry/OTSPoint3D.html#OTSPoint3D">OTSPoint3D p1 = new OTSPoint3D(this.points[0].x + Math.cos(ang), this.points[0].y + Math.sin(ang));
1574         ang = (end == null ? Math.atan2(this.points[n].y - this.points[n - 1].y, this.points[n].x - this.points[n - 1].x)
1575                 : end.getInUnit(DirectionUnit.DEFAULT)) + Math.PI / 2; // end.si + Math.PI / 2;
1576         OTSPoint3Detry/OTSPoint3D.html#OTSPoint3D">OTSPoint3D p2 = new OTSPoint3D(this.points[n].x + Math.cos(ang), this.points[n].y + Math.sin(ang));
1577 
1578         // calculate first and last center (i.e. intersection of unit offset segments), which depend on inputs 'start' and 'end'
1579         if (this.points.length > 2)
1580         {
1581             this.fractionalHelperCenters[0] =
1582                     OTSPoint3D.intersectionOfLines(this.points[0], p1, this.points[1], this.firstOffsetIntersection);
1583             this.fractionalHelperCenters[n - 1] =
1584                     OTSPoint3D.intersectionOfLines(this.points[n - 1], this.lastOffsetIntersection, this.points[n], p2);
1585             if (this.fractionalHelperCenters[n - 1] == null)
1586             {
1587                 // parallel helper lines, use direction for projection
1588                 this.fractionalHelperDirections[n - 1] = new Point2D.Double(p2.x - this.points[n].x, p2.y - this.points[n].y);
1589             }
1590         }
1591         else
1592         {
1593             // only a single segment
1594             this.fractionalHelperCenters[0] = OTSPoint3D.intersectionOfLines(this.points[0], p1, this.points[1], p2);
1595         }
1596         if (this.fractionalHelperCenters[0] == null)
1597         {
1598             // parallel helper lines, use direction for projection
1599             this.fractionalHelperDirections[0] = new Point2D.Double(p1.x - this.points[0].x, p1.y - this.points[0].y);
1600         }
1601 
1602     }
1603 
1604     /**
1605      * Helper method for fractional projection which returns an offset line to the left of a segment at a distance of 1.
1606      * @param segment int; segment number
1607      * @return parallel line to the left of a segment at a distance of 1
1608      */
1609     private synchronized OTSLine3D unitOffsetSegment(final int segment)
1610     {
1611 
1612         // double angle = Math.atan2(this.points[segment + 1].y - this.points[segment].y,
1613         // this.points[segment + 1].x - this.points[segment].x) + Math.PI / 2;
1614         // while (angle > Math.PI)
1615         // {
1616         // angle -= Math.PI;
1617         // }
1618         // while (angle < -Math.PI)
1619         // {
1620         // angle += Math.PI;
1621         // }
1622         // OTSPoint3D from = new OTSPoint3D(this.points[segment].x + Math.cos(angle), this.points[segment].y + Math.sin(angle));
1623         // OTSPoint3D to =
1624         // new OTSPoint3D(this.points[segment + 1].x + Math.cos(angle), this.points[segment + 1].y + Math.sin(angle));
1625         // try
1626         // {
1627         // return new OTSLine3D(from, to);
1628         // }
1629         // catch (OTSGeometryException oge)
1630         // {
1631         // // cannot happen as points are from this OTSLine3D which performed the same checks and 2 points are given
1632         // throw new RuntimeException(oge);
1633         // }
1634         OTSPoint3Dry/OTSPoint3D.html#OTSPoint3D">OTSPoint3D from = new OTSPoint3D(this.points[segment].x, this.points[segment].y);
1635         OTSPoint3Detry/OTSPoint3D.html#OTSPoint3D">OTSPoint3D to = new OTSPoint3D(this.points[segment + 1].x, this.points[segment + 1].y);
1636         try
1637         {
1638             OTSLine3Dtry/OTSLine3D.html#OTSLine3D">OTSLine3D line = new OTSLine3D(from, to);
1639             return line.offsetLine(1.0);
1640         }
1641         catch (OTSGeometryException oge)
1642         {
1643             // Cannot happen as points are from this OTSLine3D which performed the same checks and 2 points are given
1644             throw new RuntimeException(oge);
1645         }
1646     }
1647 
1648     /**
1649      * Returns the projected directional radius of the line at a given fraction. Negative values reflect right-hand curvature in
1650      * the design-line direction. The radius is taken as the minimum of the radii at the vertices before and after the given
1651      * fraction. The radius at a vertex is calculated as the radius of a circle that is equidistant from both edges connected to
1652      * the vertex. The circle center is on a line perpendicular to the shortest edge, crossing through the middle of the
1653      * shortest edge. This method ignores Z components.
1654      * @param fraction double; fraction along the line, between 0.0 and 1.0 (both inclusive)
1655      * @return Length; radius; the local radius; or si field set to Double.NaN if line is totally straight
1656      * @throws OTSGeometryException fraction out of bounds
1657      */
1658     public synchronized Length getProjectedRadius(final double fraction) throws OTSGeometryException
1659     {
1660         Throw.when(fraction < 0.0 || fraction > 1.0, OTSGeometryException.class, "Fraction %f is out of bounds [0.0 ... 1.0]",
1661                 fraction);
1662         if (this.vertexRadii == null)
1663         {
1664             this.vertexRadii = new Length[size() - 1];
1665         }
1666         int index = find(fraction * getLength().si);
1667         if (index > 0 && this.vertexRadii[index] == null)
1668         {
1669             this.vertexRadii[index] = getProjectedVertexRadius(index);
1670         }
1671         if (index < size() - 2 && this.vertexRadii[index + 1] == null)
1672         {
1673             this.vertexRadii[index + 1] = getProjectedVertexRadius(index + 1);
1674         }
1675         if (index == 0)
1676         {
1677             if (this.vertexRadii.length < 2)
1678             {
1679                 return Length.instantiateSI(Double.NaN);
1680             }
1681             return this.vertexRadii[1];
1682         }
1683         if (index == size() - 2)
1684         {
1685             return this.vertexRadii[size() - 2];
1686         }
1687         return Math.abs(this.vertexRadii[index].si) < Math.abs(this.vertexRadii[index + 1].si) ? this.vertexRadii[index]
1688                 : this.vertexRadii[index + 1];
1689     }
1690 
1691     /**
1692      * Calculates the directional radius at a vertex. Negative values reflect right-hand curvature in the design-line direction.
1693      * The radius at a vertex is calculated as the radius of a circle that is equidistant from both edges connected to the
1694      * vertex. The circle center is on a line perpendicular to the shortest edge, crossing through the middle of the shortest
1695      * edge. This function ignores Z components.
1696      * @param index int; index of the vertex in range [1 ... size() - 2]
1697      * @return Length; radius at the vertex
1698      * @throws OTSGeometryException if the index is out of bounds
1699      */
1700     public synchronized Length getProjectedVertexRadius(final int index) throws OTSGeometryException
1701     {
1702         Throw.when(index < 1 || index > size() - 2, OTSGeometryException.class, "Index %d is out of bounds [1 ... size() - 2].",
1703                 index);
1704         makeLengthIndexedLine();
1705         determineFractionalHelpers(null, null);
1706         double length1 = this.lengthIndexedLine[index] - this.lengthIndexedLine[index - 1];
1707         double length2 = this.lengthIndexedLine[index + 1] - this.lengthIndexedLine[index];
1708         int shortIndex = length1 < length2 ? index : index + 1;
1709         // center of shortest edge
1710         OTSPoint3Detry/OTSPoint3D.html#OTSPoint3D">OTSPoint3D p1 = new OTSPoint3D(.5 * (this.points[shortIndex - 1].x + this.points[shortIndex].x),
1711                 .5 * (this.points[shortIndex - 1].y + this.points[shortIndex].y),
1712                 .5 * (this.points[shortIndex - 1].z + this.points[shortIndex].z));
1713         // perpendicular to shortest edge, line crossing p1
1714         OTSPoint3Detry/OTSPoint3D.html#OTSPoint3D">OTSPoint3D p2 = new OTSPoint3D(p1.x + (this.points[shortIndex].y - this.points[shortIndex - 1].y),
1715                 p1.y - (this.points[shortIndex].x - this.points[shortIndex - 1].x), p1.z);
1716         // vertex
1717         OTSPoint3D p3 = this.points[index];
1718         // point on line that splits angle between edges at vertex 50-50
1719         OTSPoint3D p4 = this.fractionalHelperCenters[index];
1720         if (p4 == null)
1721         {
1722             // parallel helper lines
1723             p4 = new OTSPoint3D(p3.x + this.fractionalHelperDirections[index].x,
1724                     p3.y + this.fractionalHelperDirections[index].y);
1725         }
1726         OTSPoint3D intersection = OTSPoint3D.intersectionOfLines(p1, p2, p3, p4);
1727         if (null == intersection)
1728         {
1729             return Length.instantiateSI(Double.NaN);
1730         }
1731         // determine left or right
1732         double refLength = length1 < length2 ? length1 : length2;
1733         Length radius = intersection.horizontalDistance(p1);
1734         Length i2p2 = intersection.horizontalDistance(p2);
1735         if (radius.si < i2p2.si && i2p2.si > refLength)
1736         {
1737             // left as p1 is closer than p2 (which was placed to the right) and not on the perpendicular line
1738             return radius;
1739         }
1740         // right as not left
1741         return radius.neg();
1742     }
1743 
1744     /**
1745      * Returns the length fraction at the vertex.
1746      * @param index int; index of vertex [0 ... size() - 1]
1747      * @return double; length fraction at the vertex
1748      * @throws OTSGeometryException if the index is out of bounds
1749      */
1750     public synchronized double getVertexFraction(final int index) throws OTSGeometryException
1751     {
1752         Throw.when(index < 0 || index > size() - 1, OTSGeometryException.class, "Index %d is out of bounds [0 %d].", index,
1753                 size() - 1);
1754         makeLengthIndexedLine();
1755         return this.lengthIndexedLine[index] / getLengthSI();
1756     }
1757 
1758     /**
1759      * Calculate the centroid of this line, and the bounds, and cache for later use.
1760      */
1761     private synchronized void calcCentroidBounds()
1762     {
1763         double minX = Double.POSITIVE_INFINITY;
1764         double minY = Double.POSITIVE_INFINITY;
1765         double minZ = Double.POSITIVE_INFINITY;
1766         double maxX = Double.NEGATIVE_INFINITY;
1767         double maxY = Double.NEGATIVE_INFINITY;
1768         double maxZ = Double.NEGATIVE_INFINITY;
1769         for (OTSPoint3D p : this.points)
1770         {
1771             minX = Math.min(minX, p.x);
1772             minY = Math.min(minY, p.y);
1773             minZ = Math.min(minZ, p.z);
1774             maxX = Math.max(maxX, p.x);
1775             maxY = Math.max(maxY, p.y);
1776             maxZ = Math.max(maxZ, p.z);
1777         }
1778         this.centroid = new OTSPoint3D((maxX + minX) / 2, (maxY + minY) / 2, (maxZ + minZ) / 2);
1779         double deltaX = maxX - minX;
1780         double deltaY = maxY - minY;
1781         double deltaZ = maxZ - minZ;
1782         this.bounds = new BoundingBox(new Point3d(-deltaX / 2.0, -deltaY / 2.0, -deltaZ / 2.0),
1783                 new Point3d(deltaX / 2, deltaY / 2, deltaZ / 2));
1784         this.envelope = new Envelope(minX, maxX, minY, maxY);
1785     }
1786 
1787     /**
1788      * Retrieve the centroid of this OTSLine3D.
1789      * @return OTSPoint3D; the centroid of this OTSLine3D
1790      */
1791     public final synchronized OTSPoint3D getCentroid()
1792     {
1793         if (this.centroid == null)
1794         {
1795             calcCentroidBounds();
1796         }
1797         return this.centroid;
1798     }
1799 
1800     /**
1801      * Get the bounding rectangle of this OTSLine3D.
1802      * @return Rectangle2D; the bounding rectangle of this OTSLine3D
1803      */
1804     public final synchronized Envelope getEnvelope()
1805     {
1806         if (this.envelope == null)
1807         {
1808             calcCentroidBounds();
1809         }
1810         return this.envelope;
1811     }
1812 
1813     /** {@inheritDoc} */
1814     @Override
1815     @SuppressWarnings("checkstyle:designforextension")
1816     public synchronized DirectedPoint getLocation()
1817     {
1818         if (this.centroid == null)
1819         {
1820             calcCentroidBounds();
1821         }
1822         return this.centroid.getDirectedPoint();
1823     }
1824 
1825     /** {@inheritDoc} */
1826     @Override
1827     @SuppressWarnings("checkstyle:designforextension")
1828     public synchronized Bounds getBounds()
1829     {
1830         if (this.bounds == null)
1831         {
1832             calcCentroidBounds();
1833         }
1834         return this.bounds;
1835     }
1836 
1837     /** {@inheritDoc} */
1838     @Override
1839     @SuppressWarnings("checkstyle:designforextension")
1840     public String toString()
1841     {
1842         return Arrays.toString(this.points);
1843     }
1844 
1845     /** {@inheritDoc} */
1846     @Override
1847     @SuppressWarnings("checkstyle:designforextension")
1848     public int hashCode()
1849     {
1850         final int prime = 31;
1851         int result = 1;
1852         result = prime * result + Arrays.hashCode(this.points);
1853         return result;
1854     }
1855 
1856     /** {@inheritDoc} */
1857     @Override
1858     @SuppressWarnings({ "checkstyle:designforextension", "checkstyle:needbraces" })
1859     public boolean equals(final Object obj)
1860     {
1861         if (this == obj)
1862             return true;
1863         if (obj == null)
1864             return false;
1865         if (getClass() != obj.getClass())
1866             return false;
1867         OTSLine3D/../../../org/opentrafficsim/core/geometry/OTSLine3D.html#OTSLine3D">OTSLine3D other = (OTSLine3D) obj;
1868         if (!Arrays.equals(this.points, other.points))
1869             return false;
1870         return true;
1871     }
1872 
1873     /**
1874      * Convert the 2D projection of this OTSLine3D to something that MS-Excel can plot.
1875      * @return excel XY plottable output
1876      */
1877     public final String toExcel()
1878     {
1879         StringBuffer s = new StringBuffer();
1880         for (OTSPoint3D p : this.points)
1881         {
1882             s.append(p.x + "\t" + p.y + "\n");
1883         }
1884         return s.toString();
1885     }
1886 
1887     /**
1888      * Convert the 2D projection of this OTSLine3D to Peter's plot format.
1889      * @return Peter's format plot output
1890      */
1891     public final String toPlot()
1892     {
1893         StringBuffer result = new StringBuffer();
1894         for (OTSPoint3D p : this.points)
1895         {
1896             result.append(String.format(Locale.US, "%s%.3f,%.3f", 0 == result.length() ? "M" : " L", p.x, p.y));
1897         }
1898         result.append("\n");
1899         return result.toString();
1900     }
1901 
1902 }