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); + } + } + +}