diff options
Diffstat (limited to 'src/dynamics/solver')
38 files changed, 2096 insertions, 1375 deletions
diff --git a/src/dynamics/solver/categorization.rs b/src/dynamics/solver/categorization.rs index e3327c6..366a5b3 100644 --- a/src/dynamics/solver/categorization.rs +++ b/src/dynamics/solver/categorization.rs @@ -1,8 +1,9 @@ -use crate::dynamics::{JointGraphEdge, JointIndex, RigidBodySet}; +use crate::data::ComponentSet; +use crate::dynamics::{JointGraphEdge, JointIndex, RigidBodyType}; use crate::geometry::{ContactManifold, ContactManifoldIndex}; pub(crate) fn categorize_contacts( - _bodies: &RigidBodySet, // Unused but useful to simplify the parallel code. + _bodies: &impl ComponentSet<RigidBodyType>, // Unused but useful to simplify the parallel code. manifolds: &[&mut ContactManifold], manifold_indices: &[ContactManifoldIndex], out_ground: &mut Vec<ContactManifoldIndex>, @@ -20,7 +21,7 @@ pub(crate) fn categorize_contacts( } pub(crate) fn categorize_joints( - bodies: &RigidBodySet, + bodies: &impl ComponentSet<RigidBodyType>, joints: &[JointGraphEdge], joint_indices: &[JointIndex], ground_joints: &mut Vec<JointIndex>, @@ -28,10 +29,10 @@ pub(crate) fn categorize_joints( ) { for joint_i in joint_indices { let joint = &joints[*joint_i].weight; - let rb1 = &bodies[joint.body1]; - let rb2 = &bodies[joint.body2]; + let status1 = bodies.index(joint.body1.0); + let status2 = bodies.index(joint.body2.0); - if !rb1.is_dynamic() || !rb2.is_dynamic() { + if !status1.is_dynamic() || !status2.is_dynamic() { ground_joints.push(*joint_i); } else { nonground_joints.push(*joint_i); diff --git a/src/dynamics/solver/interaction_groups.rs b/src/dynamics/solver/interaction_groups.rs index 6b8de5a..c6baea1 100644 --- a/src/dynamics/solver/interaction_groups.rs +++ b/src/dynamics/solver/interaction_groups.rs @@ -1,24 +1,32 @@ -use crate::dynamics::{BodyPair, JointGraphEdge, JointIndex, RigidBodySet}; +use crate::data::ComponentSet; +#[cfg(feature = "parallel")] +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::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)) } } @@ -51,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: &RigidBodySet, + 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(); @@ -78,29 +89,39 @@ impl ParallelInteractionGroups { .zip(self.interaction_colors.iter_mut()) { let body_pair = interactions[*interaction_id].body_pair(); - let rb1 = &bodies[body_pair.body1]; - let rb2 = &bodies[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!(), } @@ -168,14 +189,15 @@ impl InteractionGroups { self.nongrouped_interactions.clear(); } - // FIXME: there is a lot of duplicated code with group_manifolds here. + // TODO: there is a lot of duplicated code with group_manifolds here. // But we don't refactor just now because we may end up with distinct // grouping strategies in the future. #[cfg(not(feature = "simd-is-enabled"))] pub fn group_joints( &mut self, _island_id: usize, - _bodies: &RigidBodySet, + _islands: &IslandManager, + _bodies: &impl ComponentSet<RigidBodyIds>, _interactions: &[JointGraphEdge], interaction_indices: &[JointIndex], ) { @@ -184,13 +206,16 @@ impl InteractionGroups { } #[cfg(feature = "simd-is-enabled")] - pub fn group_joints( + pub fn group_joints<Bodies>( &mut self, island_id: usize, - bodies: &RigidBodySet, + islands: &IslandManager, + bodies: &Bodies, interactions: &[JointGraphEdge], interaction_indices: &[JointIndex], - ) { + ) where + Bodies: ComponentSet<RigidBodyType> + ComponentSet<RigidBodyIds>, + { // NOTE: in 3D we have up to 10 different joint types. // In 2D we only have 5 joint types. #[cfg(feature = "dim3")] @@ -204,11 +229,11 @@ impl InteractionGroups { // Note: each bit of a body mask indicates what bucket already contains // a constraints involving this body. - // FIXME: currently, this is a bit overconservative because when a bucket + // TODO: currently, this is a bit overconservative because when a bucket // is full, we don't clear the corresponding body mask bit. This may result // in less grouped constraints. self.body_masks - .resize(bodies.active_island(island_id).len(), 0u128); + .resize(islands.active_island(island_id).len(), 0u128); // NOTE: each bit of the occupied mask indicates what bucket already // contains at least one constraint. @@ -216,10 +241,14 @@ impl InteractionGroups { for interaction_i in interaction_indices { let interaction = &interactions[*interaction_i].weight; - let body1 = &bodies[interaction.body1]; - let body2 = &bodies[interaction.body2]; - let is_static1 = !body1.is_dynamic(); - let is_static2 = !body2.is_dynamic(); + + let (status1, ids1): (&RigidBodyType, &RigidBodyIds) = + bodies.index_bundle(interaction.body1.0); + let (status2, ids2): (&RigidBodyType, &RigidBodyIds) = + bodies.index_bundle(interaction.body2.0); + + let is_static1 = !status1.is_dynamic(); + let is_static2 = !status2.is_dynamic(); if is_static1 && is_static2 { continue; @@ -232,8 +261,8 @@ impl InteractionGroups { } let ijoint = interaction.params.type_id(); - let i1 = body1.active_set_offset; - let i2 = body2.active_set_offset; + let i1 = ids1.active_set_offset; + let i2 = ids2.active_set_offset; let conflicts = self.body_masks[i1] | self.body_masks[i2] | joint_type_conflicts[ijoint]; let conflictfree_targets = !(conflicts & occupied_mask); // The & is because we consider empty buckets as free of conflicts. @@ -325,7 +354,8 @@ impl InteractionGroups { pub fn group_manifolds( &mut self, _island_id: usize, - _bodies: &RigidBodySet, + _islands: &IslandManager, + _bodies: &impl ComponentSet<RigidBodyIds>, _interactions: &[&mut ContactManifold], interaction_indices: &[ContactManifoldIndex], ) { @@ -334,21 +364,24 @@ impl InteractionGroups { } #[cfg(feature = "simd-is-enabled")] - pub fn group_manifolds( + pub fn group_manifolds<Bodies>( &mut self, island_id: usize, - bodies: &RigidBodySet, + islands: &IslandManager, + bodies: &Bodies, interactions: &[&mut ContactManifold], interaction_indices: &[ContactManifoldIndex], - ) { + ) where + Bodies: ComponentSet<RigidBodyType> + ComponentSet<RigidBodyIds>, + { // Note: each bit of a body mask indicates what bucket already contains // a constraints involving this body. - // FIXME: currently, this is a bit overconservative because when a bucket + // TODO: currently, this is a bit overconservative because when a bucket // is full, we don't clear the corresponding body mask bit. This may result // in less grouped contacts. // NOTE: body_masks and buckets are already cleared/zeroed at the end of each sort loop. self.body_masks - .resize(bodies.active_island(island_id).len(), 0u128); + .resize(islands.active_island(island_id).len(), 0u128); // NOTE: each bit of the occupied mask indicates what bucket already // contains at least one constraint. @@ -359,31 +392,44 @@ impl InteractionGroups { .max() .unwrap_or(1); - // FIXME: find a way to reduce the number of iteration. + // TODO: find a way to reduce the number of iteration. // There must be a way to iterate just once on every interaction indices // instead of MAX_MANIFOLD_POINTS times. for k in 1..=max_interaction_points { for interaction_i in interaction_indices { let interaction = &interactions[*interaction_i]; - // FIXME: how could we avoid iterating + // TODO: how could we avoid iterating // on each interaction at every iteration on k? if interaction.data.num_active_contacts() != k { continue; } - let body1 = &bodies[interaction.data.body_pair.body1]; - let body2 = &bodies[interaction.data.body_pair.body2]; - let is_static1 = !body1.is_dynamic(); - let is_static2 = !body2.is_dynamic(); + let (status1, active_set_offset1) = if let Some(rb1) = interaction.data.rigid_body1 + { + let data: (_, &RigidBodyIds) = bodies.index_bundle(rb1.0); + (*data.0, data.1.active_set_offset) + } else { + (RigidBodyType::Static, 0) + }; + let (status2, active_set_offset2) = if let Some(rb2) = interaction.data.rigid_body2 + { + let data: (_, &RigidBodyIds) = bodies.index_bundle(rb2.0); + (*data.0, data.1.active_set_offset) + } else { + (RigidBodyType::Static, 0) + }; + + let is_static1 = !status1.is_dynamic(); + let is_static2 = !status2.is_dynamic(); - // FIXME: don't generate interactions between static bodies in the first place. + // TODO: don't generate interactions between static bodies in the first place. if is_static1 && is_static2 { continue; } - let i1 = body1.active_set_offset; - let i2 = body2.active_set_offset; + let i1 = active_set_offset1; + let i2 = active_set_offset2; let conflicts = self.body_masks[i1] | self.body_masks[i2]; let conflictfree_targets = !(conflicts & occupied_mask); // The & is because we consider empty buckets as free of conflicts. let conflictfree_occupied_targets = conflictfree_targets & occupied_mask; diff --git a/src/dynamics/solver/island_solver.rs b/src/dynamics/solver/island_solver.rs index c684cc5..1e65341 100644 --- a/src/dynamics/solver/island_solver.rs +++ b/src/dynamics/solver/island_solver.rs @@ -1,10 +1,15 @@ use super::{PositionSolver, VelocitySolver}; use crate::counters::Counters; +use crate::data::{BundleSet, ComponentSet, ComponentSetMut}; use crate::dynamics::solver::{ AnyJointPositionConstraint, AnyJointVelocityConstraint, AnyPositionConstraint, AnyVelocityConstraint, SolverConstraints, }; -use crate::dynamics::{IntegrationParameters, JointGraphEdge, JointIndex, RigidBodySet}; +use crate::dynamics::{ + IntegrationParameters, JointGraphEdge, JointIndex, RigidBodyDamping, RigidBodyForces, + RigidBodyIds, RigidBodyMassProps, RigidBodyPosition, RigidBodyType, +}; +use crate::dynamics::{IslandManager, RigidBodyVelocity}; use crate::geometry::{ContactManifold, ContactManifoldIndex}; pub struct IslandSolver { @@ -24,17 +29,21 @@ impl IslandSolver { } } - pub fn solve_position_constraints( + pub fn solve_position_constraints<Bodies>( &mut self, island_id: usize, + islands: &IslandManager, counters: &mut Counters, params: &IntegrationParameters, - bodies: &mut RigidBodySet, - ) { + bodies: &mut Bodies, + ) where + Bodies: ComponentSet<RigidBodyIds> + ComponentSetMut<RigidBodyPosition>, + { counters.solver.position_resolution_time.resume(); self.position_solver.solve( island_id, params, + islands, bodies, &self.contact_constraints.position_constraints, &self.joint_constraints.position_constraints, @@ -42,31 +51,47 @@ impl IslandSolver { counters.solver.position_resolution_time.pause(); } - pub fn init_constraints_and_solve_velocity_constraints( + pub fn init_constraints_and_solve_velocity_constraints<Bodies>( &mut self, island_id: usize, counters: &mut Counters, params: &IntegrationParameters, - bodies: &mut RigidBodySet, + islands: &IslandManager, + bodies: &mut Bodies, manifolds: &mut [&mut ContactManifold], manifold_indices: &[ContactManifoldIndex], joints: &mut [JointGraphEdge], joint_indices: &[JointIndex], - ) { + ) where + Bodies: ComponentSet<RigidBodyForces> + + ComponentSetMut<RigidBodyPosition> + + ComponentSetMut<RigidBodyVelocity> + + ComponentSet<RigidBodyMassProps> + + ComponentSet<RigidBodyDamping> + + ComponentSet<RigidBodyIds> + + ComponentSet<RigidBodyType>, + { let has_constraints = manifold_indices.len() != 0 || joint_indices.len() != 0; if has_constraints { counters.solver.velocity_assembly_time.resume(); - self.contact_constraints - .init(island_id, params, bodies, manifolds, manifold_indices); + self.contact_constraints.init( + island_id, + params, + islands, + bodies, + manifolds, + manifold_indices, + ); self.joint_constraints - .init(island_id, params, bodies, joints, joint_indices); + .init(island_id, params, islands, bodies, joints, joint_indices); counters.solver.velocity_assembly_time.pause(); counters.solver.velocity_resolution_time.resume(); self.velocity_solver.solve( island_id, params, + islands, bodies, manifolds, joints, @@ -76,21 +101,50 @@ impl IslandSolver { counters.solver.velocity_resolution_time.pause(); counters.solver.velocity_update_time.resume(); - bodies.foreach_active_island_body_mut_internal(island_id, |_, rb| { - rb.apply_damping(params.dt); - rb.integrate_next_position(params.dt); - }); + + for handle in islands.active_island(island_id) { + let (poss, vels, damping, mprops): ( + &RigidBodyPosition, + &RigidBodyVelocity, + &RigidBodyDamping, + &RigidBodyMassProps, + ) = bodies.index_bundle(handle.0); + + let mut new_poss = *poss; + let new_vels = vels.apply_damping(params.dt, damping); + new_poss.next_position = + vels.integrate(params.dt, &poss.position, &mprops.mass_properties.local_com); + + bodies.set_internal(handle.0, new_vels); + bodies.set_internal(handle.0, new_poss); + } + counters.solver.velocity_update_time.pause(); } else { self.contact_constraints.clear(); self.joint_constraints.clear(); counters.solver.velocity_update_time.resume(); - bodies.foreach_active_island_body_mut_internal(island_id, |_, rb| { + + for handle in islands.active_island(island_id) { // Since we didn't run the velocity solver we need to integrate the accelerations here - rb.integrate_accelerations(params.dt); - rb.apply_damping(params.dt); - rb.integrate_next_position(params.dt); - }); + let (poss, vels, forces, damping, mprops): ( + &RigidBodyPosition, + &RigidBodyVelocity, + &RigidBodyForces, + &RigidBodyDamping, + &RigidBodyMassProps, + ) = bodies.index_bundle(handle.0); + + let mut new_poss = *poss; + let new_vels = forces + .integrate(params.dt, vels, mprops) + .apply_damping(params.dt, &damping); + new_poss.next_position = + vels.integrate(params.dt, &poss.position, &mprops.mass_properties.local_com); + + bodies.set_internal(handle.0, new_vels); + bodies.set_internal(handle.0, new_poss); + } counters.solver.velocity_update_time.pause(); } } diff --git a/src/dynamics/solver/joint_constraint/ball_position_constraint.rs b/src/dynamics/solver/joint_constraint/ball_position_constraint.rs index 93996f4..159cc77 100644 --- a/src/dynamics/solver/joint_constraint/ball_position_constraint.rs +++ b/src/dynamics/solver/joint_constraint/ball_position_constraint.rs @@ -1,4 +1,6 @@ -use crate::dynamics::{BallJoint, IntegrationParameters, RigidBody}; +use crate::dynamics::{ + BallJoint, IntegrationParameters, RigidBodyIds, RigidBodyMassProps, RigidBodyPosition, +}; #[cfg(feature = "dim2")] use crate::math::SdpMatrix; use crate::math::{AngularInertia, Isometry, Point, Real, Rotation}; @@ -23,18 +25,25 @@ pub(crate) struct BallPositionConstraint { } impl BallPositionConstraint { - pub fn from_params(rb1: &RigidBody, rb2: &RigidBody, cparams: &BallJoint) -> Self { + pub fn from_params( + rb1: (&RigidBodyMassProps, &RigidBodyIds), + rb2: (&RigidBodyMassProps, &RigidBodyIds), + cparams: &BallJoint, + ) -> Self { + let (mprops1, ids1) = rb1; + let (mprops2, ids2) = rb2; + Self { - local_com1: rb1.mass_properties.local_com, - local_com2: rb2.mass_properties.local_com, - im1: rb1.effective_inv_mass, - im2: rb2.effective_inv_mass, - ii1: rb1.effective_world_inv_inertia_sqrt.squared(), - ii2: rb2.effective_world_inv_inertia_sqrt.squared(), + local_com1: mprops1.mass_properties.local_com, + local_com2: mprops2.mass_properties.local_com, + im1: mprops1.effective_inv_mass, + im2: mprops2.effective_inv_mass, + ii1: mprops1.effective_world_inv_inertia_sqrt.squared(), + ii2: mprops2.effective_world_inv_inertia_sqrt.squared(), local_anchor1: cparams.local_anchor1, local_anchor2: cparams.local_anchor2, - position1: rb1.active_set_offset, - position2: rb2.active_set_offset, + position1: ids1.active_set_offset, + position2: ids2.active_set_offset, } } @@ -104,31 +113,34 @@ pub(crate) struct BallPositionGroundConstraint { impl BallPositionGroundConstraint { pub fn from_params( - rb1: &RigidBody, - rb2: &RigidBody, + rb1: &RigidBodyPosition, + rb2: (&RigidBodyMassProps, &RigidBodyIds), cparams: &BallJoint, flipped: bool, ) -> Self { + let poss1 = rb1; + let (mprops2, ids2) = rb2; + if flipped { // Note the only thing that is flipped here // are the local_anchors. The rb1 and rb2 have // already been flipped by the caller. Self { - anchor1: rb1.next_position * cparams.local_anchor2, - im2: rb2.effective_inv_mass, - ii2: rb2.effective_world_inv_inertia_sqrt.squared(), + anchor1: poss1.next_position * cparams.local_anchor2, + im2: mprops2.effective_inv_mass, + ii2: mprops2.effective_world_inv_inertia_sqrt.squared(), local_anchor2: cparams.local_anchor1, - position2: rb2.active_set_offset, - local_com2: rb2.mass_properties.local_com, + position2: ids2.active_set_offset, + local_com2: mprops2.mass_properties.local_com, } } else { Self { - anchor1: rb1.next_position * cparams.local_anchor1, - im2: rb2.effective_inv_mass, - ii2: rb2.effective_world_inv_inertia_sqrt.squared(), + anchor1: poss1.next_position * cparams.local_anchor1, + im2: mprops2.effective_inv_mass, + ii2: mprops2.effective_world_inv_inertia_sqrt.squared(), local_anchor2: cparams.local_anchor2, - position2: rb2.active_set_offset, - local_com2: rb2.mass_properties.local_com, + position2: ids2.active_set_offset, + local_com2: mprops2.mass_properties.local_com, } } } diff --git a/src/dynamics/solver/joint_constraint/ball_position_constraint_wide.rs b/src/dynamics/solver/joint_constraint/ball_position_constraint_wide.rs index e9162a4..6b00639 100644 --- a/src/dynamics/solver/joint_constraint/ball_position_constraint_wide.rs +++ b/src/dynamics/solver/joint_constraint/ball_position_constraint_wide.rs @@ -1,4 +1,6 @@ -use crate::dynamics::{BallJoint, IntegrationParameters, RigidBody}; +use crate::dynamics::{ + BallJoint, IntegrationParameters, RigidBodyIds, RigidBodyMassProps, RigidBodyPosition, +}; #[cfg(feature = "dim2")] use crate::math::SdpMatrix; use crate::math::{AngularInertia, Isometry, Point, Real, Rotation, SimdReal, SIMD_WIDTH}; @@ -25,26 +27,35 @@ pub(crate) struct WBallPositionConstraint { impl WBallPositionConstraint { pub fn from_params( - rbs1: [&RigidBody; SIMD_WIDTH], - rbs2: [&RigidBody; SIMD_WIDTH], + rbs1: ( + [&RigidBodyMassProps; SIMD_WIDTH], + [&RigidBodyIds; SIMD_WIDTH], + ), + rbs2: ( + [&RigidBodyMassProps; SIMD_WIDTH], + [&RigidBodyIds; SIMD_WIDTH], + ), < |