RoadNetwork.java
package org.opentrafficsim.road.network;
import java.util.Iterator;
import java.util.LinkedHashMap;
import java.util.List;
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.base.Identifiable;
import org.djutils.exceptions.Throw;
import org.djutils.immutablecollections.ImmutableSortedSet;
import org.djutils.immutablecollections.ImmutableTreeSet;
import org.djutils.multikeymap.MultiKeyMap;
import org.jgrapht.GraphPath;
import org.jgrapht.alg.shortestpath.DijkstraShortestPath;
import org.jgrapht.graph.SimpleDirectedWeightedGraph;
import org.opentrafficsim.core.dsol.OtsSimulatorInterface;
import org.opentrafficsim.core.gtu.GtuType;
import org.opentrafficsim.core.network.LateralDirectionality;
import org.opentrafficsim.core.network.Link;
import org.opentrafficsim.core.network.Network;
import org.opentrafficsim.core.network.NetworkException;
import org.opentrafficsim.core.network.Node;
import org.opentrafficsim.core.network.route.Route;
import org.opentrafficsim.road.network.lane.CrossSectionLink;
import org.opentrafficsim.road.network.lane.Lane;
/**
* RoadNetwork adds the ability to retrieve lane change information.
* <p>
* Copyright (c) 2013-2024 Delft University of Technology, PO Box 5, 2600 AA, Delft, the Netherlands. All rights reserved. <br>
* BSD-style license. See <a href="https://opentrafficsim.org/docs/license.html">OpenTrafficSim License</a>.
* </p>
* @author <a href="https://github.com/averbraeck" target="_blank">Alexander Verbraeck</a>
* @author <a href="https://github.com/wjschakel">Wouter Schakel</a>
*/
public class RoadNetwork extends Network
{
/** */
private static final long serialVersionUID = 1L;
/** Cached lane graph for legal connections, per GTU type. */
private Map<GtuType, RouteWeightedGraph> legalLaneGraph = new LinkedHashMap<>();
/** Cached lane graph for physical connections. */
private RouteWeightedGraph physicalLaneGraph = null;
/** Cached legal lane change info, over complete length of route. */
private MultiKeyMap<SortedSet<LaneChangeInfo>> legalLaneChangeInfoCache =
new MultiKeyMap<>(GtuType.class, Route.class, Lane.class);
/** Cached physical lane change info, over complete length of route. */
private MultiKeyMap<SortedSet<LaneChangeInfo>> physicalLaneChangeInfoCache = new MultiKeyMap<>(Route.class, Lane.class);
/**
* Construction of an empty network.
* @param id String; the network id.
* @param simulator OtsSimulatorInterface; the DSOL simulator engine
*/
public RoadNetwork(final String id, final OtsSimulatorInterface simulator)
{
super(id, simulator);
}
/**
* Returns lane change info from the given lane. Distances are given from the start of the lane and will never exceed the
* given range. This method returns {@code null} if no valid path exists. If there are no reasons to change lane within
* range, an empty set is returned.
* @param lane Lane; from lane.
* @param route Route; route.
* @param gtuType GtuType; GTU Type.
* @param range Length; maximum range of info to consider, from the start of the given lane.
* @param laneAccessLaw LaneAccessLaw; lane access law.
* @return ImmutableSortedSet<LaneChangeInfo>; lane change info from the given lane, or {@code null} if no path
* exists.
*/
public ImmutableSortedSet<LaneChangeInfo> getLaneChangeInfo(final Lane lane, final Route route, final GtuType gtuType,
final Length range, final LaneAccessLaw laneAccessLaw)
{
Throw.whenNull(lane, "Lane may not be null.");
Throw.whenNull(route, "Route may not be null.");
Throw.whenNull(gtuType, "GTU type may not be null.");
Throw.whenNull(range, "Range may not be null.");
Throw.whenNull(laneAccessLaw, "Lane access law may not be null.");
Throw.when(range.le0(), IllegalArgumentException.class, "Range should be a positive value.");
// get the complete info
SortedSet<LaneChangeInfo> info = getCompleteLaneChangeInfo(lane, route, gtuType, laneAccessLaw);
if (info == null)
{
return null;
}
// find first LaneChangeInfo beyond range, if any
LaneChangeInfo lcInfoBeyondHorizon = null;
Iterator<LaneChangeInfo> iterator = info.iterator();
while (lcInfoBeyondHorizon == null && iterator.hasNext())
{
LaneChangeInfo lcInfo = iterator.next();
if (lcInfo.remainingDistance().gt(range))
{
lcInfoBeyondHorizon = lcInfo;
}
}
// return subset in range
if (lcInfoBeyondHorizon != null)
{
return new ImmutableTreeSet<>(info.headSet(lcInfoBeyondHorizon));
}
return new ImmutableTreeSet<>(info); // empty, or all in range
}
/**
* Returns the complete (i.e. without range) lane change info from the given lane. It is either taken from cache, or
* created.
* @param lane Lane; from lane.
* @param route Route; route.
* @param gtuType GtuType; GTU Type.
* @param laneAccessLaw LaneAccessLaw; lane access law.
* @return SortedSet<LaneChangeInfo>; complete (i.e. without range) lane change info from the given lane, or
* {@code null} if no path exists.
*/
private SortedSet<LaneChangeInfo> getCompleteLaneChangeInfo(final Lane lane, final Route route, final GtuType gtuType,
final LaneAccessLaw laneAccessLaw)
{
// try to get info from the right cache
SortedSet<LaneChangeInfo> outputLaneChangeInfo;
if (laneAccessLaw.equals(LaneAccessLaw.LEGAL))
{
outputLaneChangeInfo = this.legalLaneChangeInfoCache.get(gtuType, route, lane);
// build info if required
if (outputLaneChangeInfo == null)
{
// get the right lane graph for the GTU type, or build it
RouteWeightedGraph graph = this.legalLaneGraph.get(gtuType);
if (graph == null)
{
graph = new RouteWeightedGraph();
this.legalLaneGraph.put(gtuType, graph);
buildGraph(graph, gtuType, laneAccessLaw);
}
List<LaneChangeInfoEdge> path = findPath(lane, graph, gtuType, route);
if (path != null)
{
// derive lane change info from every lane along the path and cache it
boolean originalPath = true;
while (!path.isEmpty())
{
SortedSet<LaneChangeInfo> laneChangeInfo = extractLaneChangeInfo(path);
if (originalPath)
{
outputLaneChangeInfo = laneChangeInfo;
originalPath = false;
}
this.legalLaneChangeInfoCache.put(laneChangeInfo, gtuType, route, path.get(0).fromLane());
path.remove(0); // next lane
}
}
}
}
else if (laneAccessLaw.equals(LaneAccessLaw.PHYSICAL))
{
outputLaneChangeInfo = this.physicalLaneChangeInfoCache.get(route, lane);
// build info if required
if (outputLaneChangeInfo == null)
{
// build the lane graph if required
if (this.physicalLaneGraph == null)
{
this.physicalLaneGraph = new RouteWeightedGraph();
// TODO: Is the GTU type actually relevant for physical? It is used still to find adjacent lanes.
buildGraph(this.physicalLaneGraph, gtuType, laneAccessLaw);
}
List<LaneChangeInfoEdge> path = findPath(lane, this.physicalLaneGraph, gtuType, route);
if (path != null)
{
// derive lane change info from every lane along the path and cache it
boolean originalPath = true;
while (!path.isEmpty())
{
SortedSet<LaneChangeInfo> laneChangeInfo = extractLaneChangeInfo(path);
if (originalPath)
{
outputLaneChangeInfo = laneChangeInfo;
originalPath = false;
}
this.physicalLaneChangeInfoCache.put(laneChangeInfo, route, path.get(0).fromLane());
path.remove(0); // next lane
}
}
}
}
else
{
// in case it is inadvertently extended in the future
throw new RuntimeException(String.format("Unknown LaneChangeLaw %s", laneAccessLaw));
}
return outputLaneChangeInfo;
}
/**
* Builds the graph.
* @param graph RouteWeightedGraph; empty graph to build.
* @param gtuType GtuType; GTU type.
* @param laneChangeLaw LaneChangeLaw; lane change law, legal or physical.
*/
private void buildGraph(final RouteWeightedGraph graph, final GtuType gtuType, final LaneAccessLaw laneChangeLaw)
{
// add vertices
boolean legal = laneChangeLaw.equals(LaneAccessLaw.LEGAL);
for (Link link : this.getLinkMap().values())
{
for (Lane lane : legal ? ((CrossSectionLink) link).getLanes() : ((CrossSectionLink) link).getLanesAndShoulders())
{
graph.addVertex(lane);
}
// each end node may be a destination for the shortest path search
graph.addVertex(link.getEndNode());
}
// add edges
for (Link link : this.getLinkMap().values())
{
if (link instanceof CrossSectionLink cLink)
{
for (Lane lane : legal ? cLink.getLanes() : cLink.getLanesAndShoulders())
{
// adjacent lanes
for (LateralDirectionality lat : List.of(LateralDirectionality.LEFT, LateralDirectionality.RIGHT))
{
Set<Lane> adjacentLanes;
if (legal)
{
adjacentLanes = lane.accessibleAdjacentLanesLegal(lat, gtuType);
}
else
{
adjacentLanes = lane.accessibleAdjacentLanesPhysical(lat, gtuType);
}
for (Lane adjacentLane : adjacentLanes)
{
LaneChangeInfoEdgeType type = lat.equals(LateralDirectionality.LEFT) ? LaneChangeInfoEdgeType.LEFT
: LaneChangeInfoEdgeType.RIGHT;
// downstream link may be null for lateral edges
LaneChangeInfoEdge edge = new LaneChangeInfoEdge(lane, type, null);
graph.addEdge(lane, adjacentLane, edge);
}
}
// next lanes
Set<Lane> nextLanes = lane.nextLanes(legal ? gtuType : null);
for (Lane nextLane : nextLanes)
{
LaneChangeInfoEdge edge =
new LaneChangeInfoEdge(lane, LaneChangeInfoEdgeType.DOWNSTREAM, nextLane.getLink());
graph.addEdge(lane, nextLane, edge);
}
// add edge towards end node so that it can be used as a destination in the shortest path search
LaneChangeInfoEdge edge = new LaneChangeInfoEdge(lane, LaneChangeInfoEdgeType.DOWNSTREAM, null);
graph.addEdge(lane, lane.getLink().getEndNode(), edge);
}
}
}
}
/**
* Returns a set of lane change info, extracted from the graph.
* @param lane Lane; from lane.
* @param graph RouteWeightedGraph; graph.
* @param gtuType GtuType; GTU Type.
* @param route Route; route.
* @return List<LaneChangeInfoEdge>; path derived from the graph, or {@code null} if there is no path.
*/
private List<LaneChangeInfoEdge> findPath(final Lane lane, final RouteWeightedGraph graph, final GtuType gtuType,
final Route route)
{
// if there is no route, find the destination node by moving down the links (no splits allowed)
Node destination = null;
Route routeForWeights = route;
if (route == null)
{
destination = graph.getNoRouteDestinationNode(gtuType);
try
{
routeForWeights = getShortestRouteBetween(gtuType, lane.getLink().getStartNode(), destination);
}
catch (NetworkException exception)
{
// this should not happen, as we obtained the destination by moving downstream towards the end of the network
throw new RuntimeException("Could not find route to destination.", exception);
}
}
else
{
// otherwise, get destination node from route, which is the last node on a link with lanes (i.e. no connector)
List<Node> nodes = route.getNodes();
for (int i = nodes.size() - 1; i > 0; i--)
{
Link link = getLink(nodes.get(i - 1), nodes.get(i));
if (link instanceof CrossSectionLink && !((CrossSectionLink) link).getLanes().isEmpty())
{
destination = nodes.get(i);
break; // found most downstream link with lanes, who's end node is the destination for lane changes
}
}
Throw.whenNull(destination, "Route has no links with lanes, "
+ "unable to find a suitable destination node regarding lane change information.");
}
// set the route on the path for route-dependent edge weights
graph.setRoute(routeForWeights);
// find the shortest path
GraphPath<Identifiable, LaneChangeInfoEdge> path = DijkstraShortestPath.findPathBetween(graph, lane, destination);
return path == null ? null : path.getEdgeList();
}
/**
* Extracts lane change info from a path.
* @param path List<LaneChangeInfoEdge>; path.
* @return SortedSet<LaneChangeInfo>; lane change info.
*/
private SortedSet<LaneChangeInfo> extractLaneChangeInfo(final List<LaneChangeInfoEdge> path)
{
SortedSet<LaneChangeInfo> info = new TreeSet<>();
Length x = Length.ZERO; // cumulative longitudinal distance
int n = 0; // number of applied lane changes
boolean inLateralState = false; // consecutive lateral moves in the path create 1 LaneChangeInfo
for (LaneChangeInfoEdge edge : path)
{
LaneChangeInfoEdgeType lcType = edge.laneChangeInfoEdgeType();
int lat = lcType.equals(LaneChangeInfoEdgeType.LEFT) ? -1 : (lcType.equals(LaneChangeInfoEdgeType.RIGHT) ? 1 : 0);
// check opposite lateral direction
if (n * lat < 0)
{
/*
* The required direction is opposite a former required direction, in which case all further lane change
* information is not yet of concern. For example, we first need to make 1 right lane change for a lane drop,
* and then later 2 lane changes to the left for a split. The latter information is pointless before the lane
* drop; we are not going to stay on the lane longer as it won't affect the ease of the left lane changes later.
*/
break;
}
// increase n, x, and trigger (consecutive) lateral move start or stop
if (lat == 0)
{
// lateral move stop
if (inLateralState)
{
// TODO: isDeadEnd should be removed from LaneChangeInfo, behavior should consider legal vs. physical
boolean isDeadEnd = false;
info.add(new LaneChangeInfo(Math.abs(n), x, isDeadEnd,
n < 0 ? LateralDirectionality.LEFT : LateralDirectionality.RIGHT));
inLateralState = false;
// don't add the length of the previous lane, that was already done for the first lane of all lateral moves
}
else
{
// longitudinal move, we need to add distance to x
x = x.plus(edge.fromLane().getLength());
}
}
else
{
// lateral move start
if (!inLateralState)
{
x = x.plus(edge.fromLane().getLength()); // need to add length of first lane of all lateral moves
inLateralState = true;
}
// increase lane change count (negative for left)
n += lat;
}
}
return info;
}
/**
* Clears all lane change info graphs and cached sets. This method should be invoked on every network change that affects
* lane changes and the distances within which they need to be performed.
*/
public void clearLaneChangeInfoCache()
{
this.legalLaneGraph.clear();
this.physicalLaneGraph = null;
this.legalLaneChangeInfoCache = new MultiKeyMap<>(GtuType.class, Route.class, Lane.class);
this.physicalLaneChangeInfoCache = new MultiKeyMap<>(Route.class, Lane.class);
}
/**
* A {@code SimpleDirectedWeightedGraph} to search over the lanes, where the weight of an edge (movement between lanes) is
* tailored to providing lane change information. The vertex type is {@code Identifiable} such that both {@code Lane}'s and
* {@code Node}'s can be used. The latter is required to find paths towards a destination node.
* <p>
* Copyright (c) 2022-2024 Delft University of Technology, PO Box 5, 2600 AA, Delft, the Netherlands. All rights reserved.
* <br>
* BSD-style license. See <a href="https://opentrafficsim.org/docs/license.html">OpenTrafficSim License</a>.
* </p>
* @author <a href="https://github.com/wjschakel">Wouter Schakel</a>
*/
private class RouteWeightedGraph extends SimpleDirectedWeightedGraph<Identifiable, LaneChangeInfoEdge>
{
/** */
private static final long serialVersionUID = 20220923L;
/** Route. */
private Route route;
/** Node in the network that is the destination if no route is used. */
private Node noRouteDestination = null;
/**
* Constructor.
*/
RouteWeightedGraph()
{
super(LaneChangeInfoEdge.class);
}
/**
* Set the route.
* @param route Route; route.
*/
public void setRoute(final Route route)
{
Throw.whenNull(route, "Route may not be null for lane change information.");
this.route = route;
}
/**
* Returns the weight of moving from one lane to the next. In order to find the latest possible location at which lane
* changes may still be performed, the longitudinal weights are 1.0 while the lateral weights are 1.0 + 1/X, where X is
* the number (index) of the link within the route. This favors later lane changes for the shortest path algorithm, as
* we are interested in the distances within which the lane change have to be performed. In the case an edge is towards
* a link that is not in a given route, a positive infinite weight is returned. Finally, when the edge is towards a
* node, which may be the destination in a route, 0.0 is returned.
*/
@Override
public double getEdgeWeight(final LaneChangeInfoEdge e)
{
if (e.laneChangeInfoEdgeType().equals(LaneChangeInfoEdgeType.LEFT)
|| e.laneChangeInfoEdgeType().equals(LaneChangeInfoEdgeType.RIGHT))
{
int indexEndNode = this.route.indexOf(e.fromLane().getLink().getEndNode());
return 1.0 + 1.0 / indexEndNode; // lateral, reduce weight for further lane changes
}
Link toLink = e.toLink();
if (toLink == null)
{
return 0.0; // edge towards Node, which may be the destination in a Route
}
if (this.route.contains(toLink.getEndNode())
&& this.route.indexOf(toLink.getEndNode()) == this.route.indexOf(toLink.getStartNode()) + 1)
{
return 1.0; // downstream, always 1.0 if the next lane is on the route
}
return Double.POSITIVE_INFINITY; // next lane not on the route, this is a dead-end branch for the route
}
/**
* Returns the destination node to use when no route is available. This will be the last node found moving downstream.
* @param gtuType GtuType; GTU type.
* @return Node; destination node to use when no route is available.
*/
public Node getNoRouteDestinationNode(final GtuType gtuType)
{
if (this.noRouteDestination == null)
{
// get any lane from the network
Lane lane = null;
Iterator<Identifiable> iterator = this.vertexSet().iterator();
while (lane == null && iterator.hasNext())
{
Identifiable next = iterator.next();
if (next instanceof Lane)
{
lane = (Lane) next;
}
}
Throw.when(lane == null, RuntimeException.class, "Requesting destination node on network without lanes.");
// move to downstream link for as long as there is 1 downstream link
try
{
Link link = lane.getLink();
Set<Link> downstreamLinks = link.getEndNode().nextLinks(gtuType, link);
while (downstreamLinks.size() == 1)
{
link = downstreamLinks.iterator().next();
downstreamLinks = link.getEndNode().nextLinks(gtuType, link);
}
Throw.when(downstreamLinks.size() > 1, RuntimeException.class, "Using null route on network with split. "
+ "Unable to find a destination to find lane change info towards.");
this.noRouteDestination = link.getEndNode();
}
catch (NetworkException ne)
{
throw new RuntimeException("Requesting lane change info from link that does not allow the GTU type.", ne);
}
}
return this.noRouteDestination;
}
}
/**
* Edge between two lanes, or between a lane and a node (to provide the shortest path algorithm with a suitable
* destination). From a list of these from a path, the lane change information along the path (distances and number of lane
* changes) can be derived.
* <p>
* Copyright (c) 2022-2024 Delft University of Technology, PO Box 5, 2600 AA, Delft, the Netherlands. All rights reserved.
* <br>
* BSD-style license. See <a href="https://opentrafficsim.org/docs/license.html">OpenTrafficSim License</a>.
* </p>
* @author <a href="https://github.com/wjschakel">Wouter Schakel</a>
* @param fromLane Lane; from lane, to allow construction of distances from a path.
* @param laneChangeInfoEdgeType LaneChangeInfoEdgeType; the type of lane to lane movement performed along this edge.
* @param toLink Link; to link (of the lane this edge moves to).
*/
private static record LaneChangeInfoEdge(Lane fromLane, LaneChangeInfoEdgeType laneChangeInfoEdgeType, Link toLink)
{
}
/**
* Enum to provide information on the lane to lane movement in a path.
* <p>
* Copyright (c) 2022-2024 Delft University of Technology, PO Box 5, 2600 AA, Delft, the Netherlands. All rights reserved.
* <br>
* BSD-style license. See <a href="https://opentrafficsim.org/docs/license.html">OpenTrafficSim License</a>.
* </p>
* @author <a href="https://github.com/wjschakel">Wouter Schakel</a>
*/
private enum LaneChangeInfoEdgeType
{
/** Left lane change. */
LEFT,
/** Right lane change. */
RIGHT,
/** Downstream movement, either towards a lane, or towards a node (which may be the destination in a route). */
DOWNSTREAM;
}
}