full d* lite impl

azalea-pathfinder/src/dstarlite.rs

@@ -3,15 +3,26 @@
 
 use priority_queue::PriorityQueue;
 use std::{
+    borrow::Cow,
     hash::{Hash, Hasher},
-    ops::Add,
+    ops::{Add, Deref},
 };
 
 #[derive(Eq, PartialEq, Clone)]
 pub struct Vertex<N: Eq + Hash + Clone, W: Ord + Copy> {
+    pub node: N,
     pub g: W,
     pub rhs: W,
-    pub node: N,
+}
+
+impl<N: Eq + Hash + Clone, W: Ord + Copy + Default> Vertex<N, W> {
+    pub fn new(node: N) -> Self {
+        Self {
+            node,
+            rhs: W::default(),
+            g: W::default(),
+        }
+    }
 }
 
 impl<N: Eq + Hash + Clone, W: Ord + Copy> Hash for Vertex<N, W> {
@@ -20,11 +31,19 @@ impl<N: Eq + Hash + Clone, W: Ord + Copy> Hash for Vertex<N, W> {
     }
 }
 
-pub struct DStarLite<N: Eq + Hash + Clone, W: Ord + Copy> {
+pub type HeuristicFn<N, W> = fn(start: &Vertex<N, W>, current: &Vertex<N, W>) -> W;
-    pub heuristic: fn(start: &Vertex<N, W>, current: &Vertex<N, W>) -> W,
-    pub successors: fn(current: &Vertex<N, W>) -> Vec<Vertex<N, W>>,
 
-    pub start: Vertex<N, W>,
+pub type EdgesFn<N, W> = fn(current: &Vertex<N, W>) -> Vec<Edge<N, W>>;
+
+pub struct DStarLite<'a, N: Eq + Hash + Clone, W: Ord + Copy> {
+    /// Rough estimate of how close we are to the goal. Lower = closer.
+    pub heuristic: HeuristicFn<N, W>,
+    /// Get the nodes that can be reached from the current one
+    pub successors: EdgesFn<N, W>,
+    /// Get the nodes that would direct us to the current node
+    pub predecessors: EdgesFn<N, W>,
+
+    pub start: Cow<'a, Vertex<N, W>>,
     pub goal: Vertex<N, W>,
 
     pub queue: PriorityQueue<Vertex<N, W>, Priority<W>>,
@@ -32,16 +51,26 @@ pub struct DStarLite<N: Eq + Hash + Clone, W: Ord + Copy> {
     pub k_m: W,
 }
 
+pub struct Edge<'a, N: Eq + Hash + Clone, W: Ord + Copy> {
+    pub predecessor: Cow<'a, Vertex<N, W>>,
+    pub successor: Cow<'a, Vertex<N, W>>,
+    pub cost: W,
+}
+
 // rust does lexicographic ordering by default when we derive Ord
 #[derive(Eq, Ord, PartialEq, PartialOrd)]
 pub struct Priority<W>(W, W)
 where
     W: Ord;
 
-impl<N, W> DStarLite<N, W>
+pub trait Max {
+    const MAX: Self;
+}
+
+impl<N, W> DStarLite<'_, N, W>
 where
     N: Eq + Hash + Clone,
-    W: Ord + Add<Output = W> + Default + Copy,
+    W: Ord + Add<Output = W> + Default + Copy + Max,
 {
     fn calculate_key(&self, s: &Vertex<N, W>) -> Priority<W> {
         // return [min(g(s), rhs(s)) + h(s_start, s) + k_m, min(g(s), rhs(s))]
@@ -51,34 +80,54 @@ where
         )
     }
 
-    fn initialize(&mut self) {
+    pub fn new(
-        self.queue = Default::default();
+        start: N,
-        self.k_m = W::default();
+        goal: N,
-        self.goal.rhs = W::default();
+        heuristic: HeuristicFn<N, W>,
-        self.queue.push(
+        successors: EdgesFn<N, W>,
-            self.goal.clone(),
+        predecessors: EdgesFn<N, W>,
-            Priority((self.heuristic)(&self.start, &self.goal), W::default()),
+    ) -> Self {
+        let start_vertex = Vertex::new(start);
+        let goal_vertex = Vertex::new(goal);
+
+        let mut queue = PriorityQueue::with_capacity(1);
+        // Vertex<N, W>, Priority<W>
+        queue.push(
+            goal_vertex.clone(),
+            Priority(heuristic(&start_vertex, &goal_vertex), W::default()),
         );
+        Self {
+            start: Cow::Owned(start_vertex),
+            goal: goal_vertex,
+
+            heuristic,
+            successors,
+            predecessors,
+
+            queue,
+            k_m: W::default(),
+            // self.goal.rhs = W::default(),
+        }
     }
 
-    fn update_vertex(&mut self, u: Vertex<N, W>) {
+    pub fn update_vertex(&mut self, u: &Vertex<N, W>) {
         // if(g(u)) != rhs(u) AND u is in U) U.Update(u, calculate_key(u))
         if u.g != u.rhs && self.queue.get(&u).is_some() {
             self.queue.change_priority(&u, self.calculate_key(&u));
         } else if u.g != u.rhs && self.queue.get(&u).is_none() {
             let key = self.calculate_key(&u);
-            self.queue.push(u, key);
+            self.queue.push(u.clone(), key);
         } else if u.g == u.rhs && self.queue.get(&u).is_some() {
             self.queue.remove(&u);
         }
     }
 
-    fn compute_shortest_path(&mut self) {
+    pub fn compute_shortest_path(&mut self) {
         while self.queue.peek().unwrap().1 < &self.calculate_key(&self.start)
             || self.start.rhs > self.start.g
         {
-            let (u, k_old) = self.queue.pop().unwrap();
+            let (mut u, k_old) = self.queue.pop().unwrap();
-            let k_new = self.calculate_key(u);
+            let k_new = self.calculate_key(&u);
             if k_old < k_new {
                 self.queue.change_priority(&u, k_new);
             } else if u.g > u.rhs {
@@ -87,31 +136,55 @@ where
                 // for all s in Pred(u)
                 // rhs(s) = min(rhs(s), c(s, u) + g(u))
                 // update_vertex(s)
-                for s in self.get_predecessors(&u) {
+                for mut edge in (self.predecessors)(&u) {
-                    s.rhs = Ord::min(s.rhs, self.get_cost(&s, &u) + u.g);
+                    edge.predecessor.to_mut().rhs = Ord::min(edge.predecessor.rhs, edge.cost + u.g);
-                    self.update_vertex(s);
+                    self.update_vertex(&edge.predecessor);
                 }
             } else {
                 let g_old = u.g;
-                u.g = W::max_value();
+                u.g = W::MAX;
                 // for all s in Pred(u) + {u}
                 //   if (rhs(s) = c(s, u) + g_old)
                 //     if (s != s_goal) rhs(s) = min s' in Succ(s) (c(s, s') + g(s'))
                 //   update_vertex(s)
-                for s in [self.get_predecessors(&u), [u]].concat() {
+                for mut predecessor_edge in ((self.predecessors)(&u)).into_iter().chain(
-                    if s.rhs == self.get_cost(&s, &u) + g_old {
+                    [Edge {
-                        if s != self.goal {
+                        predecessor: Cow::Borrowed(&u),
-                            s.rhs = self
+                        successor: Cow::Borrowed(&u),
-                                .get_successors(&s)
+                        cost: W::default(),
-                                .iter()
+                    }]
-                                .map(|s_prime| self.get_cost(&s, s_prime) + s_prime.g)
+                    .into_iter(),
-                                .min()
+                ) {
-                                .unwrap();
+                    if predecessor_edge.predecessor.rhs == predecessor_edge.cost + g_old {
+                        if &self.goal != predecessor_edge.predecessor.deref() {
+                            predecessor_edge.predecessor.to_mut().rhs =
+                                (self.successors)(&predecessor_edge.predecessor)
+                                    .iter()
+                                    .map(|successor_edge| {
+                                        successor_edge.cost + successor_edge.successor.g
+                                    })
+                                    .min()
+                                    .unwrap();
                         }
                     }
-                    self.update_vertex(s);
+                    self.update_vertex(&predecessor_edge.predecessor);
                 }
             }
         }
     }
+
+    // fn main(&mut self) {
+    //     let s_last = &self.start;
+    //     self.compute_shortest_path();
+    //     while self.start.deref() != &self.goal {
+    //         self.start = (self.successors)(&self.start)
+    //             .into_iter()
+    //             .min_by(|a, b| a.cost.cmp(&b.cost))
+    //             .expect("No possible successors")
+    //             .successor;
+    //         if self.start.rhs == W::MAX {
+    //             // no path
+    //         }
+    //     }
+    // }
 }

azalea-pathfinder/src/lib.rs

@@ -1 +1,3 @@
 mod dstarlite;
+
+pub use dstarlite::DStarLite;