From 00fcf26fcaade0185da8a9beff119a8949b459dd Mon Sep 17 00:00:00 2001 From: Ubuntu Date: Wed, 5 Oct 2022 19:59:18 +0000 Subject: [PATCH] full d* lite impl --- azalea-pathfinder/src/dstarlite.rs | 141 ++++++++++++++++++++++------- azalea-pathfinder/src/lib.rs | 2 + 2 files changed, 109 insertions(+), 34 deletions(-) diff --git a/azalea-pathfinder/src/dstarlite.rs b/azalea-pathfinder/src/dstarlite.rs index 427a5548..c681eea0 100644 --- a/azalea-pathfinder/src/dstarlite.rs +++ b/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 { + pub node: N, pub g: W, pub rhs: W, - pub node: N, +} + +impl Vertex { + pub fn new(node: N) -> Self { + Self { + node, + rhs: W::default(), + g: W::default(), + } + } } impl Hash for Vertex { @@ -20,11 +31,19 @@ impl Hash for Vertex { } } -pub struct DStarLite { - pub heuristic: fn(start: &Vertex, current: &Vertex) -> W, - pub successors: fn(current: &Vertex) -> Vec>, +pub type HeuristicFn = fn(start: &Vertex, current: &Vertex) -> W; - pub start: Vertex, +pub type EdgesFn = fn(current: &Vertex) -> Vec>; + +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, + /// Get the nodes that can be reached from the current one + pub successors: EdgesFn, + /// Get the nodes that would direct us to the current node + pub predecessors: EdgesFn, + + pub start: Cow<'a, Vertex>, pub goal: Vertex, pub queue: PriorityQueue, Priority>, @@ -32,16 +51,26 @@ pub struct DStarLite { pub k_m: W, } +pub struct Edge<'a, N: Eq + Hash + Clone, W: Ord + Copy> { + pub predecessor: Cow<'a, Vertex>, + pub successor: Cow<'a, Vertex>, + pub cost: W, +} + // rust does lexicographic ordering by default when we derive Ord #[derive(Eq, Ord, PartialEq, PartialOrd)] pub struct Priority(W, W) where W: Ord; -impl DStarLite +pub trait Max { + const MAX: Self; +} + +impl DStarLite<'_, N, W> where N: Eq + Hash + Clone, - W: Ord + Add + Default + Copy, + W: Ord + Add + Default + Copy + Max, { fn calculate_key(&self, s: &Vertex) -> Priority { // 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) { - self.queue = Default::default(); - self.k_m = W::default(); - self.goal.rhs = W::default(); - self.queue.push( - self.goal.clone(), - Priority((self.heuristic)(&self.start, &self.goal), W::default()), + pub fn new( + start: N, + goal: N, + heuristic: HeuristicFn, + successors: EdgesFn, + predecessors: EdgesFn, + ) -> Self { + let start_vertex = Vertex::new(start); + let goal_vertex = Vertex::new(goal); + + let mut queue = PriorityQueue::with_capacity(1); + // Vertex, Priority + 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) { + pub fn update_vertex(&mut self, u: &Vertex) { // 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 k_new = self.calculate_key(u); + let (mut u, k_old) = self.queue.pop().unwrap(); + 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) { - s.rhs = Ord::min(s.rhs, self.get_cost(&s, &u) + u.g); - self.update_vertex(s); + for mut edge in (self.predecessors)(&u) { + edge.predecessor.to_mut().rhs = Ord::min(edge.predecessor.rhs, edge.cost + u.g); + 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() { - if s.rhs == self.get_cost(&s, &u) + g_old { - if s != self.goal { - s.rhs = self - .get_successors(&s) - .iter() - .map(|s_prime| self.get_cost(&s, s_prime) + s_prime.g) - .min() - .unwrap(); + for mut predecessor_edge in ((self.predecessors)(&u)).into_iter().chain( + [Edge { + predecessor: Cow::Borrowed(&u), + successor: Cow::Borrowed(&u), + cost: W::default(), + }] + .into_iter(), + ) { + 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 + // } + // } + // } } diff --git a/azalea-pathfinder/src/lib.rs b/azalea-pathfinder/src/lib.rs index a99febd7..ddb92f60 100644 --- a/azalea-pathfinder/src/lib.rs +++ b/azalea-pathfinder/src/lib.rs @@ -1 +1,3 @@ mod dstarlite; + +pub use dstarlite::DStarLite;