add failing test for that weird diagonal edge case

aaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaa

7 files changed, 82 insertions, 33 deletions

diff --git a/Cargo.lock b/Cargo.lock
index ebad9585..0a6d3bd3 100644
--- a/Cargo.lock
+++ b/Cargo.lock
@@ -432,6 +432,7 @@ dependencies = [
  "bevy_app",
  "bevy_ecs",
  "bevy_time",
+ "log",
  "once_cell",
  "parking_lot",
  "uuid",
diff --git a/azalea-entity/src/lib.rs b/azalea-entity/src/lib.rs
index db42a0ba..fc312f79 100644
--- a/azalea-entity/src/lib.rs
+++ b/azalea-entity/src/lib.rs
@@ -257,6 +257,11 @@ pub struct Dead;
 /// an entity, and when raycasting from the entity.
 #[derive(Component, Clone, Copy, Debug, PartialEq, Deref, DerefMut)]
 pub struct EyeHeight(f32);
+impl EyeHeight {
+    pub fn new(height: f32) -> Self {
+        Self(height)
+    }
+}
 impl From<EyeHeight> for f32 {
     fn from(value: EyeHeight) -> Self {
         value.0
diff --git a/azalea-physics/Cargo.toml b/azalea-physics/Cargo.toml
index 58a180e1..fb482e2b 100644
--- a/azalea-physics/Cargo.toml
+++ b/azalea-physics/Cargo.toml
@@ -17,6 +17,7 @@ azalea-registry = { path = "../azalea-registry", version = "^0.7.0" }
 azalea-world = { path = "../azalea-world", version = "^0.7.0" }
 bevy_app = "0.11.1"
 bevy_ecs = "0.11.1"
+log = "0.4.20"
 once_cell = "1.18.0"
 parking_lot = "^0.12.1"
 
diff --git a/azalea-physics/src/lib.rs b/azalea-physics/src/lib.rs
index f004c0fb..00be5185 100644
--- a/azalea-physics/src/lib.rs
+++ b/azalea-physics/src/lib.rs
@@ -21,6 +21,7 @@ use bevy_ecs::{
     system::{Query, Res},
 };
 use collision::{move_colliding, MoverType};
+use log::trace;
 
 /// A Bevy [`SystemSet`] for running physics that makes entities do things.
 #[derive(SystemSet, Debug, Hash, PartialEq, Eq, Clone)]
@@ -227,6 +228,7 @@ fn handle_relative_friction_and_calculate_movement(
     position: &mut Position,
     attributes: &Attributes,
 ) -> Vec3 {
+    trace!("handle_relative_friction_and_calculate_movement {direction:?}");
     move_relative(
         physics,
         direction,
diff --git a/azalea/src/bot.rs b/azalea/src/bot.rs
index 1624db78..40ab4124 100644
--- a/azalea/src/bot.rs
+++ b/azalea/src/bot.rs
@@ -19,6 +19,7 @@ use azalea_physics::{force_jump_listener, PhysicsSet};
 use bevy_app::{FixedUpdate, Update};
 use bevy_ecs::prelude::Event;
 use bevy_ecs::schedule::IntoSystemConfigs;
+use log::trace;
 use std::f64::consts::PI;
 
 use crate::pathfinder::PathfinderPlugin;
@@ -161,6 +162,11 @@ fn look_at_listener(
         if let Ok((position, eye_height, mut look_direction)) = query.get_mut(event.entity) {
             let (y_rot, x_rot) =
                 direction_looking_at(&position.up(eye_height.into()), &event.position);
+            trace!(
+                "look at {:?} (currently at {:?})",
+                event.position,
+                **position
+            );
             (look_direction.y_rot, look_direction.x_rot) = (y_rot, x_rot);
         }
     }
diff --git a/azalea/src/pathfinder/mod.rs b/azalea/src/pathfinder/mod.rs
index d59f0046..78bde5f1 100644
--- a/azalea/src/pathfinder/mod.rs
+++ b/azalea/src/pathfinder/mod.rs
@@ -43,7 +43,7 @@ impl Plugin for PathfinderPlugin {
                 FixedUpdate,
                 // putting systems in the FixedUpdate schedule makes them run every Minecraft tick
                 // (every 50 milliseconds).
-                tick_execute_path.before(PhysicsSet),
+                tick_execute_path.after(PhysicsSet),
             )
             .add_systems(PreUpdate, add_default_pathfinder)
             .add_systems(
@@ -171,15 +171,6 @@ fn goto_listener(
                 edges
             };
 
-            // let mut pf = MTDStarLite::new(
-            //     start,
-            //     end,
-            //     |n| goal.heuristic(n),
-            //     successors,
-            //     successors,
-            //     |n| goal.success(n),
-            // );
-
             let start_time = std::time::Instant::now();
             let p = a_star(
                 start,
@@ -253,8 +244,9 @@ fn tick_execute_path(
                 position: center,
             });
             trace!(
-                "tick: pathfinder {entity:?}; going to {:?}; currently at {position:?}",
-                target.pos
+                "tick: pathfinder {entity:?}; going to {:?}; currently at {:?}",
+                target.pos,
+                **position
             );
             sprint_events.send(StartSprintEvent {
                 entity,
@@ -346,11 +338,12 @@ impl Node {
 
 #[cfg(test)]
 mod tests {
-    use std::sync::Arc;
+    use std::{collections::HashSet, sync::Arc};
 
     use azalea_core::{BlockPos, ChunkPos, Vec3};
     use azalea_world::{Chunk, ChunkStorage, PartialChunkStorage};
     use bevy_log::LogPlugin;
+    use log::info;
 
     use super::{
         goals::BlockPosGoal,
@@ -358,42 +351,82 @@ mod tests {
         GotoEvent,
     };
 
-    #[test]
-    fn test_simple_forward() {
+    fn setup_simulation(
+        partial_chunks: &mut PartialChunkStorage,
+        start_pos: BlockPos,
+        end_pos: BlockPos,
+        solid_blocks: Vec<BlockPos>,
+    ) -> Simulation {
+        let mut chunk_positions = HashSet::new();
+        for block_pos in &solid_blocks {
+            chunk_positions.insert(ChunkPos::from(block_pos));
+        }
+
         let mut chunks = ChunkStorage::default();
-        let mut partial_chunks = PartialChunkStorage::default();
-        partial_chunks.set(
-            &ChunkPos { x: 0, z: 0 },
-            Some(Chunk::default()),
-            &mut chunks,
-        );
-        chunks.set_block_state(
-            &BlockPos::new(0, 70, 0),
-            azalea_registry::Block::Stone.into(),
-        );
-        chunks.set_block_state(
-            &BlockPos::new(0, 70, 1),
-            azalea_registry::Block::Stone.into(),
-        );
-        let player = SimulatedPlayerBundle::new(Vec3::new(0.5, 71., 0.5));
+        for chunk_pos in chunk_positions {
+            partial_chunks.set(&chunk_pos, Some(Chunk::default()), &mut chunks);
+        }
+        for block_pos in solid_blocks {
+            chunks.set_block_state(&block_pos, azalea_registry::Block::Stone.into());
+        }
+        let player = SimulatedPlayerBundle::new(Vec3::new(
+            start_pos.x as f64 + 0.5,
+            start_pos.y as f64,
+            start_pos.z as f64 + 0.5,
+        ));
         let mut simulation = Simulation::new(chunks, player);
         simulation.app.add_plugins(LogPlugin {
-            level: bevy_log::Level::DEBUG,
+            level: bevy_log::Level::TRACE,
             filter: "".to_string(),
         });
 
         simulation.app.world.send_event(GotoEvent {
             entity: simulation.entity,
-            goal: Arc::new(BlockPosGoal::from(BlockPos::new(0, 71, 1))),
+            goal: Arc::new(BlockPosGoal::from(end_pos)),
         });
+        simulation
+    }
 
+    #[test]
+    fn test_simple_forward() {
+        let mut partial_chunks = PartialChunkStorage::default();
+        let mut simulation = setup_simulation(
+            &mut partial_chunks,
+            BlockPos::new(0, 71, 0),
+            BlockPos::new(0, 71, 1),
+            vec![BlockPos::new(0, 70, 0), BlockPos::new(0, 70, 1)],
+        );
         for _ in 0..20 {
             simulation.tick();
         }
-
         assert_eq!(
             BlockPos::from(simulation.position()),
             BlockPos::new(0, 71, 1)
         );
     }
+
+    // #[test]
+    // fn test_double_diagonal_with_walls() {
+    //     let mut partial_chunks = PartialChunkStorage::default();
+    //     let mut simulation = setup_simulation(
+    //         &mut partial_chunks,
+    //         BlockPos::new(0, 71, 0),
+    //         BlockPos::new(2, 71, 2),
+    //         vec![
+    //             BlockPos::new(0, 70, 0),
+    //             BlockPos::new(1, 70, 1),
+    //             BlockPos::new(2, 70, 2),
+    //             BlockPos::new(1, 72, 0),
+    //             BlockPos::new(2, 72, 1),
+    //         ],
+    //     );
+    //     for i in 0..20 {
+    //         simulation.tick();
+    //         info!("-- tick #{i} --")
+    //     }
+    //     assert_eq!(
+    //         BlockPos::from(simulation.position()),
+    //         BlockPos::new(2, 71, 2)
+    //     );
+    // }
 }
diff --git a/azalea/src/pathfinder/simulation.rs b/azalea/src/pathfinder/simulation.rs
index 372a8a3b..9ee77052 100644
--- a/azalea/src/pathfinder/simulation.rs
+++ b/azalea/src/pathfinder/simulation.rs
@@ -89,6 +89,7 @@ impl Simulation {
                 azalea_entity::LookDirection::default(),
                 Sprinting(true),
                 azalea_entity::metadata::Player,
+                azalea_entity::EyeHeight::new(player.physics.dimensions.height * 0.85),
                 player,
             ))
             .id();