more work on d* lite

Cargo.lock

@@ -232,6 +232,9 @@ dependencies = [
 [[package]]
 name = "azalea-pathfinder"
 version = "0.1.0"
+dependencies = [
+ "priority-queue",
+]
 
 [[package]]
 name = "azalea-physics"
@@ -700,6 +703,12 @@ version = "1.8.2"
 source = "registry+https://github.com/rust-lang/crates.io-index"
 checksum = "eabb4a44450da02c90444cf74558da904edde8fb4e9035a9a6a4e15445af0bd7"
 
+[[package]]
+name = "hashbrown"
+version = "0.12.3"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "8a9ee70c43aaf417c914396645a0fa852624801b24ebb7ae78fe8272889ac888"
+
 [[package]]
 name = "heck"
 version = "0.4.0"
@@ -743,6 +752,16 @@ dependencies = [
  "unicode-normalization",
 ]
 
+[[package]]
+name = "indexmap"
+version = "1.9.1"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "10a35a97730320ffe8e2d410b5d3b69279b98d2c14bdb8b70ea89ecf7888d41e"
+dependencies = [
+ "autocfg",
+ "hashbrown",
+]
+
 [[package]]
 name = "inout"
 version = "0.1.3"
@@ -1095,6 +1114,16 @@ version = "0.2.16"
 source = "registry+https://github.com/rust-lang/crates.io-index"
 checksum = "eb9f9e6e233e5c4a35559a617bf40a4ec447db2e84c20b55a6f83167b7e57872"
 
+[[package]]
+name = "priority-queue"
+version = "1.2.3"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "815082d99af3acc75a3e67efd2a07f72e67b4e81b4344eb8ca34c6ebf3dfa9c5"
+dependencies = [
+ "autocfg",
+ "indexmap",
+]
+
 [[package]]
 name = "proc-macro2"
 version = "1.0.39"

azalea-pathfinder/Cargo.toml

@@ -6,3 +6,4 @@ edition = "2021"
 # See more keys and their definitions at https://doc.rust-lang.org/cargo/reference/manifest.html
 
 [dependencies]
+priority-queue = "1.2.3"

azalea-pathfinder/src/dstarlite.rs

@@ -1,36 +1,54 @@
-//! An implementation of D* Lite (optimized version) from
+//! An implementation of D* Lite (optimized version) as described in
 //! http://idm-lab.org/bib/abstracts/papers/aaai02b.pdf
+//!
+//! Note that the words "vertex" and "key" from the paper are replaced with
+//! "node" and "priority" in this code because I think these terms better
+//! represent their purpose and are more commonly used when describing similar
+//! algorithms.
 
+use priority_queue::PriorityQueue;
 use std::collections::BinaryHeap;
 
-#[derive(Default, Ord)]
-pub struct Vertex<Weight>
+#[derive(Default, Eq, PartialEq, Hash)]
+pub struct Node<W>
 where
-    Weight: Default + Ord,
+    W: Default + Ord,
 {
-    pub g: Weight,
-    pub rhs: Weight,
+    pub g: W,
+    pub rhs: W,
 }
 
-type Heuristic<Weight> = fn(start: &Vertex<Weight>, current: &Vertex<Weight>) -> Weight;
-
-pub struct DStarLite<Weight>
+type Heuristic<W>
 where
-    Weight: Default + Ord,
+    W: Default + Ord,
+= fn(start: &Node<W>, current: &Node<W>) -> W;
+
+pub struct DStarLite<W>
+where
+    W: Default + Ord,
 {
-    pub start: Vertex<Weight>,
-    pub goal: Vertex<Weight>,
+    pub start: Node<W>,
+    pub goal: Node<W>,
 
-    pub queue: BinaryHeap<Vertex<Weight>>,
+    pub queue: PriorityQueue<Node<W>, Priority<W>>,
 
-    pub k_m: Weight,
-    pub heuristic: Heuristic<Weight>,
+    pub k_m: W,
+    pub heuristic: Heuristic<W>,
 }
 
-impl<Weight> DStarLite<Weight> {
-    fn calculate_key(&self, s: &Vertex<Weight>) -> (Weight, Weight) {
+// rust does lexicographic ordering by default when we derive Ord
+#[derive(Eq, Ord, PartialEq, PartialOrd)]
+pub struct Priority<W>(W, W)
+where
+    W: Default + Ord;
+
+impl<W> DStarLite<W>
+where
+    W: Default + Ord,
+{
+    fn calculate_key(&self, s: &Node<W>) -> Priority<W> {
         // return [min(g(s), rhs(s)) + h(s_start, s) + k_m, min(g(s), rhs(s))]
-        (
+        Priority(
             s.g.min(s.rhs) + self.heuristic(self.start, s) + self.k_m,
             s.g.min(s.rhs),
         )
@@ -40,12 +58,16 @@ impl<Weight> DStarLite<Weight> {
         let priority_queue = BinaryHeap::new();
         let k_m = 0;
         self.goal.rhs = 0;
-        priority_queue.push(self.goal, (self.heuristic(&self.start, self.goal), 0));
+        priority_queue.push(
+            self.goal,
+            Priority(self.heuristic(&self.start, self.goal), 0),
+        );
     }
 
-    fn update_vertex(&self, u: Vertex, priority_queue: BinaryHeap<Vertex>) {
+    fn update_vertex(&self, u: Node<W>, priority_queue: PriorityQueue<Node<W>, Priority<W>>) {
         // if(g(u)) != rhs(u) AND u is in U) U.Update(u, calculate_key(u))
-        // if u.g != u.rhs && priority_queue.contains(u) {
-        // 	priority_queue.update(u, calculate_key(s_start, u, h));
+        if u.g != u.rhs && priority_queue.contains(u) {
+            priority_queue.update(u, self.calculate_key(s_start, u, h));
+        }
     }
 }

azalea-pathfinder/src/lib.rs

@@ -1,16 +1 @@
 mod dstarlite;
-
-pub fn add(left: usize, right: usize) -> usize {
-    left + right
-}
-
-#[cfg(test)]
-mod tests {
-    use super::*;
-
-    #[test]
-    fn it_works() {
-        let result = add(2, 2);
-        assert_eq!(result, 4);
-    }
-}