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