ConflictUtil.java

  1. package org.opentrafficsim.road.gtu.lane.tactical.util;

  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;

  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;

  53. /**
  54.  * This class implements default behavior for intersection conflicts for use in tactical planners.
  55.  * <p>
  56.  * Copyright (c) 2013-2022 Delft University of Technology, PO Box 5, 2600 AA, Delft, the Netherlands. All rights reserved. <br>
  57.  * BSD-style license. See <a href="http://opentrafficsim.org/docs/current/license.html">OpenTrafficSim License</a>.
  58.  * <p>
  59.  * @version $Revision$, $LastChangedDate$, by $Author$, initial version Jun 3, 2016 <br>
  60.  * @author <a href="http://www.tbm.tudelft.nl/averbraeck">Alexander Verbraeck</a>
  61.  * @author <a href="http://www.tudelft.nl/pknoppers">Peter Knoppers</a>
  62.  * @author <a href="http://www.transport.citg.tudelft.nl">Wouter Schakel</a>
  63.  */
  64. // TODO do not ignore vehicles upstream of conflict if they have green
  65. // TODO conflict over multiple lanes (longitudinal in own direction)
  66. // TODO a) yielding while having priority happens only when leaders is standing still on conflict (then its useless...)
  67. // b) two vehicles can remain upstream of merge if vehicle stands on merge but leaves some space to move
  68. // probably 1 is yielding, and 1 is courtesy yielding as the other stands still
  69. // c) they might start moving together and collide further down (no response to negative headway on merge)
  70. public final class ConflictUtil
  71. {

  73.     /** Minimum time gap between events. */
  74.     public static final ParameterTypeDuration MIN_GAP = new ParameterTypeDuration("minGap", "Minimum gap for conflicts",
  75.             new Duration(0.000001, DurationUnit.SECOND), ConstraintInterface.POSITIVE);

  77.     /** Comfortable deceleration. */
  78.     public static final ParameterTypeAcceleration B = ParameterTypes.B;

  80.     /** Critical deceleration. */
  81.     public static final ParameterTypeAcceleration BCRIT = ParameterTypes.BCRIT;

  83.     /** Stopping distance. */
  84.     public static final ParameterTypeLength S0 = ParameterTypes.S0;

  86.     /** Stopping distance at conflicts. */
  87.     public static final ParameterTypeLength S0_CONF = new ParameterTypeLength("s0conf", "Stopping distance at conflicts",
  88.             new Length(1.5, LengthUnit.METER), ConstraintInterface.POSITIVE);

  90.     /** Multiplication factor on time for conservative assessment. */
  91.     public static final ParameterTypeDouble TIME_FACTOR =
  92.             new ParameterTypeDouble("timeFactor", "Safety factor on estimated time", 1.25, ConstraintInterface.ATLEASTONE);

  94.     /** Area before stop line where one is considered arrived at the intersection. */
  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);

  99.     /** Parameter of how much time before departure a bus indicates its departure to get priority. */
  100.     public static final ParameterTypeDuration TI = new ParameterTypeDuration("ti", "Indicator time before bus departure",
  101.             Duration.instantiateSI(3.0), ConstraintInterface.POSITIVE);

  103.     /** Time step for free acceleration anticipation. */
  104.     private static final Duration TIME_STEP = new Duration(0.5, DurationUnit.SI);

  106.     /** Cross standing vehicles on crossings. We allow this to prevent dead-locks. A better model should render this useless. */
  107.     private static boolean CROSSSTANDING = true;

  109.     /**
  110.      * Do not instantiate.
  111.      */
  112.     private ConflictUtil()
  113.     {
  114.         //
  115.     }

  117.     /**
  118.      * Approach conflicts by applying appropriate acceleration (or deceleration). The model may yield for a vehicle even while
  119.      * having priority. Such a 'yield plan' is remembered in <code>YieldPlans</code>. By forwarding the same <code>YieldPlans</code> for
  120.      * a GTU consistency of such plans is provided. If any conflict is not accepted to pass, stopping before a more upstream
  121.      * conflict is applied if there not sufficient stopping length in between conflicts.
  122.      * @param parameters Parameters; parameters
  123.      * @param conflicts PerceptionCollectable&lt;HeadwayConflict,Conflict&gt;; set of conflicts to approach
  124.      * @param leaders PerceptionCollectable&lt;HeadwayGTU,LaneBasedGTU&gt;; leading vehicles
  125.      * @param carFollowingModel CarFollowingModel; car-following model
  126.      * @param vehicleLength Length; length of vehicle
  127.      * @param vehicleWidth Length; width of vehicle
  128.      * @param speed Speed; current speed
  129.      * @param acceleration Acceleration; current acceleration
  130.      * @param speedLimitInfo SpeedLimitInfo; speed limit info
  131.      * @param conflictPlans ConflictPlans; set of plans for conflict
  132.      * @param gtu LaneBasedGTU; gtu
  133.      * @param lane RelativeLane; lane
  134.      * @return acceleration appropriate for approaching the conflicts
  135.      * @throws GTUException in case of an unsupported conflict rule
  136.      * @throws ParameterException if a parameter is not defined or out of bounds
  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.     {

  146.         conflictPlans.cleanPlans();

  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.         }

  158.         List<Length> prevStarts = new ArrayList<>();
  159.         List<Length> prevEnds = new ArrayList<>();
  160.         List<Class<? extends ConflictRule>> conflictRuleTypes = new ArrayList<>();
  161.         boolean blocking = false;

  163.         for (HeadwayConflict conflict : conflicts)
  164.         {

  166.             // adjust acceleration for situations where stopping might not be required
  167.             if (conflict.isCrossing())
  168.             {
  169.                 // avoid collision if crossing is occupied
  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.                     // probably evaluation for a lane-change
  177.                     a = Acceleration.min(a,
  178.                             avoidMergeCollision(parameters, conflict, carFollowingModel, speed, speedLimitInfo));
  179.                 }
  180.                 // follow leading GTUs on merge or split
  181.                 a = Acceleration.min(a, followConflictingLeaderOnMergeOrSplit(conflict, parameters, carFollowingModel, speed,
  182.                         speedLimitInfo, vehicleWidth));
  183.             }

  185.             // indicator if bus
  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.                         // TODO depending on left/right-hand traffic
  198.                         conflictPlans.setIndicatorIntent(TurnIndicatorIntent.LEFT, conflict.getDistance());
  199.                     }
  200.                 }
  201.             }

  203.             // blocking and ignoring
  204.             if (conflict.getDistance().lt0() && lane.isCurrent())
  205.             {
  206.                 if (conflict.getConflictType().isCrossing() && !conflict.getConflictPriority().isPriority())
  207.                 {
  208.                     // note that we are blocking a conflict
  209.                     blocking = true;
  210.                 }
  211.                 // ignore conflicts we are on (i.e. negative distance to start of conflict)
  212.                 continue;
  213.             }

  215.             // determine if we need to stop
  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: // TODO depending on rules, we may need to stop and not just 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; // skipped anyway
  248.                     break;
  249.                 }
  250.                 default:
  251.                 {
  252.                     throw new GTUException("Unsupported conflict rule encountered while approaching conflicts.");
  253.                 }
  254.             }

  256.             // stop if required, account for upstream conflicts to keep clear
  257.             if (!conflict.getConflictType().equals(ConflictType.SPLIT))
  258.             {

  260.                 if (stop)
  261.                 {
  262.                     prevStarts.add(conflict.getDistance());
  263.                     conflictRuleTypes.add(conflict.getConflictRuleType());
  264.                     // stop for first conflict looking upstream of this blocked conflict that allows sufficient space
  265.                     int j = 0; // most upstream conflict if not in between conflicts
  266.                     for (int i = prevEnds.size() - 1; i >= 0; i--) // downstream to upstream
  267.                     {
  268.                         // note, at this point prevStarts contains one more conflict than prevEnds
  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.                         // we are blocking a conflict, let's not stop more upstream than the conflict that forces our stop
  278.                         j = prevStarts.size() - 1;
  279.                     }

  281.                     // TODO
  282.                     // if this lowers our acceleration, we need to check if we are able to pass upstream conflicts still in time

  284.                     // stop for j'th conflict, if deceleration is too strong, for next one
  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.                             // TODO what to do when we happen to be in the stopping distance? Stopping might be reasonable,
  292.                             // while car-following might give strong deceleration due to s < s0.
  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.                                 // as it may suddenly switch state, i.e. ignore like a yellow traffic light
  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.                 }

  314.                 // keep conflict clear (when stopping for another conflict)
  315.                 prevStarts.add(conflict.getDistance());
  316.                 conflictRuleTypes.add(conflict.getConflictRuleType());
  317.                 prevEnds.add(conflict.getDistance().plus(conflict.getLength()));
  318.             }

  320.         }
  321.         conflictPlans.setBlocking(blocking);

  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.             // return Acceleration.POSITIVE_INFINITY;
  327.         }
  328.         return a;
  329.     }

  331.     /**
  332.      * Determines acceleration for following conflicting vehicles <i>on</i> a merge or split conflict.
  333.      * @param conflict HeadwayConflict; merge or split conflict
  334.      * @param parameters Parameters; parameters
  335.      * @param carFollowingModel CarFollowingModel; car-following model
  336.      * @param speed Speed; current speed
  337.      * @param speedLimitInfo SpeedLimitInfo; speed limit info
  338.      * @param vehicleWidth Length; own width
  339.      * @return acceleration for following conflicting vehicles <i>on</i> a merge or split conflict
  340.      * @throws ParameterException if a parameter is not given or out of bounds
  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.         // ignore if no conflicting GTU's, or if first is downstream of conflict
  347.         PerceptionIterable<HeadwayGTU> downstreamGTUs = conflict.getDownstreamConflictingGTUs();
  348.         if (downstreamGTUs.isEmpty() || downstreamGTUs.first().isAhead())
  349.         {
  350.             return Acceleration.POS_MAXVALUE;
  351.         }
  352.         // get the most upstream GTU to consider
  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.                     // conflict GTU completely downstream of conflict (i.e. regular car-following, ignore here)
  367.                     return Acceleration.POS_MAXVALUE;
  368.                 }
  369.                 // conflict GTU (partially) on the conflict
  370.                 // {@formatter:off}
  371.                 // ______________________________________________ 
  372.                 //   ___      virtual headway   |  ___  |
  373.                 //  |___|(-----------------------)|___|(vehicle from south, on lane from south)
  374.                 // _____________________________|_______|________
  375.                 //                              /       / 
  376.                 //                             /       /
  377.                 // {@formatter:on}
  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.                     // found first downstream GTU on conflict
  392.                     c = con;
  393.                     break;
  394.                 }
  395.             }
  396.         }
  397.         if (c == null)
  398.         {
  399.             // conflict GTU downstream of start of conflict, but upstream of us
  400.             return Acceleration.POS_MAXVALUE;
  401.         }
  402.         // follow leader
  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.         // if conflicting GTU is partially upstream of the conflict and at (near) stand-still, stop for the conflict rather than
  408.         // following the tail of the conflicting GTU
  409.         if (conflict.isMerge() && virtualHeadway.lt(conflict.getDistance()))
  410.         {
  411.             // {@formatter:off}
  412.             /*
  413.              * ______________________________________________
  414.              *    ___    stop for conflict  |       | 
  415.              *   |___|(--------------------)|   ___ |
  416.              * _____________________________|__/  /_|________ 
  417.              *                              / /__/  /
  418.              *                             /       /
  419.              */
  420.             // {@formatter:on}
  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); // max, which ever allows the largest acceleration
  426.         }
  427.         return a;
  428.     }

  430.     /**
  431.      * Determines an acceleration required to avoid a collision with GTUs <i>on</i> a crossing conflict.
  432.      * @param parameters Parameters; parameters
  433.      * @param conflict HeadwayConflict; conflict
  434.      * @param carFollowingModel CarFollowingModel; car-following model
  435.      * @param speed Speed; current speed
  436.      * @param speedLimitInfo SpeedLimitInfo; speed limit info
  437.      * @return acceleration required to avoid a collision
  438.      * @throws ParameterException if parameter is not defined
  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.     {

  445.         // TODO only within visibility
  446.         List<HeadwayGTU> conflictingGTUs = new ArrayList<>();
  447.         for (HeadwayGTU gtu : conflict.getUpstreamConflictingGTUs())
  448.         {
  449.             if (isOnRoute(conflict.getConflictingLink(), gtu))
  450.             {
  451.                 // first upstream vehicle on route to this conflict
  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.                 // vehicles beyond conflict are not a thread
  465.                 break;
  466.             }
  467.         }

  469.         if (conflictingGTUs.isEmpty())
  470.         {
  471.             return Acceleration.POS_MAXVALUE;
  472.         }

  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.             // enter before cleared
  495.             // TODO safety factor?
  496.             if (tteC.getDuration().lt(tteO.getDuration()) && tteO.getDuration().lt(ttcC.getDuration()))
  497.             {
  498.                 if (!conflictingGTU.getSpeed().eq0() || !CROSSSTANDING)
  499.                 {
  500.                     // solve parabolic speed profile s = v*t + .5*a*t*t, a =
  501.                     double acc = 2 * (conflict.getDistance().si - speed.si * ttcC.getDuration().si)
  502.                             / (ttcC.getDuration().si * ttcC.getDuration().si);
  503.                     // time till zero speed > time to avoid conflict?
  504.                     if (speed.si / -acc > ttcC.getDuration().si)
  505.                     {
  506.                         a = Acceleration.min(a, new Acceleration(acc, AccelerationUnit.SI));
  507.                     }
  508.                     else
  509.                     {
  510.                         // will reach zero speed ourselves
  511.                         a = Acceleration.min(a, CarFollowingUtil.stop(carFollowingModel, parameters, speed, speedLimitInfo,
  512.                                 conflict.getDistance()));
  513.                     }
  514.                 }
  515.                 // conflicting vehicle stand-still, ignore even at conflict
  516.             }
  517.         }
  518.         return a;
  519.     }

  521.     /**
  522.      * Avoid collision at merge. This method assumes the GTU has priority.
  523.      * @param parameters Parameters; parameters
  524.      * @param conflict HeadwayConflict; conflict
  525.      * @param carFollowingModel CarFollowingModel; car-following model
  526.      * @param speed Speed; current speed
  527.      * @param speedLimitInfo SpeedLimitInfo; speed limit info
  528.      * @return acceleration required to avoid a collision
  529.      * @throws ParameterException if parameter is not defined
  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.         // TODO: this check is simplistic, designed quick and dirty
  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.     }

  550.     /**
  551.      * Approach a priority conflict. Stopping is applied to give way to conflicting traffic in case congestion is present on the
  552.      * own lane. This is courtesy yielding.
  553.      * @param conflict HeadwayConflict; conflict to approach
  554.      * @param leaders PerceptionCollectable&lt;HeadwayGTU,LaneBasedGTU&gt;; leading vehicles in own lane
  555.      * @param speed Speed; current speed
  556.      * @param vehicleLength Length; vehicle length
  557.      * @param parameters Parameters; parameters
  558.      * @param prevEnd Length; distance to end of previous conflict that should not be blocked, {@code null} if none
  559.      * @return whether to stop for this conflict
  560.      * @throws ParameterException if parameter B is not defined
  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.     {

  567.         // check if we should stop as there is not sufficient space on the merge, to leave a previous conflict unblocked
  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.         }

  585.         // some quick -no need to stop-'s
  586.         if (leaders.isEmpty())
  587.         {
  588.             // no leader
  589.             return false;
  590.         }
  591.         if (conflict.getUpstreamConflictingGTUs().isEmpty())
  592.         {
  593.             // no conflicting vehicles
  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.                 // conflicting vehicle too far away
  603.                 return false;
  604.             }
  605.         }

  607.         // Stop as long as some leader is standing still, and this leader is not leaving sufficient space yet
  608.         // use start of conflict on merge, end of conflict on crossing
  609.         Length typeCorrection = conflict.isCrossing() ? conflict.getLength() : Length.ZERO;
  610.         // distance leader has to cover before we can pass the conflict
  611.         Length distance = conflict.getDistance().minus(leaders.first().getDistance()).plus(passable).plus(typeCorrection);
  612.         if (distance.gt0())
  613.         {
  614.             // for ourselves
  615.             Length required = conflict.getDistance().plus(typeCorrection).plus(passableDistance(vehicleLength, parameters));
  616.             for (HeadwayGTU leader : leaders)
  617.             {
  618.                 if (leader.getSpeed().eq0())
  619.                 {
  620.                     // first stand-still leader is not fully upstream of the conflict (in that case, ignore), and does not
  621.                     // allow sufficient space for all vehicles in between
  622.                     return leader.getDistance().ge(conflict.getDistance()) && required.ge(leader.getDistance());
  623.                 }
  624.                 required = required // add required distance for leaders
  625.                         .plus(passableDistance(leader.getLength(), leader.getParameters()));
  626.             }
  627.         }

  629.         // no reason found to stop
  630.         return false;
  631.     }

  633.     /**
  634.      * Approach a give-way conflict.
  635.      * @param conflict HeadwayConflict; conflict
  636.      * @param leaders PerceptionCollectable&lt;HeadwayGTU,LaneBasedGTU&gt;; leaders
  637.      * @param speed Speed; current speed
  638.      * @param acceleration Acceleration; current acceleration
  639.      * @param vehicleLength Length; vehicle length
  640.      * @param parameters Parameters; parameters
  641.      * @param speedLimitInfo SpeedLimitInfo; speed limit info
  642.      * @param carFollowingModel CarFollowingModel; car-following model
  643.      * @param bType ParameterTypeAcceleration; parameter type for considered deceleration
  644.      * @param prevEnd Length; distance to end of previous conflict that should not be blocked, {@code null} if none
  645.      * @return whether to stop for this conflict
  646.      * @throws ParameterException if a parameter is not defined
  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.     {

  656.         // TODO conflicting vehicle on crossing conflict, but will leave sooner then we enter, so no problem?
  657.         // TODO more generally, also upstream conflicting vehicles at crossings may leave the conflict before we enter
  658.         if (conflict.getConflictType().isCrossing() && !conflict.getDownstreamConflictingGTUs().isEmpty()
  659.                 && conflict.getDownstreamConflictingGTUs().first().isParallel())
  660.         {
  661.             // vehicle on the conflict
  662.             return true;
  663.         }

  665.         // Get data independent of conflicting vehicle
  666.         // parameters
  667.         Acceleration b = parameters.getParameter(bType).neg();
  668.         double f = parameters.getParameter(TIME_FACTOR);
  669.         Duration gap = parameters.getParameter(MIN_GAP);
  670.         // time till conflict is cleared
  671.         Length distance = conflict.getDistance().plus(vehicleLength);
  672.         if (conflict.isCrossing())
  673.         {
  674.             // merge is cleared at start, crossing at end
  675.             distance = distance.plus(conflict.getLength());
  676.         }
  677.         // based on acceleration, limited by free acceleration
  678.         AnticipationInfo ttcOa = AnticipationInfo.anticipateMovementFreeAcceleration(distance, speed, parameters,
  679.                 carFollowingModel, speedLimitInfo, TIME_STEP);
  680.         // time till downstream vehicle will make the conflict passible, under constant speed or safe deceleration
  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.                 // no leader so conflict is passible within a duration of 0
  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.             // stop for merge (and previous conflict) if we are likely to stop partially on the previous conflict
  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.                     // vehicle on conflict will not leave sufficient space
  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.                     // vehicle on conflict will not leave sufficient space
  722.                     return true;
  723.                 }
  724.             }
  725.         }

  727.         PerceptionCollectable<HeadwayGTU, LaneBasedGTU> conflictingVehiclesCollectable = conflict.getUpstreamConflictingGTUs();
  728.         Iterable<HeadwayGTU> conflictingVehicles;
  729.         if (conflictingVehiclesCollectable.isEmpty())
  730.         {
  731.             if (conflict.getConflictingTrafficLightDistance() == null)
  732.             {
  733.                 // none within visibility, assume a conflicting vehicle just outside of visibility driving at speed limit
  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.                 // no conflicting vehicles
  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.                 // conflicting traffic upstream of traffic light
  765.                 return false;
  766.             }
  767.             conflictingVehicles = conflictingVehiclesCollectable;
  768.         }

  770.         // Loop over conflicting vehicles
  771.         boolean first = true;
  772.         boolean ignoreBeyondFirst = false;
  773.         for (HeadwayGTU conflictingVehicle : conflictingVehicles)
  774.         {

  776.             // skip if not on route
  777.             if (!isOnRoute(conflict.getConflictingLink(), conflictingVehicle))
  778.             {
  779.                 continue;
  780.             }

  782.             // time till conflict vehicle will enter, under free acceleration and safe deceleration
  783.             AnticipationInfo tteCa;
  784.             AnticipationInfo tteCs;
  785.             if (first && conflictingVehicle.getSpeed().eq0() && conflictingVehicle.isAhead())
  786.             // && conflictingVehicle.getDistance().lt(parameters.getParameter(S0_CONF)))
  787.             {
  788.                 // do not stop if conflicting vehicle is standing still
  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.                     // Constant acceleration creates inf at stand still, triggering passing trough a congested stream
  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.             }

  827.             // check gap
  828.             if (conflict.isMerge())
  829.             {

  831.                 // Merge, will be each others followers, add time to overcome speed difference
  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.                 // check if conflict vehicle will be upstream after that time, position beyond conflict after additional time
  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; // constant speed after clearing
  842.                 Duration tMax = parameters.getParameter(ParameterTypes.TMAX);
  843.                 Length s0 = parameters.getParameter(S0);
  844.                 // 1) will clear the conflict after the conflict vehicle enters
  845.                 // 2) not sufficient time to overcome speed difference
  846.                 // 3) conflict vehicle will be too near after adjusting speed
  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.                 }

  855.             }
  856.             else if (conflict.isCrossing())
  857.             {

  859.                 // Crossing, stop if order of events is not ok
  860.                 // Should go before the conflict vehicle
  861.                 // 1) downstream vehicle must supply sufficient space before conflict vehicle will enter
  862.                 // 2) must clear the conflict before the conflict vehicle will enter
  863.                 // 3) if leader decelerates with b, conflict vehicle should be able to safely delay entering conflict
  864.                 // 4) conflict vehicle will never leave enough space beyond the conflict
  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.                 }

  873.             }
  874.             else
  875.             {
  876.                 throw new RuntimeException(
  877.                         "Conflict is of unknown type " + conflict.getConflictType() + ", which is not merge nor a crossing.");
  878.             }

  880.             if (first && ignoreBeyondFirst)
  881.             {
  882.                 return false;
  883.             }
  884.             first = false;
  885.         }

  887.         // No conflict vehicle triggered stopping
  888.         return false;

  890.     }

  892.     /**
  893.      * Approach a stop conflict. Currently this is equal to approaching a give-way conflict.
  894.      * @param conflict HeadwayConflict; conflict
  895.      * @param leaders PerceptionCollectable&lt;HeadwayGTU,LaneBasedGTU&gt;; leaders
  896.      * @param speed Speed; current speed
  897.      * @param acceleration Acceleration; current acceleration
  898.      * @param vehicleLength Length; vehicle length
  899.      * @param parameters Parameters; parameters
  900.      * @param speedLimitInfo SpeedLimitInfo; speed limit info
  901.      * @param carFollowingModel CarFollowingModel; car-following model
  902.      * @param bType ParameterTypeAcceleration; parameter type for considered deceleration
  903.      * @param prevEnd Length; distance to end of previous conflict that should not be blocked, {@code null} if none
  904.      * @return whether to stop for this conflict
  905.      * @throws ParameterException if a parameter is not defined
  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.         // TODO stopping
  915.         return stopForGiveWayConflict(conflict, leaders, speed, acceleration, vehicleLength, parameters, speedLimitInfo,
  916.                 carFollowingModel, bType, prevEnd);
  917.     }

  919.     /**
  920.      * Approach an all-stop conflict.
  921.      * @param conflict HeadwayConflict; conflict to approach
  922.      * @param conflictPlans ConflictPlans; set of plans for conflict
  923.      * @return whether to stop for this conflict
  924.      */
  925.     public static boolean stopForAllStopConflict(final HeadwayConflict conflict, final ConflictPlans conflictPlans)
  926.     {
  927.         // TODO all-stop behavior

  929.         if (conflictPlans.isStopPhaseRun(conflict.getStopLine()))
  930.         {
  931.             return false;
  932.         }

  934.         return false;
  935.     }

  937.     /**
  938.      * Returns whether the conflicting link is on the route of the given gtu.
  939.      * @param conflictingLink CrossSectionLink; conflicting link
  940.      * @param gtu HeadwayGTU; gtu
  941.      * @return whether the conflict is on the route of the given gtu
  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.                 // conservative assumption: it's on the route (gtu should be upstream of the conflict)
  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.             // conservative assumption: it's on the route (gtu should be upstream of the conflict)
  961.             return true;
  962.         }
  963.     }

  965.     /**
  966.      * Returns a speed dependent distance needed behind the leader to completely pass the conflict.
  967.      * @param vehicleLength Length; vehicle length
  968.      * @param parameters Parameters; parameters
  969.      * @return speed dependent distance needed behind the leader to completely pass the conflict
  970.      * @throws ParameterException if parameter is not available
  971.      */
  972.     private static Length passableDistance(final Length vehicleLength, final Parameters parameters) throws ParameterException
  973.     {
  974.         return parameters.getParameter(S0).plus(vehicleLength);
  975.     }

  977.     /**
  978.      * Holds the tactical plans of a driver considering conflicts. These are remembered for consistency. For instance, if the
  979.      * decision is made to yield as current deceleration suggests it's safe to do so, but the trajectory for stopping in front
  980.      * of the conflict results in deceleration slightly above what is considered safe deceleration, the plan should not be
  981.      * abandoned. Decelerations above what is considered safe deceleration may result due to numerical overshoot or other factor
  982.      * coming into play in car-following models. Many other examples exist where a driver sticks to a certain plan.
  983.      * <p>
  984.      * Copyright (c) 2013-2022 Delft University of Technology, PO Box 5, 2600 AA, Delft, the Netherlands. All rights reserved.
  985.      * <br>
  986.      * BSD-style license. See <a href="http://opentrafficsim.org/docs/current/license.html">OpenTrafficSim License</a>.
  987.      * <p>
  988.      * @version $Revision$, $LastChangedDate$, by $Author$, initial version Jun 7, 2016 <br>
  989.      * @author <a href="http://www.tbm.tudelft.nl/averbraeck">Alexander Verbraeck</a>
  990.      * @author <a href="http://www.tudelft.nl/pknoppers">Peter Knoppers</a>
  991.      * @author <a href="http://www.transport.citg.tudelft.nl">Wouter Schakel</a>
  992.      */
  993.     public static final class ConflictPlans implements Blockable, Serializable
  994.     {

  996.         /** */
  997.         private static final long serialVersionUID = 20160811L;

  999.         /** Phases of navigating an all-stop intersection per intersection. */
  1000.         private final LinkedHashMap<String, StopPhase> stopPhases = new LinkedHashMap<>();

  1002.         /** Estimated arrival times of vehicles at all-stop intersection. */
  1003.         private final LinkedHashMap<String, Time> arrivalTimes = new LinkedHashMap<>();

  1005.         /** Indicator intent. */
  1006.         private TurnIndicatorIntent indicatorIntent = TurnIndicatorIntent.NONE;

  1008.         /** Distance to object causing turn indicator intent. */
  1009.         private Length indicatorObjectDistance = null;

  1011.         /** Whether the GTU is blocking conflicts. */
  1012.         private boolean blocking;

  1014.         /**
  1015.          * Clean any yield plan that was no longer kept active in the last evaluation of conflicts.
  1016.          */
  1017.         void cleanPlans()
  1018.         {
  1019.             this.indicatorIntent = TurnIndicatorIntent.NONE;
  1020.             this.indicatorObjectDistance = null;
  1021.         }

  1023.         /**
  1024.          * Sets the estimated arrival time of a GTU.
  1025.          * @param gtu AbstractHeadwayGTU; GTU
  1026.          * @param time Time; estimated arrival time
  1027.          */
  1028.         void setArrivalTime(final AbstractHeadwayGTU gtu, final Time time)
  1029.         {
  1030.             this.arrivalTimes.put(gtu.getId(), time);
  1031.         }

  1033.         /**
  1034.          * Returns the estimated arrival time of given GTU.
  1035.          * @param gtu AbstractHeadwayGTU; GTU
  1036.          * @return estimated arrival time of given GTU
  1037.          */
  1038.         Time getArrivalTime(final AbstractHeadwayGTU gtu)
  1039.         {
  1040.             return this.arrivalTimes.get(gtu.getId());
  1041.         }

  1043.         /**
  1044.          * Sets the current phase to 'approach' for the given stop line.
  1045.          * @param stopLine HeadwayStopLine; stop line
  1046.          */
  1047.         void setStopPhaseApproach(final HeadwayStopLine stopLine)
  1048.         {
  1049.             this.stopPhases.put(stopLine.getId(), StopPhase.APPROACH);
  1050.         }

  1052.         /**
  1053.          * Sets the current phase to 'yield' for the given stop line.
  1054.          * @param stopLine HeadwayStopLine; stop line
  1055.          * @throws RuntimeException if the phase was not set to approach before
  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.         }

  1066.         /**
  1067.          * Sets the current phase to 'run' for the given stop line.
  1068.          * @param stopLine HeadwayStopLine; stop line
  1069.          * @throws RuntimeException if the phase was not set to approach before
  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.         }

  1078.         /**
  1079.          * @param stopLine HeadwayStopLine; stop line
  1080.          * @return whether the current phase is 'approach' for the given stop line
  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.         }

  1088.         /**
  1089.          * @param stopLine HeadwayStopLine; stop line
  1090.          * @return whether the current phase is 'yield' for the given stop line
  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.         }

  1098.         /**
  1099.          * @param stopLine HeadwayStopLine; stop line
  1100.          * @return whether the current phase is 'run' for the given stop line
  1101.          */
  1102.         boolean isStopPhaseRun(final HeadwayStopLine stopLine)
  1103.         {
  1104.             return this.stopPhases.containsKey(stopLine.getId()) && this.stopPhases.get(stopLine.getId()).equals(StopPhase.RUN);
  1105.         }

  1107.         /** {@inheritDoc} */
  1108.         @Override
  1109.         public String toString()
  1110.         {
  1111.             return "ConflictPlans";
  1112.         }

  1114.         /**
  1115.          * @return indicatorIntent.
  1116.          */
  1117.         public TurnIndicatorIntent getIndicatorIntent()
  1118.         {
  1119.             return this.indicatorIntent;
  1120.         }

  1122.         /**
  1123.          * @return indicatorObjectDistance.
  1124.          */
  1125.         public Length getIndicatorObjectDistance()
  1126.         {
  1127.             return this.indicatorObjectDistance;
  1128.         }

  1130.         /**
  1131.          * @param intent TurnIndicatorIntent; indicator intent
  1132.          * @param distance Length; distance to object pertaining to the turn indicator intent
  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.         }

  1143.         /** {@inheritDoc} */
  1144.         @Override
  1145.         public boolean isBlocking()
  1146.         {
  1147.             return this.blocking;
  1148.         }

  1150.         /**
  1151.          * Sets the GTU as blocking conflicts or not.
  1152.          * @param blocking boolean; whether the GTU is blocking conflicts
  1153.          */
  1154.         public void setBlocking(final boolean blocking)
  1155.         {
  1156.             this.blocking = blocking;
  1157.         }

  1159.     }

  1161.     /**
  1162.      * Phases of navigating an all-stop intersection.
  1163.      * <p>
  1164.      * Copyright (c) 2013-2022 Delft University of Technology, PO Box 5, 2600 AA, Delft, the Netherlands. All rights reserved.
  1165.      * <br>
  1166.      * BSD-style license. See <a href="http://opentrafficsim.org/docs/current/license.html">OpenTrafficSim License</a>.
  1167.      * <p>
  1168.      * @version $Revision$, $LastChangedDate$, by $Author$, initial version Jun 30, 2016 <br>
  1169.      * @author <a href="http://www.tbm.tudelft.nl/averbraeck">Alexander Verbraeck</a>
  1170.      * @author <a href="http://www.tudelft.nl/pknoppers">Peter Knoppers</a>
  1171.      * @author <a href="http://www.transport.citg.tudelft.nl">Wouter Schakel</a>
  1172.      */
  1173.     private enum StopPhase
  1174.     {
  1175.         /** Approaching stop intersection. */
  1176.         APPROACH,

  1178.         /** Yielding for stop intersection. */
  1179.         YIELD,

  1181.         /** Running over stop intersection. */
  1182.         RUN;
  1183.     }

  1185. }
Created with JaCoCo 0.8.6.202009150832