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
9
10
11
12
13
14
15
16 public final class OTSOffsetLinePK
17 {
18
19 private OTSOffsetLinePK()
20 {
21
22 }
23
24
25 static boolean debugOffsetLine = false;
26
27
28 static double circlePrecision = 0.001;
29
30
31 static double offsetMinimumFilterValue = 0.001;
32
33
34 static double offsetMaximumFilterValue = 0.1;
35
36
37
38
39
40 static double offsetFilterRatio = 10;
41
42
43
44
45
46
47
48
49
50 public static OTSLine3D offsetLine(final OTSLine3D referenceLine, final double offset) throws OTSGeometryException
51 {
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81 double bufferOffset = Math.abs(offset);
82 final double precision = 0.00001;
83 if (bufferOffset < precision)
84 {
85
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 List<OTSPoint3D> tempPoints = new ArrayList<>();
98
99 OTSPoint3D prevPoint = filteredReferenceLine.get(0);
100 Double prevAngle = null;
101 for (int index = 0; index < filteredReferenceLine.size() - 1; index++)
102 {
103 OTSPoint3D nextPoint = filteredReferenceLine.get(index + 1);
104 double angle = Math.atan2(nextPoint.y - prevPoint.y, nextPoint.x - prevPoint.x);
105 if (debugOffsetLine)
106 {
107 System.out.println("#reference segment " + prevPoint + " to " + nextPoint + " angle " + Math.toDegrees(angle));
108 }
109 OTSPoint3D segmentFrom =
110 new OTSPoint3D(prevPoint.x - Math.sin(angle) * offset, prevPoint.y + Math.cos(angle) * offset);
111 OTSPoint3D segmentTo =
112 new OTSPoint3D(nextPoint.x - Math.sin(angle) * offset, nextPoint.y + Math.cos(angle) * offset);
113 boolean addSegment = true;
114 if (index > 0)
115 {
116 double deltaAngle = angle - prevAngle;
117 if (Math.abs(deltaAngle) > Math.PI)
118 {
119 deltaAngle -= Math.signum(deltaAngle) * 2 * Math.PI;
120 }
121 if (deltaAngle * offset <= 0)
122 {
123
124
125
126 int numSegments = 1;
127 if (Math.abs(deltaAngle) > Math.PI / 2)
128 {
129 numSegments = 2;
130 }
131 while (true)
132 {
133 double maxError = bufferOffset * (1 - Math.abs(Math.cos(deltaAngle / numSegments / 2)));
134 if (maxError < circlePrecision)
135 {
136 break;
137 }
138 numSegments *= 2;
139 }
140 OTSPoint3D prevArcPoint = tempPoints.get(tempPoints.size() - 1);
141
142 for (int additionalPoint = 1; additionalPoint < numSegments; additionalPoint++)
143 {
144 double intermediateAngle =
145 (additionalPoint * angle + (numSegments - additionalPoint) * prevAngle) / numSegments;
146 if (prevAngle * angle < 0 && Math.abs(prevAngle) > Math.PI / 2 && Math.abs(angle) > Math.PI / 2)
147 {
148 intermediateAngle += Math.PI;
149 }
150 OTSPoint3D intermediatePoint = new OTSPoint3D(prevPoint.x - Math.sin(intermediateAngle) * offset,
151 prevPoint.y + Math.cos(intermediateAngle) * offset);
152
153 OTSPoint3D prevSegFrom = null;
154 int stopAt = tempPoints.size();
155 for (int i = 0; i < stopAt; i++)
156 {
157 OTSPoint3D prevSegTo = tempPoints.get(i);
158 if (null != prevSegFrom)
159 {
160 OTSPoint3D prevSegIntersection = OTSPoint3D.intersectionOfLineSegments(prevArcPoint,
161 intermediatePoint, prevSegFrom, prevSegTo);
162 if (null != prevSegIntersection
163 && prevSegIntersection.horizontalDistanceSI(prevArcPoint) > circlePrecision
164 && prevSegIntersection.horizontalDistanceSI(prevSegFrom) > circlePrecision
165 && prevSegIntersection.horizontalDistanceSI(prevSegTo) > circlePrecision)
166 {
167 if (debugOffsetLine)
168 {
169 System.out.println("#inserting intersection in arc segment " + prevSegIntersection);
170 }
171 tempPoints.add(prevSegIntersection);
172 }
173 }
174 prevSegFrom = prevSegTo;
175 }
176 OTSPoint3D nextSegmentIntersection =
177 OTSPoint3D.intersectionOfLineSegments(prevSegFrom, intermediatePoint, segmentFrom, segmentTo);
178 if (null != nextSegmentIntersection)
179 {
180 if (debugOffsetLine)
181 {
182 System.out.println(
183 "#inserting intersection of arc segment with next segment " + nextSegmentIntersection);
184 }
185 tempPoints.add(nextSegmentIntersection);
186 }
187 if (debugOffsetLine)
188 {
189 System.out.println("#inserting arc point " + intermediatePoint + " for angle "
190 + Math.toDegrees(intermediateAngle));
191 }
192 tempPoints.add(intermediatePoint);
193 prevArcPoint = intermediatePoint;
194 }
195 }
196
197
198 OTSPoint3D pPoint = null;
199 for (int i = 0; i < tempPoints.size(); i++)
200 {
201 OTSPoint3D p = tempPoints.get(i);
202 if (null != pPoint)
203 {
204 double pAngle = Math.atan2(p.y - pPoint.y, p.x - pPoint.x);
205 double angleDifference = angle - pAngle;
206 if (Math.abs(angleDifference) > Math.PI)
207 {
208 angleDifference += Math.signum(angleDifference) * 2 * Math.PI;
209 }
210 if (debugOffsetLine)
211 {
212 System.out.println("#preceding segment " + pPoint + " to " + p + ", next segment " + segmentFrom
213 + " to " + segmentTo + " angleDifference " + Math.toDegrees(angleDifference));
214 }
215 if (Math.abs(angleDifference) > 0)
216 {
217 OTSPoint3D intersection = OTSPoint3D.intersectionOfLineSegments(pPoint, p, segmentFrom, segmentTo);
218 if (null != intersection)
219 {
220 if (tempPoints.size() - 1 == i)
221 {
222 if (debugOffsetLine)
223 {
224 System.out.println("#Replacing last point of preceding segment and "
225 + "first point of next segment by their intersection " + intersection);
226 }
227 tempPoints.remove(tempPoints.size() - 1);
228 segmentFrom = intersection;
229 }
230 else
231 {
232 if (debugOffsetLine)
233 {
234 System.out.println("#Adding intersection of preceding segment and " + "next segment "
235 + intersection);
236 }
237 tempPoints.add(intersection);
238 }
239
240 }
241 }
242 else
243 {
244 if (debugOffsetLine)
245 {
246 System.out.println("#Not adding intersection of preceding segment and this segment "
247 + "(angle too small)");
248 }
249 if (i == tempPoints.size() - 1)
250 {
251 if (debugOffsetLine)
252 {
253 System.out.println("#Not adding segment");
254 }
255 addSegment = false;
256 }
257 }
258 }
259 pPoint = p;
260 }
261 }
262 if (addSegment)
263 {
264 if (debugOffsetLine)
265 {
266 System.out.println("#Adding segmentFrom " + segmentFrom);
267 }
268 tempPoints.add(segmentFrom);
269 if (debugOffsetLine)
270 {
271 System.out.println("#Adding segmentTo " + segmentTo);
272 }
273 tempPoints.add(segmentTo);
274 prevPoint = nextPoint;
275 prevAngle = angle;
276 }
277 }
278 if (debugOffsetLine)
279 {
280 System.out.println("# before cleanup: \nc1,0,0\n");
281 if (tempPoints.size() > 0)
282 {
283 OTSPoint3D p = tempPoints.get(0);
284 System.out.println(String.format(Locale.US, "M %.3f,%.3f", p.x, p.y));
285 for (int i = 1; i < tempPoints.size(); i++)
286 {
287 p = tempPoints.get(i);
288 System.out.println(String.format(Locale.US, "L %.3f,%.3f", p.x, p.y));
289 }
290 }
291 }
292
293 for (int index = 1; index < tempPoints.size() - 1; index++)
294 {
295 OTSPoint3D checkPoint = tempPoints.get(index);
296 prevPoint = null;
297 boolean tooClose = false;
298 boolean somewhereAtCorrectDistance = false;
299 for (int i = 0; i < filteredReferenceLine.size(); i++)
300 {
301 OTSPoint3D p = filteredReferenceLine.get(i);
302 if (null != prevPoint)
303 {
304 OTSPoint3D closestPoint = checkPoint.closestPointOnSegment(prevPoint, p);
305 double distance = closestPoint.horizontalDistanceSI(checkPoint);
306 if (distance < bufferOffset - circlePrecision)
307 {
308 if (debugOffsetLine)
309 {
310 System.out.print("#point " + checkPoint + " inside buffer (distance is " + distance + ") ");
311 }
312 tooClose = true;
313 break;
314 }
315 else if (distance < bufferOffset + precision)
316 {
317 somewhereAtCorrectDistance = true;
318 }
319 }
320 prevPoint = p;
321 }
322 if (tooClose || !somewhereAtCorrectDistance)
323 {
324 if (debugOffsetLine)
325 {
326 System.out.println("#Removing " + checkPoint);
327 }
328 tempPoints.remove(index);
329 index--;
330 }
331 }
332 if (debugOffsetLine)
333 {
334 System.out.println("#after cleanup " + tempPoints.size() + " points left");
335 }
336
337 for (int index = 0; index < tempPoints.size(); index++)
338 {
339 OTSPoint3D p = tempPoints.get(index);
340 if (Double.isNaN(p.z))
341 {
342 tempPoints.set(index, new OTSPoint3D(p.x, p.y, 0));
343 }
344 }
345 try
346 {
347 return OTSLine3D.createAndCleanOTSLine3D(tempPoints);
348 }
349 catch (OTSGeometryException exception)
350 {
351 exception.printStackTrace();
352 }
353 return null;
354 }
355 }