infinite pathfinding

4 files changed, 219 insertions, 78 deletions

diff --git a/azalea/src/pathfinder/astar.rs b/azalea/src/pathfinder/astar.rs
index 8069ae6a..d4812e9b 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, warn};
+use log::{trace, warn};
 use priority_queue::PriorityQueue;
 
 pub struct Path<P, M>
@@ -74,7 +74,7 @@ where
                 .get(&neighbor.movement.target)
                 .map(|n| n.g_score)
                 .unwrap_or(f32::MAX);
-            if tentative_g_score < neighbor_g_score {
+            if tentative_g_score - neighbor_g_score < MIN_IMPROVEMENT {
                 let heuristic = heuristic(neighbor.movement.target);
                 let f_score = tentative_g_score + heuristic;
                 nodes.insert(
@@ -101,20 +101,22 @@ where
 
         if start_time.elapsed() > timeout {
             // timeout, just return the best path we have so far
-            info!("Pathfinder timeout");
+            trace!("A* couldn't find a path in time, returning best path");
             break;
         }
     }
 
+    let best_path = determine_best_path(&best_paths, &heuristic);
+
     Path {
-        movements: reconstruct_path(nodes, determine_best_path(&best_paths, heuristic)),
+        movements: reconstruct_path(nodes, best_path),
         partial: true,
     }
 }
 
 const MIN_DISTANCE_PATH: f32 = 5.;
 
-fn determine_best_path<P, HeuristicFn>(best_node: &[P; 7], heuristic: HeuristicFn) -> P
+fn determine_best_path<P, HeuristicFn>(best_node: &[P; 7], heuristic: &HeuristicFn) -> P
 where
     HeuristicFn: Fn(P) -> f32,
     P: Eq + Hash + Copy + Debug,
@@ -128,7 +130,7 @@ where
         }
     }
     warn!("No best node found, returning first node");
-    return best_node[0];
+    best_node[0]
 }
 
 fn reconstruct_path<P, M>(mut nodes: HashMap<P, Node<P, M>>, current: P) -> Vec<Movement<P, M>>
diff --git a/azalea/src/pathfinder/mod.rs b/azalea/src/pathfinder/mod.rs
index e2be4da1..890ef9df 100644
--- a/azalea/src/pathfinder/mod.rs
+++ b/azalea/src/pathfinder/mod.rs
@@ -30,10 +30,10 @@ use bevy_ecs::query::Changed;
 use bevy_ecs::schedule::IntoSystemConfigs;
 use bevy_tasks::{AsyncComputeTaskPool, Task};
 use futures_lite::future;
-use log::{debug, error, trace, warn};
+use log::{debug, error, info, trace, warn};
 use std::collections::VecDeque;
 use std::sync::Arc;
-use std::time::Duration;
+use std::time::{Duration, Instant};
 
 use self::moves::ExecuteCtx;
 
@@ -69,8 +69,27 @@ impl Plugin for PathfinderPlugin {
 #[derive(Component, Default)]
 pub struct Pathfinder {
     pub path: VecDeque<astar::Movement<BlockPos, moves::MoveData>>,
+    pub queued_path: Option<VecDeque<astar::Movement<BlockPos, moves::MoveData>>>,
+    pub is_path_partial: bool,
+
     pub last_reached_node: Option<BlockPos>,
+    pub last_node_reached_at: Option<Instant>,
+    pub goal: Option<Arc<dyn Goal + Send + Sync>>,
+    pub is_calculating: bool,
 }
+#[derive(Event)]
+pub struct GotoEvent {
+    pub entity: Entity,
+    pub goal: Arc<dyn Goal + Send + Sync>,
+}
+#[derive(Event)]
+pub struct PathFoundEvent {
+    pub entity: Entity,
+    pub start: BlockPos,
+    pub path: Option<VecDeque<astar::Movement<BlockPos, moves::MoveData>>>,
+    pub is_partial: bool,
+}
+
 #[allow(clippy::type_complexity)]
 fn add_default_pathfinder(
     mut commands: Commands,
@@ -100,17 +119,6 @@ impl PathfinderClientExt for azalea_client::Client {
         });
     }
 }
-#[derive(Event)]
-pub struct GotoEvent {
-    pub entity: Entity,
-    pub goal: Arc<dyn Goal + Send + Sync>,
-}
-#[derive(Event)]
-pub struct PathFoundEvent {
-    pub entity: Entity,
-    pub start: BlockPos,
-    pub path: Option<VecDeque<astar::Movement<BlockPos, moves::MoveData>>>,
-}
 
 #[derive(Component)]
 pub struct ComputePath(Task<Option<PathFoundEvent>>);
@@ -118,7 +126,7 @@ pub struct ComputePath(Task<Option<PathFoundEvent>>);
 fn goto_listener(
     mut commands: Commands,
     mut events: EventReader<GotoEvent>,
-    mut query: Query<(&Position, &InstanceName)>,
+    mut query: Query<(&mut Pathfinder, &Position, &InstanceName)>,
     instance_container: Res<InstanceContainer>,
 ) {
     let successors_fn = moves::basic::basic_move;
@@ -126,9 +134,14 @@ fn goto_listener(
     let thread_pool = AsyncComputeTaskPool::get();
 
     for event in events.iter() {
-        let (position, instance_name) = query
+        let (mut pathfinder, position, instance_name) = query
             .get_mut(event.entity)
             .expect("Called goto on an entity that's not in the world");
+
+        // we store the goal so it can be recalculated later if necessary
+        pathfinder.goal = Some(event.goal.clone());
+        pathfinder.is_calculating = true;
+
         let start = BlockPos::from(position);
 
         let world_lock = instance_container
@@ -147,28 +160,50 @@ fn goto_listener(
                 successors_fn(&world, pos)
             };
 
-            let start_time = std::time::Instant::now();
-            let astar::Path { movements, partial } = a_star(
-                start,
-                |n| goal.heuristic(n),
-                successors,
-                |n| goal.success(n),
-                Duration::from_secs(1),
-            );
-            let end_time = std::time::Instant::now();
-            debug!("partial: {partial:?}");
-            debug!("time: {:?}", end_time - start_time);
+            let mut attempt_number = 0;
+
+            let mut path;
+            let mut is_partial: bool;
+
+            'calculate: loop {
+                let start_time = std::time::Instant::now();
+                let astar::Path { movements, partial } = a_star(
+                    start,
+                    |n| goal.heuristic(n),
+                    successors,
+                    |n| goal.success(n),
+                    Duration::from_secs(if attempt_number == 0 { 1 } else { 5 }),
+                );
+                let end_time = std::time::Instant::now();
+                debug!("partial: {partial:?}");
+                debug!("time: {:?}", end_time - start_time);
+
+                info!("Path:");
+                for movement in &movements {
+                    info!("  {:?}", movement.target);
+                }
 
-            println!("Path:");
-            for movement in &movements {
-                println!("  {:?}", movement.target);
+                path = movements.into_iter().collect::<VecDeque<_>>();
+                is_partial = partial;
+
+                if path.is_empty() && partial {
+                    if attempt_number == 0 {
+                        debug!("this path is empty, retrying with a higher timeout");
+                        attempt_number += 1;
+                        continue 'calculate;
+                    } else {
+                        debug!("this path is empty, giving up");
+                        break 'calculate;
+                    }
+                }
+                break;
             }
 
-            let path = movements.into_iter().collect::<VecDeque<_>>();
             Some(PathFoundEvent {
                 entity,
                 start,
                 path: Some(path),
+                is_partial,
             })
         });
 
@@ -201,12 +236,20 @@ fn path_found_listener(mut events: EventReader<PathFoundEvent>, mut query: Query
             .get_mut(event.entity)
             .expect("Path found for an entity that doesn't have a pathfinder");
         if let Some(path) = &event.path {
-            pathfinder.path = path.to_owned();
+            if pathfinder.path.is_empty() {
+                pathfinder.path = path.to_owned();
+            } else {
+                pathfinder.queued_path = Some(path.to_owned());
+            }
             pathfinder.last_reached_node = Some(event.start);
+            pathfinder.last_node_reached_at = Some(Instant::now());
         } else {
             error!("No path found");
             pathfinder.path.clear();
+            pathfinder.queued_path = None;
         }
+        pathfinder.is_calculating = false;
+        pathfinder.is_path_partial = event.is_partial;
     }
 }
 
@@ -216,56 +259,88 @@ fn tick_execute_path(
     mut sprint_events: EventWriter<StartSprintEvent>,
     mut walk_events: EventWriter<StartWalkEvent>,
     mut jump_events: EventWriter<JumpEvent>,
+    mut goto_events: EventWriter<GotoEvent>,
     instance_container: Res<InstanceContainer>,
 ) {
     let successors_fn = moves::basic::basic_move;
 
     for (entity, mut pathfinder, position, physics, instance_name) in &mut query {
+        if pathfinder.goal.is_none() {
+            // no goal, no pathfinding
+            continue;
+        }
+
         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;
-            };
-            // we check if the goal was reached *before* actually executing the movement so
-            // 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");
-                    walk_events.send(StartWalkEvent {
-                        entity,
-                        direction: WalkDirection::None,
-                    });
+        if !pathfinder.is_calculating {
+            // timeout check
+            if let Some(last_node_reached_at) = pathfinder.last_node_reached_at {
+                if last_node_reached_at.elapsed() > Duration::from_secs(2) {
+                    warn!("pathfinder timeout");
+                    pathfinder.path.clear();
                 }
-                // tick again, maybe we already reached the next node!
-                continue;
             }
+        }
 
-            {
-                // obstruction check
-                let successors = |pos: BlockPos| {
-                    let world = world_lock.read();
-                    successors_fn(&world, pos)
-                };
+        'skip: loop {
+            // we check if the goal was reached *before* actually executing the movement so
+            // we don't unnecessarily execute a movement when it wasn't necessary
 
-                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;
+            // see if we already reached any future nodes and can skip ahead
+            for (i, movement) in pathfinder
+                .path
+                .clone()
+                .into_iter()
+                .enumerate()
+                .take(10)
+                .rev()
+            {
+                if is_goal_reached(movement.target, position, physics) {
+                    pathfinder.path = pathfinder.path.split_off(i + 1);
+                    pathfinder.last_reached_node = Some(movement.target);
+                    pathfinder.last_node_reached_at = Some(Instant::now());
+
+                    if let Some(new_path) = pathfinder.queued_path.take() {
+                        debug!(
+                            "swapped path to {:?}",
+                            new_path.iter().take(10).collect::<Vec<_>>()
+                        );
+                        pathfinder.path = new_path;
+
+                        if pathfinder.path.is_empty() {
+                            info!("the path we just swapped to was empty, so reached end of path");
+                            walk_events.send(StartWalkEvent {
+                                entity,
+                                direction: WalkDirection::None,
+                            });
+                            break;
+                        }
+
+                        // run the function again since we just swapped
+                        continue 'skip;
+                    }
+
+                    if pathfinder.path.is_empty() {
+                        debug!("pathfinder path is now empty");
+                        walk_events.send(StartWalkEvent {
+                            entity,
+                            direction: WalkDirection::None,
+                        });
+                        if let Some(goal) = pathfinder.goal.clone() {
+                            if goal.success(movement.target) {
+                                info!("goal was reached!");
+                                pathfinder.goal = None;
+                            }
+                        }
+                    }
                 }
             }
+            break;
+        }
 
+        if let Some(movement) = pathfinder.path.front() {
             let ctx = ExecuteCtx {
                 entity,
                 target: movement.target,
@@ -277,7 +352,55 @@ fn tick_execute_path(
             };
             trace!("executing move");
             (movement.data.execute)(ctx);
-            break;
+        }
+
+        {
+            // obstruction check
+            let successors = |pos: BlockPos| {
+                let world = world_lock.read();
+                successors_fn(&world, pos)
+            };
+
+            if let Some(last_reached_node) = pathfinder.last_reached_node {
+                if let Some(obstructed_index) =
+                    check_path_obstructed(last_reached_node, &pathfinder.path, successors)
+                {
+                    warn!("path obstructed at index {obstructed_index} (starting at {last_reached_node:?}, path: {:?})", pathfinder.path);
+                    pathfinder.path.truncate(obstructed_index);
+                }
+            }
+        }
+
+        {
+            // start recalculating if the path ends soon
+            if pathfinder.path.len() < 5 && !pathfinder.is_calculating && pathfinder.is_path_partial
+            {
+                if let Some(goal) = pathfinder.goal.as_ref().cloned() {
+                    debug!("Recalculating path because it ends soon");
+                    goto_events.send(GotoEvent { entity, goal });
+
+                    if pathfinder.path.is_empty() {
+                        if let Some(new_path) = pathfinder.queued_path.take() {
+                            pathfinder.path = new_path;
+                            if pathfinder.path.is_empty() {
+                                info!(
+                                    "the path we just swapped to was empty, so reached end of path"
+                                );
+                                walk_events.send(StartWalkEvent {
+                                    entity,
+                                    direction: WalkDirection::None,
+                                });
+                                break;
+                            }
+                        } else {
+                            walk_events.send(StartWalkEvent {
+                                entity,
+                                direction: WalkDirection::None,
+                            });
+                        }
+                    }
+                }
+            }
         }
     }
 }
@@ -348,7 +471,6 @@ mod tests {
 
     use azalea_core::{BlockPos, ChunkPos, Vec3};
     use azalea_world::{Chunk, ChunkStorage, PartialChunkStorage};
-    use bevy_log::LogPlugin;
     use log::info;
 
     use super::{
@@ -381,10 +503,10 @@ mod tests {
             start_pos.z as f64 + 0.5,
         ));
         let mut simulation = Simulation::new(chunks, player);
-        simulation.app.add_plugins(LogPlugin {
-            level: bevy_log::Level::TRACE,
-            filter: "".to_string(),
-        });
+        // simulation.app.add_plugins(bevy_log::LogPlugin {
+        //     level: bevy_log::Level::TRACE,
+        //     filter: "".to_string(),
+        // });
 
         simulation.app.world.send_event(GotoEvent {
             entity: simulation.entity,
diff --git a/azalea/src/pathfinder/moves/basic.rs b/azalea/src/pathfinder/moves/basic.rs
index b8886c9e..a1175717 100644
--- a/azalea/src/pathfinder/moves/basic.rs
+++ b/azalea/src/pathfinder/moves/basic.rs
@@ -1,3 +1,5 @@
+use std::f32::consts::SQRT_2;
+
 use azalea_client::{SprintDirection, StartSprintEvent};
 use azalea_core::{BlockPos, CardinalDirection};
 use azalea_world::Instance;
@@ -183,7 +185,7 @@ fn diagonal_move(world: &Instance, pos: BlockPos) -> Vec<Edge> {
         if !is_standable(&(pos + offset), world) {
             continue;
         }
-        let cost = SPRINT_ONE_BLOCK_COST * 1.4;
+        let cost = SPRINT_ONE_BLOCK_COST * SQRT_2;
 
         edges.push(Edge {
             movement: astar::Movement {
diff --git a/azalea/src/pathfinder/moves/mod.rs b/azalea/src/pathfinder/moves/mod.rs
index 47589264..3fad823c 100644
--- a/azalea/src/pathfinder/moves/mod.rs
+++ b/azalea/src/pathfinder/moves/mod.rs
@@ -1,5 +1,7 @@
 pub mod basic;
 
+use std::fmt::Debug;
+
 use crate::{JumpEvent, LookAtEvent};
 
 use super::astar;
@@ -16,6 +18,13 @@ pub struct MoveData {
     // pub move_kind: BasicMoves,
     pub execute: &'static (dyn Fn(ExecuteCtx) + Send + Sync),
 }
+impl Debug for MoveData {
+    fn fmt(&self, f: &mut std::fmt::Formatter<'_>) -> std::fmt::Result {
+        f.debug_struct("MoveData")
+            // .field("move_kind", &self.move_kind)
+            .finish()
+    }
+}
 
 /// whether this block is passable
 fn is_block_passable(pos: &BlockPos, world: &Instance) -> bool {
@@ -29,6 +38,12 @@ fn is_block_passable(pos: &BlockPos, world: &Instance) -> bool {
         if block.waterlogged() {
             return false;
         }
+        // block.waterlogged currently doesn't account for seagrass and some other water
+        // blocks
+        if block == azalea_registry::Block::Seagrass.into() {
+            return false;
+        }
+
         block.shape() == &collision::empty_shape()
     } else {
         false