improve pathfinder path execution

3 files changed, 83 insertions, 115 deletions

diff --git a/azalea/src/pathfinder/moves/basic.rs b/azalea/src/pathfinder/moves/basic.rs
index ccf4ab79..71641716 100644
--- a/azalea/src/pathfinder/moves/basic.rs
+++ b/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,
diff --git a/azalea/src/pathfinder/moves/mod.rs b/azalea/src/pathfinder/moves/mod.rs
index 49615a3a..0d998191 100644
--- a/azalea/src/pathfinder/moves/mod.rs
+++ b/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));
diff --git a/azalea/src/pathfinder/moves/parkour.rs b/azalea/src/pathfinder/moves/parkour.rs
index fc03dc48..e13581a0 100644
--- a/azalea/src/pathfinder/moves/parkour.rs
+++ b/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 });
     }
 }