Repository: graphhopper/graphhopper
Pull request #2309 (2021-05-08 13:53:15)

map matching: properly access turn costs for virtual nodes
MapMatching uses the weighting and access the turn costs data of the baseGraph even for virtual nodes that can lead to rare exceptions ala:

```
java.lang.ArrayIndexOutOfBoundsException: Index 4476 out of bounds for length 4476
	at com.graphhopper.storage.RAMIntDataAccess.getInt(RAMIntDataAccess.java:208)
	at com.graphhopper.storage.GHNodeAccess.getTurnCostIndex(GHNodeAccess.java:90)
	at com.graphhopper.storage.TurnCostStorage.readFlags(TurnCostStorage.java:186)
	at com.graphhopper.storage.TurnCostStorage.readFlags(TurnCostStorage.java:181)
	at com.graphhopper.storage.TurnCostStorage.get(TurnCostStorage.java:167)
	at com.graphhopper.routing.weighting.DefaultTurnCostProvider.calcTurnWeight(DefaultTurnCostProvider.java:69)
	at com.graphhopper.routing.weighting.AbstractWeighting.calcTurnWeight(AbstractWeighting.java:84)
	at com.graphhopper.util.GHUtility.calcWeightWithTurnWeight(GHUtility.java:725)
	at com.graphhopper.util.GHUtility.calcWeightWithTurnWeightWithAccess(GHUtility.java:708)
	at com.graphhopper.routing.AbstractNonCHBidirAlgo.calcWeight(AbstractNonCHBidirAlgo.java:188)
	at com.graphhopper.routing.AStarBidirection.calcWeight(AStarBidirection.java:108)
	at com.graphhopper.routing.AbstractNonCHBidirAlgo.fillEdges(AbstractNonCHBidirAlgo.java:152)
	at com.graphhopper.routing.AbstractNonCHBidirAlgo.fillEdgesTo(AbstractNonCHBidirAlgo.java:142)
	at com.graphhopper.routing.AbstractBidirAlgo.runAlgo(AbstractBidirAlgo.java:154)
	at com.graphhopper.routing.AbstractBidirAlgo.calcPath(AbstractBidirAlgo.java:93)
	at com.graphhopper.matching.MapMatching.computeViterbiSequence(MapMatching.java:344)
	at com.graphhopper.matching.MapMatching.match(MapMatching.java:182)
	at com.graphhopper.resources.MapMatchingResource.match(MapMatchingResource.java:118)
```


Patch:
diff --git a/api/src/main/java/com/graphhopper/json/Statement.java b/api/src/main/java/com/graphhopper/json/Statement.java
index 7feb96e5e..4e780ec27 100644
--- a/api/src/main/java/com/graphhopper/json/Statement.java
+++ b/api/src/main/java/com/graphhopper/json/Statement.java
@@ -46,6 +46,17 @@ public class Statement {
         return value;
     }
 
+    public double apply(double externValue) {
+        switch (operation) {
+            case MULTIPLY:
+                return value * externValue;
+            case LIMIT:
+                return Math.min(value, externValue);
+            default:
+                throw new IllegalArgumentException();
+        }
+    }
+
     public enum Keyword {
         IF("if"), ELSEIF("else_if"), ELSE("else");
 
diff --git a/core/src/main/java/com/graphhopper/routing/weighting/custom/CustomModelParser.java b/core/src/main/java/com/graphhopper/routing/weighting/custom/CustomModelParser.java
index 2e77c5882..b61851f6f 100644
--- a/core/src/main/java/com/graphhopper/routing/weighting/custom/CustomModelParser.java
+++ b/core/src/main/java/com/graphhopper/routing/weighting/custom/CustomModelParser.java
@@ -42,6 +42,10 @@ import java.io.*;
 import java.util.*;
 import java.util.concurrent.atomic.AtomicLong;
 
+import static com.graphhopper.json.Statement.Keyword.ELSE;
+import static com.graphhopper.json.Statement.Keyword.IF;
+import static com.graphhopper.json.Statement.Op.LIMIT;
+
 public class CustomModelParser {
     private static final AtomicLong longVal = new AtomicLong(1);
     static final String IN_AREA_PREFIX = "in_";
@@ -405,40 +409,58 @@ public class CustomModelParser {
         }
     }
 
-    static double findMaxSpeed(CustomModel customModel, final double maxSpeed) {
-        double globalMin_maxSpeed = maxSpeed;
-        double blockMax_maxSpeed = 0;
-        for (Statement statement : customModel.getSpeed()) {
-            // Lowering the max_speed estimate for 'limit_to' only (TODO later also for MULTIPLY)
-            if (statement.getOperation() == Statement.Op.LIMIT) {
-                if (statement.getValue() > maxSpeed)
-                    throw new IllegalArgumentException("Can never apply 'limit_to': " + statement.getValue()
-                            + " because maximum vehicle speed is " + maxSpeed);
-
-                switch (statement.getKeyword()) {
-                    case IF:
-                        if ("true".equals(statement.getCondition())) {
-                            blockMax_maxSpeed = globalMin_maxSpeed = Math.min(statement.getValue(), maxSpeed);
-                        } else {
-                            blockMax_maxSpeed = statement.getValue();
-                        }
-                        break;
-                    case ELSEIF:
-                        blockMax_maxSpeed = Math.max(blockMax_maxSpeed, statement.getValue());
-                        break;
-                    case ELSE:
-                        blockMax_maxSpeed = Math.max(blockMax_maxSpeed, statement.getValue());
-                        globalMin_maxSpeed = Math.min(globalMin_maxSpeed, blockMax_maxSpeed);
-                        break;
-                    default:
-                        throw new IllegalArgumentException("unknown keyword " + statement.getKeyword());
-                }
+    static double findMaxSpeed(CustomModel customModel, double maxSpeed) {
+        return findMaxSpeed(customModel.getSpeed(), maxSpeed);
+    }
 
-                if (globalMin_maxSpeed <= 0)
-                    throw new IllegalArgumentException("speed is always limited to 0. This must not be but results from " + statement);
-            }
-        }
+    static double findMaxSpeed(List<Statement> speedStatements, final double maxSpeed) {
+        // throw an error if one of the limit statements won't possibly do anything
+        Optional<Statement> falseStatement = speedStatements.stream()
+                .filter(st -> LIMIT.equals(st.getOperation()) && st.getValue() > maxSpeed)
+                .findFirst();
+        if (falseStatement.isPresent())
+            throw new IllegalArgumentException("Can never apply 'limit_to': " + falseStatement.get().getValue()
+                    + " because maximum vehicle speed is " + maxSpeed);
+
+        // we want to find the smallest speed that cannot be exceeded by any edge. the 'blocks' of speed statements
+        // are applied one after the other.
+        double result = maxSpeed;
+        List<List<Statement>> blocks = splitIntoBlocks(speedStatements);
+        for (List<Statement> block : blocks)
+            result = getMaxSpeedForBlock(block, result);
+        if (maxSpeed <= 0)
+            throw new IllegalStateException("max speed is <= 0");
+        return result;
+    }
 
-        return globalMin_maxSpeed;
+    static double getMaxSpeedForBlock(List<Statement> block, final double maxSpeed) {
+        if (block.isEmpty() || !IF.equals(block.get(0).getKeyword()))
+            throw new IllegalArgumentException("Every block must start with an if-statement");
+        if (block.get(0).getCondition().trim().equals("true"))
+            // this if statement is always executed while the other statements are never executed
+            // -> we just apply this one statement
+            return block.get(0).apply(maxSpeed);
+
+        double blockMax = block.stream()
+                .mapToDouble(statement -> statement.apply(maxSpeed))
+                .max()
+                .orElse(maxSpeed);
+        // if there is no 'else' statement it's like there is a 'neutral' branch that leaves the initial max speed as is
+        if (block.stream().noneMatch(st -> ELSE.equals(st.getKeyword())))
+            blockMax = Math.max(blockMax, maxSpeed);
+        return blockMax;
+    }
+
+    /**
+     * Splits the specified list into several list of statements starting with if
+     */
+    static List<List<Statement>> splitIntoBlocks(List<Statement> statements) {
+        List<List<Statement>> result = new ArrayList<>();
+        List<Statement> block = null;
+        for (Statement st : statements) {
+            if (IF.equals(st.getKeyword())) result.add(block = new ArrayList<>());
+            block.add(st);
+        }
+        return result;
     }
 }
diff --git a/core/src/main/java/com/graphhopper/routing/weighting/custom/CustomWeighting.java b/core/src/main/java/com/graphhopper/routing/weighting/custom/CustomWeighting.java
index fcbfaec6a..b5bc1ea00 100644
--- a/core/src/main/java/com/graphhopper/routing/weighting/custom/CustomWeighting.java
+++ b/core/src/main/java/com/graphhopper/routing/weighting/custom/CustomWeighting.java
@@ -126,7 +126,8 @@ public final class CustomWeighting extends AbstractWeighting {
             return Double.POSITIVE_INFINITY;
 
         double speed = edgeToSpeedMapping.get(edgeState, reverse);
-        assert speed <= maxSpeed * SPEED_CONV : "for " + getName() + " speed <= maxSpeed is violated, " + speed + " <= " + maxSpeed * SPEED_CONV;
+        if (speed > maxSpeed * SPEED_CONV)
+            throw new IllegalStateException("for " + getName() + " speed <= maxSpeed is violated, " + speed + " <= " + maxSpeed * SPEED_CONV);
         if (speed == 0)
             return Double.POSITIVE_INFINITY;
         if (speed < 0)
diff --git a/map-matching/src/main/java/com/graphhopper/matching/MapMatching.java b/map-matching/src/main/java/com/graphhopper/matching/MapMatching.java
index b3ad7ae85..e3009c717 100644
--- a/map-matching/src/main/java/com/graphhopper/matching/MapMatching.java
+++ b/map-matching/src/main/java/com/graphhopper/matching/MapMatching.java
@@ -83,7 +83,8 @@ public class MapMatching {
     private double transitionProbabilityBeta = 2.0;
     private final int maxVisitedNodes;
     private final DistanceCalc distanceCalc = new DistancePlaneProjection();
-    private final Weighting weighting;
+    private final Weighting unwrappedWeighting;
+    private Weighting weighting;
     private final BooleanEncodedValue inSubnetworkEnc;
     private QueryGraph queryGraph;
 
@@ -141,7 +142,7 @@ public class MapMatching {
             landmarks = null;
         }
         graph = graphHopper.getGraphHopperStorage();
-        weighting = graphHopper.createWeighting(profile, hints);
+        unwrappedWeighting = graphHopper.createWeighting(profile, hints);
         inSubnetworkEnc = graphHopper.getEncodingManager().getBooleanEncodedValue(Subnetwork.key(profileStr));
         this.maxVisitedNodes = hints.getInt(Parameters.Routing.MAX_VISITED_NODES, Integer.MAX_VALUE);
     }
@@ -173,6 +174,7 @@ public class MapMatching {
         // Create the query graph, containing split edges so that all the places where an observation might have happened
         // are a node. This modifies the Snap objects and puts the new node numbers into them.
         queryGraph = QueryGraph.create(graph, snapsPerObservation.stream().flatMap(Collection::stream).collect(Collectors.toList()));
+        weighting = queryGraph.wrapWeighting(unwrappedWeighting);
 
         // Creates candidates from the Snaps of all observations (a candidate is basically a
         // Snap + direction).
@@ -219,7 +221,7 @@ public class MapMatching {
         double rLon = (measurementErrorSigma * 360.0 / DistanceCalcEarth.DIST_EARTH.calcCircumference(queryLat));
         double rLat = measurementErrorSigma / DistanceCalcEarth.METERS_PER_DEGREE;
         Envelope envelope = new Envelope(queryLon, queryLon, queryLat, queryLat);
-        for (int i=0; i<50; i++) {
+        for (int i = 0; i < 50; i++) {
             envelope.expandBy(rLon, rLat);
             List<Snap> snaps = findCandidateSnapsInBBox(queryLat, queryLon, BBox.fromEnvelope(envelope));
             if (!snaps.isEmpty()) {
@@ -230,7 +232,7 @@ public class MapMatching {
     }
 
     private List<Snap> findCandidateSnapsInBBox(double queryLat, double queryLon, BBox queryShape) {
-        EdgeFilter edgeFilter = new DefaultSnapFilter(weighting, inSubnetworkEnc);
+        EdgeFilter edgeFilter = new DefaultSnapFilter(unwrappedWeighting, inSubnetworkEnc);
         List<Snap> snaps = new ArrayList<>();
         IntHashSet seenEdges = new IntHashSet();
         IntHashSet seenNodes = new IntHashSet();


Test patch:
diff --git a/core/src/test/java/com/graphhopper/routing/weighting/custom/CustomModelParserTest.java b/core/src/test/java/com/graphhopper/routing/weighting/custom/CustomModelParserTest.java
index 7afab6a40..66f968c11 100644
--- a/core/src/test/java/com/graphhopper/routing/weighting/custom/CustomModelParserTest.java
+++ b/core/src/test/java/com/graphhopper/routing/weighting/custom/CustomModelParserTest.java
@@ -18,6 +18,7 @@
 
 package com.graphhopper.routing.weighting.custom;
 
+import com.graphhopper.json.Statement;
 import com.graphhopper.routing.ev.*;
 import com.graphhopper.routing.util.CarFlagEncoder;
 import com.graphhopper.routing.util.EncodingManager;
@@ -32,8 +33,7 @@ import org.junit.jupiter.api.Test;
 import org.locationtech.jts.geom.Coordinate;
 import org.locationtech.jts.geom.GeometryFactory;
 
-import java.util.HashMap;
-import java.util.Map;
+import java.util.*;
 
 import static com.graphhopper.json.Statement.*;
 import static com.graphhopper.json.Statement.Op.LIMIT;
@@ -181,17 +181,52 @@ class CustomModelParserTest {
 
     @Test
     public void testFindMaxSpeed() {
-        CustomModel customModel = new CustomModel();
-        customModel.addToSpeed(If("true", LIMIT, 100));
-        assertEquals(100, CustomModelParser.findMaxSpeed(customModel, 120));
+        List<Statement> statements = new ArrayList<>();
+        statements.add(If("true", LIMIT, 100));
+        assertEquals(100, CustomModelParser.findMaxSpeed(statements, 120));
 
-        customModel.addToSpeed(Else(LIMIT, 20));
-        assertEquals(100, CustomModelParser.findMaxSpeed(customModel, 120));
+        statements.add(Else(LIMIT, 20));
+        assertEquals(100, CustomModelParser.findMaxSpeed(statements, 120));
 
-        customModel = new CustomModel();
-        customModel.addToSpeed(If("road_environment == BRIDGE", LIMIT, 85));
-        customModel.addToSpeed(Else(LIMIT, 100));
-        assertEquals(100, CustomModelParser.findMaxSpeed(customModel, 120));
+        statements = new ArrayList<>();
+        statements.add(If("road_environment == BRIDGE", LIMIT, 85));
+        statements.add(Else(LIMIT, 100));
+        assertEquals(100, CustomModelParser.findMaxSpeed(statements, 120));
+    }
+
+    @Test
+    public void findMaxSpeed_limitAndMultiply() {
+        List<Statement> statements = Arrays.asList(
+                If("road_class == TERTIARY", LIMIT, 90),
+                ElseIf("road_class == SECONDARY", MULTIPLY, 1.0),
+                ElseIf("road_class == PRIMARY", LIMIT, 30),
+                Else(LIMIT, 3)
+        );
+        assertEquals(140, CustomModelParser.findMaxSpeed(statements, 140));
+    }
+
+    @Test
+    public void findMaxSpeed_multipleBlocks() {
+        List<Statement> statements = Arrays.asList(
+                If("road_class == TERTIARY", MULTIPLY, 0.2),
+                ElseIf("road_class == SECONDARY", LIMIT, 25),
+                If("road_environment == TUNNEL", LIMIT, 60),
+                ElseIf("road_environment == BRIDGE", LIMIT, 50),
+                Else(MULTIPLY, 0.8)
+        );
+        assertEquals(120, CustomModelParser.findMaxSpeed(statements, 150));
+        assertEquals(80, CustomModelParser.findMaxSpeed(statements, 100));
+        assertEquals(60, CustomModelParser.findMaxSpeed(statements, 60));
+
+        statements = Arrays.asList(
+                If("road_class == TERTIARY", MULTIPLY, 0.2),
+                ElseIf("road_class == SECONDARY", LIMIT, 25),
+                Else(LIMIT, 40),
+                If("road_environment == TUNNEL", MULTIPLY, 0.8),
+                ElseIf("road_environment == BRIDGE", LIMIT, 30)
+        );
+        assertEquals(40, CustomModelParser.findMaxSpeed(statements, 150));
+        assertEquals(40, CustomModelParser.findMaxSpeed(statements, 40));
     }
 
     @Test
diff --git a/core/src/test/java/com/graphhopper/routing/weighting/custom/CustomWeightingTest.java b/core/src/test/java/com/graphhopper/routing/weighting/custom/CustomWeightingTest.java
index 3cd792509..afc69af42 100644
--- a/core/src/test/java/com/graphhopper/routing/weighting/custom/CustomWeightingTest.java
+++ b/core/src/test/java/com/graphhopper/routing/weighting/custom/CustomWeightingTest.java
@@ -2,6 +2,7 @@ package com.graphhopper.routing.weighting.custom;
 
 import com.bedatadriven.jackson.datatype.jts.JtsModule;
 import com.fasterxml.jackson.databind.ObjectMapper;
+import com.graphhopper.json.Statement;
 import com.graphhopper.routing.ev.*;
 import com.graphhopper.routing.util.CarFlagEncoder;
 import com.graphhopper.routing.util.EncodingManager;
@@ -251,6 +252,18 @@ class CustomWeightingTest {
         assertTrue(ex.getMessage().startsWith("Custom Model too big"), ex.getMessage());
     }
 
+    @Test
+    public void maxSpeedViolated_bug_2307() {
+        EdgeIteratorState motorway = graph.edge(0, 1).setDistance(10).
+                set(roadClassEnc, MOTORWAY).set(avSpeedEnc, 80).set(accessEnc, true, true);
+        CustomModel customModel = new CustomModel()
+                .addToSpeed(Statement.If("road_class == MOTORWAY", Statement.Op.MULTIPLY, 0.7))
+                .addToSpeed(Statement.Else(Statement.Op.LIMIT, 30));
+        Weighting weighting = createWeighting(customModel);
+        assertEquals(1.3429, weighting.calcEdgeWeight(motorway, false), 1e-4);
+        assertEquals(10 / (80 * 0.7 / 3.6) * 1000, weighting.calcEdgeMillis(motorway, false), 1);
+    }
+
     private Weighting createWeighting(CustomModel vehicleModel) {
         return CustomModelParser.createWeighting(carFE, encodingManager, NO_TURN_COST_PROVIDER, vehicleModel);
     }