Repository: graphhopper/graphhopper
Pull request #2120 (2020-09-07 20:20:53)

Add binary heap with efficient update operation and use it for node-queue in CH preparation
So far we use a `TreeMap<Integer, Long>` to queue the nodes for CH preparation, but this wastes lots of memory. Using jol I find that 1mio nodes take about 64MB:

```java
   @Test
    public void testMemory() {
        GHTreeMapComposed heap = new GHTreeMapComposed();
        Random rnd = new Random(123);
        for (int i = 0; i < 1_000_000; i++) {
            heap.insert(i, rnd.nextFloat());
        }
        System.out.println(heap.getSize());
        System.out.println(GraphLayout.parseInstance(heap).toFootprint());
    }
```
```xml
<!-- pom.xml -->
<dependency>
  <groupId>org.openjdk.jol</groupId>
  <artifactId>jol-core</artifactId>
  <version>0.10</version>
</dependency>
```

and we use an additional float per node to store the `old_priorities` (another 4MB per 1mio nodes).
This PR adds a modified version of `IntDoubleBinaryHeap` that only uses two ints and a float per node (12MB instead of 68MB for 1mio nodes).

See inline comments for more details (The `BinaryHeapWithUpdate` class especially).

karussell: Thanks! 

> 12MB instead of 68MB for 1mio nodes

I tried the map bayern-200101.osm.pbf with 4 profiles (car, car4wd, foot and bike) and every node-based CH profile has more than 3 million nodes (it says), which would mean a memory difference of approx 3*4*56MB=672MB, which in practise does not seem to come close. This min_heap branch allowed me to do the import with `-Xmx1350m` (1300m failed) whereas for master I only needed `-Xmx1600m` to avoid OOM problems, the theory would have suggested that at least `-Xmx1950m` are necessary. Of course this is a really good improvement :) but likely the different start times reduces the memory pressure a lot. I.e. the map is small enough so that the first CH preparation is already started and reduced the queue, when the others preparations like foot have not yet fully build the queue and so peak memory demand do not come at the exact same time. (BTW: unfortunately I ran all tests with JDK 12 by accident but shouldn't matter much)

Now, the speed of the preparations seems to be the same. I only added 200MB to the minimum memory to avoid a too different memory pressure. Also our measurement tests on the server look fine speed-wise.

So this branch helps a lot for bigger maps and multiple preparations. Really good work!
easbar: Thanks for testing!

Regarding the 'memory pressure': Its true that the TreeMap we used beforehand shrinks during the preparation (it gets smaller and smaller the more nodes get contracted, and especially at the beginning of the preparation this goes really quick as many nodes get contracted without adding shortcuts). The new heap implementation does not shrink, it just keeps its size. We could trim it after certain intervals as long as we keep the full `positions` int[] array, but not sure if this is really needed. 

Regarding the Xmx experiments we have to be careful about what causes the main memory usage. Its a good idea to use several CH profiles. For example I only tested with a single one and in this case often the `sortedNodes` do not cause the maximum memory usage. Instead it was either the OSM reading or, in some cases the subnetwork removal. Especially the subnetwork removal made testing a bit complicated because sometimes my Xmx limit was exceeded and sometimes it was not. Apparently the subnetwork search sometimes takes more memory than other times.

My main motivation for this PR was the fact that in this branch: `prepare_graph` I need to allocate additional memory *at the beginning* of the preparation, so the memory size of `sortedNodes` becomes more critical than in current master. 

So I think using the new heap here has no disadvantage, except that it does not shrink, but we can implement this if really needed. I also tried this new heap in `Node/EdgeBasedWitnessPathSearcher`, but unfortunately its not an improvement. Apparently the additional cost of keeping track of the positions in `percolateUp/Down` is not worth it to speed up the `update()` method in this case.
karussell: > The new heap implementation does not shrink, it just keeps its size. 

Ah, good to know, but I don't think we need to change it for now. If we have lot of time and energy we could implement this. And independent from this we could implement some kind of a "delayed start" and start a new preparation in parallel only when enough memory is available. This way the number of threads is dynamic and we do not need to configure it and always have the shortest total preparation time possible.

> Its a good idea to use several CH profiles. 

Yeah, I also noticed the memory problems only happen for the import for 1 or 2 vehicle profiles :)

> I also tried this new heap in Node/EdgeBasedWitnessPathSearcher, but unfortunately its not an improvement. 

Ok
easbar: Hm, after merging master (no more storage.optimize()) the edge-based CH preparation got really slow in this branch (too many lazy updates). Will have to investigate..

Patch:
diff --git a/core/src/main/java/com/graphhopper/apache/commons/collections/IntDoubleBinaryHeap.java b/core/src/main/java/com/graphhopper/apache/commons/collections/IntFloatBinaryHeap.java
similarity index 78%
rename from core/src/main/java/com/graphhopper/apache/commons/collections/IntDoubleBinaryHeap.java
rename to core/src/main/java/com/graphhopper/apache/commons/collections/IntFloatBinaryHeap.java
index 3f3a72b48..0c96232b1 100644
--- a/core/src/main/java/com/graphhopper/apache/commons/collections/IntDoubleBinaryHeap.java
+++ b/core/src/main/java/com/graphhopper/apache/commons/collections/IntFloatBinaryHeap.java
@@ -15,29 +15,26 @@
  */
 package com.graphhopper.apache.commons.collections;
 
-import com.graphhopper.coll.BinHeapWrapper;
-
 import java.util.Arrays;
 import java.util.NoSuchElementException;
 
 /**
  * This class is a partial copy of the class org.apache.commons.collections.BinaryHeap for
- * just the min heap and primitive, sorted double keys and associated int elements.
+ * just the min heap and primitive, sorted float keys and associated int elements.
  * <p>
  * The library can be found here: https://commons.apache.org/proper/commons-collections/
  */
-public class IntDoubleBinaryHeap implements BinHeapWrapper<Number, Integer> {
-
+public class IntFloatBinaryHeap {
     private static final int GROW_FACTOR = 2;
     private int size;
     private int[] elements;
     private float[] keys;
 
-    public IntDoubleBinaryHeap() {
+    public IntFloatBinaryHeap() {
         this(1000);
     }
 
-    public IntDoubleBinaryHeap(int initialCapacity) {
+    public IntFloatBinaryHeap(int initialCapacity) {
         //+1 as element 0 is noop
         elements = new int[initialCapacity + 1];
         keys = new float[initialCapacity + 1];
@@ -50,12 +47,7 @@ public class IntDoubleBinaryHeap implements BinHeapWrapper<Number, Integer> {
         return elements.length == size + 1;
     }
 
-    @Override
-    public void update(Number key, Integer element) {
-        update_(key.doubleValue(), element);
-    }
-
-    public void update_(double key, int element) {
+    public void update(double key, int element) {
         int i;
         // we have no clue about the element order, so we need to search the full array
         for (i = 1; i <= size; i++) {
@@ -75,14 +67,9 @@ public class IntDoubleBinaryHeap implements BinHeapWrapper<Number, Integer> {
         }
     }
 
-    @Override
-    public void insert(Number key, Integer element) {
-        insert_(key.doubleValue(), element);
-    }
-
-    public void insert_(double key, int element) {
+    public void insert(double key, int element) {
         if (isFull()) {
-            ensureCapacity((int) (elements.length * GROW_FACTOR));
+            ensureCapacity(elements.length * GROW_FACTOR);
         }
 
         size++;
@@ -91,12 +78,7 @@ public class IntDoubleBinaryHeap implements BinHeapWrapper<Number, Integer> {
         percolateUpMinHeap(size);
     }
 
-    @Override
-    public Integer peekElement() {
-        return peek_element();
-    }
-
-    public int peek_element() {
+    public int peekElement() {
         if (isEmpty()) {
             throw new NoSuchElementException("Heap is empty. Cannot peek element.");
         } else {
@@ -104,25 +86,15 @@ public class IntDoubleBinaryHeap implements BinHeapWrapper<Number, Integer> {
         }
     }
 
-    @Override
-    public Number peekKey() {
-        return peek_key();
-    }
-
-    public float peek_key() {
+    public float peekKey() {
         if (isEmpty())
             throw new NoSuchElementException("Heap is empty. Cannot peek key.");
         else
             return keys[1];
     }
 
-    @Override
-    public Integer pollElement() {
-        return poll_element();
-    }
-
-    public int poll_element() {
-        final int result = peek_element();
+    public int poll() {
+        final int result = peekElement();
         elements[1] = elements[size];
         keys[1] = keys[size];
         size--;
@@ -179,17 +151,14 @@ public class IntDoubleBinaryHeap implements BinHeapWrapper<Number, Integer> {
         keys[hole] = key;
     }
 
-    @Override
     public boolean isEmpty() {
         return size == 0;
     }
 
-    @Override
     public int getSize() {
         return size;
     }
 
-    @Override
     public void clear() {
         trimTo(0);
     }
@@ -201,10 +170,9 @@ public class IntDoubleBinaryHeap implements BinHeapWrapper<Number, Integer> {
         Arrays.fill(elements, toSize, size + 1, 0);
     }
 
-    @Override
     public void ensureCapacity(int capacity) {
         if (capacity < size) {
-            throw new IllegalStateException("IntDoubleBinaryHeap contains too many elements to fit in new capacity.");
+            throw new IllegalStateException("IntFloatBinaryHeap contains too many elements to fit in new capacity.");
         }
 
         elements = Arrays.copyOf(elements, capacity + 1);
diff --git a/core/src/main/java/com/graphhopper/coll/BinHeapWrapper.java b/core/src/main/java/com/graphhopper/coll/BinHeapWrapper.java
deleted file mode 100644
index 3eb732089..000000000
--- a/core/src/main/java/com/graphhopper/coll/BinHeapWrapper.java
+++ /dev/null
@@ -1,43 +0,0 @@
-/*
- *  Licensed to GraphHopper GmbH under one or more contributor
- *  license agreements. See the NOTICE file distributed with this work for
- *  additional information regarding copyright ownership.
- *
- *  GraphHopper GmbH licenses this file to you under the Apache License,
- *  Version 2.0 (the "License"); you may not use this file except in
- *  compliance with the License. You may obtain a copy of the License at
- *
- *       http://www.apache.org/licenses/LICENSE-2.0
- *
- *  Unless required by applicable law or agreed to in writing, software
- *  distributed under the License is distributed on an "AS IS" BASIS,
- *  WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
- *  See the License for the specific language governing permissions and
- *  limitations under the License.
- */
-package com.graphhopper.coll;
-
-/**
- * @author Peter Karich
- */
-public interface BinHeapWrapper<K, E> {
-
-    void update(K key, E element);
-
-    void insert(K key, E element);
-
-    boolean isEmpty();
-
-    int getSize();
-
-    E peekElement();
-
-    K peekKey();
-
-    E pollElement();
-
-    // not necessary? V pollValue();
-    void clear();
-
-    void ensureCapacity(int size);
-}
diff --git a/core/src/main/java/com/graphhopper/coll/MinHeapWithUpdate.java b/core/src/main/java/com/graphhopper/coll/MinHeapWithUpdate.java
new file mode 100644
index 000000000..3ad28da34
--- /dev/null
+++ b/core/src/main/java/com/graphhopper/coll/MinHeapWithUpdate.java
@@ -0,0 +1,194 @@
+/*
+ *  Licensed to GraphHopper GmbH under one or more contributor
+ *  license agreements. See the NOTICE file distributed with this work for
+ *  additional information regarding copyright ownership.
+ *
+ *  GraphHopper GmbH licenses this file to you under the Apache License,
+ *  Version 2.0 (the "License"); you may not use this file except in
+ *  compliance with the License. You may obtain a copy of the License at
+ *
+ *       http://www.apache.org/licenses/LICENSE-2.0
+ *
+ *  Unless required by applicable law or agreed to in writing, software
+ *  distributed under the License is distributed on an "AS IS" BASIS,
+ *  WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ *  See the License for the specific language governing permissions and
+ *  limitations under the License.
+ */
+
+package com.graphhopper.coll;
+
+import com.graphhopper.apache.commons.collections.IntFloatBinaryHeap;
+
+import java.util.Arrays;
+
+/**
+ * A minimum heap implemented using a binary tree (https://en.wikipedia.org/wiki/Binary_heap). Besides the tree and the
+ * elements' values this heap also keeps track of the positions of the elements in the tree.
+ * This requires additional book-keeping when doing pushes/polls, but allows for an efficient update operation.
+ * For the same reason the heap has a fixed memory size that is determined in the constructor and the inserted element
+ * may not exceed a certain range.
+ * todo: strictly speaking the heap could automatically grow/shrink as long as the range of legal ids stays fixed, but
+ * for simplicity the heap has a fixed size for now.
+ * <p>
+ * This class is very similar to {@link IntFloatBinaryHeap}, but compared to this has an efficient update operation.
+ * In turn it is (much) less memory-efficient when the heap is used for a small number of elements from a large range.
+ */
+public class MinHeapWithUpdate {
+    private static final int NOT_PRESENT = -1;
+    private final int[] tree;
+    private final int[] positions;
+    private final float[] vals;
+    private final int max;
+    private int size;
+
+    /**
+     * @param elements the number of elements that can be stored in this heap. Currently the heap cannot be resized or
+     *                 shrunk/trimmed after initial creation. elements-1 is the maximum id that can be stored in this
+     *                 heap
+     */
+    public MinHeapWithUpdate(int elements) {
+        // we use an offset of one to make the arithmetic a bit simpler/more efficient, the 0th elements are not used!
+        tree = new int[elements + 1];
+        positions = new int[elements + 1];
+        Arrays.fill(positions, NOT_PRESENT);
+        vals = new float[elements + 1];
+        vals[0] = Float.NEGATIVE_INFINITY;
+        this.max = elements;
+    }
+
+    public int size() {
+        return size;
+    }
+
+    public boolean isEmpty() {
+        return size == 0;
+    }
+
+    /**
+     * Adds an element to the heap, the given id must not exceed the size specified in the constructor. Its illegal
+     * to push the same id twice (unless it was polled/removed before). To update the value of an id contained in the
+     * heap use the {@link #update} method.
+     */
+    public void push(int id, float value) {
+        checkIdInRange(id);
+        if (size == max)
+            throw new IllegalStateException("Cannot push anymore, the heap is already full. size: " + size);
+        if (contains(id))
+            throw new IllegalStateException("Element with id: " + id + " was pushed already, you need to use the update method if you want to change its value");
+        size++;
+        tree[size] = id;
+        positions[id] = size;
+        vals[size] = value;
+        percolateUp(size);
+    }
+
+    /**
+     * @return true if the heap contains an element with the given id
+     */
+    public boolean contains(int id) {
+        checkIdInRange(id);
+        return positions[id] != NOT_PRESENT;
+    }
+
+    /**
+     * Updates the element with the given id. The complexity of this method is O(log(N)), just like push/poll.
+     * Its illegal to update elements that are not contained in the heap. Use {@link #contains} to check the existence
+     * of an id.
+     */
+    public void update(int id, float value) {
+        checkIdInRange(id);
+        int index = positions[id];
+        if (index < 0)
+            throw new IllegalStateException("The heap does not contain: " + id + ". Use the contains method to check this before calling update");
+        float prev = vals[index];
+        vals[index] = value;
+        if (value > prev)
+            percolateDown(index);
+        else if (value < prev)
+            percolateUp(index);
+    }
+
+    /**
+     * @return the id of the next element to be polled, i.e. the same as calling poll() without removing the element
+     */
+    public int peekId() {
+        return tree[1];
+    }
+
+    /**
+     * @return the value of the next element to be polled
+     */
+    public float peekValue() {
+        return vals[1];
+    }
+
+    /**
+     * Extracts the element with minimum value from the heap
+     */
+    public int poll() {
+        int id = peekId();
+        tree[1] = tree[size];
+        vals[1] = vals[size];
+        positions[tree[1]] = 1;
+        positions[id] = NOT_PRESENT;
+        size--;
+        percolateDown(1);
+        return id;
+    }
+
+    public void clear() {
+        for (int i = 1; i <= size; i++)
+            positions[tree[i]] = NOT_PRESENT;
+        size = 0;
+    }
+
+    private void percolateUp(int index) {
+        assert index != 0;
+        if (index == 1)
+            return;
+        final int el = tree[index];
+        final float val = vals[index];
+        // the finish condition (index==0) is covered here automatically because we set vals[0]=-inf
+        while (val < vals[index >> 1]) {
+            int parent = index >> 1;
+            tree[index] = tree[parent];
+            vals[index] = vals[parent];
+            positions[tree[index]] = index;
+            index = parent;
+        }
+        tree[index] = el;
+        vals[index] = val;
+        positions[tree[index]] = index;
+    }
+
+    private void percolateDown(int index) {
+        if (size == 0)
+            return;
+        assert index > 0;
+        assert index <= size;
+        final int el = tree[index];
+        final float val = vals[index];
+        while (index << 1 <= size) {
+            int child = index << 1;
+            if (child != size && vals[child + 1] < vals[child])
+                // use the second child if it exists and has a smaller value
+                child++;
+            if (vals[child] >= val)
+                break;
+            tree[index] = tree[child];
+            vals[index] = vals[child];
+            positions[tree[index]] = index;
+            index = child;
+        }
+        tree[index] = el;
+        vals[index] = val;
+        positions[tree[index]] = index;
+    }
+
+    private void checkIdInRange(int id) {
+        if (id < 0 || id >= max)
+            throw new IllegalArgumentException("Illegal id: " + id + ", legal range: [0, " + max + "[");
+    }
+
+}
diff --git a/core/src/main/java/com/graphhopper/routing/AlternativeRouteEdgeCH.java b/core/src/main/java/com/graphhopper/routing/AlternativeRouteEdgeCH.java
index 7fcdfe728..04f54a78c 100644
--- a/core/src/main/java/com/graphhopper/routing/AlternativeRouteEdgeCH.java
+++ b/core/src/main/java/com/graphhopper/routing/AlternativeRouteEdgeCH.java
@@ -245,6 +245,9 @@ public class AlternativeRouteEdgeCH extends DijkstraBidirectionEdgeCHNoSOD {
             path.addEdge(edge.getEdge());
         }
         Iterator<EdgeIteratorState> uvtPathI = uvtPath.calcEdges().iterator();
+        if (!uvtPathI.hasNext()) { // presumably v == t, has been known to happen, no test yet
+            return suvPath;
+        }
         uvtPathI.next(); // skip u-v edge
         uvtPathI.forEachRemaining(edge -> path.addEdge(edge.getEdge()));
         path.setEndNode(uvtPath.getEndNode());
diff --git a/core/src/main/java/com/graphhopper/routing/DijkstraOneToMany.java b/core/src/main/java/com/graphhopper/routing/DijkstraOneToMany.java
index 7814d4fec..169b23d26 100644
--- a/core/src/main/java/com/graphhopper/routing/DijkstraOneToMany.java
+++ b/core/src/main/java/com/graphhopper/routing/DijkstraOneToMany.java
@@ -18,7 +18,7 @@
 package com.graphhopper.routing;
 
 import com.carrotsearch.hppc.IntArrayList;
-import com.graphhopper.apache.commons.collections.IntDoubleBinaryHeap;
+import com.graphhopper.apache.commons.collections.IntFloatBinaryHeap;
 import com.graphhopper.routing.util.TraversalMode;
 import com.graphhopper.routing.weighting.Weighting;
 import com.graphhopper.storage.Graph;
@@ -41,7 +41,7 @@ public class DijkstraOneToMany extends AbstractRoutingAlgorithm {
     protected double[] weights;
     private int[] parents;
     private int[] edgeIds;
-    private IntDoubleBinaryHeap heap;
+    private IntFloatBinaryHeap heap;
     private int visitedNodes;
     private boolean doClear = true;
     private int endNode;
@@ -61,7 +61,7 @@ public class DijkstraOneToMany extends AbstractRoutingAlgorithm {
 
         Arrays.fill(weights, Double.MAX_VALUE);
 
-        heap = new IntDoubleBinaryHeap(1000);
+        heap = new IntFloatBinaryHeap(1000);
         changedNodes = new IntArrayListWithCap();
     }
 
@@ -149,12 +149,12 @@ public class DijkstraOneToMany extends AbstractRoutingAlgorithm {
             if (heap.isEmpty() || isMaxVisitedNodesExceeded())
                 return NOT_FOUND;
 
-            currNode = heap.poll_element();
+            currNode = heap.poll();
         }
 
         visitedNodes = 0;
 
-        // we call 'finished' before heap.peek_element but this would add unnecessary overhead for this special case so we do it outside of the loop
+        // we call 'finished' before heap.peekElement but this would add unnecessary overhead for this special case so we do it outside of the loop
         if (finished()) {
             // then we need a small workaround for special cases see #707
             if (heap.isEmpty())
@@ -181,14 +181,14 @@ public class DijkstraOneToMany extends AbstractRoutingAlgorithm {
                 if (w == Double.MAX_VALUE) {
                     parents[adjNode] = currNode;
                     weights[adjNode] = tmpWeight;
-                    heap.insert_(tmpWeight, adjNode);
+                    heap.insert(tmpWeight, adjNode);
                     changedNodes.add(adjNode);
                     edgeIds[adjNode] = iter.getEdge();
 
                 } else if (w > tmpWeight) {
                     parents[adjNode] = currNode;
                     weights[adjNode] = tmpWeight;
-                    heap.update_(tmpWeight, adjNode);
+                    heap.update(tmpWeight, adjNode);
                     changedNodes.add(adjNode);
                     edgeIds[adjNode] = iter.getEdge();
                 }
@@ -198,11 +198,11 @@ public class DijkstraOneToMany extends AbstractRoutingAlgorithm {
                 return NOT_FOUND;
 
             // calling just peek and not poll is important if the next query is cached
-            currNode = heap.peek_element();
+            currNode = heap.peekElement();
             if (finished())
                 return currNode;
 
-            heap.poll_element();
+            heap.poll();
         }
     }
 
diff --git a/core/src/main/java/com/graphhopper/routing/ch/EdgeBasedWitnessPathSearcher.java b/core/src/main/java/com/graphhopper/routing/ch/EdgeBasedWitnessPathSearcher.java
index 2a0862ef1..49c2f2a84 100644
--- a/core/src/main/java/com/graphhopper/routing/ch/EdgeBasedWitnessPathSearcher.java
+++ b/core/src/main/java/com/graphhopper/routing/ch/EdgeBasedWitnessPathSearcher.java
@@ -20,7 +20,7 @@ package com.graphhopper.routing.ch;
 import com.carrotsearch.hppc.IntArrayList;
 import com.carrotsearch.hppc.IntObjectHashMap;
 import com.carrotsearch.hppc.IntObjectMap;
-import com.graphhopper.apache.commons.collections.IntDoubleBinaryHeap;
+import com.graphhopper.apache.commons.collections.IntFloatBinaryHeap;
 import com.graphhopper.util.EdgeIterator;
 import com.graphhopper.util.GHUtility;
 import com.graphhopper.util.PMap;
@@ -94,7 +94,7 @@ public class EdgeBasedWitnessPathSearcher {
     private boolean[] isPathToCenters;
     private IntObjectMap<CHEntry> initialEntryParents;
     private IntArrayList changedEdges;
-    private IntDoubleBinaryHeap dijkstraHeap;
+    private IntFloatBinaryHeap dijkstraHeap;
 
     // we keep track of the average number and distribution width of settled edges during the last searches to estimate
     // an appropriate maximum of settled edges for the next searches
@@ -197,14 +197,14 @@ public class EdgeBasedWitnessPathSearcher {
                 // that could yield a bridge-path
                 break;
             }
-            final int currKey = dijkstraHeap.peek_element();
+            final int currKey = dijkstraHeap.peekElement();
             if (weights[currKey] > bestPathWeight) {
                 // just reaching this edge is more expensive than the best path found so far including the turn costs
                 // to reach the target edge -> we can stop
                 // important: we only peeked so far, so we keep the entry for future searches
                 break;
             }
-            dijkstraHeap.poll_element();
+            dijkstraHeap.poll();
             numPolledEdges++;
             currentBatchStats.numPolledEdges++;
             totalStats.numPolledEdges++;
@@ -237,13 +237,13 @@ public class EdgeBasedWitnessPathSearcher {
                 if (!EdgeIterator.Edge.isValid(edges[key])) {
                     setEntry(key, iter, weight, currKey, isPathToCenter);
                     changedEdges.add(key);
-                    dijkstraHeap.insert_(weight, key);
+                    dijkstraHeap.insert(weight, key);
                     if (!isZeroWeightLoop) {
                         updateBestPath(targetNode, targetEdge, key);
                     }
                 } else if (weight < weights[key]) {
                     updateEntry(key, iter, weight, currKey, isPathToCenter);
-                    dijkstraHeap.update_(weight, key);
+                    dijkstraHeap.update(weight, key);
                     if (!isZeroWeightLoop) {
                         updateBestPath(targetNode, targetEdge, key);
                     }
@@ -317,7 +317,7 @@ public class EdgeBasedWitnessPathSearcher {
     private void initCollections() {
         initialEntryParents = new IntObjectHashMap<>(10);
         changedEdges = new IntArrayList(1000);
-        dijkstraHeap = new IntDoubleBinaryHeap(1000);
+        dijkstraHeap = new IntFloatBinaryHeap(1000);
     }
 
     private void setInitialEntries(int sourceNode, int sourceEdge, int centerNode) {
@@ -370,7 +370,7 @@ public class EdgeBasedWitnessPathSearcher {
             if (isPathToCenters[key]) {
                 numPathsToCenter++;
             }
-            dijkstraHeap.insert_(weights[key], key);
+            dijkstraHeap.insert(weights[key], key);
         }
     }
 
diff --git a/core/src/main/java/com/graphhopper/routing/ch/NodeBasedWitnessPathSearcher.java b/core/src/main/java/com/graphhopper/routing/ch/NodeBasedWitnessPathSearcher.java
index ba475b0df..9db7ed359 100644
--- a/core/src/main/java/com/graphhopper/routing/ch/NodeBasedWitnessPathSearcher.java
+++ b/core/src/main/java/com/graphhopper/routing/ch/NodeBasedWitnessPathSearcher.java
@@ -18,7 +18,7 @@
 package com.graphhopper.routing.ch;
 
 import com.carrotsearch.hppc.IntArrayList;
-import com.graphhopper.apache.commons.collections.IntDoubleBinaryHeap;
+import com.graphhopper.apache.commons.collections.IntFloatBinaryHeap;
 import com.graphhopper.routing.DijkstraOneToMany;
 import com.graphhopper.util.EdgeIterator;
 import com.graphhopper.util.Helper;
@@ -43,7 +43,7 @@ public class NodeBasedWitnessPathSearcher {
     protected double[] weights;
     private int[] parents;
     private int[] edgeIds;
-    private IntDoubleBinaryHeap heap;
+    private IntFloatBinaryHeap heap;
     private int ignoreNode = -1;
     private int visitedNodes;
     private boolean doClear = true;
@@ -68,7 +68,7 @@ public class NodeBasedWitnessPathSearcher {
         weights = new double[graph.getNodes()];
         Arrays.fill(weights, Double.MAX_VALUE);
 
-        heap = new IntDoubleBinaryHeap(1000);
+        heap = new IntFloatBinaryHeap(1000);
         changedNodes = new IntArrayList();
     }
 
@@ -116,12 +116,12 @@ public class NodeBasedWitnessPathSearcher {
             if (heap.isEmpty() || isMaxVisitedNodesExceeded())
                 return NOT_FOUND;
 
-            currNode = heap.poll_element();
+            currNode = heap.poll();
         }
 
         visitedNodes = 0;
 
-        // we call 'finished' before heap.peek_element but this would add unnecessary overhead for this special case so we do it outside of the loop
+        // we call 'finished' before heap.peekElement but this would add unnecessary overhead for this special case so we do it outside of the loop
         if (finished()) {
             // then we need a small workaround for special cases see #707
             if (heap.isEmpty())
@@ -146,13 +146,13 @@ public class NodeBasedWitnessPathSearcher {
                 if (w == Double.MAX_VALUE) {
                     parents[adjNode] = currNode;
                     weights[adjNode] = tmpWeight;
-                    heap.insert_(tmpWeight, adjNode);
+                    heap.insert(tmpWeight, adjNode);
                     changedNodes.add(adjNode);
                     edgeIds[adjNode] = iter.getEdge();
                 } else if (w > tmpWeight) {
                     parents[adjNode] = currNode;
                     weights[adjNode] = tmpWeight;
-                    heap.update_(tmpWeight, adjNode);
+                    heap.update(tmpWeight, adjNode);
                     changedNodes.add(adjNode);
                     edgeIds[adjNode] = iter.getEdge();
                 }
@@ -162,11 +162,11 @@ public class NodeBasedWitnessPathSearcher {
                 return NOT_FOUND;
 
             // calling just peek and not poll is important if the next query is cached
-            currNode = heap.peek_element();
+            currNode = heap.peekElement();
             if (finished())
                 return currNode;
 
-            heap.poll_element();
+            heap.poll();
         }
     }
 
diff --git a/core/src/main/java/com/graphhopper/routing/ch/PrepareContractionHierarchies.java b/core/src/main/java/com/graphhopper/routing/ch/PrepareContractionHierarchies.java
index 5f44bd310..60a991b9e 100644
--- a/core/src/main/java/com/graphhopper/routing/ch/PrepareContractionHierarchies.java
+++ b/core/src/main/java/com/graphhopper/routing/ch/PrepareContractionHierarchies.java
@@ -19,7 +19,7 @@ package com.graphhopper.routing.ch;
 
 import com.carrotsearch.hppc.IntHashSet;
 import com.carrotsearch.hppc.IntSet;
-import com.graphhopper.coll.GHTreeMapComposed;
+import com.graphhopper.coll.MinHeapWithUpdate;
 import com.graphhopper.routing.util.AbstractAlgoPreparation;
 import com.graphhopper.routing.util.TraversalMode;
 import com.graphhopper.routing.weighting.Weighting;
@@ -68,8 +68,7 @@ public class PrepareContractionHierarchies extends AbstractAlgoPreparation {
     private PrepareCHEdgeExplorer disconnectExplorer;
     private int maxLevel;
     // nodes with highest priority come last
-    private GHTreeMapComposed sortedNodes;
-    private float[] oldPriorities;
+    private MinHeapWithUpdate sortedNodes;
     private PMap pMap = new PMap();
     private int checkCounter;
 
@@ -162,14 +161,10 @@ public class PrepareContractionHierarchies extends AbstractAlgoPreparation {
         maxLevel = prepareGraph.getNodes();
         allEdgeExplorer = prepareGraph.createAllEdgeExplorer();
         disconnectExplorer = prepareGraph.createAllEdgeExplorer();
-
-        // Use an alternative to PriorityQueue as it has some advantages:
-        //   1. Gets automatically smaller if less entries are stored => less total RAM used.
-        //      Important because Graph is increasing until the end.
-        //   2. is slightly faster
-        //   but we need the additional oldPriorities array to keep the old value which is necessary for the update method
-        sortedNodes = new GHTreeMapComposed();
-        oldPriorities = new float[prepareGraph.getNodes()];
+        // we need a memory-efficient priority queue with an efficient update method
+        // TreeMap is not memory-efficient and PriorityQueue does not support an efficient update method
+        // (and is not memory efficient either)
+        sortedNodes = new MinHeapWithUpdate(prepareGraph.getNodes());
         nodeContractor.initFromGraph();
     }
 
@@ -187,8 +182,8 @@ public class PrepareContractionHierarchies extends AbstractAlgoPreparation {
         for (int node = 0; node < nodes; node++) {
             if (prepareGraph.getLevel(node) != maxLevel)
                 continue;
-            float priority = oldPriorities[node] = calculatePriority(node);
-            sortedNodes.insert(node, priority);
+            float priority = calculatePriority(node);
+            sortedNodes.push(node, priority);
         }
         periodicUpdateSW.stop();
     }
@@ -201,7 +196,7 @@ public class PrepareContractionHierarchies extends AbstractAlgoPreparation {
         updatePrioritiesOfRemainingNodes();
         logger.info("Finished building queue, took: {}s", sw.stop().getSeconds());
         nodeContractor.prepareContraction();
-        final int initSize = sortedNodes.getSize();
+        final int initSize = sortedNodes.size();
         int level = 0;
         checkCounter = 0;
         final long logSize = params.getLogMessagesPercentage() == 0
@@ -245,14 +240,14 @@ public class PrepareContractionHierarchies extends AbstractAlgoPreparation {
             }
 
             checkCounter++;
-            int polledNode = sortedNodes.pollKey();
+            int polledNode = sortedNodes.poll();
 
-            if (!sortedNodes.isEmpty() && sortedNodes.getSize() < lastNodesLazyUpdates) {
+            if (!sortedNodes.isEmpty() && sortedNodes.size() < lastNodesLazyUpdates) {
                 lazyUpdateSW.start();
-                float priority = oldPriorities[polledNode] = calculatePriority(polledNode);
+                float priority = calculatePriority(polledNode);
                 if (priority > sortedNodes.peekValue()) {
                     // current node got more important => insert as new value and contract it later
-                    sortedNodes.insert(polledNode, priority);
+                    sortedNodes.push(polledNode, priority);
                     lazyUpdateSW.stop();
                     continue;
                 }
@@ -263,7 +258,7 @@ public class PrepareContractionHierarchies extends AbstractAlgoPreparation {
             contractNode(polledNode, level);
             level++;
 
-            if (sortedNodes.getSize() < nodesToAvoidContract)
+            if (sortedNodes.size() < nodesToAvoidContract)
                 // skipped nodes are already set to maxLevel
                 break;
 
@@ -277,12 +272,9 @@ public class PrepareContractionHierarchies extends AbstractAlgoPreparation {
 
                 if (neighborUpdate && !updatedNeighbors.contains(nn) && rand.nextInt(100) < params.getNeighborUpdatePercentage()) {
                     neighborUpdateSW.start();
-                    float oldPrio = oldPriorities[nn];
-                    float priority = oldPriorities[nn] = calculatePriority(nn);
-                    if (priority != oldPrio) {
-                        sortedNodes.update(nn, oldPrio, priority);
-                        updatedNeighbors.add(nn);
-                    }
+                    float priority = calculatePriority(nn);
+                    sortedNodes.update(nn, priority);
+                    updatedNeighbors.add(nn);
                     neighborUpdateSW.stop();
                 }
 
@@ -351,7 +343,7 @@ public class PrepareContractionHierarchies extends AbstractAlgoPreparation {
         logger.info(String.format(Locale.ROOT,
                 "%s, nodes: %10s, shortcuts: %10s, updates: %2d, checked-nodes: %10s, %s, %s, %s",
                 (isEdgeBased() ? "edge" : "node"),
-                nf(sortedNodes.getSize()),
+                nf(sortedNodes.size()),
                 nf(nodeContractor.getAddedShortcutsCount()),
                 updateCounter,
                 nf(checkCounter),
@@ -431,7 +423,6 @@ public class PrepareContractionHierarchies extends AbstractAlgoPreparation {
     private void _close() {
         nodeContractor.close();
         sortedNodes = null;
-        oldPriorities = null;
     }
 
     void close() {


Test patch:
diff --git a/core/src/test/java/com/graphhopper/apache/commons/collections/IntDoubleBinaryHeapTest.java b/core/src/test/java/com/graphhopper/apache/commons/collections/IntDoubleBinaryHeapTest.java
deleted file mode 100644
index 51e708026..000000000
--- a/core/src/test/java/com/graphhopper/apache/commons/collections/IntDoubleBinaryHeapTest.java
+++ /dev/null
@@ -1,14 +0,0 @@
-package com.graphhopper.apache.commons.collections;
-
-import com.graphhopper.coll.AbstractBinHeapTest;
-import com.graphhopper.coll.BinHeapWrapper;
-
-/**
- * @author Peter Karich
- */
-public class IntDoubleBinaryHeapTest extends AbstractBinHeapTest {
-    @Override
-    public BinHeapWrapper<Number, Integer> createHeap(int capacity) {
-        return new IntDoubleBinaryHeap(capacity);
-    }
-}
diff --git a/core/src/test/java/com/graphhopper/apache/commons/collections/IntFloatBinaryHeapTest.java b/core/src/test/java/com/graphhopper/apache/commons/collections/IntFloatBinaryHeapTest.java
new file mode 100644
index 000000000..e2d51ba0c
--- /dev/null
+++ b/core/src/test/java/com/graphhopper/apache/commons/collections/IntFloatBinaryHeapTest.java
@@ -0,0 +1,76 @@
+package com.graphhopper.apache.commons.collections;
+
+import com.carrotsearch.hppc.IntArrayList;
+import com.graphhopper.coll.BinaryHeapTestInterface;
+import org.junit.jupiter.api.Test;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+/**
+ * @author Peter Karich
+ * @author easbar
+ */
+public class IntFloatBinaryHeapTest implements BinaryHeapTestInterface {
+
+    private IntFloatBinaryHeap heap;
+
+    @Override
+    public void create(int capacity) {
+        heap = new IntFloatBinaryHeap(capacity);
+    }
+
+    @Override
+    public int size() {
+        return heap.getSize();
+    }
+
+    @Override
+    public boolean isEmpty() {
+        return heap.isEmpty();
+    }
+
+    @Override
+    public void push(int id, float val) {
+        heap.insert(val, id);
+    }
+
+    @Override
+    public int peekId() {
+        return heap.peekElement();
+    }
+
+    @Override
+    public float peekVal() {
+        return heap.peekKey();
+    }
+
+    @Override
+    public void update(int id, float val) {
+        heap.update(val, id);
+    }
+
+    @Override
+    public int poll() {
+        return heap.poll();
+    }
+
+    @Override
+    public void clear() {
+        heap.clear();
+    }
+
+    @Test
+    public void growIfNeeded() {
+        create(3);
+        push(4, 1.6f);
+        push(8, 1.8f);
+        push(12, 0.7f);
+        push(5, 1.2f);
+        assertEquals(4, size());
+        IntArrayList elements = new IntArrayList();
+        while (!isEmpty()) {
+            elements.add(poll());
+        }
+        assertEquals(IntArrayList.from(12, 5, 4, 8), elements);
+    }
+}
diff --git a/core/src/test/java/com/graphhopper/coll/AbstractBinHeapTest.java b/core/src/test/java/com/graphhopper/coll/AbstractBinHeapTest.java
deleted file mode 100644
index fda5ebaef..000000000
--- a/core/src/test/java/com/graphhopper/coll/AbstractBinHeapTest.java
+++ /dev/null
@@ -1,140 +0,0 @@
-/*
- *  Licensed to GraphHopper GmbH under one or more contributor
- *  license agreements. See the NOTICE file distributed with this work for
- *  additional information regarding copyright ownership.
- *
- *  GraphHopper GmbH licenses this file to you under the Apache License,
- *  Version 2.0 (the "License"); you may not use this file except in
- *  compliance with the License. You may obtain a copy of the License at
- *
- *       http://www.apache.org/licenses/LICENSE-2.0
- *
- *  Unless required by applicable law or agreed to in writing, software
- *  distributed under the License is distributed on an "AS IS" BASIS,
- *  WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
- *  See the License for the specific language governing permissions and
- *  limitations under the License.
- */
-package com.graphhopper.coll;
-
-import com.graphhopper.routing.SPTEntry;
-import com.graphhopper.util.EdgeIterator;
-import org.junit.Test;
-
-import java.util.PriorityQueue;
-import java.util.Random;
-
-import static org.junit.Assert.assertEquals;
-
-/**
- * @author Peter Karich
- */
-public abstract class AbstractBinHeapTest {
-    public abstract BinHeapWrapper<Number, Integer> createHeap(int capacity);
-
-    @Test
-    public void test0() {
-        BinHeapWrapper<Number, Integer> binHeap = createHeap(100);
-        binHeap.insert(123, 0);
-        assertEquals(123, binHeap.peekKey().intValue());
-        assertEquals(1, binHeap.getSize());
-
-        binHeap.update(12, 0);
-        assertEquals(12, binHeap.peekKey().intValue());
-        assertEquals(1, binHeap.getSize());
-    }
-
-    @Test
-    public void testBasic() {
-        BinHeapWrapper<Number, Integer> binHeap = createHeap(100);
-        binHeap.insert(20, 1);
-        binHeap.insert(123, 2);
-        binHeap.insert(120, 3);
-        binHeap.insert(130, 4);
-        binHeap.insert(80, 5);
-
-        assertEquals(1, (int) binHeap.pollElement());
-        assertEquals(5, (int) binHeap.pollElement());
-        assertEquals(3, (int) binHeap.pollElement());
-        assertEquals(2, (int) binHeap.pollElement());
-        assertEquals(1, (int) binHeap.getSize());
-    }
-
-    @Test
-    public void testClear() {
-        BinHeapWrapper<Number, Integer> binHeap = createHeap(100);
-        binHeap.insert(20, 1);
-        binHeap.insert(123, 2);
-        assertEquals(2, binHeap.getSize());
-        binHeap.clear();
-
-        assertEquals(0, binHeap.getSize());
-        binHeap.insert(123, 2);
-        assertEquals(1, binHeap.getSize());
-        assertEquals(2, (int) binHeap.pollElement());
-    }
-
-    @Test
-    public void testSpreading() {
-        BinHeapWrapper<Number, Integer> binHeap = createHeap(100);
-        binHeap.insert(100, 101);
-        binHeap.insert(49, 51);
-        binHeap.insert(71, 71);
-        binHeap.insert(29, 31);
-        for (int i = 0; i < 20; i++) {
-            binHeap.insert(i * 10, i * 11);
-        }
-        binHeap.insert(59, 61);
-        binHeap.insert(160, 161);
-
-        assertEquals(26, binHeap.getSize());
-        assertEquals(0, binHeap.pollElement().intValue());
-        assertEquals(11, binHeap.pollElement().intValue());
-        assertEquals(22, binHeap.pollElement().intValue());
-        assertEquals(31, binHeap.pollElement().intValue());
-        assertEquals(33, binHeap.pollElement().intValue());
-        assertEquals(44, binHeap.pollElement().intValue());
-        assertEquals(51, binHeap.pollElement().intValue());
-        assertEquals(55, binHeap.pollElement().intValue());
-        assertEquals(61, binHeap.pollElement().intValue());
-        assertEquals(66, binHeap.pollElement().intValue());
-        assertEquals(77, binHeap.pollElement().intValue());
-        assertEquals(15, binHeap.getSize());
-    }
-
-    @Test
-    public void testRekey() {
-        BinHeapWrapper<Number, Integer> binHeap = createHeap(100);
-        binHeap.insert(20, 1);
-        binHeap.insert(123, 2);
-        binHeap.update(100, 2);
-        binHeap.insert(120, 3);
-        binHeap.insert(130, 4);
-
-        assertEquals(1, (int) binHeap.pollElement());
-        assertEquals(2, (int) binHeap.pollElement());
-        assertEquals(2, binHeap.getSize());
-    }
-
-    @Test
-    public void testSize() {
-        PriorityQueue<SPTEntry> juQueue = new PriorityQueue<>(100);
-        BinHeapWrapper<Number, Integer> binHeap = createHeap(100);
-
-        Random rand = new Random(1);
-        int N = 1000;
-        for (int i = 0; i < N; i++) {
-            int val = rand.nextInt();
-            binHeap.insert(val, i);
-            juQueue.add(new SPTEntry(EdgeIterator.NO_EDGE, i, val));
-        }
-
-        assertEquals(juQueue.size(), binHeap.getSize());
-
-        for (int i = 0; i < N; i++) {
-            assertEquals(juQueue.poll().adjNode, binHeap.pollElement(), 1e-5);
-        }
-
-        assertEquals(binHeap.getSize(), 0);
-    }
-}
diff --git a/core/src/test/java/com/graphhopper/coll/BinaryHeapTestInterface.java b/core/src/test/java/com/graphhopper/coll/BinaryHeapTestInterface.java
new file mode 100644
index 000000000..596e2a52c
--- /dev/null
+++ b/core/src/test/java/com/graphhopper/coll/BinaryHeapTestInterface.java
@@ -0,0 +1,212 @@
+/*
+ *  Licensed to GraphHopper GmbH under one or more contributor
+ *  license agreements. See the NOTICE file distributed with this work for
+ *  additional information regarding copyright ownership.
+ *
+ *  GraphHopper GmbH licenses this file to you under the Apache License,
+ *  Version 2.0 (the "License"); you may not use this file except in
+ *  compliance with the License. You may obtain a copy of the License at
+ *
+ *       http://www.apache.org/licenses/LICENSE-2.0
+ *
+ *  Unless required by applicable law or agreed to in writing, software
+ *  distributed under the License is distributed on an "AS IS" BASIS,
+ *  WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ *  See the License for the specific language governing permissions and
+ *  limitations under the License.
+ */
+
+package com.graphhopper.coll;
+
+import com.carrotsearch.hppc.IntArrayList;
+import com.carrotsearch.hppc.IntHashSet;
+import com.carrotsearch.hppc.IntSet;
+import org.junit.jupiter.api.Test;
+
+import java.util.PriorityQueue;
+import java.util.Random;
+
+import static org.junit.jupiter.api.Assertions.*;
+
+public interface BinaryHeapTestInterface {
+
+    void create(int capacity);
+
+    int size();
+
+    boolean isEmpty();
+
+    void push(int id, float val);
+
+    int peekId();
+
+    float peekVal();
+
+    void update(int id, float val);
+
+    int poll();
+
+    void clear();
+
+    @Test
+    default void testSize() {
+        create(10);
+        assertEquals(0, size());
+        assertTrue(isEmpty());
+        push(9, 3.6f);
+        push(5, 2.3f);
+        push(3, 2.3f);
+        assertEquals(3, size());
+        assertFalse(isEmpty());
+    }
+
+    @Test
+    default void testClear() {
+        create(5);
+        assertTrue(isEmpty());
+        push(3, 1.2f);
+        push(4, 0.3f);
+        assertEquals(2, size());
+        clear();
+        assertTrue(isEmpty());
+
+        push(4, 6.3f);
+        push(1, 2.1f);
+        assertEquals(2, size());
+        assertEquals(1, peekId());
+        assertEquals(2.1f, peekVal());
+        assertEquals(1, poll());
+        assertEquals(4, poll());
+        assertTrue(isEmpty());
+    }
+
+    @Test
+    default void testPeek() {
+        create(5);
+        push(4, -1.6f);
+        push(2, 1.3f);
+        push(1, -5.1f);
+        push(3, 0.4f);
+        assertEquals(1, peekId());
+        assertEquals(-5.1f, peekVal(), 1.e-6);
+    }
+
+    @Test
+    default void pushAndPoll() {
+        create(10);
+        push(9, 3.6f);
+        push(5, 2.3f);
+        push(3, 2.3f);
+        assertEquals(3, size());
+        poll();
+        assertEquals(2, size());
+        poll();
+        poll();
+        assertTrue(isEmpty());
+    }
+
+    @Test
+    default void pollSorted() {
+        create(10);
+        push(9, 3.6f);
+        push(5, 2.1f);
+        push(3, 2.3f);
+        push(8, 5.7f);
+        push(7, 2.2f);
+        IntArrayList polled = new IntArrayList();
+        while (!isEmpty()) {
+            polled.add(poll());
+        }
+        assertEquals(IntArrayList.from(5, 7, 3, 9, 8), polled);
+    }
+
+    @Test
+    default void update() {
+        create(10);
+        push(9, 3.6f);
+        push(5, 2.1f);
+        push(3, 2.3f);
+        update(3, 0.1f);
+        assertEquals(3, peekId());
+        update(3, 10.f);
+        assertEquals(5, peekId());
+        update(9, -1.3f);
+        assertEquals(9, peekId());
+        assertEquals(-1.3f, peekVal(), 1.e-6);
+        IntArrayList polled = new IntArrayList();
+        while (!isEmpty()) {
+            polled.add(poll());
+        }
+        assertEquals(IntArrayList.from(9, 5, 3), polled);
+    }
+
+    @Test
+    default void randomPushsThenPolls() {
+        final long seed = System.nanoTime();
+        Random rnd = new Random(seed);
+        int size = 1 + rnd.nextInt(100);
+        PriorityQueue<Entry> pq = new PriorityQueue<>(size);
+        create(size);
+        IntSet set = new IntHashSet();
+        while (pq.size() < size) {
+            int id = rnd.nextInt(size);
+            if (!set.add(id))
+                continue;
+            float val = 100 * rnd.nextFloat();
+            pq.add(new Entry(id, val));
+            push(id, val);
+        }
+        while (!pq.isEmpty()) {
+            Entry entry = pq.poll();
+            assertEquals(entry.val, peekVal());
+            assertEquals(entry.id, poll());
+            assertEquals(pq.size(), size());
+        }
+    }
+
+    @Test
+    default void randomPushsAndPolls() {
+        final long seed = System.nanoTime();
+        Random rnd = new Random(seed);
+        int size = 1 + rnd.nextInt(100);
+        PriorityQueue<Entry> pq = new PriorityQueue<>(size);
+        create(size);
+        IntSet set = new IntHashSet();
+        int pushCount = 0;
+        for (int i = 0; i < 1000; i++) {
+            boolean push = pq.isEmpty() || (rnd.nextBoolean());
+            if (push) {
+                int id = rnd.nextInt(size);
+                if (!set.add(id))
+                    continue;
+                float val = 100 * rnd.nextFloat();
+                pq.add(new Entry(id, val));
+                push(id, val);
+                pushCount++;
+            } else {
+                Entry entry = pq.poll();
+                assertEquals(entry.val, peekVal());
+                assertEquals(entry.id, poll());
+                assertEquals(pq.size(), size());
+                set.removeAll(entry.id);
+            }
+        }
+        assertTrue(pushCount > 0);
+    }
+
+    class Entry implements Comparable<Entry> {
+        int id;
+        float val;
+
+        public Entry(int id, float val) {
+            this.id = id;
+            this.val = val;
+        }
+
+        @Override
+        public int compareTo(Entry o) {
+            return Float.compare(val, o.val);
+        }
+    }
+
+}
diff --git a/core/src/test/java/com/graphhopper/coll/MinHeapWithUpdateTest.java b/core/src/test/java/com/graphhopper/coll/MinHeapWithUpdateTest.java
new file mode 100644
index 000000000..efab44856
--- /dev/null
+++ b/core/src/test/java/com/graphhopper/coll/MinHeapWithUpdateTest.java
@@ -0,0 +1,134 @@
+/*
+ *  Licensed to GraphHopper GmbH under one or more contributor
+ *  license agreements. See the NOTICE file distributed with this work for
+ *  additional information regarding copyright ownership.
+ *
+ *  GraphHopper GmbH licenses this file to you under the Apache License,
+ *  Version 2.0 (the "License"); you may not use this file except in
+ *  compliance with the License. You may obtain a copy of the License at
+ *
+ *       http://www.apache.org/licenses/LICENSE-2.0
+ *
+ *  Unless required by applicable law or agreed to in writing, software
+ *  distributed under the License is distributed on an "AS IS" BASIS,
+ *  WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ *  See the License for the specific language governing permissions and
+ *  limitations under the License.
+ */
+
+package com.graphhopper.coll;
+
+import org.junit.jupiter.api.Test;
+
+import static org.junit.jupiter.api.Assertions.*;
+
+public class MinHeapWithUpdateTest implements BinaryHeapTestInterface {
+
+    private MinHeapWithUpdate heap;
+
+    @Override
+    public void create(int capacity) {
+        heap = new MinHeapWithUpdate(capacity);
+    }
+
+    @Override
+    public int size() {
+        return heap.size();
+    }
+
+    @Override
+    public boolean isEmpty() {
+        return heap.isEmpty();
+    }
+
+    @Override
+    public void push(int id, float val) {
+        heap.push(id, val);
+    }
+
+    boolean contains(int id) {
+        return heap.contains(id);
+    }
+
+    @Override
+    public int peekId() {
+        return heap.peekId();
+    }
+
+    @Override
+    public float peekVal() {
+        return heap.peekValue();
+    }
+
+    @Override
+    public void update(int id, float val) {
+        heap.update(id, val);
+    }
+
+    @Override
+    public int poll() {
+        return heap.poll();
+    }
+
+    @Override
+    public void clear() {
+        heap.clear();
+    }
+
+    @Test
+    public void outOfRange() {
+        assertThrows(IllegalArgumentException.class, () -> new MinHeapWithUpdate(4).push(4, 1.2f));
+        assertThrows(IllegalArgumentException.class, () -> new MinHeapWithUpdate(4).push(-1, 1.2f));
+    }
+
+    @Test
+    void tooManyElements() {
+        create(3);
+        push(1, 0.1f);
+        push(2, 0.1f);
+        push(0, 0.1f);
+        // pushing element 1 again is not allowed (but this is not checked explicitly). however pushing more elements
+        // than 3 is already an error
+        assertThrows(IllegalStateException.class, () -> push(1, 0.1f));
+        assertThrows(IllegalStateException.class, () -> push(2, 6.1f));
+    }
+
+    @Test
+    void duplicateElements() {
+        create(5);
+        push(1, 0.2f);
+        push(0, 0.4f);
+        push(2, 0.1f);
+        assertEquals(2, poll());
+        // pushing 2 again is ok because it was polled before
+        push(2, 0.6f);
+        // but now its not ok to push it again
+        assertThrows(IllegalStateException.class, () -> push(2, 0.4f));
+    }
+
+    @Test
+    void testContains() {
+        create(4);
+        push(1, 0.1f);
+        push(2, 0.7f);
+        push(0, 0.5f);
+        assertFalse(contains(3));
+        assertTrue(contains(1));
+        assertEquals(1, poll());
+        assertFalse(contains(1));
+    }
+
+    @Test
+    void containsAfterClear() {
+        create(4);
+        push(1, 0.1f);
+        push(2, 0.1f);
+        assertEquals(2, size());
+        clear();
+        assertFalse(contains(0));
+        assertFalse(contains(1));
+        assertFalse(contains(2));
+    }
+
+
+}
\ No newline at end of file
diff --git a/core/src/test/java/com/graphhopper/routing/AlternativeRouteEdgeCHTest.java b/core/src/test/java/com/graphhopper/routing/AlternativeRouteEdgeCHTest.java
index 6281fd605..10bcd535a 100644
--- a/core/src/test/java/com/graphhopper/routing/AlternativeRouteEdgeCHTest.java
+++ b/core/src/test/java/com/graphhopper/routing/AlternativeRouteEdgeCHTest.java
@@ -141,9 +141,10 @@ public class AlternativeRouteEdgeCHTest {
         hints.putObject("alternative_route.max_paths", 4);
         AlternativeRouteEdgeCH altDijkstra = new AlternativeRouteEdgeCH(g.getRoutingCHGraph(), hints);
         List<AlternativeRouteEdgeCH.AlternativeInfo> pathInfos = altDijkstra.calcAlternatives(10, 5);
-        assertEquals(2, pathInfos.size());
+        assertEquals(3, pathInfos.size());
         assertEquals(IntArrayList.from(10, 4, 3, 6, 5), pathInfos.get(0).path.calcNodes());
         assertEquals(IntArrayList.from(10, 4, 8, 7, 6, 5), pathInfos.get(1).path.calcNodes());
+        assertEquals(IntArrayList.from(10, 4, 8, 7, 6, 3, 2, 9, 1, 5), pathInfos.get(2).path.calcNodes());
         // The shortest path works (no restrictions on the way back
     }
 
diff --git a/core/src/test/java/com/graphhopper/routing/ch/PrepareContractionHierarchiesTest.java b/core/src/test/java/com/graphhopper/routing/ch/PrepareContractionHierarchiesTest.java
index 0548dbc6d..426a8d768 100644
--- a/core/src/test/java/com/graphhopper/routing/ch/PrepareContractionHierarchiesTest.java
+++ b/core/src/test/java/com/graphhopper/routing/ch/PrepareContractionHierarchiesTest.java
@@ -145,6 +145,7 @@ public class PrepareContractionHierarchiesTest {
         initExampleGraph(g);
         int old = routingCHGraph.getEdges();
         PrepareContractionHierarchies prepare = createPrepareContractionHierarchies(g);
+        useNodeOrdering(prepare, new int[]{5, 3, 4, 0, 1, 2});
         prepare.doWork();
         assertEquals(old + 2, routingCHGraph.getEdges());
     }
@@ -154,6 +155,7 @@ public class PrepareContractionHierarchiesTest {
         initShortcutsGraph(g);
         int oldCount = routingCHGraph.getEdges();
         PrepareContractionHierarchies prepare = createPrepareContractionHierarchies(g);
+        useNodeOrdering(prepare, new int[]{0, 5, 6, 7, 8, 10, 11, 13, 15, 1, 3, 9, 14, 16, 12, 4, 2});
         prepare.doWork();
         assertEquals(oldCount, g.getEdges());
         assertEquals(oldCount + 7, routingCHGraph.getEdges());
@@ -187,8 +189,8 @@ public class PrepareContractionHierarchiesTest {
         int oldCount = g.getEdges();
         assertEquals(19, oldCount);
         PrepareContractionHierarchies prepare = createPrepareContractionHierarchies(g);
+        useNodeOrdering(prepare, new int[]{10, 0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 17, 11, 12, 13, 14, 15, 16});
         prepare.doWork();
-        // PrepareTowerNodesShortcutsTest.printEdges(g);
         assertEquals(oldCount, g.getEdges());
         assertEquals(oldCount, GHUtility.count(g.getAllEdges()));
 
@@ -259,6 +261,7 @@ public class PrepareContractionHierarchiesTest {
         initRoundaboutGraph(g);
         int oldCount = g.getEdges();
         PrepareContractionHierarchies prepare = createPrepareContractionHierarchies(g);
+        useNodeOrdering(prepare, new int[]{26, 6, 12, 13, 2, 3, 8, 9, 10, 11, 14, 15, 16, 17, 18, 20, 21, 23, 24, 25, 19, 22, 27, 5, 29, 30, 31, 28, 7, 1, 0, 4});
         prepare.doWork();
         assertEquals(oldCount, g.getEdges());
         assertEquals(oldCount, routingCHGraph.getBaseGraph().getEdges());
@@ -543,40 +546,11 @@ public class PrepareContractionHierarchiesTest {
         g.edge(6, 3, 1, true);
 
         PrepareContractionHierarchies prepare = createPrepareContractionHierarchies(g);
+        useNodeOrdering(prepare, new int[]{4, 1, 2, 0, 5, 6, 3});
         prepare.doWork();
         assertEquals(2, prepare.getShortcuts());
     }
 
-    // 0-1-2-3-4
-    // |     / |
-    // |    8  |
-    // \   /   /
-    //  7-6-5-/
-    void initBiGraph(Graph graph) {
-        graph.edge(0, 1, 100, true);
-        graph.edge(1, 2, 1, true);
-        graph.edge(2, 3, 1, true);
-        graph.edge(3, 4, 1, true);
-        graph.edge(4, 5, 25, true);
-        graph.edge(5, 6, 25, true);
-        graph.edge(6, 7, 5, true);
-        graph.edge(7, 0, 5, true);
-        graph.edge(3, 8, 20, true);
-        graph.edge(8, 6, 20, true);
-    }
-
-    //    public static void printEdges(CHGraph g) {
-//        RawEdgeIterator iter = g.getAllEdges();
-//        while (iter.next()) {
-//            EdgeSkipIterator single = g.getEdgeProps(iter.edge(), iter.nodeB());
-//            System.out.println(iter.nodeA() + "<->" + iter.nodeB() + " \\"
-//                    + single.skippedEdge1() + "," + single.skippedEdge2() + " (" + iter.edge() + ")"
-//                    + ", dist: " + (float) iter.weight()
-//                    + ", level:" + g.getLevel(iter.nodeA()) + "<->" + g.getLevel(iter.nodeB())
-//                    + ", bothDir:" + CarFlagEncoder.isBoth(iter.setProperties()));
-//        }
-//        System.out.println("---");
-//    }
     @Test
     public void testBits() {
         int fromNode = Integer.MAX_VALUE / 3 * 2;
@@ -603,9 +577,8 @@ public class PrepareContractionHierarchiesTest {
 
         ghStorage.freeze();
 
-        for (CHConfig c : configs) {
-            checkPath(ghStorage, c, 7, 5, IntArrayList.from(3, 9, 14, 16, 13, 12));
-        }
+        checkPath(ghStorage, carProfile, 7, 5, IntArrayList.from(3, 9, 14, 16, 13, 12), new int[]{0, 5, 6, 7, 8, 10, 11, 13, 15, 1, 3, 9, 14, 16, 12, 4, 2});
+        checkPath(ghStorage, bikeProfile, 7, 5, IntArrayList.from(3, 9, 14, 16, 13, 12), new int[]{0, 5, 6, 7, 8, 10, 11, 13, 15, 1, 3, 9, 14, 16, 12, 4, 2});
     }
 
     @Test
@@ -625,9 +598,9 @@ public class PrepareContractionHierarchiesTest {
 
         ghStorage.freeze();
 
-        checkPath(ghStorage, carConfig, 7, 5, IntArrayList.from(3, 9, 14, 16, 13, 12));
+        checkPath(ghStorage, carConfig, 7, 5, IntArrayList.from(3, 9, 14, 16, 13, 12), new int[]{0, 5, 6, 7, 8, 10, 11, 13, 15, 1, 3, 9, 14, 16, 12, 4, 2});
         // detour around blocked 9,14
-        checkPath(ghStorage, bikeConfig, 9, 5, IntArrayList.from(3, 10, 14, 16, 13, 12));
+        checkPath(ghStorage, bikeConfig, 9, 5, IntArrayList.from(3, 10, 14, 16, 13, 12), new int[]{0, 5, 6, 7, 8, 10, 11, 13, 14, 15, 9, 1, 4, 3, 2, 12, 16});
     }
 
     @Test
@@ -678,9 +651,10 @@ public class PrepareContractionHierarchiesTest {
         assertTrue("reusing node ordering should speed up ch contraction", timeMotorCycle < 0.5 * timeCar);
     }
 
-    private void checkPath(GraphHopperStorage g, CHConfig c, int expShortcuts, double expDistance, IntIndexedContainer expNodes) {
+    private void checkPath(GraphHopperStorage g, CHConfig c, int expShortcuts, double expDistance, IntIndexedContainer expNodes, int[] nodeOrdering) {
         RoutingCHGraph lg = g.getRoutingCHGraph(c.getName());
         PrepareContractionHierarchies prepare = createPrepareContractionHierarchies(g, c);
+        useNodeOrdering(prepare, nodeOrdering);
         prepare.doWork();
         assertEquals(c.toString(), expShortcuts, prepare.getShortcuts());
         RoutingAlgorithm algo = new CHRoutingAlgorithmFactory(lg).createAlgo(new PMap());
@@ -698,4 +672,18 @@ public class PrepareContractionHierarchiesTest {
         return PrepareContractionHierarchies.fromGraphHopperStorage(g, p);
     }
 
+    private void useNodeOrdering(PrepareContractionHierarchies prepare, int[] nodeOrdering) {
+        prepare.useFixedNodeOrdering(new NodeOrderingProvider() {
+            @Override
+            public int getNodeIdForLevel(int level) {
+                return nodeOrdering[level];
+            }
+
+            @Override
+            public int getNumNodes() {
+                return nodeOrdering.length;
+            }
+        });
+    }
+
 }
diff --git a/reader-osm/src/test/java/com/graphhopper/GraphHopperTest.java b/reader-osm/src/test/java/com/graphhopper/GraphHopperTest.java
index aff925ae7..c00dd6201 100644
--- a/reader-osm/src/test/java/com/graphhopper/GraphHopperTest.java
+++ b/reader-osm/src/test/java/com/graphhopper/GraphHopperTest.java
@@ -89,8 +89,8 @@ public class GraphHopperTest {
             ASTAR + ",false,439",
             DIJKSTRA_BI + ",false,208",
             ASTAR_BI + ",false,172",
-            ASTAR_BI + ",true,33",
-            DIJKSTRA_BI + ",true,30"
+            ASTAR_BI + ",true,36",
+            DIJKSTRA_BI + ",true,34"
     })
     public void testMonacoDifferentAlgorithms(String algo, boolean withCH, int expectedVisitedNodes) {
         final String vehicle = "car";