Outsource the contact manifold, SAT, and some shapes.

48 files changed, 477 insertions, 2842 deletions

diff --git a/src/dynamics/rigid_body_set.rs b/src/dynamics/rigid_body_set.rs
index b8667bc..c32c402 100644
--- a/src/dynamics/rigid_body_set.rs
+++ b/src/dynamics/rigid_body_set.rs
@@ -479,7 +479,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.num_active_contacts > 0 {
                                 let other = crate::utils::other_handle(
                                     (inter.0, inter.1),
                                     *collider_handle,
diff --git a/src/dynamics/solver/categorization.rs b/src/dynamics/solver/categorization.rs
index 9ddf791..d0a2d0f 100644
--- a/src/dynamics/solver/categorization.rs
+++ b/src/dynamics/solver/categorization.rs
@@ -12,8 +12,8 @@ pub(crate) fn categorize_position_contacts(
 ) {
     for manifold_i in manifold_indices {
         let manifold = &manifolds[*manifold_i];
-        let rb1 = &bodies[manifold.body_pair.body1];
-        let rb2 = &bodies[manifold.body_pair.body2];
+        let rb1 = &bodies[manifold.data.body_pair.body1];
+        let rb2 = &bodies[manifold.data.body_pair.body2];
 
         if !rb1.is_dynamic() || !rb2.is_dynamic() {
             match manifold.kinematics.category {
@@ -38,8 +38,8 @@ pub(crate) fn categorize_velocity_contacts(
 ) {
     for manifold_i in manifold_indices {
         let manifold = &manifolds[*manifold_i];
-        let rb1 = &bodies[manifold.body_pair.body1];
-        let rb2 = &bodies[manifold.body_pair.body2];
+        let rb1 = &bodies[manifold.data.body_pair.body1];
+        let rb2 = &bodies[manifold.data.body_pair.body2];
 
         if !rb1.is_dynamic() || !rb2.is_dynamic() {
             out_ground.push(*manifold_i)
diff --git a/src/dynamics/solver/interaction_groups.rs b/src/dynamics/solver/interaction_groups.rs
index 04acaaf..18e846b 100644
--- a/src/dynamics/solver/interaction_groups.rs
+++ b/src/dynamics/solver/interaction_groups.rs
@@ -12,7 +12,7 @@ pub(crate) trait PairInteraction {
 
 impl<'a> PairInteraction for &'a mut ContactManifold {
     fn body_pair(&self) -> BodyPair {
-        self.body_pair
+        self.data.body_pair
     }
 }
 
@@ -77,7 +77,7 @@ impl ParallelInteractionGroups {
             .iter()
             .zip(self.interaction_colors.iter_mut())
         {
-            let body_pair = interactions[*interaction_id].body_pair();
+            let body_pair = interactions[*interaction_id].data.body_pair();
             let rb1 = &bodies[body_pair.body1];
             let rb2 = &bodies[body_pair.body2];
 
@@ -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].num_active_contacts)
             .max()
             .unwrap_or(1);
 
@@ -351,12 +351,12 @@ impl InteractionGroups {
 
                 // FIXME: how could we avoid iterating
                 // on each interaction at every iteration on k?
-                if interaction.num_active_contacts() != k {
+                if interaction.num_active_contacts != k {
                     continue;
                 }
 
-                let body1 = &bodies[interaction.body_pair.body1];
-                let body2 = &bodies[interaction.body_pair.body2];
+                let body1 = &bodies[interaction.data.body_pair.body1];
+                let body2 = &bodies[interaction.data.body_pair.body2];
                 let is_static1 = !body1.is_dynamic();
                 let is_static2 = !body2.is_dynamic();
 
diff --git a/src/dynamics/solver/parallel_velocity_solver.rs b/src/dynamics/solver/parallel_velocity_solver.rs
index 303a18f..451dd32 100644
--- a/src/dynamics/solver/parallel_velocity_solver.rs
+++ b/src/dynamics/solver/parallel_velocity_solver.rs
@@ -247,13 +247,13 @@ impl ParallelVelocitySolverPart<AnyJointVelocityConstraint> {
                     VelocityConstraintDesc::NongroundGrouped(joint_id) => {
                         let joints = array![|ii| &joints_all[joint_id[ii]].weight; SIMD_WIDTH];
                         let constraint = AnyJointVelocityConstraint::from_wide_joint(params, *joint_id, joints, bodies);
-                        self.constraints[joints[0].constraint_index] = constraint;
+                        self.constraints[joints[0].data.constraint_index] = constraint;
                     }
                     #[cfg(feature = "simd-is-enabled")]
                     VelocityConstraintDesc::GroundGrouped(joint_id) => {
                         let joints = array![|ii| &joints_all[joint_id[ii]].weight; SIMD_WIDTH];
                         let constraint = AnyJointVelocityConstraint::from_wide_joint_ground(params, *joint_id, joints, bodies);
-                        self.constraints[joints[0].constraint_index] = constraint;
+                        self.constraints[joints[0].data.constraint_index] = constraint;
                     }
                 }
             }
diff --git a/src/dynamics/solver/position_constraint.rs b/src/dynamics/solver/position_constraint.rs
index 69fcf57..b329e39 100644
--- a/src/dynamics/solver/position_constraint.rs
+++ b/src/dynamics/solver/position_constraint.rs
@@ -88,8 +88,8 @@ impl PositionConstraint {
         out_constraints: &mut Vec<AnyPositionConstraint>,
         push: bool,
     ) {
-        let rb1 = &bodies[manifold.body_pair.body1];
-        let rb2 = &bodies[manifold.body_pair.body2];
+        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 =
@@ -104,8 +104,8 @@ impl PositionConstraint {
             let mut local_p2 = [Point::origin(); MAX_MANIFOLD_POINTS];
 
             for l in 0..manifold_points.len() {
-                local_p1[l] = manifold.delta1 * (manifold_points[l].local_p1 + shift1);
-                local_p2[l] = manifold.delta2 * (manifold_points[l].local_p2 + shift2);
+                local_p1[l] = manifold.data.delta1 * (manifold_points[l].local_p1 + shift1);
+                local_p2[l] = manifold.data.delta2 * (manifold_points[l].local_p2 + shift2);
             }
 
             let constraint = PositionConstraint {
@@ -132,10 +132,10 @@ impl PositionConstraint {
                 }
             } else {
                 if manifold.kinematics.category == KinematicsCategory::PointPoint {
-                    out_constraints[manifold.constraint_index + l] =
+                    out_constraints[manifold.data.constraint_index + l] =
                         AnyPositionConstraint::NongroupedPointPoint(constraint);
                 } else {
-                    out_constraints[manifold.constraint_index + l] =
+                    out_constraints[manifold.data.constraint_index + l] =
                         AnyPositionConstraint::NongroupedPlanePoint(constraint);
                 }
             }
diff --git a/src/dynamics/solver/position_constraint_wide.rs b/src/dynamics/solver/position_constraint_wide.rs
index 0633926..ecc0fc3 100644
--- a/src/dynamics/solver/position_constraint_wide.rs
+++ b/src/dynamics/solver/position_constraint_wide.rs
@@ -35,8 +35,8 @@ impl WPositionConstraint {
         out_constraints: &mut Vec<AnyPositionConstraint>,
         push: bool,
     ) {
-        let rbs1 = array![|ii| bodies.get(manifolds[ii].body_pair.body1).unwrap(); SIMD_WIDTH];
-        let rbs2 = array![|ii| bodies.get(manifolds[ii].body_pair.body2).unwrap(); SIMD_WIDTH];
+        let rbs1 = array![|ii| bodies.get(manifolds[ii].data.body_pair.body1).unwrap(); SIMD_WIDTH];
+        let rbs2 = array![|ii| bodies.get(manifolds[ii].data.body_pair.body2).unwrap(); SIMD_WIDTH];
 
         let im1 = SimdFloat::from(array![|ii| rbs1[ii].mass_properties.inv_mass; SIMD_WIDTH]);
         let sqrt_ii1: AngularInertia<SimdFloat> =
@@ -51,8 +51,8 @@ impl WPositionConstraint {
         let radius1 = SimdFloat::from(array![|ii| manifolds[ii].kinematics.radius1; SIMD_WIDTH]);
         let radius2 = SimdFloat::from(array![|ii| manifolds[ii].kinematics.radius2; SIMD_WIDTH]);
 
-        let delta1 = Isometry::from(array![|ii| manifolds[ii].delta1; SIMD_WIDTH]);
-        let delta2 = Isometry::from(array![|ii| manifolds[ii].delta2; 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 rb1 = array![|ii| rbs1[ii].active_set_offset; SIMD_WIDTH];
         let rb2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH];
@@ -100,10 +100,10 @@ impl WPositionConstraint {
                 }
             } else {
                 if manifolds[0].kinematics.category == KinematicsCategory::PointPoint {
-                    out_constraints[manifolds[0].constraint_index + l / MAX_MANIFOLD_POINTS] =
+                    out_constraints[manifolds[0].data.constraint_index + l / MAX_MANIFOLD_POINTS] =
                         AnyPositionConstraint::GroupedPointPoint(constraint);
                 } else {
-                    out_constraints[manifolds[0].constraint_index + l / MAX_MANIFOLD_POINTS] =
+                    out_constraints[manifolds[0].data.constraint_index + l / MAX_MANIFOLD_POINTS] =
                         AnyPositionConstraint::GroupedPlanePoint(constraint);
                 }
             }
diff --git a/src/dynamics/solver/position_ground_constraint.rs b/src/dynamics/solver/position_ground_constraint.rs
index dcd2d64..7a15f3e 100644
--- a/src/dynamics/solver/position_ground_constraint.rs
+++ b/src/dynamics/solver/position_ground_constraint.rs
@@ -28,8 +28,8 @@ impl PositionGroundConstraint {
         out_constraints: &mut Vec<AnyPositionConstraint>,
         push: bool,
     ) {
-        let mut rb1 = &bodies[manifold.body_pair.body1];
-        let mut rb2 = &bodies[manifold.body_pair.body2];
+        let mut rb1 = &bodies[manifold.data.body_pair.body1];
+        let mut rb2 = &bodies[manifold.data.body_pair.body2];
         let flip = !rb2.is_dynamic();
 
         let local_n1;
@@ -41,13 +41,13 @@ impl PositionGroundConstraint {
             std::mem::swap(&mut rb1, &mut rb2);
             local_n1 = manifold.local_n2;
             local_n2 = manifold.local_n1;
-            delta1 = &manifold.delta2;
-            delta2 = &manifold.delta1;
+            delta1 = &manifold.data.delta2;
+            delta2 = &manifold.data.delta1;
         } else {
             local_n1 = manifold.local_n1;
             local_n2 = manifold.local_n2;
-            delta1 = &manifold.delta1;
-            delta2 = &manifold.delta2;
+            delta1 = &manifold.data.delta1;
+            delta2 = &manifold.data.delta2;
         };
 
         let coll_pos1 = rb1.position * delta1;
@@ -105,10 +105,10 @@ impl PositionGroundConstraint {
                 }
             } else {
                 if manifold.kinematics.category == KinematicsCategory::PointPoint {
-                    out_constraints[manifold.constraint_index + l] =
+                    out_constraints[manifold.data.constraint_index + l] =
                         AnyPositionConstraint::NongroupedPointPointGround(constraint);
                 } else {
-                    out_constraints[manifold.constraint_index + l] =
+                    out_constraints[manifold.data.constraint_index + l] =
                         AnyPositionConstraint::NongroupedPlanePointGround(constraint);
                 }
             }
diff --git a/src/dynamics/solver/position_ground_constraint_wide.rs b/src/dynamics/solver/position_ground_constraint_wide.rs
index e423c0a..1609709 100644
--- a/src/dynamics/solver/position_ground_constraint_wide.rs
+++ b/src/dynamics/solver/position_ground_constraint_wide.rs
@@ -32,8 +32,10 @@ impl WPositionGroundConstraint {
         out_constraints: &mut Vec<AnyPositionConstraint>,
         push: bool,
     ) {
-        let mut rbs1 = array![|ii| bodies.get(manifolds[ii].body_pair.body1).unwrap(); SIMD_WIDTH];
-        let mut rbs2 = array![|ii| bodies.get(manifolds[ii].body_pair.body2).unwrap(); SIMD_WIDTH];
+        let mut rbs1 =
+            array![|ii| bodies.get(manifolds[ii].data.body_pair.body1).unwrap(); SIMD_WIDTH];
+        let mut rbs2 =
+            array![|ii| bodies.get(manifolds[ii].data.body_pair.body2).unwrap(); SIMD_WIDTH];
         let mut flipped = [false; SIMD_WIDTH];
 
         for ii in 0..SIMD_WIDTH {
@@ -55,10 +57,10 @@ impl WPositionGroundConstraint {
         );
 
         let delta1 = Isometry::from(
-            array![|ii| if flipped[ii] { manifolds[ii].delta2 } else { manifolds[ii].delta1 }; SIMD_WIDTH],
+            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].delta1 } else { manifolds[ii].delta2 }; SIMD_WIDTH],
+            array![|ii| if flipped[ii] { manifolds[ii].data.delta1 } else { manifolds[ii].data.delta2 }; SIMD_WIDTH],
         );
 
         let radius1 = SimdFloat::from(array![|ii| manifolds[ii].kinematics.radius1; SIMD_WIDTH]);
@@ -112,10 +114,10 @@ impl WPositionGroundConstraint {
                 }
             } else {
                 if manifolds[0].kinematics.category == KinematicsCategory::PointPoint {
-                    out_constraints[manifolds[0].constraint_index + l / MAX_MANIFOLD_POINTS] =
+                    out_constraints[manifolds[0].data.constraint_index + l / MAX_MANIFOLD_POINTS] =
                         AnyPositionConstraint::GroupedPointPointGround(constraint);
                 } else {
-                    out_constraints[manifolds[0].constraint_index + l / MAX_MANIFOLD_POINTS] =
+                    out_constraints[manifolds[0].data.constraint_index + l / MAX_MANIFOLD_POINTS] =
                         AnyPositionConstraint::GroupedPlanePointGround(constraint);
                 }
             }
diff --git a/src/dynamics/solver/velocity_constraint.rs b/src/dynamics/solver/velocity_constraint.rs
index 948081d..1bc4c0e 100644
--- a/src/dynamics/solver/velocity_constraint.rs
+++ b/src/dynamics/solver/velocity_constraint.rs
@@ -144,14 +144,14 @@ impl VelocityConstraint {
         out_constraints: &mut Vec<AnyVelocityConstraint>,
         push: bool,
     ) {
-        let rb1 = &bodies[manifold.body_pair.body1];
-        let rb2 = &bodies[manifold.body_pair.body2];
+        let rb1 = &bodies[manifold.data.body_pair.body1];
+        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.delta1;
-        let pos_coll2 = rb2.position * manifold.delta2;
+        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 warmstart_coeff = manifold.warmstart_multiplier * params.warmstart_coeff;
+        let warmstart_coeff = manifold.data.warmstart_multiplier * params.warmstart_coeff;
 
         for (l, manifold_points) in manifold
             .active_contacts()
@@ -164,7 +164,7 @@ impl VelocityConstraint {
                 elements: [VelocityConstraintElement::zero(); MAX_MANIFOLD_POINTS],
                 im1: rb1.mass_properties.inv_mass,
                 im2: rb2.mass_properties.inv_mass,
-                limit: manifold.friction,
+                limit: manifold.data.friction,
                 mj_lambda1,
                 mj_lambda2,
                 manifold_id,
@@ -207,7 +207,7 @@ impl VelocityConstraint {
                 constraint.dir1 = force_dir1;
                 constraint.im1 = rb1.mass_properties.inv_mass;
                 constraint.im2 = rb2.mass_properties.inv_mass;
-                constraint.limit = manifold.friction;
+                constraint.limit = manifold.data.friction;
                 constraint.mj_lambda1 = mj_lambda1;
                 constraint.mj_lambda2 = mj_lambda2;
                 constraint.manifold_id = manifold_id;
@@ -241,12 +241,12 @@ impl VelocityConstraint {
                     let mut rhs = (vel1 - vel2).dot(&force_dir1);
 
                     if rhs <= -params.restitution_velocity_threshold {
-                        rhs += manifold.restitution * rhs
+                        rhs += manifold.data.restitution * rhs
                     }
 
                     rhs += manifold_point.dist.max(0.0) * params.inv_dt();
 
-                    let impulse = manifold_points[k].impulse * warmstart_coeff;
+                    let impulse = manifold_points[k].data.impulse * warmstart_coeff;
 
                     constraint.elements[k].normal_part = VelocityConstraintElementPart {
                         gcross1,
@@ -275,9 +275,9 @@ impl VelocityConstraint {
                                 + gcross2.gdot(gcross2));
                         let rhs = (vel1 - vel2).dot(&tangents1[j]);
                         #[cfg(feature = "dim2")]
-                        let impulse = manifold_points[k].tangent_impulse * warmstart_coeff;
+                        let impulse = manifold_points[k].data.tangent_impulse * warmstart_coeff;
                         #[cfg(feature = "dim3")]
-                        let impulse = manifold_points[k].tangent_impulse[j] * warmstart_coeff;
+                        let impulse = manifold_points[k].data.tangent_impulse[j] * warmstart_coeff;
 
                         constraint.elements[k].tangent_part[j] = VelocityConstraintElementPart {
                             gcross1,
@@ -294,7 +294,7 @@ impl VelocityConstraint {
             if push {
                 out_constraints.push(AnyVelocityConstraint::Nongrouped(constraint));
             } else {
-                out_constraints[manifold.constraint_index + l] =
+                out_constraints[manifold.data.constraint_index + l] =
                     AnyVelocityConstraint::Nongrouped(constraint);
             }
         }
@@ -388,15 +388,15 @@ impl VelocityConstraint {
 
         for k in 0..self.num_contacts as usize {
             let active_contacts = manifold.active_contacts_mut();
-            active_contacts[k_base + k].impulse = self.elements[k].normal_part.impulse;
+            active_contacts[k_base + k].data.impulse = self.elements[k].normal_part.impulse;
             #[cfg(feature = "dim2")]
             {
-                active_contacts[k_base + k].tangent_impulse =
+                active_contacts[k_base + k].data.tangent_impulse =
                     self.elements[k].tangent_part[0].impulse;
             }
             #[cfg(feature = "dim3")]
             {
-                active_contacts[k_base + k].tangent_impulse = [
+                active_contacts[k_base + k].data.tangent_impulse = [
                     self.elements[k].tangent_part[0].impulse,
                     self.elements[k].tangent_part[1].impulse,
                 ];
diff --git a/src/dynamics/solver/velocity_constraint_wide.rs b/src/dynamics/solver/velocity_constraint_wide.rs
index 5d8078a..a4efb9a 100644
--- a/src/dynamics/solver/velocity_constraint_wide.rs
+++ b/src/dynamics/solver/velocity_constraint_wide.rs
@@ -69,11 +69,11 @@ impl WVelocityConstraint {
         push: bool,
     ) {
         let inv_dt = SimdFloat::splat(params.inv_dt());
-        let rbs1 = array![|ii| &bodies[manifolds[ii].body_pair.body1]; SIMD_WIDTH];
-        let rbs2 = array![|ii| &bodies[manifolds[ii].body_pair.body2]; SIMD_WIDTH];
+        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].delta1; SIMD_WIDTH]);
-        let delta2 = Isometry::from(array![|ii| manifolds[ii].delta2; 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 = SimdFloat::from(array![|ii| rbs1[ii].mass_properties.inv_mass; SIMD_WIDTH]);
         let ii1: AngularInertia<SimdFloat> =
@@ -103,13 +103,13 @@ impl WVelocityConstraint {
         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 = SimdFloat::from(array![|ii| manifolds[ii].friction; SIMD_WIDTH]);
-        let restitution = SimdFloat::from(array![|ii| manifolds[ii].restitution; SIMD_WIDTH]);
+        let friction = SimdFloat::from(array![|ii| manifolds[ii].data.friction; SIMD_WIDTH]);
+        let restitution = SimdFloat::from(array![|ii| manifolds[ii].data.restitution; SIMD_WIDTH]);
         let restitution_velocity_threshold =
             SimdFloat::splat(params.restitution_velocity_threshold);
 
         let warmstart_multiplier =
-            SimdFloat::from(array![|ii| manifolds[ii].warmstart_multiplier; SIMD_WIDTH]);
+            SimdFloat::from(array![|ii| manifolds[ii].data.warmstart_multiplier; SIMD_WIDTH]);
         let warmstart_coeff = warmstart_multiplier * SimdFloat::splat(params.warmstart_coeff);
 
         for l in (0..manifolds[0].num_active_contacts()).step_by(MAX_MANIFOLD_POINTS) {
@@ -140,7 +140,7 @@ impl WVelocityConstraint {
                 let dist = SimdFloat::from(array![|ii| manifold_points[ii][k].dist; SIMD_WIDTH]);
 
                 let impulse =
-                    SimdFloat::from(array![|ii| manifold_points[ii][k].impulse; SIMD_WIDTH]);
+                    SimdFloat::from(array![|ii| manifold_points[ii][k].data.impulse; SIMD_WIDTH]);
 
                 let dp1 = p1 - world_com1;
                 let dp2 = p2 - world_com2;
@@ -176,11 +176,11 @@ impl WVelocityConstraint {
                 for j in 0..DIM - 1 {
                     #[cfg(feature = "dim2")]
                     let impulse = SimdFloat::from(
-                        array![|ii| manifold_points[ii][k].tangent_impulse; SIMD_WIDTH],
+                        array![|ii| manifold_points[ii][k].data.tangent_impulse; SIMD_WIDTH],
                     );
                     #[cfg(feature = "dim3")]
                     let impulse = SimdFloat::from(
-                        array![|ii| manifold_points[ii][k].tangent_impulse[j]; SIMD_WIDTH],
+                        array![|ii| manifold_points[ii][k].data.tangent_impulse[j]; SIMD_WIDTH],
                     );
 
                     let gcross1 = ii1.transform_vector(dp1.gcross(tangents1[j]));
@@ -202,7 +202,7 @@ impl WVelocityConstraint {
             if push {
                 out_constraints.push(AnyVelocityConstraint::Grouped(constraint));
             } else {
-                out_constraints[manifolds[0].constraint_index + l / MAX_MANIFOLD_POINTS] =
+                out_constraints[manifolds[0].data.constraint_index + l / MAX_MANIFOLD_POINTS] =
                     AnyVelocityConstraint::Grouped(constraint);
             }
         }
@@ -342,15 +342,15 @@ impl WVelocityConstraint {
                 let manifold = &mut manifolds_all[self.manifold_id[ii]];
                 let k_base = self.manifold_contact_id;
                 let active_contacts = manifold.active_contacts_mut();
-                active_contacts[k_base + k].impulse = impulses[ii];
+                active_contacts[k_base + k].data.impulse = impulses[ii];
 
                 #[cfg(feature = "dim2")]
                 {
-                    active_contacts[k_base + k].tangent_impulse = tangent_impulses[ii];
+                    active_contacts[k_base + k].data.tangent_impulse = tangent_impulses[ii];
                 }
                 #[cfg(feature = "dim3")]
                 {
-                    active_contacts[k_base + k].tangent_impulse =
+                    active_contacts[k_base + k].data.tangent_impulse =
                         [tangent_impulses[ii], bitangent_impulses[ii]];
                 }
             }
diff --git a/src/dynamics/solver/velocity_ground_constraint.rs b/src/dynamics/solver/velocity_ground_constraint.rs
index d8ef8be..c3a33