Perform contact sorting in the narrow-phase directly.

src/dynamics/rigid_body_set.rs

@@ -20,13 +20,9 @@ pub struct BodyPair {
 }
 
 impl BodyPair {
-    pub(crate) fn new(body1: RigidBodyHandle, body2: RigidBodyHandle) -> Self {
+    pub fn new(body1: RigidBodyHandle, body2: RigidBodyHandle) -> Self {
         BodyPair { body1, body2 }
     }
-
-    pub(crate) fn swap(self) -> Self {
-        Self::new(self.body2, self.body1)
-    }
 }
 
 #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
@@ -479,7 +475,7 @@ impl RigidBodySet {
                 if let Some(contacts) = narrow_phase.contacts_with(*collider_handle) {
                     for inter in contacts {
                         for manifold in &inter.2.manifolds {
-                            if manifold.num_active_contacts > 0 {
+                            if !manifold.data.solver_contacts.is_empty() {
                                 let other = crate::utils::other_handle(
                                     (inter.0, inter.1),
                                     *collider_handle,

src/dynamics/solver/interaction_groups.rs

@@ -338,7 +338,7 @@ impl InteractionGroups {
         let mut occupied_mask = 0u128;
         let max_interaction_points = interaction_indices
             .iter()
-            .map(|i| interactions[*i].num_active_contacts)
+            .map(|i| interactions[*i].data.num_active_contacts())
             .max()
             .unwrap_or(1);
 
@@ -351,7 +351,7 @@ impl InteractionGroups {
 
                 // FIXME: how could we avoid iterating
                 // on each interaction at every iteration on k?
-                if interaction.num_active_contacts != k {
+                if interaction.data.num_active_contacts() != k {
                     continue;
                 }
 

src/dynamics/solver/position_constraint.rs

@@ -66,9 +66,13 @@ impl PositionConstraint {
     ) {
         let rb1 = &bodies[manifold.data.body_pair.body1];
         let rb2 = &bodies[manifold.data.body_pair.body2];
-        let active_contacts = &manifold.data.solver_contacts[..manifold.num_active_contacts];
 
-        for (l, manifold_points) in active_contacts.chunks(MAX_MANIFOLD_POINTS).enumerate() {
+        for (l, manifold_points) in manifold
+            .data
+            .solver_contacts
+            .chunks(MAX_MANIFOLD_POINTS)
+            .enumerate()
+        {
             let mut local_p1 = [Point::origin(); MAX_MANIFOLD_POINTS];
             let mut local_p2 = [Point::origin(); MAX_MANIFOLD_POINTS];
             let mut dists = [0.0; MAX_MANIFOLD_POINTS];

src/dynamics/solver/position_constraint_wide.rs

@@ -8,7 +8,7 @@ use crate::math::{
 use crate::utils::{WAngularInertia, WCross, WDot};
 
 use num::Zero;
-use simba::simd::{SimdBool as _, SimdComplexField, SimdPartialOrd, SimdValue};
+use simba::simd::{SimdBool as _, SimdPartialOrd, SimdValue};
 
 pub(crate) struct WPositionConstraint {
     pub rb1: [usize; SIMD_WIDTH],
@@ -55,10 +55,10 @@ impl WPositionConstraint {
         let rb1 = array![|ii| rbs1[ii].active_set_offset; SIMD_WIDTH];
         let rb2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH];
 
-        let num_active_contacts = manifolds[0].num_active_contacts();
+        let num_active_contacts = manifolds[0].data.num_active_contacts();
 
         for l in (0..num_active_contacts).step_by(MAX_MANIFOLD_POINTS) {
-            let manifold_points = array![|ii| &manifolds[ii].data.solver_contacts[l..num_active_contacts]; SIMD_WIDTH];
+            let manifold_points = array![|ii| &manifolds[ii].data.solver_contacts[l..]; SIMD_WIDTH];
             let num_points = manifold_points[0].len().min(MAX_MANIFOLD_POINTS);
 
             let mut constraint = WPositionConstraint {

src/dynamics/solver/position_ground_constraint.rs

@@ -39,9 +39,12 @@ impl PositionGroundConstraint {
             manifold.data.normal
         };
 
-        let active_contacts = &manifold.data.solver_contacts[..manifold.num_active_contacts];
+        for (l, manifold_contacts) in manifold
-
+            .data
-        for (l, manifold_contacts) in active_contacts.chunks(MAX_MANIFOLD_POINTS).enumerate() {
+            .solver_contacts
+            .chunks(MAX_MANIFOLD_POINTS)
+            .enumerate()
+        {
             let mut p1 = [Point::origin(); MAX_MANIFOLD_POINTS];
             let mut local_p2 = [Point::origin(); MAX_MANIFOLD_POINTS];
             let mut dists = [0.0; MAX_MANIFOLD_POINTS];

src/dynamics/solver/position_ground_constraint_wide.rs

@@ -8,7 +8,7 @@ use crate::math::{
 use crate::utils::{WAngularInertia, WCross, WDot};
 
 use num::Zero;
-use simba::simd::{SimdBool as _, SimdComplexField, SimdPartialOrd, SimdValue};
+use simba::simd::{SimdBool as _, SimdPartialOrd, SimdValue};
 
 pub(crate) struct WPositionGroundConstraint {
     pub rb2: [usize; SIMD_WIDTH],
@@ -56,10 +56,10 @@ impl WPositionGroundConstraint {
         let pos2 = Isometry::from(array![|ii| rbs2[ii].position; SIMD_WIDTH]);
         let rb2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH];
 
-        let num_active_contacts = manifolds[0].num_active_contacts();
+        let num_active_contacts = manifolds[0].data.num_active_contacts();
 
         for l in (0..num_active_contacts).step_by(MAX_MANIFOLD_POINTS) {
-            let manifold_points = array![|ii| &manifolds[ii].data.solver_contacts[l..num_active_contacts]; SIMD_WIDTH];
+            let manifold_points = array![|ii| &manifolds[ii].data.solver_contacts[l..]; SIMD_WIDTH];
             let num_points = manifold_points[0].len().min(MAX_MANIFOLD_POINTS);
 
             let mut constraint = WPositionGroundConstraint {

src/dynamics/solver/position_solver.rs

@@ -1,16 +1,6 @@
-use super::{
+use super::AnyJointPositionConstraint;
-    AnyJointPositionConstraint, InteractionGroups, PositionConstraint, PositionGroundConstraint,
+use crate::dynamics::{solver::AnyPositionConstraint, IntegrationParameters, RigidBodySet};
-};
-#[cfg(feature = "simd-is-enabled")]
-use super::{WPositionConstraint, WPositionGroundConstraint};
-use crate::dynamics::solver::categorization::categorize_joints;
-use crate::dynamics::{
-    solver::AnyPositionConstraint, IntegrationParameters, JointGraphEdge, JointIndex, RigidBodySet,
-};
-use crate::geometry::{ContactManifold, ContactManifoldIndex};
 use crate::math::Isometry;
-#[cfg(feature = "simd-is-enabled")]
-use crate::math::SIMD_WIDTH;
 
 pub(crate) struct PositionSolver {
     positions: Vec<Isometry<f32>>,

src/dynamics/solver/solver_constraints.rs

@@ -9,13 +9,11 @@ use crate::dynamics::solver::{
     PositionGroundConstraint, WPositionConstraint, WPositionGroundConstraint,
 };
 use crate::dynamics::{
-    solver::{AnyVelocityConstraint, DeltaVel},
+    solver::AnyVelocityConstraint, IntegrationParameters, JointGraphEdge, JointIndex, RigidBodySet,
-    IntegrationParameters, JointGraphEdge, JointIndex, RigidBodySet,
 };
 use crate::geometry::{ContactManifold, ContactManifoldIndex};
 #[cfg(feature = "simd-is-enabled")]
 use crate::math::SIMD_WIDTH;
-use crate::utils::WAngularInertia;
 
 pub(crate) struct SolverConstraints<VelocityConstraint, PositionConstraint> {
     pub not_ground_interactions: Vec<usize>,

src/dynamics/solver/velocity_constraint.rs

@@ -150,9 +150,13 @@ impl VelocityConstraint {
         let mj_lambda2 = rb2.active_set_offset;
         let force_dir1 = -manifold.data.normal;
         let warmstart_coeff = manifold.data.warmstart_multiplier * params.warmstart_coeff;
-        let active_contacts = &manifold.data.solver_contacts[..manifold.num_active_contacts];
 
-        for (l, manifold_points) in active_contacts.chunks(MAX_MANIFOLD_POINTS).enumerate() {
+        for (l, manifold_points) in manifold
+            .data
+            .solver_contacts
+            .chunks(MAX_MANIFOLD_POINTS)
+            .enumerate()
+        {
             #[cfg(not(target_arch = "wasm32"))]
             let mut constraint = VelocityConstraint {
                 dir1: force_dir1,
@@ -383,7 +387,7 @@ impl VelocityConstraint {
         let k_base = self.manifold_contact_id;
 
         for k in 0..self.num_contacts as usize {
-            let active_contacts = manifold.active_contacts_mut();
+            let active_contacts = &mut manifold.points[..manifold.data.num_active_contacts()];
             active_contacts[k_base + k].data.impulse = self.elements[k].normal_part.impulse;
             #[cfg(feature = "dim2")]
             {

src/dynamics/solver/velocity_constraint_wide.rs

@@ -2,8 +2,7 @@ use super::{AnyVelocityConstraint, DeltaVel};
 use crate::dynamics::{IntegrationParameters, RigidBodySet};
 use crate::geometry::{ContactManifold, ContactManifoldIndex};
 use crate::math::{
-    AngVector, AngularInertia, Isometry, Point, SimdReal, Vector, DIM, MAX_MANIFOLD_POINTS,
+    AngVector, AngularInertia, Point, SimdReal, Vector, DIM, MAX_MANIFOLD_POINTS, SIMD_WIDTH,
-    SIMD_WIDTH,
 };
 use crate::utils::{WAngularInertia, WBasis, WCross, WDot};
 use num::Zero;
@@ -100,7 +99,7 @@ impl WVelocityConstraint {
         let warmstart_multiplier =
             SimdReal::from(array![|ii| manifolds[ii].data.warmstart_multiplier; SIMD_WIDTH]);
         let warmstart_coeff = warmstart_multiplier * SimdReal::splat(params.warmstart_coeff);
-        let num_active_contacts = manifolds[0].num_active_contacts();
+        let num_active_contacts = manifolds[0].data.num_active_contacts();
 
         for l in (0..num_active_contacts).step_by(MAX_MANIFOLD_POINTS) {
             let manifold_points = array![|ii|
@@ -333,7 +332,7 @@ impl WVelocityConstraint {
             for ii in 0..SIMD_WIDTH {
                 let manifold = &mut manifolds_all[self.manifold_id[ii]];
                 let k_base = self.manifold_contact_id;
-                let active_contacts = manifold.active_contacts_mut();
+                let active_contacts = &mut manifold.points[..manifold.data.num_active_contacts()];
                 active_contacts[k_base + k].data.impulse = impulses[ii];
 
                 #[cfg(feature = "dim2")]

src/dynamics/solver/velocity_ground_constraint.rs

@@ -76,9 +76,13 @@ impl VelocityGroundConstraint {
 
         let mj_lambda2 = rb2.active_set_offset;
         let warmstart_coeff = manifold.data.warmstart_multiplier * params.warmstart_coeff;
-        let active_contacts = &manifold.data.solver_contacts[..manifold.num_active_contacts];
 
-        for (l, manifold_points) in active_contacts.chunks(MAX_MANIFOLD_POINTS).enumerate() {
+        for (l, manifold_points) in manifold
+            .data
+            .solver_contacts
+            .chunks(MAX_MANIFOLD_POINTS)
+            .enumerate()
+        {
             #[cfg(not(target_arch = "wasm32"))]
             let mut constraint = VelocityGroundConstraint {
                 dir1: force_dir1,
@@ -268,7 +272,7 @@ impl VelocityGroundConstraint {
         let k_base = self.manifold_contact_id;
 
         for k in 0..self.num_contacts as usize {
-            let active_contacts = manifold.active_contacts_mut();
+            let active_contacts = &mut manifold.points[..manifold.data.num_active_contacts()];
             active_contacts[k_base + k].data.impulse = self.elements[k].normal_part.impulse;
             #[cfg(feature = "dim2")]
             {

src/dynamics/solver/velocity_ground_constraint_wide.rs

@@ -2,8 +2,7 @@ use super::{AnyVelocityConstraint, DeltaVel};
 use crate::dynamics::{IntegrationParameters, RigidBodySet};
 use crate::geometry::{ContactManifold, ContactManifoldIndex};
 use crate::math::{
-    AngVector, AngularInertia, Isometry, Point, SimdReal, Vector, DIM, MAX_MANIFOLD_POINTS,
+    AngVector, AngularInertia, Point, SimdReal, Vector, DIM, MAX_MANIFOLD_POINTS, SIMD_WIDTH,
-    SIMD_WIDTH,
 };
 use crate::utils::{WAngularInertia, WBasis, WCross, WDot};
 use num::Zero;
@@ -100,10 +99,10 @@ impl WVelocityGroundConstraint {
         let warmstart_multiplier =
             SimdReal::from(array![|ii| manifolds[ii].data.warmstart_multiplier; SIMD_WIDTH]);
         let warmstart_coeff = warmstart_multiplier * SimdReal::splat(params.warmstart_coeff);
-        let num_active_contacts = manifolds[0].num_active_contacts();
+        let num_active_contacts = manifolds[0].data.num_active_contacts();
 
         for l in (0..num_active_contacts).step_by(MAX_MANIFOLD_POINTS) {
-            let manifold_points = array![|ii| &manifolds[ii].data.solver_contacts[l..num_active_contacts]; SIMD_WIDTH];
+            let manifold_points = array![|ii| &manifolds[ii].data.solver_contacts[l..]; SIMD_WIDTH];
             let num_points = manifold_points[0].len().min(MAX_MANIFOLD_POINTS);
 
             let mut constraint = WVelocityGroundConstraint {
@@ -283,7 +282,7 @@ impl WVelocityGroundConstraint {
             for ii in 0..SIMD_WIDTH {
                 let manifold = &mut manifolds_all[self.manifold_id[ii]];
                 let k_base = self.manifold_contact_id;
-                let active_contacts = manifold.active_contacts_mut();
+                let active_contacts = &mut manifold.points[..manifold.data.num_active_contacts()];
                 active_contacts[k_base + k].data.impulse = impulses[ii];
 
                 #[cfg(feature = "dim2")]

src/dynamics/solver/velocity_solver.rs

@@ -1,20 +1,9 @@
-use super::{
+use super::AnyJointVelocityConstraint;
-    AnyJointVelocityConstraint, InteractionGroups, VelocityConstraint, VelocityGroundConstraint,
-};
-#[cfg(feature = "simd-is-enabled")]
-use super::{WVelocityConstraint, WVelocityGroundConstraint};
-use crate::dynamics::solver::categorization::{categorize_contacts, categorize_joints};
-use crate::dynamics::solver::{
-    AnyJointPositionConstraint, AnyPositionConstraint, PositionConstraint,
-    PositionGroundConstraint, WPositionConstraint, WPositionGroundConstraint,
-};
 use crate::dynamics::{
     solver::{AnyVelocityConstraint, DeltaVel},
-    IntegrationParameters, JointGraphEdge, JointIndex, RigidBodySet,
+    IntegrationParameters, JointGraphEdge, RigidBodySet,
 };
-use crate::geometry::{ContactManifold, ContactManifoldIndex};
+use crate::geometry::ContactManifold;
-#[cfg(feature = "simd-is-enabled")]
-use crate::math::SIMD_WIDTH;
 use crate::utils::WAngularInertia;
 
 pub(crate) struct VelocitySolver {

src/geometry/contact_pair.rs

@@ -1,13 +1,7 @@
-use crate::dynamics::{BodyPair, RigidBodyHandle, RigidBodySet};
+use crate::dynamics::{BodyPair, RigidBodySet};
-use crate::geometry::{Collider, ColliderPair, ColliderSet, Contact, ContactManifold};
+use crate::geometry::{Collider, ColliderPair, ContactManifold};
-use crate::math::{Isometry, Point, Real, Vector};
+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(
+    #[inline]
-        &mut self,
+    pub fn num_active_contacts(&self) -> usize {
-        coll1: &Collider,
+        self.solver_contacts.len()
-        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)
     }
 
     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) {
+    // pub(crate) fn update_warmstart_multiplier(manifold: &mut ContactManifold) {
-        // In 2D, tall stacks will actually suffer from this
+    //     // In 2D, tall stacks will actually suffer from this
-        // because oscillation due to inaccuracies in 2D often
+    //     // because oscillation due to inaccuracies in 2D often
-        // cause contacts to break, which would result in
+    //     // cause contacts to break, which would result in
-        // a reset of the warmstart multiplier.
+    //     // a reset of the warmstart multiplier.
-        if cfg!(feature = "dim2") {
+    //     if cfg!(feature = "dim2") {
-            manifold.data.warmstart_multiplier = 1.0;
+    //         manifold.data.warmstart_multiplier = 1.0;
-            return;
+    //         return;
-        }
+    //     }
-
+    //
-        for pt in &manifold.points {
+    //     for pt in &manifold.points {
-            if pt.data.impulse != 0.0 {
+    //         if pt.data.impulse != 0.0 {
-                manifold.data.warmstart_multiplier =
+    //             manifold.data.warmstart_multiplier =
-                    (manifold.data.warmstart_multiplier * 2.0).min(1.0);
+    //                 (manifold.data.warmstart_multiplier * 2.0).min(1.0);
-                return;
+    //             return;
-            }
+    //         }
-        }
+    //     }
-
+    //
-        // Reset the multiplier.
+    //     // Reset the multiplier.
-        manifold.data.warmstart_multiplier = Self::min_warmstart_multiplier()
+    //     manifold.data.warmstart_multiplier = Self::min_warmstart_multiplier()
-    }
+    // }
 }

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;

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] {
+                // Sort contacts and generate solver contacts.
-                    let solver_contact = SolverContact {
+                let mut first_inactive_index = manifold.points.len();
-                        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);
+                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;
+                    }
+
+                    // 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())

src/pipeline/collision_pipeline.rs

@@ -64,13 +64,7 @@ impl CollisionPipeline {
             contact_pair_filter,
             events,
         );
-        narrow_phase.compute_intersections(
+        narrow_phase.compute_intersections(bodies, colliders, proximity_pair_filter, events);
-            prediction_distance,
-            bodies,
-            colliders,
-            proximity_pair_filter,
-            events,
-        );
 
         bodies.update_active_set_with_contacts(
             colliders,

src/pipeline/physics_pipeline.rs

@@ -118,13 +118,7 @@ impl PhysicsPipeline {
             contact_pair_filter,
             events,
         );
-        narrow_phase.compute_intersections(
+        narrow_phase.compute_intersections(bodies, colliders, proximity_pair_filter, events);
-            integration_parameters.prediction_distance,
-            bodies,
-            colliders,
-            proximity_pair_filter,
-            events,
-        );
         //        println!("Compute contact time: {}", instant::now() - t);
 
         self.counters.stages.island_construction_time.start();

src/pipeline/query_pipeline.rs

@@ -2,8 +2,6 @@ use crate::dynamics::RigidBodySet;
 use crate::geometry::{
     Collider, ColliderHandle, ColliderSet, InteractionGroups, Ray, RayIntersection, SimdQuadTree,
 };
-use cdl::query::TOI;
-use cdl::shape::Shape;
 
 /// A pipeline for performing queries on all the colliders of a scene.
 #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]

src/utils.rs

@@ -1,17 +1,12 @@
 //! Miscellaneous utilities.
 
 use crate::dynamics::RigidBodyHandle;
-use na::{Matrix2, Matrix3, Matrix3x2, Point2, Point3, Scalar, SimdRealField, Vector2, Vector3};
+use na::{Matrix3, Point2, Point3, Scalar, SimdRealField, Vector2, Vector3};
 use num::Zero;
 use simba::simd::SimdValue;
 
 use cdl::utils::SdpMatrix3;
-use std::ops::{Add, Mul};
+use {crate::math::SimdReal, na::SimdPartialOrd, num::One};
-use {
-    crate::math::{AngularInertia, SimdBool, SimdReal},
-    na::SimdPartialOrd,
-    num::One,
-};
 
 pub(crate) fn inv(val: f32) -> f32 {
     if val == 0.0 {

src_testbed/testbed.rs

@@ -1515,7 +1515,7 @@ Hashes at frame: {}
 fn draw_contacts(window: &mut Window, nf: &NarrowPhase, colliders: &ColliderSet) {
     for pair in nf.contact_pairs() {
         for manifold in &pair.manifolds {
-            for pt in manifold.all_contacts() {
+            for pt in manifold.contacts() {
                 let color = if pt.dist > 0.0 {
                     Point3::new(0.0, 0.0, 1.0)
                 } else {