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

Restore the collision pipeline

Browse Source
This commit is contained in:
Crozet Sébastien
2021-04-29 10:26:44 +02:00
parent 83cb981a88
commit 705876f5e5
5 changed files with 305 additions and 114 deletions
Download Patch File Download Diff File Expand all files Collapse all files

74
src/geometry/narrow_phase.rs
View File

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

181
src/pipeline/collision_pipeline.rs
View File

@@ -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>
+ ComponentSetMut<RigidBodyActivation>
+ ComponentSet<RigidBodyColliders>
Bodies: ComponentSetMut<RigidBodyPosition>
+ ComponentSetMut<RigidBodyVelocity>
+ ComponentSetMut<RigidBodyIds>
+ ComponentSetMut<RigidBodyActivation>
+ ComponentSetMut<RigidBodyChanges>
+ ComponentSet<RigidBodyColliders>
+ 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();
}
}

12
src/pipeline/physics_pipeline.rs
View File

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

16
src/pipeline/query_pipeline.rs
View File

@@ -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<Bodies, Colliders>(
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_generic<Bodies, Colliders>(
&mut self,
islands: &IslandManager,
bodies: &Bodies,

136
src/pipeline/user_changes.rs
View File

@@ -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);
}
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 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);
// 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);
// 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);
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);
}
} 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 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;
});
if id < active_set.len() {
bodies.map_mut_internal(active_set[id].0, |ids2: &mut RigidBodyIds| {
ids2.active_set_id = id;
});
}
}
}
}