1 package org.opentrafficsim.road.gtu.lane.tactical.util;
2
3 import java.io.Serializable;
4 import java.util.ArrayList;
5 import java.util.Iterator;
6 import java.util.LinkedHashMap;
7 import java.util.List;
8 import java.util.SortedMap;
9 import java.util.TreeMap;
10 import java.util.UUID;
11
12 import org.djunits.unit.AccelerationUnit;
13 import org.djunits.unit.DurationUnit;
14 import org.djunits.unit.LengthUnit;
15 import org.djunits.value.vdouble.scalar.Acceleration;
16 import org.djunits.value.vdouble.scalar.Duration;
17 import org.djunits.value.vdouble.scalar.Length;
18 import org.djunits.value.vdouble.scalar.Speed;
19 import org.djunits.value.vdouble.scalar.Time;
20 import org.djutils.exceptions.Throw;
21 import org.opentrafficsim.base.parameters.ParameterException;
22 import org.opentrafficsim.base.parameters.ParameterTypeAcceleration;
23 import org.opentrafficsim.base.parameters.ParameterTypeDouble;
24 import org.opentrafficsim.base.parameters.ParameterTypeDuration;
25 import org.opentrafficsim.base.parameters.ParameterTypeLength;
26 import org.opentrafficsim.base.parameters.ParameterTypes;
27 import org.opentrafficsim.base.parameters.Parameters;
28 import org.opentrafficsim.base.parameters.constraint.ConstraintInterface;
29 import org.opentrafficsim.core.definitions.DefaultsNl;
30 import org.opentrafficsim.core.gtu.GtuException;
31 import org.opentrafficsim.core.gtu.TurnIndicatorIntent;
32 import org.opentrafficsim.core.network.Node;
33 import org.opentrafficsim.core.network.route.Route;
34 import org.opentrafficsim.road.gtu.lane.LaneBasedGtu;
35 import org.opentrafficsim.road.gtu.lane.perception.PerceptionCollectable;
36 import org.opentrafficsim.road.gtu.lane.perception.PerceptionIterable;
37 import org.opentrafficsim.road.gtu.lane.perception.RelativeLane;
38 import org.opentrafficsim.road.gtu.lane.perception.headway.AbstractHeadwayGtu;
39 import org.opentrafficsim.road.gtu.lane.perception.headway.HeadwayConflict;
40 import org.opentrafficsim.road.gtu.lane.perception.headway.HeadwayGtu;
41 import org.opentrafficsim.road.gtu.lane.perception.headway.HeadwayGtuSimple;
42 import org.opentrafficsim.road.gtu.lane.perception.headway.HeadwayStopLine;
43 import org.opentrafficsim.road.gtu.lane.tactical.Blockable;
44 import org.opentrafficsim.road.gtu.lane.tactical.following.CarFollowingModel;
45 import org.opentrafficsim.road.gtu.lane.tactical.pt.BusSchedule;
46 import org.opentrafficsim.road.network.lane.CrossSectionLink;
47 import org.opentrafficsim.road.network.lane.conflict.BusStopConflictRule;
48 import org.opentrafficsim.road.network.lane.conflict.ConflictRule;
49 import org.opentrafficsim.road.network.lane.conflict.ConflictType;
50 import org.opentrafficsim.road.network.speed.SpeedLimitInfo;
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71 public final class ConflictUtil
72 {
73
74
75 public static final ParameterTypeDuration MIN_GAP = new ParameterTypeDuration("minGap", "Minimum gap for conflicts",
76 new Duration(0.000001, DurationUnit.SECOND), ConstraintInterface.POSITIVE);
77
78
79 public static final ParameterTypeAcceleration B = ParameterTypes.B;
80
81
82 public static final ParameterTypeAcceleration BCRIT = ParameterTypes.BCRIT;
83
84
85 public static final ParameterTypeLength S0 = ParameterTypes.S0;
86
87
88 public static final ParameterTypeLength S0_CONF = new ParameterTypeLength("s0conf", "Stopping distance at conflicts",
89 new Length(1.5, LengthUnit.METER), ConstraintInterface.POSITIVE);
90
91
92 public static final ParameterTypeDouble TIME_FACTOR =
93 new ParameterTypeDouble("timeFactor", "Safety factor on estimated time", 1.25, ConstraintInterface.ATLEASTONE);
94
95
96 public static final ParameterTypeLength STOP_AREA =
97 new ParameterTypeLength("stopArea", "Area before stop line where one is considered arrived at the intersection",
98 new Length(4, LengthUnit.METER), ConstraintInterface.POSITIVE);
99
100
101 public static final ParameterTypeDuration TI = new ParameterTypeDuration("ti", "Indicator time before bus departure",
102 Duration.instantiateSI(3.0), ConstraintInterface.POSITIVE);
103
104
105 private static final Duration TIME_STEP = new Duration(0.5, DurationUnit.SI);
106
107
108 private static boolean CROSSSTANDING = true;
109
110
111
112
113 private ConflictUtil()
114 {
115
116 }
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139 @SuppressWarnings("checkstyle:parameternumber")
140
141 public static Acceleration approachConflicts(final Parameters parameters, final Iterable<HeadwayConflict> conflicts,
142 final PerceptionCollectable<HeadwayGtu, LaneBasedGtu> leaders, final CarFollowingModel carFollowingModel,
143 final Length vehicleLength, final Length vehicleWidth, final Speed speed, final Acceleration acceleration,
144 final SpeedLimitInfo speedLimitInfo, final ConflictPlans conflictPlans, final LaneBasedGtu gtu,
145 final RelativeLane lane) throws GtuException, ParameterException
146 {
147
148 conflictPlans.cleanPlans();
149
150 Acceleration a = Acceleration.POS_MAXVALUE;
151 Length stoppingDistance = Length.instantiateSI(
152 parameters.getParameter(S0).si + vehicleLength.si + .5 * speed.si * speed.si / parameters.getParameter(B).si);
153 Iterator<HeadwayConflict> it = conflicts.iterator();
154 if (it.hasNext() && it.next().getDistance().gt(stoppingDistance))
155 {
156 conflictPlans.setBlocking(false);
157 return a;
158 }
159
160 List<Length> prevStarts = new ArrayList<>();
161 List<Length> prevEnds = new ArrayList<>();
162 List<Class<? extends ConflictRule>> conflictRuleTypes = new ArrayList<>();
163 boolean blocking = false;
164
165 for (HeadwayConflict conflict : conflicts)
166 {
167
168
169 if (conflict.isCrossing())
170 {
171
172 a = Acceleration.min(a, avoidCrossingCollision(parameters, conflict, carFollowingModel, speed, speedLimitInfo));
173 }
174 else
175 {
176 if (conflict.isMerge() && !lane.isCurrent() && conflict.getConflictPriority().isPriority())
177 {
178
179 a = Acceleration.min(a,
180 avoidMergeCollision(parameters, conflict, carFollowingModel, speed, speedLimitInfo));
181 }
182
183 a = Acceleration.min(a, followConflictingLeaderOnMergeOrSplit(conflict, parameters, carFollowingModel, speed,
184 speedLimitInfo, vehicleWidth));
185 }
186
187
188 if (lane.isCurrent())
189 {
190
191 if (gtu.getStrategicalPlanner().getRoute() instanceof BusSchedule && gtu.getType().isOfType(DefaultsNl.BUS)
192 && conflict.getConflictRuleType().equals(BusStopConflictRule.class))
193 {
194 BusSchedule busSchedule = (BusSchedule) gtu.getStrategicalPlanner().getRoute();
195 Time actualDeparture = busSchedule.getActualDepartureConflict(conflict.getId());
196 if (actualDeparture != null
197 && actualDeparture.si < gtu.getSimulator().getSimulatorTime().si + parameters.getParameter(TI).si)
198 {
199
200 conflictPlans.setIndicatorIntent(TurnIndicatorIntent.LEFT, conflict.getDistance());
201 }
202 }
203 }
204
205
206 if (conflict.getDistance().lt0() && lane.isCurrent())
207 {
208 if (conflict.getConflictType().isCrossing() && !conflict.getConflictPriority().isPriority())
209 {
210
211 blocking = true;
212 }
213
214 continue;
215 }
216
217
218 boolean stop;
219 switch (conflict.getConflictPriority())
220 {
221 case PRIORITY:
222 {
223 Length prevEnd = prevEnds.isEmpty() ? null : prevEnds.get(prevEnds.size() - 1);
224 stop = stopForPriorityConflict(conflict, leaders, speed, vehicleLength, parameters, prevEnd);
225 break;
226 }
227 case YIELD:
228 {
229 Length prevEnd = prevEnds.isEmpty() ? null : prevEnds.get(prevEnds.size() - 1);
230 stop = stopForGiveWayConflict(conflict, leaders, speed, acceleration, vehicleLength, parameters,
231 speedLimitInfo, carFollowingModel, blocking ? BCRIT : B, prevEnd);
232 break;
233 }
234 case STOP:
235 {
236 Length prevEnd = prevEnds.isEmpty() ? null : prevEnds.get(prevEnds.size() - 1);
237 stop = stopForStopConflict(conflict, leaders, speed, acceleration, vehicleLength, parameters,
238 speedLimitInfo, carFollowingModel, blocking ? BCRIT : B, prevEnd);
239 break;
240 }
241 case ALL_STOP:
242 {
243 stop = stopForAllStopConflict(conflict, conflictPlans);
244 break;
245 }
246 case SPLIT:
247 {
248 stop = false;
249 break;
250 }
251 default:
252 {
253 throw new GtuException("Unsupported conflict rule encountered while approaching conflicts.");
254 }
255 }
256
257
258 if (!conflict.getConflictType().equals(ConflictType.SPLIT))
259 {
260
261 if (stop)
262 {
263 prevStarts.add(conflict.getDistance());
264 conflictRuleTypes.add(conflict.getConflictRuleType());
265
266 int j = 0;
267 for (int i = prevEnds.size() - 1; i >= 0; i--)
268 {
269
270 if (prevStarts.get(i + 1).minus(prevEnds.get(i)).gt(passableDistance(vehicleLength, parameters)))
271 {
272 j = i + 1;
273 break;
274 }
275 }
276 if (blocking && j == 0)
277 {
278
279 j = prevStarts.size() - 1;
280 }
281
282
283
284
285
286 parameters.setParameterResettable(S0, parameters.getParameter(S0_CONF));
287 Acceleration aCF = new Acceleration(-Double.MAX_VALUE, AccelerationUnit.SI);
288 while (aCF.si < -6.0 && j < prevStarts.size())
289 {
290 if (prevStarts.get(j).lt(parameters.getParameter(S0_CONF)))
291 {
292
293
294 aCF = Acceleration.max(aCF, new Acceleration(-6.0, AccelerationUnit.SI));
295 }
296 else
297 {
298 Acceleration aStop = CarFollowingUtil.stop(carFollowingModel, parameters, speed, speedLimitInfo,
299 prevStarts.get(j));
300 if (conflictRuleTypes.get(j).equals(BusStopConflictRule.class)
301 && aStop.lt(parameters.getParameter(ParameterTypes.BCRIT).neg()))
302 {
303
304 aStop = Acceleration.POS_MAXVALUE;
305 }
306 aCF = Acceleration.max(aCF, aStop);
307 }
308 j++;
309 }
310 parameters.resetParameter(S0);
311 a = Acceleration.min(a, aCF);
312 break;
313 }
314
315
316 prevStarts.add(conflict.getDistance());
317 conflictRuleTypes.add(conflict.getConflictRuleType());
318 prevEnds.add(conflict.getDistance().plus(conflict.getLength()));
319 }
320
321 }
322 conflictPlans.setBlocking(blocking);
323
324 if (a.si < -6.0 && speed.si > 5 / 3.6)
325 {
326 System.err.println("Deceleration from conflict util stronger than 6m/s^2.");
327
328 }
329 return a;
330 }
331
332
333
334
335
336
337
338
339
340
341
342
343 private static Acceleration followConflictingLeaderOnMergeOrSplit(final HeadwayConflict conflict,
344 final Parameters parameters, final CarFollowingModel carFollowingModel, final Speed speed,
345 final SpeedLimitInfo speedLimitInfo, final Length vehicleWidth) throws ParameterException
346 {
347
348 PerceptionIterable<HeadwayGtu> downstreamGTUs = conflict.getDownstreamConflictingGTUs();
349 if (downstreamGTUs.isEmpty() || downstreamGTUs.first().isAhead())
350 {
351 return Acceleration.POS_MAXVALUE;
352 }
353
354 HeadwayGtu c = null;
355 Length virtualHeadway = null;
356 if (conflict.getDistance().gt0())
357 {
358 c = downstreamGTUs.first();
359 virtualHeadway = conflict.getDistance().plus(c.getOverlapRear());
360 }
361 else
362 {
363 for (HeadwayGtu con : downstreamGTUs)
364 {
365 if (con.isAhead())
366 {
367
368 return Acceleration.POS_MAXVALUE;
369 }
370
371
372
373
374
375
376
377
378
379 virtualHeadway = conflict.getDistance().plus(con.getOverlapRear());
380 if (virtualHeadway.gt0())
381 {
382 if (conflict.isSplit())
383 {
384 double conflictWidth = conflict.getWidthAtFraction(
385 (-conflict.getDistance().si + virtualHeadway.si) / conflict.getConflictingLength().si).si;
386 double gtuWidth = con.getWidth().si + vehicleWidth.si;
387 if (conflictWidth > gtuWidth)
388 {
389 continue;
390 }
391 }
392
393 c = con;
394 break;
395 }
396 }
397 }
398 if (c == null)
399 {
400
401 return Acceleration.POS_MAXVALUE;
402 }
403
404 SortedMap<Length, Speed> leaders = new TreeMap<>();
405 leaders.put(virtualHeadway, c.getSpeed());
406 Acceleration a = CarFollowingUtil.followSingleLeader(carFollowingModel, parameters, speed, speedLimitInfo,
407 virtualHeadway, c.getSpeed());
408
409
410 if (conflict.isMerge() && virtualHeadway.lt(conflict.getDistance()))
411 {
412
413
414
415
416
417
418
419
420
421
422 parameters.setParameterResettable(S0, parameters.getParameter(S0_CONF));
423 Acceleration aStop =
424 CarFollowingUtil.stop(carFollowingModel, parameters, speed, speedLimitInfo, conflict.getDistance());
425 parameters.resetParameter(S0);
426 a = Acceleration.max(a, aStop);
427 }
428 return a;
429 }
430
431
432
433
434
435
436
437
438
439
440
441 private static Acceleration avoidCrossingCollision(final Parameters parameters, final HeadwayConflict conflict,
442 final CarFollowingModel carFollowingModel, final Speed speed, final SpeedLimitInfo speedLimitInfo)
443 throws ParameterException
444 {
445
446
447 List<HeadwayGtu> conflictingGTUs = new ArrayList<>();
448 for (HeadwayGtu gtu : conflict.getUpstreamConflictingGTUs())
449 {
450 if (isOnRoute(conflict.getConflictingLink(), gtu))
451 {
452
453 conflictingGTUs.add(gtu);
454 break;
455 }
456 }
457 for (HeadwayGtu gtu : conflict.getDownstreamConflictingGTUs())
458 {
459 if (gtu.isParallel())
460 {
461 conflictingGTUs.add(gtu);
462 }
463 else
464 {
465
466 break;
467 }
468 }
469
470 if (conflictingGTUs.isEmpty())
471 {
472 return Acceleration.POS_MAXVALUE;
473 }
474
475 Acceleration a = Acceleration.POS_MAXVALUE;
476 for (HeadwayGtu conflictingGTU : conflictingGTUs)
477 {
478 AnticipationInfo tteC;
479 Length distance;
480 if (conflictingGTU.isParallel())
481 {
482 tteC = new AnticipationInfo(Duration.ZERO, conflictingGTU.getSpeed());
483 distance = conflictingGTU.getOverlapRear().abs().plus(conflictingGTU.getOverlap())
484 .plus(conflictingGTU.getOverlapFront().abs());
485 }
486 else
487 {
488 tteC = AnticipationInfo.anticipateMovement(conflictingGTU.getDistance(), conflictingGTU.getSpeed(),
489 Acceleration.ZERO);
490 distance = conflictingGTU.getDistance().plus(conflict.getLength()).plus(conflictingGTU.getLength());
491 }
492 AnticipationInfo ttcC = AnticipationInfo.anticipateMovement(distance, conflictingGTU.getSpeed(), Acceleration.ZERO);
493 AnticipationInfo tteO = AnticipationInfo.anticipateMovementFreeAcceleration(conflict.getDistance(), speed,
494 parameters, carFollowingModel, speedLimitInfo, TIME_STEP);
495
496
497 if (tteC.duration().lt(tteO.duration()) && tteO.duration().lt(ttcC.duration()))
498 {
499 if (!conflictingGTU.getSpeed().eq0() || !CROSSSTANDING)
500 {
501
502 double acc = 2 * (conflict.getDistance().si - speed.si * ttcC.duration().si)
503 / (ttcC.duration().si * ttcC.duration().si);
504
505 if (speed.si / -acc > ttcC.duration().si)
506 {
507 a = Acceleration.min(a, new Acceleration(acc, AccelerationUnit.SI));
508 }
509 else
510 {
511
512 a = Acceleration.min(a, CarFollowingUtil.stop(carFollowingModel, parameters, speed, speedLimitInfo,
513 conflict.getDistance()));
514 }
515 }
516
517 }
518 }
519 return a;
520 }
521
522
523
524
525
526
527
528
529
530
531
532 private static Acceleration avoidMergeCollision(final Parameters parameters, final HeadwayConflict conflict,
533 final CarFollowingModel carFollowingModel, final Speed speed, final SpeedLimitInfo speedLimitInfo)
534 throws ParameterException
535 {
536 PerceptionCollectable<HeadwayGtu, LaneBasedGtu> conflicting = conflict.getUpstreamConflictingGTUs();
537 if (conflicting.isEmpty() || conflicting.first().isParallel())
538 {
539 return Acceleration.POS_MAXVALUE;
540 }
541
542 HeadwayGtu conflictingGtu = conflicting.first();
543 double tteC = conflictingGtu.getDistance().si / conflictingGtu.getSpeed().si;
544 if (tteC < conflict.getDistance().si / speed.si + 3.0)
545 {
546 return CarFollowingUtil.stop(carFollowingModel, parameters, speed, speedLimitInfo, conflict.getDistance());
547 }
548 return Acceleration.POS_MAXVALUE;
549 }
550
551
552
553
554
555
556
557
558
559
560
561
562
563 public static boolean stopForPriorityConflict(final HeadwayConflict conflict,
564 final PerceptionCollectable<HeadwayGtu, LaneBasedGtu> leaders, final Speed speed, final Length vehicleLength,
565 final Parameters parameters, final Length prevEnd) throws ParameterException
566 {
567
568
569 Length passable = passableDistance(vehicleLength, parameters);
570 if (prevEnd != null && conflict.isMerge() && !conflict.getDownstreamConflictingGTUs().isEmpty())
571 {
572 HeadwayGtu conflictingGTU = conflict.getDownstreamConflictingGTUs().first();
573 Acceleration b = parameters.getParameter(BCRIT);
574 double t = conflictingGTU.getSpeed().divide(b).si;
575 Length stopDistance = Length.instantiateSI(conflictingGTU.getSpeed().si * t - .5 * b.si * t * t);
576 Length room = conflict
577 .getDistance().plus(stopDistance).plus(conflictingGTU.isAhead()
578 ? conflict.getLength().plus(conflictingGTU.getDistance()) : conflictingGTU.getOverlapRear())
579 .minus(prevEnd);
580 if (room.lt(passable))
581 {
582 return true;
583 }
584 }
585
586
587 if (leaders.isEmpty())
588 {
589
590 return false;
591 }
592 if (conflict.getUpstreamConflictingGTUs().isEmpty())
593 {
594
595 return false;
596 }
597 else
598 {
599 HeadwayGtu conflictingGTU = conflict.getUpstreamConflictingGTUs().first();
600 if (conflictingGTU.getSpeed().eq0() && conflictingGTU.isAhead()
601 && conflictingGTU.getDistance().gt(parameters.getParameter(S0)))
602 {
603
604 return false;
605 }
606 }
607
608
609
610 Length typeCorrection = conflict.isCrossing() ? conflict.getLength() : Length.ZERO;
611
612 Length required = conflict.getDistance().plus(typeCorrection).plus(passable);
613
614 Length requiredLeader = required.minus(leaders.first().getDistance());
615 if (requiredLeader.gt0())
616 {
617 for (HeadwayGtu leader : leaders)
618 {
619 if (leader.getSpeed().eq0())
620 {
621
622
623 return leader.getDistance().ge(conflict.getDistance()) && required.ge(leader.getDistance());
624 }
625 required = required
626 .plus(passableDistance(leader.getLength(), leader.getParameters()));
627 }
628 }
629
630
631 return false;
632 }
633
634
635
636
637
638
639
640
641
642
643
644
645
646
647
648
649 @SuppressWarnings({"checkstyle:parameternumber", "checkstyle:methodlength"})
650 public static boolean stopForGiveWayConflict(final HeadwayConflict conflict,
651 final PerceptionCollectable<HeadwayGtu, LaneBasedGtu> leaders, final Speed speed, final Acceleration acceleration,
652 final Length vehicleLength, final Parameters parameters, final SpeedLimitInfo speedLimitInfo,
653 final CarFollowingModel carFollowingModel, final ParameterTypeAcceleration bType, final Length prevEnd)
654 throws ParameterException
655 {
656
657
658
659 if (conflict.getConflictType().isCrossing() && !conflict.getDownstreamConflictingGTUs().isEmpty()
660 && conflict.getDownstreamConflictingGTUs().first().isParallel())
661 {
662
663 return true;
664 }
665
666
667
668 Acceleration b = parameters.getParameter(bType).neg();
669 double f = parameters.getParameter(TIME_FACTOR);
670 Duration gap = parameters.getParameter(MIN_GAP);
671 Length passable = passableDistance(vehicleLength, parameters);
672
673 Length distance = conflict.getDistance().plus(vehicleLength);
674 if (conflict.isCrossing())
675 {
676
677 distance = distance.plus(conflict.getLength());
678 }
679
680 AnticipationInfo ttcOa = AnticipationInfo.anticipateMovementFreeAcceleration(distance, speed, parameters,
681 carFollowingModel, speedLimitInfo, TIME_STEP);
682
683 AnticipationInfo ttpDz = null;
684 AnticipationInfo ttpDs = null;
685 if (conflict.isCrossing())
686 {
687 if (!leaders.isEmpty())
688 {
689 distance =
690 conflict.getDistance().minus(leaders.first().getDistance()).plus(conflict.getLength()).plus(passable);
691 ttpDz = AnticipationInfo.anticipateMovement(distance, leaders.first().getSpeed(), Acceleration.ZERO);
692 ttpDs = AnticipationInfo.anticipateMovement(distance, leaders.first().getSpeed(), b);
693 }
694 else
695 {
696
697 ttpDz = new AnticipationInfo(Duration.ZERO, Speed.ZERO);
698 ttpDs = new AnticipationInfo(Duration.ZERO, Speed.ZERO);
699 }
700 }
701 else if (conflict.isMerge() && prevEnd != null)
702 {
703
704 Length preGap = conflict.getDistance().minus(prevEnd);
705 PerceptionCollectable<HeadwayGtu, LaneBasedGtu> downs = conflict.getDownstreamConflictingGTUs();
706 if (!downs.isEmpty() && downs.first().isParallel())
707 {
708 distance = passable.minus(preGap).minus(downs.first().getOverlapRear());
709 ttpDz = AnticipationInfo.anticipateMovement(distance, downs.first().getSpeed(), Acceleration.ZERO);
710 if (ttpDz.duration().equals(Duration.POSITIVE_INFINITY))
711 {
712
713 return true;
714 }
715 ttpDs = AnticipationInfo.anticipateMovement(distance, downs.first().getSpeed(), b);
716 }
717 else if (!leaders.isEmpty())
718 {
719 distance = conflict.getDistance().plus(passable).minus(preGap).minus(leaders.first().getDistance());
720 ttpDz = AnticipationInfo.anticipateMovement(distance, leaders.first().getSpeed(), Acceleration.ZERO);
721 if (ttpDz.duration().equals(Duration.POSITIVE_INFINITY))
722 {
723
724 return true;
725 }
726 ttpDs = AnticipationInfo.anticipateMovement(distance, leaders.first().getSpeed(), b);
727 }
728 }
729
730 PerceptionCollectable<HeadwayGtu, LaneBasedGtu> conflictingVehiclesCollectable = conflict.getUpstreamConflictingGTUs();
731 Iterable<HeadwayGtu> conflictingVehicles;
732 if (conflictingVehiclesCollectable.isEmpty())
733 {
734 if (conflict.getConflictingTrafficLightDistance() == null)
735 {
736
737 try
738 {
739 HeadwayGtuSimple conflictGtu = new HeadwayGtuSimple("virtual " + UUID.randomUUID().toString(),
740 DefaultsNl.CAR, conflict.getConflictingVisibility(), new Length(4.0, LengthUnit.SI),
741 new Length(2.0, LengthUnit.SI), conflict.getConflictingSpeedLimit(), Acceleration.ZERO, Speed.ZERO);
742 List<HeadwayGtu> conflictingVehiclesList = new ArrayList<>();
743 conflictingVehiclesList.add(conflictGtu);
744 conflictingVehicles = conflictingVehiclesList;
745 }
746 catch (GtuException exception)
747 {
748 throw new RuntimeException("Could not create a virtual conflicting vehicle at visibility range.",
749 exception);
750 }
751 }
752 else
753 {
754
755 return false;
756 }
757 }
758 else
759 {
760 HeadwayGtu conflicting = conflictingVehiclesCollectable.first();
761 if (conflict.getConflictingTrafficLightDistance() != null && conflicting.isAhead()
762 && conflict.getConflictingTrafficLightDistance().lt(conflicting.getDistance())
763 && (conflicting.getSpeed().eq0() || conflicting.getAcceleration().lt0()))
764 {
765
766 return false;
767 }
768 conflictingVehicles = conflictingVehiclesCollectable;
769 }
770
771
772 boolean first = true;
773 for (HeadwayGtu conflictingVehicle : conflictingVehicles)
774 {
775
776
777 if (!isOnRoute(conflict.getConflictingLink(), conflictingVehicle))
778 {
779 continue;
780 }
781
782
783 AnticipationInfo tteCa;
784 AnticipationInfo tteCs;
785 if (first && conflictingVehicle.getSpeed().eq0() && conflictingVehicle.isAhead())
786 {
787
788 return false;
789 }
790 else
791 {
792 if (conflictingVehicle instanceof HeadwayGtuSimple)
793 {
794 tteCa = AnticipationInfo.anticipateMovement(conflictingVehicle.getDistance(), conflictingVehicle.getSpeed(),
795 conflictingVehicle.getAcceleration());
796 }
797 else
798 {
799 Parameters params = conflictingVehicle.getParameters();
800 SpeedLimitInfo sli = conflictingVehicle.getSpeedLimitInfo();
801 CarFollowingModel cfm = conflictingVehicle.getCarFollowingModel();
802
803 if (conflictingVehicle.isAhead())
804 {
805 tteCa = AnticipationInfo.anticipateMovementFreeAcceleration(conflictingVehicle.getDistance(),
806 conflictingVehicle.getSpeed(), params, cfm, sli, TIME_STEP);
807 }
808 else
809 {
810 tteCa = new AnticipationInfo(Duration.ZERO, conflictingVehicle.getSpeed());
811 }
812 }
813 if (conflictingVehicle.isAhead())
814 {
815 tteCs = AnticipationInfo.anticipateMovement(conflictingVehicle.getDistance(), conflictingVehicle.getSpeed(),
816 b);
817 }
818 else
819 {
820 tteCs = new AnticipationInfo(Duration.ZERO, conflictingVehicle.getSpeed());
821 }
822 }
823
824
825 if (conflict.isMerge())
826 {
827
828
829 double vSelf = ttcOa.endSpeed().si;
830 double speedDiff = conflictingVehicle.getSpeed().si - vSelf;
831 speedDiff = speedDiff > 0 ? speedDiff : 0;
832 Duration additionalTime = new Duration(speedDiff / -b.si, DurationUnit.SI);
833
834 double followerFront = conflictingVehicle.isAhead()
835 ? conflictingVehicle.getSpeed().si * ttcOa.duration().si - conflictingVehicle.getDistance().si
836 + (conflictingVehicle.getSpeed().si * additionalTime.si
837 + 0.5 * b.si * additionalTime.si * additionalTime.si)
838 : 0.0;
839 double ownRear = vSelf * additionalTime.si;
840 Duration tMax = parameters.getParameter(ParameterTypes.TMAX);
841 Length s0 = parameters.getParameter(S0);
842
843
844
845 if (ttcOa.duration().times(f).plus(gap).gt(tteCa.duration())
846
847
848
849 || (!Double.isInfinite(tteCa.duration().si) && tteCa.duration().si > 0.0
850 && ownRear < (followerFront + (tMax.si + gap.si) * vSelf + s0.si) * f))
851 {
852 return true;
853 }
854
855 }
856 else if (conflict.isCrossing())
857 {
858
859
860
861
862
863
864
865 if (ttpDz.duration().times(f).plus(gap).gt(tteCa.duration())
866 || ttcOa.duration().times(f).plus(gap).gt(tteCa.duration())
867 || ttpDs.duration().times(f).plus(gap).gt(tteCs.duration())
868 || ttpDs.duration().equals(Duration.POSITIVE_INFINITY))
869 {
870 return true;
871 }
872
873 }
874 else
875 {
876 throw new RuntimeException(
877 "Conflict is of unknown type " + conflict.getConflictType() + ", which is not merge nor a crossing.");
878 }
879
880 first = false;
881 }
882
883
884 return false;
885
886 }
887
888
889
890
891
892
893
894
895
896
897
898
899
900
901
902
903 @SuppressWarnings("checkstyle:parameternumber")
904 public static boolean stopForStopConflict(final HeadwayConflict conflict,
905 final PerceptionCollectable<HeadwayGtu, LaneBasedGtu> leaders, final Speed speed, final Acceleration acceleration,
906 final Length vehicleLength, final Parameters parameters, final SpeedLimitInfo speedLimitInfo,
907 final CarFollowingModel carFollowingModel, final ParameterTypeAcceleration bType, final Length prevEnd)
908 throws ParameterException
909 {
910
911 return stopForGiveWayConflict(conflict, leaders, speed, acceleration, vehicleLength, parameters, speedLimitInfo,
912 carFollowingModel, bType, prevEnd);
913 }
914
915
916
917
918
919
920
921 public static boolean stopForAllStopConflict(final HeadwayConflict conflict, final ConflictPlans conflictPlans)
922 {
923
924
925 if (conflictPlans.isStopPhaseRun(conflict.getStopLine()))
926 {
927 return false;
928 }
929
930 return false;
931 }
932
933
934
935
936
937
938
939 private static boolean isOnRoute(final CrossSectionLink conflictingLink, final HeadwayGtu gtu)
940 {
941 try
942 {
943 Route route = gtu.getRoute();
944 if (route == null)
945 {
946
947 return true;
948 }
949 Node startNode = conflictingLink.getStartNode();
950 Node endNode = conflictingLink.getEndNode();
951 return route.contains(startNode) && route.contains(endNode)
952 && Math.abs(route.indexOf(endNode) - route.indexOf(startNode)) == 1;
953 }
954 catch (UnsupportedOperationException uoe)
955 {
956
957 return true;
958 }
959 }
960
961
962
963
964
965
966
967
968 private static Length passableDistance(final Length vehicleLength, final Parameters parameters) throws ParameterException
969 {
970 return parameters.getParameter(S0).plus(vehicleLength);
971 }
972
973
974
975
976
977
978
979
980
981
982
983
984
985
986
987
988 public static final class ConflictPlans implements Blockable, Serializable
989 {
990
991
992 private static final long serialVersionUID = 20160811L;
993
994
995 private final LinkedHashMap<String, StopPhase> stopPhases = new LinkedHashMap<>();
996
997
998 private final LinkedHashMap<String, Time> arrivalTimes = new LinkedHashMap<>();
999
1000
1001 private TurnIndicatorIntent indicatorIntent = TurnIndicatorIntent.NONE;
1002
1003
1004 private Length indicatorObjectDistance = null;
1005
1006
1007 private boolean blocking;
1008
1009
1010
1011
1012 void cleanPlans()
1013 {
1014 this.indicatorIntent = TurnIndicatorIntent.NONE;
1015 this.indicatorObjectDistance = null;
1016 }
1017
1018
1019
1020
1021
1022
1023 void setArrivalTime(final AbstractHeadwayGtu gtu, final Time time)
1024 {
1025 this.arrivalTimes.put(gtu.getId(), time);
1026 }
1027
1028
1029
1030
1031
1032
1033 Time getArrivalTime(final AbstractHeadwayGtu gtu)
1034 {
1035 return this.arrivalTimes.get(gtu.getId());
1036 }
1037
1038
1039
1040
1041
1042 void setStopPhaseApproach(final HeadwayStopLine stopLine)
1043 {
1044 this.stopPhases.put(stopLine.getId(), StopPhase.APPROACH);
1045 }
1046
1047
1048
1049
1050
1051
1052 void setStopPhaseYield(final HeadwayStopLine stopLine)
1053 {
1054 Throw.when(
1055 !this.stopPhases.containsKey(stopLine.getId())
1056 || !this.stopPhases.get(stopLine.getId()).equals(StopPhase.APPROACH),
1057 RuntimeException.class, "Yield stop phase is set for stop line that was not approached.");
1058 this.stopPhases.put(stopLine.getId(), StopPhase.YIELD);
1059 }
1060
1061
1062
1063
1064
1065
1066 void setStopPhaseRun(final HeadwayStopLine stopLine)
1067 {
1068 Throw.when(!this.stopPhases.containsKey(stopLine.getId()), RuntimeException.class,
1069 "Run stop phase is set for stop line that was not approached.");
1070 this.stopPhases.put(stopLine.getId(), StopPhase.YIELD);
1071 }
1072
1073
1074
1075
1076
1077 boolean isStopPhaseApproach(final HeadwayStopLine stopLine)
1078 {
1079 return this.stopPhases.containsKey(stopLine.getId())
1080 && this.stopPhases.get(stopLine.getId()).equals(StopPhase.APPROACH);
1081 }
1082
1083
1084
1085
1086
1087 boolean isStopPhaseYield(final HeadwayStopLine stopLine)
1088 {
1089 return this.stopPhases.containsKey(stopLine.getId())
1090 && this.stopPhases.get(stopLine.getId()).equals(StopPhase.YIELD);
1091 }
1092
1093
1094
1095
1096
1097 boolean isStopPhaseRun(final HeadwayStopLine stopLine)
1098 {
1099 return this.stopPhases.containsKey(stopLine.getId()) && this.stopPhases.get(stopLine.getId()).equals(StopPhase.RUN);
1100 }
1101
1102
1103 @Override
1104 public String toString()
1105 {
1106 return "ConflictPlans";
1107 }
1108
1109
1110
1111
1112 public TurnIndicatorIntent getIndicatorIntent()
1113 {
1114 return this.indicatorIntent;
1115 }
1116
1117
1118
1119
1120 public Length getIndicatorObjectDistance()
1121 {
1122 return this.indicatorObjectDistance;
1123 }
1124
1125
1126
1127
1128
1129 public void setIndicatorIntent(final TurnIndicatorIntent intent, final Length distance)
1130 {
1131 if (this.indicatorObjectDistance == null || this.indicatorObjectDistance.gt(distance))
1132 {
1133 this.indicatorIntent = intent;
1134 this.indicatorObjectDistance = distance;
1135 }
1136 }
1137
1138
1139 @Override
1140 public boolean isBlocking()
1141 {
1142 return this.blocking;
1143 }
1144
1145
1146
1147
1148
1149 public void setBlocking(final boolean blocking)
1150 {
1151 this.blocking = blocking;
1152 }
1153
1154 }
1155
1156
1157
1158
1159
1160
1161
1162
1163
1164
1165
1166
1167 private enum StopPhase
1168 {
1169
1170 APPROACH,
1171
1172
1173 YIELD,
1174
1175
1176 RUN;
1177 }
1178
1179 }