Merge pull request #183 from dimforge/bundles

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

1 files changed, 35 insertions, 17 deletions

diff --git a/src/dynamics/solver/parallel_solver_constraints.rs b/src/dynamics/solver/parallel_solver_constraints.rs
index fcdcbff..6b00b73 100644
--- a/src/dynamics/solver/parallel_solver_constraints.rs
+++ b/src/dynamics/solver/parallel_solver_constraints.rs
@@ -1,11 +1,15 @@
 use super::ParallelInteractionGroups;
 use super::{AnyJointVelocityConstraint, AnyVelocityConstraint, ThreadContext};
+use crate::data::ComponentSet;
 use crate::dynamics::solver::categorization::{categorize_contacts, categorize_joints};
 use crate::dynamics::solver::{
     AnyJointPositionConstraint, AnyPositionConstraint, InteractionGroups, PositionConstraint,
     PositionGroundConstraint, VelocityConstraint, VelocityGroundConstraint,
 };
-use crate::dynamics::{IntegrationParameters, JointGraphEdge, RigidBodySet};
+use crate::dynamics::{
+    IntegrationParameters, IslandManager, JointGraphEdge, RigidBodyIds, RigidBodyMassProps,
+    RigidBodyPosition, RigidBodyType, RigidBodyVelocity,
+};
 use crate::geometry::ContactManifold;
 #[cfg(feature = "simd-is-enabled")]
 use crate::{
@@ -20,7 +24,7 @@ use std::sync::atomic::Ordering;
 // pub fn init_constraint_groups(
 //     &mut self,
 //     island_id: usize,
-//     bodies: &RigidBodySet,
+//     bodies: &impl ComponentSet<RigidBody>,
 //     manifolds: &mut [&mut ContactManifold],
 //     manifold_groups: &ParallelInteractionGroups,
 //     joints: &mut [JointGraphEdge],
@@ -75,13 +79,14 @@ macro_rules! impl_init_constraints_group {
      $data: ident$(.$constraint_index: ident)*,
      $num_active_constraints: path, $empty_velocity_constraint: expr, $empty_position_constraint: expr $(, $weight: ident)*) => {
         impl ParallelSolverConstraints<$VelocityConstraint, $PositionConstraint> {
-            pub fn init_constraint_groups(
+            pub fn init_constraint_groups<Bodies>(
                 &mut self,
                 island_id: usize,
-                bodies: &RigidBodySet,
+                islands: &IslandManager,
+                bodies: &Bodies,
                 interactions: &mut [$Interaction],
                 interaction_groups: &ParallelInteractionGroups,
-            ) {
+            ) where Bodies: ComponentSet<RigidBodyType> + ComponentSet<RigidBodyIds> {
                 let mut total_num_constraints = 0;
                 let num_groups = interaction_groups.num_groups();
 
@@ -113,12 +118,14 @@ macro_rules! impl_init_constraints_group {
 
                     self.interaction_groups.$group(
                         island_id,
+                        islands,
                         bodies,
                         interactions,
                         &self.not_ground_interactions,
                     );
                     self.ground_interaction_groups.$group(
                         island_id,
+                        islands,
                         bodies,
                         interactions,
                         &self.ground_interactions,
@@ -144,7 +151,7 @@ macro_rules! impl_init_constraints_group {
                         self.constraint_descs.push((
                             total_num_constraints,
                             ConstraintDesc::NongroundGrouped(
-                                array![|ii| interaction_i[ii]; SIMD_WIDTH],
+                                gather![|ii| interaction_i[ii]],
                             ),
                         ));
                         total_num_constraints += $num_active_constraints(interaction);
@@ -172,7 +179,7 @@ macro_rules! impl_init_constraints_group {
                         self.constraint_descs.push((
                             total_num_constraints,
                             ConstraintDesc::GroundGrouped(
-                                array![|ii| interaction_i[ii]; SIMD_WIDTH],
+                                gather![|ii| interaction_i[ii]],
                             ),
                         ));
                         total_num_constraints += $num_active_constraints(interaction);
@@ -219,13 +226,18 @@ impl_init_constraints_group!(
 );
 
 impl ParallelSolverConstraints<AnyVelocityConstraint, AnyPositionConstraint> {
-    pub fn fill_constraints(
+    pub fn fill_constraints<Bodies>(
         &mut self,
         thread: &ThreadContext,
         params: &IntegrationParameters,
-        bodies: &RigidBodySet,
+        bodies: &Bodies,
         manifolds_all: &[&mut ContactManifold],
-    ) {
+    ) where
+        Bodies: ComponentSet<RigidBodyIds>
+            + ComponentSet<RigidBodyPosition>
+            + ComponentSet<RigidBodyVelocity>
+            + ComponentSet<RigidBodyMassProps>,
+    {
         let descs = &self.constraint_descs;
 
         crate::concurrent_loop! {
@@ -244,13 +256,13 @@ impl ParallelSolverConstraints<AnyVelocityConstraint, AnyPositionConstraint> {
                     }
                     #[cfg(feature = "simd-is-enabled")]
                     ConstraintDesc::NongroundGrouped(manifold_id) => {
-                        let manifolds = array![|ii| &*manifolds_all[manifold_id[ii]]; SIMD_WIDTH];
+                        let manifolds = gather![|ii| &*manifolds_all[manifold_id[ii]]];
                         WVelocityConstraint::generate(params, *manifold_id, manifolds, bodies, &mut self.velocity_constraints, false);
                         WPositionConstraint::generate(params, manifolds, bodies, &mut self.position_constraints, false);
                     }
                     #[cfg(feature = "simd-is-enabled")]
                     ConstraintDesc::GroundGrouped(manifold_id) => {
-                        let manifolds = array![|ii| &*manifolds_all[manifold_id[ii]]; SIMD_WIDTH];
+                        let manifolds = gather![|ii| &*manifolds_all[manifold_id[ii]]];
                         WVelocityGroundConstraint::generate(params, *manifold_id, manifolds, bodies, &mut self.velocity_constraints, false);
                         WPositionGroundConstraint::generate(params, manifolds, bodies, &mut self.position_constraints, false);
                     }
@@ -261,13 +273,19 @@ impl ParallelSolverConstraints<AnyVelocityConstraint, AnyPositionConstraint> {
 }
 
 impl ParallelSolverConstraints<AnyJointVelocityConstraint, AnyJointPositionConstraint> {
-    pub fn fill_constraints(
+    pub fn fill_constraints<Bodies>(
         &mut self,
         thread: &ThreadContext,
         params: &IntegrationParameters,
-        bodies: &RigidBodySet,
+        bodies: &Bodies,
         joints_all: &[JointGraphEdge],
-    ) {
+    ) where
+        Bodies: ComponentSet<RigidBodyPosition>
+            + ComponentSet<RigidBodyVelocity>
+            + ComponentSet<RigidBodyMassProps>
+            + ComponentSet<RigidBodyIds>
+            + ComponentSet<RigidBodyType>,
+    {
         let descs = &self.constraint_descs;
 
         crate::concurrent_loop! {
@@ -290,7 +308,7 @@ impl ParallelSolverConstraints<AnyJointVelocityConstraint, AnyJointPositionConst
                     }
                     #[cfg(feature = "simd-is-enabled")]
                     ConstraintDesc::NongroundGrouped(joint_id) => {
-                        let joints = array![|ii| &joints_all[joint_id[ii]].weight; SIMD_WIDTH];
+                        let joints = gather![|ii| &joints_all[joint_id[ii]].weight];
                         let velocity_constraint = AnyJointVelocityConstraint::from_wide_joint(params, *joint_id, joints, bodies);
                         let position_constraint = AnyJointPositionConstraint::from_wide_joint(joints, bodies);
                         self.velocity_constraints[joints[0].constraint_index] = velocity_constraint;
@@ -298,7 +316,7 @@ impl ParallelSolverConstraints<AnyJointVelocityConstraint, AnyJointPositionConst
                     }
                     #[cfg(feature = "simd-is-enabled")]
                     ConstraintDesc::GroundGrouped(joint_id) => {
-                        let joints = array![|ii| &joints_all[joint_id[ii]].weight; SIMD_WIDTH];
+                        let joints = gather![|ii| &joints_all[joint_id[ii]].weight];
                         let velocity_constraint = AnyJointVelocityConstraint::from_wide_joint_ground(params, *joint_id, joints, bodies);
                         let position_constraint = AnyJointPositionConstraint::from_wide_joint_ground(joints, bodies);
                         self.velocity_constraints[joints[0].constraint_index] = velocity_constraint;