1 files changed, 136 insertions, 0 deletions
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,
+                );
+            }
+        }
+    }
+}