use better pathfinder costs and also fix relative entity updates breaking sometimes

5 files changed, 59 insertions, 11 deletions

diff --git a/azalea/src/lib.rs b/azalea/src/lib.rs
index 52551ea3..0f1ed243 100644
--- a/azalea/src/lib.rs
+++ b/azalea/src/lib.rs
@@ -3,6 +3,7 @@
 #![allow(incomplete_features)]
 #![feature(async_fn_in_trait)]
 #![feature(type_changing_struct_update)]
+#![feature(lazy_cell)]
 
 mod auto_respawn;
 mod bot;
diff --git a/azalea/src/pathfinder/costs.rs b/azalea/src/pathfinder/costs.rs
new file mode 100644
index 00000000..8ec2a49c
--- /dev/null
+++ b/azalea/src/pathfinder/costs.rs
@@ -0,0 +1,36 @@
+use std::sync::LazyLock;
+
+use num_traits::Float;
+
+// based on https://github.com/cabaletta/baritone/blob/1.20.1/src/api/java/baritone/api/pathing/movement/ActionCosts.java
+pub const WALK_ONE_BLOCK_COST: f32 = 20. / 4.317;
+pub const SPRINT_ONE_BLOCK_COST: f32 = 20. / 5.612;
+pub const FALL_ONE_BLOCK_COST: f32 = 0.5;
+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 static FALL_1_25_BLOCKS_COST: LazyLock<f32> = LazyLock::new(|| distance_to_ticks(1.25));
+pub static FALL_0_25_BLOCKS_COST: LazyLock<f32> = LazyLock::new(|| distance_to_ticks(0.25));
+pub static JUMP_ONE_BLOCK_COST: LazyLock<f32> =
+    LazyLock::new(|| *FALL_1_25_BLOCKS_COST - *FALL_0_25_BLOCKS_COST);
+
+fn velocity(ticks: usize) -> f32 {
+    (0.98.powi(ticks.try_into().unwrap()) - 1.) * -3.92
+}
+
+fn distance_to_ticks(distance: f32) -> f32 {
+    if distance == 0. {
+        // // Avoid 0/0 NaN
+        return 0.;
+    }
+    let mut temp_distance = distance;
+    let mut tick_count = 0;
+    loop {
+        let fall_distance = velocity(tick_count);
+        if temp_distance <= fall_distance {
+            return tick_count as f32 + temp_distance / fall_distance;
+        }
+        temp_distance -= fall_distance;
+        tick_count += 1;
+    }
+}
diff --git a/azalea/src/pathfinder/mod.rs b/azalea/src/pathfinder/mod.rs
index 76d901c3..0a55a598 100644
--- a/azalea/src/pathfinder/mod.rs
+++ b/azalea/src/pathfinder/mod.rs
@@ -1,4 +1,5 @@
 mod astar;
+pub mod costs;
 pub mod goals;
 mod moves;
 pub mod simulation;
@@ -156,6 +157,11 @@ fn goto_listener(
             debug!("partial: {partial:?}");
             debug!("time: {:?}", end_time - start_time);
 
+            println!("Path:");
+            for movement in &movements {
+                println!("  {:?}", movement.target);
+            }
+
             let path = movements.into_iter().collect::<VecDeque<_>>();
             Some(PathFoundEvent {
                 entity,
diff --git a/azalea/src/pathfinder/moves/basic.rs b/azalea/src/pathfinder/moves/basic.rs
index 2d0c47c2..b8886c9e 100644
--- a/azalea/src/pathfinder/moves/basic.rs
+++ b/azalea/src/pathfinder/moves/basic.rs
@@ -2,11 +2,13 @@ use azalea_client::{SprintDirection, StartSprintEvent};
 use azalea_core::{BlockPos, CardinalDirection};
 use azalea_world::Instance;
 
-use crate::{pathfinder::astar, JumpEvent, LookAtEvent};
+use crate::{
+    pathfinder::{astar, costs::*},
+    JumpEvent, LookAtEvent,
+};
 
 use super::{
     fall_distance, is_block_passable, is_passable, is_standable, Edge, ExecuteCtx, MoveData,
-    FALL_ONE_BLOCK_COST, JUMP_COST, WALK_ONE_BLOCK_COST,
 };
 
 pub fn basic_move(world: &Instance, node: BlockPos) -> Vec<Edge> {
@@ -27,7 +29,7 @@ fn forward_move(world: &Instance, pos: BlockPos) -> Vec<Edge> {
             continue;
         }
 
-        let cost = WALK_ONE_BLOCK_COST;
+        let cost = SPRINT_ONE_BLOCK_COST;
 
         edges.push(Edge {
             movement: astar::Movement {
@@ -68,11 +70,14 @@ fn ascend_move(world: &Instance, pos: BlockPos) -> Vec<Edge> {
     for dir in CardinalDirection::iter() {
         let offset = BlockPos::new(dir.x(), 1, dir.z());
 
-        if !is_block_passable(&pos.up(2), world) || !is_standable(&(pos + offset), world) {
+        if !is_block_passable(&pos.up(2), world) {
+            continue;
+        }
+        if !is_standable(&(pos + offset), world) {
             continue;
         }
 
-        let cost = WALK_ONE_BLOCK_COST + JUMP_COST;
+        let cost = SPRINT_ONE_BLOCK_COST + *JUMP_ONE_BLOCK_COST;
 
         edges.push(Edge {
             movement: astar::Movement {
@@ -121,8 +126,12 @@ fn descend_move(world: &Instance, pos: BlockPos) -> Vec<Edge> {
         if !is_passable(&new_horizontal_position, world) {
             continue;
         }
+        // check whether we can stand on the target position
+        if !is_standable(&new_position, world) {
+            continue;
+        }
 
-        let cost = WALK_ONE_BLOCK_COST + FALL_ONE_BLOCK_COST * fall_distance as f32;
+        let cost = SPRINT_ONE_BLOCK_COST + FALL_ONE_BLOCK_COST * fall_distance as f32;
 
         edges.push(Edge {
             movement: astar::Movement {
@@ -174,7 +183,7 @@ fn diagonal_move(world: &Instance, pos: BlockPos) -> Vec<Edge> {
         if !is_standable(&(pos + offset), world) {
             continue;
         }
-        let cost = WALK_ONE_BLOCK_COST * 1.4;
+        let cost = SPRINT_ONE_BLOCK_COST * 1.4;
 
         edges.push(Edge {
             movement: astar::Movement {
diff --git a/azalea/src/pathfinder/moves/mod.rs b/azalea/src/pathfinder/moves/mod.rs
index 63b74945..47589264 100644
--- a/azalea/src/pathfinder/moves/mod.rs
+++ b/azalea/src/pathfinder/moves/mod.rs
@@ -71,10 +71,6 @@ fn fall_distance(pos: &BlockPos, world: &Instance) -> u32 {
     distance
 }
 
-const JUMP_COST: f32 = 0.5;
-const WALK_ONE_BLOCK_COST: f32 = 1.0;
-const FALL_ONE_BLOCK_COST: f32 = 0.5;
-
 pub struct ExecuteCtx<'w1, 'w2, 'w3, 'w4, 'a> {
     pub entity: Entity,
     pub target: BlockPos,