fix panic in a_star and some prep for pathfinder swimming

3 files changed, 23 insertions, 9 deletions

diff --git a/azalea/src/pathfinder/astar.rs b/azalea/src/pathfinder/astar.rs
index 719036f0..04ffdb80 100644
--- a/azalea/src/pathfinder/astar.rs
+++ b/azalea/src/pathfinder/astar.rs
@@ -170,12 +170,12 @@ where
         }
     }
 
-    let best_path = determine_best_path(best_paths, 0);
+    let best_path_idx = determine_best_path_idx(best_paths, 0);
     log_perf_info(start_time, num_nodes, num_movements);
     Path {
-        movements: reconstruct_path(nodes, best_path, successors),
+        movements: reconstruct_path(nodes, best_paths[best_path_idx], successors),
         is_partial: true,
-        cost: best_path_scores[best_path],
+        cost: best_path_scores[best_path_idx],
     }
 }
 
@@ -194,16 +194,16 @@ fn log_perf_info(start_time: Instant, num_nodes: usize, num_movements: usize) {
     );
 }
 
-fn determine_best_path(best_paths: [usize; 7], start: usize) -> usize {
+fn determine_best_path_idx(best_paths: [usize; 7], start: usize) -> usize {
     // this basically makes sure we don't create a path that's really short
 
-    for node in best_paths {
+    for (i, &node) in best_paths.iter().enumerate() {
         if node != start {
-            return node;
+            return i;
         }
     }
     warn!("No best node found, returning first node");
-    best_paths[0]
+    0
 }
 
 fn reconstruct_path<P, M, SuccessorsFn>(
diff --git a/azalea/src/pathfinder/costs.rs b/azalea/src/pathfinder/costs.rs
index f9b67e5f..631e2afc 100644
--- a/azalea/src/pathfinder/costs.rs
+++ b/azalea/src/pathfinder/costs.rs
@@ -10,6 +10,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;
+
 // explanation here:
 // https://github.com/cabaletta/baritone/blob/f147519a5c291015d4f18c94558a3f1bdcdb9588/src/api/java/baritone/api/Settings.java#L405
 // it's basically just the heuristic multiplier
diff --git a/azalea/src/pathfinder/moves/mod.rs b/azalea/src/pathfinder/moves/mod.rs
index fc574a4c..33c34c3b 100644
--- a/azalea/src/pathfinder/moves/mod.rs
+++ b/azalea/src/pathfinder/moves/mod.rs
@@ -217,10 +217,22 @@ pub struct IsReachedCtx<'a> {
 #[must_use]
 pub fn default_is_reached(
     IsReachedCtx {
-        position, target, ..
+        position,
+        target,
+        physics,
+        ..
     }: IsReachedCtx,
 ) -> bool {
-    player_pos_to_block_pos(position) == target
+    let block_pos = player_pos_to_block_pos(position);
+    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 {
+        return true;
+    }
+
+    false
 }
 
 pub struct PathfinderCtx<'a> {