isochrone: fix wrong termination condition

Author: michaz

core/src/main/java/com/graphhopper/isochrone/algorithm/ShortestPathTree.java

@@ -27,6 +27,7 @@
 import com.graphhopper.storage.Graph;
 import com.graphhopper.util.EdgeIterator;
 import com.graphhopper.util.GHUtility;
+import org.jetbrains.annotations.NotNull;
 
 import java.util.ArrayList;
 import java.util.Collection;
@@ -35,6 +36,7 @@
 
 import static com.graphhopper.isochrone.algorithm.ShortestPathTree.ExploreType.*;
 import static java.util.Comparator.comparingDouble;
+import static java.util.Comparator.comparingLong;
 
 /**
  * Computes a shortest path tree by a given weighting. Terminates when all shortest paths up to
@@ -85,7 +87,8 @@ public String toString() {
     }
 
     private final IntObjectHashMap<IsoLabel> fromMap;
-    private final PriorityQueue<IsoLabel> queueByWeighting;
+    private final PriorityQueue<IsoLabel> queueByWeighting; // a.k.a. the Dijkstra queue
+    private PriorityQueue<IsoLabel> queueByZ; // so we know when we are finished
     private int visitedNodes;
     private double limit = -1;
     private ExploreType exploreType = TIME;
@@ -94,6 +97,7 @@ public String toString() {
     public ShortestPathTree(Graph g, Weighting weighting, boolean reverseFlow, TraversalMode traversalMode) {
         super(g, weighting, traversalMode);
         queueByWeighting = new PriorityQueue<>(1000, comparingDouble(l -> l.weight));
+        queueByZ = new PriorityQueue<>(1000);
         fromMap = new GHIntObjectHashMap<>(1000);
         this.reverseFlow = reverseFlow;
     }
@@ -109,6 +113,7 @@ public Path calcPath(int from, int to) {
     public void setTimeLimit(double limit) {
         exploreType = TIME;
         this.limit = limit;
+        this.queueByZ = new PriorityQueue<>(1000, comparingLong(l -> l.time));
     }
 
     /**
@@ -117,25 +122,30 @@ public void setTimeLimit(double limit) {
     public void setDistanceLimit(double limit) {
         exploreType = DISTANCE;
         this.limit = limit;
+        this.queueByZ = new PriorityQueue<>(1000, comparingDouble(l -> l.distance));
     }
 
     public void setWeightLimit(double limit) {
         exploreType = WEIGHT;
         this.limit = limit;
+        this.queueByZ = new PriorityQueue<>(1000, comparingDouble(l -> l.weight));
     }
 
     public void search(int from, final Consumer<IsoLabel> consumer) {
         checkAlreadyRun();
         IsoLabel currentLabel = new IsoLabel(from, -1, 0, 0, 0, null);
         queueByWeighting.add(currentLabel);
+        queueByZ.add(currentLabel);
         if (traversalMode == TraversalMode.NODE_BASED) {
             fromMap.put(from, currentLabel);
         }
-        while (!queueByWeighting.isEmpty()) {
+        while (!finished()) {
             currentLabel = queueByWeighting.poll();
             if (currentLabel.deleted)
                 continue;
-            consumer.accept(currentLabel);
+            if (getExploreValue(currentLabel) <= limit) {
+                consumer.accept(currentLabel);
+            }
             currentLabel.deleted = true;
             visitedNodes++;
 
@@ -152,31 +162,33 @@ public void search(int from, final Consumer<IsoLabel> consumer) {
                 double nextDistance = iter.getDistance() + currentLabel.distance;
                 long nextTime = GHUtility.calcMillisWithTurnMillis(weighting, iter, reverseFlow, currentLabel.edge) + currentLabel.time;
                 int nextTraversalId = traversalMode.createTraversalId(iter, reverseFlow);
-                IsoLabel label = fromMap.get(nextTraversalId);
-                if (label == null) {
-                    label = new IsoLabel(iter.getAdjNode(), iter.getEdge(), nextWeight, nextTime, nextDistance, currentLabel);
-                    fromMap.put(nextTraversalId, label);
-                    if (getExploreValue(label) <= limit) {
-                        queueByWeighting.add(label);
-                    }
-                } else if (label.weight > nextWeight) {
-                    label.deleted = true;
-                    label = new IsoLabel(iter.getAdjNode(), iter.getEdge(), nextWeight, nextTime, nextDistance, currentLabel);
-                    fromMap.put(nextTraversalId, label);
-                    if (getExploreValue(label) <= limit) {
-                        queueByWeighting.add(label);
-                    }
+                IsoLabel nextLabel = fromMap.get(nextTraversalId);
+                if (nextLabel == null) {
+                    nextLabel = new IsoLabel(iter.getAdjNode(), iter.getEdge(), nextWeight, nextTime, nextDistance, currentLabel);
+                    fromMap.put(nextTraversalId, nextLabel);
+                    queueByWeighting.add(nextLabel);
+                    queueByZ.add(nextLabel);
+                } else if (nextLabel.weight > nextWeight) {
+                    nextLabel.deleted = true;
+                    nextLabel = new IsoLabel(iter.getAdjNode(), iter.getEdge(), nextWeight, nextTime, nextDistance, currentLabel);
+                    fromMap.put(nextTraversalId, nextLabel);
+                    queueByWeighting.add(nextLabel);
+                    queueByZ.add(nextLabel);
                 }
             }
         }
     }
 
     public Collection<IsoLabel> getIsochroneEdges() {
         // assert alreadyRun
+        return getIsochroneEdges(limit);
+    }
+
+    public ArrayList<IsoLabel> getIsochroneEdges(double z) {
         ArrayList<IsoLabel> result = new ArrayList<>();
         for (ObjectCursor<IsoLabel> cursor : fromMap.values()) {
-            if (getExploreValue(cursor.value) > limit) {
-                assert cursor.value.parent == null || getExploreValue(cursor.value.parent) <= limit;
+            if (cursor.value.parent != null &&
+                    (getExploreValue(cursor.value) > z ^ getExploreValue(cursor.value.parent) > z)) {
                 result.add(cursor.value);
             }
         }
@@ -191,6 +203,14 @@ private double getExploreValue(IsoLabel label) {
         return label.distance;
     }
 
+    protected boolean finished() {
+        while (queueByZ.peek() != null && queueByZ.peek().deleted)
+            queueByZ.poll();
+        if (queueByZ.peek() == null)
+            return true;
+        return getExploreValue(queueByZ.peek()) >= limit;
+    }
+
     @Override
     public String getName() {
         return "reachability";

core/src/test/java/com/graphhopper/isochrone/algorithm/ShortestPathTreeTest.java

@@ -10,9 +10,12 @@
 import com.graphhopper.routing.weighting.TurnCostProvider;
 import com.graphhopper.routing.weighting.Weighting;
 import com.graphhopper.routing.weighting.custom.CustomModelParser;
+import com.graphhopper.routing.weighting.custom.CustomWeighting;
 import com.graphhopper.storage.BaseGraph;
 import com.graphhopper.storage.Graph;
 import com.graphhopper.util.CustomModel;
+import com.graphhopper.util.EdgeIterator;
+import com.graphhopper.util.EdgeIteratorState;
 import com.graphhopper.util.GHUtility;
 import org.junit.jupiter.api.AfterEach;
 import org.junit.jupiter.api.BeforeEach;
@@ -22,6 +25,8 @@
 import java.util.Collection;
 import java.util.List;
 
+import static com.graphhopper.json.Statement.If;
+import static com.graphhopper.json.Statement.Op.MULTIPLY;
 import static org.junit.jupiter.api.Assertions.*;
 
 /**
@@ -64,7 +69,8 @@ public long calcTurnMillis(int inEdge, int viaNode, int outEdge) {
 
     private final BooleanEncodedValue accessEnc = new SimpleBooleanEncodedValue("access", true);
     private final DecimalEncodedValue speedEnc = new DecimalEncodedValueImpl("speed", 5, 5, false);
-    private final EncodingManager encodingManager = EncodingManager.start().add(accessEnc).add(speedEnc).build();
+    private final BooleanEncodedValue ferryEnc = new SimpleBooleanEncodedValue("ferry", false);
+    private final EncodingManager encodingManager = EncodingManager.start().add(accessEnc).add(speedEnc).add(ferryEnc).build();
     private BaseGraph graph;
 
 
@@ -169,6 +175,33 @@ public void testNoTimeLimit() {
         );
     }
 
+    @Test
+    public void testFerry() {
+        AllEdgesIterator allEdges = graph.getAllEdges();
+        while (allEdges.next()) {
+            allEdges.set(ferryEnc, false);
+        }
+        EdgeIteratorState edge = findEdge(6, 7);
+        edge.set(ferryEnc, true);
+
+        List<ShortestPathTree.IsoLabel> result = new ArrayList<>();
+        CustomModel cm = new CustomModel();
+        cm.addToPriority(If("ferry", MULTIPLY, "0.005"));
+        CustomWeighting weighting = CustomModelParser.createWeighting(accessEnc, speedEnc, null, encodingManager, TurnCostProvider.NO_TURN_COST_PROVIDER, cm);
+        ShortestPathTree instance = new ShortestPathTree(graph, weighting, false, TraversalMode.NODE_BASED);
+        instance.setTimeLimit(30_000);
+        instance.search(0, result::add);
+        assertEquals(4, result.size());
+        assertAll(
+                () -> assertEquals(0, result.get(0).time), () -> assertEquals(0, result.get(0).node),
+                () -> assertEquals(9000, result.get(1).time), () -> assertEquals(4, result.get(1).node),
+                () -> assertEquals(18000, result.get(2).time), () -> assertEquals(6, result.get(2).node),
+                () -> assertEquals(25200, result.get(3).time), () -> assertEquals(1, result.get(3).node)
+        );
+        assertEquals(7, instance.getVisitedNodes(), "If this increases, make sure we are not traversing entire graph irl");
+    }
+
+
     @Test
     public void testEdgeBasedWithFreeUTurns() {
         List<ShortestPathTree.IsoLabel> result = new ArrayList<>();
@@ -326,4 +359,14 @@ public void testSearchByDistance() {
         );
     }
 
+    EdgeIteratorState findEdge(int a, int b) {
+        EdgeIterator edgeIterator = graph.createEdgeExplorer().setBaseNode(a);
+        while (edgeIterator.next()) {
+            if (edgeIterator.getAdjNode() == b) {
+                return edgeIterator;
+            }
+        }
+        throw new RuntimeException("nope");
+    }
+
 }