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