Use contact ids instead of contact reordering in order to identify the impulse writeback location.

2 files changed, 15 insertions, 13 deletions

diff --git a/src/geometry/contact_pair.rs b/src/geometry/contact_pair.rs
index 462d3ef..c94a2cf 100644
--- a/src/geometry/contact_pair.rs
+++ b/src/geometry/contact_pair.rs
@@ -110,6 +110,8 @@ pub struct ContactManifoldData {
 #[derive(Copy, Clone, Debug)]
 #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
 pub struct SolverContact {
+    /// The index of the manifold contact used to generate this solver contact.
+    pub contact_id: u8,
     /// The world-space contact point.
     pub point: Point<Real>,
     /// The distance between the two original contacts points along the contact normal.
@@ -203,3 +205,9 @@ impl ContactManifoldData {
     //     manifold.data.warmstart_multiplier = Self::min_warmstart_multiplier()
     // }
 }
+
+/// A contact manifold that can be modified by the user.
+pub struct ModifiableContactManifold<'a> {
+    manifold: &'a super::ContactManifold,
+    solver_contacts: &'a mut Vec<SolverContact>,
+}
diff --git a/src/geometry/narrow_phase.rs b/src/geometry/narrow_phase.rs
index 640ce12..9513fef 100644
--- a/src/geometry/narrow_phase.rs
+++ b/src/geometry/narrow_phase.rs
@@ -546,15 +546,17 @@ impl NarrowPhase {
                 manifold.data.solver_flags = solver_flags;
                 manifold.data.normal = world_pos1 * manifold.local_n1;
 
-                // Sort contacts to keep only these with distances bellow
-                // the prediction, and generate solver contacts.
-                let mut first_inactive_index = manifold.points.len();
+                // Generate solver contacts.
+                for (contact_id, contact) in manifold.points.iter().enumerate() {
+                    assert!(
+                        contact_id <= u8::MAX as usize,
+                        "A contact manifold cannot contain more than 255 contacts currently."
+                    );
 
-                while manifold.data.num_active_contacts() != first_inactive_index {
-                    let contact = &manifold.points[manifold.data.num_active_contacts()];
                     if contact.dist < prediction_distance {
                         // Generate the solver contact.
                         let solver_contact = SolverContact {
+                            contact_id: contact_id as u8,
                             point: world_pos1 * contact.local_p1
                                 + manifold.data.normal * contact.dist / 2.0,
                             dist: contact.dist,
@@ -570,14 +572,6 @@ impl NarrowPhase {
                         has_any_active_contact = true;
                         continue;
                     }
-
-                    // If we reach this code, then the contact must be ignored by the constraints solver.
-                    // Swap with the last contact.
-                    manifold.points.swap(
-                        manifold.data.num_active_contacts(),
-                        first_inactive_index - 1,
-                    );
-                    first_inactive_index -= 1;
                 }
             }