Outsource the contact manifold, SAT, and some shapes.

1 files changed, 14 insertions, 14 deletions

diff --git a/src/dynamics/solver/velocity_ground_constraint_wide.rs b/src/dynamics/solver/velocity_ground_constraint_wide.rs
index be01944..84c4182 100644
--- a/src/dynamics/solver/velocity_ground_constraint_wide.rs
+++ b/src/dynamics/solver/velocity_ground_constraint_wide.rs
@@ -65,8 +65,8 @@ impl WVelocityGroundConstraint {
         push: bool,
     ) {
         let inv_dt = SimdFloat::splat(params.inv_dt());
-        let mut rbs1 = array![|ii| &bodies[manifolds[ii].body_pair.body1]; SIMD_WIDTH];
-        let mut rbs2 = array![|ii| &bodies[manifolds[ii].body_pair.body2]; SIMD_WIDTH];
+        let mut rbs1 = array![|ii| &bodies[manifolds[ii].data.body_pair.body1]; SIMD_WIDTH];
+        let mut rbs2 = array![|ii| &bodies[manifolds[ii].data.body_pair.body2]; SIMD_WIDTH];
         let mut flipped = [false; SIMD_WIDTH];
 
         for ii in 0..SIMD_WIDTH {
@@ -90,10 +90,10 @@ impl WVelocityGroundConstraint {
         let pos2 = Isometry::from(array![|ii| rbs2[ii].position; SIMD_WIDTH]);
 
         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 coll_pos1 = pos1 * delta1;
@@ -109,13 +109,13 @@ impl WVelocityGroundConstraint {
 
         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) {
@@ -146,7 +146,7 @@ impl WVelocityGroundConstraint {
                 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;
 
@@ -178,11 +178,11 @@ impl WVelocityGroundConstraint {
                 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 gcross2 = ii2.transform_vector(dp2.gcross(-tangents1[j]));
@@ -202,7 +202,7 @@ impl WVelocityGroundConstraint {
             if push {
                 out_constraints.push(AnyVelocityConstraint::GroupedGround(constraint));
             } else {
-                out_constraints[manifolds[0].constraint_index + l / MAX_MANIFOLD_POINTS] =
+                out_constraints[manifolds[0].data.constraint_index + l / MAX_MANIFOLD_POINTS] =
                     AnyVelocityConstraint::GroupedGround(constraint);
             }
         }
@@ -302,15 +302,15 @@ impl WVelocityGroundConstraint {
                 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]];
                 }
             }