From 54454451c15c970c9cfc302f44eb58627b13f487 Mon Sep 17 00:00:00 2001 From: Marcel Rieser Date: Fri, 29 Dec 2023 14:45:53 +0100 Subject: [PATCH] support turn restrictions (disallowedNextLinks) in SpeedyGraphBuilder Turn restrictions are mode specific, but the routing graph isn't. Thus one has to specify for which mode turn restrictions should be taken into account. --- .../router/speedy/SpeedyGraphBuilder.java | 366 +++++++++++++++++- .../router/speedy/SpeedyGraphBuilderTest.java | 351 +++++++++++++++++ 2 files changed, 705 insertions(+), 12 deletions(-) create mode 100644 matsim/src/test/java/org/matsim/core/router/speedy/SpeedyGraphBuilderTest.java 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 56af0e8a5c1..89aa6a863f0 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,10 +4,22 @@ 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.ArrayList; import java.util.Arrays; +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}. + * + * @author mrieser / Simunto + */ public class SpeedyGraphBuilder { private int nodeCount; @@ -17,34 +29,284 @@ public class SpeedyGraphBuilder { private Link[] links; private Node[] nodes; + @Deprecated // use build-method with additional mode argument public static SpeedyGraph build(Network network) { + return build(network, "nothing"); + } + + public static SpeedyGraph build(Network network, String mode) { if (hasTurnRestrictions(network)) { - return new SpeedyGraphBuilder().buildWithTurnRestrictions(network); + return new SpeedyGraphBuilder().buildWithTurnRestrictions(network, mode); } return new SpeedyGraphBuilder().buildWithoutTurnRestrictions(network); } private static boolean hasTurnRestrictions(Network network) { -// for (Link link : network.getLinks().values()) { -// if (NetworkUtils.getDisallowedNextLinks() != null) { -// return true; -// } -// } + for (Link link : network.getLinks().values()) { + if (NetworkUtils.getDisallowedNextLinks(link) != null) { + return true; + } + } return false; } - private SpeedyGraph buildWithTurnRestrictions(Network network) { - throw new UnsupportedOperationException(); + private SpeedyGraph buildWithTurnRestrictions(Network network, String mode) { + /* + * 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; + } + List>> turnRestrictions = disallowedNextLinks.getDisallowedLinkSequences(mode); + 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); + } + } + } + } + + // create routing graph from context + this.nodeCount = context.nodeCount; + this.linkCount = context.linkCount; + + this.nodeData = new int[this.nodeCount * SpeedyGraph.NODE_SIZE]; + this.linkData = new int[this.linkCount * SpeedyGraph.LINK_SIZE]; + this.links = new Link[this.linkCount]; + this.nodes = new Node[this.nodeCount]; + + Arrays.fill(this.nodeData, -1); + Arrays.fill(this.linkData, -1); + + for (Node node : network.getNodes().values()) { + this.nodes[node.getId().index()] = node; + } + for (Link link : network.getLinks().values()) { + if (context.replacedLinks.get(link.getId()) == null) { + addLink(link); + } + } + for (ColoredNode node : context.coloredNodes) { + this.nodes[node.index] = node.node; + } + for (ColoredLink link : context.coloredLinks) { + addLink(link); + } + + return new SpeedyGraph(this.nodeData, this.linkData, this.nodes, this.links); + } + + private ColoredLink applyTurnRestriction(TurnRestrictionsContext context, List>> restrictions, Link startingLink) { + return this.applyTurnRestriction(context, restrictions, startingLink, null); + } + + private void applyTurnRestriction(TurnRestrictionsContext context, List>> restrictions, ColoredLink startingLink) { + this.applyTurnRestriction(context, restrictions, null, startingLink); + } + + private ColoredLink applyTurnRestriction(TurnRestrictionsContext context, List>> 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); this.linkCount = Id.getNumberOfIds(Link.class); - this.nodeData = new int[nodeCount * SpeedyGraph.NODE_SIZE]; - this.linkData = new int[linkCount * SpeedyGraph.LINK_SIZE]; - this.links = new Link[linkCount]; - this.nodes = new Node[nodeCount]; + this.nodeData = new int[this.nodeCount * SpeedyGraph.NODE_SIZE]; + this.linkData = new int[this.linkCount * SpeedyGraph.LINK_SIZE]; + this.links = new Link[this.linkCount]; + this.nodes = new Node[this.nodeCount]; Arrays.fill(this.nodeData, -1); Arrays.fill(this.linkData, -1); @@ -76,6 +338,36 @@ private void addLink(Link link) { this.links[linkIdx] = link; } + private void addLink(ColoredLink link) { + int fromNodeIdx = -1; + int toNodeIdx = -1; + int linkIdx = link.index; + + if (link.fromColoredNode != null) { + fromNodeIdx = link.fromColoredNode.index; + } + if (link.fromNode != null) { + fromNodeIdx = link.fromNode.getId().index(); + } + if (link.toColoredNode != null) { + toNodeIdx = link.toColoredNode.index; + } + if (link.toNode != null) { + toNodeIdx = link.toNode.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); + + setOutLink(fromNodeIdx, linkIdx); + setInLink(toNodeIdx, linkIdx); + + this.links[linkIdx] = link.link; + } + private void setOutLink(int fromNodeIdx, int linkIdx) { final int nodeI = fromNodeIdx * SpeedyGraph.NODE_SIZE; int outLinkIdx = this.nodeData[nodeI]; @@ -105,4 +397,54 @@ private void setInLink(int toNodeIdx, int linkIdx) { } while (inLinkIdx >= 0); 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/test/java/org/matsim/core/router/speedy/SpeedyGraphBuilderTest.java b/matsim/src/test/java/org/matsim/core/router/speedy/SpeedyGraphBuilderTest.java new file mode 100644 index 00000000000..ff50d9c5433 --- /dev/null +++ b/matsim/src/test/java/org/matsim/core/router/speedy/SpeedyGraphBuilderTest.java @@ -0,0 +1,351 @@ +package org.matsim.core.router.speedy; + +import org.apache.logging.log4j.LogManager; +import org.apache.logging.log4j.Logger; +import org.junit.jupiter.api.Assertions; +import org.junit.jupiter.api.Test; +import org.matsim.api.core.v01.Coord; +import org.matsim.api.core.v01.Id; +import org.matsim.api.core.v01.TransportMode; +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.config.groups.ScoringConfigGroup; +import org.matsim.core.network.DisallowedNextLinks; +import org.matsim.core.network.NetworkUtils; +import org.matsim.core.router.costcalculators.FreespeedTravelTimeAndDisutility; +import org.matsim.core.router.util.LeastCostPathCalculator; + +import java.util.Arrays; +import java.util.List; +import java.util.Set; +import java.util.stream.Collectors; + +/** + * @author mrieser / Simunto + */ +class SpeedyGraphBuilderTest { + + private static final Logger LOG = LogManager.getLogger(SpeedyGraphBuilderTest.class); + + @Test + public void testSingleDisallowedRightTurn() { + var f = new Fixture(); + // make some other links more expensive to prevent routing along them + f.overwriteLinkLength("jf", 10000); + f.overwriteLinkLength("ji", 10000); + + var graph = runTest(f.network, "J", "G", "jk", "kg"); + Assertions.assertEquals(16, graph.nodeCount); + Assertions.assertEquals(48, graph.linkCount); + + f.addTurnRestriction("jk", "kg"); + graph = runTest(f.network, "J", "G", "jk", "kl", "lh", "hg"); + Assertions.assertEquals(16 + 1, graph.nodeCount, "only node K should be duplicated: K'"); + Assertions.assertEquals(48 + 3, graph.linkCount, "expected the following new links: k'j, k'o, k'l"); + } + + @Test + public void testMultipleRestrictionsOnSameLink() { + var f = new Fixture(); + // make some other links more expensive to prevent routing along them + f.overwriteLinkLength("jf", 10000); + f.overwriteLinkLength("ji", 10000); + + var graph = runTest(f.network, "J", "G", "jk", "kg"); + Assertions.assertEquals(16, graph.nodeCount); + Assertions.assertEquals(48, graph.linkCount); + + // prevent u-turns by higher costs on some of those links + f.overwriteLinkLength("lk", 10000); + f.overwriteLinkLength("ko", 600); + + f.addTurnRestriction("jk", "kg"); + f.addTurnRestriction("jk", "kl", "lh"); + graph = runTest(f.network, "J", "G", "jn", "no", "ok", "kg"); + Assertions.assertEquals(16 + 2, graph.nodeCount, "expected 2 duplicated nodes: K', L'"); + Assertions.assertEquals(48 + 5, graph.linkCount, "expected the following duplicated links: k'j, k'o, k'l', l'p, l'k"); + } + + @Test + public void testMultipleRestrictionsOnMultipleLink() { + // corresponds to figure 5 in the documentation + // the "green" turn restriction starting on link kg will be applied first + var f = new Fixture(); + // make some other links more expensive to prevent routing along them + f.overwriteLinkLength("jf", 10000); + f.overwriteLinkLength("ie", 10000); + f.overwriteLinkLength("cg", 600); + f.overwriteLinkLength("cd", 600); + + var graph = runTest(f.network, "J", "B", "jk", "kg", "gc", "cb"); + Assertions.assertEquals(16, graph.nodeCount); + Assertions.assertEquals(48, graph.linkCount); + + f.addTurnRestriction("jk", "kg", "gf"); + f.addTurnRestriction("jk", "kl", "lp"); + + f.addTurnRestriction("kg", "gc", "cb"); + f.addTurnRestriction("kg", "gh"); + +// SpeedyGraphBuilder.build(f.network, TransportMode.car).printDebug(); + verifyGraph(f.network, "J", new String[][] { + new String[] {"jk", "kg", "gf"}, + new String[] {"jk", "kl", "lp"}, + new String[] {"kg", "gc", "cb"}, + new String[] {"kg", "gh"}, + new String[] {"jk", "kg", "gh"}, + new String[] {"jk", "kg", "gc", "cb"}, + }); + + graph = runTest(f.network, "J", "B", "jk", "kl", "lh", "hd", "dc", "cb"); + Assertions.assertEquals(16 + 5, graph.nodeCount, "expected duplicated nodes: G', C', K', L', G''"); + Assertions.assertEquals(48 + 13, graph.linkCount, "expected duplicated links: g'k, g'f, g'c', c'g, c'd, k'j, k'o, k'g'', g''k, g''c', k'l', l'k, l'h"); + } + + @Test + public void testMultipleRestrictionsOnMultipleLink_rotated() { + // corresponds to figure 5 in the documentation, but rotate by 90deg clockwise + // the "red" turn restriction starting on link fj (instead of jk) will be applied first + var f = new Fixture(); + // make some other links more expensive to prevent routing along them + f.overwriteLinkLength("fg", 10000); + f.overwriteLinkLength("bc", 10000); + f.overwriteLinkLength("lk", 600); + f.overwriteLinkLength("lp", 600); + f.overwriteLinkLength("gh", 550); + f.overwriteLinkLength("fe", 550); + f.overwriteLinkLength("ij", 600); + f.overwriteLinkLength("kj", 600); + f.overwriteLinkLength("ok", 600); + f.overwriteLinkLength("nj", 600); + + var graph = runTest(f.network, "F", "H", "fj", "jk", "kl", "lh"); + Assertions.assertEquals(16, graph.nodeCount); + Assertions.assertEquals(48, graph.linkCount); + + f.addTurnRestriction("fj", "jk", "kg"); + f.addTurnRestriction("fj", "jn", "nm"); + + f.addTurnRestriction("jk", "kl", "lh"); + f.addTurnRestriction("jk", "ko"); + + verifyGraph(f.network, "F", new String[][] { + new String[] {"fj", "jk", "kg"}, + new String[] {"fj", "jn", "nm"}, + new String[] {"jk", "kl", "lh"}, + new String[] {"jk", "ko"}, + new String[] {"fj", "jk", "ko"}, + new String[] {"fj", "jk", "kl", "lh"}, + }); + + graph = runTest(f.network, "F", "H", "fj", "jn", "no", "op", "pl", "lh"); + Assertions.assertEquals(16 + 7, graph.nodeCount, "expected duplicated nodes: J', N', K', K'', L', K''', L''"); + Assertions.assertEquals(48 + 18, graph.linkCount, "expected duplicated links: j'i, j'f, j'n', n'j, n'o, k'j, k'o, k'l - k''j, k''g, k''l', l'k, l'p, - jk''', k'''j, k'''l'', l''k, l''p"); + } + + @Test + public void testShortRestrictionsForReuse() { + // corresponds to figure 8 in the documentation + // the "red" turn restriction starting on link kg will be applied first + var f = new Fixture(); + // make some other links more expensive to prevent routing along them + f.overwriteLinkLength("jf", 10000); + f.overwriteLinkLength("ie", 10000); + // make link kg same as link lh + f.overwriteLinkLength("gh", 600); + f.overwriteLinkLength("cd", 600); + f.overwriteLinkLength("nj", 10000); + f.overwriteLinkLength("lp", 600); + // make link no cheaper than link jk + f.overwriteLinkLength("no", 500); + + var graph = runTest(f.network, "J", "D", "jk", "kl", "lh", "hd"); + Assertions.assertEquals(16, graph.nodeCount); + Assertions.assertEquals(48, graph.linkCount); + + f.addTurnRestriction("jk", "kg"); + + f.addTurnRestriction("kl", "lh"); + +// SpeedyGraphBuilder.build(f.network, TransportMode.car).printDebug(); + verifyGraph(f.network, "J", new String[][] { + new String[] {"jk", "kg"}, + new String[] {"kl", "lh"}, + new String[] {"jk", "kl", "lh"}, + }); + + graph = runTest(f.network, "J", "D", "jn", "no", "op", "pl", "lh", "hd"); + Assertions.assertEquals(16 + 2, graph.nodeCount, "expected duplicated nodes: K', L'"); + Assertions.assertEquals(48 + 5, graph.linkCount, "expected duplicated links: k'j, k'o, k'l', l'k, l'p"); + } + + /** Checks that none of the provided paths exist in the graph based on the provided network */ + private static void verifyGraph(Network network, String fromNode, String[][] forbiddenPaths) { + SpeedyGraph graph = SpeedyGraphBuilder.build(network, TransportMode.car); + Node realFromNode = network.getNodes().get(Id.create(fromNode, Node.class)); + + for (String[] path : forbiddenPaths) { + if (findPath(graph, realFromNode.getId().index(), path)) { + Assertions.fail("Found path that should not exist in graph"); + } + } + + SpeedyGraph.LinkIterator outLinkIterator = graph.getOutLinkIterator(); + outLinkIterator.reset(realFromNode.getId().index()); + } + + private static boolean findPath(SpeedyGraph graph, int nodeIndex, String[] path) { + Id nextLinkId = Id.create(path[0], Link.class); + SpeedyGraph.LinkIterator outLinkIterator = graph.getOutLinkIterator(); + outLinkIterator.reset(nodeIndex); + while (outLinkIterator.next()) { + int linkIndex = outLinkIterator.getLinkIndex(); + Link link = graph.getLink(linkIndex); + if (link.getId() == nextLinkId) { + if (path.length == 1) { + return true; + } + return findPath(graph, outLinkIterator.getToNodeIndex(), Arrays.copyOfRange(path, 1, path.length)); + } + } + return false; + } + + private static SpeedyGraph runTest(Network network, String fromNode, String toNode, String... expectedPath) { + SpeedyGraph graph = SpeedyGraphBuilder.build(network, TransportMode.car); + FreespeedTravelTimeAndDisutility freespeed = new FreespeedTravelTimeAndDisutility(new ScoringConfigGroup()); + SpeedyDijkstra dijkstra = new SpeedyDijkstra(graph, freespeed, freespeed); + + Node realFromNode = network.getNodes().get(Id.create(fromNode, Node.class)); + Node realToNode = network.getNodes().get(Id.create(toNode, Node.class)); + LeastCostPathCalculator.Path path = dijkstra.calcLeastCostPath(realFromNode, realToNode, 7*3600, null, null); + + assertPath(path, expectedPath); + return graph; + } + + + private static void assertPath(LeastCostPathCalculator.Path path, String... linkIds) { + var links = path.links; + LOG.info("expected path: " + Arrays.toString(linkIds) + "\nactual path: " + path.links.stream().map(link -> link.getId().toString()).collect(Collectors.joining(","))); + Assertions.assertEquals(linkIds.length, links.size()); + for (int i = 0; i < linkIds.length; i++) { + Assertions.assertEquals(linkIds[i], links.get(i).getId().toString()); + } + } + + private static class Fixture { + private final Network network; + + public Fixture() { + this.network = NetworkUtils.createNetwork(); + + Node a = createAndAddNode("A", 1000, 2500); + Node b = createAndAddNode("B", 1500, 2500); + Node c = createAndAddNode("C", 2000, 2500); + Node d = createAndAddNode("D", 2500, 2500); + Node e = createAndAddNode("E", 1000, 2000); + Node f = createAndAddNode("F", 1500, 2000); + Node g = createAndAddNode("G", 2000, 2000); + Node h = createAndAddNode("H", 2500, 2000); + Node i = createAndAddNode("I", 1000, 1500); + Node j = createAndAddNode("J", 1500, 1500); + Node k = createAndAddNode("K", 2000, 1500); + Node l = createAndAddNode("L", 2500, 1500); + Node m = createAndAddNode("M", 1000, 1000); + Node n = createAndAddNode("N", 1500, 1000); + Node o = createAndAddNode("O", 2000, 1000); + Node p = createAndAddNode("P", 2500, 1000); + + createAndAddLink("ab", a, b, 500); + createAndAddLink("ba", b, a, 501); + createAndAddLink("bc", b, c, 502); + createAndAddLink("cb", c, b, 503); + createAndAddLink("cd", c, d, 504); + createAndAddLink("dc", d, c, 505); + + createAndAddLink("ae", a, e, 506); + createAndAddLink("ea", e, a, 507); + createAndAddLink("bf", b, f, 508); + createAndAddLink("fb", f, b, 509); + createAndAddLink("cg", c, g, 510); + createAndAddLink("gc", g, c, 511); + createAndAddLink("dh", d, h, 512); + createAndAddLink("hd", h, d, 513); + + createAndAddLink("ef", e, f, 514); + createAndAddLink("fe", f, e, 515); + createAndAddLink("fg", f, g, 516); + createAndAddLink("gf", g, f, 517); + createAndAddLink("gh", g, h, 518); + createAndAddLink("hg", h, g, 519); + + createAndAddLink("ei", e, i, 520); + createAndAddLink("ie", i, e, 521); + createAndAddLink("fj", f, j, 522); + createAndAddLink("jf", j, f, 523); + createAndAddLink("gk", g, k, 524); + createAndAddLink("kg", k, g, 525); + createAndAddLink("hl", h, l, 526); + createAndAddLink("lh", l, h, 527); + + createAndAddLink("ij", i, j, 528); + createAndAddLink("ji", j, i, 529); + createAndAddLink("jk", j, k, 530); + createAndAddLink("kj", k, j, 531); + createAndAddLink("kl", k, l, 532); + createAndAddLink("lk", l, k, 533); + + createAndAddLink("im", i, m, 534); + createAndAddLink("mi", m, i, 535); + createAndAddLink("jn", j, n, 536); + createAndAddLink("nj", n, j, 537); + createAndAddLink("ko", k, o, 538); + createAndAddLink("ok", o, k, 539); + createAndAddLink("lp", l, p, 540); + createAndAddLink("pl", p, l, 541); + + createAndAddLink("mn", m, n, 542); + createAndAddLink("nm", n, m, 543); + createAndAddLink("no", n, o, 544); + createAndAddLink("on", o, n, 545); + createAndAddLink("op", o, p, 546); + createAndAddLink("po", p, o, 547); + } + + private Node createAndAddNode(String id, double x, double y) { + Node node = this.network.getFactory().createNode(Id.create(id, Node.class), new Coord(x, y)); + this.network.addNode(node); + return node; + } + + private void createAndAddLink(String id, Node fromNode, Node toNode, double length) { + Link link = this.network.getFactory().createLink(Id.create(id, Link.class), fromNode, toNode); + link.setLength(length); + link.setFreespeed(20.0); + link.setCapacity(2000); + link.setNumberOfLanes(1); + link.setAllowedModes(Set.of(TransportMode.car)); + this.network.addLink(link); + } + + private void addTurnRestriction(String linkId, String... disallowedLinks) { + Id realLinkId = Id.create(linkId, Link.class); + Link link = this.network.getLinks().get(realLinkId); + var restrictions = NetworkUtils.getDisallowedNextLinks(link); + if (restrictions == null) { + restrictions = new DisallowedNextLinks(); + NetworkUtils.setDisallowedNextLinks(link, restrictions); + } + + List> disallowedLinkIds = Arrays.stream(disallowedLinks).map(id -> Id.create(id, Link.class)).toList(); + restrictions.addDisallowedLinkSequence(TransportMode.car, disallowedLinkIds); + } + + private void overwriteLinkLength(String linkId, double length) { + this.network.getLinks().get(Id.create(linkId, Link.class)).setLength(length); + } + } + +}