View Javadoc
1   package org.opentrafficsim.core.geometry;
2   
3   import java.util.ArrayList;
4   import java.util.List;
5   import java.util.Locale;
6   
7   /**
8    * Peter Knoppers' attempt to implement offsetLine.
9    * <p>
10   * Copyright (c) 2013-2020 Delft University of Technology, PO Box 5, 2600 AA, Delft, the Netherlands. All rights reserved. <br>
11   * BSD-style license. See <a href="http://opentrafficsim.org/docs/current/license.html">OpenTrafficSim License</a>.
12   * <p>
13   * @version $Revision$, $LastChangedDate$, by $Author$, initial version Dec 1, 2015 <br>
14   * @author <a href="http://www.tudelft.nl/pknoppers">Peter Knoppers</a>
15   */
16  public final class OTSOffsetLinePK
17  {
18      /** This class should never be instantiated. */
19      private OTSOffsetLinePK()
20      {
21          // Cannot be instantiated.
22      }
23  
24      /** Debugging flag. */
25      private static boolean debugOffsetLine = false;
26  
27      /** Precision of approximation of arcs in the offsetLine method. */
28      private static double circlePrecision = 0.001;
29  
30      /** Noise in the reference line less than this value is always filtered. */
31      private static double offsetMinimumFilterValue = 0.001;
32  
33      /** Noise in the reference line greater than this value is never filtered. */
34      private static double offsetMaximumFilterValue = 0.1;
35  
36      /**
37       * Noise in the reference line less than <cite>offset / offsetFilterRatio</cite> is filtered except when the resulting value
38       * exceeds <cite>offsetMaximumFilterValue</cite>.
39       */
40      private static double offsetFilterRatio = 10;
41  
42      /**
43       * Construct an offset line.
44       * @param referenceLine OTSLine3D; the reference line
45       * @param offset double; the offset; positive values indicate left of the reference line, negative values indicate right of
46       *            the reference line
47       * @return OTSLine3D; a line at the specified offset from the reference line
48       * @throws OTSGeometryException when this method runs into major trouble and cannot produce a decent result
49       */
50      @SuppressWarnings("checkstyle:methodlength")
51      public static OTSLine3DSLine3D.html#OTSLine3D">OTSLine3D offsetLine(final OTSLine3D referenceLine, final double offset) throws OTSGeometryException
52      {
53          // if (referenceLine.size() > 1 && referenceLine.getFirst().horizontalDistanceSI(new OTSPoint3D(-200.376, -111.999)) <
54          // 0.1
55          // && referenceLine.get(1).horizontalDistanceSI(new OTSPoint3D(-204.098, -100.180)) < 0.1 && Math.abs(offset) > 1)
56  
57          // if (referenceLine.size() > 1 && referenceLine.getFirst().horizontalDistanceSI(new OTSPoint3D(-177.580, -169.726)) <
58          // 0.1
59          // && referenceLine.get(1).horizontalDistanceSI(new OTSPoint3D(-179.028, -166.084)) < 0.1 && Math.abs(offset) > 1
60          // && referenceLine.size() == 0)
61          // {
62          // debugOffsetLine = true;
63          // for (int i = 0; i < referenceLine.size(); i++)
64          // {
65          // System.out.println(String.format(
66          // Locale.US,
67          // "point %2d: %20s,%20s%s",
68          // i,
69          // referenceLine.get(i).x,
70          // referenceLine.get(i).y,
71          // (i == 0 ? "" : " ("
72          // + Math.toDegrees(Math.atan2(referenceLine.get(i).y - referenceLine.get(i - 1).y,
73          // referenceLine.get(i).x - referenceLine.get(i - 1).x)) + ")")));
74          // }
75          // System.out.println("# offset is " + offset);
76          // System.out.println(OTSGeometry.printCoordinates("#reference:\nc0,0,0\n#", referenceLine, "\n "));
77          // }
78          // else
79          // {
80          // debugOffsetLine = false;
81          // }
82          double bufferOffset = Math.abs(offset);
83          final double precision = 0.00001;
84          if (bufferOffset < precision)
85          {
86              // squash the Z-coordinate
87              List<OTSPoint3D> coordinates = new ArrayList<>(referenceLine.size());
88              for (int i = 0; i < referenceLine.size(); i++)
89              {
90                  OTSPoint3D coordinate = referenceLine.get(i);
91                  coordinates.add(new OTSPoint3D(coordinate.x, coordinate.y));
92              }
93              return OTSLine3D.createAndCleanOTSLine3D(coordinates);
94          }
95  
96          OTSLine3D filteredReferenceLine = referenceLine.noiseFilteredLine(
97                  Math.max(offsetMinimumFilterValue, Math.min(bufferOffset / offsetFilterRatio, offsetMaximumFilterValue)));
98          if (debugOffsetLine)
99          {
100             System.out.println("#filtered reference line ");
101             System.out.println(filteredReferenceLine.toPlot());
102         }
103         List<OTSPoint3D> tempPoints = new ArrayList<>();
104         // Make good use of the fact that an OTSLine3D cannot have consecutive duplicate points and has > 1 points
105         OTSPoint3D prevPoint = filteredReferenceLine.get(0);
106         Double prevAngle = null;
107         for (int index = 0; index < filteredReferenceLine.size() - 1; index++)
108         {
109             OTSPoint3D nextPoint = filteredReferenceLine.get(index + 1);
110             double angle = Math.atan2(nextPoint.y - prevPoint.y, nextPoint.x - prevPoint.x);
111             if (debugOffsetLine)
112             {
113                 System.out.println("#reference segment " + index + " from " + prevPoint + " to " + nextPoint + " angle "
114                         + Math.toDegrees(angle));
115             }
116             OTSPoint3D segmentFrom =
117                     new OTSPoint3D(prevPoint.x - Math.sin(angle) * offset, prevPoint.y + Math.cos(angle) * offset);
118             OTSPoint3D segmentTo =
119                     new OTSPoint3D(nextPoint.x - Math.sin(angle) * offset, nextPoint.y + Math.cos(angle) * offset);
120             boolean addSegment = true;
121             if (index > 0)
122             {
123                 double deltaAngle = angle - prevAngle;
124                 if (Math.abs(deltaAngle) > Math.PI)
125                 {
126                     deltaAngle -= Math.signum(deltaAngle) * 2 * Math.PI;
127                 }
128                 if (deltaAngle * offset <= 0)
129                 {
130                     // Outside of curve of reference line
131                     // Approximate an arc using straight segments.
132                     // Determine how many segments are needed.
133                     int numSegments = 1;
134                     if (Math.abs(deltaAngle) > Math.PI / 2)
135                     {
136                         numSegments = 2;
137                     }
138                     while (true)
139                     {
140                         double maxError = bufferOffset * (1 - Math.abs(Math.cos(deltaAngle / numSegments / 2)));
141                         if (maxError < circlePrecision)
142                         {
143                             break; // required precision reached
144                         }
145                         numSegments *= 2;
146                     }
147                     OTSPoint3D prevArcPoint = tempPoints.get(tempPoints.size() - 1);
148                     // Generate the intermediate points
149                     for (int additionalPoint = 1; additionalPoint < numSegments; additionalPoint++)
150                     {
151                         double intermediateAngle =
152                                 (additionalPoint * angle + (numSegments - additionalPoint) * prevAngle) / numSegments;
153                         if (prevAngle * angle < 0 && Math.abs(prevAngle) > Math.PI / 2 && Math.abs(angle) > Math.PI / 2)
154                         {
155                             intermediateAngle += Math.PI;
156                         }
157                         OTSPoint3D.html#OTSPoint3D">OTSPoint3D intermediatePoint = new OTSPoint3D(prevPoint.x - Math.sin(intermediateAngle) * offset,
158                                 prevPoint.y + Math.cos(intermediateAngle) * offset);
159                         // Find any intersection points of the new segment and all previous segments
160                         OTSPoint3D prevSegFrom = null;
161                         int stopAt = tempPoints.size();
162                         for (int i = 0; i < stopAt; i++)
163                         {
164                             OTSPoint3D prevSegTo = tempPoints.get(i);
165                             if (null != prevSegFrom)
166                             {
167                                 OTSPoint3D prevSegIntersection = OTSPoint3D.intersectionOfLineSegments(prevArcPoint,
168                                         intermediatePoint, prevSegFrom, prevSegTo);
169                                 if (null != prevSegIntersection
170                                         && prevSegIntersection.horizontalDistanceSI(prevArcPoint) > circlePrecision
171                                         && prevSegIntersection.horizontalDistanceSI(prevSegFrom) > circlePrecision
172                                         && prevSegIntersection.horizontalDistanceSI(prevSegTo) > circlePrecision)
173                                 {
174                                     if (debugOffsetLine)
175                                     {
176                                         System.out.println("#inserting intersection in arc segment " + prevSegIntersection);
177                                     }
178                                     tempPoints.add(prevSegIntersection);
179                                     // System.out.println(new OTSLine3D(tempPoints).toPlot());
180                                 }
181                             }
182                             prevSegFrom = prevSegTo;
183                         }
184                         OTSPoint3D nextSegmentIntersection =
185                                 OTSPoint3D.intersectionOfLineSegments(prevSegFrom, intermediatePoint, segmentFrom, segmentTo);
186                         if (null != nextSegmentIntersection)
187                         {
188                             if (debugOffsetLine)
189                             {
190                                 System.out.println(
191                                         "#inserting intersection of arc segment with next segment " + nextSegmentIntersection);
192                             }
193                             tempPoints.add(nextSegmentIntersection);
194                             // System.out.println(new OTSLine3D(tempPoints).toPlot());
195                         }
196                         if (debugOffsetLine)
197                         {
198                             System.out.println("#inserting arc point " + intermediatePoint + " for angle "
199                                     + Math.toDegrees(intermediateAngle));
200                         }
201                         tempPoints.add(intermediatePoint);
202                         // System.out.println(new OTSLine3D(tempPoints).toPlot());
203                         prevArcPoint = intermediatePoint;
204                     }
205                 }
206                 // Inside of curve of reference line.
207                 // Add the intersection point of each previous segment and the next segment
208                 OTSPoint3D pPoint = null;
209                 int currentSize = tempPoints.size(); // PK DO NOT use the "dynamic" limit
210                 for (int i = 0; i < currentSize /* tempPoints.size() */; i++)
211                 {
212                     OTSPoint3D p = tempPoints.get(i);
213                     if (null != pPoint)
214                     {
215                         double pAngle = Math.atan2(p.y - pPoint.y, p.x - pPoint.x);
216                         double angleDifference = angle - pAngle;
217                         if (Math.abs(angleDifference) > Math.PI)
218                         {
219                             angleDifference -= Math.signum(angleDifference) * 2 * Math.PI;
220                         }
221                         if (debugOffsetLine)
222                         {
223                             System.out.println("#preceding segment " + pPoint + " to " + p + " angle " + Math.toDegrees(pAngle)
224                                     + ", next segment " + segmentFrom + " to " + segmentTo + " angle " + Math.toDegrees(angle)
225                                     + " angleDifference " + Math.toDegrees(angleDifference));
226                         }
227                         if (Math.abs(angleDifference) > 0)// 0.01)
228                         {
229                             OTSPoint3D intersection = OTSPoint3D.intersectionOfLineSegments(pPoint, p, segmentFrom, segmentTo);
230                             if (null != intersection)
231                             {
232                                 if (tempPoints.size() - 1 == i)
233                                 {
234                                     if (debugOffsetLine)
235                                     {
236                                         System.out.println("#Replacing last point of preceding segment and "
237                                                 + "first point of next segment by their intersection " + intersection);
238                                     }
239                                     tempPoints.remove(tempPoints.size() - 1);
240                                     segmentFrom = intersection;
241                                 }
242                                 else
243                                 {
244                                     if (debugOffsetLine)
245                                     {
246                                         if (tempPoints.size() > 17)
247                                         {
248                                             System.out.println("#not good");
249                                             System.out.println(new OTSLine3D(tempPoints).toPlot());
250                                         }
251                                         System.out.println("#Adding intersection of preceding segment and " + "next segment "
252                                                 + intersection);
253                                     }
254                                     tempPoints.add(intersection);
255                                 }
256                                 // tempPoints.set(tempPoints.size() - 1, intermediatePoint);
257                                 if (tempPoints.size() > 1)
258                                 {
259                                     // System.out.println(new OTSLine3D(tempPoints).toPlot());
260                                 }
261                             }
262                         }
263                         else
264                         {
265                             // This is where things went very wrong in the TestGeometry demo.
266                             if (debugOffsetLine)
267                             {
268                                 System.out.println("#Not adding intersection of preceding segment and this segment "
269                                         + "(angle too small)");
270                             }
271                             if (i == tempPoints.size() - 1)
272                             {
273                                 if (debugOffsetLine)
274                                 {
275                                     System.out.println("#Not adding segment, but replacing end of last segment");
276                                     tempPoints.remove(tempPoints.size() - 1);
277                                     segmentFrom = tempPoints.get(tempPoints.size() - 1);
278                                     tempPoints.remove(tempPoints.size() - 1); 
279                                 }
280                             }
281                         }
282                     }
283                     pPoint = p;
284                 }
285             }
286             if (addSegment)
287             {
288                 tempPoints.add(segmentFrom);
289                 tempPoints.add(segmentTo);
290                 if (debugOffsetLine)
291                 {
292                     System.out.println("#Added segmentFrom " + segmentFrom + " and segmentTo " + segmentTo);
293                     System.out.println(new OTSLine3D(tempPoints).toPlot());
294                 }
295                 prevPoint = nextPoint;
296                 prevAngle = angle;
297             }
298         }
299         if (debugOffsetLine)
300         {
301             System.out.println("# before cleanup: \nc1,0,0\n");
302             if (tempPoints.size() > 0)
303             {
304                 OTSPoint3D p = tempPoints.get(0);
305                 System.out.println(String.format(Locale.US, "M %.3f,%.3f", p.x, p.y));
306                 for (int i = 1; i < tempPoints.size(); i++)
307                 {
308                     p = tempPoints.get(i);
309                     System.out.println(String.format(Locale.US, "L %.3f,%.3f", p.x, p.y));
310                 }
311             }
312         }
313         // Remove points that are closer than the specified offset
314         for (int index = 1; index < tempPoints.size() - 1; index++)
315         {
316             OTSPoint3D checkPoint = tempPoints.get(index);
317             prevPoint = null;
318             boolean tooClose = false;
319             boolean somewhereAtCorrectDistance = false;
320             for (int i = 0; i < filteredReferenceLine.size(); i++)
321             {
322                 OTSPoint3D p = filteredReferenceLine.get(i);
323                 if (null != prevPoint)
324                 {
325                     OTSPoint3D closestPoint = checkPoint.closestPointOnSegment(prevPoint, p);
326                     double distance = closestPoint.horizontalDistanceSI(checkPoint);
327                     if (distance < bufferOffset - circlePrecision)
328                     {
329                         if (debugOffsetLine)
330                         {
331                             System.out.print("#point " + checkPoint + " inside buffer (distance is " + distance + ") ");
332                         }
333                         tooClose = true;
334                         break;
335                     }
336                     else if (distance < bufferOffset + precision)
337                     {
338                         somewhereAtCorrectDistance = true;
339                     }
340                 }
341                 prevPoint = p;
342             }
343             if (tooClose || !somewhereAtCorrectDistance)
344             {
345                 if (debugOffsetLine)
346                 {
347                     System.out.println("#Removing " + checkPoint);
348                 }
349                 tempPoints.remove(index);
350                 index--;
351             }
352         }
353         if (debugOffsetLine)
354         {
355             System.out.println("#after cleanup " + tempPoints.size() + " points left");
356             System.out.println(new OTSLine3D(tempPoints).toPlot());
357         }
358         // Fix the z-coordinate of all points that were added as intersections of segments.
359         for (int index = 0; index < tempPoints.size(); index++)
360         {
361             OTSPoint3D p = tempPoints.get(index);
362             if (Double.isNaN(p.z))
363             {
364                 tempPoints.set(index, new OTSPoint3D(p.x, p.y, 0));
365             }
366         }
367         try
368         {
369             return OTSLine3D.createAndCleanOTSLine3D(tempPoints);
370         }
371         catch (OTSGeometryException exception)
372         {
373             exception.printStackTrace();
374         }
375         return null;
376     }
377     
378     /**
379      * Set or clear the debugging flag.
380      * @param newValue boolean; new value for the debugging flag
381      */
382     public static void setDebugOffsetLine(final boolean newValue)
383     {
384         debugOffsetLine = newValue;
385     }
386     
387 }