entity collisions

5 files changed, 86 insertions, 74 deletions

diff --git a/README.md b/README.md
index 2361154c..5af01e75 100755
--- a/README.md
+++ b/README.md
@@ -17,7 +17,7 @@ _Currently supported Minecraft version: `1.21.4`._
 
 ## Features
 
--   [Accurate physics](https://github.com/azalea-rs/azalea/blob/main/azalea-physics/src/lib.rs) (but some features like entity collisions and elytras aren't yet implemented)
+-   [Accurate physics](https://github.com/azalea-rs/azalea/blob/main/azalea-physics/src/lib.rs) (but some features like entity pushing and elytras aren't yet implemented)
 -   [Pathfinder](https://azalea.matdoes.dev/azalea/pathfinder/index.html)
 -   [Swarms](https://azalea.matdoes.dev/azalea/swarm/index.html)
 -   [Breaking blocks](https://azalea.matdoes.dev/azalea/struct.Client.html#method.mine)
diff --git a/azalea-core/src/position.rs b/azalea-core/src/position.rs
index 5d923d39..2dea1471 100755
--- a/azalea-core/src/position.rs
+++ b/azalea-core/src/position.rs
@@ -125,6 +125,16 @@ macro_rules! vec3_impl {
                     z: self.z,
                 }
             }
+
+            pub fn with_x(&self, x: $type) -> Self {
+                Self { x, ..*self }
+            }
+            pub fn with_y(&self, y: $type) -> Self {
+                Self { y, ..*self }
+            }
+            pub fn with_z(&self, z: $type) -> Self {
+                Self { z, ..*self }
+            }
         }
 
         impl Add for &$name {
diff --git a/azalea-physics/src/collision/mod.rs b/azalea-physics/src/collision/mod.rs
index 77af1232..56491e2d 100644
--- a/azalea-physics/src/collision/mod.rs
+++ b/azalea-physics/src/collision/mod.rs
@@ -15,9 +15,10 @@ use azalea_core::{
     position::{BlockPos, Vec3},
 };
 use azalea_world::{ChunkStorage, Instance, MoveEntityError};
-use bevy_ecs::world::Mut;
+use bevy_ecs::{entity::Entity, world::Mut};
 pub use blocks::BlockWithShape;
 pub use discrete_voxel_shape::*;
+use entity_collisions::{CollidableEntityQuery, PhysicsQuery, get_entity_collisions};
 pub use shape::*;
 use tracing::warn;
 
@@ -32,50 +33,27 @@ pub enum MoverType {
     Shulker,
 }
 
-// private Vec3 collide(Vec3 var1) {
-//     AABB var2 = this.getBoundingBox();
-//     List var3 = this.level.getEntityCollisions(this,
-// var2.expandTowards(var1));     Vec3 var4 = var1.lengthSqr() == 0.0D ?
-// var1 : collideBoundingBox(this, var1, var2, this.level, var3);
-//     boolean var5 = var1.x != var4.x;
-//     boolean var6 = var1.y != var4.y;
-//     boolean var7 = var1.z != var4.z;
-//     boolean var8 = this.onGround || var6 && var1.y < 0.0D;
-//     if (this.maxUpStep > 0.0F && var8 && (var5 || var7)) {
-//        Vec3 var9 = collideBoundingBox(this, new Vec3(var1.x,
-// (double)this.maxUpStep, var1.z), var2, this.level, var3);        Vec3
-// var10 = collideBoundingBox(this, new Vec3(0.0D, (double)this.maxUpStep,
-// 0.0D), var2.expandTowards(var1.x, 0.0D, var1.z), this.level, var3);
-//        if (var10.y < (double)this.maxUpStep) {
-//           Vec3 var11 = collideBoundingBox(this, new Vec3(var1.x, 0.0D,
-// var1.z), var2.move(var10), this.level, var3).add(var10);           if
-// (var11.horizontalDistanceSqr() > var9.horizontalDistanceSqr()) {
-//              var9 = var11;
-//           }
-//        }
-
-//        if (var9.horizontalDistanceSqr() > var4.horizontalDistanceSqr()) {
-//           return var9.add(collideBoundingBox(this, new Vec3(0.0D, -var9.y +
-// var1.y, 0.0D), var2.move(var9), this.level, var3));        }
-//     }
-
-//     return var4;
-// }
-fn collide(movement: &Vec3, world: &Instance, physics: &azalea_entity::Physics) -> Vec3 {
+// Entity.collide
+fn collide(
+    movement: &Vec3,
+    world: &Instance,
+    physics: &azalea_entity::Physics,
+    source_entity: Option<Entity>,
+    physics_query: &PhysicsQuery,
+    collidable_entity_query: &CollidableEntityQuery,
+) -> Vec3 {
     let entity_bounding_box = physics.bounding_box;
-    // TODO: get_entity_collisions
-    // let entity_collisions = world.get_entity_collisions(self,
-    // entity_bounding_box.expand_towards(movement));
-    let entity_collisions = Vec::new();
+    let entity_collisions = get_entity_collisions(
+        world,
+        &entity_bounding_box.expand_towards(movement),
+        source_entity,
+        physics_query,
+        collidable_entity_query,
+    );
     let collided_delta = if movement.length_squared() == 0.0 {
         *movement
     } else {
-        collide_bounding_box(
-            movement,
-            &entity_bounding_box,
-            world,
-            entity_collisions.clone(),
-        )
+        collide_bounding_box(movement, &entity_bounding_box, world, &entity_collisions)
     };
 
     let x_collision = movement.x != collided_delta.x;
@@ -87,35 +65,23 @@ fn collide(movement: &Vec3, world: &Instance, physics: &azalea_entity::Physics)
     let max_up_step = 0.6;
     if max_up_step > 0. && on_ground && (x_collision || z_collision) {
         let mut step_to_delta = collide_bounding_box(
-            &Vec3 {
-                x: movement.x,
-                y: max_up_step,
-                z: movement.z,
-            },
+            &movement.with_y(max_up_step),
             &entity_bounding_box,
             world,
-            entity_collisions.clone(),
+            &entity_collisions,
         );
         let directly_up_delta = collide_bounding_box(
-            &Vec3 {
-                x: 0.,
-                y: max_up_step,
-                z: 0.,
-            },
+            &Vec3::ZERO.with_y(max_up_step),
             &entity_bounding_box.expand_towards(&Vec3::new(movement.x, 0., movement.z)),
             world,
-            entity_collisions.clone(),
+            &entity_collisions,
         );
         if directly_up_delta.y < max_up_step {
             let target_movement = collide_bounding_box(
-                &Vec3 {
-                    x: movement.x,
-                    y: 0.,
-                    z: movement.z,
-                },
+                &movement.with_y(0.),
                 &entity_bounding_box.move_relative(directly_up_delta),
                 world,
-                entity_collisions.clone(),
+                &entity_collisions,
             )
             .add(directly_up_delta);
             if target_movement.horizontal_distance_squared()
@@ -129,14 +95,10 @@ fn collide(movement: &Vec3, world: &Instance, physics: &azalea_entity::Physics)
             > collided_delta.horizontal_distance_squared()
         {
             return step_to_delta.add(collide_bounding_box(
-                &Vec3 {
-                    x: 0.,
-                    y: -step_to_delta.y + movement.y,
-                    z: 0.,
-                },
+                &Vec3::ZERO.with_y(-step_to_delta.y + movement.y),
                 &entity_bounding_box.move_relative(step_to_delta),
                 world,
-                entity_collisions.clone(),
+                &entity_collisions,
             ));
         }
     }
@@ -147,12 +109,16 @@ fn collide(movement: &Vec3, world: &Instance, physics: &azalea_entity::Physics)
 /// Move an entity by a given delta, checking for collisions.
 ///
 /// In Mojmap, this is `Entity.move`.
+#[allow(clippy::too_many_arguments)]
 pub fn move_colliding(
     _mover_type: MoverType,
     movement: &Vec3,
     world: &Instance,
     position: &mut Mut<azalea_entity::Position>,
     physics: &mut azalea_entity::Physics,
+    source_entity: Option<Entity>,
+    physics_query: &PhysicsQuery,
+    collidable_entity_query: &CollidableEntityQuery,
 ) -> Result<(), MoveEntityError> {
     // TODO: do all these
 
@@ -175,7 +141,14 @@ pub fn move_colliding(
 
     // movement = this.maybeBackOffFromEdge(movement, moverType);
 
-    let collide_result = collide(movement, world, physics);
+    let collide_result = collide(
+        movement,
+        world,
+        physics,
+        source_entity,
+        physics_query,
+        collidable_entity_query,
+    );
 
     let move_distance = collide_result.length_squared();
 
@@ -269,12 +242,12 @@ fn collide_bounding_box(
     movement: &Vec3,
     entity_bounding_box: &AABB,
     world: &Instance,
-    entity_collisions: Vec<VoxelShape>,
+    entity_collisions: &[VoxelShape],
 ) -> Vec3 {
     let mut collision_boxes: Vec<VoxelShape> = Vec::with_capacity(entity_collisions.len() + 1);
 
     if !entity_collisions.is_empty() {
-        collision_boxes.extend(entity_collisions);
+        collision_boxes.extend_from_slice(entity_collisions);
     }
 
     // TODO: world border
diff --git a/azalea-physics/src/lib.rs b/azalea-physics/src/lib.rs
index 2e8132c8..c1603710 100644
--- a/azalea-physics/src/lib.rs
+++ b/azalea-physics/src/lib.rs
@@ -21,13 +21,18 @@ use azalea_entity::{
 use azalea_world::{Instance, InstanceContainer, InstanceName};
 use bevy_app::{App, Plugin};
 use bevy_ecs::{
+    entity::Entity,
     query::With,
     schedule::{IntoSystemConfigs, SystemSet},
     system::{Query, Res},
     world::Mut,
 };
 use clip::box_traverse_blocks;
-use collision::{BLOCK_SHAPE, BlockWithShape, MoverType, VoxelShape, move_colliding};
+use collision::{
+    BLOCK_SHAPE, BlockWithShape, MoverType, VoxelShape,
+    entity_collisions::{CollidableEntityQuery, PhysicsQuery},
+    move_colliding,
+};
 
 /// A Bevy [`SystemSet`] for running physics that makes entities do things.
 #[derive(SystemSet, Debug, Hash, PartialEq, Eq, Clone)]
@@ -352,7 +357,7 @@ fn get_block_pos_below_that_affects_movement(position: &Position) -> BlockPos {
 }
 
 /// Options for [`handle_relative_friction_and_calculate_movement`]
-struct HandleRelativeFrictionAndCalculateMovementOpts<'a> {
+struct HandleRelativeFrictionAndCalculateMovementOpts<'a, 'b, 'world, 'state> {
     block_friction: f32,
     world: &'a Instance,
     physics: &'a mut Physics,
@@ -363,6 +368,9 @@ struct HandleRelativeFrictionAndCalculateMovementOpts<'a> {
     on_climbable: &'a OnClimbable,
     pose: Option<&'a Pose>,
     jumping: &'a Jumping,
+    entity: Entity,
+    physics_query: &'a PhysicsQuery<'world, 'state, 'b>,
+    collidable_entity_query: &'a CollidableEntityQuery<'world, 'state>,
 }
 fn handle_relative_friction_and_calculate_movement(
     HandleRelativeFrictionAndCalculateMovementOpts {
@@ -376,7 +384,10 @@ fn handle_relative_friction_and_calculate_movement(
         on_climbable,
         pose,
         jumping,
-    }: HandleRelativeFrictionAndCalculateMovementOpts<'_>,
+        entity,
+        physics_query,
+        collidable_entity_query,
+    }: HandleRelativeFrictionAndCalculateMovementOpts<'_, '_, '_, '_>,
 ) -> Vec3 {
     move_relative(
         physics,
@@ -397,6 +408,9 @@ fn handle_relative_friction_and_calculate_movement(
         world,
         &mut position,
         physics,
+        Some(entity),
+        physics_query,
+        collidable_entity_query,
     )
     .expect("Entity should exist");
     // let delta_movement = entity.delta;
diff --git a/azalea-physics/src/travel.rs b/azalea-physics/src/travel.rs
index 0ee39f73..8f89de25 100644
--- a/azalea-physics/src/travel.rs
+++ b/azalea-physics/src/travel.rs
@@ -83,6 +83,8 @@ pub fn travel(
             );
         } else {
             travel_in_air(
+                &world,
+                entity,
                 &mut physics,
                 &direction,
                 position,
@@ -91,7 +93,8 @@ pub fn travel(
                 on_climbable,
                 pose,
                 jumping,
-                &world,
+                &physics_query,
+                &collidable_entity_query,
             );
         }
     }
@@ -100,6 +103,8 @@ pub fn travel(
 /// The usual movement when we're not in water or using an elytra.
 #[allow(clippy::too_many_arguments)]
 fn travel_in_air(
+    world: &Instance,
+    entity: Entity,
     physics: &mut Physics,
     direction: &LookDirection,
     position: Mut<Position>,
@@ -108,7 +113,8 @@ fn travel_in_air(
     on_climbable: &OnClimbable,
     pose: Option<&Pose>,
     jumping: &Jumping,
-    world: &Instance,
+    physics_query: &PhysicsQuery,
+    collidable_entity_query: &CollidableEntityQuery,
 ) {
     let gravity = get_effective_gravity();
 
@@ -140,6 +146,9 @@ fn travel_in_air(
             on_climbable,
             pose,
             jumping,
+            entity,
+            physics_query,
+            collidable_entity_query,
         },
     );
 
@@ -210,6 +219,9 @@ fn travel_in_fluid(
             world,
             &mut position,
             physics,
+            Some(entity),
+            physics_query,
+            collidable_entity_query,
         )
         .expect("Entity should exist");
 
@@ -231,6 +243,9 @@ fn travel_in_fluid(
             world,
             &mut position,
             physics,
+            Some(entity),
+            physics_query,
+            collidable_entity_query,
         )
         .expect("Entity should exist");