1 package org.opentrafficsim.road.network.lane.conflict;
2
3 import java.util.ArrayList;
4 import java.util.Iterator;
5 import java.util.LinkedHashMap;
6 import java.util.List;
7 import java.util.Map;
8 import java.util.SortedSet;
9 import java.util.TreeSet;
10
11 import org.djunits.value.vdouble.scalar.Length;
12 import org.djutils.exceptions.Throw;
13 import org.djutils.immutablecollections.ImmutableIterator;
14 import org.djutils.immutablecollections.ImmutableMap;
15 import org.djutils.immutablecollections.ImmutableMap.ImmutableEntry;
16 import org.opentrafficsim.core.geometry.OTSGeometryException;
17 import org.opentrafficsim.core.geometry.OTSLine3D;
18 import org.opentrafficsim.core.geometry.OTSPoint3D;
19 import org.opentrafficsim.core.gtu.GTUDirectionality;
20 import org.opentrafficsim.core.gtu.GTUType;
21 import org.opentrafficsim.core.network.Link;
22 import org.opentrafficsim.core.network.NetworkException;
23 import org.opentrafficsim.road.network.OTSRoadNetwork;
24 import org.opentrafficsim.road.network.lane.CrossSectionElement;
25 import org.opentrafficsim.road.network.lane.CrossSectionLink;
26 import org.opentrafficsim.road.network.lane.Lane;
27
28 import nl.tudelft.simulation.dsol.logger.SimLogger;
29 import nl.tudelft.simulation.dsol.simulators.DEVSSimulatorInterface;
30
31
32
33
34
35
36
37
38
39
40
41
42 public final class ConflictBuilder
43 {
44
45
46 public static final WidthGenerator DEFAULT_WIDTH_GENERATOR = new RelativeWidthGenerator(0.8);
47
48
49
50
51 private ConflictBuilder()
52 {
53
54 }
55
56
57
58
59
60
61
62
63
64 public static void buildConflicts(final OTSRoadNetwork network, final GTUType gtuType,
65 final DEVSSimulatorInterface.TimeDoubleUnit simulator, final WidthGenerator widthGenerator)
66 throws OTSGeometryException
67 {
68 buildConflicts(network, gtuType, simulator, widthGenerator, new LaneCombinationListrk/lane/conflict/LaneCombinationList.html#LaneCombinationList">LaneCombinationList(), new LaneCombinationList());
69 }
70
71
72
73
74
75
76
77
78
79
80
81 public static void buildConflicts(final OTSRoadNetwork network, final GTUType gtuType,
82 final DEVSSimulatorInterface.TimeDoubleUnit simulator, final WidthGenerator widthGenerator,
83 final LaneCombinationListflict/LaneCombinationList.html#LaneCombinationList">LaneCombinationList ignoreList, final LaneCombinationList permittedList) throws OTSGeometryException
84 {
85
86 ImmutableMap<String, Link> links = network.getLinkMap();
87 List<Lane> lanes = new ArrayList<>();
88 for (String linkId : links.keySet())
89 {
90 Link link = links.get(linkId);
91 if (link instanceof CrossSectionLink)
92 {
93 for (CrossSectionElement element : ((CrossSectionLink) link).getCrossSectionElementList())
94 {
95 if (element instanceof Lane)
96 {
97 lanes.add((Lane) element);
98 }
99 }
100 }
101 }
102 buildConflicts(lanes, gtuType, simulator, widthGenerator, ignoreList, permittedList);
103 }
104
105
106
107
108
109
110
111
112
113 public static void buildConflicts(final List<Lane> lanes, final GTUType gtuType,
114 final DEVSSimulatorInterface.TimeDoubleUnit simulator, final WidthGenerator widthGenerator)
115 throws OTSGeometryException
116 {
117 buildConflicts(lanes, gtuType, simulator, widthGenerator, new LaneCombinationListrk/lane/conflict/LaneCombinationList.html#LaneCombinationList">LaneCombinationList(), new LaneCombinationList());
118 }
119
120
121
122
123
124
125
126
127
128
129
130 public static void buildConflicts(final List<Lane> lanes, final GTUType gtuType,
131 final DEVSSimulatorInterface.TimeDoubleUnit simulator, final WidthGenerator widthGenerator,
132 final LaneCombinationListflict/LaneCombinationList.html#LaneCombinationList">LaneCombinationList ignoreList, final LaneCombinationList permittedList) throws OTSGeometryException
133 {
134
135 long totalCombinations = ((long) lanes.size()) * ((long) lanes.size() - 1) / 2;
136 long lastReported = 0;
137 Map<Lane, OTSLine3D> leftEdges = new LinkedHashMap<>();
138 Map<Lane, OTSLine3D> rightEdges = new LinkedHashMap<>();
139
140 for (int i = 0; i < lanes.size(); i++)
141 {
142 long combinationsDone = totalCombinations - ((long) (lanes.size() - i)) * ((long) (lanes.size() - i)) / 2;
143 if (combinationsDone / 1000000 > lastReported)
144 {
145 SimLogger.always()
146 .debug(String.format("generating conflicts at %.2f%%", 100.0 * combinationsDone / totalCombinations));
147 lastReported = combinationsDone / 1000000;
148 }
149 Lane lane1 = lanes.get(i);
150 for (GTUDirectionality dir1 : lane1.getLaneType().getDirectionality(gtuType).getDirectionalities())
151 {
152 ImmutableMap<Lane, GTUDirectionality> down1 = lane1.downstreamLanes(dir1, gtuType);
153 ImmutableMap<Lane, GTUDirectionality> up1 = lane1.upstreamLanes(dir1, gtuType);
154
155 for (int j = i + 1; j < lanes.size(); j++)
156 {
157 Lane lane2 = lanes.get(j);
158 if (ignoreList.contains(lane1, lane2))
159 {
160 continue;
161 }
162 boolean permitted = permittedList.contains(lane1, lane2);
163
164 for (GTUDirectionality dir2 : lane2.getLaneType().getDirectionality(gtuType).getDirectionalities())
165 {
166 ImmutableMap<Lane, GTUDirectionality> down2 = lane2.downstreamLanes(dir2, gtuType);
167 ImmutableMap<Lane, GTUDirectionality> up2 = lane2.upstreamLanes(dir2, gtuType);
168
169 try
170 {
171 buildConflicts(lane1, dir1, down1, up1, lane2, dir2, down2, up2, gtuType, permitted, simulator,
172 widthGenerator, leftEdges, rightEdges);
173 }
174 catch (NetworkException ne)
175 {
176 throw new RuntimeException("Conflict build with bad combination of types / rules.", ne);
177 }
178 }
179 }
180 }
181 }
182 }
183
184
185
186
187
188
189
190
191
192
193
194
195 @SuppressWarnings("checkstyle:parameternumber")
196 public static void buildConflicts(final Lane lane1, Lane_keyword">final GTUDirectionality dir1, final Lane lane2,
197 final GTUDirectionality dir2, final GTUType gtuType, final DEVSSimulatorInterface.TimeDoubleUnit simulator,
198 final WidthGenerator widthGenerator) throws OTSGeometryException
199 {
200 buildConflicts(lane1, dir1, lane2, dir2, gtuType, simulator, widthGenerator, false);
201 }
202
203
204
205
206
207
208
209
210
211
212
213
214
215 @SuppressWarnings("checkstyle:parameternumber")
216 public static void buildConflicts(final Lane lane1, Lane_keyword">final GTUDirectionality dir1, final Lane lane2,
217 final GTUDirectionality dir2, final GTUType gtuType, final DEVSSimulatorInterface.TimeDoubleUnit simulator,
218 final WidthGenerator widthGenerator, final boolean permitted) throws OTSGeometryException
219 {
220 ImmutableMap<Lane, GTUDirectionality> down1 = lane1.downstreamLanes(dir1, gtuType);
221 ImmutableMap<Lane, GTUDirectionality> up1 = lane1.upstreamLanes(dir1, gtuType);
222 ImmutableMap<Lane, GTUDirectionality> down2 = lane2.downstreamLanes(dir2, gtuType);
223 ImmutableMap<Lane, GTUDirectionality> up2 = lane2.upstreamLanes(dir2, gtuType);
224 try
225 {
226 buildConflicts(lane1, dir1, down1, up1, lane2, dir2, down2, up2, gtuType, permitted, simulator, widthGenerator,
227 new LinkedHashMap<>(), new LinkedHashMap<>());
228 }
229 catch (NetworkException ne)
230 {
231 throw new RuntimeException("Conflict build with bad combination of types / rules.", ne);
232 }
233 }
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254 @SuppressWarnings("checkstyle:parameternumber")
255 private static void buildConflicts(final Lane lane1, final GTUDirectionality dir1,
256 final ImmutableMap<Lane, GTUDirectionality> down1, final ImmutableMap<Lane, GTUDirectionality> up1,
257 final Lane lane2, final GTUDirectionality dir2, final ImmutableMap<Lane, GTUDirectionality> down2,
258 final ImmutableMap<Lane, GTUDirectionality> up2, final GTUType gtuType, final boolean permitted,
259 final DEVSSimulatorInterface.TimeDoubleUnit simulator, final WidthGenerator widthGenerator,
260 final Map<Lane, OTSLine3D> leftEdges, final Map<Lane, OTSLine3D> rightEdges)
261 throws OTSGeometryException, NetworkException
262 {
263
264
265 if (!lane1.getContour().intersects(lane2.getContour()))
266 {
267 return;
268 }
269
270
271 OTSLine3D left1 = leftEdges.get(lane1);
272 OTSLine3D right1 = rightEdges.get(lane1);
273 if (null == left1)
274 {
275 OTSLine3D line1 = lane1.getCenterLine();
276 left1 = line1.offsetLine(widthGenerator.getWidth(lane1, 0.0) / 2, widthGenerator.getWidth(lane1, 1.0) / 2);
277 leftEdges.put(lane1, left1);
278 right1 = line1.offsetLine(-widthGenerator.getWidth(lane1, 0.0) / 2, -widthGenerator.getWidth(lane1, 1.0) / 2);
279 rightEdges.put(lane1, right1);
280 }
281 OTSLine3D left2 = leftEdges.get(lane2);
282 OTSLine3D right2 = rightEdges.get(lane2);
283 if (null == left2)
284 {
285 OTSLine3D line2 = lane2.getCenterLine();
286 left2 = line2.offsetLine(widthGenerator.getWidth(lane2, 0.0) / 2, widthGenerator.getWidth(lane2, 1.0) / 2);
287 leftEdges.put(lane2, left2);
288 right2 = line2.offsetLine(-widthGenerator.getWidth(lane2, 0.0) / 2, -widthGenerator.getWidth(lane2, 1.0) / 2);
289 rightEdges.put(lane2, right2);
290 }
291
292
293 SortedSet<Intersection> intersections = Intersection.getIntersectionList(left1, left2, 0);
294 intersections.addAll(Intersection.getIntersectionList(left1, right2, 1));
295 intersections.addAll(Intersection.getIntersectionList(right1, left2, 2));
296 intersections.addAll(Intersection.getIntersectionList(right1, right2, 3));
297
298
299 ImmutableIterator<ImmutableEntry<Lane, GTUDirectionality>> iterator1 = down1.entrySet().iterator();
300 ImmutableIterator<ImmutableEntry<Lane, GTUDirectionality>> iterator2 = down2.entrySet().iterator();
301 boolean merge = false;
302 while (iterator1.hasNext() && !merge)
303 {
304 ImmutableEntry<Lane, GTUDirectionality> next1 = iterator1.next();
305 while (iterator2.hasNext() && !merge)
306 {
307 ImmutableEntry<Lane, GTUDirectionality> next2 = iterator2.next();
308 if (next1.equals(next2))
309 {
310
311 double fraction1 = Double.NaN;
312 double fraction2 = Double.NaN;
313 for (Intersection intersection : intersections)
314 {
315
316 if (intersection.getCombo() == 1 || intersection.getCombo() == 2)
317 {
318 fraction1 = intersection.getFraction1();
319 fraction2 = intersection.getFraction2();
320 }
321 }
322
323 Iterator<Intersection> iterator = intersections.iterator();
324 while (iterator.hasNext())
325 {
326 if (iterator.next().getFraction1() >= fraction1)
327 {
328 iterator.remove();
329 }
330 }
331 if (Double.isNaN(fraction1))
332 {
333 SimLogger.always().warn("Fixing fractions of merge conflict");
334 fraction1 = 0;
335 fraction2 = 0;
336 }
337
338 buildMergeConflict(lane1, dir1, fraction1, lane2, dir2, fraction2, gtuType, simulator, widthGenerator,
339 permitted);
340
341 merge = true;
342 }
343 }
344 }
345
346
347 iterator1 = up1.entrySet().iterator();
348 iterator2 = up2.entrySet().iterator();
349 boolean split = false;
350 while (iterator1.hasNext() && !split)
351 {
352 ImmutableEntry<Lane, GTUDirectionality> prev1 = iterator1.next();
353 while (iterator2.hasNext() && !split)
354 {
355 ImmutableEntry<Lane, GTUDirectionality> prev2 = iterator2.next();
356 if (prev1.equals(prev2))
357 {
358
359 double fraction1 = Double.NaN;
360 double fraction2 = Double.NaN;
361 for (Intersection intersection : intersections)
362 {
363
364 if (intersection.getCombo() == 1 || intersection.getCombo() == 2)
365 {
366 fraction1 = intersection.getFraction1();
367 fraction2 = intersection.getFraction2();
368 break;
369 }
370 }
371
372 Iterator<Intersection> iterator = intersections.iterator();
373 while (iterator.hasNext())
374 {
375 if (iterator.next().getFraction1() <= fraction1)
376 {
377 iterator.remove();
378 }
379 else
380 {
381
382 break;
383 }
384 }
385 if (Double.isNaN(fraction1))
386 {
387 SimLogger.always().warn("Fixing fractions of split conflict");
388 fraction1 = 1;
389 fraction2 = 1;
390 }
391
392 buildSplitConflict(lane1, dir1, fraction1, lane2, dir2, fraction2, gtuType, simulator, widthGenerator);
393
394 split = true;
395 }
396 }
397 }
398
399
400 if (!lane1.getParentLink().equals(lane2.getParentLink()))
401 {
402 boolean[] crossed = new boolean[4];
403 Iterator<Intersection> iterator = intersections.iterator();
404 double f1Start = Double.NaN;
405 double f2Start = Double.NaN;
406 double f2End = Double.NaN;
407 while (iterator.hasNext())
408 {
409 Intersection intersection = iterator.next();
410
411 if (Double.isNaN(f1Start))
412 {
413 f1Start = intersection.getFraction1();
414 }
415 f2Start = Double.isNaN(f2Start) ? intersection.getFraction2() : Math.min(f2Start, intersection.getFraction2());
416 f2End = Double.isNaN(f2End) ? intersection.getFraction2() : Math.max(f2End, intersection.getFraction2());
417
418 crossed[intersection.getCombo()] = !crossed[intersection.getCombo()];
419
420 if ((crossed[0] && crossed[1] && crossed[2] && crossed[3])
421 || (!crossed[0] && !crossed[1] && !crossed[2] && !crossed[3]))
422 {
423 if (dir2.isMinus())
424 {
425 double f2Temp = f2Start;
426 f2Start = f2End;
427 f2End = f2Temp;
428 }
429 if (Double.isNaN(f1Start) || Double.isNaN(f2Start) || Double.isNaN(f2End))
430 {
431 SimLogger.always().warn("NOT YET Fixing fractions of crossing conflict");
432 }
433 buildCrossingConflict(lane1, dir1, f1Start, intersection.getFraction2(), lane2, dir2, f2Start, f2End,
434 gtuType, simulator, widthGenerator, permitted);
435 f1Start = Double.NaN;
436 f2Start = Double.NaN;
437 f2End = Double.NaN;
438 }
439 }
440 }
441
442 }
443
444
445
446
447
448
449
450
451
452
453
454
455
456
457
458
459 @SuppressWarnings("checkstyle:parameternumber")
460 private static void buildMergeConflict(final Lane lane1, final GTUDirectionality dir1, final double f1start,
461 final Lane lane2, final GTUDirectionality dir2, final double f2start, final GTUType gtuType,
462 final DEVSSimulatorInterface.TimeDoubleUnit simulator, final WidthGenerator widthGenerator, final boolean permitted)
463 throws NetworkException, OTSGeometryException
464 {
465
466
467 double f1end = dir1.isPlus() ? 1.0 : 0.0;
468 double f2end = dir2.isPlus() ? 1.0 : 0.0;
469
470
471 Length longitudinalPosition1 = lane1.getLength().times(f1start);
472 Length longitudinalPosition2 = lane2.getLength().times(f2start);
473 Length length1 = lane1.getLength().times(Math.abs(f1end - f1start));
474 Length length2 = lane2.getLength().times(Math.abs(f2end - f2start));
475
476
477 OTSLine3D geometry1 = getGeometry(lane1, f1start, f1end, widthGenerator);
478 OTSLine3D geometry2 = getGeometry(lane2, f2start, f2end, widthGenerator);
479
480
481 ConflictRule conflictRule;
482 if (lane1.getParentLink().getPriority().isBusStop() || lane2.getParentLink().getPriority().isBusStop())
483 {
484 Throw.when(lane1.getParentLink().getPriority().isBusStop() && lane2.getParentLink().getPriority().isBusStop(),
485 IllegalArgumentException.class, "Merge conflict between two links with bus stop priority not supported.");
486 conflictRule = new BusStopConflictRule(simulator);
487 }
488 else
489 {
490 conflictRule = new DefaultConflictRule();
491 }
492
493
494 Conflict.generateConflictPair(ConflictType.MERGE, conflictRule, permitted, lane1, longitudinalPosition1, length1, dir1,
495 geometry1, gtuType, lane2, longitudinalPosition2, length2, dir2, geometry2, gtuType, simulator);
496 }
497
498
499
500
501
502
503
504
505
506
507
508
509
510
511
512 @SuppressWarnings("checkstyle:parameternumber")
513 private static void buildSplitConflict(final Lane lane1, final GTUDirectionality dir1, finalLanetrong class="jxr_keyword">double f1end, final Lane lane2,
514 final GTUDirectionality dir2, final double f2end, final GTUType gtuType,
515 final DEVSSimulatorInterface.TimeDoubleUnit simulator, final WidthGenerator widthGenerator)
516 throws NetworkException, OTSGeometryException
517 {
518
519
520 double f1start = dir1.isPlus() ? 0.0 : 1.0;
521 double f2start = dir2.isPlus() ? 0.0 : 1.0;
522
523
524 Length longitudinalPosition1 = lane1.getLength().times(f1start);
525 Length longitudinalPosition2 = lane2.getLength().times(f2start);
526 Length length1 = lane1.getLength().times(Math.abs(f1end - f1start));
527 Length length2 = lane2.getLength().times(Math.abs(f2end - f2start));
528
529
530 OTSLine3D geometry1 = getGeometry(lane1, f1start, f1end, widthGenerator);
531 OTSLine3D geometry2 = getGeometry(lane2, f2start, f2end, widthGenerator);
532
533
534 Conflict.generateConflictPair(ConflictType.SPLIT, new SplitConflictRule(), false, lane1, longitudinalPosition1, length1,
535 dir1, geometry1, gtuType, lane2, longitudinalPosition2, length2, dir2, geometry2, gtuType, simulator);
536 }
537
538
539
540
541
542
543
544
545
546
547
548
549
550
551
552
553
554
555 @SuppressWarnings("checkstyle:parameternumber")
556 private static void buildCrossingConflict(final Lane lane1, final GTUDirectionality dir1, final double f1start,
557 final double f1end, final Lane lane2, final GTUDirectionality dir2, final double f2start, final double f2end,
558 final GTUType gtuType, final DEVSSimulatorInterface.TimeDoubleUnit simulator, final WidthGenerator widthGenerator,
559 final boolean permitted) throws NetworkException, OTSGeometryException
560 {
561
562
563
564 double f1startDirected;
565 double f2startDirected;
566 if ((dir1.isPlus() && f1end < f1start) || (dir1.isMinus() && f1end > f1start))
567 {
568 f1startDirected = f1end;
569 }
570 else
571 {
572 f1startDirected = f1start;
573 }
574 if ((dir2.isPlus() && f2end < f2start) || (dir2.isMinus() && f2end > f2start))
575 {
576 f2startDirected = f2end;
577 }
578 else
579 {
580 f2startDirected = f2start;
581 }
582
583
584 Length longitudinalPosition1 = lane1.getLength().times(f1startDirected);
585 Length longitudinalPosition2 = lane2.getLength().times(f2startDirected);
586 Length length1 = lane1.getLength().times(Math.abs(f1end - f1start));
587 Length length2 = lane2.getLength().times(Math.abs(f2end - f2start));
588
589
590 OTSLine3D geometry1 = getGeometry(lane1, f1start, f1end, widthGenerator);
591 OTSLine3D geometry2 = getGeometry(lane2, f2start, f2end, widthGenerator);
592
593
594 ConflictRule conflictRule;
595 if (lane1.getParentLink().getPriority().isBusStop() || lane2.getParentLink().getPriority().isBusStop())
596 {
597 Throw.when(lane1.getParentLink().getPriority().isBusStop() && lane2.getParentLink().getPriority().isBusStop(),
598 IllegalArgumentException.class, "Merge conflict between two links with bus stop priority not supported.");
599 conflictRule = new BusStopConflictRule(simulator);
600 }
601 else
602 {
603 conflictRule = new DefaultConflictRule();
604 }
605
606
607 Conflict.generateConflictPair(ConflictType.CROSSING, conflictRule, permitted, lane1, longitudinalPosition1, length1,
608 dir1, geometry1, gtuType, lane2, longitudinalPosition2, length2, dir2, geometry2, gtuType, simulator);
609 }
610
611
612
613
614
615
616
617
618
619
620 private static OTSLine3D getGeometry(final Lane lane, final double fStart, final double fEnd,
621 final WidthGenerator widthGenerator) throws OTSGeometryException
622 {
623
624 double f1;
625 double f2;
626 if (fEnd > fStart)
627 {
628 f1 = fStart;
629 f2 = fEnd;
630 }
631 else
632 {
633 f1 = fEnd;
634 f2 = fStart;
635 }
636 OTSLine3D centerLine = lane.getCenterLine().extractFractional(f1, f2);
637 OTSLine3D left = centerLine.offsetLine(widthGenerator.getWidth(lane, f1) / 2, widthGenerator.getWidth(lane, f2) / 2);
638 OTSLine3D right =
639 centerLine.offsetLine(-widthGenerator.getWidth(lane, f1) / 2, -widthGenerator.getWidth(lane, f2) / 2).reverse();
640 OTSPoint3D[] points = new OTSPoint3D[left.size() + right.size()];
641 System.arraycopy(left.getPoints(), 0, points, 0, left.size());
642 System.arraycopy(right.getPoints(), 0, points, left.size(), right.size());
643 return new OTSLine3D(points);
644 }
645
646
647
648
649
650
651
652
653
654
655
656
657
658
659 private static class Intersection implements Comparable<Intersection>
660 {
661
662
663 private final double fraction1;
664
665
666 private final double fraction2;
667
668
669 private final int combo;
670
671
672
673
674
675
676 Intersection(final double fraction1, final double fraction2, final int combo)
677 {
678 this.fraction1 = fraction1;
679 this.fraction2 = fraction2;
680 this.combo = combo;
681 }
682
683
684
685
686 public final double getFraction1()
687 {
688 return this.fraction1;
689 }
690
691
692
693
694 public final double getFraction2()
695 {
696 return this.fraction2;
697 }
698
699
700
701
702 public final int getCombo()
703 {
704 return this.combo;
705 }
706
707
708 @Override
709 public int compareTo(final Intersection o)
710 {
711 int out = Double.compare(this.fraction1, o.fraction1);
712 if (out != 0)
713 {
714 return out;
715 }
716 out = Double.compare(this.fraction2, o.fraction2);
717 if (out != 0)
718 {
719 return out;
720 }
721 return Integer.compare(this.combo, o.combo);
722 }
723
724
725 @Override
726 public int hashCode()
727 {
728 final int prime = 31;
729 int result = 1;
730 result = prime * result + this.combo;
731 long temp;
732 temp = Double.doubleToLongBits(this.fraction1);
733 result = prime * result + (int) (temp ^ (temp >>> 32));
734 temp = Double.doubleToLongBits(this.fraction2);
735 result = prime * result + (int) (temp ^ (temp >>> 32));
736 return result;
737 }
738
739
740 @Override
741 public boolean equals(final Object obj)
742 {
743 if (this == obj)
744 {
745 return true;
746 }
747 if (obj == null)
748 {
749 return false;
750 }
751 if (getClass() != obj.getClass())
752 {
753 return false;
754 }
755 Intersection other = (Intersection) obj;
756 if (this.combo != other.combo)
757 {
758 return false;
759 }
760 if (Double.doubleToLongBits(this.fraction1) != Double.doubleToLongBits(other.fraction1))
761 {
762 return false;
763 }
764 if (Double.doubleToLongBits(this.fraction2) != Double.doubleToLongBits(other.fraction2))
765 {
766 return false;
767 }
768 return true;
769 }
770
771
772
773
774
775
776
777
778
779 public static SortedSet<Intersection> getIntersectionList(final OTSLine3D line1, final OTSLine3D line2, final int combo)
780 throws OTSGeometryException
781 {
782 SortedSet<Intersection> out = new TreeSet<>();
783 if (!line1.getBounds().intersect(line2.getBounds()))
784 {
785 return out;
786 }
787 double cumul1 = 0.0;
788 OTSPoint3D start1 = null;
789 OTSPoint3D end1 = line1.get(0);
790 for (int i = 0; i < line1.size() - 1; i++)
791 {
792 start1 = end1;
793 end1 = line1.get(i + 1);
794
795 double cumul2 = 0.0;
796 OTSPoint3D start2 = null;
797 OTSPoint3D end2 = line2.get(0);
798
799 for (int j = 0; j < line2.size() - 1; j++)
800 {
801 start2 = end2;
802 end2 = line2.get(j + 1);
803
804 OTSPoint3D p = OTSPoint3D.intersectionOfLineSegments(start1, end1, start2, end2);
805 if (p != null)
806 {
807
808 double dx = p.x - start1.x;
809 double dy = p.y - start1.y;
810 double length1 = cumul1 + Math.sqrt(dx * dx + dy * dy);
811 dx = p.x - start2.x;
812 dy = p.y - start2.y;
813 double length2 = cumul2 + Math.sqrt(dx * dx + dy * dy);
814 out.add(new Intersection(length1 / line1.getLengthSI(), length2 / line2.getLengthSI(), combo));
815 }
816
817 double dx = end2.x - start2.x;
818 double dy = end2.y - start2.y;
819 cumul2 += Math.sqrt(dx * dx + dy * dy);
820 }
821
822 double dx = end1.x - start1.x;
823 double dy = end1.y - start1.y;
824 cumul1 += Math.sqrt(dx * dx + dy * dy);
825 }
826
827 return out;
828 }
829
830
831 @Override
832 public String toString()
833 {
834 return "Intersection [fraction1=" + this.fraction1 + ", fraction2=" + this.fraction2 + ", combo=" + this.combo
835 + "]";
836 }
837
838 }
839
840
841
842
843
844
845
846
847
848
849
850
851
852 public interface WidthGenerator
853 {
854
855
856
857
858
859
860
861 double getWidth(Lane lane, double fraction);
862
863 }
864
865
866
867
868
869
870
871
872
873
874
875
876
877 public static class FixedWidthGenerator implements WidthGenerator
878 {
879
880
881 private final double width;
882
883
884
885
886
887 public FixedWidthGenerator(final Length width)
888 {
889 this.width = width.si;
890 }
891
892
893 @Override
894 public final double getWidth(final Lane lane, final double fraction)
895 {
896 return this.width;
897 }
898
899
900 @Override
901 public final String toString()
902 {
903 return "FixedWidthGenerator [width=" + this.width + "]";
904 }
905
906 }
907
908
909
910
911
912
913
914
915
916
917
918
919
920 public static class RelativeWidthGenerator implements WidthGenerator
921 {
922
923
924 private final double factor;
925
926
927
928
929
930 public RelativeWidthGenerator(final double factor)
931 {
932 this.factor = factor;
933 }
934
935
936 @Override
937 public final double getWidth(final Lane lane, final double fraction)
938 {
939 return lane.getWidth(fraction).si * this.factor;
940 }
941
942
943 @Override
944 public final String toString()
945 {
946 return "RelativeWidthGenerator [factor=" + this.factor + "]";
947 }
948
949 }
950
951 }