1 package org.opentrafficsim.core.geometry;
2
3 import java.util.ArrayList;
4 import java.util.List;
5
6 import org.djutils.logger.CategoryLogger;
7 import org.locationtech.jts.geom.Coordinate;
8 import org.locationtech.jts.geom.Geometry;
9 import org.locationtech.jts.linearref.LengthIndexedLine;
10
11 /**
12 * <p>
13 * Copyright (c) 2013-2023 Delft University of Technology, PO Box 5, 2600 AA, Delft, the Netherlands. All rights reserved. <br>
14 * BSD-style license. See <a href="https://opentrafficsim.org/docs/license.html">OpenTrafficSim License</a>.
15 * </p>
16 * @author <a href="https://github.com/averbraeck">Alexander Verbraeck</a>
17 * @author <a href="https://tudelft.nl/staff/p.knoppers-1">Peter Knoppers</a>
18 */
19 public final class OtsBufferingJts
20 {
21 /**
22 * Utility class.
23 */
24 private OtsBufferingJts()
25 {
26 // cannot be instantiated.
27 }
28
29 /**
30 * Compute the distance of a line segment to a point. If the the projected points lies outside the line segment, the nearest
31 * end point of the line segment is returned. Otherwise the point return lies between the end points of the line segment.
32 * <br>
33 * Adapted from <a href="http://paulbourke.net/geometry/pointlineplane/DistancePoint.java"> example code provided by Paul
34 * Bourke</a>.
35 * @param lineP1 OtsPoint3d; start of line segment
36 * @param lineP2 OtsPoint3d; end of line segment
37 * @param point OtsPoint3d; Point to project onto the line segment
38 * @return double; the distance of the projected point or one of the end points of the line segment to the point
39 */
40 public static double distanceLineSegmentToPoint(final OtsPoint3d lineP1, final OtsPoint3d lineP2, final OtsPoint3d point)
41 {
42 return closestPointOnSegmentToPoint(lineP1, lineP2, point).distanceSI(point);
43 }
44
45 /**
46 * Project a point on a line (2D). If the the projected points lies outside the line segment, the nearest end point of the
47 * line segment is returned. Otherwise the point return lies between the end points of the line segment. <br>
48 * Adapted from <a href="http://paulbourke.net/geometry/pointlineplane/DistancePoint.java"> example code provided by Paul
49 * Bourke</a>.
50 * @param lineP1 OtsPoint3d; start of line segment
51 * @param lineP2 OtsPoint3d; end of line segment
52 * @param point OtsPoint3d; Point to project onto the line segment
53 * @return Point2D.Double; either <cite>lineP1</cite>, or <cite>lineP2</cite> or a new OtsPoint3d that lies somewhere in
54 * between those two
55 */
56 public static OtsPoint3d closestPointOnSegmentToPoint(final OtsPoint3d lineP1, final OtsPoint3d lineP2,
57 final OtsPoint3d point)
58 {
59 double dX = lineP2.x - lineP1.x;
60 double dY = lineP2.y - lineP1.y;
61 if ((0 == dX) && (0 == dY))
62 {
63 return lineP1;
64 }
65 final double u = ((point.x - lineP1.x) * dX + (point.y - lineP1.y) * dY) / (dX * dX + dY * dY);
66 if (u < 0)
67 {
68 return lineP1;
69 }
70 else if (u > 1)
71 {
72 return lineP2;
73 }
74 else
75 {
76 return new OtsPoint3d(lineP1.x + u * dX, lineP1.y + u * dY); // could use interpolate in stead
77 }
78 }
79
80 /**
81 * Construct parallel line without.
82 * @param referenceLine OtsLine3d; the reference line
83 * @param offset double; offset distance from the reference line; positive is LEFT, negative is RIGHT
84 * @return OtsLine3d; the line that has the specified offset from the reference line
85 */
86 public static OtsLine3d offsetLine(final OtsLine3d referenceLine, final double offset)
87 {
88 try
89 {
90 double bufferOffset = Math.abs(offset);
91 final double precision = 0.00001;
92 if (bufferOffset < precision)
93 {
94 return referenceLine; // It is immutable; so we can safely return the original
95 }
96 final double circlePrecision = 0.001;
97 List<OtsPoint3d> points = new ArrayList<>();
98 // Make good use of the fact that an OtsLine3d cannot have consecutive duplicate points and has > 1 points
99 OtsPoint3d prevPoint = referenceLine.get(0);
100 Double prevAngle = null;
101 for (int index = 0; index < referenceLine.size() - 1; index++)
102 {
103 OtsPoint3d nextPoint = referenceLine.get(index + 1);
104 double angle = Math.atan2(nextPoint.y - prevPoint.y, nextPoint.x - prevPoint.x);
105 OtsPoint3d segmentFrom =
106 new OtsPoint3d(prevPoint.x - Math.sin(angle) * offset, prevPoint.y + Math.cos(angle) * offset);
107 OtsPoint3d segmentTo =
108 new OtsPoint3d(nextPoint.x - Math.sin(angle) * offset, nextPoint.y + Math.cos(angle) * offset);
109 if (index > 0)
110 {
111 double deltaAngle = angle - prevAngle;
112 if (Math.abs(deltaAngle) > Math.PI)
113 {
114 deltaAngle -= Math.signum(deltaAngle) * 2 * Math.PI;
115 }
116 if (deltaAngle * offset > 0)
117 {
118 // Inside of curve of reference line.
119 // Add the intersection point of each previous segment and the next segment
120 OtsPoint3d pPoint = null;
121 for (int i = 0; i < points.size(); i++)
122 {
123 OtsPoint3d p = points.get(i);
124 if (Double.isNaN(p.z))
125 {
126 continue; // skip this one
127 }
128 if (null != pPoint)
129 {
130 double pAngle = Math.atan2(p.y - pPoint.y, p.x - pPoint.x);
131 double totalAngle = angle - pAngle;
132 if (Math.abs(totalAngle) > Math.PI)
133 {
134 totalAngle += Math.signum(totalAngle) * 2 * Math.PI;
135 }
136 if (Math.abs(totalAngle) > 0.01)
137 {
138 // CategoryLogger.trace(Cat.CORE, "preceding segment " + pPoint + " to " + p + ", this
139 // segment "
140 // + segmentFrom + " to " + segmentTo + " totalAngle " + totalAngle);
141 OtsPoint3d intermediatePoint =
142 intersectionOfLineSegments(pPoint, p, segmentFrom, segmentTo);
143 if (null != intermediatePoint)
144 {
145 // mark it as added point at inside corner
146 intermediatePoint =
147 new OtsPoint3d(intermediatePoint.x, intermediatePoint.y, Double.NaN);
148 // CategoryLogger.trace(Cat.CORE, "Inserting intersection of preceding segment and this
149 // "
150 // + "segment " + intermediatePoint);
151 points.add(intermediatePoint);
152 }
153 }
154 }
155 pPoint = p;
156 }
157 }
158 else
159 {
160 // Outside of curve of reference line
161 // Approximate an arc using straight segments.
162 // Determine how many segments are needed.
163 int numSegments = 1;
164 if (Math.abs(deltaAngle) > Math.PI / 2)
165 {
166 numSegments = 2;
167 }
168 for (; numSegments < 1000; numSegments *= 2)
169 {
170 double maxError = bufferOffset * (1 - Math.abs(Math.cos(deltaAngle / numSegments / 2)));
171 if (maxError < circlePrecision)
172 {
173 break; // required precision reached
174 }
175 }
176 // Generate the intermediate points
177 for (int additionalPoint = 1; additionalPoint < numSegments; additionalPoint++)
178 {
179 double intermediateAngle =
180 (additionalPoint * angle + (numSegments - additionalPoint) * prevAngle) / numSegments;
181 if (prevAngle * angle < 0 && Math.abs(prevAngle) > Math.PI / 2 && Math.abs(angle) > Math.PI / 2)
182 {
183 intermediateAngle += Math.PI;
184 }
185 OtsPoint3d intermediatePoint = new OtsPoint3d(prevPoint.x - Math.sin(intermediateAngle) * offset,
186 prevPoint.y + Math.cos(intermediateAngle) * offset);
187 // CategoryLogger.trace(Cat.CORE, "inserting intermediate point " + intermediatePoint + " for angle
188 // "
189 // + Math.toDegrees(intermediateAngle));
190 points.add(intermediatePoint);
191 }
192 }
193 }
194 points.add(segmentFrom);
195 points.add(segmentTo);
196 prevPoint = nextPoint;
197 prevAngle = angle;
198 }
199 // CategoryLogger.trace(Cat.CORE, OTSGeometry.printCoordinates("#before cleanup: \nc0,0,0\n#", new
200 // OtsLine3d(points), "\n
201 // "));
202 // Remove points that are closer than the specified offset
203 for (int index = 1; index < points.size() - 1; index++)
204 {
205 OtsPoint3d checkPoint = points.get(index);
206 prevPoint = null;
207 boolean tooClose = false;
208 boolean somewhereAtCorrectDistance = false;
209 for (int i = 0; i < referenceLine.size(); i++)
210 {
211 OtsPoint3d p = referenceLine.get(i);
212 if (null != prevPoint)
213 {
214 OtsPoint3d closestPoint = closestPointOnSegmentToPoint(prevPoint, p, checkPoint);
215 if (closestPoint != referenceLine.get(0) && closestPoint != referenceLine.get(referenceLine.size() - 1))
216 {
217 double distance = closestPoint.horizontalDistanceSI(checkPoint);
218 if (distance < bufferOffset - circlePrecision)
219 {
220 // CategoryLogger.trace(Cat.CORE, "point " + checkPoint + " inside buffer (distance is " +
221 // distance +
222 // ")");
223 tooClose = true;
224 break;
225 }
226 else if (distance < bufferOffset + precision)
227 {
228 somewhereAtCorrectDistance = true;
229 }
230 }
231 }
232 prevPoint = p;
233 }
234 if (tooClose || !somewhereAtCorrectDistance)
235 {
236 // CategoryLogger.trace(Cat.CORE, "Removing " + checkPoint);
237 points.remove(index);
238 index--;
239 }
240 }
241 // Fix the z-coordinate of all points that were added as intersections of segments.
242 for (int index = 0; index < points.size(); index++)
243 {
244 OtsPoint3d p = points.get(index);
245 if (Double.isNaN(p.z))
246 {
247 points.set(index, new OtsPoint3d(p.x, p.y, 0));
248 }
249 }
250 return OtsLine3d.createAndCleanOtsLine3d(points);
251 }
252 catch (OtsGeometryException exception)
253 {
254 CategoryLogger.always().error(exception, "Exception in offsetLine - should never happen");
255 return null;
256 }
257 }
258
259 /**
260 * Compute the 2D intersection of two line segments. Both line segments are defined by two points (that should be distinct).
261 * @param line1P1 OtsPoint3d; first point of line 1
262 * @param line1P2 OtsPoint3d; second point of line 1
263 * @param line2P1 OtsPoint3d; first point of line 2
264 * @param line2P2 OtsPoint3d; second point of line 2
265 * @return OtsPoint3d; the intersection of the two lines, or null if the lines are (almost) parallel, or do not intersect
266 */
267 private static OtsPoint3d intersectionOfLineSegments(final OtsPoint3d line1P1, final OtsPoint3d line1P2,
268 final OtsPoint3d line2P1, final OtsPoint3d line2P2)
269 {
270 double denominator =
271 (line2P2.y - line2P1.y) * (line1P2.x - line1P1.x) - (line2P2.x - line2P1.x) * (line1P2.y - line1P1.y);
272 if (denominator == 0f)
273 {
274 return null; // lines are parallel (they might even be on top of each other, but we don't check that)
275 }
276 double uA = ((line2P2.x - line2P1.x) * (line1P1.y - line2P1.y) - (line2P2.y - line2P1.y) * (line1P1.x - line2P1.x))
277 / denominator;
278 if ((uA < 0f) || (uA > 1f))
279 {
280 return null; // intersection outside line 1
281 }
282 double uB = ((line1P2.x - line1P1.x) * (line1P1.y - line2P1.y) - (line1P2.y - line1P1.y) * (line1P1.x - line2P1.x))
283 / denominator;
284 if (uB < 0 || uB > 1)
285 {
286 return null; // intersection outside line 2
287 }
288 return new OtsPoint3d(line1P1.x + uA * (line1P2.x - line1P1.x), line1P1.y + uA * (line1P2.y - line1P1.y), 0);
289 }
290
291 /**
292 * Create a line at linearly varying offset from a reference line. The offset may change linearly from its initial value at
293 * the start of the reference line to its final offset value at the end of the reference line.
294 * @param referenceLine OtsLine3d; the Geometry of the reference line
295 * @param offsetAtStart double; offset at the start of the reference line (positive value is Left, negative value is Right)
296 * @param offsetAtEnd double; offset at the end of the reference line (positive value is Left, negative value is Right)
297 * @return Geometry; the Geometry of the line at linearly changing offset of the reference line
298 * @throws OtsGeometryException when this method fails to create the offset line
299 */
300 public static OtsLine3d offsetLine(final OtsLine3d referenceLine, final double offsetAtStart, final double offsetAtEnd)
301 throws OtsGeometryException
302 {
303 // CategoryLogger.trace(Cat.CORE, OTSGeometry.printCoordinates("#referenceLine: \nc1,0,0\n# offset at start is " +
304 // offsetAtStart + " at end is " + offsetAtEnd + "\n#", referenceLine, "\n "));
305
306 OtsLine3d offsetLineAtStart = offsetLine(referenceLine, offsetAtStart);
307 if (offsetAtStart == offsetAtEnd)
308 {
309 return offsetLineAtStart; // offset does not change
310 }
311 // CategoryLogger.trace(Cat.CORE, OTSGeometry.printCoordinates("#offset line at start: \nc0,0,0\n#", offsetLineAtStart,
312 // "\n"));
313 OtsLine3d offsetLineAtEnd = offsetLine(referenceLine, offsetAtEnd);
314 // CategoryLogger.trace(Cat.CORE, OTSGeometry.printCoordinates("#offset line at end: \nc0.7,0.7,0.7\n#",
315 // offsetLineAtEnd,
316 // "\n"));
317 Geometry startGeometry = offsetLineAtStart.getLineString();
318 Geometry endGeometry = offsetLineAtEnd.getLineString();
319 LengthIndexedLine first = new LengthIndexedLine(startGeometry);
320 double firstLength = startGeometry.getLength();
321 LengthIndexedLine second = new LengthIndexedLine(endGeometry);
322 double secondLength = endGeometry.getLength();
323 ArrayList<Coordinate> out = new ArrayList<Coordinate>();
324 Coordinate[] firstCoordinates = startGeometry.getCoordinates();
325 Coordinate[] secondCoordinates = endGeometry.getCoordinates();
326 int firstIndex = 0;
327 int secondIndex = 0;
328 Coordinate prevCoordinate = null;
329 final double tooClose = 0.05; // 5 cm
330 while (firstIndex < firstCoordinates.length && secondIndex < secondCoordinates.length)
331 {
332 double firstRatio = firstIndex < firstCoordinates.length ? first.indexOf(firstCoordinates[firstIndex]) / firstLength
333 : Double.MAX_VALUE;
334 double secondRatio = secondIndex < secondCoordinates.length
335 ? second.indexOf(secondCoordinates[secondIndex]) / secondLength : Double.MAX_VALUE;
336 double ratio;
337 if (firstRatio < secondRatio)
338 {
339 ratio = firstRatio;
340 firstIndex++;
341 }
342 else
343 {
344 ratio = secondRatio;
345 secondIndex++;
346 }
347 Coordinate firstCoordinate = first.extractPoint(ratio * firstLength);
348 Coordinate secondCoordinate = second.extractPoint(ratio * secondLength);
349 Coordinate resultCoordinate = new Coordinate((1 - ratio) * firstCoordinate.x + ratio * secondCoordinate.x,
350 (1 - ratio) * firstCoordinate.y + ratio * secondCoordinate.y);
351 if (null == prevCoordinate || resultCoordinate.distance(prevCoordinate) > tooClose)
352 {
353 out.add(resultCoordinate);
354 prevCoordinate = resultCoordinate;
355 }
356 }
357 Coordinate[] resultCoordinates = new Coordinate[out.size()];
358 for (int index = 0; index < out.size(); index++)
359 {
360 resultCoordinates[index] = out.get(index);
361 }
362 return new OtsLine3d(resultCoordinates);
363 }
364
365 }