better pathfinder swimming and other tweaks

8 files changed, 140 insertions, 82 deletions

diff --git a/azalea/examples/testbot/commands/debug.rs b/azalea/examples/testbot/commands/debug.rs
index 57d9ff1f..4cde3335 100644
--- a/azalea/examples/testbot/commands/debug.rs
+++ b/azalea/examples/testbot/commands/debug.rs
@@ -6,6 +6,7 @@ use azalea::{
     BlockPos,
     brigadier::prelude::*,
     chunks::ReceiveChunkEvent,
+    inventory,
     packet::game,
     pathfinder::{
         ExecutingPath, Pathfinder, custom_state::CustomPathfinderStateRef, mining::MiningCache,
@@ -14,7 +15,7 @@ use azalea::{
 };
 use azalea_core::hit_result::HitResult;
 use azalea_entity::{EntityKindComponent, metadata};
-use azalea_inventory::components::MaxStackSize;
+use azalea_inventory::{Menu, Player, components::MaxStackSize};
 use azalea_world::Worlds;
 use bevy_app::AppExit;
 use bevy_ecs::{message::Messages, query::With, world::EntityRef};
@@ -200,7 +201,7 @@ pub fn register(commands: &mut CommandDispatcher<Mutex<CommandSource>>) {
 
         let mut edges = Vec::new();
         let cached_world = CachedWorld::new(source.bot.world(), position);
-        let mining_cache = MiningCache::new(None);
+        let mining_cache = MiningCache::new(Some(Menu::Player(inventory::Player::default())));
         let custom_state = CustomPathfinderStateRef::default();
 
         azalea::pathfinder::moves::default_move(
diff --git a/azalea/src/lib.rs b/azalea/src/lib.rs
index 332dc565..3eefe316 100644
--- a/azalea/src/lib.rs
+++ b/azalea/src/lib.rs
@@ -1,5 +1,6 @@
 #![doc = include_str!("../README.md")]
 #![feature(type_changing_struct_update)]
+#![feature(float_algebraic)]
 
 pub mod accept_resource_packs;
 pub mod auto_reconnect;
diff --git a/azalea/src/pathfinder/costs.rs b/azalea/src/pathfinder/costs.rs
index 461f0b5c..81dfa02b 100644
--- a/azalea/src/pathfinder/costs.rs
+++ b/azalea/src/pathfinder/costs.rs
@@ -8,6 +8,7 @@ pub const SPRINT_ONE_BLOCK_COST: f32 = 20. / 5.612; // 3.564
 pub const WALK_OFF_BLOCK_COST: f32 = WALK_ONE_BLOCK_COST * 0.8; // 3.706
 pub const SPRINT_MULTIPLIER: f32 = SPRINT_ONE_BLOCK_COST / WALK_ONE_BLOCK_COST; // 0.769
 pub const JUMP_PENALTY: f32 = 2.;
+pub const ENTER_WATER_PENALTY: f32 = 3.;
 pub const CENTER_AFTER_FALL_COST: f32 = WALK_ONE_BLOCK_COST - WALK_OFF_BLOCK_COST; // 0.927
 pub const WALK_ONE_IN_WATER_COST: f32 = 20. / 1.960; // 10.204
 
diff --git a/azalea/src/pathfinder/debug.rs b/azalea/src/pathfinder/debug.rs
index 7fe44919..42a64982 100644
--- a/azalea/src/pathfinder/debug.rs
+++ b/azalea/src/pathfinder/debug.rs
@@ -74,7 +74,8 @@ pub fn debug_render_path_with_particles(
             // this isn't foolproof, there might be another block that could be mined
             // depending on the move, but it's good enough for debugging
             // purposes
-            let is_mining = !super::world::is_block_state_passable(target_block_state)
+            let is_mining = !(super::world::is_block_state_passable(target_block_state)
+                || super::world::is_block_state_water(target_block_state))
                 || !super::world::is_block_state_passable(above_target_block_state);
 
             let (r, g, b): (f64, f64, f64) = if i == 0 {
diff --git a/azalea/src/pathfinder/moves/basic.rs b/azalea/src/pathfinder/moves/basic.rs
index 8739c920..e5081964 100644
--- a/azalea/src/pathfinder/moves/basic.rs
+++ b/azalea/src/pathfinder/moves/basic.rs
@@ -17,7 +17,6 @@ pub fn basic_move(ctx: &mut PathfinderCtx, node: RelBlockPos) {
     ascend_move(ctx, node);
     descend_move(ctx, node);
     diagonal_move(ctx, node);
-    descend_forward_1_move(ctx, node);
     downward_move(ctx, node);
 }
 
@@ -25,7 +24,7 @@ fn forward_move(ctx: &mut PathfinderCtx, pos: RelBlockPos) {
     let mut base_cost = SPRINT_ONE_BLOCK_COST;
     // it's for us cheaper to have the water cost be applied when leaving the water
     // rather than when entering
-    let currently_in_water = ctx.world.is_block_water(pos.down(1));
+    let currently_in_water = ctx.world.is_block_water(pos);
     if currently_in_water {
         if BARITONE_COMPAT {
             base_cost = WALK_ONE_BLOCK_COST;
@@ -39,24 +38,32 @@ fn forward_move(ctx: &mut PathfinderCtx, pos: RelBlockPos) {
 
         let mut cost = base_cost;
 
-        let break_cost = ctx.world.cost_for_standing(pos + offset, ctx.mining_cache);
-        if break_cost == f32::INFINITY {
-            continue;
-        }
-        cost += break_cost;
+        let new_position = pos + offset;
 
-        if BARITONE_COMPAT && currently_in_water {
-            let dest_in_water = ctx.world.is_block_water((pos + offset).down(1));
+        let break_cost;
+        if currently_in_water {
+            let dest_in_water = ctx.world.is_block_water(new_position);
             if !dest_in_water {
-                // baritone does a descend when we enter water, doesn't matter much in practice
-                // though
-                cost += 2. + FALL_N_BLOCKS_COST[1] + WALK_OFF_BLOCK_COST - SPRINT_ONE_BLOCK_COST;
+                continue;
             }
+
+            break_cost = ctx
+                .world
+                .cost_for_breaking_block(new_position.up(1), ctx.mining_cache);
+        } else {
+            break_cost = ctx.world.cost_for_standing(new_position, ctx.mining_cache);
+        }
+        if break_cost == f32::INFINITY {
+            continue;
         }
 
+        // TODO: benchmark this
+        // cost = cost.algebraic_add(break_cost);
+        cost += break_cost;
+
         ctx.edges.push(Edge {
             movement: astar::Movement {
-                target: pos + offset,
+                target: new_position,
                 data: MoveData {
                     execute: &execute_forward_move,
                     is_reached: &default_is_reached,
@@ -90,17 +97,19 @@ fn ascend_move(ctx: &mut PathfinderCtx, pos: RelBlockPos) {
     let mut stair_facing = None;
 
     if is_unusual_shape {
-        // this is potentially expensive but it's rare enough that it shouldn't matter
-        // much
-        let block_below = ctx.world.get_block_state(pos.down(1));
-
-        let Some(found_stair_facing) = validate_stair_and_get_facing(block_below) else {
-            // return if it's not a stair or it's not facing the right way (like, if it's
-            // upside down or something)
-            return;
-        };
-
-        stair_facing = Some(found_stair_facing);
+        if !ctx.world.is_block_water(pos) {
+            // this is potentially expensive but it's rare enough that it shouldn't matter
+            // much
+            let block_below = ctx.world.get_block_state(pos.down(1));
+
+            let Some(found_stair_facing) = validate_stair_and_get_facing(block_below) else {
+                // return if it's not a stair or it's not facing the right way (like, if it's
+                // upside down or something)
+                return;
+            };
+
+            stair_facing = Some(found_stair_facing);
+        }
     }
 
     let break_cost_1 = ctx
@@ -260,16 +269,29 @@ fn descend_move(ctx: &mut PathfinderCtx, pos: RelBlockPos) {
             continue;
         }
 
+        let mut into_water = false;
         if fall_distance == 0 {
-            // if the fall distance is 0, set it to 1 so we try mining
-            fall_distance = 1
+            if ctx.world.is_block_water(new_horizontal_position.down(1)) {
+                fall_distance = 1;
+                into_water = true;
+            } else {
+                continue;
+            }
         }
 
         let new_position = new_horizontal_position.down(fall_distance as i32);
 
         // only mine if we're descending 1 block
-        let break_cost_2;
-        if fall_distance == 1 {
+        let mut break_cost_2;
+        if into_water {
+            break_cost_2 = ctx
+                .world
+                .cost_for_breaking_block(new_position.up(1), ctx.mining_cache);
+            if break_cost_2 == f32::INFINITY {
+                continue;
+            }
+            break_cost_2 += ENTER_WATER_PENALTY;
+        } else if fall_distance == 1 {
             break_cost_2 = ctx.world.cost_for_standing(new_position, ctx.mining_cache);
             if break_cost_2 == f32::INFINITY {
                 continue;
@@ -379,11 +401,9 @@ pub fn descend_is_reached(
     false
 }
 
-fn descend_forward_1_move(ctx: &mut PathfinderCtx, pos: RelBlockPos) {
-    if BARITONE_COMPAT {
-        return;
-    }
-
+// TODO: disabled for now (for performance), this should probably be moved into
+// its own category of moves
+fn _descend_forward_1_move(ctx: &mut PathfinderCtx, pos: RelBlockPos) {
     for dir in CardinalDirection::iter() {
         let dir_delta = RelBlockPos::new(dir.x(), 0, dir.z());
         let gap_horizontal_position = pos + dir_delta;
@@ -438,7 +458,7 @@ fn descend_forward_1_move(ctx: &mut PathfinderCtx, pos: RelBlockPos) {
 fn diagonal_move(ctx: &mut PathfinderCtx, pos: RelBlockPos) {
     let mut base_cost = SPRINT_ONE_BLOCK_COST;
 
-    let currently_in_water = ctx.world.is_block_water(pos.down(1));
+    let currently_in_water = ctx.world.is_block_water(pos);
     if currently_in_water {
         if BARITONE_COMPAT {
             base_cost = WALK_ONE_BLOCK_COST;
@@ -458,11 +478,40 @@ fn diagonal_move(ctx: &mut PathfinderCtx, pos: RelBlockPos) {
 
         let mut cost = base_cost;
 
-        let left_passable = ctx.world.is_passable(left_pos);
-        let right_passable = ctx.world.is_passable(right_pos);
+        let left_passable;
+        let right_passable;
 
-        if !left_passable && !right_passable {
-            continue;
+        if currently_in_water {
+            left_passable =
+                ctx.world.is_block_water(left_pos) && ctx.world.is_block_passable(left_pos.up(1));
+            if !left_passable {
+                // don't bother hugging corners while in water
+                continue;
+            }
+            right_passable =
+                ctx.world.is_block_water(right_pos) && ctx.world.is_block_passable(right_pos.up(1));
+            if !right_passable {
+                continue;
+            }
+        } else {
+            left_passable = ctx.world.is_passable(left_pos);
+            right_passable = ctx.world.is_passable(right_pos);
+            if !left_passable && !right_passable {
+                continue;
+            }
+        }
+
+        let new_position = pos + offset;
+        if currently_in_water {
+            if !ctx.world.is_block_water(new_position)
+                || !ctx.world.is_block_passable(new_position.up(1))
+            {
+                continue;
+            }
+        } else {
+            if !ctx.world.is_standable(new_position) {
+                continue;
+            }
         }
 
         if !left_passable || !right_passable {
@@ -474,22 +523,9 @@ fn diagonal_move(ctx: &mut PathfinderCtx, pos: RelBlockPos) {
             }
         }
 
-        if !ctx.world.is_standable(pos + offset) {
-            continue;
-        }
-
-        if BARITONE_COMPAT && currently_in_water {
-            let dest_in_water = ctx.world.is_block_water((pos + offset).down(1));
-            if !dest_in_water {
-                // baritone does a descend when we enter water, doesn't matter much in practice
-                // though
-                cost += 2. + FALL_N_BLOCKS_COST[1] + WALK_OFF_BLOCK_COST - SPRINT_ONE_BLOCK_COST;
-            }
-        }
-
         ctx.edges.push(Edge {
             movement: astar::Movement {
-                target: pos + offset,
+                target: new_position,
                 data: MoveData {
                     execute: &execute_diagonal_move,
                     is_reached: &default_is_reached,
diff --git a/azalea/src/pathfinder/moves/mod.rs b/azalea/src/pathfinder/moves/mod.rs
index 3d7f0ade..1d16d42e 100644
--- a/azalea/src/pathfinder/moves/mod.rs
+++ b/azalea/src/pathfinder/moves/mod.rs
@@ -28,7 +28,7 @@ use super::{
 use crate::{
     auto_tool::best_tool_in_hotbar_for_block,
     bot::{JumpEvent, LookAtEvent},
-    pathfinder::player_pos_to_block_pos,
+    pathfinder::{player_pos_to_block_pos, world::is_block_state_water},
 };
 
 type Edge = astar::Edge<RelBlockPos, MoveData>;
@@ -131,7 +131,7 @@ impl ExecuteCtx<'_, '_, '_, '_, '_, '_, '_, '_> {
     /// Returns whether this block could be mined.
     pub fn should_mine(&mut self, block: BlockPos) -> bool {
         let block_state = self.world.read().get_block_state(block).unwrap_or_default();
-        if is_block_state_passable(block_state) {
+        if is_block_state_passable(block_state) || is_block_state_water(block_state) {
             // block is already passable, no need to mine it
             return false;
         }
@@ -222,8 +222,8 @@ pub fn default_is_reached(
     if block_pos == target {
         return true;
     }
-    // it's fine if we slightly go under the target while swimming
-    if physics.is_in_water() && block_pos.up(1) == target {
+    // it's fine if we go over the target while swimming
+    if physics.is_in_water() && block_pos.down(1) == target {
         return true;
     }
 
diff --git a/azalea/src/pathfinder/moves/parkour.rs b/azalea/src/pathfinder/moves/parkour.rs
index bb92a137..09f4078a 100644
--- a/azalea/src/pathfinder/moves/parkour.rs
+++ b/azalea/src/pathfinder/moves/parkour.rs
@@ -34,6 +34,9 @@ fn parkour_forward_1_move(ctx: &mut PathfinderCtx, pos: RelBlockPos) {
             1
         } else if ctx.world.is_standable(pos + offset) {
             // forward
+
+            // no FALL_N_BLOCKS_COST[1] added here because we mostly don't have to wait for
+            // falling
             0
         } else {
             continue;
@@ -80,15 +83,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 mut cost = JUMP_PENALTY + WALK_ONE_BLOCK_COST * 3.;
 
         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];
+            // we mostly don't really wait for falling during parkour except here
+            cost += FALL_N_BLOCKS_COST[2] / 2.;
             -1
         } else {
             continue;
@@ -162,7 +165,7 @@ fn parkour_forward_3_move(ctx: &mut PathfinderCtx, pos: RelBlockPos) {
             continue;
         }
 
-        let cost = JUMP_PENALTY + SPRINT_ONE_BLOCK_COST * 4. + CENTER_AFTER_FALL_COST;
+        let cost = JUMP_PENALTY + SPRINT_ONE_BLOCK_COST * 4.;
 
         ctx.edges.push(Edge {
             movement: astar::Movement {
diff --git a/azalea/src/pathfinder/world.rs b/azalea/src/pathfinder/world.rs
index ac783ac1..ec2dbe80 100644
--- a/azalea/src/pathfinder/world.rs
+++ b/azalea/src/pathfinder/world.rs
@@ -344,6 +344,7 @@ impl CachedWorld {
             return 0.;
         }
 
+        let rel_pos = pos;
         let pos = pos.apply(self.origin);
 
         let (section_pos, section_block_pos) =
@@ -357,7 +358,9 @@ impl CachedWorld {
         let south_is_in_same_section = section_block_pos.z != 15;
         let west_is_in_same_section = section_block_pos.x != 0;
 
-        let Some(mining_cost) = self.with_section(section_pos, |section| {
+        let mut is_falling_block_above = false;
+
+        let Some(mut mining_cost) = self.with_section(section_pos, |section| {
             let block_state = section.get_at_index(u16::from(section_block_pos) as usize);
             let mining_cost = mining_cache.cost_for(block_state);
 
@@ -369,9 +372,12 @@ impl CachedWorld {
             // if there's a falling block or liquid above this block, abort
             if up_is_in_same_section {
                 let up_block = section.get_at_index(u16::from(section_block_pos.up(1)) as usize);
-                if mining_cache.is_liquid(up_block) || mining_cache.is_falling_block(up_block) {
+                if mining_cache.is_liquid(up_block) {
                     return f32::INFINITY;
                 }
+                if mining_cache.is_falling_block(up_block) {
+                    is_falling_block_above = true;
+                }
             }
 
             // if there's a liquid to the north of this block, abort
@@ -429,44 +435,55 @@ impl CachedWorld {
             return f32::INFINITY;
         }
 
-        let check_should_avoid_this_block = |pos: BlockPos, check: &dyn Fn(BlockState) -> bool| {
-            let block_state = self
+        fn check_should_avoid_this_block(
+            world: &CachedWorld,
+            pos: BlockPos,
+            check: impl FnOnce(BlockState) -> bool,
+        ) -> bool {
+            let block_state = world
                 .with_section(ChunkSectionPos::from(pos), |section| {
                     section.get_at_index(u16::from(ChunkSectionBlockPos::from(pos)) as usize)
                 })
                 .unwrap_or_default();
             check(block_state)
-        };
+        }
 
         // check the adjacent blocks that weren't in the same section
-        if !up_is_in_same_section
-            && check_should_avoid_this_block(pos.up(1), &|b| {
-                mining_cache.is_liquid(b) || mining_cache.is_falling_block(b)
-            })
-        {
-            return f32::INFINITY;
+        if !up_is_in_same_section {
+            if check_should_avoid_this_block(&self, pos.up(1), |b| {
+                if mining_cache.is_falling_block(b) {
+                    is_falling_block_above = true;
+                }
+                mining_cache.is_liquid(b)
+            }) {
+                return f32::INFINITY;
+            }
         }
         if !north_is_in_same_section
-            && check_should_avoid_this_block(pos.north(1), &|b| mining_cache.is_liquid(b))
+            && check_should_avoid_this_block(&self, pos.north(1), &|b| mining_cache.is_liquid(b))
         {
             return f32::INFINITY;
         }
         if !east_is_in_same_section
-            && check_should_avoid_this_block(pos.east(1), &|b| mining_cache.is_liquid(b))
+            && check_should_avoid_this_block(&self, pos.east(1), |b| mining_cache.is_liquid(b))
         {
             return f32::INFINITY;
         }
         if !south_is_in_same_section
-            && check_should_avoid_this_block(pos.south(1), &|b| mining_cache.is_liquid(b))
+            && check_should_avoid_this_block(&self, pos.south(1), |b| mining_cache.is_liquid(b))
         {
             return f32::INFINITY;
         }
         if !west_is_in_same_section
-            && check_should_avoid_this_block(pos.west(1), &|b| mining_cache.is_liquid(b))
+            && check_should_avoid_this_block(&self, pos.west(1), |b| mining_cache.is_liquid(b))
         {
             return f32::INFINITY;
         }
 
+        if is_falling_block_above {
+            mining_cost += self.cost_for_breaking_block(rel_pos.up(1), mining_cache);
+        }
+
         mining_cost
     }
 
@@ -501,7 +518,8 @@ impl CachedWorld {
         self.cost_for_passing(pos, mining_cache)
     }
 
-    /// Get the amount of air blocks until the next solid block below this one.
+    /// Get the amount of air/passable blocks until the next non-passable block
+    /// below this one.
     pub fn fall_distance(&self, pos: RelBlockPos) -> u32 {
         let mut distance = 0;
         let mut current_pos = pos.down(1);
@@ -630,9 +648,6 @@ pub fn is_block_state_standable(block_state: BlockState) -> bool {
     if is_block_state_solid(block_state) {
         return true;
     }
-    if is_block_state_water(block_state) {
-        return true;
-    }
 
     let block = BlockKind::from(block_state);
     if tags::blocks::SLABS.contains(&block) || tags::blocks::STAIRS.contains(&block) {