Perform contact sorting in the narrow-phase directly.

3 files changed, 62 insertions, 119 deletions

diff --git a/src/geometry/contact_pair.rs b/src/geometry/contact_pair.rs
index 6b0815b..3ad4f70 100644
--- a/src/geometry/contact_pair.rs
+++ b/src/geometry/contact_pair.rs
@@ -1,13 +1,7 @@
-use crate::dynamics::{BodyPair, RigidBodyHandle, RigidBodySet};
-use crate::geometry::{Collider, ColliderPair, ColliderSet, Contact, ContactManifold};
-use crate::math::{Isometry, Point, Real, Vector};
+use crate::dynamics::{BodyPair, RigidBodySet};
+use crate::geometry::{Collider, ColliderPair, ContactManifold};
+use crate::math::{Point, Real, Vector};
 use cdl::query::ContactManifoldsWorkspace;
-use cdl::utils::MaybeSerializableData;
-#[cfg(feature = "simd-is-enabled")]
-use {
-    crate::math::{SimdReal, SIMD_WIDTH},
-    simba::simd::SimdValue,
-};
 
 bitflags::bitflags! {
     #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
@@ -19,33 +13,6 @@ bitflags::bitflags! {
     }
 }
 
-#[cfg(feature = "simd-is-enabled")]
-pub(crate) struct WContact {
-    pub local_p1: Point<SimdReal>,
-    pub local_p2: Point<SimdReal>,
-    pub local_n1: Vector<SimdReal>,
-    pub local_n2: Vector<SimdReal>,
-    pub dist: SimdReal,
-    pub fid1: [u8; SIMD_WIDTH],
-    pub fid2: [u8; SIMD_WIDTH],
-}
-
-#[cfg(feature = "simd-is-enabled")]
-impl WContact {
-    pub fn extract(&self, i: usize) -> (Contact, Vector<f32>, Vector<f32>) {
-        let c = Contact {
-            local_p1: self.local_p1.extract(i),
-            local_p2: self.local_p2.extract(i),
-            dist: self.dist.extract(i),
-            fid1: self.fid1[i],
-            fid2: self.fid2[i],
-            data: ContactData::default(),
-        };
-
-        (c, self.local_n1.extract(i), self.local_n2.extract(i))
-    }
-}
-
 #[derive(Copy, Clone, Debug)]
 #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
 /// A single contact between two collider.
@@ -112,40 +79,13 @@ impl ContactPair {
     /// An active contact is a contact that may result in a non-zero contact force.
     pub fn has_any_active_contact(&self) -> bool {
         for manifold in &self.manifolds {
-            if manifold.num_active_contacts != 0 {
+            if manifold.data.num_active_contacts() != 0 {
                 return true;
             }
         }
 
         false
     }
-
-    pub(crate) fn single_manifold<'a, 'b>(
-        &'a mut self,
-        colliders: &'b ColliderSet,
-        flags: SolverFlags,
-    ) -> (
-        &'b Collider,
-        &'b Collider,
-        &'a mut ContactManifold,
-        Option<&'a mut (dyn MaybeSerializableData)>,
-    ) {
-        let coll1 = &colliders[self.pair.collider1];
-        let coll2 = &colliders[self.pair.collider2];
-
-        if self.manifolds.len() == 0 {
-            let manifold_data = ContactManifoldData::with_subshape_indices(coll1, coll2, flags);
-            self.manifolds
-                .push(ContactManifold::with_data((0, 0), manifold_data));
-        }
-
-        (
-            coll1,
-            coll2,
-            &mut self.manifolds[0],
-            self.workspace.as_mut().map(|w| &mut *w.0),
-        )
-    }
 }
 
 #[derive(Clone, Debug)]
@@ -206,22 +146,9 @@ impl ContactManifoldData {
         }
     }
 
-    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(
-        coll1: &Collider,
-        coll2: &Collider,
-        solver_flags: SolverFlags,
-    ) -> Self {
-        Self::new(BodyPair::new(coll1.parent, coll2.parent), solver_flags)
+    #[inline]
+    pub fn num_active_contacts(&self) -> usize {
+        self.solver_contacts.len()
     }
 
     pub(crate) fn min_warmstart_multiplier() -> f32 {
@@ -232,25 +159,25 @@ impl ContactManifoldData {
         1.0 // 0.01
     }
 
-    pub(crate) fn update_warmstart_multiplier(manifold: &mut ContactManifold) {
-        // In 2D, tall stacks will actually suffer from this
-        // because oscillation due to inaccuracies in 2D often
-        // cause contacts to break, which would result in
-        // a reset of the warmstart multiplier.
-        if cfg!(feature = "dim2") {
-            manifold.data.warmstart_multiplier = 1.0;
-            return;
-        }
-
-        for pt in &manifold.points {
-            if pt.data.impulse != 0.0 {
-                manifold.data.warmstart_multiplier =
-                    (manifold.data.warmstart_multiplier * 2.0).min(1.0);
-                return;
-            }
-        }
-
-        // Reset the multiplier.
-        manifold.data.warmstart_multiplier = Self::min_warmstart_multiplier()
-    }
+    // pub(crate) fn update_warmstart_multiplier(manifold: &mut ContactManifold) {
+    //     // In 2D, tall stacks will actually suffer from this
+    //     // because oscillation due to inaccuracies in 2D often
+    //     // cause contacts to break, which would result in
+    //     // a reset of the warmstart multiplier.
+    //     if cfg!(feature = "dim2") {
+    //         manifold.data.warmstart_multiplier = 1.0;
+    //         return;
+    //     }
+    //
+    //     for pt in &manifold.points {
+    //         if pt.data.impulse != 0.0 {
+    //             manifold.data.warmstart_multiplier =
+    //                 (manifold.data.warmstart_multiplier * 2.0).min(1.0);
+    //             return;
+    //         }
+    //     }
+    //
+    //     // Reset the multiplier.
+    //     manifold.data.warmstart_multiplier = Self::min_warmstart_multiplier()
+    // }
 }
diff --git a/src/geometry/mod.rs b/src/geometry/mod.rs
index 23e34cc..2b6d14f 100644
--- a/src/geometry/mod.rs
+++ b/src/geometry/mod.rs
@@ -81,8 +81,6 @@ impl IntersectionEvent {
 
 pub(crate) use self::broad_phase_multi_sap::{BroadPhasePairEvent, ColliderPair};
 pub(crate) use self::collider_set::RemovedCollider;
-#[cfg(feature = "simd-is-enabled")]
-pub(crate) use self::contact_pair::WContact;
 pub use self::interaction_groups::InteractionGroups;
 pub(crate) use self::narrow_phase::ContactManifoldIndex;
 pub(crate) use cdl::partitioning::SimdQuadTree;
diff --git a/src/geometry/narrow_phase.rs b/src/geometry/narrow_phase.rs
index 7d89301..5d9cca5 100644
--- a/src/geometry/narrow_phase.rs
+++ b/src/geometry/narrow_phase.rs
@@ -3,7 +3,7 @@ use rayon::prelude::*;
 
 use crate::data::pubsub::Subscription;
 use crate::data::Coarena;
-use crate::dynamics::RigidBodySet;
+use crate::dynamics::{BodyPair, RigidBodySet};
 use crate::geometry::{
     BroadPhasePairEvent, ColliderGraphIndex, ColliderHandle, ContactData, ContactEvent,
     ContactManifoldData, ContactPairFilter, IntersectionEvent, PairFilterContext,
@@ -12,7 +12,7 @@ use crate::geometry::{
 use crate::geometry::{ColliderSet, ContactManifold, ContactPair, InteractionGraph};
 use crate::math::Vector;
 use crate::pipeline::EventHandler;
-use cdl::query::{DefaultQueryDispatcher, PersistentQueryDispatcher, QueryDispatcher};
+use cdl::query::{DefaultQueryDispatcher, PersistentQueryDispatcher};
 use std::collections::HashMap;
 use std::sync::Arc;
 
@@ -389,7 +389,6 @@ impl NarrowPhase {
 
     pub(crate) fn compute_intersections(
         &mut self,
-        prediction_distance: f32,
         bodies: &RigidBodySet,
         colliders: &ColliderSet,
         pair_filter: Option<&dyn ProximityPairFilter>,
@@ -528,21 +527,40 @@ impl NarrowPhase {
             // TODO: don't write this everytime?
             for manifold in &mut pair.manifolds {
                 manifold.data.solver_contacts.clear();
-                manifold.data.set_from_colliders(co1, co2, solver_flags);
+                manifold.data.body_pair = BodyPair::new(co1.parent(), co2.parent());
+                manifold.data.solver_flags = 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,
-                    };
+                // Sort contacts and generate solver contacts.
+                let mut first_inactive_index = manifold.points.len();
+
+                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 {
+                            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,
+                        };
+
+                        // TODO: apply the user-defined contact modification/removal, if needed.
+
+                        manifold.data.solver_contacts.push(solver_contact);
+                        continue;
+                    }
 
-                    manifold.data.solver_contacts.push(solver_contact);
+                    // 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;
                 }
             }
         });
@@ -569,7 +587,7 @@ impl NarrowPhase {
                     .data
                     .solver_flags
                     .contains(SolverFlags::COMPUTE_IMPULSES)
-                    && manifold.num_active_contacts() != 0
+                    && manifold.data.num_active_contacts() != 0
                     && (rb1.is_dynamic() || rb2.is_dynamic())
                     && (!rb1.is_dynamic() || !rb1.is_sleeping())
                     && (!rb2.is_dynamic() || !rb2.is_sleeping())