more reliable pathfinding on almost-full blocks

5 files changed, 45 insertions, 32 deletions

diff --git a/azalea/src/pathfinder/mod.rs b/azalea/src/pathfinder/mod.rs
index 6902e224..b91060d9 100644
--- a/azalea/src/pathfinder/mod.rs
+++ b/azalea/src/pathfinder/mod.rs
@@ -39,7 +39,10 @@ use azalea_client::{
     mining::{Mining, MiningSystems, StartMiningBlockEvent},
     movement::MoveEventsSystems,
 };
-use azalea_core::{position::BlockPos, tick::GameTick};
+use azalea_core::{
+    position::{BlockPos, Vec3},
+    tick::GameTick,
+};
 use azalea_entity::{LocalEntity, Physics, Position, metadata::Player};
 use azalea_physics::PhysicsSystems;
 use azalea_world::{InstanceContainer, InstanceName};
@@ -303,7 +306,9 @@ pub fn goto_listener(
             continue;
         };
 
-        if event.goal.success(BlockPos::from(position)) {
+        let cur_pos = player_pos_to_block_pos(**position);
+
+        if event.goal.success(cur_pos) {
             // we're already at the goal, nothing to do
             pathfinder.goal = None;
             pathfinder.opts = None;
@@ -333,16 +338,15 @@ pub fn goto_listener(
                 .movement
                 .target
         } else {
-            BlockPos::from(position)
+            cur_pos
         };
 
-        if start == BlockPos::from(position) {
+        if start == cur_pos {
             info!("got goto {:?}, starting from {start:?}", event.goal);
         } else {
             info!(
-                "got goto {:?}, starting from {start:?} (currently at {:?})",
+                "got goto {:?}, starting from {start:?} (currently at {cur_pos:?})",
                 event.goal,
-                BlockPos::from(position)
             );
         }
 
@@ -381,6 +385,12 @@ pub fn goto_listener(
     }
 }
 
+#[inline]
+pub(crate) fn player_pos_to_block_pos(position: Vec3) -> BlockPos {
+    // 0.5 to account for non-full blocks
+    BlockPos::from(position.up(0.5))
+}
+
 pub struct CalculatePathCtx {
     pub entity: Entity,
     pub start: BlockPos,
@@ -677,7 +687,8 @@ pub fn timeout_movement(
         {
             warn!("pathfinder timeout, trying to patch path");
             executing_path.queued_path = None;
-            executing_path.last_reached_node = BlockPos::from(position);
+            let cur_pos = player_pos_to_block_pos(**position);
+            executing_path.last_reached_node = cur_pos;
 
             let world_lock = instance_container
                 .get(instance_name)
@@ -747,8 +758,7 @@ pub fn check_node_reached(
                     let z_difference_from_center = position.z - (movement.target.z as f64 + 0.5);
                     // this is to make sure we don't fall off immediately after finishing the path
                     physics.on_ground()
-                    // 0.5 to handle non-full blocks
-                    && BlockPos::from(position.up(0.5)) == movement.target
+                    && player_pos_to_block_pos(**position) == movement.target
                     // 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.velocity.x).abs() < 0.2
diff --git a/azalea/src/pathfinder/moves/basic.rs b/azalea/src/pathfinder/moves/basic.rs
index e352f385..be97f1e3 100644
--- a/azalea/src/pathfinder/moves/basic.rs
+++ b/azalea/src/pathfinder/moves/basic.rs
@@ -8,7 +8,7 @@ use azalea_core::{
 };
 
 use super::{Edge, ExecuteCtx, IsReachedCtx, MoveData, PathfinderCtx, default_is_reached};
-use crate::pathfinder::{astar, costs::*, rel_block_pos::RelBlockPos};
+use crate::pathfinder::{astar, costs::*, player_pos_to_block_pos, rel_block_pos::RelBlockPos};
 
 pub fn basic_move(ctx: &mut PathfinderCtx, node: RelBlockPos) {
     forward_move(ctx, node);
@@ -182,7 +182,7 @@ fn execute_ascend_move(mut ctx: ExecuteCtx) {
         }
     }
 
-    if BlockPos::from(position) == start {
+    if player_pos_to_block_pos(position) == start {
         // only jump if the target is more than 0.5 blocks above us
         if target.y as f64 - position.y > 0.5 {
             ctx.jump();
@@ -307,7 +307,7 @@ fn execute_descend_move(mut ctx: ExecuteCtx) {
         start_center.z + (center.z - start_center.z) * 1.5,
     );
 
-    if BlockPos::from(position) != target || horizontal_distance_from_target > 0.25 {
+    if player_pos_to_block_pos(position) != target || horizontal_distance_from_target > 0.25 {
         if horizontal_distance_from_start < 1.25 {
             // this basically just exists to avoid doing spins while we're falling
             ctx.look_at(dest_ahead);
@@ -336,11 +336,13 @@ pub fn descend_is_reached(
         start.z + (target.z - start.z) * 2,
     );
 
-    if BlockPos::from(position) == target || BlockPos::from(position) == dest_ahead {
+    if player_pos_to_block_pos(position) == target
+        || player_pos_to_block_pos(position) == dest_ahead
+    {
         if (position.y - target.y as f64) < 0.5 {
             return true;
         }
-    } else if BlockPos::from(position).up(1) == target && physics.on_ground() {
+    } else if player_pos_to_block_pos(position).up(1) == target && physics.on_ground() {
         return true;
     }
     false
diff --git a/azalea/src/pathfinder/moves/mod.rs b/azalea/src/pathfinder/moves/mod.rs
index e74a79b4..39ef786b 100644
--- a/azalea/src/pathfinder/moves/mod.rs
+++ b/azalea/src/pathfinder/moves/mod.rs
@@ -28,6 +28,7 @@ use super::{
 use crate::{
     auto_tool::best_tool_in_hotbar_for_block,
     bot::{JumpEvent, LookAtEvent},
+    pathfinder::player_pos_to_block_pos,
 };
 
 type Edge = astar::Edge<RelBlockPos, MoveData>;
@@ -176,8 +177,8 @@ impl ExecuteCtx<'_, '_, '_, '_, '_, '_, '_, '_> {
         let horizontal_distance_from_start = (self.start.center() - self.position)
             .horizontal_distance_squared()
             .sqrt();
-        let at_start_position =
-            BlockPos::from(self.position) == self.start && horizontal_distance_from_start < 0.25;
+        let at_start_position = player_pos_to_block_pos(self.position) == self.start
+            && horizontal_distance_from_start < 0.25;
 
         if self.should_mine(block) {
             if at_start_position {
diff --git a/azalea/src/pathfinder/moves/parkour.rs b/azalea/src/pathfinder/moves/parkour.rs
index d4f136be..89659f65 100644
--- a/azalea/src/pathfinder/moves/parkour.rs
+++ b/azalea/src/pathfinder/moves/parkour.rs
@@ -3,7 +3,7 @@ use azalea_core::{direction::CardinalDirection, position::BlockPos};
 use tracing::trace;
 
 use super::{Edge, ExecuteCtx, IsReachedCtx, MoveData, PathfinderCtx};
-use crate::pathfinder::{astar, costs::*, rel_block_pos::RelBlockPos};
+use crate::pathfinder::{astar, costs::*, player_pos_to_block_pos, rel_block_pos::RelBlockPos};
 
 pub fn parkour_move(ctx: &mut PathfinderCtx, node: RelBlockPos) {
     if !ctx.world.is_block_solid(node.down(1)) {
@@ -85,6 +85,8 @@ fn parkour_forward_2_move(ctx: &mut PathfinderCtx, pos: RelBlockPos) {
             1
         } else if ctx.world.is_standable(pos + offset) {
             0
+        } else if ctx.world.is_standable(pos + offset.down(1)) {
+            -1
         } else {
             continue;
         };
@@ -201,8 +203,8 @@ fn execute_parkour_move(mut ctx: ExecuteCtx) {
     let dir = BlockPos::new(x_dir, 0, z_dir);
     let jump_at_pos = start + dir;
 
-    let is_at_start_block = BlockPos::from(position) == start;
-    let is_at_jump_block = BlockPos::from(position) == jump_at_pos;
+    let is_at_start_block = player_pos_to_block_pos(position) == start;
+    let is_at_jump_block = player_pos_to_block_pos(position) == jump_at_pos;
 
     let required_distance_from_center = if jump_distance <= 2 {
         // 1 block gap
@@ -244,11 +246,10 @@ pub fn parkour_is_reached(
     }: IsReachedCtx,
 ) -> bool {
     // 0.094 and not 0 for lilypads
-    if BlockPos::from(position) == target && (position.y - target.y as f64) < 0.094 {
+    if player_pos_to_block_pos(position) == target && (position.y - target.y as f64) < 0.094 {
         return true;
     }
 
-    // this is to make it handle things like slabs correctly, if we're on the block
-    // below the target but on_ground
-    BlockPos::from(position).up(1) == target && physics.on_ground()
+    // this is to make it handle things like slabs correctly
+    player_pos_to_block_pos(position) == target && physics.on_ground()
 }
diff --git a/azalea/src/pathfinder/world.rs b/azalea/src/pathfinder/world.rs
index 30f4dd7c..318ed55e 100644
--- a/azalea/src/pathfinder/world.rs
+++ b/azalea/src/pathfinder/world.rs
@@ -565,6 +565,7 @@ pub fn is_block_state_passable(block_state: BlockState) -> bool {
 
 /// Whether this block has a solid hitbox at the top (i.e. we can stand on it
 /// and do parkour from it).
+#[inline]
 pub fn is_block_state_solid(block_state: BlockState) -> bool {
     if block_state.is_air() {
         // fast path
@@ -582,17 +583,19 @@ pub fn is_block_state_solid(block_state: BlockState) -> bool {
         return true;
     }
 
+    let block = Block::from(block_state);
+    // solid enough
+    if matches!(block, Block::DirtPath | Block::Farmland) {
+        return true;
+    }
+
     false
 }
 
 /// Whether we can stand on this block (but not necessarily do parkour jumps
 /// from it).
 pub fn is_block_state_standable(block_state: BlockState) -> bool {
-    if block_state.is_air() {
-        // fast path
-        return false;
-    }
-    if block_state.is_collision_shape_full() {
+    if is_block_state_solid(block_state) {
         return true;
     }
 
@@ -601,10 +604,6 @@ pub fn is_block_state_standable(block_state: BlockState) -> bool {
         return true;
     }
 
-    if matches!(block, Block::DirtPath | Block::Farmland) {
-        return true;
-    }
-
     false
 }