nichkara/rapier
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Fix compilation of the parallel version

Browse Source
This commit is contained in:
Crozet Sébastien
2021-04-29 11:42:44 +02:00
parent 3810466326
commit 5cf805075e
9 changed files with 151 additions and 83 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
src/data/component_set.rs
View File

@@ -5,7 +5,7 @@ use crate::data::Index;
// fn get(&self, handle: Index) -> Option<&T>; // fn get(&self, handle: Index) -> Option<&T>;
// } // }
pub trait ComponentSetOption<T> { pub trait ComponentSetOption<T>: Sync {
fn get(&self, handle: Index) -> Option<&T>; fn get(&self, handle: Index) -> Option<&T>;
} }

1
src/dynamics/island_manager.rs
View File

@@ -115,7 +115,6 @@ impl IslandManager {
&self.active_dynamic_set[..] &self.active_dynamic_set[..]
} }
#[cfg(not(feature = "parallel"))]
pub(crate) fn active_island(&self, island_id: usize) -> &[RigidBodyHandle] { pub(crate) fn active_island(&self, island_id: usize) -> &[RigidBodyHandle] {
let island_range = self.active_islands[island_id]..self.active_islands[island_id + 1]; let island_range = self.active_islands[island_id]..self.active_islands[island_id + 1];
&self.active_dynamic_set[island_range] &self.active_dynamic_set[island_range]

3
src/dynamics/rigid_body_set.rs
View File

@@ -1,6 +1,3 @@
#[cfg(feature = "parallel")]
use rayon::prelude::*;
use crate::data::{Arena, ComponentSet, ComponentSetMut, ComponentSetOption}; use crate::data::{Arena, ComponentSet, ComponentSetMut, ComponentSetOption};
use crate::dynamics::{ use crate::dynamics::{
IslandManager, RigidBodyActivation, RigidBodyColliders, RigidBodyDominance, RigidBodyHandle, IslandManager, RigidBodyActivation, RigidBodyColliders, RigidBodyDominance, RigidBodyHandle,

60
src/dynamics/solver/interaction_groups.rs
View File

@@ -1,33 +1,32 @@
use crate::data::ComponentSet; use crate::data::ComponentSet;
#[cfg(feature = "parallel")] #[cfg(feature = "parallel")]
use crate::dynamics::BodyPair; use crate::dynamics::RigidBodyHandle;
use crate::dynamics::{IslandManager, RigidBodyIds}; use crate::dynamics::{IslandManager, JointGraphEdge, JointIndex, RigidBodyIds};
use crate::dynamics::{JointGraphEdge, JointIndex};
use crate::geometry::{ContactManifold, ContactManifoldIndex}; use crate::geometry::{ContactManifold, ContactManifoldIndex};
#[cfg(feature = "simd-is-enabled")] #[cfg(feature = "simd-is-enabled")]
use { use {
crate::data::BundleSet, crate::data::BundleSet,
crate::dynamics::RigidBodyType,
crate::math::{SIMD_LAST_INDEX, SIMD_WIDTH}, crate::math::{SIMD_LAST_INDEX, SIMD_WIDTH},
vec_map::VecMap, vec_map::VecMap,
}; };
#[cfg(feature = "parallel")] #[cfg(feature = "parallel")]
pub(crate) trait PairInteraction { 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")] #[cfg(feature = "parallel")]
impl<'a> PairInteraction for &'a mut ContactManifold { impl<'a> PairInteraction for &'a mut ContactManifold {
fn body_pair(&self) -> BodyPair { fn body_pair(&self) -> (Option<RigidBodyHandle>, Option<RigidBodyHandle>) {
self.data.body_pair (self.data.rigid_body1, self.data.rigid_body2)
} }
} }
#[cfg(feature = "parallel")] #[cfg(feature = "parallel")]
impl<'a> PairInteraction for JointGraphEdge { impl<'a> PairInteraction for JointGraphEdge {
fn body_pair(&self) -> BodyPair { fn body_pair(&self) -> (Option<RigidBodyHandle>, Option<RigidBodyHandle>) {
BodyPair::new(self.weight.body1, self.weight.body2) (Some(self.weight.body1), Some(self.weight.body2))
} }
} }
@@ -60,14 +59,17 @@ impl ParallelInteractionGroups {
self.groups.len() - 1 self.groups.len() - 1
} }
pub fn group_interactions<Interaction: PairInteraction>( pub fn group_interactions<Bodies, Interaction: PairInteraction>(
&mut self, &mut self,
island_id: usize, island_id: usize,
bodies: &impl ComponentSet<RigidBody>, islands: &IslandManager,
bodies: &Bodies,
interactions: &[Interaction], interactions: &[Interaction],
interaction_indices: &[usize], interaction_indices: &[usize],
) { ) where
let num_island_bodies = bodies.active_island(island_id).len(); Bodies: ComponentSet<RigidBodyIds> + ComponentSet<RigidBodyType>,
{
let num_island_bodies = islands.active_island(island_id).len();
self.bodies_color.clear(); self.bodies_color.clear();
self.interaction_indices.clear(); self.interaction_indices.clear();
self.groups.clear(); self.groups.clear();
@@ -87,29 +89,39 @@ impl ParallelInteractionGroups {
.zip(self.interaction_colors.iter_mut()) .zip(self.interaction_colors.iter_mut())
{ {
let body_pair = interactions[*interaction_id].body_pair(); let body_pair = interactions[*interaction_id].body_pair();
let rb1 = bodies.index(body_pair.body1); let is_static1 = body_pair
let rb2 = bodies.index(body_pair.body2); .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 (rb1.is_static(), rb2.is_static()) { match (is_static1, is_static2) {
(false, false) => { (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 = 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 = (!color_mask).trailing_zeros() as usize;
color_len[*color] += 1; color_len[*color] += 1;
bcolors[rb1.active_set_offset] |= 1 << *color; bcolors[rb_ids1.active_set_offset] |= 1 << *color;
bcolors[rb2.active_set_offset] |= 1 << *color; bcolors[rb_ids2.active_set_offset] |= 1 << *color;
} }
(true, false) => { (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 = (!color_mask).trailing_zeros() as usize;
color_len[*color] += 1; color_len[*color] += 1;
bcolors[rb2.active_set_offset] |= 1 << *color; bcolors[rb_ids2.active_set_offset] |= 1 << *color;
} }
(false, true) => { (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 = (!color_mask).trailing_zeros() as usize;
color_len[*color] += 1; color_len[*color] += 1;
bcolors[rb1.active_set_offset] |= 1 << *color; bcolors[rb_ids1.active_set_offset] |= 1 << *color;
} }
(true, true) => unreachable!(), (true, true) => unreachable!(),
} }

112
src/dynamics/solver/parallel_island_solver.rs
View File

@@ -1,10 +1,14 @@
use super::{DeltaVel, ParallelInteractionGroups, ParallelVelocitySolver}; use super::{DeltaVel, ParallelInteractionGroups, ParallelVelocitySolver};
use crate::data::ComponentSet; use crate::data::{BundleSet, ComponentSet, ComponentSetMut};
use crate::dynamics::solver::{ use crate::dynamics::solver::{
AnyJointPositionConstraint, AnyJointVelocityConstraint, AnyPositionConstraint, AnyJointPositionConstraint, AnyJointVelocityConstraint, AnyPositionConstraint,
AnyVelocityConstraint, ParallelPositionSolver, ParallelSolverConstraints, 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::geometry::{ContactManifold, ContactManifoldIndex};
use crate::math::{Isometry, Real}; use crate::math::{Isometry, Real};
use crate::utils::WAngularInertia; use crate::utils::WAngularInertia;
@@ -155,17 +159,18 @@ impl ParallelIslandSolver {
&'s mut self, &'s mut self,
scope: &Scope<'s>, scope: &Scope<'s>,
island_id: usize, island_id: usize,
islands: &'s IslandManager,
params: &'s IntegrationParameters, params: &'s IntegrationParameters,
bodies: &'s mut Bodies, bodies: &'s mut Bodies,
) where ) where
Bodies: ComponentSet<RigidBody>, Bodies: ComponentSetMut<RigidBodyPosition> + ComponentSet<RigidBodyIds>,
{ {
let num_threads = rayon::current_num_threads(); 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? 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.thread = ThreadContext::new(8); // TODO: could we compute some kind of optimal value here?
self.positions.clear(); self.positions.clear();
self.positions 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 { for _ in 0..num_task_per_island {
// We use AtomicPtr because it is Send+Sync while *mut is not. // 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. enable_flush_to_zero!(); // Ensure this is enabled on each thread.
// Write results back to rigid bodies and integrate velocities. // Write results back to rigid bodies and integrate velocities.
let island_range = bodies.active_island_range(island_id); let island_range = islands.active_island_range(island_id);
let active_bodies = &bodies.active_dynamic_set[island_range]; let active_bodies = &islands.active_dynamic_set[island_range];
let bodies = &mut bodies.bodies;
concurrent_loop! { concurrent_loop! {
let batch_size = thread.batch_size; let batch_size = thread.batch_size;
for handle in active_bodies.index(thread.body_integration_index, thread.num_integrated_bodies) { for handle in active_bodies[thread.body_integration_index, thread.num_integrated_bodies] {
let rb = &mut bodies.index(handle.0); let (rb_ids, rb_pos): (&RigidBodyIds, &RigidBodyPosition) = bodies.index_bundle(handle.0);
positions[rb.active_set_offset] = rb.next_position; positions[rb_ids.active_set_offset] = rb_pos.next_position;
} }
} }
@@ -219,9 +223,9 @@ impl ParallelIslandSolver {
// Write results back to rigid bodies. // Write results back to rigid bodies.
concurrent_loop! { concurrent_loop! {
let batch_size = thread.batch_size; let batch_size = thread.batch_size;
for handle in active_bodies.index(thread.position_writeback_index) { for handle in active_bodies[thread.position_writeback_index] {
let rb = &mut bodies.index(handle.0); let rb_ids: RigidBodyIds = *bodies.index(handle.0);
rb.set_next_position(positions[rb.active_set_offset]); 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, &'s mut self,
scope: &Scope<'s>, scope: &Scope<'s>,
island_id: usize, island_id: usize,
islands: &'s IslandManager,
params: &'s IntegrationParameters, params: &'s IntegrationParameters,
bodies: &'s mut Bodies, bodies: &'s mut Bodies,
manifolds: &'s mut Vec<&'s mut ContactManifold>, manifolds: &'s mut Vec<&'s mut ContactManifold>,
@@ -239,23 +244,41 @@ impl ParallelIslandSolver {
joints: &'s mut Vec<JointGraphEdge>, joints: &'s mut Vec<JointGraphEdge>,
joint_indices: &[JointIndex], joint_indices: &[JointIndex],
) where ) 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_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? 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.thread = ThreadContext::new(8); // TODO: could we compute some kind of optimal value here?
self.parallel_groups self.parallel_groups.group_interactions(
.group_interactions(island_id, bodies, manifolds, manifold_indices); island_id,
self.parallel_joint_groups islands,
.group_interactions(island_id, bodies, joints, joint_indices); bodies,
manifolds,
manifold_indices,
);
self.parallel_joint_groups.group_interactions(
island_id,
islands,
bodies,
joints,
joint_indices,
);
self.parallel_contact_constraints.init_constraint_groups( self.parallel_contact_constraints.init_constraint_groups(
island_id, island_id,
islands,
bodies, bodies,
manifolds, manifolds,
&self.parallel_groups, &self.parallel_groups,
); );
self.parallel_joint_constraints.init_constraint_groups( self.parallel_joint_constraints.init_constraint_groups(
island_id, island_id,
islands,
bodies, bodies,
joints, joints,
&self.parallel_joint_groups, &self.parallel_joint_groups,
@@ -263,10 +286,10 @@ impl ParallelIslandSolver {
self.mj_lambdas.clear(); self.mj_lambdas.clear();
self.mj_lambdas 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.clear();
self.positions 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 { for _ in 0..num_task_per_island {
// We use AtomicPtr because it is Send+Sync while *mut is not. // 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): // Initialize `mj_lambdas` (per-body velocity deltas) with external accelerations (gravity etc):
{ {
let island_range = bodies.active_island_range(island_id); let island_range = islands.active_island_range(island_id);
let active_bodies = &bodies.active_dynamic_set[island_range]; let active_bodies = &islands.active_dynamic_set[island_range];
let bodies = &mut bodies.bodies;
concurrent_loop! { concurrent_loop! {
let batch_size = thread.batch_size; let batch_size = thread.batch_size;
for handle in active_bodies.index(thread.body_force_integration_index, thread.num_force_integrated_bodies) { for handle in active_bodies[thread.body_force_integration_index, thread.num_force_integrated_bodies] {
let rb = &mut bodies.index(handle.0); let (rb_ids, rb_forces, rb_mass_props): (&RigidBodyIds, &RigidBodyForces, &RigidBodyMassProps) = bodies.index_bundle(handle.0);
let dvel = &mut mj_lambdas[rb.active_set_offset]; let dvel = &mut mj_lambdas[rb_ids.active_set_offset];
// NOTE: `dvel.angular` is actually storing angular velocity delta multiplied // NOTE: `dvel.angular` is actually storing angular velocity delta multiplied
// by the square root of the inertia tensor: // by the square root of the inertia tensor:
dvel.angular += rb.effective_world_inv_inertia_sqrt * rb.torque * params.dt; dvel.angular += rb_mass_props.effective_world_inv_inertia_sqrt * rb_forces.torque * params.dt;
dvel.linear += rb.force * (rb.effective_inv_mass * 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. // Write results back to rigid bodies and integrate velocities.
let island_range = bodies.active_island_range(island_id); let island_range = islands.active_island_range(island_id);
let active_bodies = &bodies.active_dynamic_set[island_range]; let active_bodies = &islands.active_dynamic_set[island_range];
let bodies = &mut bodies.bodies;
concurrent_loop! { concurrent_loop! {
let batch_size = thread.batch_size; let batch_size = thread.batch_size;
for handle in active_bodies.index(thread.body_integration_index, thread.num_integrated_bodies) { for handle in active_bodies[thread.body_integration_index, thread.num_integrated_bodies] {
let rb = &mut bodies.index(handle.0); let (rb_ids, rb_pos, rb_vels, rb_damping, rb_mprops): (
let dvel = mj_lambdas[rb.active_set_offset]; &RigidBodyIds,
rb.linvel += dvel.linear; &RigidBodyPosition,
rb.angvel += rb.effective_world_inv_inertia_sqrt.transform_vector(dvel.angular); &RigidBodyVelocity,
rb.apply_damping(params.dt); &RigidBodyDamping,
rb.integrate_next_position(params.dt); &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);
} }
} }
}) })

42
src/dynamics/solver/parallel_solver_constraints.rs
View File

@@ -1,11 +1,15 @@
use super::ParallelInteractionGroups; use super::ParallelInteractionGroups;
use super::{AnyJointVelocityConstraint, AnyVelocityConstraint, ThreadContext}; use super::{AnyJointVelocityConstraint, AnyVelocityConstraint, ThreadContext};
use crate::data::ComponentSet;
use crate::dynamics::solver::categorization::{categorize_contacts, categorize_joints}; use crate::dynamics::solver::categorization::{categorize_contacts, categorize_joints};
use crate::dynamics::solver::{ use crate::dynamics::solver::{
AnyJointPositionConstraint, AnyPositionConstraint, InteractionGroups, PositionConstraint, AnyJointPositionConstraint, AnyPositionConstraint, InteractionGroups, PositionConstraint,
PositionGroundConstraint, VelocityConstraint, VelocityGroundConstraint, PositionGroundConstraint, VelocityConstraint, VelocityGroundConstraint,
}; };
use crate::dynamics::{IntegrationParameters, JointGraphEdge}; use crate::dynamics::{
IntegrationParameters, IslandManager, JointGraphEdge, RigidBodyIds, RigidBodyMassProps,
RigidBodyPosition, RigidBodyType, RigidBodyVelocity,
};
use crate::geometry::ContactManifold; use crate::geometry::ContactManifold;
#[cfg(feature = "simd-is-enabled")] #[cfg(feature = "simd-is-enabled")]
use crate::{ use crate::{
@@ -36,9 +40,9 @@ pub(crate) enum ConstraintDesc {
NongroundNongrouped(usize), NongroundNongrouped(usize),
GroundNongrouped(usize), GroundNongrouped(usize),
#[cfg(feature = "simd-is-enabled")] #[cfg(feature = "simd-is-enabled")]
NongroundGrouped([usize]), NongroundGrouped([usize; SIMD_WIDTH]),
#[cfg(feature = "simd-is-enabled")] #[cfg(feature = "simd-is-enabled")]
GroundGrouped([usize]), GroundGrouped([usize; SIMD_WIDTH]),
} }
pub(crate) struct ParallelSolverConstraints<VelocityConstraint, PositionConstraint> { pub(crate) struct ParallelSolverConstraints<VelocityConstraint, PositionConstraint> {
@@ -75,13 +79,14 @@ macro_rules! impl_init_constraints_group {
$data: ident$(.$constraint_index: ident)*, $data: ident$(.$constraint_index: ident)*,
$num_active_constraints: path, $empty_velocity_constraint: expr, $empty_position_constraint: expr $(, $weight: ident)*) => { $num_active_constraints: path, $empty_velocity_constraint: expr, $empty_position_constraint: expr $(, $weight: ident)*) => {
impl ParallelSolverConstraints<$VelocityConstraint, $PositionConstraint> { impl ParallelSolverConstraints<$VelocityConstraint, $PositionConstraint> {
pub fn init_constraint_groups( pub fn init_constraint_groups<Bodies>(
&mut self, &mut self,
island_id: usize, island_id: usize,
bodies: &impl ComponentSet<RigidBody>, islands: &IslandManager,
bodies: &Bodies,
interactions: &mut [$Interaction], interactions: &mut [$Interaction],
interaction_groups: &ParallelInteractionGroups, interaction_groups: &ParallelInteractionGroups,
) { ) where Bodies: ComponentSet<RigidBodyType> + ComponentSet<RigidBodyIds> {
let mut total_num_constraints = 0; let mut total_num_constraints = 0;
let num_groups = interaction_groups.num_groups(); let num_groups = interaction_groups.num_groups();
@@ -113,12 +118,14 @@ macro_rules! impl_init_constraints_group {
self.interaction_groups.$group( self.interaction_groups.$group(
island_id, island_id,
islands,
bodies, bodies,
interactions, interactions,
&self.not_ground_interactions, &self.not_ground_interactions,
); );
self.ground_interaction_groups.$group( self.ground_interaction_groups.$group(
island_id, island_id,
islands,
bodies, bodies,
interactions, interactions,
&self.ground_interactions, &self.ground_interactions,
@@ -219,13 +226,18 @@ impl_init_constraints_group!(
); );
impl ParallelSolverConstraints<AnyVelocityConstraint, AnyPositionConstraint> { impl ParallelSolverConstraints<AnyVelocityConstraint, AnyPositionConstraint> {
pub fn fill_constraints( pub fn fill_constraints<Bodies>(
&mut self, &mut self,
thread: &ThreadContext, thread: &ThreadContext,
params: &IntegrationParameters, params: &IntegrationParameters,
bodies: &impl ComponentSet<RigidBody>, bodies: &Bodies,
manifolds_all: &[&mut ContactManifold], manifolds_all: &[&mut ContactManifold],
) { ) where
Bodies: ComponentSet<RigidBodyIds>
+ ComponentSet<RigidBodyPosition>
+ ComponentSet<RigidBodyVelocity>
+ ComponentSet<RigidBodyMassProps>,
{
let descs = &self.constraint_descs; let descs = &self.constraint_descs;
crate::concurrent_loop! { crate::concurrent_loop! {
@@ -261,13 +273,19 @@ impl ParallelSolverConstraints<AnyVelocityConstraint, AnyPositionConstraint> {
} }
impl ParallelSolverConstraints<AnyJointVelocityConstraint, AnyJointPositionConstraint> { impl ParallelSolverConstraints<AnyJointVelocityConstraint, AnyJointPositionConstraint> {
pub fn fill_constraints( pub fn fill_constraints<Bodies>(
&mut self, &mut self,
thread: &ThreadContext, thread: &ThreadContext,
params: &IntegrationParameters, params: &IntegrationParameters,
bodies: &impl ComponentSet<RigidBody>, bodies: &Bodies,
joints_all: &[JointGraphEdge], joints_all: &[JointGraphEdge],
) { ) where
Bodies: ComponentSet<RigidBodyPosition>
+ ComponentSet<RigidBodyVelocity>
+ ComponentSet<RigidBodyMassProps>
+ ComponentSet<RigidBodyIds>
+ ComponentSet<RigidBodyType>,
{
let descs = &self.constraint_descs; let descs = &self.constraint_descs;
crate::concurrent_loop! { crate::concurrent_loop! {

7
src/pipeline/collision_pipeline.rs
View File

@@ -3,16 +3,19 @@
use crate::data::{ComponentSet, ComponentSetMut, ComponentSetOption}; use crate::data::{ComponentSet, ComponentSetMut, ComponentSetOption};
use crate::dynamics::{ use crate::dynamics::{
RigidBodyActivation, RigidBodyChanges, RigidBodyColliders, RigidBodyDominance, RigidBodyHandle, RigidBodyActivation, RigidBodyChanges, RigidBodyColliders, RigidBodyDominance, RigidBodyHandle,
RigidBodyIds, RigidBodyPosition, RigidBodySet, RigidBodyType, RigidBodyVelocity, RigidBodyIds, RigidBodyPosition, RigidBodyType, RigidBodyVelocity,
}; };
use crate::geometry::{ use crate::geometry::{
BroadPhase, BroadPhasePairEvent, ColliderBroadPhaseData, ColliderChanges, ColliderGroups, BroadPhase, BroadPhasePairEvent, ColliderBroadPhaseData, ColliderChanges, ColliderGroups,
ColliderHandle, ColliderMaterial, ColliderPair, ColliderParent, ColliderPosition, ColliderSet, ColliderHandle, ColliderMaterial, ColliderPair, ColliderParent, ColliderPosition,
ColliderShape, ColliderType, NarrowPhase, ColliderShape, ColliderType, NarrowPhase,
}; };
use crate::math::Real; use crate::math::Real;
use crate::pipeline::{EventHandler, PhysicsHooks}; 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. /// 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 /// This structure only contains temporary data buffers. It can be dropped and replaced by a fresh

6
src/pipeline/physics_pipeline.rs
View File

@@ -183,7 +183,7 @@ impl PhysicsPipeline {
use rayon::prelude::*; use rayon::prelude::*;
use std::sync::atomic::Ordering; 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 solvers = &mut self.solvers[..num_islands];
let bodies = &std::sync::atomic::AtomicPtr::new(bodies as *mut _); let bodies = &std::sync::atomic::AtomicPtr::new(bodies as *mut _);
@@ -200,6 +200,7 @@ impl PhysicsPipeline {
solver.solve_position_constraints( solver.solve_position_constraints(
scope, scope,
island_id, island_id,
islands,
integration_parameters, integration_parameters,
bodies, bodies,
) )
@@ -306,7 +307,7 @@ impl PhysicsPipeline {
use rayon::prelude::*; use rayon::prelude::*;
use std::sync::atomic::Ordering; 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 solvers = &mut self.solvers[..num_islands];
let bodies = &std::sync::atomic::AtomicPtr::new(bodies as *mut _); let bodies = &std::sync::atomic::AtomicPtr::new(bodies as *mut _);
let manifolds = &std::sync::atomic::AtomicPtr::new(&mut manifolds 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( solver.init_constraints_and_solve_velocity_constraints(
scope, scope,
island_id, island_id,
islands,
integration_parameters, integration_parameters,
bodies, bodies,
manifolds, manifolds,

1
src_testbed/harness/mod.rs
View File

@@ -176,6 +176,7 @@ impl Harness {
physics.pipeline.step( physics.pipeline.step(
&physics.gravity, &physics.gravity,
&physics.integration_parameters, &physics.integration_parameters,
&mut physics.islands,
&mut physics.broad_phase, &mut physics.broad_phase,
&mut physics.narrow_phase, &mut physics.narrow_phase,
&mut physics.bodies, &mut physics.bodies,