diff options
Diffstat (limited to 'src/pipeline')
| -rw-r--r-- | src/pipeline/collision_pipeline.rs | 20 | ||||
| -rw-r--r-- | src/pipeline/physics_pipeline.rs | 57 | ||||
| -rw-r--r-- | src/pipeline/query_pipeline.rs | 129 | ||||
| -rw-r--r-- | src/pipeline/user_changes.rs | 224 |
4 files changed, 253 insertions, 177 deletions
diff --git a/src/pipeline/collision_pipeline.rs b/src/pipeline/collision_pipeline.rs index ac92a04..b086fbe 100644 --- a/src/pipeline/collision_pipeline.rs +++ b/src/pipeline/collision_pipeline.rs @@ -5,7 +5,7 @@ use crate::geometry::{ BroadPhase, BroadPhasePairEvent, ColliderChanges, ColliderHandle, ColliderPair, NarrowPhase, }; use crate::math::Real; -use crate::pipeline::{EventHandler, PhysicsHooks}; +use crate::pipeline::{EventHandler, PhysicsHooks, QueryPipeline}; use crate::{dynamics::RigidBodySet, geometry::ColliderSet}; /// The collision pipeline, responsible for performing collision detection between colliders. @@ -111,6 +111,7 @@ impl CollisionPipeline { narrow_phase: &mut NarrowPhase, bodies: &mut RigidBodySet, colliders: &mut ColliderSet, + query_pipeline: Option<&mut QueryPipeline>, hooks: &dyn PhysicsHooks, events: &dyn EventHandler, ) { @@ -127,9 +128,20 @@ impl CollisionPipeline { None, bodies, colliders, + &mut ImpulseJointSet::new(), + &mut MultibodyJointSet::new(), &modified_bodies, &mut modified_colliders, ); + + // Disabled colliders are treated as if they were removed. + removed_colliders.extend( + modified_colliders + .iter() + .copied() + .filter(|h| colliders.get(*h).map(|c| !c.is_enabled()).unwrap_or(false)), + ); + self.detect_collisions( prediction_distance, broad_phase, @@ -143,6 +155,10 @@ impl CollisionPipeline { true, ); + if let Some(queries) = query_pipeline { + queries.update_incremental(colliders, &modified_colliders, &removed_colliders, true); + } + self.clear_modified_colliders(colliders, &mut modified_colliders); removed_colliders.clear(); } @@ -187,6 +203,7 @@ mod tests { &mut narrow_phase, &mut rigid_body_set, &mut collider_set, + None, &physics_hooks, &(), ); @@ -238,6 +255,7 @@ mod tests { &mut narrow_phase, &mut rigid_body_set, &mut collider_set, + None, &physics_hooks, &(), ); diff --git a/src/pipeline/physics_pipeline.rs b/src/pipeline/physics_pipeline.rs index 391b39a..358efed 100644 --- a/src/pipeline/physics_pipeline.rs +++ b/src/pipeline/physics_pipeline.rs @@ -5,7 +5,7 @@ use crate::counters::Counters; use crate::dynamics::IslandSolver; use crate::dynamics::{ CCDSolver, ImpulseJointSet, IntegrationParameters, IslandManager, MultibodyJointSet, - RigidBodyPosition, RigidBodyType, + RigidBodyChanges, RigidBodyHandle, RigidBodyPosition, RigidBodyType, }; #[cfg(feature = "parallel")] use crate::dynamics::{JointGraphEdge, ParallelIslandSolver as IslandSolver}; @@ -14,7 +14,7 @@ use crate::geometry::{ ContactManifoldIndex, NarrowPhase, TemporaryInteractionIndex, }; use crate::math::{Real, Vector}; -use crate::pipeline::{EventHandler, PhysicsHooks}; +use crate::pipeline::{EventHandler, PhysicsHooks, QueryPipeline}; use {crate::dynamics::RigidBodySet, crate::geometry::ColliderSet}; /// The physics pipeline, responsible for stepping the whole physics simulation. @@ -77,6 +77,18 @@ impl PhysicsPipeline { } } + fn clear_modified_bodies( + &mut self, + bodies: &mut RigidBodySet, + modified_bodies: &mut Vec<RigidBodyHandle>, + ) { + for handle in modified_bodies.drain(..) { + if let Some(rb) = bodies.get_mut_internal(handle) { + rb.changes = RigidBodyChanges::empty(); + } + } + } + fn detect_collisions( &mut self, integration_parameters: &IntegrationParameters, @@ -404,6 +416,7 @@ impl PhysicsPipeline { impulse_joints: &mut ImpulseJointSet, multibody_joints: &mut MultibodyJointSet, ccd_solver: &mut CCDSolver, + mut query_pipeline: Option<&mut QueryPipeline>, hooks: &dyn PhysicsHooks, events: &dyn EventHandler, ) { @@ -422,21 +435,34 @@ impl PhysicsPipeline { // Apply modifications. let mut modified_colliders = colliders.take_modified(); let mut removed_colliders = colliders.take_removed(); + super::user_changes::handle_user_changes_to_colliders( bodies, colliders, &modified_colliders[..], ); - let modified_bodies = bodies.take_modified(); + let mut modified_bodies = bodies.take_modified(); super::user_changes::handle_user_changes_to_rigid_bodies( Some(islands), bodies, colliders, + impulse_joints, + multibody_joints, &modified_bodies, &mut modified_colliders, ); + // Disabled colliders are treated as if they were removed. + // NOTE: this must be called here, after handle_user_changes_to_rigid_bodies to take into + // account colliders disabled because of their parent rigid-body. + removed_colliders.extend( + modified_colliders + .iter() + .copied() + .filter(|h| colliders.get(*h).map(|c| !c.is_enabled()).unwrap_or(false)), + ); + // TODO: do this only on user-change. // TODO: do we want some kind of automatic inverse kinematics? for multibody in &mut multibody_joints.multibodies { @@ -455,14 +481,19 @@ impl PhysicsPipeline { colliders, impulse_joints, multibody_joints, - &modified_colliders[..], - &mut removed_colliders, + &modified_colliders, + &removed_colliders, hooks, events, true, ); + if let Some(queries) = query_pipeline.as_deref_mut() { + queries.update_incremental(colliders, &modified_colliders, &removed_colliders, false); + } + self.clear_modified_colliders(colliders, &mut modified_colliders); + self.clear_modified_bodies(bodies, &mut modified_bodies); removed_colliders.clear(); let mut remaining_time = integration_parameters.dt; @@ -582,13 +613,22 @@ impl PhysicsPipeline { colliders, impulse_joints, multibody_joints, - &mut modified_colliders, - &mut removed_colliders, + &modified_colliders, + &[], hooks, events, false, ); + if let Some(queries) = query_pipeline.as_deref_mut() { + queries.update_incremental( + colliders, + &modified_colliders, + &[], + remaining_substeps == 0, + ); + } + self.clear_modified_colliders(colliders, &mut modified_colliders); } @@ -650,6 +690,7 @@ mod test { &mut impulse_joints, &mut multibody_joints, &mut CCDSolver::new(), + None, &(), &(), ); @@ -705,6 +746,7 @@ mod test { &mut impulse_joints, &mut multibody_joints, &mut CCDSolver::new(), + None, &(), &(), ); @@ -807,6 +849,7 @@ mod test { &mut impulse_joints, &mut multibody_joints, &mut ccd, + None, &physics_hooks, &event_handler, ); diff --git a/src/pipeline/query_pipeline.rs b/src/pipeline/query_pipeline.rs index 00825e2..a248426 100644 --- a/src/pipeline/query_pipeline.rs +++ b/src/pipeline/query_pipeline.rs @@ -1,10 +1,10 @@ -use crate::dynamics::{IslandManager, RigidBodyHandle}; +use crate::dynamics::RigidBodyHandle; use crate::geometry::{ Aabb, Collider, ColliderHandle, InteractionGroups, PointProjection, Qbvh, Ray, RayIntersection, }; use crate::math::{Isometry, Point, Real, Vector}; use crate::{dynamics::RigidBodySet, geometry::ColliderSet}; -use parry::partitioning::QbvhDataGenerator; +use parry::partitioning::{QbvhDataGenerator, QbvhUpdateWorkspace}; use parry::query::details::{ IntersectionCompositeShapeShapeBestFirstVisitor, NonlinearTOICompositeShapeShapeBestFirstVisitor, PointCompositeShapeProjBestFirstVisitor, @@ -30,8 +30,9 @@ pub struct QueryPipeline { )] query_dispatcher: Arc<dyn QueryDispatcher>, qbvh: Qbvh<ColliderHandle>, - tree_built: bool, dilation_factor: Real, + #[cfg_attr(feature = "serde-serialize", serde(skip))] + workspace: QbvhUpdateWorkspace, } struct QueryPipelineAsCompositeShape<'a> { @@ -310,8 +311,8 @@ impl QueryPipeline { Self { query_dispatcher: Arc::new(d), qbvh: Qbvh::new(), - tree_built: false, dilation_factor: 0.01, + workspace: QbvhUpdateWorkspace::default(), } } @@ -320,25 +321,46 @@ impl QueryPipeline { &*self.query_dispatcher } - /// Update the acceleration structure on the query pipeline. - pub fn update( + /// Update the query pipeline incrementally, avoiding a complete rebuild of its + /// internal data-structure. + pub fn update_incremental( &mut self, - islands: &IslandManager, - bodies: &RigidBodySet, colliders: &ColliderSet, + modified_colliders: &[ColliderHandle], + removed_colliders: &[ColliderHandle], + refit_and_rebalance: bool, ) { - self.update_with_mode( - islands, - bodies, - colliders, - QueryPipelineMode::CurrentPosition, - ) + // We remove first. This is needed to avoid the ABA problem: if a collider was removed + // and another added right after with the same handle index, we can remove first, and + // then update the new one (but only if its actually exists, to address the case where + // a collider was added/modified and then removed during the same frame). + for removed in removed_colliders { + self.qbvh.remove(*removed); + } + + for modified in modified_colliders { + // Check that the collider still exists as it may have been removed. + if colliders.contains(*modified) { + self.qbvh.pre_update_or_insert(*modified); + } + } + + if refit_and_rebalance { + let _ = self.qbvh.refit(0.0, &mut self.workspace, |handle| { + colliders[*handle].compute_aabb() + }); + self.qbvh.rebalance(0.0, &mut self.workspace); + } + } + + /// Update the acceleration structure on the query pipeline. + pub fn update(&mut self, bodies: &RigidBodySet, colliders: &ColliderSet) { + self.update_with_mode(bodies, colliders, QueryPipelineMode::CurrentPosition) } /// Update the acceleration structure on the query pipeline. pub fn update_with_mode( &mut self, - islands: &IslandManager, bodies: &RigidBodySet, colliders: &ColliderSet, mode: QueryPipelineMode, @@ -358,12 +380,12 @@ impl QueryPipeline { fn for_each(&mut self, mut f: impl FnMut(ColliderHandle, Aabb)) { match self.mode { QueryPipelineMode::CurrentPosition => { - for (h, co) in self.colliders.iter() { + for (h, co) in self.colliders.iter_enabled() { f(h, co.shape.compute_aabb(&co.pos)) } } QueryPipelineMode::SweepTestWithNextPosition => { - for (h, co) in self.colliders.iter() { + for (h, co) in self.colliders.iter_enabled() { if let Some(co_parent) = co.parent { let rb_next_pos = &self.bodies[co_parent.handle].pos.next_position; let next_position = rb_next_pos * co_parent.pos_wrt_parent; @@ -374,7 +396,7 @@ impl QueryPipeline { } } QueryPipelineMode::SweepTestWithPredictedPosition { dt } => { - for (h, co) in self.colliders.iter() { + for (h, co) in self.colliders.iter_enabled() { if let Some(co_parent) = co.parent { let rb = &self.bodies[co_parent.handle]; let predicted_pos = rb.pos.integrate_forces_and_velocities( @@ -392,71 +414,12 @@ impl QueryPipeline { } } - if !self.tree_built { - let generator = DataGenerator { - bodies, - colliders, - mode, - }; - self.qbvh.clear_and_rebuild(generator, self.dilation_factor); - - // FIXME: uncomment this once we handle insertion/removals properly. - // self.tree_built = true; - return; - } - - for handle in islands.iter_active_bodies() { - let rb = &bodies[handle]; - for handle in &rb.colliders.0 { - self.qbvh.pre_update(*handle) - } - } - - match mode { - QueryPipelineMode::CurrentPosition => { - self.qbvh.update( - |handle| { - let co = &colliders[*handle]; - co.shape.compute_aabb(&co.pos) - }, - self.dilation_factor, - ); - } - QueryPipelineMode::SweepTestWithNextPosition => { - self.qbvh.update( - |handle| { - let co = &colliders[*handle]; - if let Some(parent) = &co.parent { - let rb_next_pos = &bodies[parent.handle].pos.next_position; - let next_position = rb_next_pos * parent.pos_wrt_parent; - co.shape.compute_swept_aabb(&co.pos, &next_position) - } else { - co.shape.compute_aabb(&co.pos) - } - }, - self.dilation_factor, - ); - } - QueryPipelineMode::SweepTestWithPredictedPosition { dt } => { - self.qbvh.update( - |handle| { - let co = &colliders[*handle]; - if let Some(parent) = co.parent { - let rb = &bodies[parent.handle]; - let predicted_pos = rb.pos.integrate_forces_and_velocities( - dt, &rb.forces, &rb.vels, &rb.mprops, - ); - - let next_position = predicted_pos * parent.pos_wrt_parent; - co.shape.compute_swept_aabb(&co.pos, &next_position) - } else { - co.shape.compute_aabb(&co.pos) - } - }, - self.dilation_factor, - ); - } - } + let generator = DataGenerator { + bodies, + colliders, + mode, + }; + self.qbvh.clear_and_rebuild(generator, self.dilation_factor); } /// Find the closest intersection between a ray and a set of collider. diff --git a/src/pipeline/user_changes.rs b/src/pipeline/user_changes.rs index e7da68a..eeda97a 100644 --- a/src/pipeline/user_changes.rs +++ b/src/pipeline/user_changes.rs @@ -1,7 +1,10 @@ use crate::dynamics::{ - IslandManager, RigidBodyChanges, RigidBodyHandle, RigidBodySet, RigidBodyType, + ImpulseJointSet, IslandManager, JointEnabled, MultibodyJointSet, RigidBodyChanges, + RigidBodyHandle, RigidBodySet, RigidBodyType, +}; +use crate::geometry::{ + ColliderChanges, ColliderEnabled, ColliderHandle, ColliderPosition, ColliderSet, }; -use crate::geometry::{ColliderChanges, ColliderHandle, ColliderPosition, ColliderSet}; pub(crate) fn handle_user_changes_to_colliders( bodies: &mut RigidBodySet, @@ -21,10 +24,12 @@ pub(crate) fn handle_user_changes_to_colliders( } } - if co - .changes - .intersects(ColliderChanges::SHAPE | ColliderChanges::LOCAL_MASS_PROPERTIES) - { + if co.changes.intersects( + ColliderChanges::SHAPE + | ColliderChanges::LOCAL_MASS_PROPERTIES + | ColliderChanges::ENABLED_OR_DISABLED + | ColliderChanges::PARENT, + ) { if let Some(rb) = co .parent .and_then(|p| bodies.get_mut_internal_with_modification_tracking(p.handle)) @@ -40,12 +45,15 @@ pub(crate) fn handle_user_changes_to_rigid_bodies( mut islands: Option<&mut IslandManager>, bodies: &mut RigidBodySet, colliders: &mut ColliderSet, + impulse_joints: &mut ImpulseJointSet, + _multibody_joints: &mut MultibodyJointSet, // FIXME: propagate disabled state to multibodies modified_bodies: &[RigidBodyHandle], modified_colliders: &mut Vec<ColliderHandle>, ) { enum FinalAction { - UpdateActiveKinematicSetId, - UpdateActiveDynamicSetId, + UpdateActiveKinematicSetId(usize), + UpdateActiveDynamicSetId(usize), + RemoveFromIsland, } for handle in modified_bodies { @@ -57,94 +65,92 @@ pub(crate) fn handle_user_changes_to_rigid_bodies( } let rb = bodies.index_mut_internal(*handle); - let mut changes = rb.changes; let mut ids = rb.ids; - let mut activation = rb.activation; + let changes = rb.changes; + let activation = rb.activation; { - // The body's status changed. We need to make sure - // it is on the correct active set. - if let Some(islands) = islands.as_deref_mut() { - if changes.contains(RigidBodyChanges::TYPE) { - match rb.body_type { - RigidBodyType::Dynamic => { - // Remove from the active kinematic set if it was there. - if islands.active_kinematic_set.get(ids.active_set_id) == Some(handle) { - islands.active_kinematic_set.swap_remove(ids.active_set_id); - final_action = Some(( - FinalAction::UpdateActiveKinematicSetId, - ids.active_set_id, - )); - } - - // Add to the active dynamic set. - activation.wake_up(true); - // Make sure the sleep change flag is set (even if for some - // reasons the rigid-body was already awake) to make - // sure the code handling sleeping change adds the body to - // the active_dynamic_set. - changes.set(RigidBodyChanges::SLEEP, true); - } - RigidBodyType::KinematicVelocityBased - | RigidBodyType::KinematicPositionBased => { - // Remove from the active dynamic set if it was there. - if islands.active_dynamic_set.get(ids.active_set_id) == Some(handle) { - islands.active_dynamic_set.swap_remove(ids.active_set_id); - final_action = Some(( - FinalAction::UpdateActiveDynamicSetId, - ids.active_set_id, - )); + if rb.is_enabled() { + // The body's status changed. We need to make sure + // it is on the correct active set. + if let Some(islands) = islands.as_deref_mut() { + if changes.contains(RigidBodyChanges::TYPE) { + match rb.body_type { + RigidBodyType::Dynamic => { + // Remove from the active kinematic set if it was there. + if islands.active_kinematic_set.get(ids.active_set_id) + == Some(handle) + { + islands.active_kinematic_set.swap_remove(ids.active_set_id); + final_action = Some(FinalAction::UpdateActiveKinematicSetId( + ids.active_set_id, + )); + } } - - // Add to the active kinematic set. - if islands.active_kinematic_set.get(ids.active_set_id) != Some(handle) { - ids.active_set_id = islands.active_kinematic_set.len(); - islands.active_kinematic_set.push(*handle); + RigidBodyType::KinematicVelocityBased + | RigidBodyType::KinematicPositionBased => { + // Remove from the active dynamic set if it was there. + if islands.active_dynamic_set.get(ids.active_set_id) == Some(handle) + { + islands.active_dynamic_set.swap_remove(ids.active_set_id); + final_action = Some(FinalAction::UpdateActiveDynamicSetId( + ids.active_set_id, + )); + } + + // Add to the active kinematic set. + if islands.active_kinematic_set.get(ids.active_set_id) + != Some(handle) + { + ids.active_set_id = islands.active_kinematic_set.len(); + islands.active_kinematic_set.push(*handle); + } } + RigidBodyType::Fixed => {} } - RigidBodyType::Fixed => {} } - } - // Update the positions of the colliders. - if changes.contains(RigidBodyChanges::POSITION) - || changes.contains(RigidBodyChanges::COLLIDERS) - { - rb.colliders - .update_positions(colliders, modified_colliders, &rb.pos.position); + // Update the active kinematic set. + if changes.contains(RigidBodyChanges::POSITION) + || changes.contains(RigidBodyChanges::COLLIDERS) + { + if rb.is_kinematic() + && islands.active_kinematic_set.get(ids.active_set_id) != Some(handle) + { + ids.active_set_id = islands.active_kinematic_set.len(); + islands.active_kinematic_set.push(*handle); + } + } - if rb.is_kinematic() - && islands.active_kinematic_set.get(ids.active_set_id) != Some(handle) + // Push the body to the active set if it is not + // sleeping and if it is not already inside of the active set. + if changes.contains(RigidBodyChanges::SLEEP) + && rb.is_enabled() + && !rb.activation.sleeping // May happen if the body was put to sleep manually. + && rb.is_dynamic() // Only dynamic bodies are in the active dynamic set. + && islands.active_dynamic_set.get(ids.active_set_id) != Some(handle) { - ids.active_set_id = islands.active_kinematic_set.len(); - islands.active_kinematic_set.push(*handle); + ids.active_set_id = islands.active_dynamic_set.len(); // This will handle the case where the activation_channel contains duplicates. + islands.active_dynamic_set.push(*handle); } } + } - // Push the body to the active set if it is not - // sleeping and if it is not already inside of the active set. - if changes.contains(RigidBodyChanges::SLEEP) - && !activation.sleeping // May happen if the body was put to sleep manually. - && rb.is_dynamic() // Only dynamic bodies are in the active dynamic set. - && islands.active_dynamic_set.get(ids.active_set_id) != Some(handle) - { - ids.active_set_id = islands.active_dynamic_set.len(); // This will handle the case where the activation_channel contains duplicates. - islands.active_dynamic_set.push(*handle); - } - } else { - // We don't use islands. So just update the colliders' positions. - if changes.contains(RigidBodyChanges::POSITION) - || changes.contains(RigidBodyChanges::COLLIDERS) - { - rb.colliders - .update_positions(colliders, modified_colliders, &rb.pos.position); - } + // Update the colliders' positions. + if changes.contains(RigidBodyChanges::POSITION) + || changes.contains(RigidBodyChanges::COLLIDERS) + { + rb.colliders + .update_positions(colliders, modified_colliders, &rb.pos.position); } if changes.contains(RigidBodyChanges::DOMINANCE) || changes.contains(RigidBodyChanges::TYPE) { for handle in rb.colliders.0.iter() { + // NOTE: we can’t just use `colliders.get_mut_internal_with_modification_tracking` + // here because that would modify the `modified_colliders` inside of the `ColliderSet` + // instead of the one passed to this method. let co = colliders.index_mut_internal(*handle); if !co.changes.contains(ColliderChanges::MODIFIED) { modified_colliders.push(*handle); @@ -165,22 +171,68 @@ pub(crate) fn handle_user_changes_to_rigid_bodies( ); } - rb.changes = RigidBodyChanges::empty(); + if changes.contains(RigidBodyChanges::ENABLED_OR_DISABLED) { + // Propagate the rigid-body’s enabled/disable status to its colliders. + for handle in rb.colliders.0.iter() { + // NOTE: we can’t just use `colliders.get_mut_internal_with_modification_tracking` + // here because that would modify the `modified_colliders` inside of the `ColliderSet` + // instead of the one passed to this method. + let co = colliders.index_mut_internal(*handle); + if !co.changes.contains(ColliderChanges::MODIFIED) { + modified_colliders.push(*handle); + } + + if rb.enabled && co.flags.enabled == ColliderEnabled::DisabledByParent { + co.flags.enabled = ColliderEnabled::Enabled; + } else if !rb.enabled && co.flags.enabled == ColliderEnabled::Enabled { + co.flags.enabled = ColliderEnabled::DisabledByParent; + } + + co.changes |= ColliderChanges::MODIFIED | ColliderChanges::ENABLED_OR_DISABLED; + } + + // Propagate the rigid-body’s enabled/disable status to its attached impulse joints. + impulse_joints.map_attached_joints_mut(*handle, |_, _, _, joint| { + if rb.enabled && joint.data.enabled == JointEnabled::DisabledByAttachedBody { + joint.data.enabled = JointEnabled::Enabled; + } else if !rb.enabled && joint.data.enabled == JointEnabled::Enabled { + joint.data.enabled = JointEnabled::DisabledByAttachedBody; + } + }); + + // FIXME: Propagate the rigid-body’s enabled/disable status to its attached multibody joints. + + // Remove the rigid-body from the island manager. + if !rb.enabled { + final_action = Some(FinalAction::RemoveFromIsland); + } + } + rb.ids = ids; rb.activation = activation; } // Adjust some ids, if needed. if let Some(islands) = islands.as_deref_mut() { - if let Some((action, id)) = final_action { - let active_set = match action { - FinalAction::UpdateActiveKinematicSetId => &mut islands.active_kinematic_set, - FinalAction::UpdateActiveDynamicSetId => &mut islands.active_dynamic_set, + if let Some(action) = final_action { + match action { + FinalAction::RemoveFromIsland => { + let ids = rb.ids; + islands.rigid_body_removed(*handle, &ids, bodies); + } + FinalAction::UpdateActiveKinematicSetId(id) => { + let active_set = &mut islands.active_kinematic_set; + if id < active_set.len() { + bodies.index_mut_internal(active_set[id]).ids.active_set_id = id; + } + } + FinalAction::UpdateActiveDynamicSetId(id) => { + let active_set = &mut islands.active_dynamic_set; + if id < active_set.len() { + bodies.index_mut_internal(active_set[id]).ids.active_set_id = id; + } + } }; - - if id < active_set.len() { - bodies.index_mut_internal(active_set[id]).ids.active_set_id = id; - } } } } |