Perform contact sorting in the narrow-phase directly.

12 files changed, 47 insertions, 57 deletions

diff --git a/src/dynamics/solver/interaction_groups.rs b/src/dynamics/solver/interaction_groups.rs
index 18e846b..b5f8173 100644
--- a/src/dynamics/solver/interaction_groups.rs
+++ b/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;
                 }
 
diff --git a/src/dynamics/solver/position_constraint.rs b/src/dynamics/solver/position_constraint.rs
index 1ef0660..843fd1a 100644
--- a/src/dynamics/solver/position_constraint.rs
+++ b/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];
diff --git a/src/dynamics/solver/position_constraint_wide.rs b/src/dynamics/solver/position_constraint_wide.rs
index 3a3abb0..7df7d5e 100644
--- a/src/dynamics/solver/position_constraint_wide.rs
+++ b/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 {
diff --git a/src/dynamics/solver/position_ground_constraint.rs b/src/dynamics/solver/position_ground_constraint.rs
index 80175fe..fcfdcdb 100644
--- a/src/dynamics/solver/position_ground_constraint.rs
+++ b/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 active_contacts.chunks(MAX_MANIFOLD_POINTS).enumerate() {
+        for (l, manifold_contacts) in manifold
+            .data
+            .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];
diff --git a/src/dynamics/solver/position_ground_constraint_wide.rs b/src/dynamics/solver/position_ground_constraint_wide.rs
index 76c78d8..dbea1db 100644
--- a/src/dynamics/solver/position_ground_constraint_wide.rs
+++ b/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 {
diff --git a/src/dynamics/solver/position_solver.rs b/src/dynamics/solver/position_solver.rs
index b5a953f..cf23168 100644
--- a/src/dynamics/solver/position_solver.rs
+++ b/src/dynamics/solver/position_solver.rs
@@ -1,16 +1,6 @@
-use super::{
-    AnyJointPositionConstraint, InteractionGroups, PositionConstraint, PositionGroundConstraint,
-};
-#[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 super::AnyJointPositionConstraint;
+use crate::dynamics::{solver::AnyPositionConstraint, IntegrationParameters, RigidBodySet};
 use crate::math::Isometry;
-#[cfg(feature = "simd-is-enabled")]
-use crate::math::SIMD_WIDTH;
 
 pub(crate) struct PositionSolver {
     positions: Vec<Isometry<f32>>,
diff --git a/src/dynamics/solver/solver_constraints.rs b/src/dynamics/solver/solver_constraints.rs
index 88371ba..92faa3b 100644
--- a/src/dynamics/solver/solver_constraints.rs
+++ b/src/dynamics/solver/solver_constraints.rs
@@ -9,13 +9,11 @@ use crate::dynamics::solver::{
     PositionGroundConstraint, WPositionConstraint, WPositionGroundConstraint,
 };
 use crate::dynamics::{
-    solver::{AnyVelocityConstraint, DeltaVel},
-    IntegrationParameters, JointGraphEdge, JointIndex, RigidBodySet,
+    solver::AnyVelocityConstraint, 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>,
diff --git a/src/dynamics/solver/velocity_constraint.rs b/src/dynamics/solver/velocity_constraint.rs
index 4f94fba..6614b48 100644
--- a/src/dynamics/solver/velocity_constraint.rs
+++ b/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")]
             {
diff --git a/src/dynamics/solver/velocity_constraint_wide.rs b/src/dynamics/solver/velocity_constraint_wide.rs
index fddd9ef..c75d926 100644
--- a/src/dynamics/solver/velocity_constraint_wide.rs
+++ b/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,
-    SIMD_WIDTH,
+    AngVector, AngularInertia, Point, SimdReal, Vector, DIM, MAX_MANIFOLD_POINTS, 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")]
diff --git a/src/dynamics/solver/velocity_ground_constraint.rs b/src/dynamics/solver/velocity_ground_constraint.rs
index cbb6bb8..fa2a23e 100644
--- a/src/dynamics/solver/velocity_ground_constraint.rs
+++ b/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")]
             {
diff --git a/src/dynamics/solver/velocity_ground_constraint_wide.rs b/src/dynamics/solver/velocity_ground_constraint_wide.rs
index 3bb7393..feee344 100644
--- a/src/dynamics/solver/velocity_ground_constraint_wide.rs
+++ b/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,
-    SIMD_WIDTH,
+    AngVector, AngularInertia, Point, SimdReal, Vector, DIM, MAX_MANIFOLD_POINTS, 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")]
diff --git a/src/dynamics/solver/velocity_solver.rs b/src/dynamics/solver/velocity_solver.rs
index 89f2809..332d809 100644
--- a/src/dynamics/solver/velocity_solver.rs
+++ b/src/dynamics/solver/velocity_solver.rs
@@ -1,20 +1,9 @@
-use super::{
-    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 super::AnyJointVelocityConstraint;
 use crate::dynamics::{
     solver::{AnyVelocityConstraint, DeltaVel},
-    IntegrationParameters, JointGraphEdge, JointIndex, RigidBodySet,
+    IntegrationParameters, JointGraphEdge, RigidBodySet,
 };
-use crate::geometry::{ContactManifold, ContactManifoldIndex};
-#[cfg(feature = "simd-is-enabled")]
-use crate::math::SIMD_WIDTH;
+use crate::geometry::ContactManifold;
 use crate::utils::WAngularInertia;
 
 pub(crate) struct VelocitySolver {