diff options
| -rw-r--r-- | src/data/component_set.rs | 2 | ||||
| -rw-r--r-- | src/dynamics/island_manager.rs | 1 | ||||
| -rw-r--r-- | src/dynamics/rigid_body_set.rs | 3 | ||||
| -rw-r--r-- | src/dynamics/solver/interaction_groups.rs | 62 | ||||
| -rw-r--r-- | src/dynamics/solver/parallel_island_solver.rs | 112 | ||||
| -rw-r--r-- | src/dynamics/solver/parallel_solver_constraints.rs | 42 | ||||
| -rw-r--r-- | src/pipeline/collision_pipeline.rs | 7 | ||||
| -rw-r--r-- | src/pipeline/physics_pipeline.rs | 6 | ||||
| -rw-r--r-- | src_testbed/harness/mod.rs | 1 |
9 files changed, 152 insertions, 84 deletions
diff --git a/src/data/component_set.rs b/src/data/component_set.rs index 76e076a..ca7df67 100644 --- a/src/data/component_set.rs +++ b/src/data/component_set.rs @@ -5,7 +5,7 @@ use crate::data::Index; // fn get(&self, handle: Index) -> Option<&T>; // } -pub trait ComponentSetOption<T> { +pub trait ComponentSetOption<T>: Sync { fn get(&self, handle: Index) -> Option<&T>; } diff --git a/src/dynamics/island_manager.rs b/src/dynamics/island_manager.rs index c29609f..551e5a4 100644 --- a/src/dynamics/island_manager.rs +++ b/src/dynamics/island_manager.rs @@ -115,7 +115,6 @@ impl IslandManager { &self.active_dynamic_set[..] } - #[cfg(not(feature = "parallel"))] pub(crate) fn active_island(&self, island_id: usize) -> &[RigidBodyHandle] { let island_range = self.active_islands[island_id]..self.active_islands[island_id + 1]; &self.active_dynamic_set[island_range] diff --git a/src/dynamics/rigid_body_set.rs b/src/dynamics/rigid_body_set.rs index 7b4a31f..b0cf39a 100644 --- a/src/dynamics/rigid_body_set.rs +++ b/src/dynamics/rigid_body_set.rs @@ -1,6 +1,3 @@ -#[cfg(feature = "parallel")] -use rayon::prelude::*; - use crate::data::{Arena, ComponentSet, ComponentSetMut, ComponentSetOption}; use crate::dynamics::{ IslandManager, RigidBodyActivation, RigidBodyColliders, RigidBodyDominance, RigidBodyHandle, diff --git a/src/dynamics/solver/interaction_groups.rs b/src/dynamics/solver/interaction_groups.rs index 872ff3a..c6baea1 100644 --- a/src/dynamics/solver/interaction_groups.rs +++ b/src/dynamics/solver/interaction_groups.rs @@ -1,33 +1,32 @@ use crate::data::ComponentSet; #[cfg(feature = "parallel")] -use crate::dynamics::BodyPair; -use crate::dynamics::{IslandManager, RigidBodyIds}; -use crate::dynamics::{JointGraphEdge, JointIndex}; +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::dynamics::RigidBodyType, 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)) } } @@ -60,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: &impl ComponentSet<RigidBody>, + 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(); @@ -87,29 +89,39 @@ impl ParallelInteractionGroups { .zip(self.interaction_colors.iter_mut()) { let body_pair = interactions[*interaction_id].body_pair(); - let rb1 = bodies.index(body_pair.body1); - let rb2 = bodies.index(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!(), } diff --git a/src/dynamics/solver/parallel_island_solver.rs b/src/dynamics/solver/parallel_island_solver.rs index 115b099..9d565c1 100644 --- a/src/dynamics/solver/parallel_island_solver.rs +++ b/src/dynamics/solver/parallel_island_solver.rs @@ -1,10 +1,14 @@ use super::{DeltaVel, ParallelInteractionGroups, ParallelVelocitySolver}; -use crate::data::ComponentSet; +use crate::data::{BundleSet, ComponentSet, ComponentSetMut}; use crate::dynamics::solver::{ AnyJointPositionConstraint, AnyJointVelocityConstraint, AnyPositionConstraint, AnyVelocityConstraint, ParallelPositionSolver, ParallelSolverConstraints, }; -use crate::dynamics::{IntegrationParameters, JointGraphEdge, JointIndex}; +use crate::dynamics::{ + IntegrationParameters, IslandManager, JointGraphEdge, JointIndex, RigidBodyDamping, + RigidBodyForces, RigidBodyIds, RigidBodyMassProps, RigidBodyPosition, RigidBodyType, + RigidBodyVelocity, +}; use crate::geometry::{ContactManifold, ContactManifoldIndex}; use crate::math::{Isometry, Real}; use crate::utils::WAngularInertia; @@ -155,17 +159,18 @@ impl ParallelIslandSolver { &'s mut self, scope: &Scope<'s>, island_id: usize, + islands: &'s IslandManager, params: &'s IntegrationParameters, bodies: &'s mut Bodies, ) where - Bodies: ComponentSet<RigidBody>, + Bodies: ComponentSetMut<RigidBodyPosition> + ComponentSet<RigidBodyIds>, { let num_threads = rayon::current_num_threads(); let num_task_per_island = num_threads; // (num_threads / num_islands).max(1); // TODO: not sure this is the best value. Also, perhaps it is better to interleave tasks of each island? self.thread = ThreadContext::new(8); // TODO: could we compute some kind of optimal value here? self.positions.clear(); self.positions - .resize(bodies.active_island(island_id).len(), Isometry::identity()); + .resize(islands.active_island(island_id).len(), Isometry::identity()); for _ in 0..num_task_per_island { // We use AtomicPtr because it is Send+Sync while *mut is not. @@ -194,15 +199,14 @@ impl ParallelIslandSolver { enable_flush_to_zero!(); // Ensure this is enabled on each thread. // Write results back to rigid bodies and integrate velocities. - let island_range = bodies.active_island_range(island_id); - let active_bodies = &bodies.active_dynamic_set[island_range]; - let bodies = &mut bodies.bodies; + let island_range = islands.active_island_range(island_id); + let active_bodies = &islands.active_dynamic_set[island_range]; concurrent_loop! { let batch_size = thread.batch_size; - for handle in active_bodies.index(thread.body_integration_index, thread.num_integrated_bodies) { - let rb = &mut bodies.index(handle.0); - positions[rb.active_set_offset] = rb.next_position; + for handle in active_bodies[thread.body_integration_index, thread.num_integrated_bodies] { + let (rb_ids, rb_pos): (&RigidBodyIds, &RigidBodyPosition) = bodies.index_bundle(handle.0); + positions[rb_ids.active_set_offset] = rb_pos.next_position; } } @@ -219,9 +223,9 @@ impl ParallelIslandSolver { // Write results back to rigid bodies. concurrent_loop! { let batch_size = thread.batch_size; - for handle in active_bodies.index(thread.position_writeback_index) { - let rb = &mut bodies.index(handle.0); - rb.set_next_position(positions[rb.active_set_offset]); + for handle in active_bodies[thread.position_writeback_index] { + let rb_ids: RigidBodyIds = *bodies.index(handle.0); + bodies.map_mut_internal(handle.0, |rb_pos: &mut RigidBodyPosition| rb_pos.next_position = positions[rb_ids.active_set_offset]); } } }) @@ -232,6 +236,7 @@ impl ParallelIslandSolver { &'s mut self, scope: &Scope<'s>, island_id: usize, + islands: &'s IslandManager, params: &'s IntegrationParameters, bodies: &'s mut Bodies, manifolds: &'s mut Vec<&'s mut ContactManifold>, @@ -239,23 +244,41 @@ impl ParallelIslandSolver { joints: &'s mut Vec<JointGraphEdge>, joint_indices: &[JointIndex], ) where - Bodies: ComponentSet<RigidBody>, + Bodies: ComponentSet<RigidBodyForces> + + ComponentSetMut<RigidBodyPosition> + + ComponentSetMut<RigidBodyVelocity> + + ComponentSet<RigidBodyMassProps> + + ComponentSet<RigidBodyDamping> + + ComponentSet<RigidBodyIds> + + ComponentSet<RigidBodyType>, { let num_threads = rayon::current_num_threads(); let num_task_per_island = num_threads; // (num_threads / num_islands).max(1); // TODO: not sure this is the best value. Also, perhaps it is better to interleave tasks of each island? self.thread = ThreadContext::new(8); // TODO: could we compute some kind of optimal value here? - self.parallel_groups - .group_interactions(island_id, bodies, manifolds, manifold_indices); - self.parallel_joint_groups - .group_interactions(island_id, bodies, joints, joint_indices); + self.parallel_groups.group_interactions( + island_id, + islands, + bodies, + manifolds, + manifold_indices, + ); + self.parallel_joint_groups.group_interactions( + island_id, + islands, + bodies, + joints, + joint_indices, + ); self.parallel_contact_constraints.init_constraint_groups( island_id, + islands, bodies, manifolds, &self.parallel_groups, ); self.parallel_joint_constraints.init_constraint_groups( island_id, + islands, bodies, joints, &self.parallel_joint_groups, @@ -263,10 +286,10 @@ impl ParallelIslandSolver { self.mj_lambdas.clear(); self.mj_lambdas - .resize(bodies.active_island(island_id).len(), DeltaVel::zero()); + .resize(islands.active_island(island_id).len(), DeltaVel::zero()); self.positions.clear(); self.positions - .resize(bodies.active_island(island_id).len(), Isometry::identity()); + .resize(islands.active_island(island_id).len(), Isometry::identity()); for _ in 0..num_task_per_island { // We use AtomicPtr because it is Send+Sync while *mut is not. @@ -302,20 +325,19 @@ impl ParallelIslandSolver { // Initialize `mj_lambdas` (per-body velocity deltas) with external accelerations (gravity etc): { - let island_range = bodies.active_island_range(island_id); - let active_bodies = &bodies.active_dynamic_set[island_range]; - let bodies = &mut bodies.bodies; + let island_range = islands.active_island_range(island_id); + let active_bodies = &islands.active_dynamic_set[island_range]; concurrent_loop! { let batch_size = thread.batch_size; - for handle in active_bodies.index(thread.body_force_integration_index, thread.num_force_integrated_bodies) { - let rb = &mut bodies.index(handle.0); - let dvel = &mut mj_lambdas[rb.active_set_offset]; + for handle in active_bodies[thread.body_force_integration_index, thread.num_force_integrated_bodies] { + let (rb_ids, rb_forces, rb_mass_props): (&RigidBodyIds, &RigidBodyForces, &RigidBodyMassProps) = bodies.index_bundle(handle.0); + let dvel = &mut mj_lambdas[rb_ids.active_set_offset]; // NOTE: `dvel.angular` is actually storing angular velocity delta multiplied // by the square root of the inertia tensor: - dvel.angular += rb.effective_world_inv_inertia_sqrt * rb.torque * params.dt; - dvel.linear += rb.force * (rb.effective_inv_mass * params.dt); + dvel.angular += rb_mass_props.effective_world_inv_inertia_sqrt * rb_forces.torque * params.dt; + dvel.linear += rb_forces.force * (rb_mass_props.effective_inv_mass * params.dt); } } @@ -347,19 +369,33 @@ impl ParallelIslandSolver { ); // Write results back to rigid bodies and integrate velocities. - let island_range = bodies.active_island_range(island_id); - let active_bodies = &bodies.active_dynamic_set[island_range]; - let bodies = &mut bodies.bodies; + let island_range = islands.active_island_range(island_id); + let active_bodies = &islands.active_dynamic_set[island_range]; concurrent_loop! { let batch_size = thread.batch_size; - for handle in active_bodies.index(thread.body_integration_index, thread.num_integrated_bodies) { - let rb = &mut bodies.index(handle.0); - let dvel = mj_lambdas[rb.active_set_offset]; - rb.linvel += dvel.linear; - rb.angvel += rb.effective_world_inv_inertia_sqrt.transform_vector(dvel.angular); - rb.apply_damping(params.dt); - rb.integrate_next_position(params.dt); + for handle in active_bodies[thread.body_integration_index, thread.num_integrated_bodies] { + let (rb_ids, rb_pos, rb_vels, rb_damping, rb_mprops): ( + &RigidBodyIds, + &RigidBodyPosition, + &RigidBodyVelocity, + &RigidBodyDamping, + &RigidBodyMassProps, + ) = bodies.index_bundle(handle.0); + + let mut new_rb_pos = *rb_pos; + let mut new_rb_vels = *rb_vels; + + let dvels = mj_lambdas[rb_ids.active_set_offset]; + new_rb_vels.linvel += dvels.linear; + new_rb_vels.angvel += rb_mprops.effective_world_inv_inertia_sqrt.transform_vector(dvels.angular); + + let new_rb_vels = new_rb_vels.apply_damping(params.dt, rb_damping); + new_rb_pos.next_position = + new_rb_vels.integrate(params.dt, &rb_pos.position, &rb_mprops.mass_properties.local_com); + + bodies.set_internal(handle.0, new_rb_vels); + bodies.set_internal(handle.0, new_rb_pos); } } }) diff --git a/src/dynamics/solver/parallel_solver_constraints.rs b/src/dynamics/solver/parallel_solver_constraints.rs index dd1a19f..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}; +use crate::dynamics::{ + IntegrationParameters, IslandManager, JointGraphEdge, RigidBodyIds, RigidBodyMassProps, + RigidBodyPosition, RigidBodyType, RigidBodyVelocity, +}; use crate::geometry::ContactManifold; #[cfg(feature = "simd-is-enabled")] use crate::{ @@ -36,9 +40,9 @@ pub(crate) enum ConstraintDesc { NongroundNongrouped(usize), GroundNongrouped(usize), #[cfg(feature = "simd-is-enabled")] - NongroundGrouped([usize]), + NongroundGrouped([usize; SIMD_WIDTH]), #[cfg(feature = "simd-is-enabled")] - GroundGrouped([usize]), + GroundGrouped([usize; SIMD_WIDTH]), } pub(crate) struct ParallelSolverConstraints<VelocityConstraint, PositionConstraint> { @@ -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: &impl ComponentSet<RigidBody>, + 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, @@ -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: &impl ComponentSet<RigidBody>, + bodies: &Bodies, manifolds_all: &[&mut ContactManifold], - ) { + ) where + Bodies: ComponentSet<RigidBodyIds> + + ComponentSet<RigidBodyPosition> + + ComponentSet<RigidBodyVelocity> + + ComponentSet<RigidBodyMassProps>, + { let descs = &self.constraint_descs; crate::concurrent_loop! { @@ -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: &impl ComponentSet<RigidBody>, + 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! { diff --git a/src/pipeline/collision_pipeline.rs b/src/pipeline/collision_pipeline.rs index c9ab119..455e75d 100644 --- a/src/pipeline/collision_pipeline.rs +++ b/src/pipeline/collision_pipeline.rs @@ -3,16 +3,19 @@ use crate::data::{ComponentSet, ComponentSetMut, ComponentSetOption}; use crate::dynamics::{ RigidBodyActivation, RigidBodyChanges, RigidBodyColliders, RigidBodyDominance, RigidBodyHandle, - RigidBodyIds, RigidBodyPosition, RigidBodySet, RigidBodyType, RigidBodyVelocity, + RigidBodyIds, RigidBodyPosition, RigidBodyType, RigidBodyVelocity, }; use crate::geometry::{ BroadPhase, BroadPhasePairEvent, ColliderBroadPhaseData, ColliderChanges, ColliderGroups, - ColliderHandle, ColliderMaterial, ColliderPair, ColliderParent, ColliderPosition, ColliderSet, + ColliderHandle, ColliderMaterial, ColliderPair, ColliderParent, ColliderPosition, ColliderShape, ColliderType, NarrowPhase, }; use crate::math::Real; use crate::pipeline::{EventHandler, PhysicsHooks}; +#[cfg(feature = "default-sets")] +use crate::{dynamics::RigidBodySet, geometry::ColliderSet}; + /// The collision pipeline, responsible for performing collision detection between colliders. /// /// This structure only contains temporary data buffers. It can be dropped and replaced by a fresh diff --git a/src/pipeline/physics_pipeline.rs b/src/pipeline/physics_pipeline.rs index 9557d1f..328e810 100644 --- a/src/pipeline/physics_pipeline.rs +++ b/src/pipeline/physics_pipeline.rs @@ -183,7 +183,7 @@ impl PhysicsPipeline { use rayon::prelude::*; use std::sync::atomic::Ordering; - let num_islands = ilands.num_islands(); + let num_islands = islands.num_islands(); let solvers = &mut self.solvers[..num_islands]; let bodies = &std::sync::atomic::AtomicPtr::new(bodies as *mut _); @@ -200,6 +200,7 @@ impl PhysicsPipeline { solver.solve_position_constraints( scope, island_id, + islands, integration_parameters, bodies, ) @@ -306,7 +307,7 @@ impl PhysicsPipeline { use rayon::prelude::*; use std::sync::atomic::Ordering; - let num_islands = bodies.num_islands(); + let num_islands = islands.num_islands(); let solvers = &mut self.solvers[..num_islands]; let bodies = &std::sync::atomic::AtomicPtr::new(bodies as *mut _); let manifolds = &std::sync::atomic::AtomicPtr::new(&mut manifolds as *mut _); @@ -331,6 +332,7 @@ impl PhysicsPipeline { solver.init_constraints_and_solve_velocity_constraints( scope, island_id, + islands, integration_parameters, bodies, manifolds, diff --git a/src_testbed/harness/mod.rs b/src_testbed/harness/mod.rs index 8447fd0..e37e63d 100644 --- a/src_testbed/harness/mod.rs +++ b/src_testbed/harness/mod.rs @@ -176,6 +176,7 @@ impl Harness { physics.pipeline.step( &physics.gravity, &physics.integration_parameters, + &mut physics.islands, &mut physics.broad_phase, &mut physics.narrow_phase, &mut physics.bodies, |