Try using solver contacts again, but in a more cache-coherent way.

3 files changed, 47 insertions, 41 deletions

diff --git a/src/geometry/contact_pair.rs b/src/geometry/contact_pair.rs
index 7b945ae..6b0815b 100644
--- a/src/geometry/contact_pair.rs
+++ b/src/geometry/contact_pair.rs
@@ -1,6 +1,6 @@
 use crate::dynamics::{BodyPair, RigidBodyHandle, RigidBodySet};
 use crate::geometry::{Collider, ColliderPair, ColliderSet, Contact, ContactManifold};
-use crate::math::{Isometry, Point, Vector};
+use crate::math::{Isometry, Point, Real, Vector};
 use cdl::query::ContactManifoldsWorkspace;
 use cdl::utils::MaybeSerializableData;
 #[cfg(feature = "simd-is-enabled")]
@@ -134,7 +134,7 @@ impl ContactPair {
         let coll2 = &colliders[self.pair.collider2];
 
         if self.manifolds.len() == 0 {
-            let manifold_data = ContactManifoldData::from_colliders(coll1, coll2, flags);
+            let manifold_data = ContactManifoldData::with_subshape_indices(coll1, coll2, flags);
             self.manifolds
                 .push(ContactManifold::with_data((0, 0), manifold_data));
         }
@@ -164,18 +164,21 @@ pub struct ContactManifoldData {
     pub(crate) position_constraint_index: usize,
     // We put the following fields here to avoids reading the colliders inside of the
     // contact preparation method.
-    /// The friction coefficient for of all the contacts on this contact manifold.
-    pub friction: f32,
-    /// The restitution coefficient for all the contacts on this contact manifold.
-    pub restitution: f32,
-    /// The relative position between the first collider and its parent at the time the
-    /// contact points were generated.
-    pub delta1: Isometry<f32>,
-    /// The relative position between the second collider and its parent at the time the
-    /// contact points were generated.
-    pub delta2: Isometry<f32>,
     /// Flags used to control some aspects of the constraints solver for this contact manifold.
     pub solver_flags: SolverFlags,
+    pub normal: Vector<Real>,
+    pub solver_contacts: Vec<SolverContact>,
+}
+
+#[derive(Copy, Clone, Debug)]
+#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
+pub struct SolverContact {
+    pub point: Point<Real>,
+    pub dist: Real,
+    pub friction: Real,
+    pub restitution: Real,
+    pub surface_velocity: Vector<Real>,
+    pub data: ContactData,
 }
 
 impl Default for ContactManifoldData {
@@ -185,39 +188,32 @@ impl Default for ContactManifoldData {
                 RigidBodySet::invalid_handle(),
                 RigidBodySet::invalid_handle(),
             ),
-            Isometry::identity(),
-            Isometry::identity(),
-            0.0,
-            0.0,
             SolverFlags::empty(),
         )
     }
 }
 
 impl ContactManifoldData {
-    pub(crate) fn new(
-        body_pair: BodyPair,
-        delta1: Isometry<f32>,
-        delta2: Isometry<f32>,
-        friction: f32,
-        restitution: f32,
-        solver_flags: SolverFlags,
-    ) -> ContactManifoldData {
+    pub(crate) fn new(body_pair: BodyPair, solver_flags: SolverFlags) -> ContactManifoldData {
         Self {
             body_pair,
             warmstart_multiplier: Self::min_warmstart_multiplier(),
-            friction,
-            restitution,
-            delta1,
-            delta2,
             constraint_index: 0,
             position_constraint_index: 0,
             solver_flags,
+            normal: Vector::zeros(),
+            solver_contacts: Vec::new(),
         }
     }
 
-    pub(crate) fn from_colliders(coll1: &Collider, coll2: &Collider, flags: SolverFlags) -> Self {
-        Self::with_subshape_indices(coll1, coll2, flags)
+    pub(crate) fn set_from_colliders(
+        &mut self,
+        coll1: &Collider,
+        coll2: &Collider,
+        flags: SolverFlags,
+    ) {
+        self.body_pair = BodyPair::new(coll1.parent, coll2.parent);
+        self.solver_flags = flags;
     }
 
     pub(crate) fn with_subshape_indices(
@@ -225,14 +221,7 @@ impl ContactManifoldData {
         coll2: &Collider,
         solver_flags: SolverFlags,
     ) -> Self {
-        Self::new(
-            BodyPair::new(coll1.parent, coll2.parent),
-            *coll1.position_wrt_parent(),
-            *coll2.position_wrt_parent(),
-            (coll1.friction + coll2.friction) * 0.5,
-            (coll1.restitution + coll2.restitution) * 0.5,
-            solver_flags,
-        )
+        Self::new(BodyPair::new(coll1.parent, coll2.parent), solver_flags)
     }
 
     pub(crate) fn min_warmstart_multiplier() -> f32 {
diff --git a/src/geometry/mod.rs b/src/geometry/mod.rs
index 0e8a288..cfc3086 100644
--- a/src/geometry/mod.rs
+++ b/src/geometry/mod.rs
@@ -4,7 +4,7 @@ pub use self::broad_phase_multi_sap::BroadPhase;
 pub use self::collider::{Collider, ColliderBuilder, ColliderShape};
 pub use self::collider_set::{ColliderHandle, ColliderSet};
 pub use self::contact_pair::{ContactData, ContactManifoldData};
-pub use self::contact_pair::{ContactPair, SolverFlags};
+pub use self::contact_pair::{ContactPair, SolverContact, SolverFlags};
 pub use self::interaction_graph::{
     ColliderGraphIndex, InteractionGraph, RigidBodyGraphIndex, TemporaryInteractionIndex,
 };
diff --git a/src/geometry/narrow_phase.rs b/src/geometry/narrow_phase.rs
index 85f8f70..7d89301 100644
--- a/src/geometry/narrow_phase.rs
+++ b/src/geometry/narrow_phase.rs
@@ -7,9 +7,10 @@ use crate::dynamics::RigidBodySet;
 use crate::geometry::{
     BroadPhasePairEvent, ColliderGraphIndex, ColliderHandle, ContactData, ContactEvent,
     ContactManifoldData, ContactPairFilter, IntersectionEvent, PairFilterContext,
-    ProximityPairFilter, RemovedCollider, SolverFlags,
+    ProximityPairFilter, RemovedCollider, SolverContact, SolverFlags,
 };
 use crate::geometry::{ColliderSet, ContactManifold, ContactPair, InteractionGraph};
+use crate::math::Vector;
 use crate::pipeline::EventHandler;
 use cdl::query::{DefaultQueryDispatcher, PersistentQueryDispatcher, QueryDispatcher};
 use std::collections::HashMap;
@@ -526,7 +527,23 @@ impl NarrowPhase {
 
             // TODO: don't write this everytime?
             for manifold in &mut pair.manifolds {
-                manifold.data = ContactManifoldData::from_colliders(co1, co2, solver_flags);
+                manifold.data.solver_contacts.clear();
+                manifold.data.set_from_colliders(co1, co2, solver_flags);
+                manifold.data.normal = co1.position() * manifold.local_n1;
+
+                for contact in &manifold.points[..manifold.num_active_contacts] {
+                    let solver_contact = SolverContact {
+                        point: co1.position() * contact.local_p1
+                            + manifold.data.normal * contact.dist / 2.0,
+                        dist: contact.dist,
+                        friction: (co1.friction + co2.friction) / 2.0,
+                        restitution: (co1.restitution + co2.restitution) / 2.0,
+                        surface_velocity: Vector::zeros(),
+                        data: contact.data,
+                    };
+
+                    manifold.data.solver_contacts.push(solver_contact);
+                }
             }
         });
     }