diff --git a/matsim/src/main/java/org/matsim/core/network/algorithms/TarjanSCCNetworkCleaner.java b/matsim/src/main/java/org/matsim/core/network/algorithms/TarjanSCCNetworkCleaner.java new file mode 100644 index 00000000000..5d7c2f9f4f1 --- /dev/null +++ b/matsim/src/main/java/org/matsim/core/network/algorithms/TarjanSCCNetworkCleaner.java @@ -0,0 +1,214 @@ +package org.matsim.core.network.algorithms; + +import org.apache.logging.log4j.LogManager; +import org.apache.logging.log4j.Logger; +import org.matsim.api.core.v01.Id; +import org.matsim.api.core.v01.network.Link; +import org.matsim.api.core.v01.network.Network; +import org.matsim.api.core.v01.network.Node; +import org.matsim.core.router.turnRestrictions.ColoredLink; +import org.matsim.core.router.turnRestrictions.TurnRestrictionsContext; +import org.matsim.core.utils.misc.Counter; + +import java.util.*; + +/** + * A network cleaner that uses Tarjan's algorithm to find all strongly + * connected components (SCCs). It then retains only the largest SCC + * (by number of nodes) in the Network. + * + * Also accounts for disallowed next links. To capture multi step and overlapping restrictions, the algorithm + * is applied iteratively until the delta in node sizes between two iterations is zero. + * + * DISCLAIMER: + * - This is a recursive implementation. Large or deep networks may cause a StackOverflowError. + * - For extremely large networks, consider an iterative Tarjan approach or increase JVM stack size (e.g.,'-Xss100m') + * + * @author nkuehnel / MOIA + */ +public final class TarjanSCCNetworkCleaner { + + private static final Logger log = LogManager.getLogger(TarjanSCCNetworkCleaner.class); + + // ------------------------- + // Internal Tarjan structures + // ------------------------- + private List>> sccList; + private Map, List>> adjacencyList; // adjacency for each node ID + private Map, Integer> discoveryTime; // discovery index/time for each node + private Map, Integer> lowLink; // low-link values for each node + private Deque> stack; // stack of active nodes + private Set> onStack; // quick check if a node is on the stack + private int timeCounter = 0; // global "time" counter for DFS + private TurnRestrictionsContext turnRestrictionsContext; + + // ------------------------- + // The network to be cleaned + // ------------------------- + private final Network network; + + public TarjanSCCNetworkCleaner(Network network) { + this.network = network; + } + + /** + * Runs Tarjan's algorithm to find strongly connected components, + * then reduces the network so it only contains the largest SCC. + */ + public void run() { + int currentSize = network.getLinks().size(); + int delta = Integer.MAX_VALUE; + int iteration = 0; + + while (delta > 0) { + log.info("Running iteration " + iteration); + runImpl(); + + delta = currentSize - network.getLinks().size(); + log.info("Current delta: " + delta); + currentSize = network.getLinks().size(); + iteration++; + } + } + + private void runImpl() { + turnRestrictionsContext = TurnRestrictionsContext.buildContext(network); + + log.info("Building adjacency list from the MATSim network..."); + buildAdjacencyList(); + + log.info("Running Tarjan's algorithm to find SCCs..."); + // Initialize Tarjan data structures + sccList = new ArrayList<>(); + discoveryTime = new HashMap<>(adjacencyList.size()); + lowLink = new HashMap<>(adjacencyList.size()); + stack = new ArrayDeque<>(); + onStack = new HashSet<>(); + + // Run Tarjan DFS on each unvisited node + for (Id nodeId : adjacencyList.keySet()) { + if (!discoveryTime.containsKey(nodeId)) { + strongConnect(nodeId); + } + } + + log.info("Found " + sccList.size() + " strongly connected components in total."); + + // Identify the largest SCC and remove all others + log.info("Retaining only the largest SCC..."); + List>> largestSCC = findLargestSCCs(sccList); + for (Set> set : largestSCC) { + reduceToCluster(set); + } + } + + // ----------------------------------------------------------------------- + // 1. Build adjacency from the MATSim Network, ignoring turn restrictions + // ----------------------------------------------------------------------- + private void buildAdjacencyList() { + adjacencyList = new HashMap<>(network.getNodes().size()); + + // Initialize adjacency lists + for (Node node : network.getNodes().values()) { + adjacencyList.put(node.getId(), new ArrayList<>()); + } + + Counter counter = new Counter("Node ", "", 10); + + for (Node node : network.getNodes().values()) { + List> outNeighbors = adjacencyList.get(node.getId()); + + for (Link link : node.getOutLinks().values()) { + + // Collect possible colored links from replacedLinks + // If there's a replaced link add it to the stack + Deque stack = new ArrayDeque<>(); + if (turnRestrictionsContext.getReplacedLinks().containsKey(link.getId())) { + stack.add(turnRestrictionsContext.getReplacedLinks().get(link.getId())); + } else { + outNeighbors.add(link.getToNode().getId()); + } + + // 2) Expand each colored link using a stack approach + Set visitedColoredLinks = new HashSet<>(); + + while (!stack.isEmpty()) { + ColoredLink currentLink = stack.pop(); + + // Skip if we've already processed this colored link + if (!visitedColoredLinks.add(currentLink)) { + continue; + } + + // If it leads to another colored node, push out-links + if (currentLink.getToColoredNode() != null) { + for (ColoredLink outLink : currentLink.getToColoredNode().outLinks()) { + stack.push(outLink); + } + } else { + // Else, we reached a real node => add to adjacency + Node realToNode = currentLink.getToNode(); + outNeighbors.add(realToNode.getId()); + } + } + } + + counter.incCounter(); + } + counter.printCounter(); + } + + private void strongConnect(Id nodeId) { + // Initialize discovery time and low-link + discoveryTime.put(nodeId, timeCounter); + lowLink.put(nodeId, timeCounter); + timeCounter++; + stack.push(nodeId); + onStack.add(nodeId); + + // Explore neighbors + for (Id neighborId : adjacencyList.get(nodeId)) { + if (!discoveryTime.containsKey(neighborId)) { + // Neighbor not visited, recurse + strongConnect(neighborId); + // Update lowLink + lowLink.put(nodeId, Math.min(lowLink.get(nodeId), lowLink.get(neighborId))); + } else if (onStack.contains(neighborId)) { + // Neighbor is in the current stack, so it's part of the SCC + lowLink.put(nodeId, Math.min(lowLink.get(nodeId), discoveryTime.get(neighborId))); + } + } + + // If nodeId is a root node, pop the stack and generate an SCC + if (lowLink.get(nodeId).equals(discoveryTime.get(nodeId))) { + // Start a new SCC + Set> scc = new HashSet<>(); + Id w; + do { + w = stack.pop(); + onStack.remove(w); + scc.add(w); + } while (!w.equals(nodeId)); + // Add this SCC to the list + sccList.add(scc); + } + } + + private static List>> findLargestSCCs(List>> sccList) { + return sccList.stream().sorted(Comparator.comparingInt((Set> ids) -> ids.size()).reversed()).limit(1).toList(); + } + + private void reduceToCluster(Set> biggestCluster) { + log.info("Biggest SCC has " + biggestCluster.size() + " nodes."); + log.info("Removing all nodes/links not in the largest SCC..."); + + List allNodes = new ArrayList<>(network.getNodes().values()); + for (Node node : allNodes) { + if (!biggestCluster.contains(node.getId())) { + network.removeNode(node.getId()); + } + } + log.info("Resulting network has " + network.getNodes().size() + " nodes and " + + network.getLinks().size() + " links."); + } +} diff --git a/matsim/src/main/java/org/matsim/core/router/speedy/SpeedyGraphBuilder.java b/matsim/src/main/java/org/matsim/core/router/speedy/SpeedyGraphBuilder.java index 54f41ee1aa4..4a3f08fb280 100644 --- a/matsim/src/main/java/org/matsim/core/router/speedy/SpeedyGraphBuilder.java +++ b/matsim/src/main/java/org/matsim/core/router/speedy/SpeedyGraphBuilder.java @@ -4,17 +4,11 @@ import org.matsim.api.core.v01.network.Link; import org.matsim.api.core.v01.network.Network; import org.matsim.api.core.v01.network.Node; -import org.matsim.core.network.DisallowedNextLinks; import org.matsim.core.network.NetworkUtils; +import org.matsim.core.router.turnRestrictions.ColoredLink; +import org.matsim.core.router.turnRestrictions.TurnRestrictionsContext; -import java.util.ArrayList; import java.util.Arrays; -import java.util.Collection; -import java.util.HashMap; -import java.util.HashSet; -import java.util.List; -import java.util.Map; -import java.util.Set; /** * Creates a {@link SpeedyGraph} for a provided {@link Network}. @@ -47,67 +41,12 @@ private static boolean hasTurnRestrictions(Network network) { } private SpeedyGraph buildWithTurnRestrictions(Network network) { - /* - * The implementation follows the algorithm developed by - * Marcel Rieser (Simunto) and Hannes Rewald (Volkswagen Group) - * in October 2023 during the MATSim Code Sprint week in Berlin, - * and documented at https://github.com/matsim-org/matsim-code-examples/wiki/turn-restrictions. - * - * TL;DR: - * Main idea of algorithm: for each link with turn-restrictions, create a sub-graph of the network - * containing all links required to model all allowed paths, but exclude the last link of turn restrictions' - * link sequence to enforce the "disallow" along that route. - * - * - * Implementation details: - * - The easiest solution would be to make a copy of the original network, then start modifying it - * according to the algorithm above (e.g. add and delete links and nodes), and then to convert the - * resulting network into a graph. This would require substantial amount of memory for duplicating - * the complete network, and might pose problems as we will have multiple links with the same id. - * - Given the assumption that turn restrictions apply only to a small amount of the full network, - * we keep the original network intact. Instead, we keep all modifications in separate data-structures - * so they can be used to create the routing-graph. - * - If the network is already filtered for a specific mode, it might be that links referenced - * in a turn restriction are missing. The implementation must be able to deal with such cases, - * prevent NullPointerExceptions. - * - As turn restrictions are mode-specific, the algorithm needs to know for which mode the - * turn restriction need to be considered. - */ - - TurnRestrictionsContext context = new TurnRestrictionsContext(network); - - for (Link startingLink : network.getLinks().values()) { - DisallowedNextLinks disallowedNextLinks = NetworkUtils.getDisallowedNextLinks(startingLink); - if (disallowedNextLinks == null) { - continue; - } - Collection>> turnRestrictions = disallowedNextLinks.getMergedDisallowedLinkSequences(); - if (turnRestrictions == null || turnRestrictions.isEmpty()) { - continue; - } - // steps 1 to 5: - ColoredLink coloredStartingLink = applyTurnRestriction(context, turnRestrictions, startingLink); - - // step 6: turn restrictions have to be applied separately to existing colored links as well. - // see if there are already colored link copies available for this starting link - List coloredLinks = context.coloredLinksPerLinkMap.get(startingLink.getId()); - if (coloredLinks != null) { - for (ColoredLink coloredLink : coloredLinks) { - // optimization: re-point toNode instead of re-applying full turn restrictions - if (coloredLink.toColoredNode == null) { - coloredLink.toColoredNode = coloredStartingLink.toColoredNode; - coloredLink.toNode = null; - } else { - applyTurnRestriction(context, turnRestrictions, coloredLink); - } - } - } - } + TurnRestrictionsContext context = TurnRestrictionsContext.buildContext(network); // create routing graph from context - this.nodeCount = context.nodeCount; - this.linkCount = context.linkCount; + this.nodeCount = context.getNodeCount(); + this.linkCount = context.getLinkCount(); this.nodeData = new int[this.nodeCount * SpeedyGraph.NODE_SIZE]; this.linkData = new int[this.linkCount * SpeedyGraph.LINK_SIZE]; @@ -121,179 +60,21 @@ private SpeedyGraph buildWithTurnRestrictions(Network network) { this.nodes[node.getId().index()] = node; } for (Link link : network.getLinks().values()) { - if (context.replacedLinks.get(link.getId()) == null) { + if (context.getReplacedLinks().get(link.getId()) == null) { addLink(link); } } - for (ColoredNode node : context.coloredNodes) { - this.nodes[node.index] = node.node; + for (TurnRestrictionsContext.ColoredNode node : context.getColoredNodes()) { + this.nodes[node.index()] = node.node(); } - for (ColoredLink link : context.coloredLinks) { + for (ColoredLink link : context.getColoredLinks()) { addLink(link); } return new SpeedyGraph(this.nodeData, this.linkData, this.nodes, this.links, true); } - private ColoredLink applyTurnRestriction(TurnRestrictionsContext context, Collection>> restrictions, Link startingLink) { - return this.applyTurnRestriction(context, restrictions, startingLink, null); - } - private void applyTurnRestriction(TurnRestrictionsContext context, Collection>> restrictions, ColoredLink startingLink) { - this.applyTurnRestriction(context, restrictions, null, startingLink); - } - - private ColoredLink applyTurnRestriction(TurnRestrictionsContext context, Collection>> restrictions, Link startingLink, ColoredLink coloredStartingLink) { - Set affectedNodes = new HashSet<>(); - Set affectedColoredNodes = new HashSet<>(); - Set affectedLinks = new HashSet<>(); - Set affectedColoredLinks = new HashSet<>(); - Set> endLinkIds = new HashSet<>(); - - // step 1 and 2: collect end-links, affected-links and affected-nodes - for (List> restriction : restrictions) { - - Link currentLink; - ColoredLink currentColoredLink; - Node currentNode = startingLink == null ? null : startingLink.getToNode(); - // due to the optimization in step 6, every colored starting link leads to a colored to-node - ColoredNode currentColoredNode = coloredStartingLink == null ? null : coloredStartingLink.toColoredNode; - - // walk along the restricted path, collect affectedLinks, affectedNodes and endLink - for (Id linkId : restriction) { - if (currentNode != null) { - // handle regular node - affectedNodes.add(currentNode); - currentLink = null; - currentColoredLink = null; - for (Link outLink : currentNode.getOutLinks().values()) { - if (outLink.getId() == linkId) { - currentColoredLink = context.replacedLinks.get(linkId); - if (currentColoredLink == null) { - currentLink = outLink; - } - break; - } - } - - if (currentLink != null) { - affectedLinks.add(currentLink); - currentNode = currentLink.getToNode(); - currentColoredNode = null; - } - if (currentColoredLink != null) { - affectedColoredLinks.add(currentColoredLink); - currentNode = currentColoredLink.toNode; - currentColoredNode = currentColoredLink.toColoredNode; - } - if (currentLink == null && currentColoredLink == null) { - // link of restriction is no longer part of the network, maybe we are in a sub-graph - break; - } - } else if (currentColoredNode != null) { - // handle colored node - affectedColoredNodes.add(currentColoredNode); - currentLink = null; - currentColoredLink = null; - for (ColoredLink outLink : currentColoredNode.outLinks) { - if (outLink.link.getId() == linkId) { - currentColoredLink = outLink; - break; - } - } - if (currentColoredLink != null) { - affectedColoredLinks.add(currentColoredLink); - currentNode = currentColoredLink.toNode; - currentColoredNode = currentColoredLink.toColoredNode; - } - if (currentColoredLink == null) { - // link of restriction is no longer part of the network, maybe we are in a sub-graph - break; - } - } - } - endLinkIds.add(restriction.get(restriction.size() - 1)); - } - - // step 3: create colored copies of nodes - Map, ColoredNode> newlyColoredNodes = new HashMap<>(); - for (Node affectedNode : affectedNodes) { - int nodeIndex = context.nodeCount; - context.nodeCount++; - ColoredNode newlyColoredNode = new ColoredNode(nodeIndex, affectedNode, new ArrayList<>()); - newlyColoredNodes.put(affectedNode.getId(), newlyColoredNode); - context.coloredNodes.add(newlyColoredNode); - } - for (ColoredNode affectedColoredNode : affectedColoredNodes) { - int nodeIndex = context.nodeCount; - context.nodeCount++; - ColoredNode newlyColoredNode = new ColoredNode(nodeIndex, affectedColoredNode.node, new ArrayList<>()); - newlyColoredNodes.put(affectedColoredNode.node.getId(), newlyColoredNode); - context.coloredNodes.add(newlyColoredNode); - } - - // step 4: create colored copies of links - for (Node affectedNode : affectedNodes) { - for (Link outLink : affectedNode.getOutLinks().values()) { - if (endLinkIds.contains(outLink.getId())) { - continue; - } - ColoredLink replacedOutLink = context.replacedLinks.get(outLink.getId()); - int linkIndex = context.linkCount; - context.linkCount++; - ColoredLink newlyColoredLink; - ColoredNode fromNode = newlyColoredNodes.get(outLink.getFromNode().getId()); - if (affectedLinks.contains(outLink) || (replacedOutLink != null && affectedColoredLinks.contains(replacedOutLink))) { - ColoredNode toNode = newlyColoredNodes.get(outLink.getToNode().getId()); - newlyColoredLink = new ColoredLink(linkIndex, outLink, fromNode, null, toNode, null); - } else { - Node toNode = outLink.getToNode(); - newlyColoredLink = new ColoredLink(linkIndex, outLink, fromNode, null, null, toNode); - } - fromNode.outLinks.add(newlyColoredLink); - context.coloredLinks.add(newlyColoredLink); - context.coloredLinksPerLinkMap.computeIfAbsent(outLink.getId(), id -> new ArrayList<>(3)).add(newlyColoredLink); - } - } - for (ColoredNode affectedNode : affectedColoredNodes) { - for (ColoredLink outLink : affectedNode.outLinks) { - if (endLinkIds.contains(outLink.link.getId())) { - continue; - } - int linkIndex = context.linkCount; - context.linkCount++; - ColoredLink newlyColoredLink; - ColoredNode fromNode = newlyColoredNodes.get(outLink.link.getFromNode().getId()); - if (affectedColoredLinks.contains(outLink)) { - ColoredNode toNode = newlyColoredNodes.get(outLink.link.getToNode().getId()); - newlyColoredLink = new ColoredLink(linkIndex, outLink.link, fromNode, null, toNode, null); - } else { - newlyColoredLink = new ColoredLink(linkIndex, outLink.link, fromNode, null, outLink.toColoredNode, outLink.toNode); - } - fromNode.outLinks.add(newlyColoredLink); - context.coloredLinks.add(newlyColoredLink); - context.coloredLinksPerLinkMap.computeIfAbsent(outLink.link.getId(), id -> new ArrayList<>(3)).add(newlyColoredLink); - } - } - - // step 5: replace starting link - if (startingLink != null) { - ColoredNode toNode = newlyColoredNodes.get(startingLink.getToNode().getId()); - int linkIndex = startingLink.getId().index(); // re-use the index - ColoredLink newlyColoredStartingLink = new ColoredLink(linkIndex, startingLink, null, startingLink.getFromNode(), toNode, null); - context.coloredLinks.add(newlyColoredStartingLink); - context.replacedLinks.put(startingLink.getId(), newlyColoredStartingLink); - - return newlyColoredStartingLink; - } - if (coloredStartingLink != null) { - // don't really replace the colored started link, but re-point it to the newly colored node - coloredStartingLink.toColoredNode = newlyColoredNodes.get(coloredStartingLink.link.getToNode().getId()); - return null; - - } - throw new IllegalArgumentException("either startingLink or coloredStartingLink must be set"); - } private SpeedyGraph buildWithoutTurnRestrictions(Network network) { this.nodeCount = Id.getNumberOfIds(Node.class); @@ -337,31 +118,31 @@ private void addLink(Link link) { private void addLink(ColoredLink link) { int fromNodeIdx = -1; int toNodeIdx = -1; - int linkIdx = link.index; + int linkIdx = link.getIndex(); - if (link.fromColoredNode != null) { - fromNodeIdx = link.fromColoredNode.index; + if (link.getFromColoredNode() != null) { + fromNodeIdx = link.getFromColoredNode().index(); } - if (link.fromNode != null) { - fromNodeIdx = link.fromNode.getId().index(); + if (link.getFromNode() != null) { + fromNodeIdx = link.getFromNode().getId().index(); } - if (link.toColoredNode != null) { - toNodeIdx = link.toColoredNode.index; + if (link.getToColoredNode() != null) { + toNodeIdx = link.getToColoredNode().index(); } - if (link.toNode != null) { - toNodeIdx = link.toNode.getId().index(); + if (link.getToNode() != null) { + toNodeIdx = link.getToNode().getId().index(); } int base = linkIdx * SpeedyGraph.LINK_SIZE; this.linkData[base + 2] = fromNodeIdx; this.linkData[base + 3] = toNodeIdx; - this.linkData[base + 4] = (int) Math.round(link.link.getLength() * 100.0); - this.linkData[base + 5] = (int) Math.round(link.link.getLength() / link.link.getFreespeed() * 100.0); + this.linkData[base + 4] = (int) Math.round(link.getLink().getLength() * 100.0); + this.linkData[base + 5] = (int) Math.round(link.getLink().getLength() / link.getLink().getFreespeed() * 100.0); setOutLink(fromNodeIdx, linkIdx); setInLink(toNodeIdx, linkIdx); - this.links[linkIdx] = link.link; + this.links[linkIdx] = link.getLink(); } private void setOutLink(int fromNodeIdx, int linkIdx) { @@ -394,53 +175,4 @@ private void setInLink(int toNodeIdx, int linkIdx) { this.linkData[lastLinkIdx * SpeedyGraph.LINK_SIZE + 1] = linkIdx; } - private static class TurnRestrictionsContext { - int nodeCount; - int linkCount; - final Network network; - Map, ColoredLink> replacedLinks = new HashMap<>(); - List coloredNodes = new ArrayList<>(); - List coloredLinks = new ArrayList<>(); - Map, List> coloredLinksPerLinkMap = new HashMap<>(); - - public TurnRestrictionsContext(Network network) { - this.network = network; - this.nodeCount = Id.getNumberOfIds(Node.class); - this.linkCount = Id.getNumberOfIds(Link.class); - - } - } - - private static final class ColoredLink { - private final int index; - private final Link link; - private final ColoredNode fromColoredNode; - private final Node fromNode; - private ColoredNode toColoredNode; - private Node toNode; - - private ColoredLink( - int index, - Link link, - ColoredNode fromColoredNode, - Node fromNode, - ColoredNode toColoredNode, - Node toNode - ) { - this.index = index; - this.link = link; - this.fromColoredNode = fromColoredNode; - this.fromNode = fromNode; - this.toColoredNode = toColoredNode; - this.toNode = toNode; - } - } - - private record ColoredNode ( - int index, - Node node, - List outLinks - ) { - } - } diff --git a/matsim/src/main/java/org/matsim/core/router/turnRestrictions/ColoredLink.java b/matsim/src/main/java/org/matsim/core/router/turnRestrictions/ColoredLink.java new file mode 100644 index 00000000000..f38098d8ddc --- /dev/null +++ b/matsim/src/main/java/org/matsim/core/router/turnRestrictions/ColoredLink.java @@ -0,0 +1,63 @@ +package org.matsim.core.router.turnRestrictions; + +import org.matsim.api.core.v01.network.Link; +import org.matsim.api.core.v01.network.Node; + +public final class ColoredLink { + private final int index; + private final Link link; + private final TurnRestrictionsContext.ColoredNode fromColoredNode; + private final Node fromNode; + private TurnRestrictionsContext.ColoredNode toColoredNode; + private Node toNode; + + public ColoredLink( + int index, + Link link, + TurnRestrictionsContext.ColoredNode fromColoredNode, + Node fromNode, + TurnRestrictionsContext.ColoredNode toColoredNode, + Node toNode + ) { + this.index = index; + this.link = link; + this.fromColoredNode = fromColoredNode; + this.fromNode = fromNode; + this.toColoredNode = toColoredNode; + this.toNode = toNode; + } + + public int getIndex() { + return index; + } + + public Link getLink() { + return link; + } + + public TurnRestrictionsContext.ColoredNode getFromColoredNode() { + return fromColoredNode; + } + + public Node getFromNode() { + return fromNode; + } + + public TurnRestrictionsContext.ColoredNode getToColoredNode() { + return toColoredNode; + } + + public Node getToNode() { + return toNode; + } + + //package private only + void setToColoredNode(TurnRestrictionsContext.ColoredNode toColoredNode) { + this.toColoredNode = toColoredNode; + } + + //package private only + void setToNode(Node node) { + this.toNode = node; + } +} diff --git a/matsim/src/main/java/org/matsim/core/router/turnRestrictions/TurnRestrictionsContext.java b/matsim/src/main/java/org/matsim/core/router/turnRestrictions/TurnRestrictionsContext.java new file mode 100644 index 00000000000..8ca7b5f8494 --- /dev/null +++ b/matsim/src/main/java/org/matsim/core/router/turnRestrictions/TurnRestrictionsContext.java @@ -0,0 +1,278 @@ +package org.matsim.core.router.turnRestrictions; + +import org.matsim.api.core.v01.Id; +import org.matsim.api.core.v01.network.Link; +import org.matsim.api.core.v01.network.Network; +import org.matsim.api.core.v01.network.Node; +import org.matsim.core.network.DisallowedNextLinks; +import org.matsim.core.network.NetworkUtils; + +import java.util.*; + +public final class TurnRestrictionsContext { + + private int nodeCount; + private int linkCount; + + private Map, ColoredLink> replacedLinks = new HashMap<>(); + private List coloredNodes = new ArrayList<>(); + private List coloredLinks = new ArrayList<>(); + private Map, List> coloredLinksPerLinkMap = new HashMap<>(); + + private TurnRestrictionsContext() { + this.nodeCount = Id.getNumberOfIds(Node.class); + this.linkCount = Id.getNumberOfIds(Link.class); + } + + public static TurnRestrictionsContext buildContext(Network network) { + /* + * The implementation follows the algorithm developed by + * Marcel Rieser (Simunto) and Hannes Rewald (Volkswagen Group) + * in October 2023 during the MATSim Code Sprint week in Berlin, + * and documented at https://github.com/matsim-org/matsim-code-examples/wiki/turn-restrictions. + * + * TL;DR: + * Main idea of algorithm: for each link with turn-restrictions, create a sub-graph of the network + * containing all links required to model all allowed paths, but exclude the last link of turn restrictions' + * link sequence to enforce the "disallow" along that route. + * + * + * Implementation details: + * - The easiest solution would be to make a copy of the original network, then start modifying it + * according to the algorithm above (e.g. add and delete links and nodes), and then to convert the + * resulting network into a graph. This would require substantial amount of memory for duplicating + * the complete network, and might pose problems as we will have multiple links with the same id. + * - Given the assumption that turn restrictions apply only to a small amount of the full network, + * we keep the original network intact. Instead, we keep all modifications in separate data-structures + * so they can be used to create the routing-graph. + * - If the network is already filtered for a specific mode, it might be that links referenced + * in a turn restriction are missing. The implementation must be able to deal with such cases, + * prevent NullPointerExceptions. + * - As turn restrictions are mode-specific, the algorithm needs to know for which mode the + * turn restriction need to be considered. + */ + + TurnRestrictionsContext context = new TurnRestrictionsContext(); + + for (Link startingLink : network.getLinks().values()) { + DisallowedNextLinks disallowedNextLinks = NetworkUtils.getDisallowedNextLinks(startingLink); + if (disallowedNextLinks == null) { + continue; + } + Collection>> turnRestrictions = disallowedNextLinks.getMergedDisallowedLinkSequences(); + if (turnRestrictions == null || turnRestrictions.isEmpty()) { + continue; + } + + // steps 1 to 5: + ColoredLink coloredStartingLink = context.applyTurnRestriction(context, turnRestrictions, startingLink); + + // step 6: turn restrictions have to be applied separately to existing colored links as well. + // see if there are already colored link copies available for this starting link + List coloredLinks = context.coloredLinksPerLinkMap.get(startingLink.getId()); + if (coloredLinks != null) { + for (ColoredLink coloredLink : coloredLinks) { + // optimization: re-point toNode instead of re-applying full turn restrictions + if (coloredLink.getToColoredNode() == null) { + coloredLink.setToColoredNode(coloredStartingLink.getToColoredNode()); + coloredLink.setToNode(null); + } else { + context.applyTurnRestriction(context, turnRestrictions, coloredLink); + } + } + } + } + + return context; + } + + private ColoredLink applyTurnRestriction(TurnRestrictionsContext context, Collection>> restrictions, Link startingLink) { + return this.applyTurnRestriction(context, restrictions, startingLink, null); + } + + private void applyTurnRestriction(TurnRestrictionsContext context, Collection>> restrictions, ColoredLink startingLink) { + this.applyTurnRestriction(context, restrictions, null, startingLink); + } + + private ColoredLink applyTurnRestriction(TurnRestrictionsContext context, Collection>> restrictions, Link startingLink, ColoredLink coloredStartingLink) { + Set affectedNodes = new HashSet<>(); + Set affectedColoredNodes = new HashSet<>(); + Set affectedLinks = new HashSet<>(); + Set affectedColoredLinks = new HashSet<>(); + Set> endLinkIds = new HashSet<>(); + + // step 1 and 2: collect end-links, affected-links and affected-nodes + for (List> restriction : restrictions) { + + Link currentLink; + ColoredLink currentColoredLink; + Node currentNode = startingLink == null ? null : startingLink.getToNode(); + // due to the optimization in step 6, every colored starting link leads to a colored to-node + ColoredNode currentColoredNode = coloredStartingLink == null ? null : coloredStartingLink.getToColoredNode(); + + // walk along the restricted path, collect affectedLinks, affectedNodes and endLink + for (Id linkId : restriction) { + if (currentNode != null) { + // handle regular node + affectedNodes.add(currentNode); + currentLink = null; + currentColoredLink = null; + for (Link outLink : currentNode.getOutLinks().values()) { + if (outLink.getId() == linkId) { + currentColoredLink = context.replacedLinks.get(linkId); + if (currentColoredLink == null) { + currentLink = outLink; + } + break; + } + } + + if (currentLink != null) { + affectedLinks.add(currentLink); + currentNode = currentLink.getToNode(); + currentColoredNode = null; + } + if (currentColoredLink != null) { + affectedColoredLinks.add(currentColoredLink); + currentNode = currentColoredLink.getToNode(); + currentColoredNode = currentColoredLink.getToColoredNode(); + } + if (currentLink == null && currentColoredLink == null) { + // link of restriction is no longer part of the network, maybe we are in a sub-graph + break; + } + } else if (currentColoredNode != null) { + // handle colored node + affectedColoredNodes.add(currentColoredNode); + currentLink = null; + currentColoredLink = null; + for (ColoredLink outLink : currentColoredNode.outLinks) { + if (outLink.getLink().getId() == linkId) { + currentColoredLink = outLink; + break; + } + } + if (currentColoredLink != null) { + affectedColoredLinks.add(currentColoredLink); + currentNode = currentColoredLink.getToNode(); + currentColoredNode = currentColoredLink.getToColoredNode(); + } + if (currentColoredLink == null) { + // link of restriction is no longer part of the network, maybe we are in a sub-graph + break; + } + } + } + endLinkIds.add(restriction.get(restriction.size() - 1)); + } + + // step 3: create colored copies of nodes + Map, ColoredNode> newlyColoredNodes = new HashMap<>(); + for (Node affectedNode : affectedNodes) { + int nodeIndex = context.nodeCount; + context.nodeCount++; + ColoredNode newlyColoredNode = new ColoredNode(nodeIndex, affectedNode, new ArrayList<>()); + newlyColoredNodes.put(affectedNode.getId(), newlyColoredNode); + context.coloredNodes.add(newlyColoredNode); + } + for (ColoredNode affectedColoredNode : affectedColoredNodes) { + int nodeIndex = context.nodeCount; + context.nodeCount++; + ColoredNode newlyColoredNode = new ColoredNode(nodeIndex, affectedColoredNode.node, new ArrayList<>()); + newlyColoredNodes.put(affectedColoredNode.node.getId(), newlyColoredNode); + context.coloredNodes.add(newlyColoredNode); + } + + // step 4: create colored copies of links + for (Node affectedNode : affectedNodes) { + for (Link outLink : affectedNode.getOutLinks().values()) { + if (endLinkIds.contains(outLink.getId())) { + continue; + } + ColoredLink replacedOutLink = context.replacedLinks.get(outLink.getId()); + int linkIndex = context.linkCount; + context.linkCount++; + ColoredLink newlyColoredLink; + ColoredNode fromNode = newlyColoredNodes.get(outLink.getFromNode().getId()); + if (affectedLinks.contains(outLink) || (replacedOutLink != null && affectedColoredLinks.contains(replacedOutLink))) { + ColoredNode toNode = newlyColoredNodes.get(outLink.getToNode().getId()); + newlyColoredLink = new ColoredLink(linkIndex, outLink, fromNode, null, toNode, null); + } else { + Node toNode = outLink.getToNode(); + newlyColoredLink = new ColoredLink(linkIndex, outLink, fromNode, null, null, toNode); + } + fromNode.outLinks.add(newlyColoredLink); + context.coloredLinks.add(newlyColoredLink); + context.coloredLinksPerLinkMap.computeIfAbsent(outLink.getId(), id -> new ArrayList<>(3)).add(newlyColoredLink); + } + } + for (ColoredNode affectedNode : affectedColoredNodes) { + for (ColoredLink outLink : affectedNode.outLinks) { + if (endLinkIds.contains(outLink.getLink().getId())) { + continue; + } + int linkIndex = context.linkCount; + context.linkCount++; + ColoredLink newlyColoredLink; + ColoredNode fromNode = newlyColoredNodes.get(outLink.getLink().getFromNode().getId()); + if (affectedColoredLinks.contains(outLink)) { + ColoredNode toNode = newlyColoredNodes.get(outLink.getLink().getToNode().getId()); + newlyColoredLink = new ColoredLink(linkIndex, outLink.getLink(), fromNode, null, toNode, null); + } else { + newlyColoredLink = new ColoredLink(linkIndex, outLink.getLink(), fromNode, null, outLink.getToColoredNode(), outLink.getToNode()); + } + fromNode.outLinks.add(newlyColoredLink); + context.coloredLinks.add(newlyColoredLink); + context.coloredLinksPerLinkMap.computeIfAbsent(outLink.getLink().getId(), id -> new ArrayList<>(3)).add(newlyColoredLink); + } + } + + // step 5: replace starting link + if (startingLink != null) { + ColoredNode toNode = newlyColoredNodes.get(startingLink.getToNode().getId()); + int linkIndex = startingLink.getId().index(); // re-use the index + ColoredLink newlyColoredStartingLink = new ColoredLink(linkIndex, startingLink, null, startingLink.getFromNode(), toNode, null); + context.coloredLinks.add(newlyColoredStartingLink); + context.replacedLinks.put(startingLink.getId(), newlyColoredStartingLink); + + return newlyColoredStartingLink; + } + if (coloredStartingLink != null) { + // don't really replace the colored started link, but re-point it to the newly colored node + coloredStartingLink.setToColoredNode(newlyColoredNodes.get(coloredStartingLink.getLink().getToNode().getId())); + return null; + + } + throw new IllegalArgumentException("either startingLink or coloredStartingLink must be set"); + } + + public record ColoredNode( + int index, + Node node, + List outLinks + ) {} + + public int getNodeCount() { + return nodeCount; + } + + public int getLinkCount() { + return linkCount; + } + + public Map, ColoredLink> getReplacedLinks() { + return Collections.unmodifiableMap(replacedLinks); + } + + public List getColoredNodes() { + return Collections.unmodifiableList(coloredNodes); + } + + public List getColoredLinks() { + return Collections.unmodifiableList(coloredLinks); + } + + public Map, List> getColoredLinksPerLinkMap() { + return Collections.unmodifiableMap(coloredLinksPerLinkMap); + } +}