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-2019 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 static boolean debugOffsetLine = false;
26
27 /** Precision of approximation of arcs in the offsetLine method. */
28 static double circlePrecision = 0.001;
29
30 /** Noise in the reference line less than this value is always filtered. */
31 static double offsetMinimumFilterValue = 0.001;
32
33 /** Noise in the reference line greater than this value is never filtered. */
34 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 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 public static OTSLine3D offsetLine(final OTSLine3D referenceLine, final double offset) throws OTSGeometryException
51 {
52 // if (referenceLine.size() > 1 && referenceLine.getFirst().horizontalDistanceSI(new OTSPoint3D(-200.376, -111.999)) <
53 // 0.1
54 // && referenceLine.get(1).horizontalDistanceSI(new OTSPoint3D(-204.098, -100.180)) < 0.1 && Math.abs(offset) > 1)
55
56 // if (referenceLine.size() > 1 && referenceLine.getFirst().horizontalDistanceSI(new OTSPoint3D(-177.580, -169.726)) <
57 // 0.1
58 // && referenceLine.get(1).horizontalDistanceSI(new OTSPoint3D(-179.028, -166.084)) < 0.1 && Math.abs(offset) > 1
59 // && referenceLine.size() == 0)
60 // {
61 // debugOffsetLine = true;
62 // for (int i = 0; i < referenceLine.size(); i++)
63 // {
64 // System.out.println(String.format(
65 // Locale.US,
66 // "point %2d: %20s,%20s%s",
67 // i,
68 // referenceLine.get(i).x,
69 // referenceLine.get(i).y,
70 // (i == 0 ? "" : " ("
71 // + Math.toDegrees(Math.atan2(referenceLine.get(i).y - referenceLine.get(i - 1).y,
72 // referenceLine.get(i).x - referenceLine.get(i - 1).x)) + ")")));
73 // }
74 // System.out.println("# offset is " + offset);
75 // System.out.println(OTSGeometry.printCoordinates("#reference:\nc0,0,0\n#", referenceLine, "\n "));
76 // }
77 // else
78 // {
79 // debugOffsetLine = false;
80 // }
81 double bufferOffset = Math.abs(offset);
82 final double precision = 0.00001;
83 if (bufferOffset < precision)
84 {
85 // squash the Z-coordinate
86 List<OTSPoint3D> coordinates = new ArrayList<>(referenceLine.size());
87 for (int i = 0; i < referenceLine.size(); i++)
88 {
89 OTSPoint3D coordinate = referenceLine.get(i);
90 coordinates.add(new OTSPoint3D(coordinate.x, coordinate.y));
91 }
92 return OTSLine3D.createAndCleanOTSLine3D(coordinates);
93 }
94
95 OTSLine3D filteredReferenceLine = referenceLine.noiseFilteredLine(
96 Math.max(offsetMinimumFilterValue, Math.min(bufferOffset / offsetFilterRatio, offsetMaximumFilterValue)));
97 if (debugOffsetLine)
98 {
99 System.out.println("#filtered reference line ");
100 System.out.println(filteredReferenceLine.toPlot());
101 }
102 List<OTSPoint3D> tempPoints = new ArrayList<>();
103 // Make good use of the fact that an OTSLine3D cannot have consecutive duplicate points and has > 1 points
104 OTSPoint3D prevPoint = filteredReferenceLine.get(0);
105 Double prevAngle = null;
106 for (int index = 0; index < filteredReferenceLine.size() - 1; index++)
107 {
108 OTSPoint3D nextPoint = filteredReferenceLine.get(index + 1);
109 double angle = Math.atan2(nextPoint.y - prevPoint.y, nextPoint.x - prevPoint.x);
110 if (debugOffsetLine)
111 {
112 System.out.println("#reference segment " + index + " from " + prevPoint + " to " + nextPoint + " angle "
113 + Math.toDegrees(angle));
114 }
115 OTSPoint3D segmentFrom =
116 new OTSPoint3D(prevPoint.x - Math.sin(angle) * offset, prevPoint.y + Math.cos(angle) * offset);
117 OTSPoint3D segmentTo =
118 new OTSPoint3D(nextPoint.x - Math.sin(angle) * offset, nextPoint.y + Math.cos(angle) * offset);
119 boolean addSegment = true;
120 if (index > 0)
121 {
122 double deltaAngle = angle - prevAngle;
123 if (Math.abs(deltaAngle) > Math.PI)
124 {
125 deltaAngle -= Math.signum(deltaAngle) * 2 * Math.PI;
126 }
127 if (deltaAngle * offset <= 0)
128 {
129 // Outside of curve of reference line
130 // Approximate an arc using straight segments.
131 // Determine how many segments are needed.
132 int numSegments = 1;
133 if (Math.abs(deltaAngle) > Math.PI / 2)
134 {
135 numSegments = 2;
136 }
137 while (true)
138 {
139 double maxError = bufferOffset * (1 - Math.abs(Math.cos(deltaAngle / numSegments / 2)));
140 if (maxError < circlePrecision)
141 {
142 break; // required precision reached
143 }
144 numSegments *= 2;
145 }
146 OTSPoint3D prevArcPoint = tempPoints.get(tempPoints.size() - 1);
147 // Generate the intermediate points
148 for (int additionalPoint = 1; additionalPoint < numSegments; additionalPoint++)
149 {
150 double intermediateAngle =
151 (additionalPoint * angle + (numSegments - additionalPoint) * prevAngle) / numSegments;
152 if (prevAngle * angle < 0 && Math.abs(prevAngle) > Math.PI / 2 && Math.abs(angle) > Math.PI / 2)
153 {
154 intermediateAngle += Math.PI;
155 }
156 OTSPoint3D intermediatePoint = new OTSPoint3D(prevPoint.x - Math.sin(intermediateAngle) * offset,
157 prevPoint.y + Math.cos(intermediateAngle) * offset);
158 // Find any intersection points of the new segment and all previous segments
159 OTSPoint3D prevSegFrom = null;
160 int stopAt = tempPoints.size();
161 for (int i = 0; i < stopAt; i++)
162 {
163 OTSPoint3D prevSegTo = tempPoints.get(i);
164 if (null != prevSegFrom)
165 {
166 OTSPoint3D prevSegIntersection = OTSPoint3D.intersectionOfLineSegments(prevArcPoint,
167 intermediatePoint, prevSegFrom, prevSegTo);
168 if (null != prevSegIntersection
169 && prevSegIntersection.horizontalDistanceSI(prevArcPoint) > circlePrecision
170 && prevSegIntersection.horizontalDistanceSI(prevSegFrom) > circlePrecision
171 && prevSegIntersection.horizontalDistanceSI(prevSegTo) > circlePrecision)
172 {
173 if (debugOffsetLine)
174 {
175 System.out.println("#inserting intersection in arc segment " + prevSegIntersection);
176 }
177 tempPoints.add(prevSegIntersection);
178 // System.out.println(new OTSLine3D(tempPoints).toPlot());
179 }
180 }
181 prevSegFrom = prevSegTo;
182 }
183 OTSPoint3D nextSegmentIntersection =
184 OTSPoint3D.intersectionOfLineSegments(prevSegFrom, intermediatePoint, segmentFrom, segmentTo);
185 if (null != nextSegmentIntersection)
186 {
187 if (debugOffsetLine)
188 {
189 System.out.println(
190 "#inserting intersection of arc segment with next segment " + nextSegmentIntersection);
191 }
192 tempPoints.add(nextSegmentIntersection);
193 // System.out.println(new OTSLine3D(tempPoints).toPlot());
194 }
195 if (debugOffsetLine)
196 {
197 System.out.println("#inserting arc point " + intermediatePoint + " for angle "
198 + Math.toDegrees(intermediateAngle));
199 }
200 tempPoints.add(intermediatePoint);
201 // System.out.println(new OTSLine3D(tempPoints).toPlot());
202 prevArcPoint = intermediatePoint;
203 }
204 }
205 // Inside of curve of reference line.
206 // Add the intersection point of each previous segment and the next segment
207 OTSPoint3D pPoint = null;
208 int currentSize = tempPoints.size(); // PK DO NOT use the "dynamic" limit
209 for (int i = 0; i < currentSize /* tempPoints.size() */; i++)
210 {
211 OTSPoint3D p = tempPoints.get(i);
212 if (null != pPoint)
213 {
214 double pAngle = Math.atan2(p.y - pPoint.y, p.x - pPoint.x);
215 double angleDifference = angle - pAngle;
216 if (Math.abs(angleDifference) > Math.PI)
217 {
218 angleDifference -= Math.signum(angleDifference) * 2 * Math.PI;
219 }
220 if (debugOffsetLine)
221 {
222 System.out.println("#preceding segment " + pPoint + " to " + p + " angle " + Math.toDegrees(pAngle)
223 + ", next segment " + segmentFrom + " to " + segmentTo + " angle " + Math.toDegrees(angle)
224 + " angleDifference " + Math.toDegrees(angleDifference));
225 }
226 if (Math.abs(angleDifference) > 0)// 0.01)
227 {
228 OTSPoint3D intersection = OTSPoint3D.intersectionOfLineSegments(pPoint, p, segmentFrom, segmentTo);
229 if (null != intersection)
230 {
231 if (tempPoints.size() - 1 == i)
232 {
233 if (debugOffsetLine)
234 {
235 System.out.println("#Replacing last point of preceding segment and "
236 + "first point of next segment by their intersection " + intersection);
237 }
238 tempPoints.remove(tempPoints.size() - 1);
239 segmentFrom = intersection;
240 }
241 else
242 {
243 if (debugOffsetLine)
244 {
245 if (tempPoints.size() > 17)
246 {
247 System.out.println("#not good");
248 System.out.println(new OTSLine3D(tempPoints).toPlot());
249 }
250 System.out.println("#Adding intersection of preceding segment and " + "next segment "
251 + intersection);
252 }
253 tempPoints.add(intersection);
254 }
255 // tempPoints.set(tempPoints.size() - 1, intermediatePoint);
256 if (tempPoints.size() > 1)
257 {
258 // System.out.println(new OTSLine3D(tempPoints).toPlot());
259 }
260 }
261 }
262 else
263 {
264 if (debugOffsetLine)
265 {
266 System.out.println("#Not adding intersection of preceding segment and this segment "
267 + "(angle too small)");
268 }
269 if (i == tempPoints.size() - 1)
270 {
271 if (debugOffsetLine)
272 {
273 System.out.println("#Not adding segment");
274 }
275 addSegment = false;
276 }
277 }
278 }
279 pPoint = p;
280 }
281 }
282 if (addSegment)
283 {
284 tempPoints.add(segmentFrom);
285 tempPoints.add(segmentTo);
286 if (debugOffsetLine)
287 {
288 System.out.println("#Added segmentFrom " + segmentFrom + " and segmentTo " + segmentTo);
289 System.out.println(new OTSLine3D(tempPoints).toPlot());
290 }
291 prevPoint = nextPoint;
292 prevAngle = angle;
293 }
294 }
295 if (debugOffsetLine)
296 {
297 System.out.println("# before cleanup: \nc1,0,0\n");
298 if (tempPoints.size() > 0)
299 {
300 OTSPoint3D p = tempPoints.get(0);
301 System.out.println(String.format(Locale.US, "M %.3f,%.3f", p.x, p.y));
302 for (int i = 1; i < tempPoints.size(); i++)
303 {
304 p = tempPoints.get(i);
305 System.out.println(String.format(Locale.US, "L %.3f,%.3f", p.x, p.y));
306 }
307 }
308 }
309 // Remove points that are closer than the specified offset
310 for (int index = 1; index < tempPoints.size() - 1; index++)
311 {
312 OTSPoint3D checkPoint = tempPoints.get(index);
313 prevPoint = null;
314 boolean tooClose = false;
315 boolean somewhereAtCorrectDistance = false;
316 for (int i = 0; i < filteredReferenceLine.size(); i++)
317 {
318 OTSPoint3D p = filteredReferenceLine.get(i);
319 if (null != prevPoint)
320 {
321 OTSPoint3D closestPoint = checkPoint.closestPointOnSegment(prevPoint, p);
322 double distance = closestPoint.horizontalDistanceSI(checkPoint);
323 if (distance < bufferOffset - circlePrecision)
324 {
325 if (debugOffsetLine)
326 {
327 System.out.print("#point " + checkPoint + " inside buffer (distance is " + distance + ") ");
328 }
329 tooClose = true;
330 break;
331 }
332 else if (distance < bufferOffset + precision)
333 {
334 somewhereAtCorrectDistance = true;
335 }
336 }
337 prevPoint = p;
338 }
339 if (tooClose || !somewhereAtCorrectDistance)
340 {
341 if (debugOffsetLine)
342 {
343 System.out.println("#Removing " + checkPoint);
344 }
345 tempPoints.remove(index);
346 index--;
347 }
348 }
349 if (debugOffsetLine)
350 {
351 System.out.println("#after cleanup " + tempPoints.size() + " points left");
352 System.out.println(new OTSLine3D(tempPoints).toPlot());
353 }
354 // Fix the z-coordinate of all points that were added as intersections of segments.
355 for (int index = 0; index < tempPoints.size(); index++)
356 {
357 OTSPoint3D p = tempPoints.get(index);
358 if (Double.isNaN(p.z))
359 {
360 tempPoints.set(index, new OTSPoint3D(p.x, p.y, 0));
361 }
362 }
363 try
364 {
365 return OTSLine3D.createAndCleanOTSLine3D(tempPoints);
366 }
367 catch (OTSGeometryException exception)
368 {
369 exception.printStackTrace();
370 }
371 return null;
372 }
373 }