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 }