simplify pathfinder

1 files changed, 109 insertions, 43 deletions

diff --git a/azalea/src/pathfinder/astar.rs b/azalea/src/pathfinder/astar.rs
index 0bdd0f17..ed39510f 100644
--- a/azalea/src/pathfinder/astar.rs
+++ b/azalea/src/pathfinder/astar.rs
@@ -1,110 +1,176 @@
-use std::{cmp::Reverse, collections::HashMap, fmt::Debug, hash::Hash, ops::Add};
+use std::{
+    cmp::Reverse,
+    collections::HashMap,
+    fmt::Debug,
+    hash::Hash,
+    time::{Duration, Instant},
+};
 
+use log::info;
 use priority_queue::PriorityQueue;
 
-pub fn a_star<N, W, HeuristicFn, SuccessorsFn, SuccessFn>(
-    start: N,
+pub struct Path<P, M>
+where
+    P: Eq + Hash + Copy + Debug,
+{
+    pub movements: Vec<Movement<P, M>>,
+    pub partial: bool,
+}
+
+pub fn a_star<P, M, HeuristicFn, SuccessorsFn, SuccessFn>(
+    start: P,
     heuristic: HeuristicFn,
     successors: SuccessorsFn,
     success: SuccessFn,
-) -> Option<Vec<N>>
+    timeout: Duration,
+) -> Path<P, M>
 where
-    N: Eq + Hash + Copy + Debug,
-    W: PartialOrd + Default + Copy + num_traits::Bounded + Debug + Add<Output = W>,
-    HeuristicFn: Fn(&N) -> W,
-    SuccessorsFn: Fn(&N) -> Vec<Edge<N, W>>,
-    SuccessFn: Fn(&N) -> bool,
+    P: Eq + Hash + Copy + Debug,
+    HeuristicFn: Fn(P) -> f32,
+    SuccessorsFn: Fn(P) -> Vec<Edge<P, M>>,
+    SuccessFn: Fn(P) -> bool,
 {
+    let start_time = Instant::now();
+
     let mut open_set = PriorityQueue::new();
-    open_set.push(start, Reverse(Weight(W::default())));
-    let mut nodes: HashMap<N, Node<N, W>> = HashMap::new();
+    open_set.push(start, Reverse(Weight(0.)));
+    let mut nodes: HashMap<P, Node<P, M>> = HashMap::new();
     nodes.insert(
         start,
         Node {
-            data: start,
+            position: start,
+            movement_data: None,
             came_from: None,
-            g_score: W::default(),
-            f_score: W::max_value(),
+            g_score: f32::default(),
+            f_score: f32::MAX,
         },
     );
 
+    let mut best_node = start;
+    let mut best_node_score = heuristic(start);
+
     while let Some((current_node, _)) = open_set.pop() {
-        if success(&current_node) {
-            return Some(reconstruct_path(&nodes, current_node));
+        if success(current_node) {
+            return Path {
+                movements: reconstruct_path(nodes, current_node),
+                partial: false,
+            };
         }
 
         let current_g_score = nodes
             .get(&current_node)
             .map(|n| n.g_score)
-            .unwrap_or(W::max_value());
+            .unwrap_or(f32::MAX);
 
-        for neighbor in successors(&current_node) {
+        for neighbor in successors(current_node) {
             let tentative_g_score = current_g_score + neighbor.cost;
             let neighbor_g_score = nodes
-                .get(&neighbor.target)
+                .get(&neighbor.movement.target)
                 .map(|n| n.g_score)
-                .unwrap_or(W::max_value());
+                .unwrap_or(f32::MAX);
             if tentative_g_score < neighbor_g_score {
-                let f_score = tentative_g_score + heuristic(&neighbor.target);
+                let heuristic = heuristic(neighbor.movement.target);
+                let f_score = tentative_g_score + heuristic;
                 nodes.insert(
-                    neighbor.target,
+                    neighbor.movement.target,
                     Node {
-                        data: neighbor.target,
+                        position: neighbor.movement.target,
+                        movement_data: Some(neighbor.movement.data),
                         came_from: Some(current_node),
                         g_score: tentative_g_score,
                         f_score,
                     },
                 );
-                open_set.push(neighbor.target, Reverse(Weight(f_score)));
+                open_set.push(neighbor.movement.target, Reverse(Weight(f_score)));
+
+                let node_score = heuristic + tentative_g_score / 1.5;
+                if node_score < best_node_score {
+                    best_node = neighbor.movement.target;
+                    best_node_score = node_score;
+                }
             }
         }
+
+        if start_time.elapsed() > timeout {
+            // timeout, just return the best path we have so far
+            info!("Pathfinder timeout");
+            break;
+        }
     }
 
-    None
+    Path {
+        movements: reconstruct_path(nodes, best_node),
+        partial: true,
+    }
 }
 
-fn reconstruct_path<N, W>(nodes: &HashMap<N, Node<N, W>>, current: N) -> Vec<N>
+fn reconstruct_path<P, M>(mut nodes: HashMap<P, Node<P, M>>, current: P) -> Vec<Movement<P, M>>
 where
-    N: Eq + Hash + Copy + Debug,
-    W: PartialOrd + Default + Copy + num_traits::Bounded + Debug,
+    P: Eq + Hash + Copy + Debug,
 {
-    let mut path = vec![current];
+    let mut path = Vec::new();
     let mut current = current;
-    while let Some(node) = nodes.get(&current) {
+    while let Some(node) = nodes.remove(&current) {
         if let Some(came_from) = node.came_from {
-            path.push(came_from);
             current = came_from;
         } else {
             break;
         }
+        path.push(Movement {
+            target: node.position,
+            data: node.movement_data.unwrap(),
+        });
     }
     path.reverse();
     path
 }
 
-pub struct Node<N, W> {
-    pub data: N,
-    pub came_from: Option<N>,
-    pub g_score: W,
-    pub f_score: W,
+pub struct Node<P, M> {
+    pub position: P,
+    pub movement_data: Option<M>,
+    pub came_from: Option<P>,
+    pub g_score: f32,
+    pub f_score: f32,
+}
+
+pub struct Edge<P: Hash + Copy, M> {
+    pub movement: Movement<P, M>,
+    pub cost: f32,
+}
+
+pub struct Movement<P: Hash + Copy, M> {
+    pub target: P,
+    pub data: M,
 }
 
-pub struct Edge<N: Eq + Hash + Copy, W: PartialOrd + Copy> {
-    pub target: N,
-    pub cost: W,
+impl<P: Hash + Copy + Debug, M: Debug> Debug for Movement<P, M> {
+    fn fmt(&self, f: &mut std::fmt::Formatter<'_>) -> std::fmt::Result {
+        f.debug_struct("Movement")
+            .field("target", &self.target)
+            .field("data", &self.data)
+            .finish()
+    }
+}
+impl<P: Hash + Copy + Clone, M: Clone> Clone for Movement<P, M> {
+    fn clone(&self) -> Self {
+        Self {
+            target: self.target,
+            data: self.data.clone(),
+        }
+    }
 }
 
 #[derive(PartialEq)]
-pub struct Weight<W: PartialOrd + Debug>(W);
-impl<W: PartialOrd + Debug> Ord for Weight<W> {
+pub struct Weight(f32);
+impl Ord for Weight {
     fn cmp(&self, other: &Self) -> std::cmp::Ordering {
         self.0
             .partial_cmp(&other.0)
             .unwrap_or(std::cmp::Ordering::Equal)
     }
 }
-impl<W: PartialOrd + Debug> Eq for Weight<W> {}
-impl<W: PartialOrd + Debug> PartialOrd for Weight<W> {
+impl Eq for Weight {}
+impl PartialOrd for Weight {
     fn partial_cmp(&self, other: &Self) -> Option<std::cmp::Ordering> {
         Some(self.cmp(other))
     }