Improve binary heap performance

factorio-pathfinding/src/belt_finding/mod.rs

@@ -1,7 +1,8 @@
 use crate::examples::HashMapMap;
 use crate::graph::wheighted_graph::WheightedGraph;
 use crate::misc::Map;
-use crate::priority_queue::BinaryHeap;
+use crate::priority_queue::binary_heap::BinaryHeap;
+use crate::priority_queue::binary_heap::FastBinaryHeap;
 use crate::Connection;
 use crate::Map as _;
 use crate::SinglePathInput;
@@ -268,7 +269,7 @@ impl Problem {
                 map: &self.map,
                 end: self.end[i],
             };
-            let p = dijkstra::<MapInternal, BinaryHeap<_>>(&m, self.start[i], self.end[i]);
+            let p = dijkstra::<MapInternal, FastBinaryHeap<_>>(&m, self.start[i], self.end[i]);
             if let Some(p) = p {
                 self.path[i] = p;
             } else {

factorio-pathfinding/src/graph/wheighted_graph/shortest_path.rs

@@ -123,7 +123,7 @@ mod test {
         graph::wheighted_graph::{
             adjacency_list::WheightedAdjacencyList, shortest_path::dijkstra, WheightedGraph,
         },
-        priority_queue::{fibonacci_heap::FibonacciHeap, BinaryHeap},
+        priority_queue::{binary_heap::BinaryHeap, fibonacci_heap::FibonacciHeap},
     };
 
     #[test]

factorio-pathfinding/src/priority_queue/binary_heap.rs

@@ -0,0 +1,231 @@
+use std::{collections::HashMap, fmt::Debug};
+
+use super::PriorityQueue;
+
+#[derive(Debug)]
+pub struct BinaryHeap<Item> {
+    nextfree: usize,
+    data: Vec<BinaryHeapEntry<Item>>,
+}
+
+#[derive(Debug)]
+struct BinaryHeapEntry<Item> {
+    id: usize,
+    item: Item,
+}
+
+impl<Item> BinaryHeap<Item>
+where
+    Item: PartialOrd,
+{
+    fn downheap(&mut self, index: usize) {
+        let left = 2 * index + 1;
+        let right = 2 * index + 2;
+
+        if right < self.data.len() {
+            let smaller = if self.data[left].item < self.data[right].item {
+                left
+            } else {
+                right
+            };
+            if self.data[index].item > self.data[smaller].item {
+                self.data.swap(index, smaller);
+                self.downheap(smaller);
+            }
+        } else if left < self.data.len() && self.data[index].item > self.data[left].item {
+            self.data.swap(index, left);
+            self.downheap(left);
+        }
+    }
+
+    fn upheap(&mut self, index: usize) {
+        if index > 0 {
+            let parent = (index - 1) / 2;
+            if self.data[parent].item > self.data[index].item {
+                self.data.swap(parent, index);
+                self.upheap(parent);
+            }
+        }
+    }
+
+    fn search(&self, id: usize) -> Option<usize> {
+        for (i, d) in self.data.iter().enumerate() {
+            if d.id == id {
+                return Some(i);
+            }
+        }
+        None
+    }
+}
+
+impl<Item> PriorityQueue<Item> for BinaryHeap<Item>
+where
+    Item: PartialOrd + Clone,
+{
+    type Handle = usize;
+
+    fn insert(&mut self, item: Item) -> Self::Handle {
+        self.data.push(BinaryHeapEntry {
+            id: self.nextfree,
+            item: item.clone(),
+        });
+        self.upheap(self.data.len() - 1);
+        self.nextfree += 1;
+        self.nextfree - 1
+    }
+
+    fn pop_min(&mut self) -> Option<Item> {
+        if self.data.is_empty() {
+            None
+        } else {
+            let d = self.data.swap_remove(0);
+            self.downheap(0);
+            Some(d.item)
+        }
+    }
+
+    fn decrease_key(&mut self, handle: &Self::Handle, f: impl Fn(&mut Item)) {
+        if let Some(index) = self.search(*handle) {
+            f(&mut self.data[index].item);
+            self.upheap(index);
+        }
+    }
+
+    fn new() -> Self {
+        Self {
+            data: Vec::new(),
+            nextfree: 0,
+        }
+    }
+}
+
+#[derive(Debug)]
+pub struct Trace<P> {
+    inner: P,
+}
+
+impl<P, I> PriorityQueue<I> for Trace<P>
+where
+    I: PartialOrd + Clone + Debug,
+    P: PriorityQueue<I>,
+{
+    type Handle = P::Handle;
+
+    fn new() -> Self {
+        println!("New priority queue.");
+        Self { inner: P::new() }
+    }
+
+    fn insert(&mut self, item: I) -> Self::Handle {
+        println!("Insert: {item:?}");
+        self.inner.insert(item)
+    }
+
+    fn pop_min(&mut self) -> Option<I> {
+        let min = self.inner.pop_min();
+        println!("Pop min: {min:?}");
+        min
+    }
+
+    fn decrease_key(&mut self, handle: &Self::Handle, f: impl Fn(&mut I)) {
+        self.inner.decrease_key(handle, |i| {
+            let old_i = i.clone();
+            f(i);
+            println!("Decrease key: {old_i:?} -> {i:?}");
+        })
+    }
+}
+
+#[derive(Debug)]
+pub struct FastBinaryHeap<Item> {
+    nextfree: usize,
+    data: Vec<BinaryHeapEntry<Item>>,
+    map: HashMap<usize, usize>,
+}
+
+impl<Item> FastBinaryHeap<Item>
+where
+    Item: PartialOrd,
+{
+    fn downheap(&mut self, index: usize) {
+        let left = 2 * index + 1;
+        let right = 2 * index + 2;
+
+        if right < self.data.len() {
+            let smaller = if self.data[left].item < self.data[right].item {
+                left
+            } else {
+                right
+            };
+            if self.data[index].item > self.data[smaller].item {
+                self.data.swap(index, smaller);
+                self.downheap(smaller);
+            }
+        } else if left < self.data.len() && self.data[index].item > self.data[left].item {
+            self.data.swap(index, left);
+            self.downheap(left);
+        }
+        self.map.insert(self.data[index].id, index);
+    }
+
+    fn upheap(&mut self, index: usize) {
+        if index > 0 {
+            let parent = (index - 1) / 2;
+            if self.data[parent].item > self.data[index].item {
+                self.data.swap(parent, index);
+                self.upheap(parent);
+            }
+        }
+        self.map.insert(self.data[index].id, index);
+    }
+
+    fn search(&self, id: usize) -> Option<usize> {
+        self.map.get(&id).copied()
+    }
+}
+
+impl<Item> PriorityQueue<Item> for FastBinaryHeap<Item>
+where
+    Item: PartialOrd + Clone,
+{
+    type Handle = usize;
+
+    fn insert(&mut self, item: Item) -> Self::Handle {
+        self.map.insert(self.nextfree, self.data.len());
+        self.data.push(BinaryHeapEntry {
+            id: self.nextfree,
+            item: item.clone(),
+        });
+        self.upheap(self.data.len() - 1);
+        self.nextfree += 1;
+        self.nextfree - 1
+    }
+
+    fn pop_min(&mut self) -> Option<Item> {
+        if self.data.is_empty() {
+            None
+        } else {
+            let d = self.data.swap_remove(0);
+            self.map.remove(&d.id);
+            if !self.data.is_empty() {
+                self.downheap(0);
+            }
+            Some(d.item)
+        }
+    }
+
+    fn decrease_key(&mut self, handle: &Self::Handle, f: impl Fn(&mut Item)) {
+        if let Some(index) = self.search(*handle) {
+            f(&mut self.data[index].item);
+            self.upheap(index);
+        }
+    }
+
+    fn new() -> Self {
+        Self {
+            data: Vec::new(),
+            map: HashMap::new(),
+            nextfree: 0,
+        }
+    }
+}

factorio-pathfinding/src/priority_queue/mod.rs

@@ -1,4 +1,5 @@
 use std::fmt::Debug;
+pub mod binary_heap;
 pub mod fibonacci_heap;
 
 pub trait PriorityQueue<Item>
@@ -16,144 +17,10 @@ where
     fn decrease_key(&mut self, handle: &Self::Handle, f: impl Fn(&mut Item));
 }
 
-#[derive(Debug)]
-pub struct BinaryHeap<Item> {
-    nextfree: usize,
-    data: Vec<BinaryHeapEntry<Item>>,
-}
-
-#[derive(Debug)]
-struct BinaryHeapEntry<Item> {
-    id: usize,
-    item: Item,
-}
-
-impl<Item> BinaryHeap<Item>
-where
-    Item: PartialOrd,
-{
-    fn downheap(&mut self, index: usize) {
-        let left = 2 * index + 1;
-        let right = 2 * index + 2;
-
-        if right < self.data.len() {
-            let smaller = if self.data[left].item < self.data[right].item {
-                left
-            } else {
-                right
-            };
-            if self.data[index].item > self.data[smaller].item {
-                self.data.swap(index, smaller);
-                self.downheap(smaller);
-            }
-        } else if left < self.data.len() && self.data[index].item > self.data[left].item {
-            self.data.swap(index, left);
-            self.downheap(left);
-        }
-    }
-
-    fn upheap(&mut self, index: usize) {
-        if index > 0 {
-            let parent = (index - 1) / 2;
-            if self.data[parent].item > self.data[index].item {
-                self.data.swap(parent, index);
-                self.upheap(parent);
-            }
-        }
-    }
-
-    fn search(&self, id: usize) -> Option<usize> {
-        for (i, d) in self.data.iter().enumerate() {
-            if d.id == id {
-                return Some(i);
-            }
-        }
-        None
-    }
-}
-
-impl<Item> PriorityQueue<Item> for BinaryHeap<Item>
-where
-    Item: PartialOrd + Clone,
-{
-    type Handle = usize;
-
-    fn insert(&mut self, item: Item) -> Self::Handle {
-        self.data.push(BinaryHeapEntry {
-            id: self.nextfree,
-            item: item.clone(),
-        });
-        self.upheap(self.data.len() - 1);
-        self.nextfree += 1;
-        self.nextfree - 1
-    }
-
-    fn pop_min(&mut self) -> Option<Item> {
-        if self.data.is_empty() {
-            None
-        } else {
-            let d = self.data.swap_remove(0);
-            self.downheap(0);
-            Some(d.item)
-        }
-    }
-
-    fn decrease_key(&mut self, handle: &Self::Handle, f: impl Fn(&mut Item)) {
-        if let Some(index) = self.search(*handle) {
-            f(&mut self.data[index].item);
-            self.upheap(index);
-        }
-    }
-
-    fn new() -> Self {
-        Self {
-            data: Vec::new(),
-            nextfree: 0,
-        }
-    }
-}
-
-#[derive(Debug)]
-pub struct Trace<P> {
-    inner: P,
-}
-
-impl<P, I> PriorityQueue<I> for Trace<P>
-where
-    I: PartialOrd + Clone + Debug,
-    P: PriorityQueue<I>,
-{
-    type Handle = P::Handle;
-
-    fn new() -> Self {
-        println!("New priority queue.");
-        Self { inner: P::new() }
-    }
-
-    fn insert(&mut self, item: I) -> Self::Handle {
-        println!("Insert: {item:?}");
-        self.inner.insert(item)
-    }
-
-    fn pop_min(&mut self) -> Option<I> {
-        let min = self.inner.pop_min();
-        println!("Pop min: {min:?}");
-        min
-    }
-
-    fn decrease_key(&mut self, handle: &Self::Handle, f: impl Fn(&mut I)) {
-        self.inner.decrease_key(handle, |i| {
-            let old_i = i.clone();
-            f(i);
-            println!("Decrease key: {old_i:?} -> {i:?}");
-        })
-    }
-}
-
 #[cfg(test)]
 mod tests {
+    use super::binary_heap::{BinaryHeap, FastBinaryHeap};
     use super::fibonacci_heap::FibonacciHeap;
-    use super::BinaryHeap;
     use super::PriorityQueue;
 
     macro_rules! test_generics {
@@ -184,7 +51,7 @@ mod tests {
     }
 
     mod test_macro {
-        test_generics!(basic_generic decrease_key_generic; BinaryHeap FibonacciHeap);
+        test_generics!(basic_generic decrease_key_generic; BinaryHeap FastBinaryHeap FibonacciHeap);
     }
 
     fn basic_generic<T: PriorityQueue<usize>>() {