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>;
// }
pub trait ComponentSetOption<T> {
pub trait ComponentSetOption<T>: Sync {
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[..]
}
#[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]

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::dynamics::{
IslandManager, RigidBodyActivation, RigidBodyColliders, RigidBodyDominance, RigidBodyHandle,

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

@@ -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);
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 (rb1.is_static(), rb2.is_static()) {
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!(),
}

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

@@ -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);
}
}
})

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

@@ -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! {

7
src/pipeline/collision_pipeline.rs
View File

@@ -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

6
src/pipeline/physics_pipeline.rs
View File

@@ -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,

1
src_testbed/harness/mod.rs
View File

@@ -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,