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