diff options
| author | Crozet Sébastien <developer@crozet.re> | 2021-04-29 10:26:44 +0200 |
|---|---|---|
| committer | Crozet Sébastien <developer@crozet.re> | 2021-04-29 10:26:44 +0200 |
| commit | 705876f5e595a0a311b5c73cd71705c93f4f23d8 (patch) | |
| tree | f89e12e33d6624853f7fa86787f667a205482a50 | |
| parent | 83cb981a88772626dcfb091430e8c3dba62ba1e8 (diff) | |
| download | rapier-705876f5e595a0a311b5c73cd71705c93f4f23d8.tar.gz rapier-705876f5e595a0a311b5c73cd71705c93f4f23d8.tar.bz2 rapier-705876f5e595a0a311b5c73cd71705c93f4f23d8.zip | |
Restore the collision pipeline
| -rw-r--r-- | src/geometry/narrow_phase.rs | 74 | ||||
| -rw-r--r-- | src/pipeline/collision_pipeline.rs | 179 | ||||
| -rw-r--r-- | src/pipeline/physics_pipeline.rs | 12 | ||||
| -rw-r--r-- | src/pipeline/query_pipeline.rs | 16 | ||||
| -rw-r--r-- | src/pipeline/user_changes.rs | 140 |
5 files changed, 306 insertions, 115 deletions
diff --git a/src/geometry/narrow_phase.rs b/src/geometry/narrow_phase.rs index bb402da..57504f5 100644 --- a/src/geometry/narrow_phase.rs +++ b/src/geometry/narrow_phase.rs @@ -174,7 +174,7 @@ impl NarrowPhase { /// Maintain the narrow-phase internal state by taking collider removal into account. pub fn handle_user_changes<Bodies, Colliders>( &mut self, - islands: &mut IslandManager, + mut islands: Option<&mut IslandManager>, modified_colliders: &[ColliderHandle], removed_colliders: &[ColliderHandle], colliders: &mut Colliders, @@ -210,7 +210,7 @@ impl NarrowPhase { self.remove_collider( intersection_graph_id, contact_graph_id, - islands, + islands.as_deref_mut(), colliders, bodies, &mut prox_id_remap, @@ -226,7 +226,7 @@ impl NarrowPhase { &mut self, intersection_graph_id: ColliderGraphIndex, contact_graph_id: ColliderGraphIndex, - islands: &mut IslandManager, + islands: Option<&mut IslandManager>, colliders: &mut Colliders, bodies: &mut Bodies, prox_id_remap: &mut HashMap<ColliderHandle, ColliderGraphIndex>, @@ -238,13 +238,15 @@ impl NarrowPhase { Colliders: ComponentSetOption<ColliderParent>, { // Wake up every body in contact with the deleted collider. - for (a, b, _) in self.contact_graph.interactions_with(contact_graph_id) { - if let Some(parent) = colliders.get(a.0).map(|c| c.handle) { - islands.wake_up(bodies, parent, true) - } + if let Some(islands) = islands { + for (a, b, _) in self.contact_graph.interactions_with(contact_graph_id) { + if let Some(parent) = colliders.get(a.0).map(|c| c.handle) { + islands.wake_up(bodies, parent, true) + } - if let Some(parent) = colliders.get(b.0).map(|c| c.handle) { - islands.wake_up(bodies, parent, true) + if let Some(parent) = colliders.get(b.0).map(|c| c.handle) { + islands.wake_up(bodies, parent, true) + } } } @@ -269,7 +271,7 @@ impl NarrowPhase { pub(crate) fn handle_modified_colliders<Bodies, Colliders>( &mut self, - islands: &mut IslandManager, + mut islands: Option<&mut IslandManager>, modified_colliders: &[ColliderHandle], colliders: &Colliders, bodies: &mut Bodies, @@ -305,19 +307,22 @@ impl NarrowPhase { let (co_changes, co_type): (&ColliderChanges, &ColliderType) = colliders.index_bundle(handle.0); - if let Some(co_parent) = co_parent { - islands.wake_up(bodies, co_parent.handle, true); - } + if let Some(islands) = islands.as_deref_mut() { + if let Some(co_parent) = co_parent { + islands.wake_up(bodies, co_parent.handle, true); + } - for inter in self - .contact_graph - .interactions_with(gid.contact_graph_index) - { - let other_handle = if *handle == inter.0 { inter.1 } else { inter.0 }; - let other_parent: Option<&ColliderParent> = colliders.get(other_handle.0); + for inter in self + .contact_graph + .interactions_with(gid.contact_graph_index) + { + let other_handle = if *handle == inter.0 { inter.1 } else { inter.0 }; + let other_parent: Option<&ColliderParent> = + colliders.get(other_handle.0); - if let Some(other_parent) = other_parent { - islands.wake_up(bodies, other_parent.handle, true); + if let Some(other_parent) = other_parent { + islands.wake_up(bodies, other_parent.handle, true); + } } } @@ -364,7 +369,14 @@ impl NarrowPhase { // Remove the pair from the relevant graph. for pair in &pairs_to_remove { - self.remove_pair(islands, colliders, bodies, &pair.0, events, pair.1); + self.remove_pair( + islands.as_deref_mut(), + colliders, + bodies, + &pair.0, + events, + pair.1, + ); } // Add the paid removed pair to the relevant graph. @@ -375,7 +387,7 @@ impl NarrowPhase { fn remove_pair<Bodies, Colliders>( &mut self, - islands: &mut IslandManager, + islands: Option<&mut IslandManager>, colliders: &Colliders, bodies: &mut Bodies, pair: &ColliderPair, @@ -425,12 +437,14 @@ impl NarrowPhase { let co_parent2: Option<&ColliderParent> = colliders.get(pair.collider2.0); - if let Some(co_parent1) = co_parent1 { - islands.wake_up(bodies, co_parent1.handle, true); - } + if let Some(islands) = islands { + if let Some(co_parent1) = co_parent1 { + islands.wake_up(bodies, co_parent1.handle, true); + } - if let Some(co_parent2) = co_parent2 { - islands.wake_up(bodies, co_parent2.handle, true); + if let Some(co_parent2) = co_parent2 { + islands.wake_up(bodies, co_parent2.handle, true); + } } events.handle_contact_event(ContactEvent::Stopped( @@ -526,7 +540,7 @@ impl NarrowPhase { pub(crate) fn register_pairs<Bodies, Colliders>( &mut self, - islands: &mut IslandManager, + mut islands: Option<&mut IslandManager>, colliders: &Colliders, bodies: &mut Bodies, broad_phase_events: &[BroadPhasePairEvent], @@ -544,7 +558,7 @@ impl NarrowPhase { } BroadPhasePairEvent::DeletePair(pair) => { self.remove_pair( - islands, + islands.as_deref_mut(), colliders, bodies, pair, diff --git a/src/pipeline/collision_pipeline.rs b/src/pipeline/collision_pipeline.rs index 074ba75..c9ab119 100644 --- a/src/pipeline/collision_pipeline.rs +++ b/src/pipeline/collision_pipeline.rs @@ -1,11 +1,15 @@ //! Physics pipeline structures. -use crate::data::{ComponentSet, ComponentSetMut}; +use crate::data::{ComponentSet, ComponentSetMut, ComponentSetOption}; use crate::dynamics::{ - IslandManager, JointSet, RigidBodyActivation, RigidBodyColliders, RigidBodyDominance, - RigidBodyIds, RigidBodyType, RigidBodyVelocity, + RigidBodyActivation, RigidBodyChanges, RigidBodyColliders, RigidBodyDominance, RigidBodyHandle, + RigidBodyIds, RigidBodyPosition, RigidBodySet, RigidBodyType, RigidBodyVelocity, +}; +use crate::geometry::{ + BroadPhase, BroadPhasePairEvent, ColliderBroadPhaseData, ColliderChanges, ColliderGroups, + ColliderHandle, ColliderMaterial, ColliderPair, ColliderParent, ColliderPosition, ColliderSet, + ColliderShape, ColliderType, NarrowPhase, }; -use crate::geometry::{BroadPhase, BroadPhasePairEvent, ColliderPair, ColliderShape, NarrowPhase}; use crate::math::Real; use crate::pipeline::{EventHandler, PhysicsHooks}; @@ -18,7 +22,6 @@ use crate::pipeline::{EventHandler, PhysicsHooks}; pub struct CollisionPipeline { broadphase_collider_pairs: Vec<ColliderPair>, broad_phase_events: Vec<BroadPhasePairEvent>, - empty_joints: JointSet, } #[allow(dead_code)] @@ -33,30 +36,168 @@ impl CollisionPipeline { CollisionPipeline { broadphase_collider_pairs: Vec::new(), broad_phase_events: Vec::new(), - empty_joints: JointSet::new(), + } + } + + fn detect_collisions<Bodies, Colliders>( + &mut self, + prediction_distance: Real, + broad_phase: &mut BroadPhase, + narrow_phase: &mut NarrowPhase, + bodies: &mut Bodies, + colliders: &mut Colliders, + modified_colliders: &[ColliderHandle], + removed_colliders: &[ColliderHandle], + hooks: &dyn PhysicsHooks<Bodies, Colliders>, + events: &dyn EventHandler, + handle_user_changes: bool, + ) where + Bodies: ComponentSetMut<RigidBodyActivation> + + ComponentSet<RigidBodyType> + + ComponentSetMut<RigidBodyIds> + + ComponentSet<RigidBodyDominance>, + Colliders: ComponentSetMut<ColliderBroadPhaseData> + + ComponentSet<ColliderChanges> + + ComponentSet<ColliderPosition> + + ComponentSet<ColliderShape> + + ComponentSetOption<ColliderParent> + + ComponentSet<ColliderType> + + ComponentSet<ColliderGroups> + + ComponentSet<ColliderMaterial>, + { + // Update broad-phase. + self.broad_phase_events.clear(); + self.broadphase_collider_pairs.clear(); + + broad_phase.update( + prediction_distance, + colliders, + modified_colliders, + removed_colliders, + &mut self.broad_phase_events, + ); + + // Update narrow-phase. + if handle_user_changes { + narrow_phase.handle_user_changes( + None, + modified_colliders, + removed_colliders, + colliders, + bodies, + events, + ); + } + + narrow_phase.register_pairs(None, colliders, bodies, &self.broad_phase_events, events); + narrow_phase.compute_contacts( + prediction_distance, + bodies, + colliders, + modified_colliders, + hooks, + events, + ); + narrow_phase.compute_intersections(bodies, colliders, modified_colliders, hooks, events); + } + + fn clear_modified_colliders( + &mut self, + colliders: &mut impl ComponentSetMut<ColliderChanges>, + modified_colliders: &mut Vec<ColliderHandle>, + ) { + for handle in modified_colliders.drain(..) { + colliders.set_internal(handle.0, ColliderChanges::empty()) } } /// Executes one step of the collision detection. - pub fn step<Bodies, Colliders>( + #[cfg(feature = "default-sets")] + pub fn step( &mut self, - _prediction_distance: Real, - _broad_phase: &mut BroadPhase, - _narrow_phase: &mut NarrowPhase, - _islands: &mut IslandManager, - _bodies: &mut Bodies, - _colliders: &mut Colliders, - _hooks: &dyn PhysicsHooks<Bodies, Colliders>, - _events: &dyn EventHandler, + prediction_distance: Real, + broad_phase: &mut BroadPhase, + narrow_phase: &mut NarrowPhase, + bodies: &mut RigidBodySet, + colliders: &mut ColliderSet, + hooks: &dyn PhysicsHooks<RigidBodySet, ColliderSet>, + events: &dyn EventHandler, + ) { + let mut modified_bodies = bodies.take_modified(); + let mut modified_colliders = colliders.take_modified(); + let mut removed_colliders = colliders.take_removed(); + + self.step_generic( + prediction_distance, + broad_phase, + narrow_phase, + bodies, + colliders, + &mut modified_bodies, + &mut modified_colliders, + &mut removed_colliders, + hooks, + events, + ); + } + + /// Executes one step of the collision detection. + pub fn step_generic<Bodies, Colliders>( + &mut self, + prediction_distance: Real, + broad_phase: &mut BroadPhase, + narrow_phase: &mut NarrowPhase, + bodies: &mut Bodies, + colliders: &mut Colliders, + modified_bodies: &mut Vec<RigidBodyHandle>, + modified_colliders: &mut Vec<ColliderHandle>, + removed_colliders: &mut Vec<ColliderHandle>, + hooks: &dyn PhysicsHooks<Bodies, Colliders>, + events: &dyn EventHandler, ) where - Bodies: ComponentSetMut<RigidBodyIds> + Bodies: ComponentSetMut<RigidBodyPosition> + + ComponentSetMut<RigidBodyVelocity> + + ComponentSetMut<RigidBodyIds> + ComponentSetMut<RigidBodyActivation> + + ComponentSetMut<RigidBodyChanges> + ComponentSet<RigidBodyColliders> - + ComponentSetMut<RigidBodyVelocity> + ComponentSet<RigidBodyDominance> + ComponentSet<RigidBodyType>, - Colliders: ComponentSetMut<ColliderShape>, + Colliders: ComponentSetMut<ColliderBroadPhaseData> + + ComponentSetMut<ColliderChanges> + + ComponentSetMut<ColliderPosition> + + ComponentSet<ColliderShape> + + ComponentSetOption<ColliderParent> + + ComponentSet<ColliderType> + + ComponentSet<ColliderGroups> + + ComponentSet<ColliderMaterial>, { - unimplemented!() + super::user_changes::handle_user_changes_to_colliders( + bodies, + colliders, + &modified_colliders[..], + ); + super::user_changes::handle_user_changes_to_rigid_bodies( + None, + bodies, + colliders, + &modified_bodies, + modified_colliders, + ); + self.detect_collisions( + prediction_distance, + broad_phase, + narrow_phase, + bodies, + colliders, + &modified_colliders[..], + removed_colliders, + hooks, + events, + true, + ); + + self.clear_modified_colliders(colliders, modified_colliders); + removed_colliders.clear(); } } diff --git a/src/pipeline/physics_pipeline.rs b/src/pipeline/physics_pipeline.rs index 5a7a827..9557d1f 100644 --- a/src/pipeline/physics_pipeline.rs +++ b/src/pipeline/physics_pipeline.rs @@ -126,7 +126,7 @@ impl PhysicsPipeline { // Update narrow-phase. if handle_user_changes { narrow_phase.handle_user_changes( - islands, + Some(islands), modified_colliders, removed_colliders, colliders, @@ -134,7 +134,13 @@ impl PhysicsPipeline { events, ); } - narrow_phase.register_pairs(islands, colliders, bodies, &self.broad_phase_events, events); + narrow_phase.register_pairs( + Some(islands), + colliders, + bodies, + &self.broad_phase_events, + events, + ); narrow_phase.compute_contacts( integration_parameters.prediction_distance, bodies, @@ -519,7 +525,7 @@ impl PhysicsPipeline { &modified_colliders[..], ); super::user_changes::handle_user_changes_to_rigid_bodies( - islands, + Some(islands), bodies, colliders, &modified_bodies, diff --git a/src/pipeline/query_pipeline.rs b/src/pipeline/query_pipeline.rs index 522e4c4..31bf3a4 100644 --- a/src/pipeline/query_pipeline.rs +++ b/src/pipeline/query_pipeline.rs @@ -23,6 +23,9 @@ use parry::query::{DefaultQueryDispatcher, NonlinearRigidMotion, QueryDispatcher use parry::shape::{FeatureId, Shape, TypedSimdCompositeShape}; use std::sync::Arc; +#[cfg(feature = "default-sets")] +use crate::{dynamics::RigidBodySet, geometry::ColliderSet}; + /// A pipeline for performing queries on all the colliders of a scene. #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] #[derive(Clone)] @@ -147,8 +150,19 @@ impl QueryPipeline { &*self.query_dispatcher } + #[cfg(feature = "default-sets")] + /// Update the acceleration structure on the query pipeline. + pub fn update( + &mut self, + islands: &IslandManager, + bodies: &RigidBodySet, + colliders: &ColliderSet, + ) { + self.update_generic(islands, bodies, colliders); + } + /// Update the acceleration structure on the query pipeline. - pub fn update<Bodies, Colliders>( + pub fn update_generic<Bodies, Colliders>( &mut self, islands: &IslandManager, bodies: &Bodies, diff --git a/src/pipeline/user_changes.rs b/src/pipeline/user_changes.rs index 354ff3d..34a34a2 100644 --- a/src/pipeline/user_changes.rs +++ b/src/pipeline/user_changes.rs @@ -37,7 +37,7 @@ pub(crate) fn handle_user_changes_to_colliders<Colliders>( } pub(crate) fn handle_user_changes_to_rigid_bodies<Bodies, Colliders>( - islands: &mut IslandManager, + mut islands: Option<&mut IslandManager>, bodies: &mut Bodies, colliders: &mut Colliders, modified_bodies: &[RigidBodyHandle], @@ -73,65 +73,79 @@ pub(crate) fn handle_user_changes_to_rigid_bodies<Bodies, Colliders>( { // The body's status changed. We need to make sure // it is on the correct active set. - if changes.contains(RigidBodyChanges::TYPE) { - match status { - 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)); + if let Some(islands) = islands.as_deref_mut() { + if changes.contains(RigidBodyChanges::TYPE) { + match status { + 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); } - - // 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::Kinematic => { - // 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::Kinematic => { + // 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::Static => {} } - RigidBodyType::Static => {} } - } - - // Update the positions of the colliders. - if changes.contains(RigidBodyChanges::POSITION) - || changes.contains(RigidBodyChanges::COLLIDERS) - { - rb_colliders.update_positions(colliders, modified_colliders, &poss.position); - if status.is_kinematic() - && islands.active_kinematic_set.get(ids.active_set_id) != Some(handle) + // Update the positions of the colliders. + if changes.contains(RigidBodyChanges::POSITION) + || changes.contains(RigidBodyChanges::COLLIDERS) { - ids.active_set_id = islands.active_kinematic_set.len(); - islands.active_kinematic_set.push(*handle); + rb_colliders.update_positions(colliders, modified_colliders, &poss.position); + + if status.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); + } } - } - // 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. - && status.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); + // 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. + && status.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, &poss.position); + } } bodies.set_internal(handle.0, RigidBodyChanges::empty()); @@ -140,16 +154,18 @@ pub(crate) fn handle_user_changes_to_rigid_bodies<Bodies, Colliders>( } // Adjust some ids, if needed. - 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 id < active_set.len() { - bodies.map_mut_internal(active_set[id].0, |ids2: &mut RigidBodyIds| { - ids2.active_set_id = id; - }); + 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 id < active_set.len() { + bodies.map_mut_internal(active_set[id].0, |ids2: &mut RigidBodyIds| { + ids2.active_set_id = id; + }); + } } } } |
