1 package org.opentrafficsim.core.geometry;
2
3 import java.awt.geom.Line2D;
4 import java.util.ArrayList;
5 import java.util.List;
6
7 import org.djutils.logger.CategoryLogger;
8 import org.locationtech.jts.geom.Coordinate;
9 import org.locationtech.jts.geom.Geometry;
10 import org.locationtech.jts.linearref.LengthIndexedLine;
11 import org.locationtech.jts.operation.buffer.BufferParameters;
12 import org.opentrafficsim.base.logger.Cat;
13 import org.opentrafficsim.core.network.NetworkException;
14
15
16
17
18
19
20
21
22
23
24
25 public final class OTSBufferingJTS
26 {
27
28 private static final int QUADRANTSEGMENTS = 16;
29
30
31
32
33 private OTSBufferingJTS()
34 {
35
36 }
37
38
39
40
41
42
43 private static double norm(final double angle)
44 {
45 double normalized = angle % (2 * Math.PI);
46 if (normalized < 0.0)
47 {
48 normalized += 2 * Math.PI;
49 }
50 return normalized;
51 }
52
53
54
55
56
57
58 private static double angle(final Coordinate c1, final Coordinate c2)
59 {
60 return norm(Math.atan2(c2.y - c1.y, c2.x - c1.x));
61 }
62
63
64
65
66
67
68
69
70
71
72
73
74 public static double distanceLineSegmentToPoint(final OTSPoint3DOTSPoint3D.html#OTSPoint3D">OTSPoint3DOTSPoint3D.html#OTSPoint3D">OTSPoint3D lineP1, final OTSPoint3DOTSPoint3D.html#OTSPoint3D">OTSPoint3D lineP2, final OTSPoint3D point)
75 {
76 return closestPointOnSegmentToPoint(lineP1, lineP2, point).distanceSI(point);
77 }
78
79
80
81
82
83
84
85
86
87
88
89
90 public static OTSPoint3DOTSPoint3D.html#OTSPoint3D">OTSPoint3Dint3D">OTSPoint3D closestPointOnSegmentToPoint(final OTSPoint3DOTSPoint3D.html#OTSPoint3D">OTSPoint3D lineP1, final OTSPoint3D lineP2,
91 final OTSPoint3D point)
92 {
93 double dX = lineP2.x - lineP1.x;
94 double dY = lineP2.y - lineP1.y;
95 if ((0 == dX) && (0 == dY))
96 {
97 return lineP1;
98 }
99 final double u = ((point.x - lineP1.x) * dX + (point.y - lineP1.y) * dY) / (dX * dX + dY * dY);
100 if (u < 0)
101 {
102 return lineP1;
103 }
104 else if (u > 1)
105 {
106 return lineP2;
107 }
108 else
109 {
110 return new OTSPoint3D(lineP1.x + u * dX, lineP1.y + u * dY);
111 }
112 }
113
114
115
116
117
118
119
120 public static OTSLine3DSLine3D.html#OTSLine3D">OTSLine3D offsetLine(final OTSLine3D referenceLine, final double offset)
121 {
122 try
123 {
124 double bufferOffset = Math.abs(offset);
125 final double precision = 0.00001;
126 if (bufferOffset < precision)
127 {
128 return referenceLine;
129 }
130 final double circlePrecision = 0.001;
131 List<OTSPoint3D> points = new ArrayList<>();
132
133 OTSPoint3D prevPoint = referenceLine.get(0);
134 Double prevAngle = null;
135 for (int index = 0; index < referenceLine.size() - 1; index++)
136 {
137 OTSPoint3D nextPoint = referenceLine.get(index + 1);
138 double angle = Math.atan2(nextPoint.y - prevPoint.y, nextPoint.x - prevPoint.x);
139 OTSPoint3D segmentFrom =
140 new OTSPoint3D(prevPoint.x - Math.sin(angle) * offset, prevPoint.y + Math.cos(angle) * offset);
141 OTSPoint3D segmentTo =
142 new OTSPoint3D(nextPoint.x - Math.sin(angle) * offset, nextPoint.y + Math.cos(angle) * offset);
143 if (index > 0)
144 {
145 double deltaAngle = angle - prevAngle;
146 if (Math.abs(deltaAngle) > Math.PI)
147 {
148 deltaAngle -= Math.signum(deltaAngle) * 2 * Math.PI;
149 }
150 if (deltaAngle * offset > 0)
151 {
152
153
154 OTSPoint3D pPoint = null;
155 for (int i = 0; i < points.size(); i++)
156 {
157 OTSPoint3D p = points.get(i);
158 if (Double.isNaN(p.z))
159 {
160 continue;
161 }
162 if (null != pPoint)
163 {
164 double pAngle = Math.atan2(p.y - pPoint.y, p.x - pPoint.x);
165 double totalAngle = angle - pAngle;
166 if (Math.abs(totalAngle) > Math.PI)
167 {
168 totalAngle += Math.signum(totalAngle) * 2 * Math.PI;
169 }
170 if (Math.abs(totalAngle) > 0.01)
171 {
172
173
174
175 OTSPoint3D intermediatePoint =
176 intersectionOfLineSegments(pPoint, p, segmentFrom, segmentTo);
177 if (null != intermediatePoint)
178 {
179
180 intermediatePoint =
181 new OTSPoint3D(intermediatePoint.x, intermediatePoint.y, Double.NaN);
182
183
184
185 points.add(intermediatePoint);
186 }
187 }
188 }
189 pPoint = p;
190 }
191 }
192 else
193 {
194
195
196
197 int numSegments = 1;
198 if (Math.abs(deltaAngle) > Math.PI / 2)
199 {
200 numSegments = 2;
201 }
202 for (; numSegments < 1000; numSegments *= 2)
203 {
204 double maxError = bufferOffset * (1 - Math.abs(Math.cos(deltaAngle / numSegments / 2)));
205 if (maxError < circlePrecision)
206 {
207 break;
208 }
209 }
210
211 for (int additionalPoint = 1; additionalPoint < numSegments; additionalPoint++)
212 {
213 double intermediateAngle =
214 (additionalPoint * angle + (numSegments - additionalPoint) * prevAngle) / numSegments;
215 if (prevAngle * angle < 0 && Math.abs(prevAngle) > Math.PI / 2 && Math.abs(angle) > Math.PI / 2)
216 {
217 intermediateAngle += Math.PI;
218 }
219 OTSPoint3D.html#OTSPoint3D">OTSPoint3D intermediatePoint = new OTSPoint3D(prevPoint.x - Math.sin(intermediateAngle) * offset,
220 prevPoint.y + Math.cos(intermediateAngle) * offset);
221
222
223
224 points.add(intermediatePoint);
225 }
226 }
227 }
228 points.add(segmentFrom);
229 points.add(segmentTo);
230 prevPoint = nextPoint;
231 prevAngle = angle;
232 }
233
234
235
236
237 for (int index = 1; index < points.size() - 1; index++)
238 {
239 OTSPoint3D checkPoint = points.get(index);
240 prevPoint = null;
241 boolean tooClose = false;
242 boolean somewhereAtCorrectDistance = false;
243 for (int i = 0; i < referenceLine.size(); i++)
244 {
245 OTSPoint3D p = referenceLine.get(i);
246 if (null != prevPoint)
247 {
248 OTSPoint3D closestPoint = closestPointOnSegmentToPoint(prevPoint, p, checkPoint);
249 if (closestPoint != referenceLine.get(0) && closestPoint != referenceLine.get(referenceLine.size() - 1))
250 {
251 double distance = closestPoint.horizontalDistanceSI(checkPoint);
252 if (distance < bufferOffset - circlePrecision)
253 {
254
255
256
257 tooClose = true;
258 break;
259 }
260 else if (distance < bufferOffset + precision)
261 {
262 somewhereAtCorrectDistance = true;
263 }
264 }
265 }
266 prevPoint = p;
267 }
268 if (tooClose || !somewhereAtCorrectDistance)
269 {
270
271 points.remove(index);
272 index--;
273 }
274 }
275
276 for (int index = 0; index < points.size(); index++)
277 {
278 OTSPoint3D p = points.get(index);
279 if (Double.isNaN(p.z))
280 {
281 points.set(index, new OTSPoint3D(p.x, p.y, 0));
282 }
283 }
284 return OTSLine3D.createAndCleanOTSLine3D(points);
285 }
286 catch (OTSGeometryException exception)
287 {
288 CategoryLogger.always().error(exception, "Exception in offsetLine - should never happen");
289 return null;
290 }
291 }
292
293
294
295
296
297
298
299
300
301 private static OTSPoint3DTSPoint3D.html#OTSPoint3D">OTSPoint3DPoint3D">OTSPoint3D intersectionOfLineSegments(final OTSPoint3DTSPoint3D.html#OTSPoint3D">OTSPoint3D line1P1, final OTSPoint3D line1P2,
302 final OTSPoint3DTSPoint3D.html#OTSPoint3D">OTSPoint3D line2P1, final OTSPoint3D line2P2)
303 {
304 double denominator =
305 (line2P2.y - line2P1.y) * (line1P2.x - line1P1.x) - (line2P2.x - line2P1.x) * (line1P2.y - line1P1.y);
306 if (denominator == 0f)
307 {
308 return null;
309 }
310 double uA = ((line2P2.x - line2P1.x) * (line1P1.y - line2P1.y) - (line2P2.y - line2P1.y) * (line1P1.x - line2P1.x))
311 / denominator;
312 if ((uA < 0f) || (uA > 1f))
313 {
314 return null;
315 }
316 double uB = ((line1P2.x - line1P1.x) * (line1P1.y - line2P1.y) - (line1P2.y - line1P1.y) * (line1P1.x - line2P1.x))
317 / denominator;
318 if (uB < 0 || uB > 1)
319 {
320 return null;
321 }
322 return new OTSPoint3D(line1P1.x + uA * (line1P2.x - line1P1.x), line1P1.y + uA * (line1P2.y - line1P1.y), 0);
323 }
324
325
326
327
328
329
330
331
332 @SuppressWarnings("checkstyle:methodlength")
333 public static OTSLine3D.html#OTSLine3D">OTSLine3D offsetGeometryOLD(final OTSLine3D referenceLine, final double offset) throws OTSGeometryException
334 {
335 Coordinate[] referenceCoordinates = referenceLine.getCoordinates();
336
337 double bufferOffset = Math.abs(offset);
338 final double precision = 0.000001;
339 if (bufferOffset < precision)
340 {
341
342 return new OTSLine3D(referenceCoordinates);
343 }
344 Geometry geometryLine = referenceLine.getLineString();
345 Coordinate[] bufferCoordinates =
346 geometryLine.buffer(bufferOffset, QUADRANTSEGMENTS, BufferParameters.CAP_FLAT).getCoordinates();
347
348
349
350
351
352 Coordinate sC0 = referenceCoordinates[0];
353 Coordinate sC1 = referenceCoordinates[1];
354 Coordinate eCm1 = referenceCoordinates[referenceCoordinates.length - 1];
355 Coordinate eCm2 = referenceCoordinates[referenceCoordinates.length - 2];
356
357 double expectedStartAngle = norm(angle(sC0, sC1) + Math.signum(offset) * Math.PI / 2.0);
358 double expectedEndAngle = norm(angle(eCm2, eCm1) + Math.signum(offset) * Math.PI / 2.0);
359 Coordinate sExpected = new Coordinate(sC0.x + bufferOffset * Math.cos(expectedStartAngle),
360 sC0.y + bufferOffset * Math.sin(expectedStartAngle));
361 Coordinate eExpected = new Coordinate(eCm1.x + bufferOffset * Math.cos(expectedEndAngle),
362 eCm1.y + bufferOffset * Math.sin(expectedEndAngle));
363
364
365 double dS = Double.MAX_VALUE;
366 double dE = Double.MAX_VALUE;
367 int sIndex = -1;
368 int eIndex = -1;
369 for (int i = 0; i < bufferCoordinates.length; i++)
370 {
371 Coordinate c = bufferCoordinates[i];
372 double dsc = c.distance(sExpected);
373 double dec = c.distance(eExpected);
374 if (dsc < dS)
375 {
376 dS = dsc;
377 sIndex = i;
378 }
379 if (dec < dE)
380 {
381 dE = dec;
382 eIndex = i;
383 }
384 }
385
386 if (sIndex == -1)
387 {
388 throw new OTSGeometryException("offsetGeometry: startIndex not found for line " + referenceLine);
389 }
390 if (eIndex == -1)
391 {
392 throw new OTSGeometryException("offsetGeometry: endIndex not found for line " + referenceLine);
393 }
394 if (dS > 0.01)
395 {
396 CategoryLogger.filter(Cat.CORE).trace(referenceLine.toExcel() + "\n\n\n\n"
397 + new OTSLine3D(bufferCoordinates).toExcel() + "\n\n\n\n" + sExpected + "\n" + eExpected);
398 throw new OTSGeometryException("offsetGeometry: startDistance too big (" + dS + ") for line " + referenceLine);
399 }
400 if (dE > 0.01)
401 {
402 throw new OTSGeometryException("offsetGeometry: endDistance too big (" + dE + ") for line " + referenceLine);
403 }
404
405
406 boolean ok = true;
407 int i = sIndex;
408 Coordinate lastC = null;
409 List<OTSPoint3D> result = new ArrayList<>();
410 while (ok)
411 {
412 Coordinate c = bufferCoordinates[i];
413 if (lastC != null && close(c, lastC, sC0, eCm1))
414 {
415 ok = false;
416 break;
417 }
418 result.add(new OTSPoint3D(c));
419 if (i == eIndex)
420 {
421 return OTSLine3D.createAndCleanOTSLine3D(result);
422 }
423 i = (i == bufferCoordinates.length - 1) ? 0 : i + 1;
424 lastC = c;
425 }
426
427
428 ok = true;
429 i = sIndex;
430 lastC = null;
431 result = new ArrayList<>();
432 while (ok)
433 {
434 Coordinate c = bufferCoordinates[i];
435 if (lastC != null && close(c, lastC, sC0, eCm1))
436 {
437 ok = false;
438 break;
439 }
440 result.add(new OTSPoint3D(c));
441 if (i == eIndex)
442 {
443 return OTSLine3D.createAndCleanOTSLine3D(result);
444 }
445 i = (i == 0) ? bufferCoordinates.length - 1 : i - 1;
446 lastC = c;
447 }
448
449
450
451 throw new OTSGeometryException("offsetGeometry: could not find offset in either direction for line " + referenceLine);
452 }
453
454
455
456
457
458
459
460
461 private static boolean close(final Coordinate lineC1, final Coordinate lineC2, final Coordinate... check)
462 {
463 Line2D.Double line = new Line2D.Double(lineC1.x, lineC1.y, lineC2.x, lineC2.y);
464 for (Coordinate c : check)
465 {
466 if (line.ptSegDist(c.x, c.y) < 0.01)
467 {
468 return true;
469 }
470 }
471 return false;
472 }
473
474
475
476
477
478
479
480
481
482
483 public static OTSLine3DSLine3D.html#OTSLine3D">OTSLine3D offsetLine(final OTSLine3D referenceLine, final double offsetAtStart, final double offsetAtEnd)
484 throws OTSGeometryException
485 {
486
487
488
489 OTSLine3D offsetLineAtStart = offsetLine(referenceLine, offsetAtStart);
490 if (offsetAtStart == offsetAtEnd)
491 {
492 return offsetLineAtStart;
493 }
494
495
496 OTSLine3D offsetLineAtEnd = offsetLine(referenceLine, offsetAtEnd);
497
498
499
500 Geometry startGeometry = offsetLineAtStart.getLineString();
501 Geometry endGeometry = offsetLineAtEnd.getLineString();
502 LengthIndexedLine first = new LengthIndexedLine(startGeometry);
503 double firstLength = startGeometry.getLength();
504 LengthIndexedLine second = new LengthIndexedLine(endGeometry);
505 double secondLength = endGeometry.getLength();
506 ArrayList<Coordinate> out = new ArrayList<Coordinate>();
507 Coordinate[] firstCoordinates = startGeometry.getCoordinates();
508 Coordinate[] secondCoordinates = endGeometry.getCoordinates();
509 int firstIndex = 0;
510 int secondIndex = 0;
511 Coordinate prevCoordinate = null;
512 final double tooClose = 0.05;
513 while (firstIndex < firstCoordinates.length && secondIndex < secondCoordinates.length)
514 {
515 double firstRatio = firstIndex < firstCoordinates.length ? first.indexOf(firstCoordinates[firstIndex]) / firstLength
516 : Double.MAX_VALUE;
517 double secondRatio = secondIndex < secondCoordinates.length
518 ? second.indexOf(secondCoordinates[secondIndex]) / secondLength : Double.MAX_VALUE;
519 double ratio;
520 if (firstRatio < secondRatio)
521 {
522 ratio = firstRatio;
523 firstIndex++;
524 }
525 else
526 {
527 ratio = secondRatio;
528 secondIndex++;
529 }
530 Coordinate firstCoordinate = first.extractPoint(ratio * firstLength);
531 Coordinate secondCoordinate = second.extractPoint(ratio * secondLength);
532 Coordinate resultCoordinate = new Coordinate((1 - ratio) * firstCoordinate.x + ratio * secondCoordinate.x,
533 (1 - ratio) * firstCoordinate.y + ratio * secondCoordinate.y);
534 if (null == prevCoordinate || resultCoordinate.distance(prevCoordinate) > tooClose)
535 {
536 out.add(resultCoordinate);
537 prevCoordinate = resultCoordinate;
538 }
539 }
540 Coordinate[] resultCoordinates = new Coordinate[out.size()];
541 for (int index = 0; index < out.size(); index++)
542 {
543 resultCoordinates[index] = out.get(index);
544 }
545 return new OTSLine3D(resultCoordinates);
546 }
547
548
549
550
551
552
553 public static void main(final String[] args) throws NetworkException, OTSGeometryException
554 {
555
556
557
558
559
560 OTSLine3Dtry/OTSLine3D.html#OTSLine3D">OTSLine3D line = new OTSLine3D(new OTSPoint3Dometry/OTSPoint3D.html#OTSPoint3D">OTSPoint3D[] {new OTSPoint3D(-579.253, 60.157, 4.710),
561 new OTSPoint3DOTSPoint3D">OTSPoint3D(-579.253, 60.144, 4.712), new OTSPoint3D(-579.253, 60.144, 0.000),
562 new OTSPoint3DOTSPoint3D">OTSPoint3D(-579.251, 60.044, 0.000), new OTSPoint3D(-579.246, 59.944, 0.000),
563 new OTSPoint3DOTSPoint3D">OTSPoint3D(-579.236, 59.845, 0.000), new OTSPoint3D(-579.223, 59.746, 0.000),
564 new OTSPoint3DOTSPoint3D">OTSPoint3D(-579.206, 59.647, 0.000), new OTSPoint3D(-579.185, 59.549, 0.000),
565 new OTSPoint3DOTSPoint3D">OTSPoint3D(-579.161, 59.452, 0.000), new OTSPoint3D(-579.133, 59.356, 0.000),
566 new OTSPoint3DOTSPoint3D">OTSPoint3D(-579.101, 59.261, 0.000), new OTSPoint3D(-579.066, 59.168, 0.000),
567 new OTSPoint3DOTSPoint3D">OTSPoint3D(-579.028, 59.075, 0.000), new OTSPoint3D(-578.986, 58.985, 0.000),
568 new OTSPoint3DOTSPoint3D">OTSPoint3D(-578.940, 58.896, 0.000), new OTSPoint3D(-578.891, 58.809, 0.000),
569 new OTSPoint3DOTSPoint3D">OTSPoint3D(-578.839, 58.723, 0.000), new OTSPoint3D(-578.784, 58.640, 0.000),
570 new OTSPoint3DOTSPoint3D">OTSPoint3D(-578.725, 58.559, 0.000), new OTSPoint3D(-578.664, 58.480, 0.000),
571 new OTSPoint3DOTSPoint3D">OTSPoint3D(-578.599, 58.403, 0.000), new OTSPoint3D(-578.532, 58.329, 0.000),
572 new OTSPoint3DOTSPoint3D">OTSPoint3D(-578.462, 58.258, 0.000), new OTSPoint3D(-578.390, 58.189, 0.000),
573 new OTSPoint3DOTSPoint3D">OTSPoint3D(-578.314, 58.123, 0.000), new OTSPoint3D(-578.237, 58.060, 0.000),
574 new OTSPoint3DOTSPoint3D">OTSPoint3D(-578.157, 58.000, 0.000), new OTSPoint3D(-578.075, 57.943, 0.000),
575 new OTSPoint3DOTSPoint3D">OTSPoint3D(-577.990, 57.889, 0.000), new OTSPoint3D(-577.904, 57.839, 0.000),
576 new OTSPoint3DOTSPoint3D">OTSPoint3D(-577.816, 57.791, 0.000), new OTSPoint3D(-577.726, 57.747, 0.000),
577 new OTSPoint3DOTSPoint3D">OTSPoint3D(-577.635, 57.707, 0.000), new OTSPoint3D(-577.542, 57.670, 0.000),
578 new OTSPoint3DOTSPoint3D">OTSPoint3D(-577.448, 57.636, 0.000), new OTSPoint3D(-577.352, 57.606, 0.000),
579 new OTSPoint3DOTSPoint3D">OTSPoint3D(-577.256, 57.580, 0.000), new OTSPoint3D(-577.159, 57.557, 0.000),
580 new OTSPoint3DOTSPoint3D">OTSPoint3D(-577.060, 57.538, 0.000), new OTSPoint3D(-576.962, 57.523, 0.000),
581 new OTSPoint3DOTSPoint3D">OTSPoint3D(-576.862, 57.512, 0.000), new OTSPoint3D(-576.763, 57.504, 0.000),
582 new OTSPoint3DOTSPoint3D">OTSPoint3D(-576.663, 57.500, 0.000), new OTSPoint3D(-576.623, 57.500, 6.278),
583 new OTSPoint3DOTSPoint3D">OTSPoint3D(-576.610, 57.500, 6.280), new OTSPoint3D(-567.499, 57.473, 6.280)});
584 System.out.println(line.toExcel());
585 System.out.println(OTSBufferingJTS.offsetGeometryOLD(line, -1.831));
586 }
587 }