Split rigid-bodies and colliders into multiple components

1 files changed, 19 insertions, 14 deletions

diff --git a/src/dynamics/solver/position_ground_constraint.rs b/src/dynamics/solver/position_ground_constraint.rs
index e1a4016..53df7f8 100644
--- a/src/dynamics/solver/position_ground_constraint.rs
+++ b/src/dynamics/solver/position_ground_constraint.rs
@@ -1,5 +1,6 @@
 use super::AnyPositionConstraint;
-use crate::dynamics::{IntegrationParameters, RigidBodySet};
+use crate::data::{BundleSet, ComponentSet};
+use crate::dynamics::{IntegrationParameters, RigidBodyIds, RigidBodyMassProps, RigidBodyPosition};
 use crate::geometry::ContactManifold;
 use crate::math::{
     AngularInertia, Isometry, Point, Real, Rotation, Translation, Vector, MAX_MANIFOLD_POINTS,
@@ -21,24 +22,28 @@ pub(crate) struct PositionGroundConstraint {
 }
 
 impl PositionGroundConstraint {
-    pub fn generate(
+    pub fn generate<Bodies>(
         params: &IntegrationParameters,
         manifold: &ContactManifold,
-        bodies: &RigidBodySet,
+        bodies: &Bodies,
         out_constraints: &mut Vec<AnyPositionConstraint>,
         push: bool,
-    ) {
-        let mut rb1 = &bodies[manifold.data.body_pair.body1];
-        let mut rb2 = &bodies[manifold.data.body_pair.body2];
+    ) where
+        Bodies: ComponentSet<RigidBodyPosition>
+            + ComponentSet<RigidBodyMassProps>
+            + ComponentSet<RigidBodyIds>,
+    {
         let flip = manifold.data.relative_dominance < 0;
 
-        let n1 = if flip {
-            std::mem::swap(&mut rb1, &mut rb2);
-            -manifold.data.normal
+        let (handle2, n1) = if flip {
+            (manifold.data.rigid_body1.unwrap(), -manifold.data.normal)
         } else {
-            manifold.data.normal
+            (manifold.data.rigid_body2.unwrap(), manifold.data.normal)
         };
 
+        let (ids2, poss2, mprops2): (&RigidBodyIds, &RigidBodyPosition, &RigidBodyMassProps) =
+            bodies.index_bundle(handle2.0);
+
         for (l, manifold_contacts) in manifold
             .data
             .solver_contacts
@@ -51,20 +56,20 @@ impl PositionGroundConstraint {
 
             for k in 0..manifold_contacts.len() {
                 p1[k] = manifold_contacts[k].point;
-                local_p2[k] = rb2
+                local_p2[k] = poss2
                     .position
                     .inverse_transform_point(&manifold_contacts[k].point);
                 dists[k] = manifold_contacts[k].dist;
             }
 
             let constraint = PositionGroundConstraint {
-                rb2: rb2.active_set_offset,
+                rb2: ids2.active_set_offset,
                 p1,
                 local_p2,
                 n1,
                 dists,
-                im2: rb2.effective_inv_mass,
-                ii2: rb2.effective_world_inv_inertia_sqrt.squared(),
+                im2: mprops2.effective_inv_mass,
+                ii2: mprops2.effective_world_inv_inertia_sqrt.squared(),
                 num_contacts: manifold_contacts.len() as u8,
                 erp: params.erp,
                 max_linear_correction: params.max_linear_correction,