Merge pull request #3670 from moia-oss/speedyLeastCostPathtree

Update speedy routers and tree for turn restrictions
matsim-org · Jan 20, 2025 · 28cc45b · 28cc45b
2 parents 1159628 + 81cf2d3
commit 28cc45b
Show file tree

Hide file tree

Showing 3 changed files with 39 additions and 2 deletions.
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
@@ -1,6 +1,8 @@
 package org.matsim.core.router.speedy;
 
+import com.google.common.base.Preconditions;
 import org.matsim.api.core.v01.network.Link;
+import org.matsim.api.core.v01.network.Node;
 import org.matsim.api.core.v01.population.Person;
 import org.matsim.core.router.util.TravelDisutility;
 import org.matsim.core.router.util.TravelTime;
@@ -9,8 +11,6 @@
 
 import java.util.Arrays;
 
-import com.google.common.base.Preconditions;
-
 /**
  * Implements a least-cost-path-tree upon a {@link SpeedyGraph} datastructure. Besides using the more efficient Graph datastructure, it also makes use of a custom priority-queue implementation (NodeMinHeap)
  * which operates directly on the least-cost-path-three data for additional performance gains.
@@ -92,6 +92,10 @@ public void calculate(int startNode, double startTime, Person person, Vehicle ve
                 }
             }
         }
+
+        if(graph.hasTurnRestrictions()) {
+            consolidateColoredNodes();
+        }
     }
 
     public void calculateBackwards(int arrivalNode, double arrivalTime, Person person, Vehicle vehicle) {
@@ -142,6 +146,37 @@ public void calculateBackwards(int arrivalNode, double arrivalTime, Person perso
                 }
             }
         }
+
+        if(graph.hasTurnRestrictions()) {
+            consolidateColoredNodes();
+        }
+    }
+
+    private void consolidateColoredNodes() {
+        // update node values with the minimum of their colored copies, if any
+        for (int i = 0; i < data.length / 3; i++) {
+            Node uncoloredNode = graph.getNode(i);
+            if (uncoloredNode != null) {
+
+                // the index points to a node with a different index -> colored copy
+                if (uncoloredNode.getId().index() != i) {
+                    int uncoloredIndex = uncoloredNode.getId().index();
+                    double uncoloredCost = getCost(uncoloredIndex);
+
+                    double coloredTime = getTimeRaw(i);
+                    double coloredDistance = getDistance(i);
+                    double coloredCost = getCost(i);
+
+                    if (Double.isFinite(uncoloredCost)) {
+                        if (coloredCost < uncoloredCost) {
+                            setData(uncoloredIndex, coloredCost, coloredTime, coloredDistance);
+                        }
+                    } else {
+                        setData(uncoloredIndex, coloredCost, coloredTime, coloredDistance);
+                    }
+                }
+            }
+        }
     }
 
     public double getCost(int nodeIndex) {

diff --git a/matsim/src/main/java/org/matsim/core/router/speedy/SpeedyALT.java b/matsim/src/main/java/org/matsim/core/router/speedy/SpeedyALT.java
@@ -115,6 +115,7 @@ public Path calcLeastCostPath(Node startNode, Node endNode, double startTime, Pe
 			// if turn restrictions are used, we might be on a colored node, so check for the original node
 			if (hasTurnRestrictions && this.graph.getNode(nodeIdx).getId().index() == endNodeIndex) {
 				foundEndNode = true;
+				endNodeIndex = nodeIdx;
 				break;
 			}
 

diff --git a/matsim/src/main/java/org/matsim/core/router/speedy/SpeedyDijkstra.java b/matsim/src/main/java/org/matsim/core/router/speedy/SpeedyDijkstra.java
@@ -92,6 +92,7 @@ public Path calcLeastCostPath(Node startNode, Node endNode, double startTime, Pe
 			// if turn restrictions are used, we might be on a colored node, so check for the original node
 			if (hasTurnRestrictions && this.graph.getNode(nodeIdx).getId().index() == endNodeIndex) {
 				foundEndNode = true;
+				endNodeIndex = nodeIdx;
 				break;
 			}