character interactions with entities

5 files changed, 152 insertions, 16 deletions

diff --git a/src/sim/entity/mod.rs b/src/sim/entity/mod.rs
index 4a8eda4..59479a2 100644
--- a/src/sim/entity/mod.rs
+++ b/src/sim/entity/mod.rs
@@ -2,7 +2,6 @@ use glam::{IVec2, Vec2};
 use rapier2d::{
     dynamics::{RigidBody, RigidBodyBuilder, RigidBodyHandle},
     geometry::{Collider, ColliderHandle},
-    math::Pose2,
 };
 
 use crate::{
@@ -14,13 +13,6 @@ use crate::{
     sprite_loader::load_sprite_to_cells,
 };
 
-fn mass_from_cells(cells: &[Cell]) -> f32 {
-    cells
-        .iter()
-        .fold(0.0, |acc, cur| acc + cur.material.def().density as f32)
-        * MASS_SCALING
-}
-
 pub struct EntityCells {
     pub size: IVec2,
     pub cells: Vec<Cell>,
@@ -35,6 +27,13 @@ impl EntityCells {
     pub fn set_cell_at_local_position(&mut self, pos: IVec2, cell: Cell) {
         self.cells[pos.x as usize + pos.y as usize * self.size.x as usize] = cell;
     }
+
+    pub fn mass(&self) -> f32 {
+        self.cells
+            .iter()
+            .fold(0.0, |acc, cur| acc + cur.material.def().density as f32)
+            * MASS_SCALING
+    }
 }
 
 impl Marchable for EntityCells {
@@ -76,7 +75,7 @@ impl EntityDef {
         behaviour: Option<Box<dyn EntityBehaviour>>,
     ) -> Self {
         let collider = RbManager::convex_hull_collider_from_marchable(&cells, Some(10))
-            .mass(mass_from_cells(&cells.cells))
+            .mass(cells.mass())
             .build();
 
         EntityDef {
@@ -95,7 +94,7 @@ impl EntityDef {
         let rb = RigidBodyBuilder::dynamic().translation(position).build();
 
         let collider = RbManager::convex_hull_collider_from_marchable(&cells, Some(10))
-            .mass(mass_from_cells(&cells.cells))
+            .mass(cells.mass())
             .build();
 
         EntityDef {
@@ -135,7 +134,7 @@ impl EntityDef {
             .build();
 
         let collider = RbManager::convex_hull_collider_from_marchable(&cells, Some(10))
-            .mass(mass_from_cells(&cells.cells))
+            .mass(cells.mass())
             .build();
 
         EntityDef {
@@ -259,7 +258,7 @@ impl Entity {
     pub fn compute_collider(&self) -> Option<Collider> {
         self.data.cells.as_ref().map(|cells| {
             RbManager::convex_hull_collider_from_marchable(cells, Some(10))
-                .mass(mass_from_cells(&cells.cells))
+                .mass(cells.mass())
                 .build()
         })
     }
diff --git a/src/sim/lib/components.rs b/src/sim/lib/components.rs
index d43a567..ac62366 100644
--- a/src/sim/lib/components.rs
+++ b/src/sim/lib/components.rs
@@ -111,7 +111,7 @@ pub fn compute_components(cells: &[Cell], w: i32, h: i32) -> Vec<PositionedCompo
                         as usize] = old_cell.cell;
                 }
 
-                let pc_centre = (c.bounds[0] + c.bounds[1] + IVec2::ONE).as_vec2() / 2.0 ;
+                let pc_centre = (c.bounds[0] + c.bounds[1] + IVec2::ONE).as_vec2() / 2.0;
                 let adjusted_pc_centre = pc_centre - (Vec2::new(w as f32, h as f32) / 2.0);
                 let pc = PositionedComponent {
                     position: adjusted_pc_centre,
diff --git a/src/sim/rb_manager/character_impulses.rs b/src/sim/rb_manager/character_impulses.rs
new file mode 100644
index 0000000..7016956
--- /dev/null
+++ b/src/sim/rb_manager/character_impulses.rs
@@ -0,0 +1,136 @@
+//! Local port of `KinematicCharacterController::solve_character_collision_impulses`.
+//!
+//! Rapier's version (0.35.2, still present on master) accumulates every nearby collider's
+//! contact manifolds into one shared `Vec` and then slices it with `manifolds[prev_len..]`.
+//! That assumes `contact_manifolds` appends, but parry's composite-shape paths (compound,
+//! polyline, trimesh, ...) `mem::take` the output vec and rebuild it for the current pair
+//! only. With two or more dynamic bodies inside the character's AABB the second call empties
+//! the vec and the slice panics with "range start index 1 out of range for slice of length 0".
+//! The character's collider here is a convex decomposition (a compound), so we hit it.
+//!
+//! This port uses a fresh scratch vec per collider, which is what rapier's own code intended.
+
+use rapier2d::{
+    control::{CharacterCollision, CharacterLength, KinematicCharacterController},
+    geometry::{ContactManifold, Shape},
+    math::{Pose, Real},
+    parry::{
+        bounding_volume::BoundingVolume,
+        query::{DefaultQueryDispatcher, PersistentQueryDispatcher},
+    },
+    pipeline::QueryPipelineMut,
+};
+
+/// Apply approximate impulses to the dynamic bodies a kinematic character ran into.
+pub fn solve_character_collision_impulses<'a>(
+    controller: &KinematicCharacterController,
+    dt: Real,
+    queries: &mut QueryPipelineMut,
+    character_shape: &dyn Shape,
+    character_mass: Real,
+    collisions: impl IntoIterator<Item = &'a CharacterCollision>,
+) {
+    for collision in collisions {
+        solve_single_character_collision_impulse(
+            controller,
+            dt,
+            queries,
+            character_shape,
+            character_mass,
+            collision,
+        );
+    }
+}
+
+fn eval_length(length: CharacterLength, value: Real) -> Real {
+    match length {
+        CharacterLength::Relative(x) => value * x,
+        CharacterLength::Absolute(x) => x,
+    }
+}
+
+fn solve_single_character_collision_impulse(
+    controller: &KinematicCharacterController,
+    dt: Real,
+    queries: &mut QueryPipelineMut,
+    character_shape: &dyn Shape,
+    character_mass: Real,
+    collision: &CharacterCollision,
+) {
+    let extents = character_shape.compute_local_aabb().extents();
+    let up_extent = extents.dot(controller.up.abs());
+    let movement_to_transfer =
+        collision.hit.normal1 * collision.translation_remaining.dot(collision.hit.normal1);
+    // `KinematicCharacterController::predict_ground` is private; this is its body.
+    let prediction = eval_length(controller.offset, up_extent) + 0.05;
+
+    let dispatcher = DefaultQueryDispatcher;
+
+    let mut manifolds: Vec<ContactManifold> = Vec::new();
+    // World pose of the collider each manifold was computed against: the `local_p2`
+    // points are in the collider's frame, which differs from its body's when offset.
+    let mut manifold_collider_poses: Vec<Pose> = Vec::new();
+    let character_aabb = character_shape
+        .compute_aabb(&collision.character_pos)
+        .loosened(prediction);
+
+    for (_, collider) in queries.as_ref().intersect_aabb_conservative(character_aabb) {
+        let Some(parent) = collider.parent() else {
+            continue;
+        };
+        let Some(body) = queries.bodies.get(parent) else {
+            continue;
+        };
+        if !body.is_dynamic() {
+            continue;
+        }
+
+        let pos12 = collision.character_pos.inv_mul(collider.position());
+        // Fresh vec per pair: parry may take/replace it rather than append.
+        let mut pair_manifolds: Vec<ContactManifold> = Vec::new();
+        let _ = dispatcher.contact_manifolds(
+            &pos12,
+            character_shape,
+            collider.shape(),
+            prediction,
+            &mut pair_manifolds,
+            &mut None,
+        );
+
+        for mut m in pair_manifolds {
+            m.data.rigid_body2 = Some(parent);
+            m.data.normal = collision.character_pos.rotation * m.local_n1;
+            manifolds.push(m);
+            manifold_collider_poses.push(*collider.position());
+        }
+    }
+
+    let inv_dt = if dt != 0.0 { 1.0 / dt } else { 0.0 };
+    let velocity_to_transfer = movement_to_transfer * inv_dt;
+
+    for (manifold, collider_pos) in manifolds.iter().zip(manifold_collider_poses.iter()) {
+        let Some(body_handle) = manifold.data.rigid_body2 else {
+            continue;
+        };
+        let Some(body) = queries.bodies.get_mut(body_handle) else {
+            continue;
+        };
+
+        for pt in &manifold.points {
+            if pt.dist <= prediction {
+                let body_mass = body.mass();
+                let contact_point = *collider_pos * pt.local_p2;
+                let delta_vel_per_contact = (velocity_to_transfer
+                    - body.velocity_at_point(contact_point))
+                .dot(manifold.data.normal);
+                let mass_ratio = body_mass * character_mass / (body_mass + character_mass);
+
+                body.apply_impulse_at_point(
+                    manifold.data.normal * delta_vel_per_contact.max(0.0) * mass_ratio,
+                    contact_point,
+                    true,
+                );
+            }
+        }
+    }
+}
diff --git a/src/sim/rb_manager/mod.rs b/src/sim/rb_manager/mod.rs
index 1be024a..1da5e40 100644
--- a/src/sim/rb_manager/mod.rs
+++ b/src/sim/rb_manager/mod.rs
@@ -1,3 +1,4 @@
+pub mod character_impulses;
 pub mod debug_render;
 
 use fxhash::FxHashMap;
diff --git a/src/sim/sim_manager/mod.rs b/src/sim/sim_manager/mod.rs
index bbfd05e..1e6da4d 100644
--- a/src/sim/sim_manager/mod.rs
+++ b/src/sim/sim_manager/mod.rs
@@ -177,9 +177,9 @@ impl SimManager {
         if input_manager.pressed(Input::Action2)
             && let Some(entity_id) = written_entities_scope
                 .get_entity_id_at_position(input_manager.world_mouse_pos.round().as_ivec2())
-            {
-                self.atomize_entity(entity_id);
-            }
+        {
+            self.atomize_entity(entity_id);
+        }
 
         // TEST ATOMIZATION