DefaultRouteSystem.java
package org.opentrafficsim.road.gtu.lane.tactical.routesystem;
import java.util.LinkedHashMap;
import java.util.LinkedHashSet;
import java.util.Map;
import java.util.Set;
import java.util.SortedSet;
import java.util.TreeSet;
import org.djunits.value.vdouble.scalar.Length;
import org.djutils.exceptions.Throw;
import org.djutils.immutablecollections.ImmutableMap;
import org.djutils.immutablecollections.ImmutableMap.ImmutableEntry;
import org.djutils.multikeymap.MultiKeyMap;
import org.opentrafficsim.core.gtu.GTUDirectionality;
import org.opentrafficsim.core.gtu.GTUException;
import org.opentrafficsim.core.gtu.GTUType;
import org.opentrafficsim.core.network.LateralDirectionality;
import org.opentrafficsim.core.network.Node;
import org.opentrafficsim.core.network.route.Route;
import org.opentrafficsim.road.network.lane.DirectedLanePosition;
import org.opentrafficsim.road.network.lane.Lane;
/**
* Default route system. This system stores a set for each combination of a {@code Lane}, {@code GTUDirectionality},
* {@code Route} and {@code GTUType}. The set provides route information over a given distance beyond the end of the lane. If
* more distance is required, the set is recalculated. If less is required, a subset is returned. Distances in the returned
* route information are adjusted for the specific position of the GTU.
* <p>
* Copyright (c) 2013-2019 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 7 nov. 2019 <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 class DefaultRouteSystem implements RouteSystem
{
/** Cache. */
private MultiKeyMap<LaneChangeInfoSet> cache = new MultiKeyMap<>(Lane.class, GTUDirectionality.class, Route.class,
GTUType.class);
/** {@inheritDoc} */
@Override
public SortedSet<LaneChangeInfo> getLaneChangeInfo(final DirectedLanePosition position, final Length front,
final Route route, final GTUType gtuType, final Length distance)
{
/*
// obtain set
LaneChangeInfoSet set = this.cache.get(() -> determineSet(position, route, gtuType, distance), position.getLane(),
position.getGtuDirection(), route, gtuType);
// check if previous search was far enough
if (set.suppliesDistance(distance))
{
// far enough, return info with distances adjusted to GTU position
Length pos = position.getGtuDirection().isPlus() ? position.getPosition() : position.getLane().getLength().minus(
position.getPosition());
return set.getAdjustedSet(pos, distance);
}
*/
// previously calculated set does not have sufficient length, clear and recalculate
this.cache.clear(position.getLane(), position.getGtuDirection(), route, gtuType);
return getLaneChangeInfo(position, front, route, gtuType, distance);
}
/**
* Determines a set of lane change info.
* @param position DirectedLanePosition; position
* @param front Length; distance required for the front (relative to reference position)
* @param route Route; route, may be {@code null}
* @param gtuType GTUType; GTU type
* @param distance Length; distance over which required lane changes are desired to be known
* @return SortedSet<LaneChangeInfo>; lane change information
* @throws GTUException in case of multiple adjacent lanes
*/
private LaneChangeInfoSet determineSet(final DirectedLanePosition position, final Length front, final Route route,
final GTUType gtuType, final Length distance) throws GTUException
{
// the search can stop as soon as all lanes are reachable on a link that starts beyond 'distance' of the end of the lane
Map<LaneRecord, LaneChangeInfo> currentSet = new LinkedHashMap<>();
Map<LaneRecord, LaneChangeInfo> nextSet = new LinkedHashMap<>();
Length startDistance = (position.getGtuDirection().isPlus() ? position.getPosition() : position.getLane().getLength()
.minus(position.getPosition())).plus(front).neg();
LaneRecord record = new LaneRecord(startDistance, position.getLane(), position.getGtuDirection());
Length remainingDistance = position.getLane().getLength().minus(startDistance);
currentSet.put(record, new LaneChangeInfo(0, remainingDistance, record.isDeadEnd(gtuType),
LateralDirectionality.NONE));
while (!currentSet.isEmpty())
{
// move lateral
nextSet.putAll(currentSet);
for (LateralDirectionality lat : new LateralDirectionality[] {LateralDirectionality.LEFT,
LateralDirectionality.RIGHT})
{
for (LaneRecord laneRecord : currentSet.keySet())
{
laneRecord = lat.isLeft() ? laneRecord.left(gtuType) : laneRecord.right(gtuType);
if (!currentSet.containsKey(laneRecord))
{
LaneChangeInfo info = currentSet.get(laneRecord);
LaneChangeInfo adjInfo = new LaneChangeInfo(info.getNumberOfLaneChanges() + 1, remainingDistance,
laneRecord.isDeadEnd(gtuType), lat);
nextSet.put(laneRecord, adjInfo);
}
}
}
}
return new LaneChangeInfoSet(null, null);
}
/**
* Set of lane change info, that can be adjusted for a specific GTU position on the origin lane.
* <p>
* Copyright (c) 2013-2014 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 Feb 11, 2020 <br>
* @author <a href="http://www.tbm.tudelft.nl/averbraeck">Alexander Verbraeck</a>
* @author <a href="http://Hansvanlint.weblog.tudelft.nl">Hans van Lint</a>
* @author <a href="http://www.tudelft.nl/pknoppers">Peter Knoppers</a>
* @author <a href="http://www.citg.tudelft.nl">Guus Tamminga</a>
* @author <a href="http://www.citg.tudelft.nl">Yufei Yuan</a>
*/
private class LaneChangeInfoSet
{
/** Distance of info that this set can supply. */
private final Length dist;
/** Set with info. */
private final SortedSet<LaneChangeInfo> set;
/**
* @param distance Length; distance within which the info is gathered
* @param set SortedSet<LaneChangeInfo>; set with info
*/
LaneChangeInfoSet(final Length distance, final SortedSet<LaneChangeInfo> set)
{
this.dist = distance;
this.set = set;
}
/**
* Returns whether this set can supply up to the requested distance.
* @param distance Length; requested distance of info
* @return boolean; whether this set can supply up to the requested distance
*/
public boolean suppliesDistance(final Length distance)
{
return distance.le(this.dist);
}
/**
* Returns the set of information with distances adjusted to the GTU position.
* @param position Length; position of the GTU on the origin lane
* @param distance Length; maximum distance of included info
* @return SortedSet<LaneChangeInfo>; set of information with distances adjusted to the GTU position
*/
public SortedSet<LaneChangeInfo> getAdjustedSet(final Length position, final Length distance)
{
SortedSet<LaneChangeInfo> adjustedSet = new TreeSet<>();
for (LaneChangeInfo info : this.set)
{
Length adjustedDistance = info.getRemainingDistance().minus(position);
if (adjustedDistance.le(distance))
{
adjustedSet.add(new LaneChangeInfo(info.getNumberOfLaneChanges(), adjustedDistance, info.deadEnd(), info
.getLateralDirectionality()));
}
else
{
// we can stop, further info is beyond the requested scope
break;
}
}
return adjustedSet;
}
}
/**
* Helper class to aid route algorithms.
* <p>
* Copyright (c) 2013-2020 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 13 feb. 2020 <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>
*/
private class LaneRecord
{
/** Start distance. */
private final Length startDistance;
/** Lane. */
private final Lane lane;
/** Direction of travel. */
private final GTUDirectionality direction;
/** Previous record. */
private LaneRecord prev;
/** Left record. */
private LaneRecord left;
/** Whether the left records was determined. */
private boolean determinedLeft = false;
/** Right record. */
private LaneRecord right;
/** Whether the right records was determined. */
private boolean determinedRight = false;
/**
* @param startDistance Length; start distance
* @param lane Lane; lane
* @param direction GTUDirectionality; direction of travel
*/
LaneRecord(final Length startDistance, final Lane lane, final GTUDirectionality direction)
{
this.startDistance = startDistance;
this.lane = lane;
this.direction = direction;
}
/**
* Returns a set of next lanes along the route. This set may be empty. Note that this does not mean the lane can be
* considered a dead-end, as the route is considered.
* @param route Route route;
* @param gtuType GTUType; gtuType
* @return set of next lanes, which may be empty.
*/
public Set<LaneRecord> next(final Route route, final GTUType gtuType)
{
Set<LaneRecord> set = new LinkedHashSet<>();
ImmutableMap<Lane, GTUDirectionality> lanes = this.lane.downstreamLanes(this.direction, gtuType);
for (ImmutableEntry<Lane, GTUDirectionality> entry : lanes.entrySet())
{
// check route
Node from;
Node to;
if (entry.getValue().isPlus())
{
from = entry.getKey().getParentLink().getStartNode();
to = entry.getKey().getParentLink().getEndNode();
}
else
{
from = entry.getKey().getParentLink().getEndNode();
to = entry.getKey().getParentLink().getStartNode();
}
if (route.indexOf(to) == route.indexOf(from) + 1)
{
LaneRecord record = new LaneRecord(this.getStartDistance().plus(this.lane.getLength()), entry.getKey(),
entry.getValue());
record.prev = this;
set.add(record);
}
}
return set;
}
/**
* Returns the previous (upstream) record.
* @return LaneRecord; previous (upstream) record
*/
public LaneRecord prev()
{
return this.prev;
}
/**
* Returns the left adjacent lane, or {@code null} if no such lane.
* @param gtuType GTUType; GTU type
* @return left adjacent lane, or {@code null} if no such lane
* @throws GTUException in case of multiple adjacent lanes
*/
public LaneRecord left(final GTUType gtuType) throws GTUException
{
if (!this.determinedLeft)
{
this.left = adjacent(gtuType, LateralDirectionality.LEFT);
this.determinedLeft = true;
if (this.left.lane.accessibleAdjacentLanesLegal(LateralDirectionality.RIGHT, gtuType, this.left.direction)
.contains(this.lane))
{
this.left.right = this;
}
this.left.determinedRight = true;
}
return this.left;
}
/**
* Returns the right adjacent lane, or {@code null} if no such lane.
* @param gtuType GTUType; GTU type
* @return right adjacent lane, or {@code null} if no such lane
* @throws GTUException in case of multiple adjacent lanes
*/
public LaneRecord right(final GTUType gtuType) throws GTUException
{
if (!this.determinedRight)
{
this.right = adjacent(gtuType, LateralDirectionality.RIGHT);
this.determinedRight = true;
if (this.right.lane.accessibleAdjacentLanesLegal(LateralDirectionality.LEFT, gtuType, this.right.direction)
.contains(this.lane))
{
this.right.left = this;
}
this.right.determinedLeft = true;
}
return this.right;
}
/**
* Returns the left or right adjacent lane, or {@code null} if no such lane.
* @param gtuType GTUType; GTU type
* @param lat LateralDirectionality; left or right
* @return left or right adjacent lane, or {@code null} if no such lane
* @throws GTUException in case of multiple adjacent lanes
*/
private LaneRecord adjacent(final GTUType gtuType, final LateralDirectionality lat) throws GTUException
{
Set<Lane> set = this.lane.accessibleAdjacentLanesLegal(lat, gtuType, this.direction);
Throw.when(set.size() > 1, GTUException.class,
"Default route system found multiple adjacent lanes, which is not supported.");
if (set.size() == 1)
{
return new LaneRecord(this.startDistance, set.iterator().next(), this.direction);
}
return null;
}
/**
* Returns the start distance, relative to the end of the origin lane.
* @return Length; start distance, relative to the end of the origin lane
*/
public Length getStartDistance()
{
return this.startDistance;
}
/**
* Returns whether there is no available next lane (ignoring the route).
* @param gtuType GTUType; GTU type
* @return whether there is no available next lane (ignoring the route)
*/
public boolean isDeadEnd(final GTUType gtuType)
{
return this.lane.downstreamLanes(this.direction, gtuType).isEmpty();
}
}
}