detect obstructions while pathfinding and better results on timeout

4 files changed, 106 insertions, 20 deletions

diff --git a/azalea/examples/testbot.rs b/azalea/examples/testbot.rs
index 8e6f5b98..c562041e 100644
--- a/azalea/examples/testbot.rs
+++ b/azalea/examples/testbot.rs
@@ -129,6 +129,9 @@ async fn handle(mut bot: Client, event: Event, _state: State) -> anyhow::Result<
                     println!("going to {target_pos:?}");
                     bot.goto(BlockPosGoal::from(target_pos));
                 }
+                "worldborder" => {
+                    bot.goto(BlockPosGoal::from(BlockPos::new(30_000_000, 70, 0)));
+                }
                 "look" => {
                     let Some(entity) = entity else {
                         bot.chat("I can't see you");
diff --git a/azalea/src/pathfinder/astar.rs b/azalea/src/pathfinder/astar.rs
index ed39510f..8069ae6a 100644
--- a/azalea/src/pathfinder/astar.rs
+++ b/azalea/src/pathfinder/astar.rs
@@ -6,7 +6,7 @@ use std::{
     time::{Duration, Instant},
 };
 
-use log::info;
+use log::{info, warn};
 use priority_queue::PriorityQueue;
 
 pub struct Path<P, M>
@@ -17,6 +17,12 @@ where
     pub partial: bool,
 }
 
+// used for better results when timing out
+// see https://github.com/cabaletta/baritone/blob/1.19.4/src/main/java/baritone/pathing/calc/AbstractNodeCostSearch.java#L68
+const COEFFICIENTS: [f32; 7] = [1.5, 2., 2.5, 3., 4., 5., 10.];
+
+const MIN_IMPROVEMENT: f32 = 0.01;
+
 pub fn a_star<P, M, HeuristicFn, SuccessorsFn, SuccessFn>(
     start: P,
     heuristic: HeuristicFn,
@@ -46,8 +52,8 @@ where
         },
     );
 
-    let mut best_node = start;
-    let mut best_node_score = heuristic(start);
+    let mut best_paths: [P; 7] = [start; 7];
+    let mut best_path_scores: [f32; 7] = [heuristic(start); 7];
 
     while let Some((current_node, _)) = open_set.pop() {
         if success(current_node) {
@@ -83,10 +89,12 @@ where
                 );
                 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;
+                for (coefficient_i, &coefficient) in COEFFICIENTS.iter().enumerate() {
+                    let node_score = heuristic + tentative_g_score / coefficient;
+                    if best_path_scores[coefficient_i] - node_score > MIN_IMPROVEMENT {
+                        best_paths[coefficient_i] = neighbor.movement.target;
+                        best_path_scores[coefficient_i] = node_score;
+                    }
                 }
             }
         }
@@ -99,11 +107,30 @@ where
     }
 
     Path {
-        movements: reconstruct_path(nodes, best_node),
+        movements: reconstruct_path(nodes, determine_best_path(&best_paths, heuristic)),
         partial: true,
     }
 }
 
+const MIN_DISTANCE_PATH: f32 = 5.;
+
+fn determine_best_path<P, HeuristicFn>(best_node: &[P; 7], heuristic: HeuristicFn) -> P
+where
+    HeuristicFn: Fn(P) -> f32,
+    P: Eq + Hash + Copy + Debug,
+{
+    // this basically makes sure we don't create a path that's really short
+
+    for node in best_node.iter() {
+        // square MIN_DISTANCE_PATH because we're comparing squared distances
+        if heuristic(*node) > MIN_DISTANCE_PATH * MIN_DISTANCE_PATH {
+            return *node;
+        }
+    }
+    warn!("No best node found, returning first node");
+    return best_node[0];
+}
+
 fn reconstruct_path<P, M>(mut nodes: HashMap<P, Node<P, M>>, current: P) -> Vec<Movement<P, M>>
 where
     P: Eq + Hash + Copy + Debug,
diff --git a/azalea/src/pathfinder/mod.rs b/azalea/src/pathfinder/mod.rs
index dbaa57a2..e2be4da1 100644
--- a/azalea/src/pathfinder/mod.rs
+++ b/azalea/src/pathfinder/mod.rs
@@ -30,7 +30,7 @@ use bevy_ecs::query::Changed;
 use bevy_ecs::schedule::IntoSystemConfigs;
 use bevy_tasks::{AsyncComputeTaskPool, Task};
 use futures_lite::future;
-use log::{debug, error, trace};
+use log::{debug, error, trace, warn};
 use std::collections::VecDeque;
 use std::sync::Arc;
 use std::time::Duration;
@@ -69,6 +69,7 @@ impl Plugin for PathfinderPlugin {
 #[derive(Component, Default)]
 pub struct Pathfinder {
     pub path: VecDeque<astar::Movement<BlockPos, moves::MoveData>>,
+    pub last_reached_node: Option<BlockPos>,
 }
 #[allow(clippy::type_complexity)]
 fn add_default_pathfinder(
@@ -107,6 +108,7 @@ pub struct GotoEvent {
 #[derive(Event)]
 pub struct PathFoundEvent {
     pub entity: Entity,
+    pub start: BlockPos,
     pub path: Option<VecDeque<astar::Movement<BlockPos, moves::MoveData>>>,
 }
 
@@ -124,13 +126,13 @@ fn goto_listener(
     let thread_pool = AsyncComputeTaskPool::get();
 
     for event in events.iter() {
-        let (position, world_name) = query
+        let (position, instance_name) = query
             .get_mut(event.entity)
             .expect("Called goto on an entity that's not in the world");
         let start = BlockPos::from(position);
 
         let world_lock = instance_container
-            .get(world_name)
+            .get(instance_name)
             .expect("Entity tried to pathfind but the entity isn't in a valid world");
         let end = event.goal.goal_node();
 
@@ -165,6 +167,7 @@ fn goto_listener(
             let path = movements.into_iter().collect::<VecDeque<_>>();
             Some(PathFoundEvent {
                 entity,
+                start,
                 path: Some(path),
             })
         });
@@ -199,6 +202,7 @@ fn path_found_listener(mut events: EventReader<PathFoundEvent>, mut query: Query
             .expect("Path found for an entity that doesn't have a pathfinder");
         if let Some(path) = &event.path {
             pathfinder.path = path.to_owned();
+            pathfinder.last_reached_node = Some(event.start);
         } else {
             error!("No path found");
             pathfinder.path.clear();
@@ -207,13 +211,20 @@ fn path_found_listener(mut events: EventReader<PathFoundEvent>, mut query: Query
 }
 
 fn tick_execute_path(
-    mut query: Query<(Entity, &mut Pathfinder, &Position, &Physics)>,
+    mut query: Query<(Entity, &mut Pathfinder, &Position, &Physics, &InstanceName)>,
     mut look_at_events: EventWriter<LookAtEvent>,
     mut sprint_events: EventWriter<StartSprintEvent>,
     mut walk_events: EventWriter<StartWalkEvent>,
     mut jump_events: EventWriter<JumpEvent>,
+    instance_container: Res<InstanceContainer>,
 ) {
-    for (entity, mut pathfinder, position, physics) in &mut query {
+    let successors_fn = moves::basic::basic_move;
+
+    for (entity, mut pathfinder, position, physics, instance_name) in &mut query {
+        let world_lock = instance_container
+            .get(instance_name)
+            .expect("Entity tried to pathfind but the entity isn't in a valid world");
+
         loop {
             let Some(movement) = pathfinder.path.front() else {
                 break;
@@ -222,6 +233,7 @@ fn tick_execute_path(
             // we don't unnecessarily execute a movement when it wasn't necessary
             if is_goal_reached(movement.target, position, physics) {
                 // println!("reached target");
+                pathfinder.last_reached_node = Some(movement.target);
                 pathfinder.path.pop_front();
                 if pathfinder.path.is_empty() {
                     // println!("reached goal");
@@ -234,6 +246,26 @@ fn tick_execute_path(
                 continue;
             }
 
+            {
+                // obstruction check
+                let successors = |pos: BlockPos| {
+                    let world = world_lock.read();
+                    successors_fn(&world, pos)
+                };
+
+                if let Some(obstructed_index) =
+                    check_path_obstructed(
+                        pathfinder.last_reached_node.expect("last_reached_node is set when we start pathfinding, so it should always be Some here"),
+                        &pathfinder.path,
+                        successors
+                    )
+                {
+                    warn!("path obstructed at index {obstructed_index}");
+                    pathfinder.path.truncate(obstructed_index);
+                    continue;
+                }
+            }
+
             let ctx = ExecuteCtx {
                 entity,
                 target: movement.target,
@@ -278,16 +310,38 @@ pub trait Goal {
 /// next node.
 #[must_use]
 pub fn is_goal_reached(goal_pos: BlockPos, current_pos: &Position, physics: &Physics) -> bool {
-    // println!(
-    //     "entity.delta.y: {} {:?}=={:?}, self.vertical_vel={:?}",
-    //     entity.delta.y,
-    //     BlockPos::from(entity.pos()),
-    //     self.pos,
-    //     self.vertical_vel
-    // );
     BlockPos::from(current_pos) == goal_pos && physics.on_ground
 }
 
+/// Checks whether the path has been obstructed, and returns Some(index) if it
+/// has been. The index is of the first obstructed node.
+fn check_path_obstructed<SuccessorsFn>(
+    mut current_position: BlockPos,
+    path: &VecDeque<astar::Movement<BlockPos, moves::MoveData>>,
+    successors_fn: SuccessorsFn,
+) -> Option<usize>
+where
+    SuccessorsFn: Fn(BlockPos) -> Vec<astar::Edge<BlockPos, moves::MoveData>>,
+{
+    for (i, movement) in path.iter().enumerate() {
+        let mut found_obstruction = false;
+        for edge in successors_fn(current_position) {
+            if edge.movement.target == movement.target {
+                current_position = movement.target;
+                found_obstruction = false;
+                break;
+            } else {
+                found_obstruction = true;
+            }
+        }
+        if found_obstruction {
+            return Some(i);
+        }
+    }
+
+    None
+}
+
 #[cfg(test)]
 mod tests {
     use std::{collections::HashSet, sync::Arc};
diff --git a/azalea/src/pathfinder/simulation.rs b/azalea/src/pathfinder/simulation.rs
index 4f179795..42d42ff9 100644
--- a/azalea/src/pathfinder/simulation.rs
+++ b/azalea/src/pathfinder/simulation.rs
@@ -1,3 +1,5 @@
+//! Simulate the Minecraft world, currently only used for tests.
+
 use std::{sync::Arc, time::Duration};
 
 use azalea_client::PhysicsState;