implement mtd* lite

Cargo.lock

@@ -244,6 +244,10 @@ dependencies = [
 name = "azalea-pathfinder"
 version = "0.1.0"
 dependencies = [
+ "anyhow",
+ "async-trait",
+ "azalea",
+ "azalea-client",
  "num-traits",
  "priority-queue",
 ]

azalea-pathfinder/Cargo.toml

@@ -1,10 +1,14 @@
 [package]
+edition = "2021"
 name = "azalea-pathfinder"
 version = "0.1.0"
-edition = "2021"
 
 # See more keys and their definitions at https://doc.rust-lang.org/cargo/reference/manifest.html
 
 [dependencies]
+anyhow = "1.0.65"
+async-trait = "0.1.57"
+azalea = {version = "0.1", path = "../azalea"}
+azalea-client = { version = "0.1.0", path = "../azalea-client" }
 num-traits = "0.2.15"
 priority-queue = "1.2.3"

azalea-pathfinder/src/dstarlite.rs

@@ -39,14 +39,14 @@ pub struct DStarLite<
     W: PartialOrd + Eq + Default + Copy + num_traits::Bounded + Debug,
     HeuristicFn: Fn(&N, &N) -> W,
     SuccessorsFn: Fn(&N) -> Vec<EdgeTo<N, W>>,
-    PredcessorsFn: Fn(&N) -> Vec<EdgeTo<N, W>>,
+    PredecessorsFn: Fn(&N) -> Vec<EdgeTo<N, W>>,
 > {
     /// 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: SuccessorsFn,
     /// Get the nodes that would direct us to the current node
-    pub predecessors: PredcessorsFn,
+    pub predecessors: PredecessorsFn,
 
     pub start: Cow<'a, N>,
     start_last: Cow<'a, N>,
@@ -231,10 +231,6 @@ impl<
             } else {
                 let g_old = u_score.g;
                 u_score.g = W::max_value();
-                // 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.predecessors)(&u)).into_iter().chain(
                     [EdgeTo {
                         target: u,

azalea-pathfinder/src/lib.rs

@@ -1,3 +1,45 @@
-mod dstarlite;
+#![feature(let_chains)]
 
+mod dstarlite;
+mod mtdstarlite;
+
+use async_trait::async_trait;
+use azalea::{Client, Event};
 pub use dstarlite::DStarLite;
+pub use mtdstarlite::MTDStarLite;
+use std::sync::{Arc, Mutex};
+
+// #[derive(Default)]
+// pub struct Plugin {
+//     pub state: Arc<Mutex<State>>,
+// }
+
+// #[derive(Default)]
+// pub struct State {
+//     // pathfinder: Option<DStarLite< Node, FloatOrd<f32>>>,
+// }
+
+// #[async_trait]
+// impl azalea::Plugin for Plugin {
+//     async fn handle(self: Arc<Self>, bot: Client, event: Arc<Event>) {
+//         // match *
+//     }
+// }
+
+// pub trait Trait {
+//     fn goto(&self, goal: impl Goal);
+// }
+
+// impl Trait for azalea_client::Client {
+//     fn goto(&self, goal: impl Goal) {
+//         let start = BlockPos::from(self.position());
+
+//         let pf = DStarLite::new();
+//     }
+// }
+
+// // fn heuristt
+
+// pub trait Goal {
+//     fn heuristic(&self, x: i32, y: i32) -> f32;
+// }

azalea-pathfinder/src/mtdstarlite.rs

@@ -0,0 +1,374 @@
+//! An implementation of Moving Target D* Lite as described in
+//! <http://idm-lab.org/bib/abstracts/papers/aamas10a.pdf>
+//!
+//! Future optimization attempt ideas:
+//! - Use a different priority queue (e.g. fibonacci heap)
+//! - Use FxHash instead of the default hasher
+//! - Have `par` be a raw pointer
+//! - Try borrowing vs copying the Node in several places (like state_mut)
+//! - Store edge costs in their own map
+
+use priority_queue::DoublePriorityQueue;
+use std::{
+    collections::HashMap,
+    fmt::Debug,
+    hash::Hash,
+    ops::{Add, Sub},
+};
+
+/// Nodes are coordinates.
+pub struct MTDStarLite<
+    N: Eq + Hash + Copy + Debug + Sub<Output = NDelta> + Add<NDelta, Output = N>,
+    W: PartialOrd + Eq + Default + Copy + num_traits::Bounded + Debug,
+    NDelta,
+    HeuristicFn: Fn(&N, &N) -> W,
+    SuccessorsFn: Fn(&N) -> Vec<Edge<N, W>>,
+    PredecessorsFn: Fn(&N) -> Vec<Edge<N, W>>,
+> {
+    /// Returns a rough estimate of how close we are to the goal. Lower = closer.
+    pub heuristic: HeuristicFn,
+    /// Returns the nodes that can be reached from the given node.
+    pub successors: SuccessorsFn,
+    /// Returns the nodes that would direct us to the given node.
+    pub predecessors: PredecessorsFn,
+
+    start: N,
+    goal: N,
+
+    // TODO: these are only used because the paper does it like this
+    // we should get rid of these and only rely on `start` and `goal` in the
+    // future
+    pub new_start: N,
+    pub new_goal: N,
+
+    old_start: N,
+    old_goal: N,
+
+    k_m: W,
+    open: DoublePriorityQueue<N, Priority<W>>,
+    node_states: HashMap<N, NodeState<N, W>>,
+    updated_edge_costs: Vec<ChangedEdge<N, W>>,
+
+    /// This only exists so it can be referenced by `state()` when there's no state.
+    default_state: NodeState<N, W>,
+}
+
+impl<
+        N: Eq + Hash + Copy + Debug + Sub<Output = NDelta> + Add<NDelta, Output = N>,
+        W: PartialOrd + Eq + Add<Output = W> + Default + Copy + num_traits::Bounded + Debug,
+        NDelta,
+        HeuristicFn: Fn(&N, &N) -> W,
+        SuccessorsFn: Fn(&N) -> Vec<Edge<N, W>>,
+        PredecessorsFn: Fn(&N) -> Vec<Edge<N, W>>,
+    > MTDStarLite<N, W, NDelta, HeuristicFn, SuccessorsFn, PredecessorsFn>
+{
+    fn calculate_key(&self, n: &N) -> Priority<W> {
+        let s = self.state(n);
+        let min_score = if s.g < s.rhs { s.g } else { s.rhs };
+        Priority(
+            if min_score == W::max_value() {
+                min_score
+            } else {
+                min_score + (self.heuristic)(n, &self.goal) + self.k_m
+            },
+            min_score,
+        )
+    }
+
+    pub fn new(
+        start: N,
+        goal: N,
+        heuristic: HeuristicFn,
+        successors: SuccessorsFn,
+        predecessors: PredecessorsFn,
+    ) -> Self {
+        let open = DoublePriorityQueue::default();
+        let k_m = W::default();
+
+        let known_nodes = vec![start, goal];
+
+        let mut pf = MTDStarLite {
+            heuristic,
+            successors,
+            predecessors,
+
+            start,
+            goal,
+
+            new_start: start,
+            new_goal: goal,
+
+            old_start: start,
+            old_goal: goal,
+
+            k_m,
+            open,
+            node_states: HashMap::new(),
+            updated_edge_costs: Vec::new(),
+
+            default_state: NodeState::default(),
+        };
+
+        for n in &known_nodes {
+            *pf.state_mut(n) = NodeState::default();
+        }
+        (*pf.state_mut(&start)).rhs = W::default();
+        pf.open.push(start, pf.calculate_key(&start));
+
+        pf
+    }
+
+    fn update_state(&mut self, n: &N) {
+        let u = self.state_mut(n);
+        if u.g != u.rhs {
+            if self.open.get(n).is_some() {
+                self.open.change_priority(n, self.calculate_key(n));
+            } else {
+                self.open.push(*n, self.calculate_key(n));
+            }
+        } else if self.open.get(n).is_some() {
+            self.open.remove(n);
+        }
+    }
+
+    fn compute_cost_minimal_path(&mut self) {
+        while {
+            if let Some((_, top_key)) = self.open.peek_min() {
+                (top_key < &self.calculate_key(&self.goal)) || {
+                    let goal_state = self.state(&self.goal);
+                    goal_state.rhs > goal_state.g
+                }
+            } else {
+                false
+            }
+        } {
+            let (u_node, k_old) = self.open.pop_min().unwrap();
+            let k_new = self.calculate_key(&u_node);
+            if k_old < k_new {
+                self.open.change_priority(&u_node, k_new);
+                continue;
+            }
+            let u = self.state_mut(&u_node);
+            if u.g > u.rhs {
+                u.g = u.rhs;
+                let u = self.state(&u_node);
+                self.open.remove(&u_node);
+                for edge in (self.successors)(&u_node) {
+                    let s_node = edge.target;
+                    let s = self.state(&s_node);
+                    let u = self.state(&u_node);
+                    if s_node != self.start && (s.rhs > u.g + edge.cost) {
+                        let s_rhs = u.g + edge.cost;
+                        let s = self.state_mut(&s_node);
+                        s.par = Some(u_node);
+                        s.rhs = s_rhs;
+                        self.update_state(&s_node);
+                    }
+                }
+            } else {
+                u.g = W::max_value();
+                let u_edge = Edge {
+                    target: u_node,
+                    cost: W::default(),
+                };
+                for edge in (self.successors)(&u_node)
+                    .iter()
+                    .chain([&u_edge].into_iter())
+                {
+                    let s_node = edge.target;
+                    let s = self.state(&s_node);
+                    if s_node != self.start && s.par == Some(u_node) {
+                        let mut min_pred = u_node;
+                        let mut min_score = W::max_value();
+
+                        for edge in (self.predecessors)(&s_node) {
+                            let s = self.state(&edge.target);
+                            let score = s.g + edge.cost;
+                            if score < min_score {
+                                min_score = score;
+                                min_pred = edge.target;
+                            }
+                        }
+
+                        let s = self.state_mut(&s_node);
+                        s.rhs = min_score;
+                        if s.rhs == W::max_value() {
+                            s.par = None;
+                        } else {
+                            s.par = Some(min_pred);
+                        }
+                    }
+                    self.update_state(&s_node);
+                }
+            }
+        }
+    }
+
+    pub fn find_path(&mut self) -> Option<()> {
+        if self.start == self.goal {
+            return None;
+        }
+
+        self.old_start = self.start;
+        self.old_goal = self.goal;
+
+        self.compute_cost_minimal_path();
+        if self.state(&self.goal).rhs == W::max_value() {
+            // no path exists
+            return None;
+        }
+
+        let mut reverse_path = vec![];
+
+        // identify a path from sstart to sgoal using the parent pointers
+        let mut target = self.state(&self.goal).par;
+        while !(Some(self.start) == target) && let Some(this_target) = target {
+            // hunter follows path from self.start to self.goal;
+            reverse_path.push(this_target);
+            target = self.state(&this_target).par;
+        }
+
+        // if hunter caught target {
+        //     return None;
+        // }
+
+        self.start = self.new_start;
+        self.goal = self.new_goal;
+
+        self.k_m = self.k_m + (self.heuristic)(&self.goal, &self.old_goal);
+
+        if self.old_start != self.start {
+            self.optimized_deletion();
+        }
+
+        while let Some(edge) = self.updated_edge_costs.pop() {
+            let (u_node, v_node) = (edge.predecessor, edge.successor);
+            // update the edge cost c(u, v);
+            if edge.old_cost > edge.cost {
+                let u_g = self.state(&u_node).g;
+                if v_node != self.start && self.state(&v_node).rhs > u_g + edge.cost {
+                    let v = self.state_mut(&v_node);
+                    v.par = Some(u_node);
+                    v.rhs = u_g + edge.cost;
+                }
+            } else if v_node != self.start && self.state(&v_node).par == Some(u_node) {
+                let mut min_pred = u_node;
+                let mut min_score = W::max_value();
+
+                for edge in (self.predecessors)(&v_node) {
+                    let s = self.state(&edge.target);
+                    let score = s.g + edge.cost;
+                    if score < min_score {
+                        min_score = score;
+                        min_pred = edge.target;
+                    }
+                }
+
+                let v = self.state_mut(&v_node);
+                v.rhs = min_score;
+                if v.rhs == W::max_value() {
+                    v.par = None;
+                } else {
+                    v.par = Some(min_pred);
+                }
+                self.update_state(&v_node);
+            }
+        }
+
+        Some(())
+    }
+
+    fn optimized_deletion(&mut self) {
+        let start = self.start;
+        self.state_mut(&start).par = None;
+
+        let mut min_pred = self.old_start;
+        let mut min_score = W::max_value();
+
+        for edge in (self.predecessors)(&self.old_start) {
+            let s = self.state(&edge.target);
+            let score = s.g + edge.cost;
+            if score < min_score {
+                min_score = score;
+                min_pred = edge.target;
+            }
+        }
+
+        let old_start = self.old_start;
+        let s = self.state_mut(&old_start);
+        s.rhs = min_score;
+        if s.rhs == W::max_value() {
+            s.par = None;
+        } else {
+            s.par = Some(min_pred);
+        }
+        self.update_state(&old_start);
+    }
+
+    fn state(&self, n: &N) -> &NodeState<N, W> {
+        self.node_states.get(n).unwrap_or(&self.default_state)
+    }
+
+    fn state_mut(&mut self, n: &N) -> &mut NodeState<N, W> {
+        self.node_states.entry(*n).or_default()
+    }
+}
+
+#[derive(Eq, PartialEq, Debug)]
+pub struct Priority<W>(W, W)
+where
+    W: PartialOrd + Debug;
+
+impl<W: PartialOrd + Debug> PartialOrd for Priority<W> {
+    fn partial_cmp(&self, other: &Self) -> Option<std::cmp::Ordering> {
+        if self.0 < other.0 {
+            Some(std::cmp::Ordering::Less)
+        } else if self.0 > other.0 {
+            Some(std::cmp::Ordering::Greater)
+        } else if self.1 < other.1 {
+            Some(std::cmp::Ordering::Less)
+        } else if self.1 > other.1 {
+            Some(std::cmp::Ordering::Greater)
+        } else {
+            Some(std::cmp::Ordering::Equal)
+        }
+    }
+}
+impl<W: PartialOrd + Debug + Eq> Ord for Priority<W> {
+    fn cmp(&self, other: &Self) -> std::cmp::Ordering {
+        self.partial_cmp(other)
+            .expect("Partial compare should not fail for Priority")
+    }
+}
+
+#[derive(Debug)]
+pub struct NodeState<N: Eq + Hash + Copy + Debug, W: Default + num_traits::Bounded + Debug> {
+    pub g: W,
+    pub rhs: W,
+    // future possible optimization: try making this a pointer
+    pub par: Option<N>,
+}
+
+impl<N: Eq + Hash + Copy + Debug, W: Default + num_traits::Bounded + Debug> Default
+    for NodeState<N, W>
+{
+    fn default() -> Self {
+        NodeState {
+            g: W::max_value(),
+            rhs: W::max_value(),
+            par: None,
+        }
+    }
+}
+
+pub struct Edge<N: Eq + Hash + Copy, W: PartialOrd + Copy> {
+    pub target: N,
+    pub cost: W,
+}
+
+pub struct ChangedEdge<N: Eq + Hash + Clone, W: PartialOrd + Copy> {
+    pub predecessor: N,
+    pub successor: N,
+    pub old_cost: W,
+    pub cost: W,
+}