improve pathfinder path execution

azalea/src/pathfinder/costs.rs

@@ -5,16 +5,41 @@ use num_traits::Float;
 // based on https://github.com/cabaletta/baritone/blob/1.20.1/src/api/java/baritone/api/pathing/movement/ActionCosts.java
 pub const WALK_ONE_BLOCK_COST: f32 = 20. / 4.317; // 4.633
 pub const SPRINT_ONE_BLOCK_COST: f32 = 20. / 5.612; // 3.564
-pub const FALL_ONE_BLOCK_COST: f32 = 1.;
 pub const WALK_OFF_BLOCK_COST: f32 = WALK_ONE_BLOCK_COST * 0.8;
 pub const SPRINT_MULTIPLIER: f32 = SPRINT_ONE_BLOCK_COST / WALK_ONE_BLOCK_COST;
 pub const JUMP_PENALTY: f32 = 2.;
+pub const CENTER_AFTER_FALL_COST: f32 = WALK_ONE_BLOCK_COST - WALK_OFF_BLOCK_COST; // 0.927
 
 pub static FALL_1_25_BLOCKS_COST: LazyLock<f32> = LazyLock::new(|| distance_to_ticks(1.25));
 pub static FALL_0_25_BLOCKS_COST: LazyLock<f32> = LazyLock::new(|| distance_to_ticks(0.25));
 pub static JUMP_ONE_BLOCK_COST: LazyLock<f32> =
     LazyLock::new(|| *FALL_1_25_BLOCKS_COST - *FALL_0_25_BLOCKS_COST); // 3.163
 
+pub static FALL_N_BLOCKS_COST: LazyLock<[f32; 4097]> = LazyLock::new(|| {
+    let mut fall_n_blocks_cost = [0.; 4097];
+
+    let mut distance = 0;
+
+    // this is the same as calling distance_to_ticks a bunch of times but more
+    // efficient
+    let mut temp_distance = distance as f32;
+    let mut tick_count = 0;
+    loop {
+        let fall_distance = velocity(tick_count);
+        if temp_distance <= fall_distance {
+            fall_n_blocks_cost[distance] = tick_count as f32 + temp_distance / fall_distance;
+            distance += 1;
+            if distance >= fall_n_blocks_cost.len() {
+                break;
+            }
+        }
+        temp_distance -= fall_distance;
+        tick_count += 1;
+    }
+
+    fall_n_blocks_cost
+});
+
 fn velocity(ticks: usize) -> f32 {
     (0.98.powi(ticks.try_into().unwrap()) - 1.) * -3.92
 }

azalea/src/pathfinder/goals.rs

@@ -3,7 +3,7 @@ use std::f32::consts::SQRT_2;
 use azalea_core::position::{BlockPos, Vec3};
 
 use super::{
-    costs::{FALL_ONE_BLOCK_COST, JUMP_ONE_BLOCK_COST},
+    costs::{FALL_N_BLOCKS_COST, JUMP_ONE_BLOCK_COST},
     Goal,
 };
 
@@ -55,10 +55,10 @@ impl Goal for XZGoal {
 }
 
 fn y_heuristic(dy: f32) -> f32 {
-    if dy < 0.0 {
-        FALL_ONE_BLOCK_COST * -dy
-    } else {
+    if dy > 0.0 {
         *JUMP_ONE_BLOCK_COST * dy
+    } else {
+        FALL_N_BLOCKS_COST[2] / 2. * -dy
     }
 }
 

azalea/src/pathfinder/mod.rs

@@ -20,22 +20,26 @@ use crate::ecs::{
     system::{Commands, Query, Res},
 };
 use crate::pathfinder::moves::PathfinderCtx;
+use azalea_client::chat::SendChatEvent;
 use azalea_client::movement::walk_listener;
 use azalea_client::{StartSprintEvent, StartWalkEvent};
-use azalea_core::position::BlockPos;
+use azalea_core::position::{BlockPos, Vec3};
 use azalea_entity::metadata::Player;
 use azalea_entity::LocalEntity;
 use azalea_entity::{Physics, Position};
 use azalea_physics::PhysicsSet;
 use azalea_world::{InstanceContainer, InstanceName};
 use bevy_app::{FixedUpdate, PreUpdate, Update};
+use bevy_ecs::event::Events;
 use bevy_ecs::prelude::Event;
 use bevy_ecs::query::Changed;
 use bevy_ecs::schedule::IntoSystemConfigs;
+use bevy_ecs::system::{Local, ResMut};
 use bevy_tasks::{AsyncComputeTaskPool, Task};
 use futures_lite::future;
 use log::{debug, error, info, trace, warn};
 use std::collections::VecDeque;
+use std::sync::atomic::{self, AtomicUsize};
 use std::sync::Arc;
 use std::time::{Duration, Instant};
 
@@ -51,9 +55,13 @@ impl Plugin for PathfinderPlugin {
                 FixedUpdate,
                 // putting systems in the FixedUpdate schedule makes them run every Minecraft tick
                 // (every 50 milliseconds).
-                tick_execute_path
-                    .after(PhysicsSet)
-                    .after(azalea_client::movement::send_position),
+                (
+                    tick_execute_path
+                        .after(PhysicsSet)
+                        .after(azalea_client::movement::send_position),
+                    debug_render_path_with_particles,
+                )
+                    .chain(),
             )
             .add_systems(PreUpdate, add_default_pathfinder)
             .add_systems(
@@ -81,6 +89,8 @@ pub struct Pathfinder {
     pub goal: Option<Arc<dyn Goal + Send + Sync>>,
     pub successors_fn: Option<SuccessorsFn>,
     pub is_calculating: bool,
+
+    pub goto_id: Arc<AtomicUsize>,
 }
 #[derive(Event)]
 pub struct GotoEvent {
@@ -157,7 +167,7 @@ fn goto_listener(
             // if we're currently pathfinding and got a goto event, start a little ahead
             pathfinder
                 .path
-                .get(5)
+                .get(20)
                 .unwrap_or_else(|| pathfinder.path.back().unwrap())
                 .target
         };
@@ -175,6 +185,9 @@ fn goto_listener(
         let goal = event.goal.clone();
         let entity = event.entity;
 
+        let goto_id_atomic = pathfinder.goto_id.clone();
+        let goto_id = goto_id_atomic.fetch_add(1, atomic::Ordering::Relaxed) + 1;
+
         let task = thread_pool.spawn(async move {
             debug!("start: {start:?}");
 
@@ -204,6 +217,8 @@ fn goto_listener(
                 let duration = end_time - start_time;
                 if partial {
                     info!("Pathfinder took {duration:?} (timed out)");
+                    // wait a bit so it's not a busy loop
+                    std::thread::sleep(Duration::from_millis(100));
                 } else {
                     info!("Pathfinder took {duration:?}");
                 }
@@ -216,6 +231,13 @@ fn goto_listener(
                 path = movements.into_iter().collect::<VecDeque<_>>();
                 is_partial = partial;
 
+                let goto_id_now = goto_id_atomic.load(atomic::Ordering::Relaxed);
+                if goto_id != goto_id_now {
+                    // we must've done another goto while calculating this path, so throw it away
+                    warn!("finished calculating a path, but it's outdated");
+                    return None;
+                }
+
                 if path.is_empty() && partial {
                     if attempt_number == 0 {
                         debug!("this path is empty, retrying with a higher timeout");
@@ -275,6 +297,7 @@ fn path_found_listener(
                 pathfinder.path = path.to_owned();
                 debug!("set path to {:?}", path.iter().take(10).collect::<Vec<_>>());
                 pathfinder.last_reached_node = Some(event.start);
+                pathfinder.last_node_reached_at = Some(Instant::now());
             } else {
                 let mut new_path = VecDeque::new();
 
@@ -313,7 +336,6 @@ fn path_found_listener(
                 );
                 pathfinder.queued_path = Some(new_path);
             }
-            pathfinder.last_node_reached_at = Some(Instant::now());
         } else {
             error!("No path found");
             pathfinder.path.clear();
@@ -353,6 +375,9 @@ fn tick_execute_path(
                 if last_node_reached_at.elapsed() > Duration::from_secs(2) {
                     warn!("pathfinder timeout");
                     pathfinder.path.clear();
+                    pathfinder.queued_path = None;
+                    pathfinder.last_reached_node = None;
+                    pathfinder.goto_id.fetch_add(1, atomic::Ordering::Relaxed);
                     // set partial to true to make sure that the recalculation happens
                     pathfinder.is_path_partial = true;
                 }
@@ -386,8 +411,10 @@ fn tick_execute_path(
                     // this is to make sure we don't fall off immediately after finishing the path
                     physics.on_ground
                         && BlockPos::from(position) == movement.target
-                        && x_difference_from_center.abs() < 0.2
-                        && z_difference_from_center.abs() < 0.2
+                        // adding the delta like this isn't a perfect solution but it helps to make
+                        // sure we don't keep going if our delta is high
+                        && (x_difference_from_center + physics.delta.x).abs() < 0.2
+                        && (z_difference_from_center + physics.delta.z).abs() < 0.2
                 } else {
                     true
                 };
@@ -470,13 +497,16 @@ fn tick_execute_path(
                 {
                     warn!("path obstructed at index {obstructed_index} (starting at {last_reached_node:?}, path: {:?})", pathfinder.path);
                     pathfinder.path.truncate(obstructed_index);
+                    pathfinder.is_path_partial = true;
                 }
             }
         }
 
         {
             // start recalculating if the path ends soon
-            if pathfinder.path.len() < 5 && !pathfinder.is_calculating && pathfinder.is_path_partial
+            if (pathfinder.path.len() == 20 || 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");
@@ -507,6 +537,12 @@ fn tick_execute_path(
                             });
                         }
                     }
+                } else if pathfinder.path.is_empty() {
+                    // idk when this can happen but stop moving just in case
+                    walk_events.send(StartWalkEvent {
+                        entity,
+                        direction: WalkDirection::None,
+                    });
                 }
             }
         }
@@ -529,6 +565,79 @@ fn stop_pathfinding_on_instance_change(
     }
 }
 
+/// A component that makes bots run /particle commands while pathfinding to show
+/// where they're going. This requires the bots to have op permissions, and
+/// it'll make them spam *a lot* of commands.
+#[derive(Component)]
+pub struct PathfinderDebugParticles;
+
+fn debug_render_path_with_particles(
+    mut query: Query<(Entity, &Pathfinder), With<PathfinderDebugParticles>>,
+    // chat_events is Option because the tests don't have SendChatEvent
+    // and we have to use ResMut<Events> because bevy doesn't support Option<EventWriter>
+    chat_events: Option<ResMut<Events<SendChatEvent>>>,
+    mut tick_count: Local<usize>,
+) {
+    let Some(mut chat_events) = chat_events else {
+        return;
+    };
+    if *tick_count >= 2 {
+        *tick_count = 0;
+    } else {
+        *tick_count += 1;
+        return;
+    }
+    for (entity, pathfinder) in &mut query {
+        if pathfinder.path.is_empty() {
+            continue;
+        }
+
+        let mut start = pathfinder
+            .last_reached_node
+            .unwrap_or_else(|| pathfinder.path.front().unwrap().target);
+        for movement in &pathfinder.path {
+            // /particle dust 0 1 1 1 ~ ~ ~ 0 0 0.2 0 100
+
+            let end = movement.target;
+
+            let start_vec3 = start.center();
+            let end_vec3 = end.center();
+
+            let step_count = (start_vec3.distance_to_sqr(&end_vec3).sqrt() * 4.0) as usize;
+
+            // interpolate between the start and end positions
+            for i in 0..step_count {
+                let percent = i as f64 / step_count as f64;
+                let pos = Vec3 {
+                    x: start_vec3.x + (end_vec3.x - start_vec3.x) * percent,
+                    y: start_vec3.y + (end_vec3.y - start_vec3.y) * percent,
+                    z: start_vec3.z + (end_vec3.z - start_vec3.z) * percent,
+                };
+                let particle_command = format!(
+                "/particle dust {r} {g} {b} {size} {start_x} {start_y} {start_z} {delta_x} {delta_y} {delta_z} 0 {count}",
+                r = 0,
+                g = 1,
+                b = 1,
+                size = 1,
+                start_x = pos.x,
+                start_y = pos.y,
+                start_z = pos.z,
+                delta_x = 0,
+                delta_y = 0,
+                delta_z = 0,
+                count = 1
+            );
+                chat_events.send(SendChatEvent {
+                    entity,
+                    content: particle_command,
+                });
+            }
+
+            start = movement.target;
+        }
+    }
+}
+
 pub trait Goal {
     fn heuristic(&self, n: BlockPos) -> f32;
     fn success(&self, n: BlockPos) -> bool;
@@ -729,6 +838,29 @@ mod tests {
         );
     }
 
+    #[test]
+    fn test_small_descend_and_parkour_2_block_gap() {
+        let mut partial_chunks = PartialChunkStorage::default();
+        let mut simulation = setup_simulation(
+            &mut partial_chunks,
+            BlockPos::new(0, 71, 0),
+            BlockPos::new(0, 70, 5),
+            vec![
+                BlockPos::new(0, 70, 0),
+                BlockPos::new(0, 70, 1),
+                BlockPos::new(0, 69, 2),
+                BlockPos::new(0, 69, 5),
+            ],
+        );
+        for _ in 0..40 {
+            simulation.tick();
+        }
+        assert_eq!(
+            BlockPos::from(simulation.position()),
+            BlockPos::new(0, 70, 5)
+        );
+    }
+
     #[test]
     fn test_quickly_descend() {
         let mut partial_chunks = PartialChunkStorage::default();

azalea/src/pathfinder/moves/basic.rs

@@ -164,9 +164,16 @@ fn descend_move(edges: &mut Vec<Edge>, ctx: &PathfinderCtx, pos: BlockPos) {
             continue;
         }
 
-        let cost = SPRINT_ONE_BLOCK_COST
-            + WALK_OFF_BLOCK_COST
-            + FALL_ONE_BLOCK_COST * fall_distance as f32;
+        let cost = WALK_OFF_BLOCK_COST
+            + f32::max(
+                FALL_N_BLOCKS_COST
+                    .get(fall_distance as usize)
+                    .copied()
+                    // avoid panicking if we fall more than the size of FALL_N_BLOCKS_COST
+                    // probably not possible but just in case
+                    .unwrap_or(f32::MAX),
+                CENTER_AFTER_FALL_COST,
+            );
 
         edges.push(Edge {
             movement: astar::Movement {
@@ -255,21 +262,27 @@ fn diagonal_move(edges: &mut Vec<Edge>, ctx: &PathfinderCtx, pos: BlockPos) {
     for dir in CardinalDirection::iter() {
         let right = dir.right();
         let offset = BlockPos::new(dir.x() + right.x(), 0, dir.z() + right.z());
+        let left_pos = BlockPos::new(pos.x + dir.x(), pos.y, pos.z + dir.z());
+        let right_pos = BlockPos::new(pos.x + right.x(), pos.y, pos.z + right.z());
 
-        if !ctx.is_passable(BlockPos::new(pos.x + dir.x(), pos.y, pos.z + dir.z()))
-            && !ctx.is_passable(BlockPos::new(
-                pos.x + dir.right().x(),
-                pos.y,
-                pos.z + dir.right().z(),
-            ))
-        {
+        // +0.001 so it doesn't unnecessarily go diagonal sometimes
+        let mut cost = SPRINT_ONE_BLOCK_COST * SQRT_2 + 0.001;
+
+        let left_passable = ctx.is_passable(left_pos);
+        let right_passable = ctx.is_passable(right_pos);
+
+        if !left_passable && !right_passable {
             continue;
         }
+
+        if !left_passable || !right_passable {
+            // add a bit of cost because it'll probably be hugging a wall here
+            cost += WALK_ONE_BLOCK_COST / 2.;
+        }
+
         if !ctx.is_standable(pos + offset) {
             continue;
         }
-        // +0.001 so it doesn't unnecessarily go diagonal sometimes
-        let cost = SPRINT_ONE_BLOCK_COST * SQRT_2 + 0.001;
 
         edges.push(Edge {
             movement: astar::Movement {
@@ -292,10 +305,11 @@ fn execute_diagonal_move(
         ..
     }: ExecuteCtx,
 ) {
-    let center = target.center();
+    let target_center = target.center();
+
     look_at_events.send(LookAtEvent {
         entity,
-        position: center,
+        position: target_center,
     });
     sprint_events.send(StartSprintEvent {
         entity,

azalea/src/pathfinder/moves/mod.rs

@@ -122,20 +122,7 @@ impl PathfinderCtx {
         let chunk_pos = ChunkPos::from(pos);
 
         let mut cached_chunks = self.cached_chunks.borrow_mut();
-        if let Some(sections) = cached_chunks.iter().find_map(|(pos, sections)| {
-            if *pos == chunk_pos {
-                Some(sections)
-            } else {
-                None
-            }
-        }) {
-            let section_index =
-                azalea_world::chunk_storage::section_index(pos.y, self.min_y) as usize;
-            if section_index >= sections.len() {
-                // y position is out of bounds
-                return None;
-            };
-            let section = &sections[section_index];
+        if let Some(section) = self.get_section_from_cache(&cached_chunks, pos) {
             let chunk_section_pos = ChunkSectionBlockPos::from(pos);
             return Some(section.get(chunk_section_pos));
         }
@@ -156,6 +143,33 @@ impl PathfinderCtx {
         Some(section.get(chunk_section_pos))
     }
 
+    fn get_section_from_cache<'a>(
+        &self,
+        cached_chunks: &'a [(ChunkPos, Vec<azalea_world::Section>)],
+        pos: BlockPos,
+    ) -> Option<&'a azalea_world::Section> {
+        let chunk_pos = ChunkPos::from(pos);
+
+        if let Some(sections) = cached_chunks.iter().find_map(|(pos, sections)| {
+            if *pos == chunk_pos {
+                Some(sections)
+            } else {
+                None
+            }
+        }) {
+            let section_index =
+                azalea_world::chunk_storage::section_index(pos.y, self.min_y) as usize;
+            if section_index >= sections.len() {
+                // y position is out of bounds
+                return None;
+            };
+            let section = &sections[section_index];
+            return Some(section);
+        }
+
+        None
+    }
+
     pub fn is_block_passable(&self, pos: BlockPos) -> bool {
         let (section_pos, section_block_pos) =
             (ChunkSectionPos::from(pos), ChunkSectionBlockPos::from(pos));

azalea/src/pathfinder/moves/parkour.rs

@@ -10,7 +10,6 @@ use super::{default_is_reached, Edge, ExecuteCtx, IsReachedCtx, MoveData, Pathfi
 
 pub fn parkour_move(edges: &mut Vec<Edge>, ctx: &PathfinderCtx, node: BlockPos) {
     parkour_forward_1_move(edges, ctx, node);
-    parkour_headhitter_forward_1_move(edges, ctx, node);
     parkour_forward_2_move(edges, ctx, node);
 }
 
@@ -23,15 +22,25 @@ fn parkour_forward_1_move(edges: &mut Vec<Edge>, ctx: &PathfinderCtx, pos: Block
         if ctx.is_block_solid((pos + gap_offset).down(1)) {
             continue;
         }
-        if !ctx.is_standable(pos + offset) {
-            continue;
-        }
         if !ctx.is_passable(pos + gap_offset) {
             continue;
         }
+
+        let ascend: i32 = if ctx.is_standable(pos + offset.up(1)) {
+            // ascend
+            1
+        } else if ctx.is_standable(pos + offset) {
+            // forward
+            0
+        } else {
+            continue;
+        };
+
+        // make sure we have space to jump
         if !ctx.is_block_passable((pos + gap_offset).up(2)) {
             continue;
         }
+
         // make sure it's not a headhitter
         if !ctx.is_block_passable(pos.up(2)) {
             continue;
@@ -41,7 +50,7 @@ fn parkour_forward_1_move(edges: &mut Vec<Edge>, ctx: &PathfinderCtx, pos: Block
 
         edges.push(Edge {
             movement: astar::Movement {
-                target: pos + offset,
+                target: pos + offset.up(ascend),
                 data: MoveData {
                     execute: &execute_parkour_move,
                     is_reached: &parkour_is_reached,
@@ -100,44 +109,6 @@ fn parkour_forward_2_move(edges: &mut Vec<Edge>, ctx: &PathfinderCtx, pos: Block
     }
 }
 
-fn parkour_headhitter_forward_1_move(edges: &mut Vec<Edge>, ctx: &PathfinderCtx, pos: BlockPos) {
-    for dir in CardinalDirection::iter() {
-        let gap_offset = BlockPos::new(dir.x(), 0, dir.z());
-        let offset = BlockPos::new(dir.x() * 2, 0, dir.z() * 2);
-
-        // make sure we actually have to jump
-        if ctx.is_block_solid((pos + gap_offset).down(1)) {
-            continue;
-        }
-        if !ctx.is_standable(pos + offset) {
-            continue;
-        }
-        if !ctx.is_passable(pos + gap_offset) {
-            continue;
-        }
-        if !ctx.is_block_passable((pos + gap_offset).up(2)) {
-            continue;
-        }
-        // make sure it is a headhitter
-        if !ctx.is_block_solid(pos.up(2)) {
-            continue;
-        }
-
-        let cost = JUMP_PENALTY + WALK_ONE_BLOCK_COST + WALK_ONE_BLOCK_COST;
-
-        edges.push(Edge {
-            movement: astar::Movement {
-                target: pos + offset,
-                data: MoveData {
-                    execute: &execute_headhitter_parkour_move,
-                    is_reached: &default_is_reached,
-                },
-            },
-            cost,
-        })
-    }
-}
-
 fn execute_parkour_move(
     ExecuteCtx {
         entity,
@@ -151,15 +122,17 @@ fn execute_parkour_move(
         ..
     }: ExecuteCtx,
 ) {
-    let center = target.center();
+    let start_center = start.center();
+    let target_center = target.center();
     look_at_events.send(LookAtEvent {
         entity,
-        position: center,
+        position: target_center,
     });
 
     let jump_distance = i32::max((target - start).x.abs(), (target - start).z.abs());
 
     if jump_distance >= 4 {
+        // 3 block gap
         sprint_events.send(StartSprintEvent {
             entity,
             direction: SprintDirection::Forward,
@@ -178,53 +151,20 @@ fn execute_parkour_move(
 
     let is_at_start_block = BlockPos::from(position) == start;
     let is_at_jump_block = BlockPos::from(position) == jump_at_pos;
-    let is_in_air = position.y - start.y as f64 > 0.0001;
 
-    if !is_at_start_block && (is_at_jump_block || is_in_air) {
-        jump_events.send(JumpEvent { entity });
-    }
-}
-
-fn execute_headhitter_parkour_move(
-    ExecuteCtx {
-        entity,
-        target,
-        start,
-        position,
-        look_at_events,
-        sprint_events,
-        walk_events,
-        jump_events,
-        ..
-    }: ExecuteCtx,
-) {
-    let center = target.center();
-    look_at_events.send(LookAtEvent {
-        entity,
-        position: center,
-    });
-
-    let jump_distance = i32::max((target - start).x.abs(), (target - start).z.abs());
-
-    if jump_distance > 2 {
-        sprint_events.send(StartSprintEvent {
-            entity,
-            direction: SprintDirection::Forward,
-        });
+    let required_distance_from_center = if jump_distance <= 2 {
+        // 1 block gap
+        0.
     } else {
-        walk_events.send(StartWalkEvent {
-            entity,
-            direction: WalkDirection::Forward,
-        });
-    }
-
-    let start_center = start.center();
+        0.6
+    };
     let distance_from_start = f64::max(
-        (start_center.x - position.x).abs(),
-        (start_center.z - position.z).abs(),
+        (position.x - start_center.x).abs(),
+        (position.z - start_center.z).abs(),
     );
 
-    if distance_from_start > 0.75 {
+    if !is_at_start_block && is_at_jump_block && distance_from_start > required_distance_from_center
+    {
         jump_events.send(JumpEvent { entity });
     }
 }