LmrsUtil.java
package org.opentrafficsim.road.gtu.lane.tactical.util.lmrs;
import static org.opentrafficsim.core.gtu.behavioralcharacteristics.AbstractParameterType.Check.UNITINTERVAL;
import java.util.HashSet;
import java.util.LinkedHashSet;
import java.util.Set;
import java.util.SortedMap;
import java.util.SortedSet;
import java.util.TreeMap;
import org.djunits.unit.AccelerationUnit;
import org.djunits.unit.TimeUnit;
import org.djunits.value.vdouble.scalar.Acceleration;
import org.djunits.value.vdouble.scalar.Duration;
import org.djunits.value.vdouble.scalar.Length;
import org.djunits.value.vdouble.scalar.Speed;
import org.djunits.value.vdouble.scalar.Time;
import org.opentrafficsim.core.gtu.GTUException;
import org.opentrafficsim.core.gtu.TurnIndicatorStatus;
import org.opentrafficsim.core.gtu.behavioralcharacteristics.BehavioralCharacteristics;
import org.opentrafficsim.core.gtu.behavioralcharacteristics.ParameterException;
import org.opentrafficsim.core.gtu.behavioralcharacteristics.ParameterTypeDouble;
import org.opentrafficsim.core.gtu.behavioralcharacteristics.ParameterTypes;
import org.opentrafficsim.core.gtu.plan.operational.OperationalPlanException;
import org.opentrafficsim.core.network.LateralDirectionality;
import org.opentrafficsim.core.network.NetworkException;
import org.opentrafficsim.road.gtu.lane.LaneBasedGTU;
import org.opentrafficsim.road.gtu.lane.perception.InfrastructureLaneChangeInfo;
import org.opentrafficsim.road.gtu.lane.perception.LanePerception;
import org.opentrafficsim.road.gtu.lane.perception.RelativeLane;
import org.opentrafficsim.road.gtu.lane.perception.categories.InfrastructurePerception;
import org.opentrafficsim.road.gtu.lane.perception.categories.IntersectionPerception;
import org.opentrafficsim.road.gtu.lane.perception.categories.NeighborsPerception;
import org.opentrafficsim.road.gtu.lane.perception.headway.AbstractHeadwayGTU;
import org.opentrafficsim.road.gtu.lane.perception.headway.HeadwayConflict;
import org.opentrafficsim.road.gtu.lane.plan.operational.LaneOperationalPlanBuilder.LaneChange;
import org.opentrafficsim.road.gtu.lane.plan.operational.SimpleOperationalPlan;
import org.opentrafficsim.road.gtu.lane.tactical.following.CarFollowingModel;
import org.opentrafficsim.road.gtu.lane.tactical.util.CarFollowingUtil;
import org.opentrafficsim.road.network.speed.SpeedLimitInfo;
import org.opentrafficsim.road.network.speed.SpeedLimitProspect;
import nl.tudelft.simulation.language.Throw;
/**
* <p>
* Copyright (c) 2013-2016 Delft University of Technology, PO Box 5, 2600 AA, Delft, the Netherlands. All rights reserved. <br>
* BSD-style license. See <a href="http://opentrafficsim.org/docs/current/license.html">OpenTrafficSim License</a>.
* <p>
* @version $Revision$, $LastChangedDate$, by $Author$, initial version Jul 26, 2016 <br>
* @author <a href="http://www.tbm.tudelft.nl/averbraeck">Alexander Verbraeck</a>
* @author <a href="http://www.tudelft.nl/pknoppers">Peter Knoppers</a>
* @author <a href="http://www.transport.citg.tudelft.nl">Wouter Schakel</a>
*/
public final class LmrsUtil
{
/** Free lane change desire threshold. */
public static final ParameterTypeDouble DFREE =
new ParameterTypeDouble("dFree", "Free lane change desire threshold.", 0.365, UNITINTERVAL)
{
/** */
private static final long serialVersionUID = 20160413L;
public void check(final double value, final BehavioralCharacteristics bc) throws ParameterException
{
if (bc.contains(DSYNC))
{
Throw.when(value >= bc.getParameter(DSYNC), ParameterException.class,
"Value of dFree is above or equal to dSync.");
}
if (bc.contains(DCOOP))
{
Throw.when(value >= bc.getParameter(DCOOP), ParameterException.class,
"Value of dFree is above or equal to dCoop.");
}
}
};
/** Synchronized lane change desire threshold. */
public static final ParameterTypeDouble DSYNC =
new ParameterTypeDouble("dSync", "Synchronized lane change desire threshold.", 0.577, UNITINTERVAL)
{
/** */
private static final long serialVersionUID = 20160413L;
public void check(final double value, final BehavioralCharacteristics bc) throws ParameterException
{
if (bc.contains(DFREE))
{
Throw.when(value <= bc.getParameter(DFREE), ParameterException.class,
"Value of dSync is below or equal to dFree.");
}
if (bc.contains(DCOOP))
{
Throw.when(value >= bc.getParameter(DCOOP), ParameterException.class,
"Value of dSync is above or equal to dCoop.");
}
}
};
/** Cooperative lane change desire threshold. */
public static final ParameterTypeDouble DCOOP =
new ParameterTypeDouble("dCoop", "Cooperative lane change desire threshold.", 0.788, UNITINTERVAL)
{
/** */
private static final long serialVersionUID = 20160413L;
public void check(final double value, final BehavioralCharacteristics bc) throws ParameterException
{
if (bc.contains(DFREE))
{
Throw.when(value <= bc.getParameter(DFREE), ParameterException.class,
"Value of dCoop is below or equal to dFree.");
}
if (bc.contains(DSYNC))
{
Throw.when(value <= bc.getParameter(DSYNC), ParameterException.class,
"Value of dCoop is below or equal to dSync.");
}
}
};
/** Current left lane change desire. */
public static final ParameterTypeDouble DLEFT = new ParameterTypeDouble("dLeft", "Left lane change desire.", 0.0);
/** Current right lane change desire. */
public static final ParameterTypeDouble DRIGHT = new ParameterTypeDouble("dRight", "Right lane change desire.", 0.0);
/** Lane change desire of current lane change. */
public static final ParameterTypeDouble DLC = new ParameterTypeDouble("dLaneChange", "Desire of current lane change.", 0.0);
/**
* Do not instantiate.
*/
private LmrsUtil()
{
//
}
/**
* Determines a simple representation of an operational plan.
* @param gtu gtu
* @param startTime start time
* @param carFollowingModel car-following model
* @param laneChange lane change status
* @param lmrsData LMRS data
* @param perception perception
* @param mandatoryIncentives set of mandatory lane change incentives
* @param voluntaryIncentives set of voluntary lane change incentives
* @return simple operational plan
* @throws GTUException gtu exception
* @throws NetworkException network exception
* @throws ParameterException parameter exception
* @throws OperationalPlanException operational plan exception
*/
@SuppressWarnings("checkstyle:parameternumber")
public static SimpleOperationalPlan determinePlan(final LaneBasedGTU gtu, final Time startTime,
final CarFollowingModel carFollowingModel, final LaneChange laneChange, final LmrsData lmrsData,
final LanePerception perception, final LinkedHashSet<MandatoryIncentive> mandatoryIncentives,
final LinkedHashSet<VoluntaryIncentive> voluntaryIncentives)
throws GTUException, NetworkException, ParameterException, OperationalPlanException
{
// TODO this is a hack to prevent right lane changes of all vehicles on the left lane when placed in network at t=0
if (startTime.si == 0.0)
{
return new SimpleOperationalPlan(Acceleration.ZERO, LateralDirectionality.NONE);
}
// obtain objects to get info
SpeedLimitProspect slp =
perception.getPerceptionCategory(InfrastructurePerception.class).getSpeedLimitProspect(RelativeLane.CURRENT);
SpeedLimitInfo sli = slp.getSpeedLimitInfo(Length.ZERO);
BehavioralCharacteristics bc = gtu.getBehavioralCharacteristics();
// regular car-following
Speed speed = gtu.getSpeed();
SortedSet<AbstractHeadwayGTU> leaders =
perception.getPerceptionCategory(NeighborsPerception.class).getLeaders(RelativeLane.CURRENT);
if (!leaders.isEmpty() && lmrsData.isNewLeader(leaders.first()))
{
initHeadwayRelaxation(bc, leaders.first());
}
Acceleration a = CarFollowingUtil.followLeaders(carFollowingModel, bc, speed, sli, leaders);
// stop for end
Length remainingDist = null;
for (InfrastructureLaneChangeInfo ili : perception.getPerceptionCategory(InfrastructurePerception.class)
.getInfrastructureLaneChangeInfo(RelativeLane.CURRENT))
{
if (remainingDist == null || remainingDist.gt(ili.getRemainingDistance()))
{
remainingDist = ili.getRemainingDistance();
}
}
if (remainingDist != null)
{
remainingDist.minus(bc.getParameter(ParameterTypes.S0));
Acceleration bMin = new Acceleration(.5 * speed.si * speed.si / remainingDist.si, AccelerationUnit.SI);
Acceleration bCrit = bc.getParameter(ParameterTypes.BCRIT);
if (bMin.ge(bCrit))
{
a = Acceleration.min(a, bMin.neg());
}
}
// during a lane change, both leaders are followed
LateralDirectionality initiatedLaneChange;
if (laneChange.isChangingLane())
{
RelativeLane secondLane = laneChange.getSecondLane(gtu);
initiatedLaneChange = LateralDirectionality.NONE;
SortedSet<AbstractHeadwayGTU> secondLeaders =
perception.getPerceptionCategory(NeighborsPerception.class).getLeaders(secondLane);
Acceleration aSecond = CarFollowingUtil.followLeaders(carFollowingModel, bc, speed, sli, secondLeaders);
if (!secondLeaders.isEmpty() && lmrsData.isNewLeader(secondLeaders.first()))
{
initHeadwayRelaxation(bc, secondLeaders.first());
}
a = Acceleration.min(a, aSecond);
}
else
{
// determine lane change desire based on incentives
Desire desire = getLaneChangeDesire(bc, perception, carFollowingModel, mandatoryIncentives, voluntaryIncentives);
// gap acceptance
boolean acceptLeft = perception.getLaneStructure().getRootLSR().getLeft() != null
&& acceptGap(perception, bc, sli, carFollowingModel, desire.getLeft(), speed, LateralDirectionality.LEFT);
boolean acceptRight = perception.getLaneStructure().getRootLSR().getRight() != null
&& acceptGap(perception, bc, sli, carFollowingModel, desire.getRight(), speed, LateralDirectionality.RIGHT);
// lane change decision
double dFree = bc.getParameter(DFREE);
double dSync = bc.getParameter(DSYNC);
double dCoop = bc.getParameter(DCOOP);
// decide
TurnIndicatorStatus turnIndicatorStatus;
if (desire.leftIsLargerOrEqual() && desire.getLeft() >= dFree && acceptLeft)
{
// change left
initiatedLaneChange = LateralDirectionality.LEFT;
turnIndicatorStatus = TurnIndicatorStatus.LEFT;
bc.setParameter(DLC, desire.getLeft());
setDesiredHeadway(bc, desire.getLeft());
leaders = perception.getPerceptionCategory(NeighborsPerception.class).getLeaders(RelativeLane.LEFT);
if (!leaders.isEmpty())
{
// don't respond on its lane change desire, but remember it such that it isn't a new leader in the next step
lmrsData.isNewLeader(leaders.first());
}
}
else if (!desire.leftIsLargerOrEqual() && desire.getRight() >= dFree && acceptRight)
{
// change right
initiatedLaneChange = LateralDirectionality.RIGHT;
turnIndicatorStatus = TurnIndicatorStatus.RIGHT;
bc.setParameter(DLC, desire.getRight());
setDesiredHeadway(bc, desire.getRight());
leaders = perception.getPerceptionCategory(NeighborsPerception.class).getLeaders(RelativeLane.RIGHT);
if (!leaders.isEmpty())
{
// don't respond on its lane change desire, but remember it such that it isn't a new leader in the next step
lmrsData.isNewLeader(leaders.first());
}
}
else
{
initiatedLaneChange = LateralDirectionality.NONE;
turnIndicatorStatus = TurnIndicatorStatus.NONE;
}
laneChange.setLaneChangeDuration(gtu.getBehavioralCharacteristics().getParameter(ParameterTypes.LCDUR));
// take action if we cannot change lane
Acceleration aSync;
if (initiatedLaneChange.equals(LateralDirectionality.NONE))
{
// synchronize
if (desire.leftIsLargerOrEqual() && desire.getLeft() >= dSync)
{
aSync = synchronize(perception, bc, sli, carFollowingModel, desire.getLeft(), speed,
LateralDirectionality.LEFT);
a = Acceleration.min(a, aSync);
}
else if (!desire.leftIsLargerOrEqual() && desire.getRight() >= dSync)
{
aSync = synchronize(perception, bc, sli, carFollowingModel, desire.getRight(), speed,
LateralDirectionality.RIGHT);
a = Acceleration.min(a, aSync);
}
// use indicators to indicate lane change need
if (desire.leftIsLargerOrEqual() && desire.getLeft() >= dCoop)
{
// switch on left indicator
turnIndicatorStatus = TurnIndicatorStatus.LEFT;
}
else if (!desire.leftIsLargerOrEqual() && desire.getRight() >= dCoop)
{
// switch on right indicator
turnIndicatorStatus = TurnIndicatorStatus.RIGHT;
}
bc.setParameter(DLEFT, desire.getLeft());
bc.setParameter(DRIGHT, desire.getRight());
}
else
{
bc.setParameter(DLEFT, 0.0);
bc.setParameter(DRIGHT, 0.0);
}
gtu.setTurnIndicatorStatus(turnIndicatorStatus);
// cooperate
aSync = cooperate(perception, bc, sli, carFollowingModel, speed, LateralDirectionality.LEFT);
a = Acceleration.min(a, aSync);
aSync = cooperate(perception, bc, sli, carFollowingModel, speed, LateralDirectionality.RIGHT);
a = Acceleration.min(a, aSync);
// relaxation
exponentialHeadwayRelaxation(bc);
}
lmrsData.finalizeStep();
return new SimpleOperationalPlan(a, initiatedLaneChange);
}
/**
* Sets the headway as a response to a new leader.
* @param bc behavioral characteristics
* @param leader leader
* @throws ParameterException if DLC is not present
*/
private static void initHeadwayRelaxation(final BehavioralCharacteristics bc, final AbstractHeadwayGTU leader)
throws ParameterException
{
if (leader.getBehavioralCharacteristics().contains(DLC))
{
setDesiredHeadway(bc, leader.getBehavioralCharacteristics().getParameter(DLC));
}
// else could not be perceived
}
/**
* Updates the desired headway following an exponential shape approximated with fixed time step <tt>DT</tt>.
* @param bc behavioral characteristics
* @throws ParameterException in case of a parameter exception
*/
private static void exponentialHeadwayRelaxation(final BehavioralCharacteristics bc) throws ParameterException
{
double ratio = bc.getParameter(ParameterTypes.DT).si / bc.getParameter(ParameterTypes.TAU).si;
bc.setParameter(ParameterTypes.T, Duration.interpolate(bc.getParameter(ParameterTypes.T),
bc.getParameter(ParameterTypes.TMAX), ratio <= 1.0 ? ratio : 1.0));
}
/**
* Determines lane change desire for the given RSU. Mandatory desire is deduced as the maximum of a set of mandatory
* incentives, while voluntary desires are added. Depending on the level of mandatory lane change desire, voluntary desire
* may be included partially. If both are positive or negative, voluntary desire is fully included. Otherwise, voluntary
* desire is less considered within the range dSync < |mandatory| < dCoop. The absolute value is used as large
* negative mandatory desire may also dominate voluntary desire.
* @param behavioralCharacteristics behavioral characteristics
* @param perception perception
* @param carFollowingModel car-following model
* @param mandatoryIncentives mandatory incentives
* @param voluntaryIncentives voluntary incentives
* @return lane change desire for gtu
* @throws ParameterException if a parameter is not defined
* @throws GTUException if there is no mandatory incentive, the model requires at least one
* @throws OperationalPlanException perception exception
*/
private static Desire getLaneChangeDesire(final BehavioralCharacteristics behavioralCharacteristics,
final LanePerception perception, final CarFollowingModel carFollowingModel,
final LinkedHashSet<MandatoryIncentive> mandatoryIncentives,
final LinkedHashSet<VoluntaryIncentive> voluntaryIncentives)
throws ParameterException, GTUException, OperationalPlanException
{
double dSync = behavioralCharacteristics.getParameter(DSYNC);
double dCoop = behavioralCharacteristics.getParameter(DCOOP);
// Mandatory desire
double dLeftMandatory = Double.NEGATIVE_INFINITY;
double dRightMandatory = Double.NEGATIVE_INFINITY;
Desire mandatoryDesire = new Desire(dLeftMandatory, dRightMandatory);
for (MandatoryIncentive incentive : mandatoryIncentives)
{
Desire d = incentive.determineDesire(behavioralCharacteristics, perception, carFollowingModel, mandatoryDesire);
dLeftMandatory = d.getLeft() > dLeftMandatory ? d.getLeft() : dLeftMandatory;
dRightMandatory = d.getRight() > dRightMandatory ? d.getRight() : dRightMandatory;
mandatoryDesire = new Desire(dLeftMandatory, dRightMandatory);
}
// Voluntary desire
double dLeftVoluntary = 0;
double dRightVoluntary = 0;
Desire voluntaryDesire = new Desire(dLeftVoluntary, dRightVoluntary);
for (VoluntaryIncentive incentive : voluntaryIncentives)
{
Desire d = incentive.determineDesire(behavioralCharacteristics, perception, carFollowingModel, mandatoryDesire,
voluntaryDesire);
dLeftVoluntary += d.getLeft();
dRightVoluntary += d.getRight();
voluntaryDesire = new Desire(dLeftVoluntary, dRightVoluntary);
}
// Total desire
double thetaLeft = 0;
if (dLeftMandatory <= dSync || dLeftMandatory * dLeftVoluntary >= 0)
{
// low mandatory desire, or same sign
thetaLeft = 1;
}
else if (dSync < dLeftMandatory && dLeftMandatory < dCoop && dLeftMandatory * dLeftVoluntary < 0)
{
// linear from 1 at dSync to 0 at dCoop
thetaLeft = (dCoop - Math.abs(dLeftMandatory)) / (dCoop - dSync);
}
double thetaRight = 0;
if (dRightMandatory <= dSync || dRightMandatory * dRightVoluntary >= 0)
{
// low mandatory desire, or same sign
thetaRight = 1;
}
else if (dSync < dRightMandatory && dRightMandatory < dCoop && dRightMandatory * dRightVoluntary < 0)
{
// linear from 1 at dSync to 0 at dCoop
thetaRight = (dCoop - Math.abs(dRightMandatory)) / (dCoop - dSync);
}
return new Desire(dLeftMandatory + thetaLeft * dLeftVoluntary, dRightMandatory + thetaRight * dRightVoluntary);
}
/**
* Determine whether a gap is acceptable.
* @param perception perception
* @param bc behavioral characteristics
* @param sli speed limit info
* @param cfm car-following model
* @param desire level of lane change desire
* @param ownSpeed own speed
* @param lat lateral direction for synchronization
* @return whether a gap is acceptable
* @throws ParameterException if a parameter is not defined
* @throws OperationalPlanException perception exception
*/
private static boolean acceptGap(final LanePerception perception, final BehavioralCharacteristics bc,
final SpeedLimitInfo sli, final CarFollowingModel cfm, final double desire, final Speed ownSpeed,
final LateralDirectionality lat) throws ParameterException, OperationalPlanException
{
Acceleration b = bc.getParameter(ParameterTypes.B);
if (perception.getPerceptionCategory(InfrastructurePerception.class).getLegalLaneChangePossibility(RelativeLane.CURRENT,
lat).si > 0 && !perception.getPerceptionCategory(NeighborsPerception.class).isGtuAlongside(lat))
{
Acceleration aFollow = new Acceleration(Double.POSITIVE_INFINITY, AccelerationUnit.SI);
for (AbstractHeadwayGTU follower : perception.getPerceptionCategory(NeighborsPerception.class)
.getFirstFollowers(lat))
{
Acceleration a = singleAcceleration(follower.getDistance(), follower.getSpeed(), ownSpeed, desire,
follower.getBehavioralCharacteristics(), follower.getSpeedLimitInfo(), follower.getCarFollowingModel());
aFollow = Acceleration.min(aFollow, a);
}
Acceleration aSelf = new Acceleration(Double.POSITIVE_INFINITY, AccelerationUnit.SI);
for (AbstractHeadwayGTU leader : perception.getPerceptionCategory(NeighborsPerception.class).getFirstLeaders(lat))
{
Acceleration a = singleAcceleration(leader.getDistance(), ownSpeed, leader.getSpeed(), desire, bc, sli, cfm);
aSelf = Acceleration.min(aSelf, a);
}
Acceleration threshold = b.multiplyBy(-desire);
return aFollow.ge(threshold) && aSelf.ge(threshold);
}
return false;
}
/**
* Sets value for T depending on level of lane change desire.
* @param bc behavioral characteristics
* @param desire lane change desire
* @throws ParameterException if T, TMIN or TMAX is not in the behavioral characteristics
*/
private static void setDesiredHeadway(final BehavioralCharacteristics bc, final double desire) throws ParameterException
{
double limitedDesire = desire < 0 ? 0 : desire > 1 ? 1 : desire;
double tDes = limitedDesire * bc.getParameter(ParameterTypes.TMIN).si
+ (1 - limitedDesire) * bc.getParameter(ParameterTypes.TMAX).si;
double t = bc.getParameter(ParameterTypes.T).si;
bc.setParameter(ParameterTypes.T, new Duration(tDes < t ? tDes : t, TimeUnit.SI));
}
/**
* Resets value for T depending on level of lane change desire.
* @param bc behavioral characteristics
* @throws ParameterException if T is not in the behavioral characteristics
*/
private static void resetDesiredHeadway(final BehavioralCharacteristics bc) throws ParameterException
{
bc.resetParameter(ParameterTypes.T);
}
/**
* Determine acceleration for synchronization.
* @param perception perception
* @param bc behavioral characteristics
* @param sli speed limit info
* @param cfm car-following model
* @param desire level of lane change desire
* @param ownSpeed own speed
* @param lat lateral direction for synchronization
* @return acceleration for synchronization
* @throws ParameterException if a parameter is not defined
* @throws OperationalPlanException perception exception
*/
private static Acceleration synchronize(final LanePerception perception, final BehavioralCharacteristics bc,
final SpeedLimitInfo sli, final CarFollowingModel cfm, final double desire, final Speed ownSpeed,
final LateralDirectionality lat) throws ParameterException, OperationalPlanException
{
if ((lat.isLeft() && !perception.getLaneStructure().getCrossSection().contains(RelativeLane.LEFT))
|| (lat.isRight() && !perception.getLaneStructure().getCrossSection().contains(RelativeLane.RIGHT)))
{
return new Acceleration(Double.POSITIVE_INFINITY, AccelerationUnit.SI);
}
Acceleration b = bc.getParameter(ParameterTypes.B);
Acceleration a = new Acceleration(Double.POSITIVE_INFINITY, AccelerationUnit.SI);
RelativeLane relativeLane = new RelativeLane(lat, 1);
SortedSet<AbstractHeadwayGTU> set = removeAllUpstreamOfConflicts(removeAllUpstreamOfConflicts(
perception.getPerceptionCategory(NeighborsPerception.class).getLeaders(relativeLane), perception, relativeLane),
perception, RelativeLane.CURRENT);
if (!set.isEmpty())
{
Acceleration aSingle =
singleAcceleration(set.first().getDistance(), ownSpeed, set.first().getSpeed(), desire, bc, sli, cfm);
a = Acceleration.min(a, aSingle);
}
return Acceleration.max(a, b.neg());
}
/**
* Determine acceleration for cooperation.
* @param perception perception
* @param bc behavioral characteristics
* @param sli speed limit info
* @param cfm car-following model
* @param ownSpeed own speed
* @param lat lateral direction for cooperation
* @return acceleration for synchronization
* @throws ParameterException if a parameter is not defined
* @throws OperationalPlanException perception exception
*/
private static Acceleration cooperate(final LanePerception perception, final BehavioralCharacteristics bc,
final SpeedLimitInfo sli, final CarFollowingModel cfm, final Speed ownSpeed, final LateralDirectionality lat)
throws ParameterException, OperationalPlanException
{
if ((lat.isLeft() && !perception.getLaneStructure().getCrossSection().contains(RelativeLane.LEFT))
|| (lat.isRight() && !perception.getLaneStructure().getCrossSection().contains(RelativeLane.RIGHT)))
{
return new Acceleration(Double.MAX_VALUE, AccelerationUnit.SI);
}
Acceleration b = bc.getParameter(ParameterTypes.B);
Acceleration a = new Acceleration(Double.POSITIVE_INFINITY, AccelerationUnit.SI);
double dCoop = bc.getParameter(DCOOP);
RelativeLane relativeLane = new RelativeLane(lat, 1);
for (AbstractHeadwayGTU leader : removeAllUpstreamOfConflicts(removeAllUpstreamOfConflicts(
perception.getPerceptionCategory(NeighborsPerception.class).getLeaders(relativeLane), perception, relativeLane),
perception, RelativeLane.CURRENT))
{
BehavioralCharacteristics bc2 = leader.getBehavioralCharacteristics();
double desire = lat.equals(LateralDirectionality.LEFT) && bc2.contains(DRIGHT) ? bc2.getParameter(DRIGHT)
: lat.equals(LateralDirectionality.RIGHT) && bc2.contains(DLEFT) ? bc2.getParameter(DLEFT) : 0;
if (desire >= dCoop)
{
Acceleration aSingle =
singleAcceleration(leader.getDistance(), ownSpeed, leader.getSpeed(), desire, bc, sli, cfm);
a = Acceleration.min(a, aSingle);
}
}
return Acceleration.max(a, b.neg());
}
/**
* Removes all GTUs from the set, that are found upstream on the conflicting lane of a conflict in the current lane.
* @param set set of GTUs
* @param perception perception
* @param relativeLane relative lane
* @return the input set
* @throws OperationalPlanException if the {@code IntersectionPerception} category is not present
*/
private static SortedSet<AbstractHeadwayGTU> removeAllUpstreamOfConflicts(final SortedSet<AbstractHeadwayGTU> set,
final LanePerception perception, final RelativeLane relativeLane) throws OperationalPlanException
{
for (HeadwayConflict conflict : perception.getPerceptionCategory(IntersectionPerception.class)
.getConflicts(relativeLane))
{
if (conflict.isCrossing())
{
for (AbstractHeadwayGTU conflictGtu : conflict.getUpstreamConflictingGTUs())
{
for (AbstractHeadwayGTU gtu : set)
{
if (conflictGtu.getId().equals(gtu.getId()))
{
set.remove(gtu);
break;
}
}
}
}
}
return set;
}
/**
* Determine acceleration from car-following.
* @param distance distance from follower to leader
* @param followerSpeed speed of follower
* @param leaderSpeed speed of leader
* @param desire level of lane change desire
* @param bc behavioral characteristics
* @param sli speed limit info
* @param cfm car-following model
* @return acceleration from car-following
* @throws ParameterException if a parameter is not defined
*/
private static Acceleration singleAcceleration(final Length distance, final Speed followerSpeed, final Speed leaderSpeed,
final double desire, final BehavioralCharacteristics bc, final SpeedLimitInfo sli, final CarFollowingModel cfm)
throws ParameterException
{
// set T
setDesiredHeadway(bc, desire);
// calculate acceleration
SortedMap<Length, Speed> leaders = new TreeMap<>();
leaders.put(distance, leaderSpeed);
Acceleration a = cfm.followingAcceleration(bc, followerSpeed, sli, leaders);
// reset T
resetDesiredHeadway(bc);
return a;
}
/**
* Keeps data for LMRS for a specific GTU.
* <p>
* Copyright (c) 2013-2016 Delft University of Technology, PO Box 5, 2600 AA, Delft, the Netherlands. All rights reserved.
* <br>
* BSD-style license. See <a href="http://opentrafficsim.org/node/13">OpenTrafficSim License</a>.
* <p>
* @version $Revision$, $LastChangedDate$, by $Author$, initial version 8 nov. 2016 <br>
* @author <a href="http://www.tbm.tudelft.nl/averbraeck">Alexander Verbraeck</a>
* @author <a href="http://www.tudelft.nl/pknoppers">Peter Knoppers</a>
* @author <a href="http://www.transport.citg.tudelft.nl">Wouter Schakel</a>
*/
public static final class LmrsData
{
/** Most recent leaders. */
private final Set<String> leaders = new HashSet<>();
/** Current leaders. */
private final Set<String> tempLeaders = new HashSet<>();
/**
* Checks if the given leader is a new leader.
* @param gtu gtu to check
* @return whether the gtu is a new leader
*/
public boolean isNewLeader(final AbstractHeadwayGTU gtu)
{
this.tempLeaders.add(gtu.getId());
return !this.leaders.contains(gtu.getId());
}
/**
* Remembers the leaders of the current time step (those forwarded to isNewLeader()) for the next time step.
*/
public void finalizeStep()
{
this.leaders.clear();
this.leaders.addAll(this.tempLeaders);
this.tempLeaders.clear();
}
}
}