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.Set;
9 import java.util.SortedSet;
10 import java.util.TreeSet;
11 import java.util.concurrent.Executors;
12 import java.util.concurrent.ThreadPoolExecutor;
13 import java.util.concurrent.atomic.AtomicInteger;
14
15 import org.djunits.value.vdouble.scalar.Length;
16 import org.djutils.exceptions.Throw;
17 import org.djutils.immutablecollections.ImmutableMap;
18 import org.opentrafficsim.core.definitions.DefaultsNl;
19 import org.opentrafficsim.core.dsol.OtsSimulatorInterface;
20 import org.opentrafficsim.core.geometry.OtsGeometryException;
21 import org.opentrafficsim.core.geometry.OtsLine3d;
22 import org.opentrafficsim.core.geometry.OtsPoint3d;
23 import org.opentrafficsim.core.network.Link;
24 import org.opentrafficsim.core.network.NetworkException;
25 import org.opentrafficsim.road.network.RoadNetwork;
26 import org.opentrafficsim.road.network.lane.CrossSectionElement;
27 import org.opentrafficsim.road.network.lane.CrossSectionLink;
28 import org.opentrafficsim.road.network.lane.Lane;
29 import org.pmw.tinylog.Level;
30
31
32
33
34
35
36
37
38
39
40
41 public final class ConflictBuilder
42 {
43
44 private static AtomicInteger numberMergeConflicts = new AtomicInteger(0);
45
46
47 private static AtomicInteger numberSplitConflicts = new AtomicInteger(0);
48
49
50 private static AtomicInteger numberCrossConflicts = new AtomicInteger(0);
51
52
53 public static final WidthGenerator DEFAULT_WIDTH_GENERATOR = new RelativeWidthGenerator(0.8);
54
55
56
57
58 private ConflictBuilder()
59 {
60
61 }
62
63
64
65
66
67
68
69
70 public static void buildConflicts(final RoadNetwork network, final OtsSimulatorInterface simulator,
71 final WidthGenerator widthGenerator) throws OtsGeometryException
72 {
73 buildConflicts(network, simulator, widthGenerator, new LaneCombinationList(), new LaneCombinationList());
74 }
75
76
77
78
79
80
81
82
83
84
85 public static void buildConflicts(final RoadNetwork network, final OtsSimulatorInterface simulator,
86 final WidthGenerator widthGenerator, final LaneCombinationList ignoreList, final LaneCombinationList permittedList)
87 throws OtsGeometryException
88 {
89
90 ImmutableMap<String, Link> links = network.getLinkMap();
91 List<Lane> lanes = new ArrayList<>();
92 for (String linkId : links.keySet())
93 {
94 Link link = links.get(linkId);
95 if (link instanceof CrossSectionLink)
96 {
97 for (CrossSectionElement element : ((CrossSectionLink) link).getCrossSectionElementList())
98 {
99 if (element instanceof Lane)
100 {
101 lanes.add((Lane) element);
102 }
103 }
104 }
105 }
106 buildConflicts(lanes, simulator, widthGenerator, ignoreList, permittedList, null);
107 }
108
109
110
111
112
113
114
115
116 public static void buildConflicts(final List<Lane> lanes, final OtsSimulatorInterface simulator,
117 final WidthGenerator widthGenerator) throws OtsGeometryException
118 {
119 buildConflicts(lanes, simulator, widthGenerator, new LaneCombinationList(), new LaneCombinationList(), null);
120 }
121
122
123
124
125
126
127
128
129
130
131
132 public static void buildConflicts(final List<Lane> lanes, final OtsSimulatorInterface simulator,
133 final WidthGenerator widthGenerator, final LaneCombinationList ignoreList, final LaneCombinationList permittedList,
134 final String conflictId) throws OtsGeometryException
135 {
136 long totalCombinations = ((long) lanes.size()) * ((long) lanes.size() - 1) / 2;
137 simulator.getLogger().always().trace("GENERATING CONFLICTS (NON-PARALLEL MODE). {} COMBINATIONS", totalCombinations);
138 long lastReported = 0;
139 Map<Lane, OtsLine3d> leftEdges = new LinkedHashMap<>();
140 Map<Lane, OtsLine3d> rightEdges = new LinkedHashMap<>();
141
142 for (int i = 0; i < lanes.size(); i++)
143 {
144 long combinationsDone = totalCombinations - ((long) (lanes.size() - i)) * ((long) (lanes.size() - i)) / 2;
145 if (combinationsDone / 100000000 > lastReported)
146 {
147 simulator.getLogger().always()
148 .debug(String.format(
149 "generating conflicts at %.0f%% (generated %d merge conflicts, %d split "
150 + "conflicts, %d crossing conflicts)",
151 100.0 * combinationsDone / totalCombinations, numberMergeConflicts.get(),
152 numberSplitConflicts.get(), numberCrossConflicts.get()));
153 lastReported = combinationsDone / 100000000;
154 }
155 Lane lane1 = lanes.get(i);
156 Set<Lane> down1 = lane1.nextLanes(null);
157 Set<Lane> up1 = lane1.prevLanes(null);
158
159 for (int j = i + 1; j < lanes.size(); j++)
160 {
161 Lane lane2 = lanes.get(j);
162 if (ignoreList.contains(lane1, lane2))
163 {
164 continue;
165 }
166 boolean permitted = permittedList.contains(lane1, lane2);
167
168 Set<Lane> down2 = lane2.nextLanes(null);
169 Set<Lane> up2 = lane2.prevLanes(null);
170
171 try
172 {
173 buildConflicts(lane1, down1, up1, lane2, down2, up2, permitted, simulator, widthGenerator, leftEdges,
174 rightEdges, true, conflictId);
175 }
176 catch (NetworkException ne)
177 {
178 throw new RuntimeException("Conflict build with bad combination of types / rules.", ne);
179 }
180 }
181 }
182 simulator.getLogger().always()
183 .trace(String.format(
184 "generating conflicts complete (generated %d merge conflicts, %d split "
185 + "conflicts, %d crossing conflicts)",
186 numberMergeConflicts.get(), numberSplitConflicts.get(), numberCrossConflicts.get()));
187 }
188
189
190
191
192
193
194
195
196
197 @SuppressWarnings("checkstyle:parameternumber")
198 public static void buildConflicts(final Lane lane1, final Lane lane2, final OtsSimulatorInterface simulator,
199 final WidthGenerator widthGenerator) throws OtsGeometryException
200 {
201 buildConflicts(lane1, lane2, simulator, widthGenerator, false);
202 }
203
204
205
206
207
208
209
210
211
212
213 @SuppressWarnings("checkstyle:parameternumber")
214 public static void buildConflicts(final Lane lane1, final Lane lane2, final OtsSimulatorInterface simulator,
215 final WidthGenerator widthGenerator, final boolean permitted) throws OtsGeometryException
216 {
217 Set<Lane> down1 = lane1.nextLanes(null);
218 Set<Lane> up1 = lane1.prevLanes(null);
219 Set<Lane> down2 = lane2.nextLanes(null);
220 Set<Lane> up2 = lane2.prevLanes(null);
221 try
222 {
223 buildConflicts(lane1, down1, up1, lane2, down2, up2, permitted, simulator, widthGenerator, new LinkedHashMap<>(),
224 new LinkedHashMap<>(), true, null);
225 }
226 catch (NetworkException ne)
227 {
228 throw new RuntimeException("Conflict build with bad combination of types / rules.", ne);
229 }
230 }
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250 @SuppressWarnings({"checkstyle:parameternumber", "checkstyle:methodlength"})
251 static void buildConflicts(final Lane lane1, final Set<Lane> down1, final Set<Lane> up1, final Lane lane2,
252 final Set<Lane> down2, final Set<Lane> up2, final boolean permitted, final OtsSimulatorInterface simulator,
253 final WidthGenerator widthGenerator, final Map<Lane, OtsLine3d> leftEdges, final Map<Lane, OtsLine3d> rightEdges,
254 final boolean intersectionCheck, final String conflictId) throws OtsGeometryException, NetworkException
255 {
256
257 if (intersectionCheck)
258 {
259 if (!lane1.getContour().intersects(lane2.getContour()))
260 {
261 return;
262 }
263 }
264
265
266
267 String paddedConflictId = null == conflictId ? "" : (" in conflict group " + conflictId);
268
269 OtsLine3d left1;
270 OtsLine3d right1;
271 synchronized (lane1)
272 {
273 left1 = leftEdges.get(lane1);
274 right1 = rightEdges.get(lane1);
275 OtsLine3d line1 = lane1.getCenterLine();
276 if (null == left1)
277 {
278 left1 = line1.offsetLine(widthGenerator.getWidth(lane1, 0.0) / 2, widthGenerator.getWidth(lane1, 1.0) / 2);
279 leftEdges.put(lane1, left1);
280 }
281 if (null == right1)
282 {
283 right1 = line1.offsetLine(-widthGenerator.getWidth(lane1, 0.0) / 2, -widthGenerator.getWidth(lane1, 1.0) / 2);
284 rightEdges.put(lane1, right1);
285 }
286 }
287
288 OtsLine3d left2;
289 OtsLine3d right2;
290 synchronized (lane2)
291 {
292 left2 = leftEdges.get(lane2);
293 right2 = rightEdges.get(lane2);
294 OtsLine3d line2 = lane2.getCenterLine();
295 if (null == left2)
296 {
297 left2 = line2.offsetLine(widthGenerator.getWidth(lane2, 0.0) / 2, widthGenerator.getWidth(lane2, 1.0) / 2);
298 leftEdges.put(lane2, left2);
299 }
300 if (null == right2)
301 {
302 right2 = line2.offsetLine(-widthGenerator.getWidth(lane2, 0.0) / 2, -widthGenerator.getWidth(lane2, 1.0) / 2);
303 rightEdges.put(lane2, right2);
304 }
305 }
306
307
308 SortedSet<Intersection> intersections = Intersection.getIntersectionList(left1, left2, 0);
309 intersections.addAll(Intersection.getIntersectionList(left1, right2, 1));
310 intersections.addAll(Intersection.getIntersectionList(right1, left2, 2));
311 intersections.addAll(Intersection.getIntersectionList(right1, right2, 3));
312
313
314 Iterator<Lane> iterator1 = down1.iterator();
315 Iterator<Lane> iterator2 = down2.iterator();
316 boolean merge = false;
317 while (iterator1.hasNext() && !merge)
318 {
319 Lane next1 = iterator1.next();
320 while (iterator2.hasNext() && !merge)
321 {
322 Lane next2 = iterator2.next();
323 if (next1.equals(next2))
324 {
325
326 double fraction1 = Double.NaN;
327 double fraction2 = Double.NaN;
328 for (Intersection intersection : intersections)
329 {
330
331 if (intersection.getCombo() == 1 || intersection.getCombo() == 2)
332 {
333 fraction1 = intersection.getFraction1();
334 fraction2 = intersection.getFraction2();
335 }
336 }
337
338 Iterator<Intersection> iterator = intersections.iterator();
339 while (iterator.hasNext())
340 {
341 if (iterator.next().getFraction1() >= fraction1)
342 {
343 iterator.remove();
344 }
345 }
346 if (Double.isNaN(fraction1))
347 {
348 simulator.getLogger().always().info("Fixing fractions of merge conflict{}", paddedConflictId);
349 fraction1 = 0;
350 fraction2 = 0;
351 }
352
353 buildMergeConflict(lane1, fraction1, lane2, fraction2, simulator, widthGenerator, permitted);
354
355 merge = true;
356 }
357 }
358 }
359
360
361 iterator1 = up1.iterator();
362 iterator2 = up2.iterator();
363 boolean split = false;
364 while (iterator1.hasNext() && !split)
365 {
366 Lane prev1 = iterator1.next();
367 while (iterator2.hasNext() && !split)
368 {
369 Lane prev2 = iterator2.next();
370 if (prev1.equals(prev2))
371 {
372
373 double fraction1 = Double.NaN;
374 double fraction2 = Double.NaN;
375 for (Intersection intersection : intersections)
376 {
377
378 if (intersection.getCombo() == 1 || intersection.getCombo() == 2)
379 {
380 fraction1 = intersection.getFraction1();
381 fraction2 = intersection.getFraction2();
382 break;
383 }
384 }
385
386 Iterator<Intersection> iterator = intersections.iterator();
387 while (iterator.hasNext())
388 {
389 if (iterator.next().getFraction1() <= fraction1)
390 {
391 iterator.remove();
392 }
393 else
394 {
395
396 break;
397 }
398 }
399 if (Double.isNaN(fraction1))
400 {
401 simulator.getLogger().always().info("Fixing fractions of split conflict{}", paddedConflictId);
402 fraction1 = 1;
403 fraction2 = 1;
404 }
405
406 buildSplitConflict(lane1, fraction1, lane2, fraction2, simulator, widthGenerator);
407
408 split = true;
409 }
410 }
411 }
412
413
414 if (!lane1.getParentLink().equals(lane2.getParentLink()))
415 {
416 boolean[] crossed = new boolean[4];
417 Iterator<Intersection> iterator = intersections.iterator();
418 double f1Start = Double.NaN;
419 double f2Start = Double.NaN;
420 double f2End = Double.NaN;
421 while (iterator.hasNext())
422 {
423 Intersection intersection = iterator.next();
424
425 if (Double.isNaN(f1Start))
426 {
427 f1Start = intersection.getFraction1();
428 }
429 f2Start = Double.isNaN(f2Start) ? intersection.getFraction2() : Math.min(f2Start, intersection.getFraction2());
430 f2End = Double.isNaN(f2End) ? intersection.getFraction2() : Math.max(f2End, intersection.getFraction2());
431
432 crossed[intersection.getCombo()] = !crossed[intersection.getCombo()];
433
434 if ((crossed[0] && crossed[1] && crossed[2] && crossed[3])
435 || (!crossed[0] && !crossed[1] && !crossed[2] && !crossed[3]))
436 {
437 if (Double.isNaN(f1Start) || Double.isNaN(f2Start) || Double.isNaN(f2End))
438 {
439 simulator.getLogger().always().warn("NOT YET Fixing fractions of crossing conflict{}",
440 paddedConflictId);
441 }
442 buildCrossingConflict(lane1, f1Start, intersection.getFraction1(), lane2, f2Start, f2End, simulator,
443 widthGenerator, permitted);
444 f1Start = Double.NaN;
445 f2Start = Double.NaN;
446 f2End = Double.NaN;
447 }
448 }
449 }
450
451 }
452
453
454
455
456
457
458
459
460
461
462
463
464
465 @SuppressWarnings("checkstyle:parameternumber")
466 private static void buildMergeConflict(final Lane lane1, final double f1start, final Lane lane2, final double f2start,
467 final OtsSimulatorInterface simulator, final WidthGenerator widthGenerator, final boolean permitted)
468 throws NetworkException, OtsGeometryException
469 {
470
471
472 double f1end = 1.0;
473 double f2end = 1.0;
474
475
476 Length longitudinalPosition1 = lane1.getLength().times(f1start);
477 Length longitudinalPosition2 = lane2.getLength().times(f2start);
478 Length length1 = lane1.getLength().times(Math.abs(f1end - f1start));
479 Length length2 = lane2.getLength().times(Math.abs(f2end - f2start));
480
481
482 OtsLine3d geometry1 = getGeometry(lane1, f1start, f1end, widthGenerator);
483 OtsLine3d geometry2 = getGeometry(lane2, f2start, f2end, widthGenerator);
484
485
486 ConflictRule conflictRule;
487 if (lane1.getParentLink().getPriority().isBusStop() || lane2.getParentLink().getPriority().isBusStop())
488 {
489 Throw.when(lane1.getParentLink().getPriority().isBusStop() && lane2.getParentLink().getPriority().isBusStop(),
490 IllegalArgumentException.class, "Merge conflict between two links with bus stop priority not supported.");
491
492 conflictRule = new BusStopConflictRule(simulator, DefaultsNl.BUS);
493 }
494 else
495 {
496 conflictRule = new DefaultConflictRule();
497 }
498
499
500 Conflict.generateConflictPair(ConflictType.MERGE, conflictRule, permitted, lane1, longitudinalPosition1, length1,
501 geometry1, lane2, longitudinalPosition2, length2, geometry2, simulator);
502
503 numberMergeConflicts.incrementAndGet();
504 }
505
506
507
508
509
510
511
512
513
514
515
516
517 @SuppressWarnings("checkstyle:parameternumber")
518 private static void buildSplitConflict(final Lane lane1, final double f1end, final Lane lane2, final double f2end,
519 final OtsSimulatorInterface simulator, final WidthGenerator widthGenerator)
520 throws NetworkException, OtsGeometryException
521 {
522
523
524 double f1start = 0.0;
525 double f2start = 0.0;
526
527
528 Length longitudinalPosition1 = lane1.getLength().times(f1start);
529 Length longitudinalPosition2 = lane2.getLength().times(f2start);
530 Length length1 = lane1.getLength().times(Math.abs(f1end - f1start));
531 Length length2 = lane2.getLength().times(Math.abs(f2end - f2start));
532
533
534 OtsLine3d geometry1 = getGeometry(lane1, f1start, f1end, widthGenerator);
535 OtsLine3d geometry2 = getGeometry(lane2, f2start, f2end, widthGenerator);
536
537
538 Conflict.generateConflictPair(ConflictType.SPLIT, new SplitConflictRule(), false, lane1, longitudinalPosition1, length1,
539 geometry1, lane2, longitudinalPosition2, length2, geometry2, simulator);
540
541 numberSplitConflicts.incrementAndGet();
542 }
543
544
545
546
547
548
549
550
551
552
553
554
555
556
557
558 @SuppressWarnings("checkstyle:parameternumber")
559 private static void buildCrossingConflict(final Lane lane1, final double f1start, final double f1end, final Lane lane2,
560 final double f2start, final double f2end, final OtsSimulatorInterface simulator,
561 final WidthGenerator widthGenerator, final boolean permitted) throws NetworkException, OtsGeometryException
562 {
563
564
565
566 double f1startDirected;
567 double f2startDirected;
568 if (f1end < f1start)
569 {
570 f1startDirected = f1end;
571 }
572 else
573 {
574 f1startDirected = f1start;
575 }
576 if (f2end < f2start)
577 {
578 f2startDirected = f2end;
579 }
580 else
581 {
582 f2startDirected = f2start;
583 }
584
585
586 Length longitudinalPosition1 = lane1.getLength().times(f1startDirected);
587 Length longitudinalPosition2 = lane2.getLength().times(f2startDirected);
588 Length length1 = lane1.getLength().times(Math.abs(f1end - f1start));
589 Length length2 = lane2.getLength().times(Math.abs(f2end - f2start));
590
591
592 OtsLine3d geometry1 = getGeometry(lane1, f1start, f1end, widthGenerator);
593 OtsLine3d geometry2 = getGeometry(lane2, f2start, f2end, widthGenerator);
594
595
596 ConflictRule conflictRule;
597 if (lane1.getParentLink().getPriority().isBusStop() || lane2.getParentLink().getPriority().isBusStop())
598 {
599 Throw.when(lane1.getParentLink().getPriority().isBusStop() && lane2.getParentLink().getPriority().isBusStop(),
600 IllegalArgumentException.class, "Merge conflict between two links with bus stop priority not supported.");
601
602 conflictRule = new BusStopConflictRule(simulator, DefaultsNl.BUS);
603 }
604 else
605 {
606 conflictRule = new DefaultConflictRule();
607 }
608
609
610 Conflict.generateConflictPair(ConflictType.CROSSING, conflictRule, permitted, lane1, longitudinalPosition1, length1,
611 geometry1, lane2, longitudinalPosition2, length2, geometry2, simulator);
612
613 numberCrossConflicts.incrementAndGet();
614 }
615
616
617
618
619
620
621
622
623
624
625 private static OtsLine3d getGeometry(final Lane lane, final double fStart, final double fEnd,
626 final WidthGenerator widthGenerator) throws OtsGeometryException
627 {
628
629 double f1;
630 double f2;
631 if (fEnd > fStart)
632 {
633 f1 = fStart;
634 f2 = fEnd;
635 }
636 else
637 {
638 f1 = fEnd;
639 f2 = fStart;
640 }
641 if (Math.abs(f1 - f2) < 1E-8)
642 {
643 lane.getParentLink().getSimulator().getLogger().always()
644 .debug("f1 (" + f1 + ") equals f2 (" + f2 + "); problematic lane is " + lane.toString());
645
646 if (f1 > 0)
647 {
648 f1 = f1 - f1 / 1000;
649 }
650 else
651 {
652 f2 = f2 + f2 / 1000;
653 }
654 }
655 OtsLine3d centerLine = lane.getCenterLine().extractFractional(f1, f2);
656 OtsLine3d left = centerLine.offsetLine(widthGenerator.getWidth(lane, f1) / 2, widthGenerator.getWidth(lane, f2) / 2);
657 OtsLine3d right =
658 centerLine.offsetLine(-widthGenerator.getWidth(lane, f1) / 2, -widthGenerator.getWidth(lane, f2) / 2).reverse();
659 OtsPoint3d[] points = new OtsPoint3d[left.size() + right.size()];
660 System.arraycopy(left.getPoints(), 0, points, 0, left.size());
661 System.arraycopy(right.getPoints(), 0, points, left.size(), right.size());
662 return new OtsLine3d(points);
663 }
664
665
666
667
668
669
670
671
672
673
674
675
676
677 private static class Intersection implements Comparable<Intersection>
678 {
679
680
681 private final double fraction1;
682
683
684 private final double fraction2;
685
686
687 private final int combo;
688
689
690
691
692
693
694 Intersection(final double fraction1, final double fraction2, final int combo)
695 {
696 this.fraction1 = fraction1;
697 this.fraction2 = fraction2;
698 this.combo = combo;
699 }
700
701
702
703
704 public final double getFraction1()
705 {
706 return this.fraction1;
707 }
708
709
710
711
712 public final double getFraction2()
713 {
714 return this.fraction2;
715 }
716
717
718
719
720 public final int getCombo()
721 {
722 return this.combo;
723 }
724
725
726 @Override
727 public int compareTo(final Intersection o)
728 {
729 int out = Double.compare(this.fraction1, o.fraction1);
730 if (out != 0)
731 {
732 return out;
733 }
734 out = Double.compare(this.fraction2, o.fraction2);
735 if (out != 0)
736 {
737 return out;
738 }
739 return Integer.compare(this.combo, o.combo);
740 }
741
742
743 @Override
744 public int hashCode()
745 {
746 final int prime = 31;
747 int result = 1;
748 result = prime * result + this.combo;
749 long temp;
750 temp = Double.doubleToLongBits(this.fraction1);
751 result = prime * result + (int) (temp ^ (temp >>> 32));
752 temp = Double.doubleToLongBits(this.fraction2);
753 result = prime * result + (int) (temp ^ (temp >>> 32));
754 return result;
755 }
756
757
758 @Override
759 public boolean equals(final Object obj)
760 {
761 if (this == obj)
762 {
763 return true;
764 }
765 if (obj == null)
766 {
767 return false;
768 }
769 if (getClass() != obj.getClass())
770 {
771 return false;
772 }
773 Intersection other = (Intersection) obj;
774 if (this.combo != other.combo)
775 {
776 return false;
777 }
778 if (Double.doubleToLongBits(this.fraction1) != Double.doubleToLongBits(other.fraction1))
779 {
780 return false;
781 }
782 if (Double.doubleToLongBits(this.fraction2) != Double.doubleToLongBits(other.fraction2))
783 {
784 return false;
785 }
786 return true;
787 }
788
789
790
791
792
793
794
795
796
797 public static SortedSet<Intersection> getIntersectionList(final OtsLine3d line1, final OtsLine3d line2, final int combo)
798 throws OtsGeometryException
799 {
800 SortedSet<Intersection> out = new TreeSet<>();
801
802
803
804
805 double cumul1 = 0.0;
806 OtsPoint3d start1 = null;
807 OtsPoint3d end1 = line1.get(0);
808 for (int i = 0; i < line1.size() - 1; i++)
809 {
810 start1 = end1;
811 end1 = line1.get(i + 1);
812
813 double cumul2 = 0.0;
814 OtsPoint3d start2 = null;
815 OtsPoint3d end2 = line2.get(0);
816
817 for (int j = 0; j < line2.size() - 1; j++)
818 {
819 start2 = end2;
820 end2 = line2.get(j + 1);
821
822 OtsPoint3d p = OtsPoint3d.intersectionOfLineSegments(start1, end1, start2, end2);
823 if (p != null)
824 {
825
826 double dx = p.x - start1.x;
827 double dy = p.y - start1.y;
828 double length1 = cumul1 + Math.sqrt(dx * dx + dy * dy);
829 dx = p.x - start2.x;
830 dy = p.y - start2.y;
831 double length2 = cumul2 + Math.sqrt(dx * dx + dy * dy);
832 out.add(new Intersection(length1 / line1.getLengthSI(), length2 / line2.getLengthSI(), combo));
833 }
834
835 double dx = end2.x - start2.x;
836 double dy = end2.y - start2.y;
837 cumul2 += Math.sqrt(dx * dx + dy * dy);
838 }
839
840 double dx = end1.x - start1.x;
841 double dy = end1.y - start1.y;
842 cumul1 += Math.sqrt(dx * dx + dy * dy);
843 }
844
845 return out;
846 }
847
848
849 @Override
850 public String toString()
851 {
852 return "Intersection [fraction1=" + this.fraction1 + ", fraction2=" + this.fraction2 + ", combo=" + this.combo
853 + "]";
854 }
855
856 }
857
858
859
860
861
862
863
864
865
866
867
868
869 public interface WidthGenerator
870 {
871
872
873
874
875
876
877
878 double getWidth(Lane lane, double fraction);
879
880 }
881
882
883
884
885
886
887
888
889
890
891
892
893 public static class FixedWidthGenerator implements WidthGenerator
894 {
895
896
897 private final double width;
898
899
900
901
902
903 public FixedWidthGenerator(final Length width)
904 {
905 this.width = width.si;
906 }
907
908
909 @Override
910 public final double getWidth(final Lane lane, final double fraction)
911 {
912 return this.width;
913 }
914
915
916 @Override
917 public final String toString()
918 {
919 return "FixedWidthGenerator [width=" + this.width + "]";
920 }
921
922 }
923
924
925
926
927
928
929
930
931
932
933
934
935 public static class RelativeWidthGenerator implements WidthGenerator
936 {
937
938
939 private final double factor;
940
941
942
943
944
945 public RelativeWidthGenerator(final double factor)
946 {
947 this.factor = factor;
948 }
949
950
951 @Override
952 public final double getWidth(final Lane lane, final double fraction)
953 {
954 return lane.getWidth(fraction).si * this.factor;
955 }
956
957
958 @Override
959 public final String toString()
960 {
961 return "RelativeWidthGenerator [factor=" + this.factor + "]";
962 }
963
964 }
965
966
967
968
969
970
971
972
973
974
975
976
977
978
979
980
981 public static void buildConflictsParallel(final RoadNetwork network, final OtsSimulatorInterface simulator,
982 final WidthGenerator widthGenerator) throws OtsGeometryException
983 {
984 buildConflictsParallel(network, simulator, widthGenerator, new LaneCombinationList(), new LaneCombinationList());
985 }
986
987
988
989
990
991
992
993
994
995
996 public static void buildConflictsParallel(final RoadNetwork network, final OtsSimulatorInterface simulator,
997 final WidthGenerator widthGenerator, final LaneCombinationList ignoreList, final LaneCombinationList permittedList)
998 throws OtsGeometryException
999 {
1000
1001 ImmutableMap<String, Link> links = network.getLinkMap();
1002 List<Lane> lanes = new ArrayList<>();
1003 for (String linkId : links.keySet())
1004 {
1005 Link link = links.get(linkId);
1006 if (link instanceof CrossSectionLink)
1007 {
1008 for (CrossSectionElement element : ((CrossSectionLink) link).getCrossSectionElementList())
1009 {
1010 if (element instanceof Lane)
1011 {
1012 lanes.add((Lane) element);
1013 }
1014 }
1015 }
1016 }
1017 buildConflictsParallelBig(lanes, simulator, widthGenerator, ignoreList, permittedList);
1018 }
1019
1020
1021
1022
1023
1024
1025
1026
1027 public static void buildConflictsParallel(final List<Lane> lanes, final OtsSimulatorInterface simulator,
1028 final WidthGenerator widthGenerator) throws OtsGeometryException
1029 {
1030 buildConflictsParallelBig(lanes, simulator, widthGenerator, new LaneCombinationList(), new LaneCombinationList());
1031 }
1032
1033
1034
1035
1036
1037
1038
1039
1040
1041
1042 public static void buildConflictsParallelSmall(final List<Lane> lanes, final OtsSimulatorInterface simulator,
1043 final WidthGenerator widthGenerator, final LaneCombinationList ignoreList, final LaneCombinationList permittedList)
1044 throws OtsGeometryException
1045 {
1046 long totalCombinations = ((long) lanes.size()) * ((long) lanes.size() - 1) / 2;
1047 System.out.println("PARALLEL GENERATING OF CONFLICTS (SMALL JOBS). " + totalCombinations + " COMBINATIONS");
1048 long lastReported = 0;
1049 Map<Lane, OtsLine3d> leftEdges = new LinkedHashMap<>();
1050 Map<Lane, OtsLine3d> rightEdges = new LinkedHashMap<>();
1051
1052
1053 for (Lane lane : lanes)
1054 {
1055 lane.getContour().getEnvelope();
1056 }
1057
1058
1059 int cores = Runtime.getRuntime().availableProcessors();
1060 System.out.println("USING " + cores + " CORES");
1061 ThreadPoolExecutor executor = (ThreadPoolExecutor) Executors.newFixedThreadPool(cores);
1062 AtomicInteger numberOfJobs = new AtomicInteger(0);
1063 final int maxqueue = 2 * cores;
1064
1065 for (int i = 0; i < lanes.size(); i++)
1066 {
1067 long combinationsDone = totalCombinations - ((long) (lanes.size() - i)) * ((long) (lanes.size() - i)) / 2;
1068 if (combinationsDone / 100000000 > lastReported)
1069 {
1070 simulator.getLogger().always()
1071 .debug(String.format(
1072 "generating conflicts at %.0f%% (generated %d merge conflicts, %d split "
1073 + "conflicts, %d crossing conflicts)",
1074 100.0 * combinationsDone / totalCombinations, numberMergeConflicts.get(),
1075 numberSplitConflicts.get(), numberCrossConflicts.get()));
1076 lastReported = combinationsDone / 100000000;
1077 }
1078 Lane lane1 = lanes.get(i);
1079 Set<Lane> down1 = lane1.nextLanes(null);
1080 Set<Lane> up1 = lane1.prevLanes(null);
1081
1082 for (int j = i + 1; j < lanes.size(); j++)
1083 {
1084 Lane lane2 = lanes.get(j);
1085 if (ignoreList.contains(lane1, lane2))
1086 {
1087 continue;
1088 }
1089
1090 try
1091 {
1092 if (!lane1.getContour().intersects(lane2.getContour()))
1093 {
1094 continue;
1095 }
1096 }
1097 catch (Exception e)
1098 {
1099 System.err.println("Contour problem - lane1 = [" + lane1.getFullId() + "], lane2 = [" + lane2.getFullId()
1100 + "]; skipped");
1101 continue;
1102 }
1103
1104 boolean permitted = permittedList.contains(lane1, lane2);
1105
1106 while (numberOfJobs.get() > maxqueue)
1107 {
1108 try
1109 {
1110 Thread.sleep(1);
1111 }
1112 catch (InterruptedException exception)
1113 {
1114
1115 }
1116 }
1117 numberOfJobs.incrementAndGet();
1118 Set<Lane> down2 = lane2.nextLanes(null);
1119 Set<Lane> up2 = lane2.prevLanes(null);
1120 ConflictBuilderRecordSmall cbr = new ConflictBuilderRecordSmall(lane1, down1, up1, lane2, down2, up2, permitted,
1121 simulator, widthGenerator, leftEdges, rightEdges);
1122 executor.execute(new CbrTaskSmall(numberOfJobs, cbr));
1123 }
1124 }
1125
1126 long time = System.currentTimeMillis();
1127
1128 while (numberOfJobs.get() > 0 && System.currentTimeMillis() - time < 60000)
1129 {
1130 try
1131 {
1132 Thread.sleep(10);
1133 }
1134 catch (InterruptedException exception)
1135 {
1136
1137 }
1138 }
1139
1140 executor.shutdown();
1141 while (!executor.isTerminated())
1142 {
1143 try
1144 {
1145 Thread.sleep(1);
1146 }
1147 catch (InterruptedException exception)
1148 {
1149
1150 }
1151 }
1152
1153 simulator.getLogger().always()
1154 .debug(String.format(
1155 "generating conflicts complete (generated %d merge conflicts, %d split "
1156 + "conflicts, %d crossing conflicts)",
1157 numberMergeConflicts.get(), numberSplitConflicts.get(), numberCrossConflicts.get()));
1158 }
1159
1160
1161
1162
1163
1164
1165
1166
1167
1168
1169 public static void buildConflictsParallelBig(final List<Lane> lanes, final OtsSimulatorInterface simulator,
1170 final WidthGenerator widthGenerator, final LaneCombinationList ignoreList, final LaneCombinationList permittedList)
1171 throws OtsGeometryException
1172 {
1173 long totalCombinations = ((long) lanes.size()) * ((long) lanes.size() - 1) / 2;
1174 System.out.println("PARALLEL GENERATING OF CONFLICTS (BIG JOBS). " + totalCombinations + " COMBINATIONS");
1175 long lastReported = 0;
1176 Map<Lane, OtsLine3d> leftEdges = new LinkedHashMap<>();
1177 Map<Lane, OtsLine3d> rightEdges = new LinkedHashMap<>();
1178
1179
1180 for (Lane lane : lanes)
1181 {
1182 lane.getContour().getEnvelope();
1183 }
1184
1185
1186 int cores = Runtime.getRuntime().availableProcessors();
1187 System.out.println("USING " + cores + " CORES");
1188 ThreadPoolExecutor executor = (ThreadPoolExecutor) Executors.newFixedThreadPool(cores);
1189 AtomicInteger numberOfJobs = new AtomicInteger(0);
1190 final int maxqueue = 200;
1191
1192 for (int i = 0; i < lanes.size(); i++)
1193 {
1194 long combinationsDone = totalCombinations - ((long) (lanes.size() - i)) * ((long) (lanes.size() - i)) / 2;
1195 if (combinationsDone / 100000000 > lastReported)
1196 {
1197 simulator.getLogger().always()
1198 .debug(String.format(
1199 "generating conflicts at %.0f%% (generated %d merge conflicts, %d split "
1200 + "conflicts, %d crossing conflicts)",
1201 100.0 * combinationsDone / totalCombinations, numberMergeConflicts.get(),
1202 numberSplitConflicts.get(), numberCrossConflicts.get()));
1203 lastReported = combinationsDone / 100000000;
1204 }
1205 Lane lane1 = lanes.get(i);
1206 Set<Lane> down1 = lane1.nextLanes(null);
1207 Set<Lane> up1 = lane1.prevLanes(null);
1208
1209 while (numberOfJobs.get() > maxqueue)
1210 {
1211 try
1212 {
1213 Thread.sleep(0, 10);
1214 }
1215 catch (InterruptedException exception)
1216 {
1217
1218 }
1219 }
1220 numberOfJobs.incrementAndGet();
1221
1222 ConflictBuilderRecordBig cbr = new ConflictBuilderRecordBig(i, lanes, ignoreList, permittedList, lane1, down1, up1,
1223 simulator, widthGenerator, leftEdges, rightEdges);
1224 executor.execute(new CbrTaskBig(numberOfJobs, cbr));
1225
1226 }
1227
1228 long time = System.currentTimeMillis();
1229
1230 while (numberOfJobs.get() > 0 && System.currentTimeMillis() - time < 60000)
1231 {
1232 try
1233 {
1234 Thread.sleep(10);
1235 }
1236 catch (InterruptedException exception)
1237 {
1238
1239 }
1240 }
1241
1242 executor.shutdown();
1243 while (!executor.isTerminated())
1244 {
1245 try
1246 {
1247 Thread.sleep(1);
1248 }
1249 catch (InterruptedException exception)
1250 {
1251
1252 }
1253 }
1254
1255 simulator.getLogger().always()
1256 .debug(String.format(
1257 "generating conflicts complete (generated %d merge conflicts, %d split "
1258 + "conflicts, %d crossing conflicts)",
1259 numberMergeConflicts.get(), numberSplitConflicts.get(), numberCrossConflicts.get()));
1260 }
1261
1262
1263
1264
1265
1266
1267
1268
1269
1270
1271 public static void buildConflictsParallel(final RoadNetwork network, final Map<String, Set<Link>> conflictCandidateMap,
1272 final OtsSimulatorInterface simulator, final WidthGenerator widthGenerator) throws OtsGeometryException
1273 {
1274 for (String conflictId : conflictCandidateMap.keySet())
1275 {
1276
1277 List<Lane> lanes = new ArrayList<>();
1278 for (Link link : conflictCandidateMap.get(conflictId))
1279 {
1280 if (link instanceof CrossSectionLink)
1281 {
1282 for (CrossSectionElement element : ((CrossSectionLink) link).getCrossSectionElementList())
1283 {
1284 if (element instanceof Lane)
1285 {
1286 lanes.add((Lane) element);
1287 }
1288 }
1289 }
1290 }
1291
1292
1293 buildConflicts(lanes, simulator, widthGenerator, new LaneCombinationList(), new LaneCombinationList(), conflictId);
1294 simulator.getLogger().setAllLogLevel(Level.DEBUG);
1295 }
1296 }
1297
1298
1299 static class CbrTaskSmall implements Runnable
1300 {
1301
1302 final ConflictBuilderRecordSmall cbr;
1303
1304
1305 final AtomicInteger nrOfJobs;
1306
1307
1308
1309
1310
1311 CbrTaskSmall(final AtomicInteger nrOfJobs, final ConflictBuilderRecordSmall cbr)
1312 {
1313 this.nrOfJobs = nrOfJobs;
1314 this.cbr = cbr;
1315 }
1316
1317 @Override
1318 public void run()
1319 {
1320
1321 try
1322 {
1323 buildConflictsSmall(this.cbr);
1324 }
1325 catch (Exception e)
1326 {
1327 e.printStackTrace();
1328 }
1329 this.nrOfJobs.decrementAndGet();
1330 }
1331
1332 }
1333
1334
1335
1336
1337
1338 static void buildConflictsSmall(final ConflictBuilderRecordSmall cbr)
1339 {
1340
1341 try
1342 {
1343 buildConflicts(cbr.lane1, cbr.down1, cbr.up1, cbr.lane2, cbr.down2, cbr.up2, cbr.permitted, cbr.simulator,
1344 cbr.widthGenerator, cbr.leftEdges, cbr.rightEdges, false, null);
1345 }
1346 catch (NetworkException | OtsGeometryException ne)
1347 {
1348 throw new RuntimeException("Conflict build with bad combination of types / rules.", ne);
1349 }
1350 }
1351
1352
1353 @SuppressWarnings("checkstyle:visibilitymodifier")
1354 static class ConflictBuilderRecordSmall
1355 {
1356
1357 final Lane lane1;
1358
1359
1360 final Set<Lane> down1;
1361
1362
1363 final Set<Lane> up1;
1364
1365
1366 final Lane lane2;
1367
1368
1369 final Set<Lane> down2;
1370
1371
1372 final Set<Lane> up2;
1373
1374
1375 final boolean permitted;
1376
1377
1378 final OtsSimulatorInterface simulator;
1379
1380
1381 final WidthGenerator widthGenerator;
1382
1383
1384 final Map<Lane, OtsLine3d> leftEdges;
1385
1386
1387 final Map<Lane, OtsLine3d> rightEdges;
1388
1389
1390
1391
1392
1393
1394
1395
1396
1397
1398
1399
1400
1401
1402
1403 @SuppressWarnings("checkstyle:parameternumber")
1404 ConflictBuilderRecordSmall(final Lane lane1, final Set<Lane> down1, final Set<Lane> up1, final Lane lane2,
1405 final Set<Lane> down2, final Set<Lane> up2, final boolean permitted, final OtsSimulatorInterface simulator,
1406 final WidthGenerator widthGenerator, final Map<Lane, OtsLine3d> leftEdges,
1407 final Map<Lane, OtsLine3d> rightEdges)
1408 {
1409 this.lane1 = lane1;
1410 this.down1 = down1;
1411 this.up1 = up1;
1412 this.lane2 = lane2;
1413 this.down2 = down2;
1414 this.up2 = up2;
1415 this.permitted = permitted;
1416 this.simulator = simulator;
1417 this.widthGenerator = widthGenerator;
1418 this.leftEdges = leftEdges;
1419 this.rightEdges = rightEdges;
1420 }
1421 }
1422
1423
1424 static class CbrTaskBig implements Runnable
1425 {
1426
1427 final ConflictBuilderRecordBig cbr;
1428
1429
1430 final AtomicInteger nrOfJobs;
1431
1432
1433
1434
1435
1436 CbrTaskBig(final AtomicInteger nrOfJobs, final ConflictBuilderRecordBig cbr)
1437 {
1438 this.nrOfJobs = nrOfJobs;
1439 this.cbr = cbr;
1440 }
1441
1442 @Override
1443 public void run()
1444 {
1445
1446 try
1447 {
1448 for (int j = this.cbr.starti + 1; j < this.cbr.lanes.size(); j++)
1449 {
1450 Lane lane2 = this.cbr.lanes.get(j);
1451 if (this.cbr.ignoreList.contains(this.cbr.lane1, lane2))
1452 {
1453 continue;
1454 }
1455
1456 try
1457 {
1458 if (!this.cbr.lane1.getContour().intersects(lane2.getContour()))
1459 {
1460 continue;
1461 }
1462 }
1463 catch (Exception e)
1464 {
1465 System.err.println("Contour problem - lane1 = [" + this.cbr.lane1.getFullId() + "], lane2 = ["
1466 + lane2.getFullId() + "]; skipped");
1467 continue;
1468 }
1469
1470 boolean permitted = this.cbr.permittedList.contains(this.cbr.lane1, lane2);
1471
1472 Set<Lane> down2 = lane2.nextLanes(null);
1473 Set<Lane> up2 = lane2.prevLanes(null);
1474
1475 try
1476 {
1477 buildConflicts(this.cbr.lane1, this.cbr.down1, this.cbr.up1, lane2, down2, up2, permitted,
1478 this.cbr.simulator, this.cbr.widthGenerator, this.cbr.leftEdges, this.cbr.rightEdges, false,
1479 null);
1480 }
1481 catch (NetworkException | OtsGeometryException ne)
1482 {
1483 lane2.getParentLink().getSimulator().getLogger().always().error(ne,
1484 "Conflict build with bad combination of types / rules.");
1485 }
1486 }
1487
1488 }
1489 catch (Exception e)
1490 {
1491 e.printStackTrace();
1492 }
1493 this.nrOfJobs.decrementAndGet();
1494 }
1495 }
1496
1497
1498 @SuppressWarnings("checkstyle:visibilitymodifier")
1499 static class ConflictBuilderRecordBig
1500 {
1501
1502 final int starti;
1503
1504
1505 final List<Lane> lanes;
1506
1507
1508 final LaneCombinationList ignoreList;
1509
1510
1511 final LaneCombinationList permittedList;
1512
1513
1514 final Lane lane1;
1515
1516
1517 final Set<Lane> down1;
1518
1519
1520 final Set<Lane> up1;
1521
1522
1523 final OtsSimulatorInterface simulator;
1524
1525
1526 final WidthGenerator widthGenerator;
1527
1528
1529 final Map<Lane, OtsLine3d> leftEdges;
1530
1531
1532 final Map<Lane, OtsLine3d> rightEdges;
1533
1534
1535
1536
1537
1538
1539
1540
1541
1542
1543
1544
1545
1546
1547
1548 @SuppressWarnings("checkstyle:parameternumber")
1549 ConflictBuilderRecordBig(final int starti, final List<Lane> lanes, final LaneCombinationList ignoreList,
1550 final LaneCombinationList permittedList, final Lane lane1, final Set<Lane> down1, final Set<Lane> up1,
1551 final OtsSimulatorInterface simulator, final WidthGenerator widthGenerator,
1552 final Map<Lane, OtsLine3d> leftEdges, final Map<Lane, OtsLine3d> rightEdges)
1553 {
1554 this.starti = starti;
1555 this.lanes = lanes;
1556 this.ignoreList = ignoreList;
1557 this.permittedList = permittedList;
1558 this.lane1 = lane1;
1559 this.down1 = down1;
1560 this.up1 = up1;
1561 this.simulator = simulator;
1562 this.widthGenerator = widthGenerator;
1563 this.leftEdges = leftEdges;
1564 this.rightEdges = rightEdges;
1565 }
1566 }
1567
1568 }