Merge pull request matsim-org#3726 from moia-oss/turnRestrictionsLeastCostPathTree

nkuehnel · web-flow · commit c0a6c24f0016 · 2025-02-13T15:06:50.000+01:00
Use path iterators for constructing paths outside of least cost path tree
diff --git a/contribs/dvrp/src/main/java/org/matsim/contrib/dvrp/path/OneToManyPathCalculator.java b/contribs/dvrp/src/main/java/org/matsim/contrib/dvrp/path/OneToManyPathCalculator.java
@@ -67,8 +67,6 @@ class OneToManyPathCalculator {
 		this.forwardSearch = forwardSearch;
 		this.fromLink = fromLink;
 		this.startTime = startTime;
-
-		verifyParallelLinks();
 	}
 
 	void calculateDijkstraTree(Collection<Link> toLinks) {
@@ -127,7 +125,7 @@ Path createPath(Node toNode) {
 			return null;
 		}
 		var nodes = constructNodeSequence(dijkstraTree, toNode, forwardSearch);
-		var links = constructLinkSequence(nodes);
+		var links = constructLinkSequence(dijkstraTree, toNode, forwardSearch);
 		double cost = dijkstraTree.getCost(toNodeIndex);
 		return new Path(nodes, links, travelTime, cost);
 	}
@@ -145,32 +143,23 @@ private List<Node> constructNodeSequence(LeastCostPathTree dijkstraTree, Node to
 		ArrayList<Node> nodes = new ArrayList<>();
 		nodes.add(toNode);
 
-		LeastCostPathTree.PathIterator pathIterator = dijkstraTree.getComingFromIterator(toNode);
-		while (pathIterator.hasNext()) {
-			nodes.add(pathIterator.next());
-		}
+		LeastCostPathTree.PathIterator pathIterator = dijkstraTree.getNodePathIterator(toNode);
+		pathIterator.forEachRemaining(nodes::add);
 
 		if (forward) {
 			Collections.reverse(nodes);
 		}
 		return nodes;
 	}
 
-	private List<Link> constructLinkSequence(List<Node> nodes) {
-		List<Link> links = new ArrayList<>(nodes.size() - 1);
-		Node prevNode = nodes.get(0);
-		for (int i = 1; i < nodes.size(); i++) {
-			Node nextNode = nodes.get(i);
-			for (Link link : prevNode.getOutLinks().values()) {
-				//FIXME this method will not work properly if there are many prevNode -> nextNode links
-				//TODO save link idx in tree OR pre-check: at most 1 arc per each node pair OR choose faster/better link
-				// sh, 26/07/2023, added a check further below to increase awareness
-				if (link.getToNode() == nextNode) {
-					links.add(link);
-					break;
-				}
-			}
-			prevNode = nextNode;
+	private List<Link> constructLinkSequence(LeastCostPathTree dijkstraTree, Node toNode, boolean forward) {
+		ArrayList<Link> links = new ArrayList<>();
+
+		LeastCostPathTree.LinkPathIterator pathIterator = dijkstraTree.getLinkPathIterator(toNode);
+		pathIterator.forEachRemaining(links::add);
+
+		if (forward) {
+			Collections.reverse(links);
 		}
 		return links;
 	}
@@ -189,31 +178,4 @@ private double getFirstAndLastLinkTT(Link fromLink, Link toLink, double pathTrav
 				VrpPaths.getLastLinkTT(travelTime, fromLink, time);
 		return FIRST_LINK_TT + lastLinkTT;
 	}
-
-	private final static Logger logger = LogManager.getLogger(OneToManyPathCalculator.class);
-	private static int parallelLinksWarningCount = 0;
-
-	private void verifyParallelLinks() {
-		if (parallelLinksWarningCount < 20) {
-			for (Node prevNode : nodeMap.values()) {
-				Set<Integer> candidates = new HashSet<>();
-
-				for (Link link : prevNode.getOutLinks().values()) {
-					if (!candidates.add(link.getToNode().getId().index())) {
-						logger.warn(
-								"Found parallel links between nodes {} and {}. This may lead to problems in path calculation.",
-								prevNode.getId().toString(), link.getToNode().getId().toString());
-
-						if (parallelLinksWarningCount > 20) {
-							logger.warn("Consider using NetworkSegmentDoubleLinks.run on your network");
-							logger.warn("Only showing 20 of these warnings ...");
-							return;
-						}
-
-						parallelLinksWarningCount++;
-					}
-				}
-			}
-		}
-	}
 }
diff --git a/matsim/src/main/java/org/matsim/core/router/speedy/LeastCostPathTree.java b/matsim/src/main/java/org/matsim/core/router/speedy/LeastCostPathTree.java
@@ -23,6 +23,7 @@
  * implementation NOT thread-safe.
  *
  * @author mrieser / Simunto, sponsored by SBB Swiss Federal Railways
+ * @author hrewald, nkuehnel / MOIA turn restriction adjustments
  */
 public class LeastCostPathTree {
 
@@ -31,6 +32,8 @@ public class LeastCostPathTree {
     private final TravelDisutility td;
     private final double[] data; // 3 entries per node: time, cost, distance
     private final int[] comingFrom;
+    private final int[] fromLink;
+    private final int[] comingFromLink;
     private final SpeedyGraph.LinkIterator outLI;
     private final SpeedyGraph.LinkIterator inLI;
     private final NodeMinHeap pq;
@@ -41,6 +44,8 @@ public LeastCostPathTree(SpeedyGraph graph, TravelTime tt, TravelDisutility td)
         this.td = td;
         this.data = new double[graph.nodeCount * 3];
         this.comingFrom = new int[graph.nodeCount];
+        this.fromLink = new int[graph.nodeCount];
+        this.comingFromLink = new int[graph.linkCount];
         this.pq = new NodeMinHeap(graph.nodeCount, this::getCost, this::setCost);
         this.outLI = graph.getOutLinkIterator();
         this.inLI = graph.getInLinkIterator();
@@ -53,6 +58,7 @@ public void calculate(int startNode, double startTime, Person person, Vehicle ve
     public void calculate(int startNode, double startTime, Person person, Vehicle vehicle, StopCriterion stopCriterion) {
         Arrays.fill(this.data, Double.POSITIVE_INFINITY);
         Arrays.fill(this.comingFrom, -1);
+        Arrays.fill(this.fromLink, -1);
 
         setData(startNode, 0, startTime, 0);
 
@@ -86,18 +92,32 @@ public void calculate(int startNode, double startTime, Person person, Vehicle ve
                         this.pq.decreaseKey(toNode, newCost);
                         setData(toNode, newCost, newTime, currDistance + link.getLength());
                         this.comingFrom[toNode] = nodeIdx;
+                        this.fromLink[toNode] = linkIdx;
                     }
                 } else {
                     setData(toNode, newCost, newTime, currDistance + link.getLength());
                     this.pq.insert(toNode);
                     this.comingFrom[toNode] = nodeIdx;
+                    this.fromLink[toNode] = linkIdx;
                 }
             }
         }
 
         if (graph.hasTurnRestrictions()) {
             consolidateColoredNodes();
         }
+
+        Arrays.fill(this.comingFromLink, -1);
+        for (int i = 0; i < graph.nodeCount; i++) {
+            Node node = graph.getNode(i);
+            if(node != null) {
+                this.outLI.reset(i);
+                while (this.outLI.next()) {
+                    int previousLinkIdx = fromLink[i];
+                    this.comingFromLink[outLI.getLinkIndex()] = previousLinkIdx;
+                }
+            }
+        }
     }
 
     public void calculateBackwards(int arrivalNode, double arrivalTime, Person person, Vehicle vehicle) {
@@ -107,6 +127,7 @@ public void calculateBackwards(int arrivalNode, double arrivalTime, Person perso
     public void calculateBackwards(int arrivalNode, double arrivalTime, Person person, Vehicle vehicle, StopCriterion stopCriterion) {
         Arrays.fill(this.data, Double.POSITIVE_INFINITY);
         Arrays.fill(this.comingFrom, -1);
+        Arrays.fill(this.fromLink, -1);
 
         setData(arrivalNode, 0, arrivalTime, 0);
 
@@ -140,18 +161,32 @@ public void calculateBackwards(int arrivalNode, double arrivalTime, Person perso
                         this.pq.decreaseKey(fromNode, newCost);
                         setData(fromNode, newCost, newTime, currDistance + link.getLength());
                         this.comingFrom[fromNode] = nodeIdx;
+                        this.fromLink[fromNode] = linkIdx;
                     }
                 } else {
                     setData(fromNode, newCost, newTime, currDistance + link.getLength());
                     this.pq.insert(fromNode);
                     this.comingFrom[fromNode] = nodeIdx;
+                    this.fromLink[fromNode] = linkIdx;
                 }
             }
         }
 
         if (graph.hasTurnRestrictions()) {
             consolidateColoredNodes();
         }
+
+        Arrays.fill(this.comingFromLink, -1);
+        for (int i = 0; i < graph.nodeCount; i++) {
+            Node node = graph.getNode(i);
+            if(node != null) {
+                this.inLI.reset(i);
+                while (this.inLI.next()) {
+                    int previousLinkIdx = fromLink[i];
+                    this.comingFromLink[inLI.getLinkIndex()] = previousLinkIdx;
+                }
+            }
+        }
     }
 
     private void consolidateColoredNodes() {
@@ -169,6 +204,7 @@ private void consolidateColoredNodes() {
                     if (coloredCost < uncoloredCost) {
                         setData(uncoloredIndex, coloredCost, getTimeRaw(i), getDistance(i));
                         this.comingFrom[uncoloredIndex] = this.comingFrom[i];
+                        this.fromLink[uncoloredIndex] = this.fromLink[i];
                     }
                 }
             }
@@ -206,10 +242,14 @@ private void setData(int nodeIndex, double cost, double time, double distance) {
         this.data[index + 2] = distance;
     }
 
-    public PathIterator getComingFromIterator(Node node) {
+    public PathIterator getNodePathIterator(Node node) {
         return new PathIterator(node);
     }
 
+    public LinkPathIterator getLinkPathIterator(Node node) {
+        return new LinkPathIterator(node);
+    }
+
     public interface StopCriterion {
 
         boolean stop(int nodeIndex, double arrivalTime, double travelCost, double distance, double departureTime);
@@ -253,7 +293,7 @@ public PathIterator(Node startNode) {
         }
 
         @Override
-        public Node next() throws NoSuchElementException {
+        public Node next() {
             current = comingFrom[current];
             if (current < 0) {
                 throw new NoSuchElementException();
@@ -266,4 +306,34 @@ public boolean hasNext() {
             return comingFrom[current] >= 0;
         }
     }
+
+    // by not exposing internal indices to the outside we ensure that only uncolored nodes are returned. nkuehnel Feb'25
+    public final class LinkPathIterator implements Iterator<Link> {
+
+        private boolean firstStep = true;
+
+        private int current;
+
+        public LinkPathIterator(Node startNode) {
+            current = fromLink[startNode.getId().index()];
+        }
+
+        @Override
+        public Link next() {
+            if(firstStep) {
+                firstStep = false;
+                return graph.getLink(current);
+            }
+            current = comingFromLink[current];
+            if (current < 0) {
+                throw new NoSuchElementException();
+            }
+            return graph.getLink(current);
+        }
+
+        @Override
+        public boolean hasNext() {
+            return current >= 0 && (comingFromLink[current] >= 0 || firstStep);
+        }
+    }
 }
