implement stepping up stairs

6 files changed, 119 insertions, 36 deletions

diff --git a/azalea-client/src/movement.rs b/azalea-client/src/movement.rs
index ad26f49c..919be0e6 100644
--- a/azalea-client/src/movement.rs
+++ b/azalea-client/src/movement.rs
@@ -53,7 +53,9 @@ impl Plugin for PlayerMovePlugin {
             .add_systems(
                 FixedUpdate,
                 (
-                    local_player_ai_step.in_set(PhysicsSet),
+                    local_player_ai_step
+                        .in_set(PhysicsSet)
+                        .before(azalea_physics::ai_step),
                     send_position.after(update_in_loaded_chunk),
                 )
                     .chain(),
diff --git a/azalea-core/src/position.rs b/azalea-core/src/position.rs
index 77dc256a..eb6bcd89 100755
--- a/azalea-core/src/position.rs
+++ b/azalea-core/src/position.rs
@@ -24,6 +24,10 @@ macro_rules! vec3_impl {
                 (self - other).length_sqr()
             }
 
+            pub fn horizontal_distance_sqr(&self) -> $type {
+                self.x * self.x + self.z * self.z
+            }
+
             /// Return a new instance of this position with the y coordinate
             /// decreased by the given number.
             pub fn down(&self, y: $type) -> Self {
diff --git a/azalea-entity/src/plugin/mod.rs b/azalea-entity/src/plugin/mod.rs
index 7b514fcc..a1c7fcff 100644
--- a/azalea-entity/src/plugin/mod.rs
+++ b/azalea-entity/src/plugin/mod.rs
@@ -5,7 +5,7 @@ use std::collections::HashSet;
 
 use azalea_core::{BlockPos, Vec3};
 use azalea_world::{InstanceContainer, InstanceName, MinecraftEntityId};
-use bevy_app::{App, Plugin, PostUpdate, PreUpdate, Update};
+use bevy_app::{App, FixedUpdate, Plugin, PostUpdate, PreUpdate, Update};
 use bevy_ecs::prelude::*;
 use derive_more::{Deref, DerefMut};
 use log::debug;
@@ -63,12 +63,12 @@ impl Plugin for EntityPlugin {
                     relative_updates::debug_detect_updates_received_on_local_entities,
                     debug_new_entity,
                     add_dead,
-                    update_bounding_box,
                     clamp_look_direction,
                     update_fluid_on_eyes,
                 ),
             ),
         )
+        .add_systems(FixedUpdate, update_bounding_box)
         .init_resource::<EntityUuidIndex>();
     }
 }
diff --git a/azalea-physics/src/collision/mod.rs b/azalea-physics/src/collision/mod.rs
index e24df378..96dc1991 100644
--- a/azalea-physics/src/collision/mod.rs
+++ b/azalea-physics/src/collision/mod.rs
@@ -4,6 +4,8 @@ mod mergers;
 mod shape;
 mod world_collisions;
 
+use std::ops::Add;
+
 use azalea_core::{Axis, Vec3, AABB, EPSILON};
 use azalea_world::{Instance, MoveEntityError};
 pub use blocks::BlockWithShape;
@@ -55,15 +57,86 @@ fn collide(movement: &Vec3, world: &Instance, physics: &azalea_entity::Physics)
     // let entity_collisions = world.get_entity_collisions(self,
     // entity_bounding_box.expand_towards(movement));
     let entity_collisions = Vec::new();
-    if movement.length_sqr() == 0.0 {
+    let collided_movement = if movement.length_sqr() == 0.0 {
         *movement
     } else {
-        collide_bounding_box(movement, &entity_bounding_box, world, entity_collisions)
-    }
+        collide_bounding_box(movement, &entity_bounding_box, world, entity_collisions.clone())
+    };
+
+    let x_collision = movement.x != collided_movement.x;
+    let y_collision = movement.y != collided_movement.y;
+    let z_collision = movement.z != collided_movement.z;
+
+    let on_ground = physics.on_ground || y_collision && movement.y < 0.;
+
+    let max_up_step = 0.6;
+    if on_ground  && (x_collision || z_collision) {
+        // Vec3 var9 = collideBoundingBox(this, new Vec3(movement.x, (double)this.maxUpStep(), movement.z), var2, this.level(), var3);
+        // Vec3 var10 = collideBoundingBox(this, new Vec3(0.0, (double)this.maxUpStep(), 0.0), var2.expandTowards(movement.x, 0.0, movement.z), this.level(), var3);
+        // if (var10.y < (double)this.maxUpStep()) {
+        //    Vec3 var11 = collideBoundingBox(this, new Vec3(movement.x, 0.0, movement.z), var2.move(var10), this.level(), var3).add(var10);
+        //    if (var11.horizontalDistanceSqr() > var9.horizontalDistanceSqr()) {
+        //       var9 = var11;
+        //    }
+        // }
+
+        // if (var9.horizontalDistanceSqr() > collidedMovement.horizontalDistanceSqr()) {
+        //    return var9.add(collideBoundingBox(this, new Vec3(0.0, -var9.y + movement.y, 0.0), var2.move(var9), this.level(), var3));
+        // }
+
+
+        let mut var9 = collide_bounding_box(
+            &Vec3 {
+                x: movement.x,
+                y: max_up_step,
+                z: movement.z,
+            },
+            &entity_bounding_box,
+            world,
+            entity_collisions.clone(),
+        );
+        let var10 = collide_bounding_box(
+            &Vec3 {
+                x: 0.,
+                y: max_up_step,
+                z: 0.,
+            },
+            &entity_bounding_box.expand_towards(&Vec3::new(movement.x, 0., movement.z)),
+            world,
+            entity_collisions.clone(),
+        );
+        if var10.y < max_up_step {
+            let var11 = collide_bounding_box(
+                &Vec3 {
+                    x: movement.x,
+                    y: 0.,
+                    z: movement.z,
+                },
+                &entity_bounding_box.move_relative(&var10),
+                world,
+                entity_collisions.clone(),
+            )
+            .add(var10);
+            if var11.horizontal_distance_sqr() > var9.horizontal_distance_sqr() {
+                var9 = var11;
+            }
+        }
 
-    // TODO: stepping (for stairs and stuff)
+        if var9.horizontal_distance_sqr() > collided_movement.horizontal_distance_sqr() {
+            return var9.add(collide_bounding_box(
+                &Vec3 {
+                    x: 0.,
+                    y: -var9.y + movement.y,
+                    z: 0.,
+                },
+                &entity_bounding_box.move_relative(&var9),
+                world,
+                entity_collisions.clone(),
+            ));
+        }
+    }
 
-    // collided_movement
+    collided_movement
 }
 
 /// Move an entity by a given delta, checking for collisions.
diff --git a/azalea-physics/src/lib.rs b/azalea-physics/src/lib.rs
index 00be5185..50b2c11b 100644
--- a/azalea-physics/src/lib.rs
+++ b/azalea-physics/src/lib.rs
@@ -33,11 +33,15 @@ impl Plugin for PhysicsPlugin {
         app.add_event::<ForceJumpEvent>()
             .add_systems(
                 Update,
-                force_jump_listener
-                    .before(azalea_entity::update_bounding_box)
-                    .after(azalea_entity::clamp_look_direction),
+                force_jump_listener.after(azalea_entity::clamp_look_direction),
             )
-            .add_systems(FixedUpdate, (ai_step, travel).chain().in_set(PhysicsSet));
+            .add_systems(
+                FixedUpdate,
+                (ai_step, travel)
+                    .chain()
+                    .in_set(PhysicsSet)
+                    .after(azalea_entity::update_bounding_box),
+            );
     }
 }
 
diff --git a/azalea/src/pathfinder/mod.rs b/azalea/src/pathfinder/mod.rs
index 78bde5f1..ae88b0cc 100644
--- a/azalea/src/pathfinder/mod.rs
+++ b/azalea/src/pathfinder/mod.rs
@@ -405,28 +405,28 @@ mod tests {
         );
     }
 
-    // #[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)
-    //     );
-    // }
+    #[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..30 {
+            simulation.tick();
+            info!("-- tick #{i} --")
+        }
+        assert_eq!(
+            BlockPos::from(simulation.position()),
+            BlockPos::new(2, 71, 2)
+        );
+    }
 }