pathfinder swimming

9 files changed, 119 insertions, 27 deletions

diff --git a/azalea/examples/testbot/commands.rs b/azalea/examples/testbot/commands.rs
index beb87510..930f41ca 100644
--- a/azalea/examples/testbot/commands.rs
+++ b/azalea/examples/testbot/commands.rs
@@ -30,7 +30,7 @@ impl CommandSource {
         }
     }
 
-    pub fn entity(&mut self) -> Option<azalea::EntityRef> {
+    pub fn entity(&self) -> Option<azalea::EntityRef> {
         let username = self.chat.sender()?;
         self.bot
             .any_entity_by::<&GameProfileComponent, With<Player>>(
diff --git a/azalea/examples/testbot/commands/debug.rs b/azalea/examples/testbot/commands/debug.rs
index 36c699a4..1f9d5e5d 100644
--- a/azalea/examples/testbot/commands/debug.rs
+++ b/azalea/examples/testbot/commands/debug.rs
@@ -7,7 +7,10 @@ use azalea::{
     brigadier::prelude::*,
     chunks::ReceiveChunkEvent,
     packet::game,
-    pathfinder::{ExecutingPath, Pathfinder},
+    pathfinder::{
+        ExecutingPath, Pathfinder, custom_state::CustomPathfinderStateRef, mining::MiningCache,
+        moves::PathfinderCtx, rel_block_pos::RelBlockPos, world::CachedWorld,
+    },
 };
 use azalea_core::hit_result::HitResult;
 use azalea_entity::{EntityKindComponent, metadata};
@@ -33,7 +36,7 @@ pub fn register(commands: &mut CommandDispatcher<Mutex<CommandSource>>) {
     }));
 
     commands.register(literal("whereami").executes(|ctx: &Ctx| {
-        let mut source = ctx.source.lock();
+        let source = ctx.source.lock();
         let Some(entity) = source.entity() else {
             source.reply("You aren't in render distance!");
             return 0;
@@ -47,7 +50,7 @@ pub fn register(commands: &mut CommandDispatcher<Mutex<CommandSource>>) {
     }));
 
     commands.register(literal("entityid").executes(|ctx: &Ctx| {
-        let mut source = ctx.source.lock();
+        let source = ctx.source.lock();
         let Some(entity) = source.entity() else {
             source.reply("You aren't in render distance!");
             return 0;
@@ -160,7 +163,7 @@ pub fn register(commands: &mut CommandDispatcher<Mutex<CommandSource>>) {
         let source = ctx.source.lock();
         let pathfinder = source.bot.get_component::<Pathfinder>();
         let Some(pathfinder) = pathfinder else {
-            source.reply("I don't have the Pathfinder ocmponent");
+            source.reply("I don't have the Pathfinder component");
             return 1;
         };
         source.reply(format!(
@@ -185,6 +188,42 @@ pub fn register(commands: &mut CommandDispatcher<Mutex<CommandSource>>) {
         ));
         1
     }));
+    commands.register(literal("pathfindermoves").executes(|ctx: &Ctx| {
+        let source = ctx.source.lock();
+
+        let Some(entity) = source.entity() else {
+            source.reply("You aren't in render distance!");
+            return 0;
+        };
+        let position = entity.position();
+        let position = BlockPos::from(position);
+
+        let mut edges = Vec::new();
+        let cached_world = CachedWorld::new(source.bot.world(), position);
+        let mining_cache = MiningCache::new(None);
+        let custom_state = CustomPathfinderStateRef::default();
+
+        azalea::pathfinder::moves::default_move(
+            &mut PathfinderCtx {
+                edges: &mut edges,
+                world: &cached_world,
+                mining_cache: &mining_cache,
+                custom_state: &custom_state,
+            },
+            RelBlockPos::from_origin(position, position),
+        );
+
+        if edges.is_empty() {
+            source.reply("No possible moves.");
+        } else {
+            source.reply("Moves:");
+            for (i, edge) in edges.iter().enumerate() {
+                source.reply(format!("{}) {edge:?}", i + 1));
+            }
+        }
+
+        1
+    }));
 
     commands.register(literal("startuseitem").executes(|ctx: &Ctx| {
         let source = ctx.source.lock();
diff --git a/azalea/examples/testbot/commands/movement.rs b/azalea/examples/testbot/commands/movement.rs
index c1af4143..209a1c80 100644
--- a/azalea/examples/testbot/commands/movement.rs
+++ b/azalea/examples/testbot/commands/movement.rs
@@ -15,7 +15,7 @@ pub fn register(commands: &mut CommandDispatcher<Mutex<CommandSource>>) {
     commands.register(
         literal("goto")
             .executes(|ctx: &Ctx| {
-                let mut source = ctx.source.lock();
+                let source = ctx.source.lock();
                 println!("got goto");
                 // look for the sender
                 let Some(entity) = source.entity() else {
@@ -88,7 +88,7 @@ pub fn register(commands: &mut CommandDispatcher<Mutex<CommandSource>>) {
         literal("look")
             .executes(|ctx: &Ctx| {
                 // look for the sender
-                let mut source = ctx.source.lock();
+                let source = ctx.source.lock();
                 let Some(entity) = source.entity() else {
                     source.reply("I can't see you!");
                     return 0;
diff --git a/azalea/src/pathfinder/astar.rs b/azalea/src/pathfinder/astar.rs
index 04ffdb80..58a0322c 100644
--- a/azalea/src/pathfinder/astar.rs
+++ b/azalea/src/pathfinder/astar.rs
@@ -75,9 +75,13 @@ where
     let mut num_movements = 0;
 
     while let Some(WeightedNode { index, g_score, .. }) = open_set.pop() {
+        let (&node, node_data) = nodes.get_index(index).unwrap();
+        if g_score > node_data.g_score {
+            continue;
+        }
+
         num_nodes += 1;
 
-        let (&node, node_data) = nodes.get_index(index).unwrap();
         if success(node) {
             let best_path = index;
             log_perf_info(start_time, num_nodes, num_movements);
@@ -89,10 +93,6 @@ where
             };
         }
 
-        if g_score > node_data.g_score {
-            continue;
-        }
-
         for neighbor in successors(node) {
             let tentative_g_score = g_score + neighbor.cost;
             // let neighbor_heuristic = heuristic(neighbor.movement.target);
@@ -124,6 +124,9 @@ where
                 }
             }
 
+            // we don't update the existing node, which means that the same node might be
+            // present in the open_set multiple times. this is fine because at the start of
+            // the loop we check `g_score > node_data.g_score`.
             open_set.push(WeightedNode {
                 index: neighbor_index,
                 g_score: tentative_g_score,
@@ -180,11 +183,12 @@ where
 }
 
 fn log_perf_info(start_time: Instant, num_nodes: usize, num_movements: usize) {
-    let elapsed_seconds = start_time.elapsed().as_secs_f64();
+    let elapsed = start_time.elapsed();
+    let elapsed_seconds = elapsed.as_secs_f64();
     let nodes_per_second = (num_nodes as f64 / elapsed_seconds) as u64;
     let num_movements_per_second = (num_movements as f64 / elapsed_seconds) as u64;
     debug!(
-        "Nodes considered: {}",
+        "Considered {} nodes in {elapsed:?}",
         num_nodes.to_formatted_string(&num_format::Locale::en)
     );
     debug!(
diff --git a/azalea/src/pathfinder/costs.rs b/azalea/src/pathfinder/costs.rs
index 631e2afc..7d59fc0e 100644
--- a/azalea/src/pathfinder/costs.rs
+++ b/azalea/src/pathfinder/costs.rs
@@ -9,8 +9,7 @@ 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 const SWIM_ONE_BLOCK_COST: f32 = 20. / 1.960;
+pub const WALK_ONE_IN_WATER_COST: f32 = 20. / 1.960; // 10.204
 
 // explanation here:
 // https://github.com/cabaletta/baritone/blob/f147519a5c291015d4f18c94558a3f1bdcdb9588/src/api/java/baritone/api/Settings.java#L405
diff --git a/azalea/src/pathfinder/moves/basic.rs b/azalea/src/pathfinder/moves/basic.rs
index 539d989d..128a6daf 100644
--- a/azalea/src/pathfinder/moves/basic.rs
+++ b/azalea/src/pathfinder/moves/basic.rs
@@ -20,10 +20,17 @@ pub fn basic_move(ctx: &mut PathfinderCtx, node: RelBlockPos) {
 }
 
 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
+    if ctx.world.is_block_water(pos.down(1)) {
+        base_cost = WALK_ONE_IN_WATER_COST;
+    }
+
     for dir in CardinalDirection::iter() {
         let offset = RelBlockPos::new(dir.x(), 0, dir.z());
 
-        let mut cost = SPRINT_ONE_BLOCK_COST;
+        let mut cost = base_cost;
 
         let break_cost = ctx.world.cost_for_standing(pos + offset, ctx.mining_cache);
         if break_cost == f32::INFINITY {
@@ -407,14 +414,21 @@ 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;
+    if ctx.world.is_block_water(pos.down(1)) {
+        base_cost = WALK_ONE_IN_WATER_COST;
+    }
+
+    // add 0.001 as a tie-breaker to avoid unnecessarily going diagonal
+    base_cost = base_cost.mul_add(SQRT_2, 0.001);
+
     for dir in CardinalDirection::iter() {
         let right = dir.right();
         let offset = RelBlockPos::new(dir.x() + right.x(), 0, dir.z() + right.z());
         let left_pos = RelBlockPos::new(pos.x + dir.x(), pos.y, pos.z + dir.z());
         let right_pos = RelBlockPos::new(pos.x + right.x(), pos.y, pos.z + right.z());
 
-        // +0.001 so it doesn't unnecessarily go diagonal sometimes
-        let mut cost = SPRINT_ONE_BLOCK_COST * SQRT_2 + 0.001;
+        let mut cost = base_cost;
 
         let left_passable = ctx.world.is_passable(left_pos);
         let right_passable = ctx.world.is_passable(right_pos);
diff --git a/azalea/src/pathfinder/moves/parkour.rs b/azalea/src/pathfinder/moves/parkour.rs
index b4a03732..9ff10417 100644
--- a/azalea/src/pathfinder/moves/parkour.rs
+++ b/azalea/src/pathfinder/moves/parkour.rs
@@ -22,7 +22,7 @@ fn parkour_forward_1_move(ctx: &mut PathfinderCtx, pos: RelBlockPos) {
         let offset = RelBlockPos::new(dir.x() * 2, 0, dir.z() * 2);
 
         // make sure we actually have to jump
-        if ctx.world.is_block_solid((pos + gap_offset).down(1)) {
+        if ctx.world.is_block_standable((pos + gap_offset).down(1)) {
             continue;
         }
         if !ctx.world.is_passable(pos + gap_offset) {
@@ -75,8 +75,8 @@ fn parkour_forward_2_move(ctx: &mut PathfinderCtx, pos: RelBlockPos) {
         let offset = RelBlockPos::new(dir.x() * 3, 0, dir.z() * 3);
 
         // make sure we actually have to jump
-        if ctx.world.is_block_solid((pos + gap_1_offset).down(1))
-            || ctx.world.is_block_solid((pos + gap_2_offset).down(1))
+        if ctx.world.is_block_standable((pos + gap_1_offset).down(1))
+            || ctx.world.is_block_standable((pos + gap_2_offset).down(1))
         {
             continue;
         }
@@ -134,9 +134,9 @@ fn parkour_forward_3_move(ctx: &mut PathfinderCtx, pos: RelBlockPos) {
         let offset = RelBlockPos::new(dir.x() * 4, 0, dir.z() * 4);
 
         // make sure we actually have to jump
-        if ctx.world.is_block_solid((pos + gap_1_offset).down(1))
-            || ctx.world.is_block_solid((pos + gap_2_offset).down(1))
-            || ctx.world.is_block_solid((pos + gap_3_offset).down(1))
+        if ctx.world.is_block_standable((pos + gap_1_offset).down(1))
+            || ctx.world.is_block_standable((pos + gap_2_offset).down(1))
+            || ctx.world.is_block_standable((pos + gap_3_offset).down(1))
         {
             continue;
         }
diff --git a/azalea/src/pathfinder/rel_block_pos.rs b/azalea/src/pathfinder/rel_block_pos.rs
index 65b61ffa..0900ac91 100644
--- a/azalea/src/pathfinder/rel_block_pos.rs
+++ b/azalea/src/pathfinder/rel_block_pos.rs
@@ -28,7 +28,7 @@ impl RelBlockPos {
     }
 
     #[inline]
-    pub fn new(x: i16, y: i32, z: i16) -> Self {
+    pub const fn new(x: i16, y: i32, z: i16) -> Self {
         Self { x, y, z }
     }
 
diff --git a/azalea/src/pathfinder/world.rs b/azalea/src/pathfinder/world.rs
index a44af6b5..956b0226 100644
--- a/azalea/src/pathfinder/world.rs
+++ b/azalea/src/pathfinder/world.rs
@@ -1,3 +1,4 @@
+use core::f32;
 use std::{
     cell::{RefCell, UnsafeCell},
     sync::Arc,
@@ -94,6 +95,7 @@ pub struct CachedSection {
     pub solid_bitset: FastFixedBitSet<4096>,
     /// Blocks that we can stand on but might not be able to parkour from.
     pub standable_bitset: FastFixedBitSet<4096>,
+    pub water_bitset: FastFixedBitSet<4096>,
 }
 
 impl CachedWorld {
@@ -208,6 +210,8 @@ impl CachedWorld {
             let mut passable_bitset = FastFixedBitSet::<4096>::new();
             let mut solid_bitset = FastFixedBitSet::<4096>::new();
             let mut standable_bitset = FastFixedBitSet::<4096>::new();
+            let mut water_bitset = FastFixedBitSet::<4096>::new();
+
             for i in 0..4096 {
                 let block_state = section.get_at_index(i);
                 if is_block_state_passable(block_state) {
@@ -219,12 +223,16 @@ impl CachedWorld {
                 if is_block_state_standable(block_state) {
                     standable_bitset.set(i);
                 }
+                if is_block_state_water(block_state) {
+                    water_bitset.set(i);
+                }
             }
             CachedSection {
                 pos: section_pos,
                 passable_bitset,
                 solid_bitset,
                 standable_bitset,
+                water_bitset,
             }
         })
     }
@@ -232,7 +240,6 @@ impl CachedWorld {
     pub fn is_block_passable(&self, pos: RelBlockPos) -> bool {
         self.is_block_pos_passable(pos.apply(self.origin))
     }
-
     fn is_block_pos_passable(&self, pos: BlockPos) -> bool {
         let (section_pos, section_block_pos) =
             (ChunkSectionPos::from(pos), ChunkSectionBlockPos::from(pos));
@@ -251,6 +258,27 @@ impl CachedWorld {
         passable
     }
 
+    pub fn is_block_water(&self, pos: RelBlockPos) -> bool {
+        self.is_block_pos_water(pos.apply(self.origin))
+    }
+    fn is_block_pos_water(&self, pos: BlockPos) -> bool {
+        let (section_pos, section_block_pos) =
+            (ChunkSectionPos::from(pos), ChunkSectionBlockPos::from(pos));
+        let index = u16::from(section_block_pos) as usize;
+        // SAFETY: we're only accessing this from one thread
+        let cached_blocks = unsafe { &mut *self.cached_blocks.get() };
+        if let Some(cached) = cached_blocks.get_mut(section_pos) {
+            return cached.water_bitset.index(index);
+        }
+
+        let Some(cached) = self.calculate_bitsets_for_section(section_pos) else {
+            return false;
+        };
+        let water = cached.water_bitset.index(index);
+        cached_blocks.insert(cached);
+        water
+    }
+
     /// Get the block state at the given position.
     ///
     /// This is relatively slow, so you should avoid it whenever possible.
@@ -622,6 +650,9 @@ 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) {
@@ -631,6 +662,11 @@ pub fn is_block_state_standable(block_state: BlockState) -> bool {
     false
 }
 
+pub fn is_block_state_water(block_state: BlockState) -> bool {
+    // only the default blockstate
+    block_state == BlockState::from(BlockKind::Water)
+}
+
 #[cfg(test)]
 mod tests {
     use azalea_world::{Chunk, ChunkStorage, PartialInstance};