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

8 files changed, 122 insertions, 226 deletions

diff --git a/src/dynamics/solver/position_constraint.rs b/src/dynamics/solver/position_constraint.rs
index b329e39..72dd534 100644
--- a/src/dynamics/solver/position_constraint.rs
+++ b/src/dynamics/solver/position_constraint.rs
@@ -63,9 +63,9 @@ pub(crate) struct PositionConstraint {
     // NOTE: the points are relative to the center of masses.
     pub local_p1: [Point<f32>; MAX_MANIFOLD_POINTS],
     pub local_p2: [Point<f32>; MAX_MANIFOLD_POINTS],
+    pub dists: [f32; MAX_MANIFOLD_POINTS],
     pub local_n1: Vector<f32>,
     pub num_contacts: u8,
-    pub radius: f32,
     pub im1: f32,
     pub im2: f32,
     pub ii1: AngularInertia<f32>,
@@ -90,22 +90,21 @@ impl PositionConstraint {
     ) {
         let rb1 = &bodies[manifold.data.body_pair.body1];
         let rb2 = &bodies[manifold.data.body_pair.body2];
-        let shift1 = manifold.local_n1 * -manifold.kinematics.radius1;
-        let shift2 = manifold.local_n2 * -manifold.kinematics.radius2;
-        let radius =
-            manifold.kinematics.radius1 + manifold.kinematics.radius2 /*- params.allowed_linear_error*/;
-
-        for (l, manifold_points) in manifold
-            .active_contacts()
-            .chunks(MAX_MANIFOLD_POINTS)
-            .enumerate()
-        {
+        let active_contacts = &manifold.data.solver_contacts[..manifold.num_active_contacts];
+
+        for (l, manifold_points) in active_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];
 
             for l in 0..manifold_points.len() {
-                local_p1[l] = manifold.data.delta1 * (manifold_points[l].local_p1 + shift1);
-                local_p2[l] = manifold.data.delta2 * (manifold_points[l].local_p2 + shift2);
+                local_p1[l] = rb1
+                    .position
+                    .inverse_transform_point(&manifold_points[l].point);
+                local_p2[l] = rb2
+                    .position
+                    .inverse_transform_point(&manifold_points[l].point);
+                dists[l] = manifold_points[l].dist;
             }
 
             let constraint = PositionConstraint {
@@ -113,8 +112,8 @@ impl PositionConstraint {
                 rb2: rb2.active_set_offset,
                 local_p1,
                 local_p2,
-                local_n1: manifold.local_n1,
-                radius,
+                local_n1: rb1.position.inverse_transform_vector(&manifold.data.normal),
+                dists,
                 im1: rb1.mass_properties.inv_mass,
                 im2: rb2.mass_properties.inv_mass,
                 ii1: rb1.world_inv_inertia_sqrt.squared(),
@@ -152,9 +151,9 @@ impl PositionConstraint {
         let mut pos1 = positions[self.rb1];
         let mut pos2 = positions[self.rb2];
         let allowed_err = params.allowed_linear_error;
-        let target_dist = self.radius - allowed_err;
 
         for k in 0..self.num_contacts as usize {
+            let target_dist = -self.dists[k] - allowed_err;
             let p1 = pos1 * self.local_p1[k];
             let p2 = pos2 * self.local_p2[k];
             let dpos = p2 - p1;
@@ -204,9 +203,9 @@ impl PositionConstraint {
         let mut pos1 = positions[self.rb1];
         let mut pos2 = positions[self.rb2];
         let allowed_err = params.allowed_linear_error;
-        let target_dist = self.radius - allowed_err;
 
         for k in 0..self.num_contacts as usize {
+            let target_dist = -self.dists[k] - allowed_err;
             let n1 = pos1 * self.local_n1;
             let p1 = pos1 * self.local_p1[k];
             let p2 = pos2 * self.local_p2[k];
diff --git a/src/dynamics/solver/position_constraint_wide.rs b/src/dynamics/solver/position_constraint_wide.rs
index 260d495..89378ab 100644
--- a/src/dynamics/solver/position_constraint_wide.rs
+++ b/src/dynamics/solver/position_constraint_wide.rs
@@ -16,8 +16,8 @@ pub(crate) struct WPositionConstraint {
     // NOTE: the points are relative to the center of masses.
     pub local_p1: [Point<SimdReal>; MAX_MANIFOLD_POINTS],
     pub local_p2: [Point<SimdReal>; MAX_MANIFOLD_POINTS],
+    pub dists: [SimdReal; MAX_MANIFOLD_POINTS],
     pub local_n1: Vector<SimdReal>,
-    pub radius: SimdReal,
     pub im1: SimdReal,
     pub im2: SimdReal,
     pub ii1: AngularInertia<SimdReal>,
@@ -45,22 +45,20 @@ impl WPositionConstraint {
         let sqrt_ii2: AngularInertia<SimdReal> =
             AngularInertia::from(array![|ii| rbs2[ii].world_inv_inertia_sqrt; SIMD_WIDTH]);
 
-        let local_n1 = Vector::from(array![|ii| manifolds[ii].local_n1; SIMD_WIDTH]);
-        let local_n2 = Vector::from(array![|ii| manifolds[ii].local_n2; SIMD_WIDTH]);
+        let pos1 = Isometry::from(array![|ii| rbs1[ii].position; SIMD_WIDTH]);
+        let pos2 = Isometry::from(array![|ii| rbs2[ii].position; SIMD_WIDTH]);
 
-        let radius1 = SimdReal::from(array![|ii| manifolds[ii].kinematics.radius1; SIMD_WIDTH]);
-        let radius2 = SimdReal::from(array![|ii| manifolds[ii].kinematics.radius2; SIMD_WIDTH]);
-
-        let delta1 = Isometry::from(array![|ii| manifolds[ii].data.delta1; SIMD_WIDTH]);
-        let delta2 = Isometry::from(array![|ii| manifolds[ii].data.delta2; SIMD_WIDTH]);
+        let local_n1 = pos1.inverse_transform_vector(&Vector::from(
+            array![|ii| manifolds[ii].local_n1; SIMD_WIDTH],
+        ));
 
         let rb1 = array![|ii| rbs1[ii].active_set_offset; SIMD_WIDTH];
         let rb2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH];
 
-        let radius = radius1 + radius2 /*- SimdReal::splat(params.allowed_linear_error)*/;
+        let num_active_contacts = manifolds[0].num_active_contacts();
 
-        for l in (0..manifolds[0].num_active_contacts()).step_by(MAX_MANIFOLD_POINTS) {
-            let manifold_points = array![|ii| &manifolds[ii].active_contacts()[l..]; SIMD_WIDTH];
+        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 num_points = manifold_points[0].len().min(MAX_MANIFOLD_POINTS);
 
             let mut constraint = WPositionConstraint {
@@ -69,7 +67,7 @@ impl WPositionConstraint {
                 local_p1: [Point::origin(); MAX_MANIFOLD_POINTS],
                 local_p2: [Point::origin(); MAX_MANIFOLD_POINTS],
                 local_n1,
-                radius,
+                dists: [SimdReal::zero(); MAX_MANIFOLD_POINTS],
                 im1,
                 im2,
                 ii1: sqrt_ii1.squared(),
@@ -79,17 +77,12 @@ impl WPositionConstraint {
                 num_contacts: num_points as u8,
             };
 
-            let shift1 = local_n1 * -radius1;
-            let shift2 = local_n2 * -radius2;
-
             for i in 0..num_points {
-                let local_p1 =
-                    Point::from(array![|ii| manifold_points[ii][i].local_p1; SIMD_WIDTH]);
-                let local_p2 =
-                    Point::from(array![|ii| manifold_points[ii][i].local_p2; SIMD_WIDTH]);
-
-                constraint.local_p1[i] = delta1 * (local_p1 + shift1);
-                constraint.local_p2[i] = delta2 * (local_p2 + shift2);
+                let point = Point::from(array![|ii| manifold_points[ii][i].point; SIMD_WIDTH]);
+                let dist = SimdReal::from(array![|ii| manifold_points[ii][i].dist; SIMD_WIDTH]);
+                constraint.local_p1[i] = pos1.inverse_transform_point(&point);
+                constraint.local_p2[i] = pos2.inverse_transform_point(&point);
+                constraint.dists[i] = dist;
             }
 
             if push {
@@ -120,9 +113,9 @@ impl WPositionConstraint {
         let mut pos1 = Isometry::from(array![|ii| positions[self.rb1[ii]]; SIMD_WIDTH]);
         let mut pos2 = Isometry::from(array![|ii| positions[self.rb2[ii]]; SIMD_WIDTH]);
         let allowed_err = SimdReal::splat(params.allowed_linear_error);
-        let target_dist = self.radius - allowed_err;
 
         for k in 0..self.num_contacts as usize {
+            let target_dist = -self.dists[k] - allowed_err;
             let p1 = pos1 * self.local_p1[k];
             let p2 = pos2 * self.local_p2[k];
 
@@ -174,9 +167,9 @@ impl WPositionConstraint {
         let mut pos1 = Isometry::from(array![|ii| positions[self.rb1[ii]]; SIMD_WIDTH]);
         let mut pos2 = Isometry::from(array![|ii| positions[self.rb2[ii]]; SIMD_WIDTH]);
         let allowed_err = SimdReal::splat(params.allowed_linear_error);
-        let target_dist = self.radius - allowed_err;
 
         for k in 0..self.num_contacts as usize {
+            let target_dist = -self.dists[k] - allowed_err;
             let n1 = pos1 * self.local_n1;
             let p1 = pos1 * self.local_p1[k];
             let p2 = pos2 * self.local_p2[k];
diff --git a/src/dynamics/solver/position_ground_constraint.rs b/src/dynamics/solver/position_ground_constraint.rs
index 7a15f3e..ada41e8 100644
--- a/src/dynamics/solver/position_ground_constraint.rs
+++ b/src/dynamics/solver/position_ground_constraint.rs
@@ -11,9 +11,9 @@ pub(crate) struct PositionGroundConstraint {
     // NOTE: the points are relative to the center of masses.
     pub p1: [Point<f32>; MAX_MANIFOLD_POINTS],
     pub local_p2: [Point<f32>; MAX_MANIFOLD_POINTS],
+    pub dists: [f32; MAX_MANIFOLD_POINTS],
     pub n1: Vector<f32>,
     pub num_contacts: u8,
-    pub radius: f32,
     pub im2: f32,
     pub ii2: AngularInertia<f32>,
     pub erp: f32,
@@ -32,52 +32,26 @@ impl PositionGroundConstraint {
         let mut rb2 = &bodies[manifold.data.body_pair.body2];
         let flip = !rb2.is_dynamic();
 
-        let local_n1;
-        let local_n2;
-        let delta1;
-        let delta2;
-
-        if flip {
+        let n1 = if flip {
             std::mem::swap(&mut rb1, &mut rb2);
-            local_n1 = manifold.local_n2;
-            local_n2 = manifold.local_n1;
-            delta1 = &manifold.data.delta2;
-            delta2 = &manifold.data.delta1;
+            -manifold.data.normal
         } else {
-            local_n1 = manifold.local_n1;
-            local_n2 = manifold.local_n2;
-            delta1 = &manifold.data.delta1;
-            delta2 = &manifold.data.delta2;
+            manifold.data.normal
         };
 
-        let coll_pos1 = rb1.position * delta1;
-        let shift1 = local_n1 * -manifold.kinematics.radius1;
-        let shift2 = local_n2 * -manifold.kinematics.radius2;
-        let n1 = coll_pos1 * local_n1;
-        let radius =
-            manifold.kinematics.radius1 + manifold.kinematics.radius2 /* - params.allowed_linear_error */;
-
-        for (l, manifold_contacts) in manifold
-            .active_contacts()
-            .chunks(MAX_MANIFOLD_POINTS)
-            .enumerate()
-        {
+        let active_contacts = &manifold.data.solver_contacts[..manifold.num_active_contacts];
+
+        for (l, manifold_contacts) in active_contacts.chunks(MAX_MANIFOLD_POINTS).enumerate() {
             let mut p1 = [Point::origin(); MAX_MANIFOLD_POINTS];
             let mut local_p2 = [Point::origin(); MAX_MANIFOLD_POINTS];
-
-            if flip {
-                // Don't forget that we already swapped rb1 and rb2 above.
-                // So if we flip, only manifold_contacts[k].{local_p1,local_p2} have to
-                // be swapped.
-                for k in 0..manifold_contacts.len() {
-                    p1[k] = coll_pos1 * (manifold_contacts[k].local_p2 + shift1);
-                    local_p2[k] = delta2 * (manifold_contacts[k].local_p1 + shift2);
-                }
-            } else {
-                for k in 0..manifold_contacts.len() {
-                    p1[k] = coll_pos1 * (manifold_contacts[k].local_p1 + shift1);
-                    local_p2[k] = delta2 * (manifold_contacts[k].local_p2 + shift2);
-                }
+            let mut dists = [0.0; MAX_MANIFOLD_POINTS];
+
+            for k in 0..manifold_contacts.len() {
+                p1[k] = manifold_contacts[k].point;
+                local_p2[k] = rb2
+                    .position
+                    .inverse_transform_point(&manifold_contacts[k].point);
+                dists[k] = manifold_contacts[k].dist;
             }
 
             let constraint = PositionGroundConstraint {
@@ -85,7 +59,7 @@ impl PositionGroundConstraint {
                 p1,
                 local_p2,
                 n1,
-                radius,
+                dists,
                 im2: rb2.mass_properties.inv_mass,
                 ii2: rb2.world_inv_inertia_sqrt.squared(),
                 num_contacts: manifold_contacts.len() as u8,
@@ -123,9 +97,9 @@ impl PositionGroundConstraint {
         // Compute jacobians.
         let mut pos2 = positions[self.rb2];
         let allowed_err = params.allowed_linear_error;
-        let target_dist = self.radius - allowed_err;
 
         for k in 0..self.num_contacts as usize {
+            let target_dist = -self.dists[k] - allowed_err;
             let p1 = self.p1[k];
             let p2 = pos2 * self.local_p2[k];
             let dpos = p2 - p1;
@@ -165,9 +139,9 @@ impl PositionGroundConstraint {
         // Compute jacobians.
         let mut pos2 = positions[self.rb2];
         let allowed_err = params.allowed_linear_error;
-        let target_dist = self.radius - allowed_err;
 
         for k in 0..self.num_contacts as usize {
+            let target_dist = -self.dists[k] - allowed_err;
             let n1 = self.n1;
             let p1 = self.p1[k];
             let p2 = pos2 * self.local_p2[k];
diff --git a/src/dynamics/solver/position_ground_constraint_wide.rs b/src/dynamics/solver/position_ground_constraint_wide.rs
index 2531b2e..40d5a68 100644
--- a/src/dynamics/solver/position_ground_constraint_wide.rs
+++ b/src/dynamics/solver/position_ground_constraint_wide.rs
@@ -15,8 +15,8 @@ pub(crate) struct WPositionGroundConstraint {
     // NOTE: the points are relative to the center of masses.
     pub p1: [Point<SimdReal>; MAX_MANIFOLD_POINTS],
     pub local_p2: [Point<SimdReal>; MAX_MANIFOLD_POINTS],
+    pub dists: [SimdReal; MAX_MANIFOLD_POINTS],
     pub n1: Vector<SimdReal>,
-    pub radius: SimdReal,
     pub im2: SimdReal,
     pub ii2: AngularInertia<SimdReal>,
     pub erp: SimdReal,
@@ -49,34 +49,17 @@ impl WPositionGroundConstraint {
         let sqrt_ii2: AngularInertia<SimdReal> =
             AngularInertia::from(array![|ii| rbs2[ii].world_inv_inertia_sqrt; SIMD_WIDTH]);
 
-        let local_n1 = Vector::from(
-            array![|ii| if flipped[ii] { manifolds[ii].local_n2 } else { manifolds[ii].local_n1 }; SIMD_WIDTH],
-        );
-        let local_n2 = Vector::from(
-            array![|ii| if flipped[ii] { manifolds[ii].local_n1 } else { manifolds[ii].local_n2 }; SIMD_WIDTH],
-        );
-
-        let delta1 = Isometry::from(
-            array![|ii| if flipped[ii] { manifolds[ii].data.delta2 } else { manifolds[ii].data.delta1 }; SIMD_WIDTH],
-        );
-        let delta2 = Isometry::from(
-            array![|ii| if flipped[ii] { manifolds[ii].data.delta1 } else { manifolds[ii].data.delta2 }; SIMD_WIDTH],
+        let n1 = Vector::from(
+            array![|ii| if flipped[ii] { -manifolds[ii].data.normal } else { manifolds[ii].data.normal }; SIMD_WIDTH],
         );
 
-        let radius1 = SimdReal::from(array![|ii| manifolds[ii].kinematics.radius1; SIMD_WIDTH]);
-        let radius2 = SimdReal::from(array![|ii| manifolds[ii].kinematics.radius2; SIMD_WIDTH]);
-
-        let coll_pos1 =
-            delta1 * Isometry::from(array![|ii| rbs1[ii].predicted_position; SIMD_WIDTH]);
-
+        let pos2 = Isometry::from(array![|ii| rbs2[ii].position; SIMD_WIDTH]);
         let rb2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH];
 
-        let radius = radius1 + radius2 /*- SimdReal::splat(params.allowed_linear_error)*/;
-
-        let n1 = coll_pos1 * local_n1;
+        let num_active_contacts = manifolds[0].num_active_contacts();
 
-        for l in (0..manifolds[0].num_active_contacts()).step_by(MAX_MANIFOLD_POINTS) {
-            let manifold_points = array![|ii| &manifolds[ii].active_contacts()[l..]; SIMD_WIDTH];
+        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 num_points = manifold_points[0].len().min(MAX_MANIFOLD_POINTS);
 
             let mut constraint = WPositionGroundConstraint {
@@ -84,7 +67,7 @@ impl WPositionGroundConstraint {
                 p1: [Point::origin(); MAX_MANIFOLD_POINTS],
                 local_p2: [Point::origin(); MAX_MANIFOLD_POINTS],
                 n1,
-                radius,
+                dists: [SimdReal::zero(); MAX_MANIFOLD_POINTS],
                 im2,
                 ii2: sqrt_ii2.squared(),
                 erp: SimdReal::splat(params.erp),
@@ -93,15 +76,11 @@ impl WPositionGroundConstraint {
             };
 
             for i in 0..num_points {
-                let local_p1 = Point::from(
-                    array![|ii| if flipped[ii] { manifold_points[ii][i].local_p2 } else { manifold_points[ii][i].local_p1 }; SIMD_WIDTH],
-                );
-                let local_p2 = Point::from(
-                    array![|ii| if flipped[ii] { manifold_points[ii][i].local_p1 } else { manifold_points[ii][i].local_p2 }; SIMD_WIDTH],
-                );
-
-                constraint.p1[i] = coll_pos1 * local_p1 - n1 * radius1;
-                constraint.local_p2[i] = delta2 * (local_p2 - local_n2 * radius2);
+                let point = Point::from(array![|ii| manifold_points[ii][i].point; SIMD_WIDTH]);
+                let dist = SimdReal::from(array![|ii| manifold_points[ii][i].dist; SIMD_WIDTH]);
+                constraint.p1[i] = point;
+                constraint.local_p2[i] = pos2.inverse_transform_point(&point);
+                constraint.dists[i] = dist;
             }
 
             if push {
@@ -132,9 +111,9 @@ impl WPositionGroundConstraint {
         // Compute jacobians.
         let mut pos2 = Isometry::from(array![|ii| positions[self.rb2[ii]]; SIMD_WIDTH]);
         let allowed_err = SimdReal::splat(params.allowed_linear_error);
-        let target_dist = self.radius - allowed_err;
 
         for k in 0..self.num_contacts as usize {
+            let target_dist = -self.dists[k] - allowed_err;
             let p1 = self.p1[k];
             let p2 = pos2 * self.local_p2[k];
 
@@ -174,9 +153,9 @@ impl WPositionGroundConstraint {
         // Compute jacobians.
         let mut pos2 = Isometry::from(array![|ii| positions[self.rb2[ii]]; SIMD_WIDTH]);
         let allowed_err = SimdReal::splat(params.allowed_linear_error);
-        let target_dist = self.radius - allowed_err;
 
         for k in 0..self.num_contacts as usize {
+            let target_dist = -self.dists[k] - allowed_err;
             let n1 = self.n1;
             let p1 = self.p1[k];
             let p2 = pos2 * self.local_p2[k];
diff --git a/src/dynamics/solver/velocity_constraint.rs b/src/dynamics/solver/velocity_constraint.rs
index 1bc4c0e..4f94fba 100644
--- a/src/dynamics/solver/velocity_constraint.rs
+++ b/src/dynamics/solver/velocity_constraint.rs
@@ -148,23 +148,18 @@ impl VelocityConstraint {
         let rb2 = &bodies[manifold.data.body_pair.body2];
         let mj_lambda1 = rb1.active_set_offset;
         let mj_lambda2 = rb2.active_set_offset;
-        let pos_coll1 = rb1.position * manifold.data.delta1;
-        let pos_coll2 = rb2.position * manifold.data.delta2;
-        let force_dir1 = pos_coll1 * (-manifold.local_n1);
+        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 manifold
-            .active_contacts()
-            .chunks(MAX_MANIFOLD_POINTS)
-            .enumerate()
-        {
+        for (l, manifold_points) in active_contacts.chunks(MAX_MANIFOLD_POINTS).enumerate() {
             #[cfg(not(target_arch = "wasm32"))]
             let mut constraint = VelocityConstraint {
                 dir1: force_dir1,
                 elements: [VelocityConstraintElement::zero(); MAX_MANIFOLD_POINTS],
                 im1: rb1.mass_properties.inv_mass,
                 im2: rb2.mass_properties.inv_mass,
-                limit: manifold.data.friction,
+                limit: 0.0,
                 mj_lambda1,
                 mj_lambda2,
                 manifold_id,
@@ -217,12 +212,13 @@ impl VelocityConstraint {
 
             for k in 0..manifold_points.len() {
                 let manifold_point = &manifold_points[k];
-                let dp1 = (pos_coll1 * manifold_point.local_p1) - rb1.world_com;
-                let dp2 = (pos_coll2 * manifold_point.local_p2) - rb2.world_com;
+                let dp1 = manifold_point.point - rb1.world_com;
+                let dp2 = manifold_point.point - rb2.world_com;
 
                 let vel1 = rb1.linvel + rb1.angvel.gcross(dp1);
                 let vel2 = rb2.linvel + rb2.angvel.gcross(dp2);
 
+                constraint.limit = manifold_point.friction;
                 // Normal part.
                 {
                     let gcross1 = rb1
@@ -241,12 +237,12 @@ impl VelocityConstraint {
                     let mut rhs = (vel1 - vel2).dot(&force_dir1);
 
                     if rhs <= -params.restitution_velocity_threshold {
-                        rhs += manifold.data.restitution * rhs
+                        rhs += manifold_point.restitution * rhs
                     }
 
                     rhs += manifold_point.dist.max(0.0) * params.inv_dt();
 
-                    let impulse = manifold_points[k].data.impulse * warmstart_coeff;
+                    let impulse = manifold_point.data.impulse * warmstart_coeff;
 
                     constraint.elements[k].normal_part = VelocityConstraintElementPart {
                         gcross1,
@@ -275,9 +271,9 @@ impl VelocityConstraint {
                                 + gcross2.gdot(gcross2));
                         let rhs = (vel1 - vel2).dot(&tangents1[j]);
                         #[cfg(feature = "dim2")]
-                        let impulse = manifold_points[k].data.tangent_impulse * warmstart_coeff;
+                        let impulse = manifold_point.data.tangent_impulse * warmstart_coeff;
                         #[cfg(feature = "dim3")]
-                        let impulse = manifold_points[k].data.tangent_impulse[j] * warmstart_coeff;
+                        let impulse = manifold_point.data.tangent_impulse[j] * warmstart_coeff;
 
                         constraint.elements[k].tangent_part[j] = VelocityConstraintElementPart {
                             gcross1,
diff --git a/src/dynamics/solver/velocity_constraint_wide.rs b/src/dynamics/solver/velocity_constraint_wide.rs
index 11e7a9d..fddd9ef 100644
--- a/src/dynamics/solver/velocity_constraint_wide.rs
+++ b/src/dynamics/solver/velocity_constraint_wide.rs
@@ -72,9 +72,6 @@ impl WVelocityConstraint {
         let rbs1 = array![|ii| &bodies[manifolds[ii].data.body_pair.body1]; SIMD_WIDTH];
         let rbs2 = array![|ii| &bodies[manifolds[ii].data.body_pair.body2]; SIMD_WIDTH];
 
-        let delta1 = Isometry::from(array![|ii| manifolds[ii].data.delta1; SIMD_WIDTH]);
-        let delta2 = Isometry::from(array![|ii| manifolds[ii].data.delta2; SIMD_WIDTH]);
-
         let im1 = SimdReal::from(array![|ii| rbs1[ii].mass_properties.inv_mass; SIMD_WIDTH]);
         let ii1: AngularInertia<SimdReal> =
             AngularInertia::from(array![|ii| rbs1[ii].world_inv_inertia_sqrt; SIMD_WIDTH]);
@@ -82,7 +79,6 @@ impl WVelocityConstraint {
         let linvel1 = Vector::from(array![|ii| rbs1[ii].linvel; SIMD_WIDTH]);
         let angvel1 = AngVector::<SimdReal>::from(array![|ii| rbs1[ii].angvel; SIMD_WIDTH]);
 
-        let pos1 = Isometry::from(array![|ii| rbs1[ii].position; SIMD_WIDTH]);
         let world_com1 = Point::from(array![|ii| rbs1[ii].world_com; SIMD_WIDTH]);
 
         let im2 = SimdReal::from(array![|ii| rbs2[ii].mass_properties.inv_mass; SIMD_WIDTH]);
@@ -92,27 +88,24 @@ impl WVelocityConstraint {
         let linvel2 = Vector::from(array![|ii| rbs2[ii].linvel; SIMD_WIDTH]);
         let angvel2 = AngVector::<SimdReal>::from(array![|ii| rbs2[ii].angvel; SIMD_WIDTH]);
 
-        let pos2 = Isometry::from(array![|ii| rbs2[ii].position; SIMD_WIDTH]);
         let world_com2 = Point::from(array![|ii| rbs2[ii].world_com; SIMD_WIDTH]);
 
-        let coll_pos1 = pos1 * delta1;
-        let coll_pos2 = pos2 * delta2;
-
-        let force_dir1 = coll_pos1 * -Vector::from(array![|ii| manifolds[ii].local_n1; SIMD_WIDTH]);
+        let force_dir1 = -Vector::from(array![|ii| manifolds[ii].data.normal; SIMD_WIDTH]);
 
         let mj_lambda1 = array![|ii| rbs1[ii].active_set_offset; SIMD_WIDTH];
         let mj_lambda2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH];
 
-        let friction = SimdReal::from(array![|ii| manifolds[ii].data.friction; SIMD_WIDTH]);
-        let restitution = SimdReal::from(array![|ii| manifolds[ii].data.restitution; SIMD_WIDTH]);
         let restitution_velocity_threshold = SimdReal::splat(params.restitution_velocity_threshold);
 
         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();
 
-        for l in (0..manifolds[0].num_active_contacts()).step_by(MAX_MANIFOLD_POINTS) {
-            let manifold_points = array![|ii| &manifolds[ii].active_contacts()[l..]; SIMD_WIDTH];
+        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 num_points = manifold_points[0].len().min(MAX_MANIFOLD_POINTS);
 
             let mut constraint = WVelocityConstraint {
@@ -120,7 +113,7 @@ impl WVelocityConstraint {
                 elements: [WVelocityConstraintElement::zero(); MAX_MANIFOLD_POINTS],
                 im1,
                 im2,
-                limit: friction,
+                limit: SimdReal::splat(0.0),
                 mj_lambda1,
                 mj_lambda2,
                 manifold_id,
@@ -129,24 +122,24 @@ impl WVelocityConstraint {
             };
 
             for k in 0..num_points {
-                // FIXME: can we avoid the multiplications by coll_pos1/coll_pos2 here?
-                // By working as much as possible in local-space.
-                let p1 = coll_pos1
-                    * Point::from(array![|ii| manifold_points[ii][k].local_p1; SIMD_WIDTH]);
-                let p2 = coll_pos2
-                    * Point::from(array![|ii| manifold_points[ii][k].local_p2; SIMD_WIDTH]);
-
+                let friction =
+                    SimdReal::from(array![|ii| manifold_points[ii][k].friction; SIMD_WIDTH]);
+                let restitution =
+                    SimdReal::from(array![|ii| manifold_points[ii][k].restitution; SIMD_WIDTH]);
+                let point = Point::from(array![|ii| manifold_points[ii][k].point; SIMD_WIDTH]);
                 let dist = SimdReal::from(array![|ii| manifold_points[ii][k].dist; SIMD_WIDTH]);
 
                 let impulse =
                     SimdReal::from(array![|ii| manifold_points[ii][k].data.impulse; SIMD_WIDTH]);
 
-                let dp1 = p1 - world_com1;
-                let dp2 = p2 - world_com2;
+                let dp1 = point - world_com1;
+                let dp2 = point - world_com2;
 
                 let vel1 = linvel1 + angvel1.gcross(dp1);
                 let vel2 = linvel2 + angvel2.gcross(dp2);
 
+                constraint.limit = friction;
+
                 // Normal part.
                 {
                     let gcross1 = ii1.transform_vector(dp1.gcross(force_dir1));
diff --git a/src/dynamics/solver/velocity_ground_constraint.rs b/src/dynamics/solver/velocity_ground_constraint.rs
index c3a3356..cbb6bb8 100644
--- a/src/dynamics/solver/velocity_ground_constraint.rs
+++ b/src/dynamics/solver/velocity_ground_constraint.rs
@@ -66,35 +66,25 @@ impl VelocityGroundConstraint {
         let mut rb1 = &bodies[manifold.data.body_pair.body1];
         let mut rb2 = &bodies[manifold.data.body_pair.body2];
         let flipped = !rb2.is_dynamic();
-        let force_dir1;
-        let coll_pos1;
-        let coll_pos2;
-
-        if flipped {
-            coll_pos1 = rb2.position * manifold.data.delta2;
-            coll_pos2 = rb1.position * manifold.data.delta1;
-            force_dir1 = coll_pos1 * (-manifold.local_n2);
+
+        let force_dir1 = if flipped {
             std::mem::swap(&mut rb1, &mut rb2);
+            manifold.data.normal
         } else {
-            coll_pos1 = rb1.position * manifold.data.delta1;
-            coll_pos2 = rb2.position * manifold.data.delta2;
-            force_dir1 = coll_pos1 * (-manifold.local_n1);
-        }
+            -manifold.data.normal
+        };
 
         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 manifold
-            .active_contacts()
-            .chunks(MAX_MANIFOLD_POINTS)
-            .enumerate()
-        {
+        for (l, manifold_points) in active_contacts.chunks(MAX_MANIFOLD_POINTS).enumerate() {
             #[cfg(not(target_arch = "wasm32"))]
             let mut constraint = VelocityGroundConstraint {
                 dir1: force_dir1,
                 elements: [VelocityGroundConstraintElement::zero(); MAX_MANIFOLD_POINTS],
                 im2: rb2.mass_properties.inv_mass,
-                limit: manifold.data.friction,
+                limit: 0.0,
                 mj_lambda2,
                 manifold_id,
                 manifold_contact_id: l * MAX_MANIFOLD_POINTS,
@@ -144,24 +134,13 @@ impl VelocityGroundConstraint {
 
             for k in 0..manifold_points.len() {
                 let manifold_point = &manifold_points[k];
-                let (p1, p2) = if flipped {
-                    // NOTE: we already swapped rb1 and rb2
-                    // so we multiply by coll_pos1/coll_pos2.
-                    (
-                        coll_pos1 * manifold_point.local_p2,
-                        coll_pos2 * manifold_point.local_p1,
-                    )
-                } else {
-                    (
-                        coll_pos1 * manifold_point.local_p1,
-                        coll_pos2 * manifold_point.local_p2,
-                    )
-                };
-                let dp2 = p2 - rb2.world_com;
-                let dp1 = p1 - rb1.world_com;
+                let dp2 = manifold_point.point - rb2.world_com;
+                let dp1 = manifold_point.point - rb1.world_com;
                 let vel1 = rb1.linvel + rb1.angvel.gcross(dp1);
                 let vel2 = rb2.linvel + rb2.angvel.gcross(dp2);
 
+                constraint.limit = manifold_point.friction;
+
                 // Normal part.
                 {
                     let gcross2 = rb2
@@ -173,7 +152,7 @@ impl VelocityGroundConstraint {
                     let mut rhs = (vel1 - vel2).dot(&force_dir1);
 
                     if rhs <= -params.restitution_velocity_threshold {
-                        rhs += manifold.data.restitution * rhs
+                        rhs += manifold_point.restitution * rhs
                     }
 
                     rhs += manifold_point.dist.max(0.0) * params.inv_dt();
diff --git a/src/dynamics/solver/velocity_ground_constraint_wide.rs b/src/dynamics/solver/velocity_ground_constraint_wide.rs
index ad16efe..3bb7393 100644
--- a/src/dynamics/solver/velocity_ground_constraint_wide.rs
+++ b/src/dynamics/solver/velocity_ground_constraint_wide.rs
@@ -86,46 +86,31 @@ impl WVelocityGroundConstraint {
         let linvel2 = Vector::from(array![|ii| rbs2[ii].linvel; SIMD_WIDTH]);
         let angvel2 = AngVector::<SimdReal>::from(array![|ii| rbs2[ii].angvel; SIMD_WIDTH]);
 
-        let pos1 = Isometry::from(array![|ii| rbs1[ii].position; SIMD_WIDTH]);
-        let pos2 = Isometry::from(array![|ii| rbs2[ii].position; SIMD_WIDTH]);
-
-        let delta1 = Isometry::from(
-            array![|ii| if flipped[ii] { manifolds[ii].data.delta2 } else { manifolds[ii].data.delta1 }; SIMD_WIDTH],
-        );
-        let delta2 = Isometry::from(
-            array![|ii| if flipped[ii] { manifolds[ii].data.delta1 } else { manifolds[ii].data.delta2 }; SIMD_WIDTH],
-        );
-
-        let coll_pos1 = pos1 * delta1;
-        let coll_pos2 = pos2 * delta2;
-
         let world_com1 = Point::from(array![|ii| rbs1[ii].world_com; SIMD_WIDTH]);
         let world_com2 = Point::from(array![|ii| rbs2[ii].world_com; SIMD_WIDTH]);
 
-        let force_dir1 = coll_pos1
-            * -Vector::from(
-                array![|ii| if flipped[ii] { manifolds[ii].local_n2 } else { manifolds[ii].local_n1 }; SIMD_WIDTH],
-            );
+        let force_dir1 = Vector::from(
+            array![|ii| if flipped[ii] { manifolds[ii].data.normal } else { -manifolds[ii].data.normal }; SIMD_WIDTH],
+        );
 
         let mj_lambda2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH];
 
-        let friction = SimdReal::from(array![|ii| manifolds[ii].data.friction; SIMD_WIDTH]);
-        let restitution = SimdReal::from(array![|ii| manifolds[ii].data.restitution; SIMD_WIDTH]);
         let restitution_velocity_threshold = SimdReal::splat(params.restitution_velocity_threshold);
 
         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();
 
-        for l in (0..manifolds[0].num_active_contacts()).step_by(MAX_MANIFOLD_POINTS) {
-            let manifold_points = array![|ii| &manifolds[ii].active_contacts()[l..]; SIMD_WIDTH];
+        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 num_points = manifold_points[0].len().min(MAX_MANIFOLD_POINTS);
 
             let mut constraint = WVelocityGroundConstraint {
                 dir1: force_dir1,
                 elements: [WVelocityGroundConstraintElement::zero(); MAX_MANIFOLD_POINTS],
                 im2,
-                limit: friction,
+                limit: SimdReal::splat(0.0),
                 mj_lambda2,
                 manifold_id,
                 manifold_contact_id: l,
@@ -133,25 +118,23 @@ impl WVelocityGroundConstraint {
             };
 
             for k in 0..num_points {
-                let p1 = coll_pos1
-                    * Point::from(
-                        array![|ii| if flipped[ii] { manifold_points[ii][k].local_p2 } else { manifold_points[ii][k].local_p1 }; SIMD_WIDTH],
-                    );
-                let p2 = coll_pos2
-                    * Point::from(
-                        array![|ii| if flipped[ii] { manifold_points[ii][k].local_p1 } else { manifold_points[ii][k].local_p2 }; SIMD_WIDTH],
-                    );
-
+                let friction =
+                    SimdReal::from(array![|ii| manifold_points[ii][k].friction; SIMD_WIDTH]);
+                let restitution =
+                    SimdReal::from(array![|ii| manifold_points[ii][k].restitution; SIMD_WIDTH]);
+                let point = Point::from(array![|ii| manifold_points[ii][k].point; SIMD_WIDTH]);
                 let dist = SimdReal::from(array![|ii| manifold_points[ii][k].dist; SIMD_WIDTH]);
 
                 le