pathfinder: don't spin while descending

5 files changed, 112 insertions, 28 deletions

diff --git a/azalea-client/src/movement.rs b/azalea-client/src/movement.rs
index 782e98ff..67588794 100644
--- a/azalea-client/src/movement.rs
+++ b/azalea-client/src/movement.rs
@@ -381,6 +381,8 @@ impl Client {
 
 /// An event sent when the client starts walking. This does not get sent for
 /// non-local entities.
+///
+/// To stop walking or sprinting, send this event with `WalkDirection::None`.
 #[derive(Event, Debug)]
 pub struct StartWalkEvent {
     pub entity: Entity,
diff --git a/azalea/README.md b/azalea/README.md
index 69a6f26a..ea882fc5 100755
--- a/azalea/README.md
+++ b/azalea/README.md
@@ -12,10 +12,10 @@ default nightly`.
 
 Then, add one of the following lines to your Cargo.toml:
 
-Latest bleeding-edge version:
+Latest bleeding-edge version (recommended):
 `azalea = { git="https://github.com/mat-1/azalea" }`\
 Latest "stable" release:
-`azalea = "0.7.0"`
+`azalea = "0.8.0"`
 
 ## Optimization
 
diff --git a/azalea/src/pathfinder/mod.rs b/azalea/src/pathfinder/mod.rs
index e656be47..cd8a3301 100644
--- a/azalea/src/pathfinder/mod.rs
+++ b/azalea/src/pathfinder/mod.rs
@@ -35,7 +35,7 @@ use std::collections::VecDeque;
 use std::sync::Arc;
 use std::time::{Duration, Instant};
 
-use self::moves::ExecuteCtx;
+use self::moves::{ExecuteCtx, IsReachedCtx, SuccessorsFn};
 
 #[derive(Clone, Default)]
 pub struct PathfinderPlugin;
@@ -75,12 +75,16 @@ pub struct Pathfinder {
     pub last_reached_node: Option<BlockPos>,
     pub last_node_reached_at: Option<Instant>,
     pub goal: Option<Arc<dyn Goal + Send + Sync>>,
+    pub successors_fn: Option<SuccessorsFn>,
     pub is_calculating: bool,
 }
 #[derive(Event)]
 pub struct GotoEvent {
     pub entity: Entity,
     pub goal: Arc<dyn Goal + Send + Sync>,
+    /// The function that's used for checking what moves are possible. Usually
+    /// `pathfinder::moves::basic::basic_move`
+    pub successors_fn: SuccessorsFn,
 }
 #[derive(Event)]
 pub struct PathFoundEvent {
@@ -88,6 +92,7 @@ pub struct PathFoundEvent {
     pub start: BlockPos,
     pub path: Option<VecDeque<astar::Movement<BlockPos, moves::MoveData>>>,
     pub is_partial: bool,
+    pub successors_fn: SuccessorsFn,
 }
 
 #[allow(clippy::type_complexity)]
@@ -116,6 +121,7 @@ impl PathfinderClientExt for azalea_client::Client {
         self.ecs.lock().send_event(GotoEvent {
             entity: self.entity,
             goal: Arc::new(goal),
+            successors_fn: moves::basic::basic_move,
         });
     }
 }
@@ -129,8 +135,6 @@ fn goto_listener(
     mut query: Query<(&mut Pathfinder, &Position, &InstanceName)>,
     instance_container: Res<InstanceContainer>,
 ) {
-    let successors_fn = moves::basic::basic_move;
-
     let thread_pool = AsyncComputeTaskPool::get();
 
     for event in events.iter() {
@@ -140,6 +144,7 @@ fn goto_listener(
 
         // we store the goal so it can be recalculated later if necessary
         pathfinder.goal = Some(event.goal.clone());
+        pathfinder.successors_fn = Some(event.successors_fn.clone());
         pathfinder.is_calculating = true;
 
         let start = if pathfinder.path.is_empty() {
@@ -157,6 +162,8 @@ fn goto_listener(
             BlockPos::from(position)
         );
 
+        let successors_fn: moves::SuccessorsFn = event.successors_fn;
+
         let world_lock = instance_container
             .get(instance_name)
             .expect("Entity tried to pathfind but the entity isn't in a valid world");
@@ -216,6 +223,7 @@ fn goto_listener(
                 start,
                 path: Some(path),
                 is_partial,
+                successors_fn,
             })
         });
 
@@ -266,7 +274,7 @@ fn path_found_listener(
                         let world_lock = instance_container.get(instance_name).expect(
                             "Entity tried to pathfind but the entity isn't in a valid world",
                         );
-                        let successors_fn = moves::basic::basic_move;
+                        let successors_fn: moves::SuccessorsFn = event.successors_fn;
                         let successors = |pos: BlockPos| {
                             let world = world_lock.read();
                             successors_fn(&world, pos)
@@ -312,14 +320,16 @@ fn tick_execute_path(
     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 successors_fn: moves::SuccessorsFn = pathfinder
+            .successors_fn
+            .expect("pathfinder.successors_fn should be Some if the goal is Some");
+
         let world_lock = instance_container
             .get(instance_name)
             .expect("Entity tried to pathfind but the entity isn't in a valid world");
@@ -349,7 +359,15 @@ fn tick_execute_path(
                 .take(10)
                 .rev()
             {
-                if is_goal_reached(movement.target, position, physics) {
+                let is_reached_ctx = IsReachedCtx {
+                    target: movement.target,
+                    start: pathfinder.last_reached_node.expect(
+                        "pathfinder.last_node_reached_at should always be present if there's a path",
+                    ),
+                    position: **position,
+                    physics,
+                };
+                if (movement.data.is_reached)(is_reached_ctx) {
                     pathfinder.path = pathfinder.path.split_off(i + 1);
                     pathfinder.last_reached_node = Some(movement.target);
                     pathfinder.last_node_reached_at = Some(Instant::now());
@@ -384,6 +402,7 @@ fn tick_execute_path(
                             if goal.success(movement.target) {
                                 info!("goal was reached!");
                                 pathfinder.goal = None;
+                                pathfinder.successors_fn = None;
                             }
                         }
                     }
@@ -397,6 +416,9 @@ fn tick_execute_path(
                 entity,
                 target: movement.target,
                 position: **position,
+                start: pathfinder.last_reached_node.expect(
+                    "pathfinder.last_reached_node should always be present if there's a path",
+                ),
                 look_at_events: &mut look_at_events,
                 sprint_events: &mut sprint_events,
                 walk_events: &mut walk_events,
@@ -429,7 +451,11 @@ fn tick_execute_path(
             {
                 if let Some(goal) = pathfinder.goal.as_ref().cloned() {
                     debug!("Recalculating path because it ends soon");
-                    goto_events.send(GotoEvent { entity, goal });
+                    goto_events.send(GotoEvent {
+                        entity,
+                        goal,
+                        successors_fn,
+                    });
 
                     if pathfinder.path.is_empty() {
                         if let Some(new_path) = pathfinder.queued_path.take() {
@@ -478,13 +504,6 @@ pub trait Goal {
     fn success(&self, n: BlockPos) -> bool;
 }
 
-/// Returns whether the entity is at the node and should start going to the
-/// next node.
-#[must_use]
-pub fn is_goal_reached(goal_pos: BlockPos, current_pos: &Position, physics: &Physics) -> bool {
-    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>(
@@ -524,6 +543,7 @@ mod tests {
 
     use super::{
         goals::BlockPosGoal,
+        moves,
         simulation::{SimulatedPlayerBundle, Simulation},
         GotoEvent,
     };
@@ -560,6 +580,7 @@ mod tests {
         simulation.app.world.send_event(GotoEvent {
             entity: simulation.entity,
             goal: Arc::new(BlockPosGoal(end_pos)),
+            successors_fn: moves::basic::basic_move,
         });
         simulation
     }
diff --git a/azalea/src/pathfinder/moves/basic.rs b/azalea/src/pathfinder/moves/basic.rs
index a1175717..5d8f1c54 100644
--- a/azalea/src/pathfinder/moves/basic.rs
+++ b/azalea/src/pathfinder/moves/basic.rs
@@ -10,7 +10,8 @@ use crate::{
 };
 
 use super::{
-    fall_distance, is_block_passable, is_passable, is_standable, Edge, ExecuteCtx, MoveData,
+    default_is_reached, fall_distance, is_block_passable, is_passable, is_standable, Edge,
+    ExecuteCtx, IsReachedCtx, MoveData,
 };
 
 pub fn basic_move(world: &Instance, node: BlockPos) -> Vec<Edge> {
@@ -38,6 +39,7 @@ fn forward_move(world: &Instance, pos: BlockPos) -> Vec<Edge> {
                 target: pos + offset,
                 data: MoveData {
                     execute: &execute_forward_move,
+                    is_reached: &default_is_reached,
                 },
             },
             cost,
@@ -86,6 +88,7 @@ fn ascend_move(world: &Instance, pos: BlockPos) -> Vec<Edge> {
                 target: pos + offset,
                 data: MoveData {
                     execute: &execute_ascend_move,
+                    is_reached: &default_is_reached,
                 },
             },
             cost,
@@ -140,6 +143,7 @@ fn descend_move(world: &Instance, pos: BlockPos) -> Vec<Edge> {
                 target: new_position,
                 data: MoveData {
                     execute: &execute_descend_move,
+                    is_reached: &is_reached_descend_move,
                 },
             },
             cost,
@@ -151,20 +155,46 @@ fn execute_descend_move(
     ExecuteCtx {
         entity,
         target,
+        start,
         look_at_events,
         sprint_events,
+        position,
         ..
     }: ExecuteCtx,
 ) {
     let center = target.center();
-    look_at_events.send(LookAtEvent {
-        entity,
-        position: center,
-    });
-    sprint_events.send(StartSprintEvent {
-        entity,
-        direction: SprintDirection::Forward,
-    });
+    let horizontal_distance_from_target = (center - position).horizontal_distance_sqr().sqrt();
+
+    let dest_ahead = (start + (target - start) * 2).center();
+
+    println!();
+    println!("center: {center:?}, dest_ahead: {dest_ahead:?}");
+    println!("position: {position:?}");
+
+    if BlockPos::from(position) != target || horizontal_distance_from_target > 0.25 {
+        look_at_events.send(LookAtEvent {
+            entity,
+            position: dest_ahead,
+        });
+        sprint_events.send(StartSprintEvent {
+            entity,
+            direction: SprintDirection::Forward,
+        });
+    }
+}
+#[must_use]
+pub fn is_reached_descend_move(
+    IsReachedCtx {
+        target,
+        start,
+        position,
+        ..
+    }: IsReachedCtx,
+) -> bool {
+    let dest_ahead = start + (target - start) * 2;
+
+    (BlockPos::from(position) == target || BlockPos::from(position) == dest_ahead)
+        && (position.y - target.y as f64) < 0.5
 }
 
 fn diagonal_move(world: &Instance, pos: BlockPos) -> Vec<Edge> {
@@ -192,6 +222,7 @@ fn diagonal_move(world: &Instance, pos: BlockPos) -> Vec<Edge> {
                 target: pos + offset,
                 data: MoveData {
                     execute: &execute_diagonal_move,
+                    is_reached: &default_is_reached,
                 },
             },
             cost,
diff --git a/azalea/src/pathfinder/moves/mod.rs b/azalea/src/pathfinder/moves/mod.rs
index 3fad823c..9aa6b363 100644
--- a/azalea/src/pathfinder/moves/mod.rs
+++ b/azalea/src/pathfinder/moves/mod.rs
@@ -13,10 +13,16 @@ use bevy_ecs::{entity::Entity, event::EventWriter};
 
 type Edge = astar::Edge<BlockPos, MoveData>;
 
+pub type SuccessorsFn =
+    fn(&azalea_world::Instance, BlockPos) -> Vec<astar::Edge<BlockPos, MoveData>>;
+
 #[derive(Clone)]
 pub struct MoveData {
-    // pub move_kind: BasicMoves,
+    /// Use the context to determine what events should be sent to complete this
+    /// movement.
     pub execute: &'static (dyn Fn(ExecuteCtx) + Send + Sync),
+    /// Whether we've reached the target.
+    pub is_reached: &'static (dyn Fn(IsReachedCtx) -> bool + Send + Sync),
 }
 impl Debug for MoveData {
     fn fmt(&self, f: &mut std::fmt::Formatter<'_>) -> std::fmt::Result {
@@ -85,10 +91,12 @@ fn fall_distance(pos: &BlockPos, world: &Instance) -> u32 {
     }
     distance
 }
-
 pub struct ExecuteCtx<'w1, 'w2, 'w3, 'w4, 'a> {
     pub entity: Entity,
+    /// The node that we're trying to reach.
     pub target: BlockPos,
+    /// The last node that we reached.
+    pub start: BlockPos,
     pub position: Vec3,
 
     pub look_at_events: &'a mut EventWriter<'w1, LookAtEvent>,
@@ -96,6 +104,28 @@ pub struct ExecuteCtx<'w1, 'w2, 'w3, 'w4, 'a> {
     pub walk_events: &'a mut EventWriter<'w3, StartWalkEvent>,
     pub jump_events: &'a mut EventWriter<'w4, JumpEvent>,
 }
+pub struct IsReachedCtx<'a> {
+    /// The node that we're trying to reach.
+    pub target: BlockPos,
+    /// The last node that we reached.
+    pub start: BlockPos,
+    pub position: Vec3,
+    pub physics: &'a azalea_entity::Physics,
+}
+
+/// Returns whether the entity is at the node and should start going to the
+/// next node.
+#[must_use]
+pub fn default_is_reached(
+    IsReachedCtx {
+        position,
+        target,
+        physics,
+        ..
+    }: IsReachedCtx,
+) -> bool {
+    BlockPos::from(position) == target && physics.on_ground
+}
 
 #[cfg(test)]
 mod tests {