diff options
Diffstat (limited to 'src/dynamics/solver/parallel_solver_constraints.rs')
| -rw-r--r-- | src/dynamics/solver/parallel_solver_constraints.rs | 52 |
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; |