Merge pull request #183 from dimforge/bundles

Make Rapier accept any kind of data storage instead of RigidBodySet/ColliderSet

38 files changed, 2096 insertions, 1375 deletions

diff --git a/src/dynamics/solver/categorization.rs b/src/dynamics/solver/categorization.rs
index e3327c6..366a5b3 100644
--- a/src/dynamics/solver/categorization.rs
+++ b/src/dynamics/solver/categorization.rs
@@ -1,8 +1,9 @@
-use crate::dynamics::{JointGraphEdge, JointIndex, RigidBodySet};
+use crate::data::ComponentSet;
+use crate::dynamics::{JointGraphEdge, JointIndex, RigidBodyType};
 use crate::geometry::{ContactManifold, ContactManifoldIndex};
 
 pub(crate) fn categorize_contacts(
-    _bodies: &RigidBodySet, // Unused but useful to simplify the parallel code.
+    _bodies: &impl ComponentSet<RigidBodyType>, // Unused but useful to simplify the parallel code.
     manifolds: &[&mut ContactManifold],
     manifold_indices: &[ContactManifoldIndex],
     out_ground: &mut Vec<ContactManifoldIndex>,
@@ -20,7 +21,7 @@ pub(crate) fn categorize_contacts(
 }
 
 pub(crate) fn categorize_joints(
-    bodies: &RigidBodySet,
+    bodies: &impl ComponentSet<RigidBodyType>,
     joints: &[JointGraphEdge],
     joint_indices: &[JointIndex],
     ground_joints: &mut Vec<JointIndex>,
@@ -28,10 +29,10 @@ pub(crate) fn categorize_joints(
 ) {
     for joint_i in joint_indices {
         let joint = &joints[*joint_i].weight;
-        let rb1 = &bodies[joint.body1];
-        let rb2 = &bodies[joint.body2];
+        let status1 = bodies.index(joint.body1.0);
+        let status2 = bodies.index(joint.body2.0);
 
-        if !rb1.is_dynamic() || !rb2.is_dynamic() {
+        if !status1.is_dynamic() || !status2.is_dynamic() {
             ground_joints.push(*joint_i);
         } else {
             nonground_joints.push(*joint_i);
diff --git a/src/dynamics/solver/interaction_groups.rs b/src/dynamics/solver/interaction_groups.rs
index 6b8de5a..c6baea1 100644
--- a/src/dynamics/solver/interaction_groups.rs
+++ b/src/dynamics/solver/interaction_groups.rs
@@ -1,24 +1,32 @@
-use crate::dynamics::{BodyPair, JointGraphEdge, JointIndex, RigidBodySet};
+use crate::data::ComponentSet;
+#[cfg(feature = "parallel")]
+use crate::dynamics::RigidBodyHandle;
+use crate::dynamics::{IslandManager, JointGraphEdge, JointIndex, RigidBodyIds};
 use crate::geometry::{ContactManifold, ContactManifoldIndex};
 #[cfg(feature = "simd-is-enabled")]
 use {
+    crate::data::BundleSet,
     crate::math::{SIMD_LAST_INDEX, SIMD_WIDTH},
     vec_map::VecMap,
 };
-
+#[cfg(feature = "parallel")]
 pub(crate) trait PairInteraction {
-    fn body_pair(&self) -> BodyPair;
+    fn body_pair(&self) -> (Option<RigidBodyHandle>, Option<RigidBodyHandle>);
 }
+#[cfg(any(feature = "parallel", feature = "simd-is-enabled"))]
+use crate::dynamics::RigidBodyType;
 
+#[cfg(feature = "parallel")]
 impl<'a> PairInteraction for &'a mut ContactManifold {
-    fn body_pair(&self) -> BodyPair {
-        self.data.body_pair
+    fn body_pair(&self) -> (Option<RigidBodyHandle>, Option<RigidBodyHandle>) {
+        (self.data.rigid_body1, self.data.rigid_body2)
     }
 }
 
+#[cfg(feature = "parallel")]
 impl<'a> PairInteraction for JointGraphEdge {
-    fn body_pair(&self) -> BodyPair {
-        BodyPair::new(self.weight.body1, self.weight.body2)
+    fn body_pair(&self) -> (Option<RigidBodyHandle>, Option<RigidBodyHandle>) {
+        (Some(self.weight.body1), Some(self.weight.body2))
     }
 }
 
@@ -51,14 +59,17 @@ impl ParallelInteractionGroups {
         self.groups.len() - 1
     }
 
-    pub fn group_interactions<Interaction: PairInteraction>(
+    pub fn group_interactions<Bodies, Interaction: PairInteraction>(
         &mut self,
         island_id: usize,
-        bodies: &RigidBodySet,
+        islands: &IslandManager,
+        bodies: &Bodies,
         interactions: &[Interaction],
         interaction_indices: &[usize],
-    ) {
-        let num_island_bodies = bodies.active_island(island_id).len();
+    ) where
+        Bodies: ComponentSet<RigidBodyIds> + ComponentSet<RigidBodyType>,
+    {
+        let num_island_bodies = islands.active_island(island_id).len();
         self.bodies_color.clear();
         self.interaction_indices.clear();
         self.groups.clear();
@@ -78,29 +89,39 @@ impl ParallelInteractionGroups {
             .zip(self.interaction_colors.iter_mut())
         {
             let body_pair = interactions[*interaction_id].body_pair();
-            let rb1 = &bodies[body_pair.body1];
-            let rb2 = &bodies[body_pair.body2];
-
-            match (rb1.is_static(), rb2.is_static()) {
+            let is_static1 = body_pair
+                .0
+                .map(|b| ComponentSet::<RigidBodyType>::index(bodies, b.0).is_static())
+                .unwrap_or(true);
+            let is_static2 = body_pair
+                .1
+                .map(|b| ComponentSet::<RigidBodyType>::index(bodies, b.0).is_static())
+                .unwrap_or(true);
+
+            match (is_static1, is_static2) {
                 (false, false) => {
+                    let rb_ids1: &RigidBodyIds = bodies.index(body_pair.0.unwrap().0);
+                    let rb_ids2: &RigidBodyIds = bodies.index(body_pair.1.unwrap().0);
                     let color_mask =
-                        bcolors[rb1.active_set_offset] | bcolors[rb2.active_set_offset];
+                        bcolors[rb_ids1.active_set_offset] | bcolors[rb_ids2.active_set_offset];
                     *color = (!color_mask).trailing_zeros() as usize;
                     color_len[*color] += 1;
-                    bcolors[rb1.active_set_offset] |= 1 << *color;
-                    bcolors[rb2.active_set_offset] |= 1 << *color;
+                    bcolors[rb_ids1.active_set_offset] |= 1 << *color;
+                    bcolors[rb_ids2.active_set_offset] |= 1 << *color;
                 }
                 (true, false) => {
-                    let color_mask = bcolors[rb2.active_set_offset];
+                    let rb_ids2: &RigidBodyIds = bodies.index(body_pair.1.unwrap().0);
+                    let color_mask = bcolors[rb_ids2.active_set_offset];
                     *color = (!color_mask).trailing_zeros() as usize;
                     color_len[*color] += 1;
-                    bcolors[rb2.active_set_offset] |= 1 << *color;
+                    bcolors[rb_ids2.active_set_offset] |= 1 << *color;
                 }
                 (false, true) => {
-                    let color_mask = bcolors[rb1.active_set_offset];
+                    let rb_ids1: &RigidBodyIds = bodies.index(body_pair.0.unwrap().0);
+                    let color_mask = bcolors[rb_ids1.active_set_offset];
                     *color = (!color_mask).trailing_zeros() as usize;
                     color_len[*color] += 1;
-                    bcolors[rb1.active_set_offset] |= 1 << *color;
+                    bcolors[rb_ids1.active_set_offset] |= 1 << *color;
                 }
                 (true, true) => unreachable!(),
             }
@@ -168,14 +189,15 @@ impl InteractionGroups {
         self.nongrouped_interactions.clear();
     }
 
-    // FIXME: there is a lot of duplicated code with group_manifolds here.
+    // TODO: there is a lot of duplicated code with group_manifolds here.
     // But we don't refactor just now because we may end up with distinct
     // grouping strategies in the future.
     #[cfg(not(feature = "simd-is-enabled"))]
     pub fn group_joints(
         &mut self,
         _island_id: usize,
-        _bodies: &RigidBodySet,
+        _islands: &IslandManager,
+        _bodies: &impl ComponentSet<RigidBodyIds>,
         _interactions: &[JointGraphEdge],
         interaction_indices: &[JointIndex],
     ) {
@@ -184,13 +206,16 @@ impl InteractionGroups {
     }
 
     #[cfg(feature = "simd-is-enabled")]
-    pub fn group_joints(
+    pub fn group_joints<Bodies>(
         &mut self,
         island_id: usize,
-        bodies: &RigidBodySet,
+        islands: &IslandManager,
+        bodies: &Bodies,
         interactions: &[JointGraphEdge],
         interaction_indices: &[JointIndex],
-    ) {
+    ) where
+        Bodies: ComponentSet<RigidBodyType> + ComponentSet<RigidBodyIds>,
+    {
         // NOTE: in 3D we have up to 10 different joint types.
         // In 2D we only have 5 joint types.
         #[cfg(feature = "dim3")]
@@ -204,11 +229,11 @@ impl InteractionGroups {
 
         // Note: each bit of a body mask indicates what bucket already contains
         // a constraints involving this body.
-        // FIXME: currently, this is a bit overconservative because when a bucket
+        // TODO: currently, this is a bit overconservative because when a bucket
         // is full, we don't clear the corresponding body mask bit. This may result
         // in less grouped constraints.
         self.body_masks
-            .resize(bodies.active_island(island_id).len(), 0u128);
+            .resize(islands.active_island(island_id).len(), 0u128);
 
         // NOTE: each bit of the occupied mask indicates what bucket already
         // contains at least one constraint.
@@ -216,10 +241,14 @@ impl InteractionGroups {
 
         for interaction_i in interaction_indices {
             let interaction = &interactions[*interaction_i].weight;
-            let body1 = &bodies[interaction.body1];
-            let body2 = &bodies[interaction.body2];
-            let is_static1 = !body1.is_dynamic();
-            let is_static2 = !body2.is_dynamic();
+
+            let (status1, ids1): (&RigidBodyType, &RigidBodyIds) =
+                bodies.index_bundle(interaction.body1.0);
+            let (status2, ids2): (&RigidBodyType, &RigidBodyIds) =
+                bodies.index_bundle(interaction.body2.0);
+
+            let is_static1 = !status1.is_dynamic();
+            let is_static2 = !status2.is_dynamic();
 
             if is_static1 && is_static2 {
                 continue;
@@ -232,8 +261,8 @@ impl InteractionGroups {
             }
 
             let ijoint = interaction.params.type_id();
-            let i1 = body1.active_set_offset;
-            let i2 = body2.active_set_offset;
+            let i1 = ids1.active_set_offset;
+            let i2 = ids2.active_set_offset;
             let conflicts =
                 self.body_masks[i1] | self.body_masks[i2] | joint_type_conflicts[ijoint];
             let conflictfree_targets = !(conflicts & occupied_mask); // The & is because we consider empty buckets as free of conflicts.
@@ -325,7 +354,8 @@ impl InteractionGroups {
     pub fn group_manifolds(
         &mut self,
         _island_id: usize,
-        _bodies: &RigidBodySet,
+        _islands: &IslandManager,
+        _bodies: &impl ComponentSet<RigidBodyIds>,
         _interactions: &[&mut ContactManifold],
         interaction_indices: &[ContactManifoldIndex],
     ) {
@@ -334,21 +364,24 @@ impl InteractionGroups {
     }
 
     #[cfg(feature = "simd-is-enabled")]
-    pub fn group_manifolds(
+    pub fn group_manifolds<Bodies>(
         &mut self,
         island_id: usize,
-        bodies: &RigidBodySet,
+        islands: &IslandManager,
+        bodies: &Bodies,
         interactions: &[&mut ContactManifold],
         interaction_indices: &[ContactManifoldIndex],
-    ) {
+    ) where
+        Bodies: ComponentSet<RigidBodyType> + ComponentSet<RigidBodyIds>,
+    {
         // Note: each bit of a body mask indicates what bucket already contains
         // a constraints involving this body.
-        // FIXME: currently, this is a bit overconservative because when a bucket
+        // TODO: currently, this is a bit overconservative because when a bucket
         // is full, we don't clear the corresponding body mask bit. This may result
         // in less grouped contacts.
         // NOTE: body_masks and buckets are already cleared/zeroed at the end of each sort loop.
         self.body_masks
-            .resize(bodies.active_island(island_id).len(), 0u128);
+            .resize(islands.active_island(island_id).len(), 0u128);
 
         // NOTE: each bit of the occupied mask indicates what bucket already
         // contains at least one constraint.
@@ -359,31 +392,44 @@ impl InteractionGroups {
             .max()
             .unwrap_or(1);
 
-        // FIXME: find a way to reduce the number of iteration.
+        // TODO: find a way to reduce the number of iteration.
         // There must be a way to iterate just once on every interaction indices
         // instead of MAX_MANIFOLD_POINTS times.
         for k in 1..=max_interaction_points {
             for interaction_i in interaction_indices {
                 let interaction = &interactions[*interaction_i];
 
-                // FIXME: how could we avoid iterating
+                // TODO: how could we avoid iterating
                 // on each interaction at every iteration on k?
                 if interaction.data.num_active_contacts() != k {
                     continue;
                 }
 
-                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();
+                let (status1, active_set_offset1) = if let Some(rb1) = interaction.data.rigid_body1
+                {
+                    let data: (_, &RigidBodyIds) = bodies.index_bundle(rb1.0);
+                    (*data.0, data.1.active_set_offset)
+                } else {
+                    (RigidBodyType::Static, 0)
+                };
+                let (status2, active_set_offset2) = if let Some(rb2) = interaction.data.rigid_body2
+                {
+                    let data: (_, &RigidBodyIds) = bodies.index_bundle(rb2.0);
+                    (*data.0, data.1.active_set_offset)
+                } else {
+                    (RigidBodyType::Static, 0)
+                };
+
+                let is_static1 = !status1.is_dynamic();
+                let is_static2 = !status2.is_dynamic();
 
-                // FIXME: don't generate interactions between static bodies in the first place.
+                // TODO: don't generate interactions between static bodies in the first place.
                 if is_static1 && is_static2 {
                     continue;
                 }
 
-                let i1 = body1.active_set_offset;
-                let i2 = body2.active_set_offset;
+                let i1 = active_set_offset1;
+                let i2 = active_set_offset2;
                 let conflicts = self.body_masks[i1] | self.body_masks[i2];
                 let conflictfree_targets = !(conflicts & occupied_mask); // The & is because we consider empty buckets as free of conflicts.
                 let conflictfree_occupied_targets = conflictfree_targets & occupied_mask;
diff --git a/src/dynamics/solver/island_solver.rs b/src/dynamics/solver/island_solver.rs
index c684cc5..1e65341 100644
--- a/src/dynamics/solver/island_solver.rs
+++ b/src/dynamics/solver/island_solver.rs
@@ -1,10 +1,15 @@
 use super::{PositionSolver, VelocitySolver};
 use crate::counters::Counters;
+use crate::data::{BundleSet, ComponentSet, ComponentSetMut};
 use crate::dynamics::solver::{
     AnyJointPositionConstraint, AnyJointVelocityConstraint, AnyPositionConstraint,
     AnyVelocityConstraint, SolverConstraints,
 };
-use crate::dynamics::{IntegrationParameters, JointGraphEdge, JointIndex, RigidBodySet};
+use crate::dynamics::{
+    IntegrationParameters, JointGraphEdge, JointIndex, RigidBodyDamping, RigidBodyForces,
+    RigidBodyIds, RigidBodyMassProps, RigidBodyPosition, RigidBodyType,
+};
+use crate::dynamics::{IslandManager, RigidBodyVelocity};
 use crate::geometry::{ContactManifold, ContactManifoldIndex};
 
 pub struct IslandSolver {
@@ -24,17 +29,21 @@ impl IslandSolver {
         }
     }
 
-    pub fn solve_position_constraints(
+    pub fn solve_position_constraints<Bodies>(
         &mut self,
         island_id: usize,
+        islands: &IslandManager,
         counters: &mut Counters,
         params: &IntegrationParameters,
-        bodies: &mut RigidBodySet,
-    ) {
+        bodies: &mut Bodies,
+    ) where
+        Bodies: ComponentSet<RigidBodyIds> + ComponentSetMut<RigidBodyPosition>,
+    {
         counters.solver.position_resolution_time.resume();
         self.position_solver.solve(
             island_id,
             params,
+            islands,
             bodies,
             &self.contact_constraints.position_constraints,
             &self.joint_constraints.position_constraints,
@@ -42,31 +51,47 @@ impl IslandSolver {
         counters.solver.position_resolution_time.pause();
     }
 
-    pub fn init_constraints_and_solve_velocity_constraints(
+    pub fn init_constraints_and_solve_velocity_constraints<Bodies>(
         &mut self,
         island_id: usize,
         counters: &mut Counters,
         params: &IntegrationParameters,
-        bodies: &mut RigidBodySet,
+        islands: &IslandManager,
+        bodies: &mut Bodies,
         manifolds: &mut [&mut ContactManifold],
         manifold_indices: &[ContactManifoldIndex],
         joints: &mut [JointGraphEdge],
         joint_indices: &[JointIndex],
-    ) {
+    ) where
+        Bodies: ComponentSet<RigidBodyForces>
+            + ComponentSetMut<RigidBodyPosition>
+            + ComponentSetMut<RigidBodyVelocity>
+            + ComponentSet<RigidBodyMassProps>
+            + ComponentSet<RigidBodyDamping>
+            + ComponentSet<RigidBodyIds>
+            + ComponentSet<RigidBodyType>,
+    {
         let has_constraints = manifold_indices.len() != 0 || joint_indices.len() != 0;
 
         if has_constraints {
             counters.solver.velocity_assembly_time.resume();
-            self.contact_constraints
-                .init(island_id, params, bodies, manifolds, manifold_indices);
+            self.contact_constraints.init(
+                island_id,
+                params,
+                islands,
+                bodies,
+                manifolds,
+                manifold_indices,
+            );
             self.joint_constraints
-                .init(island_id, params, bodies, joints, joint_indices);
+                .init(island_id, params, islands, bodies, joints, joint_indices);
             counters.solver.velocity_assembly_time.pause();
 
             counters.solver.velocity_resolution_time.resume();
             self.velocity_solver.solve(
                 island_id,
                 params,
+                islands,
                 bodies,
                 manifolds,
                 joints,
@@ -76,21 +101,50 @@ impl IslandSolver {
             counters.solver.velocity_resolution_time.pause();
 
             counters.solver.velocity_update_time.resume();
-            bodies.foreach_active_island_body_mut_internal(island_id, |_, rb| {
-                rb.apply_damping(params.dt);
-                rb.integrate_next_position(params.dt);
-            });
+
+            for handle in islands.active_island(island_id) {
+                let (poss, vels, damping, mprops): (
+                    &RigidBodyPosition,
+                    &RigidBodyVelocity,
+                    &RigidBodyDamping,
+                    &RigidBodyMassProps,
+                ) = bodies.index_bundle(handle.0);
+
+                let mut new_poss = *poss;
+                let new_vels = vels.apply_damping(params.dt, damping);
+                new_poss.next_position =
+                    vels.integrate(params.dt, &poss.position, &mprops.mass_properties.local_com);
+
+                bodies.set_internal(handle.0, new_vels);
+                bodies.set_internal(handle.0, new_poss);
+            }
+
             counters.solver.velocity_update_time.pause();
         } else {
             self.contact_constraints.clear();
             self.joint_constraints.clear();
             counters.solver.velocity_update_time.resume();
-            bodies.foreach_active_island_body_mut_internal(island_id, |_, rb| {
+
+            for handle in islands.active_island(island_id) {
                 // Since we didn't run the velocity solver we need to integrate the accelerations here
-                rb.integrate_accelerations(params.dt);
-                rb.apply_damping(params.dt);
-                rb.integrate_next_position(params.dt);
-            });
+                let (poss, vels, forces, damping, mprops): (
+                    &RigidBodyPosition,
+                    &RigidBodyVelocity,
+                    &RigidBodyForces,
+                    &RigidBodyDamping,
+                    &RigidBodyMassProps,
+                ) = bodies.index_bundle(handle.0);
+
+                let mut new_poss = *poss;
+                let new_vels = forces
+                    .integrate(params.dt, vels, mprops)
+                    .apply_damping(params.dt, &damping);
+                new_poss.next_position =
+                    vels.integrate(params.dt, &poss.position, &mprops.mass_properties.local_com);
+
+                bodies.set_internal(handle.0, new_vels);
+                bodies.set_internal(handle.0, new_poss);
+            }
             counters.solver.velocity_update_time.pause();
         }
     }
diff --git a/src/dynamics/solver/joint_constraint/ball_position_constraint.rs b/src/dynamics/solver/joint_constraint/ball_position_constraint.rs
index 93996f4..159cc77 100644
--- a/src/dynamics/solver/joint_constraint/ball_position_constraint.rs
+++ b/src/dynamics/solver/joint_constraint/ball_position_constraint.rs
@@ -1,4 +1,6 @@
-use crate::dynamics::{BallJoint, IntegrationParameters, RigidBody};
+use crate::dynamics::{
+    BallJoint, IntegrationParameters, RigidBodyIds, RigidBodyMassProps, RigidBodyPosition,
+};
 #[cfg(feature = "dim2")]
 use crate::math::SdpMatrix;
 use crate::math::{AngularInertia, Isometry, Point, Real, Rotation};
@@ -23,18 +25,25 @@ pub(crate) struct BallPositionConstraint {
 }
 
 impl BallPositionConstraint {
-    pub fn from_params(rb1: &RigidBody, rb2: &RigidBody, cparams: &BallJoint) -> Self {
+    pub fn from_params(
+        rb1: (&RigidBodyMassProps, &RigidBodyIds),
+        rb2: (&RigidBodyMassProps, &RigidBodyIds),
+        cparams: &BallJoint,
+    ) -> Self {
+        let (mprops1, ids1) = rb1;
+        let (mprops2, ids2) = rb2;
+
         Self {
-            local_com1: rb1.mass_properties.local_com,
-            local_com2: rb2.mass_properties.local_com,
-            im1: rb1.effective_inv_mass,
-            im2: rb2.effective_inv_mass,
-            ii1: rb1.effective_world_inv_inertia_sqrt.squared(),
-            ii2: rb2.effective_world_inv_inertia_sqrt.squared(),
+            local_com1: mprops1.mass_properties.local_com,
+            local_com2: mprops2.mass_properties.local_com,
+            im1: mprops1.effective_inv_mass,
+            im2: mprops2.effective_inv_mass,
+            ii1: mprops1.effective_world_inv_inertia_sqrt.squared(),
+            ii2: mprops2.effective_world_inv_inertia_sqrt.squared(),
             local_anchor1: cparams.local_anchor1,
             local_anchor2: cparams.local_anchor2,
-            position1: rb1.active_set_offset,
-            position2: rb2.active_set_offset,
+            position1: ids1.active_set_offset,
+            position2: ids2.active_set_offset,
         }
     }
 
@@ -104,31 +113,34 @@ pub(crate) struct BallPositionGroundConstraint {
 
 impl BallPositionGroundConstraint {
     pub fn from_params(
-        rb1: &RigidBody,
-        rb2: &RigidBody,
+        rb1: &RigidBodyPosition,
+        rb2: (&RigidBodyMassProps, &RigidBodyIds),
         cparams: &BallJoint,
         flipped: bool,
     ) -> Self {
+        let poss1 = rb1;
+        let (mprops2, ids2) = rb2;
+
         if flipped {
             // Note the only thing that is flipped here
             // are the local_anchors. The rb1 and rb2 have
             // already been flipped by the caller.
             Self {
-                anchor1: rb1.next_position * cparams.local_anchor2,
-                im2: rb2.effective_inv_mass,
-                ii2: rb2.effective_world_inv_inertia_sqrt.squared(),
+                anchor1: poss1.next_position * cparams.local_anchor2,
+                im2: mprops2.effective_inv_mass,
+                ii2: mprops2.effective_world_inv_inertia_sqrt.squared(),
                 local_anchor2: cparams.local_anchor1,
-                position2: rb2.active_set_offset,
-                local_com2: rb2.mass_properties.local_com,
+                position2: ids2.active_set_offset,
+                local_com2: mprops2.mass_properties.local_com,
             }
         } else {
             Self {
-                anchor1: rb1.next_position * cparams.local_anchor1,
-                im2: rb2.effective_inv_mass,
-                ii2: rb2.effective_world_inv_inertia_sqrt.squared(),
+                anchor1: poss1.next_position * cparams.local_anchor1,
+                im2: mprops2.effective_inv_mass,
+                ii2: mprops2.effective_world_inv_inertia_sqrt.squared(),
                 local_anchor2: cparams.local_anchor2,
-