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

8 files changed, 71 insertions, 36 deletions

diff --git a/azalea-entity/src/plugin/indexing.rs b/azalea-entity/src/plugin/indexing.rs
index be5c0a5b..507e6737 100644
--- a/azalea-entity/src/plugin/indexing.rs
+++ b/azalea-entity/src/plugin/indexing.rs
@@ -290,7 +290,7 @@ pub fn remove_despawned_entities_from_indexes(
                 warn!("Tried to remove entity from chunk {chunk:?} but the entity was not there.");
             }
         } else {
-            warn!("Tried to remove entity from chunk {chunk:?} but the chunk was not found.");
+            debug!("Tried to remove entity from chunk {chunk:?} but the chunk was not found.");
         }
         // remove it from the uuid index
         if entity_infos.entity_by_uuid.remove(uuid).is_none() {
diff --git a/azalea-entity/src/plugin/mod.rs b/azalea-entity/src/plugin/mod.rs
index 41ec4e6a..94e8e79d 100644
--- a/azalea-entity/src/plugin/mod.rs
+++ b/azalea-entity/src/plugin/mod.rs
@@ -59,7 +59,6 @@ impl Plugin for EntityPlugin {
                 )
                     .in_set(EntityUpdateSet::Index),
                 (
-                    relative_updates::add_updates_received,
                     relative_updates::debug_detect_updates_received_on_local_entities,
                     debug_new_entity,
                     add_dead,
diff --git a/azalea-entity/src/plugin/relative_updates.rs b/azalea-entity/src/plugin/relative_updates.rs
index 7d01feda..21b57cff 100644
--- a/azalea-entity/src/plugin/relative_updates.rs
+++ b/azalea-entity/src/plugin/relative_updates.rs
@@ -20,8 +20,8 @@ use std::sync::Arc;
 use azalea_world::{MinecraftEntityId, PartialInstance};
 use bevy_ecs::{
     prelude::{Component, Entity},
-    query::{Changed, With, Without},
-    system::{Commands, EntityCommand, Query},
+    query::With,
+    system::{EntityCommand, Query},
     world::{EntityMut, World},
 };
 use derive_more::{Deref, DerefMut};
@@ -69,24 +69,29 @@ impl EntityCommand for RelativeEntityUpdate {
         };
 
         let entity_id = *entity_mut.get::<MinecraftEntityId>().unwrap();
-        let Some(updates_received) = entity_mut.get_mut::<UpdatesReceived>() else {
+        if entity_mut.contains::<Local>() {
             // a client tried to update another client, which isn't allowed
             return;
-        };
+        }
 
         let this_client_updates_received = partial_entity_infos
             .updates_received
             .get(&entity_id)
             .copied();
 
-        let can_update = this_client_updates_received.unwrap_or(1) == **updates_received;
+        let can_update = if let Some(updates_received) = entity_mut.get::<UpdatesReceived>() {
+            this_client_updates_received.unwrap_or(1) == **updates_received
+        } else {
+            // no UpdatesReceived means the entity was just spawned
+            true
+        };
         if can_update {
             let new_updates_received = this_client_updates_received.unwrap_or(0) + 1;
             partial_entity_infos
                 .updates_received
                 .insert(entity_id, new_updates_received);
 
-            **entity_mut.get_mut::<UpdatesReceived>().unwrap() = new_updates_received;
+            entity_mut.insert(UpdatesReceived(new_updates_received));
 
             let mut entity = world.entity_mut(entity);
             (self.update)(&mut entity);
@@ -94,23 +99,6 @@ impl EntityCommand for RelativeEntityUpdate {
     }
 }
 
-#[allow(clippy::type_complexity)]
-pub fn add_updates_received(
-    mut commands: Commands,
-    query: Query<
-        Entity,
-        (
-            Changed<MinecraftEntityId>,
-            (Without<UpdatesReceived>, Without<Local>),
-        ),
-    >,
-) {
-    for entity in query.iter() {
-        // entities always start with 1 update received
-        commands.entity(entity).insert(UpdatesReceived(1));
-    }
-}
-
 /// The [`UpdatesReceived`] component should never be on [`Local`] entities.
 /// This warns if an entity has both components.
 pub fn debug_detect_updates_received_on_local_entities(
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,