several pathfinder fixes

5 files changed, 33 insertions, 14 deletions

diff --git a/azalea/src/pathfinder/goals.rs b/azalea/src/pathfinder/goals.rs
index 18ab50c1..068bcb2e 100644
--- a/azalea/src/pathfinder/goals.rs
+++ b/azalea/src/pathfinder/goals.rs
@@ -223,9 +223,17 @@ impl Goal for ReachBlockPosGoal {
         BlockPosGoal(self.pos).heuristic(n)
     }
     fn success(&self, n: BlockPos) -> bool {
-        if n.up(1) == self.pos {
-            // our head is in the block, assume it's always reachable (to reduce the amount
-            // of impossible goals)
+        let head = n.up(1);
+        // the player's head is in the block or adjacent to it, so assume that it's
+        // always reachable (we do this to account for mining)
+        if head == self.pos
+            || head.north(1) == self.pos
+            || head.south(1) == self.pos
+            || head.east(1) == self.pos
+            || head.west(1) == self.pos
+            || head.up(1) == self.pos
+            || head.down(1) == self.pos
+        {
             return true;
         }
 
diff --git a/azalea/src/pathfinder/mod.rs b/azalea/src/pathfinder/mod.rs
index b91060d9..6d78b043 100644
--- a/azalea/src/pathfinder/mod.rs
+++ b/azalea/src/pathfinder/mod.rs
@@ -385,8 +385,13 @@ pub fn goto_listener(
     }
 }
 
+/// Convert a player position to a block position, used internally in the
+/// pathfinder.
+///
+/// This is almost the same as `BlockPos::from(position)`, except that non-full
+/// blocks are handled correctly.
 #[inline]
-pub(crate) fn player_pos_to_block_pos(position: Vec3) -> BlockPos {
+pub fn player_pos_to_block_pos(position: Vec3) -> BlockPos {
     // 0.5 to account for non-full blocks
     BlockPos::from(position.up(0.5))
 }
diff --git a/azalea/src/pathfinder/moves/basic.rs b/azalea/src/pathfinder/moves/basic.rs
index be97f1e3..270dc102 100644
--- a/azalea/src/pathfinder/moves/basic.rs
+++ b/azalea/src/pathfinder/moves/basic.rs
@@ -147,15 +147,19 @@ fn execute_ascend_move(mut ctx: ExecuteCtx) {
     // these checks are to make sure we don't fall if our velocity is too high in
     // the wrong direction
 
-    let x_axis = (start.x - target.x).abs(); // either 0 or 1
-    let z_axis = (start.z - target.z).abs(); // either 0 or 1
+    let x_axis = target.x - start.x; // -1, 0, or 1
+    let z_axis = target.z - start.z; // -1, 0, or 1
 
-    let flat_distance_to_next = x_axis as f64 * (target_center.x - position.x)
-        + z_axis as f64 * (target_center.z - position.z);
-    let side_distance = z_axis as f64 * (target_center.x - position.x).abs()
-        + x_axis as f64 * (target_center.z - position.z).abs();
+    let x_axis_abs = x_axis.abs(); // either 0 or 1
+    let z_axis_abs = z_axis.abs(); // either 0 or 1
 
-    let lateral_motion = x_axis as f64 * physics.velocity.z + z_axis as f64 * physics.velocity.x;
+    let flat_distance_to_next = x_axis_abs as f64 * (target_center.x - position.x)
+        + z_axis_abs as f64 * (target_center.z - position.z);
+    let side_distance = z_axis_abs as f64 * (target_center.x - position.x).abs()
+        + x_axis_abs as f64 * (target_center.z - position.z).abs();
+
+    let lateral_motion =
+        x_axis_abs as f64 * physics.velocity.z + z_axis_abs as f64 * physics.velocity.x;
     if lateral_motion.abs() > 0.1 {
         return;
     }
diff --git a/azalea/src/pathfinder/moves/parkour.rs b/azalea/src/pathfinder/moves/parkour.rs
index 89659f65..b4a03732 100644
--- a/azalea/src/pathfinder/moves/parkour.rs
+++ b/azalea/src/pathfinder/moves/parkour.rs
@@ -81,11 +81,15 @@ fn parkour_forward_2_move(ctx: &mut PathfinderCtx, pos: RelBlockPos) {
             continue;
         }
 
+        let mut cost = JUMP_PENALTY + WALK_ONE_BLOCK_COST * 3. + CENTER_AFTER_FALL_COST;
+
         let ascend: i32 = if ctx.world.is_standable(pos + offset.up(1)) {
             1
         } else if ctx.world.is_standable(pos + offset) {
+            cost += FALL_N_BLOCKS_COST[1];
             0
         } else if ctx.world.is_standable(pos + offset.down(1)) {
+            cost += FALL_N_BLOCKS_COST[2];
             -1
         } else {
             continue;
@@ -109,8 +113,6 @@ fn parkour_forward_2_move(ctx: &mut PathfinderCtx, pos: RelBlockPos) {
             continue;
         }
 
-        let cost = JUMP_PENALTY + WALK_ONE_BLOCK_COST * 3. + CENTER_AFTER_FALL_COST;
-
         ctx.edges.push(Edge {
             movement: astar::Movement {
                 target: pos + offset.up(ascend),
diff --git a/azalea/src/pathfinder/rel_block_pos.rs b/azalea/src/pathfinder/rel_block_pos.rs
index 553ac471..b5fd23dc 100644
--- a/azalea/src/pathfinder/rel_block_pos.rs
+++ b/azalea/src/pathfinder/rel_block_pos.rs
@@ -12,7 +12,7 @@ use azalea_core::position::BlockPos;
 pub struct RelBlockPos {
     pub x: i16,
     pub z: i16,
-    /// Note that the y isn't relative.
+    /// The actual non-relative Y coordinate of the block.
     pub y: i32,
 }