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