Merge pull request graphhopper#977 from graphhopper/issue_976

Fix CH preparation bug graphhopper#976

Author: karussell

core/src/main/java/com/graphhopper/routing/VirtualEdgeIterator.java

@@ -187,7 +187,7 @@ public void setSkippedEdges(int edge1, int edge2) {
     }
 
     @Override
-    public boolean canBeOverwritten(long flags) {
+    public int getMergeStatus(long flags) {
         throw new UnsupportedOperationException("Not supported.");
     }
 }

core/src/main/java/com/graphhopper/routing/VirtualEdgeIteratorState.java

@@ -177,7 +177,7 @@ public int getAdditionalField() {
     }
 
     @Override
-    public boolean canBeOverwritten(long flags) {
+    public int getMergeStatus(long flags) {
         throw new UnsupportedOperationException("Not supported.");
     }
 

core/src/main/java/com/graphhopper/routing/ch/PrepareContractionHierarchies.java

@@ -27,10 +27,7 @@
 import org.slf4j.Logger;
 import org.slf4j.LoggerFactory;
 
-import java.util.HashMap;
-import java.util.Map;
-import java.util.Random;
-import java.util.Set;
+import java.util.*;
 
 import static com.graphhopper.util.Parameters.Algorithms.ASTAR_BI;
 import static com.graphhopper.util.Parameters.Algorithms.DIJKSTRA_BI;
@@ -281,6 +278,7 @@ void contractNodes() {
 
             counter++;
             int polledNode = sortedNodes.pollKey();
+
             if (!sortedNodes.isEmpty() && sortedNodes.getSize() < lastNodesLazyUpdates) {
                 lazySW.start();
                 int priority = oldPriorities[polledNode] = calculatePriority(polledNode);
@@ -293,8 +291,10 @@ void contractNodes() {
                 lazySW.stop();
             }
 
-            // contract!
-            newShortcuts += addShortcuts(polledNode);
+            // contract node v!
+            shortcuts.clear();
+            findShortcuts(addScHandler.setNode(polledNode));
+            newShortcuts += addShortcuts(shortcuts.keySet());
             prepareGraph.setLevel(polledNode, level);
             level++;
 
@@ -505,19 +505,27 @@ void findShortcuts(ShortcutHandler sch) {
     /**
      * Introduces the necessary shortcuts for adjNode v in the graph.
      */
-    int addShortcuts(int v) {
-        shortcuts.clear();
-        findShortcuts(addScHandler.setNode(v));
+    int addShortcuts(Collection<Shortcut> tmpShortcuts) {
         int tmpNewShortcuts = 0;
         NEXT_SC:
-        for (Shortcut sc : shortcuts.keySet()) {
+        for (Shortcut sc : tmpShortcuts) {
             boolean updatedInGraph = false;
             // check if we need to update some existing shortcut in the graph
             CHEdgeIterator iter = vehicleOutExplorer.setBaseNode(sc.from);
             while (iter.next()) {
-                if (iter.isShortcut() && iter.getAdjNode() == sc.to && iter.canBeOverwritten(sc.flags)) {
-                    if (sc.weight >= prepareWeighting.calcWeight(iter, false, EdgeIterator.NO_EDGE))
+                if (iter.isShortcut() && iter.getAdjNode() == sc.to) {
+                    int status = iter.getMergeStatus(sc.flags);
+                    if (status == 0)
+                        continue;
+
+                    if (sc.weight >= prepareWeighting.calcWeight(iter, false, EdgeIterator.NO_EDGE)) {
+                        // special case if a bidirectional shortcut has worse weight and still has to be added as otherwise the opposite direction would be missing
+                        // see testShortcutMergeBug
+                        if (status == 2)
+                            break;
+
                         continue NEXT_SC;
+                    }
 
                     if (iter.getEdge() == sc.skippedEdge1 || iter.getEdge() == sc.skippedEdge2) {
                         throw new IllegalStateException("Shortcut cannot update itself! " + iter.getEdge()
@@ -871,7 +879,10 @@ public void foundShortcut(int u_fromNode, int w_toNode,
                 }
             }
 
-            shortcuts.put(sc, sc);
+            Shortcut old = shortcuts.put(sc, sc);
+            if (old != null)
+                throw new IllegalStateException("Shortcut did not exist (" + sc + ") but was overwriting another one? " + old);
+
             sc.skippedEdge1 = skippedEdge1;
             sc.skippedEdge2 = outgoingEdges.getEdge();
             sc.originalEdges = incomingEdgeOrigCount + getOrigEdgeCount(outgoingEdges.getEdge());

core/src/main/java/com/graphhopper/routing/ch/PrepareEncoder.java

@@ -18,8 +18,8 @@
 package com.graphhopper.routing.ch;
 
 /**
- * The flags are stored differently for shortcuts: just one weight and the direction flags.
- * <p>
+ * The flags are stored differently for shortcuts: just one weight and the two direction bits which is handled by this
+ * class for now as static methods.
  *
  * @author Peter Karich
  */
@@ -29,33 +29,49 @@ public class PrepareEncoder {
     private static final long scBwdDir = 0x2;
     private static final long scDirMask = 0x3;
 
+    /**
+     * A bitmask for two directions
+     */
     public static final long getScDirMask() {
         return scDirMask;
     }
 
+    /**
+     * The bit for forward direction
+     */
     public static final long getScFwdDir() {
         return scFwdDir;
     }
 
+    /**
+     * The bit for backward direction
+     */
     public static final long getScBwdDir() {
         return scBwdDir;
     }
 
     /**
-     * Returns true if flags1 can be overwritten in the edge by flags2 without restricting or
-     * changing the directions of flags1.
-     * <p>
+     * Returns 1 if existingScFlags of an existing shortcut can be overwritten with a new shortcut by
+     * newScFlags without limiting or changing the directions of the existing shortcut.
+     * The method returns 2 for the same condition but only if the new shortcut has to be added
+     * even if weight is higher than existing shortcut weight.
+     * <pre>
+     *                 | newScFlags:
+     * existingScFlags | -> | <- | <->
+     * ->              |  1 | 0  | 2
+     * <-              |  0 | 1  | 2
+     * <->             |  0 | 0  | 1
+     * </pre>
      *
-     * @return true if flags2 is enabled in both directions or if both flags are pointing into the
-     * same direction.
+     * @return 1 if newScFlags is identical to existingScFlags for the two direction bits and 0 otherwise.
+     * There are two special cases when it returns 2.
      */
-    //        \  flags2:
-    // flags1  \ -> | <- | <->
-    // ->         t | f  | t
-    // <-         f | t  | t
-    // <->        f | f  | t
-    public static final boolean canBeOverwritten(long flags1, long flags2) {
-        return (flags2 & scDirMask) == scDirMask
-                || (flags1 & scDirMask) == (flags2 & scDirMask);
+    public static final int getScMergeStatus(long existingScFlags, long newScFlags) {
+        if ((existingScFlags & scDirMask) == (newScFlags & scDirMask))
+            return 1;
+        else if ((newScFlags & scDirMask) == scDirMask)
+            return 2;
+
+        return 0;
     }
 }

core/src/main/java/com/graphhopper/storage/CHGraphImpl.java

@@ -523,8 +523,8 @@ public final EdgeIteratorState setWayGeometry(PointList list) {
         }
 
         @Override
-        public boolean canBeOverwritten(long flags) {
-            return PrepareEncoder.canBeOverwritten(getDirectFlags(), flags);
+        public int getMergeStatus(long flags) {
+            return PrepareEncoder.getScMergeStatus(getDirectFlags(), flags);
         }
     }
 
@@ -619,8 +619,8 @@ public final double getWeight() {
         }
 
         @Override
-        public boolean canBeOverwritten(long flags) {
-            return PrepareEncoder.canBeOverwritten(getDirectFlags(), flags);
+        public int getMergeStatus(long flags) {
+            return PrepareEncoder.getScMergeStatus(getDirectFlags(), flags);
         }
     }
 }

core/src/main/java/com/graphhopper/util/CHEdgeIteratorState.java

@@ -45,11 +45,10 @@ public interface CHEdgeIteratorState extends EdgeIteratorState {
 
     /**
      * This method is only used on preparation.
-     * <p>
      *
-     * @see PrepareEncoder#canBeOverwritten(long, long)
+     * @see PrepareEncoder#getScMergeStatus(long, long)
      */
-    boolean canBeOverwritten(long flags);
+    int getMergeStatus(long flags);
 
     /**
      * Returns the weight of this shortcut.

core/src/main/java/com/graphhopper/util/GHUtility.java

@@ -491,7 +491,7 @@ public CHEdgeIteratorState setWeight(double weight) {
         }
 
         @Override
-        public boolean canBeOverwritten(long flags) {
+        public int getMergeStatus(long flags) {
             throw new UnsupportedOperationException("Not supported. Edge is empty.");
         }
     }

core/src/test/java/com/graphhopper/routing/ch/PrepareContractionHierarchiesTest.java

@@ -645,4 +645,31 @@ void checkPath(GraphHopperStorage ghStorage, Weighting w, int expShortcuts, doub
         assertEquals(w.toString(), expDistance, p.getDistance(), 1e-5);
         assertEquals(w.toString(), expNodes, p.calcNodes());
     }
+
+    @Test
+    public void testShortcutMergeBug() {
+        // We refer to this real world situation http://www.openstreetmap.org/#map=19/52.71205/-1.77326
+        // assume the following graph:
+        //
+        // ---1---->----2-----3
+        //    \--------/
+        //
+        // where there are two roads from 1 to 2 and the directed road has a smaller weight
+        // leading to two shortcuts sc1 (unidir) and sc2 (bidir) where the second should NOT be rejected due to the larger weight
+        GraphHopperStorage g = createGHStorage();
+        g.edge(1, 2, 1, true);
+        g.edge(1, 2, 1, false);
+        g.edge(2, 3, 1, true);
+
+        CHGraph lg = g.getGraph(CHGraph.class);
+        PrepareContractionHierarchies prepare = new PrepareContractionHierarchies(dir, g, lg, weighting, tMode);
+        prepare.initFromGraph();
+
+        // order is important here
+        Shortcut sc1 = new Shortcut(1, 3, 6.81620625, 121.18);
+        Shortcut sc2 = new Shortcut(1, 3, 6.82048125, 121.25);
+        sc2.flags = PrepareEncoder.getScDirMask();
+        List<Shortcut> list = Arrays.asList(sc1, sc2);
+        assertEquals(2, prepare.addShortcuts(list));
+    }
 }

core/src/test/java/com/graphhopper/routing/ch/PrepareEncoderTest.java

@@ -19,6 +19,7 @@
 
 import org.junit.Test;
 
+import static org.junit.Assert.assertEquals;
 import static org.junit.Assert.assertFalse;
 import static org.junit.Assert.assertTrue;
 
@@ -32,15 +33,15 @@ public void testOverwrite() {
         long forward = PrepareEncoder.getScFwdDir();
         long backward = PrepareEncoder.getScFwdDir() ^ PrepareEncoder.getScDirMask();
         long both = PrepareEncoder.getScDirMask();
-        assertTrue(PrepareEncoder.canBeOverwritten(forward, forward));
-        assertTrue(PrepareEncoder.canBeOverwritten(backward, backward));
-        assertTrue(PrepareEncoder.canBeOverwritten(forward, both));
-        assertTrue(PrepareEncoder.canBeOverwritten(backward, both));
+        assertEquals(1, PrepareEncoder.getScMergeStatus(forward, forward));
+        assertEquals(1, PrepareEncoder.getScMergeStatus(backward, backward));
+        assertEquals(2, PrepareEncoder.getScMergeStatus(forward, both));
+        assertEquals(2, PrepareEncoder.getScMergeStatus(backward, both));
 
-        assertTrue(PrepareEncoder.canBeOverwritten(both, both));
-        assertFalse(PrepareEncoder.canBeOverwritten(both, forward));
-        assertFalse(PrepareEncoder.canBeOverwritten(both, backward));
-        assertFalse(PrepareEncoder.canBeOverwritten(forward, backward));
-        assertFalse(PrepareEncoder.canBeOverwritten(backward, forward));
+        assertEquals(1, PrepareEncoder.getScMergeStatus(both, both));
+        assertEquals(0, PrepareEncoder.getScMergeStatus(both, forward));
+        assertEquals(0, PrepareEncoder.getScMergeStatus(both, backward));
+        assertEquals(0, PrepareEncoder.getScMergeStatus(forward, backward));
+        assertEquals(0, PrepareEncoder.getScMergeStatus(backward, forward));
     }
 }