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

Use the publish-subscribe mechanism to handle collider removals across pipelines.

Browse Source
This commit is contained in:
Crozet Sébastien
2020-10-05 19:04:18 +02:00
parent 2d0a888484
commit 93aa7b6e1e
11 changed files with 236 additions and 174 deletions
Download Patch File Download Diff File Expand all files Collapse all files

1
src/data/mod.rs
View File

@@ -2,3 +2,4 @@
pub mod arena;
pub(crate) mod graph;
pub mod pubsub;

19
src/data/pubsub.rs
View File

@@ -1,16 +1,18 @@
//! Publish-subscribe mechanism for internal events.
use serde::export::PhantomData;
use std::collections::VecDeque;
/// The position of a subscriber on a pub-sub queue.
#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
pub struct PubSubCursor {
pub struct PubSubCursor<T> {
// Index of the next message to read.
id: u32,
next: u32,
_phantom: PhantomData<T>,
}
impl PubSubCursor {
impl<T> PubSubCursor<T> {
fn id(&self, num_deleted: u32) -> usize {
(self.id - num_deleted) as usize
}
@@ -53,18 +55,25 @@ impl<T> PubSub<T> {
/// Subscribe to the queue.
///
/// A subscription cannot be cancelled.
pub fn subscribe(&mut self) -> PubSubCursor {
pub fn subscribe(&mut self) -> PubSubCursor<T> {
let cursor = PubSubCursor {
next: self.messages.len() as u32 + self.deleted_messages,
id: self.offsets.len() as u32 + self.deleted_offsets,
_phantom: PhantomData,
};
self.offsets.push_back(cursor.next);
cursor
}
/// Read the i-th message not yet read by the given subsciber.
pub fn read_ith(&self, cursor: &PubSubCursor<T>, i: usize) -> Option<&T> {
self.messages
.get(cursor.next(self.deleted_messages) as usize + i)
}
/// Get the messages not yet read by the given subscriber.
pub fn read(&self, cursor: &PubSubCursor) -> impl Iterator<Item = &T> {
pub fn read(&self, cursor: &PubSubCursor<T>) -> impl Iterator<Item = &T> {
let next = cursor.next(self.deleted_messages);
// TODO: use self.queue.range(next..) once it is stabilised.
@@ -77,7 +86,7 @@ impl<T> PubSub<T> {
/// Makes the given subscribe acknowledge all the messages in the queue.
///
/// A subscriber cannot read acknowledged messages any more.
pub fn ack(&mut self, cursor: &mut PubSubCursor) {
pub fn ack(&mut self, cursor: &mut PubSubCursor<T>) {
// Update the cursor.
cursor.next = self.messages.len() as u32 + self.deleted_messages;
self.offsets[cursor.id(self.deleted_offsets)] = u32::MAX;

34
src/dynamics/rigid_body_set.rs
View File

@@ -2,7 +2,8 @@
use rayon::prelude::*;
use crate::data::arena::Arena;
use crate::dynamics::{BodyStatus, Joint, RigidBody};
use crate::data::pubsub::PubSub;
use crate::dynamics::{BodyStatus, Joint, JointSet, RigidBody};
use crate::geometry::{ColliderHandle, ColliderSet, ContactPair, InteractionGraph};
use crossbeam::channel::{Receiver, Sender};
use num::Zero;
@@ -176,12 +177,17 @@ impl RigidBodySet {
handle
}
pub(crate) fn num_islands(&self) -> usize {
self.active_islands.len() - 1
}
pub(crate) fn remove_internal(&mut self, handle: RigidBodyHandle) -> Option<RigidBody> {
/// Removes a rigid-body, and all its attached colliders and joints, from these sets.
pub fn remove(
&mut self,
handle: RigidBodyHandle,
colliders: &mut ColliderSet,
joints: &mut JointSet,
) -> Option<RigidBody> {
let rb = self.bodies.remove(handle)?;
/*
* Update active sets.
*/
let mut active_sets = [&mut self.active_kinematic_set, &mut self.active_dynamic_set];
for active_set in &mut active_sets {
@@ -194,9 +200,25 @@ impl RigidBodySet {
}
}
/*
* Remove colliders attached to this rigid-body.
*/
for collider in &rb.colliders {
colliders.remove(*collider, self);
}
/*
* Remove joints attached to this rigid-body.
*/
joints.remove_rigid_body(rb.joint_graph_index, self);
Some(rb)
}
pub(crate) fn num_islands(&self) -> usize {
self.active_islands.len() - 1
}
/// Forces the specified rigid-body to wake up if it is dynamic.
///
/// If `strong` is `true` then it is assured that the rigid-body will

85
src/geometry/broad_phase_multi_sap.rs
View File

@@ -1,5 +1,6 @@
use crate::data::pubsub::PubSubCursor;
use crate::dynamics::RigidBodySet;
use crate::geometry::{ColliderHandle, ColliderPair, ColliderSet};
use crate::geometry::{Collider, ColliderHandle, ColliderPair, ColliderSet, RemovedCollider};
use crate::math::{Point, Vector, DIM};
#[cfg(feature = "enhanced-determinism")]
use crate::utils::FxHashMap32 as HashMap;
@@ -381,6 +382,7 @@ impl SAPRegion {
pub struct BroadPhase {
proxies: Proxies,
regions: HashMap<Point<i32>, SAPRegion>,
removed_colliders: Option<PubSubCursor<RemovedCollider>>,
deleted_any: bool,
// We could think serializing this workspace is useless.
// It turns out is is important to serialize at least its capacity
@@ -469,6 +471,7 @@ impl BroadPhase {
/// Create a new empty broad-phase.
pub fn new() -> Self {
BroadPhase {
removed_colliders: None,
proxies: Proxies::new(),
regions: HashMap::default(),
reporting: HashMap::default(),
@@ -476,46 +479,60 @@ impl BroadPhase {
}
}
pub(crate) fn remove_colliders(&mut self, handles: &[ColliderHandle], colliders: &ColliderSet) {
for collider in handles.iter().filter_map(|h| colliders.get(*h)) {
if collider.proxy_index == crate::INVALID_USIZE {
// This collider has not been added to the broad-phase yet.
continue;
/// Maintain the broad-phase internal state by taking collider removal into account.
pub fn maintain(&mut self, colliders: &mut ColliderSet) {
// Ensure we already subscribed.
if self.removed_colliders.is_none() {
self.removed_colliders = Some(colliders.removed_colliders.subscribe());
}
let mut cursor = self.removed_colliders.take().unwrap();
for collider in colliders.removed_colliders.read(&cursor) {
self.remove_collider(collider.proxy_index);
}
colliders.removed_colliders.ack(&mut cursor);
self.removed_colliders = Some(cursor);
}
fn remove_collider<'a>(&mut self, proxy_index: usize) {
if proxy_index == crate::INVALID_USIZE {
// This collider has not been added to the broad-phase yet.
return;
}
let proxy = &mut self.proxies[proxy_index];
// Push the proxy to infinity, but not beyond the sentinels.
proxy.aabb.mins.coords.fill(SENTINEL_VALUE / 2.0);
proxy.aabb.maxs.coords.fill(SENTINEL_VALUE / 2.0);
// Discretize the AABB to find the regions that need to be invalidated.
let start = point_key(proxy.aabb.mins);
let end = point_key(proxy.aabb.maxs);
#[cfg(feature = "dim2")]
for i in start.x..=end.x {
for j in start.y..=end.y {
if let Some(region) = self.regions.get_mut(&Point::new(i, j)) {
region.predelete_proxy(proxy_index);
self.deleted_any = true;
}
}
}
let proxy = &mut self.proxies[collider.proxy_index];
// Push the proxy to infinity, but not beyond the sentinels.
proxy.aabb.mins.coords.fill(SENTINEL_VALUE / 2.0);
proxy.aabb.maxs.coords.fill(SENTINEL_VALUE / 2.0);
// Discretize the AABB to find the regions that need to be invalidated.
let start = point_key(proxy.aabb.mins);
let end = point_key(proxy.aabb.maxs);
#[cfg(feature = "dim2")]
for i in start.x..=end.x {
for j in start.y..=end.y {
if let Some(region) = self.regions.get_mut(&Point::new(i, j)) {
region.predelete_proxy(collider.proxy_index);
#[cfg(feature = "dim3")]
for i in start.x..=end.x {
for j in start.y..=end.y {
for k in start.z..=end.z {
if let Some(region) = self.regions.get_mut(&Point::new(i, j, k)) {
region.predelete_proxy(proxy_index);
self.deleted_any = true;
}
}
}
#[cfg(feature = "dim3")]
for i in start.x..=end.x {
for j in start.y..=end.y {
for k in start.z..=end.z {
if let Some(region) = self.regions.get_mut(&Point::new(i, j, k)) {
region.predelete_proxy(collider.proxy_index);
self.deleted_any = true;
}
}
}
}
self.proxies.remove(collider.proxy_index);
}
self.proxies.remove(proxy_index);
}
pub(crate) fn update_aabbs(

45
src/geometry/collider_set.rs
View File

@@ -1,14 +1,25 @@
use crate::data::arena::Arena;
use crate::data::pubsub::PubSub;
use crate::dynamics::{RigidBodyHandle, RigidBodySet};
use crate::geometry::Collider;
use crate::geometry::{Collider, ColliderGraphIndex};
use std::ops::{Index, IndexMut};
/// The unique identifier of a collider added to a collider set.
pub type ColliderHandle = crate::data::arena::Index;
#[derive(Copy, Clone, Debug)]
#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
pub(crate) struct RemovedCollider {
pub handle: ColliderHandle,
pub(crate) contact_graph_index: ColliderGraphIndex,
pub(crate) proximity_graph_index: ColliderGraphIndex,
pub(crate) proxy_index: usize,
}
#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
/// A set of colliders that can be handled by a physics `World`.
pub struct ColliderSet {
pub(crate) removed_colliders: PubSub<RemovedCollider>,
pub(crate) colliders: Arena<Collider>,
}
@@ -16,6 +27,7 @@ impl ColliderSet {
/// Create a new empty set of colliders.
pub fn new() -> Self {
ColliderSet {
removed_colliders: PubSub::new(),
colliders: Arena::new(),
}
}
@@ -60,8 +72,35 @@ impl ColliderSet {
handle
}
pub(crate) fn remove_internal(&mut self, handle: ColliderHandle) -> Option<Collider> {
self.colliders.remove(handle)
/// Remove a collider from this set and update its parent accordingly.
pub fn remove(
&mut self,
handle: ColliderHandle,
bodies: &mut RigidBodySet,
) -> Option<Collider> {
let collider = self.colliders.remove(handle)?;
/*
* Delete the collider from its parent body.
*/
if let Some(mut parent) = bodies.get_mut_internal(collider.parent) {
parent.remove_collider_internal(handle, &collider);
bodies.wake_up(collider.parent, true);
}
/*
* Publish removal.
*/
let message = RemovedCollider {
handle,
contact_graph_index: collider.contact_graph_index,
proximity_graph_index: collider.proximity_graph_index,
proxy_index: collider.proxy_index,
};
self.removed_colliders.publish(message);
Some(collider)
}
/// Gets the collider with the given handle without a known generation.

1
src/geometry/mod.rs
View File

@@ -45,6 +45,7 @@ pub type RayIntersection = ncollide::query::RayIntersection<f32>;
pub(crate) use self::ball::WBall;
pub(crate) use self::broad_phase::{ColliderPair, WAABBHierarchy, WAABBHierarchyIntersections};
pub(crate) use self::broad_phase_multi_sap::BroadPhasePairEvent;
pub(crate) use self::collider_set::RemovedCollider;
#[cfg(feature = "simd-is-enabled")]
pub(crate) use self::contact::WContact;
#[cfg(feature = "dim2")]

110
src/geometry/narrow_phase.rs
View File

@@ -14,13 +14,16 @@ use crate::geometry::proximity_detector::{
// proximity_detector::ProximityDetectionContextSimd, WBall,
//};
use crate::geometry::{
BroadPhasePairEvent, ColliderHandle, ContactEvent, ProximityEvent, ProximityPair,
BroadPhasePairEvent, Collider, ColliderGraphIndex, ColliderHandle, ContactEvent,
ProximityEvent, ProximityPair, RemovedCollider,
};
use crate::geometry::{ColliderSet, ContactManifold, ContactPair, InteractionGraph};
//#[cfg(feature = "simd-is-enabled")]
//use crate::math::{SimdFloat, SIMD_WIDTH};
use crate::data::pubsub::PubSubCursor;
use crate::ncollide::query::Proximity;
use crate::pipeline::EventHandler;
use std::collections::HashMap;
//use simba::simd::SimdValue;
/// The narrow-phase responsible for computing precise contact information between colliders.
@@ -28,6 +31,7 @@ use crate::pipeline::EventHandler;
pub struct NarrowPhase {
contact_graph: InteractionGraph<ContactPair>,
proximity_graph: InteractionGraph<ProximityPair>,
removed_colliders: Option<PubSubCursor<RemovedCollider>>,
// ball_ball: Vec<usize>, // Workspace: Vec<*mut ContactPair>,
// shape_shape: Vec<usize>, // Workspace: Vec<*mut ContactPair>,
// ball_ball_prox: Vec<usize>, // Workspace: Vec<*mut ProximityPair>,
@@ -42,6 +46,7 @@ impl NarrowPhase {
Self {
contact_graph: InteractionGraph::new(),
proximity_graph: InteractionGraph::new(),
removed_colliders: None,
// ball_ball: Vec::new(),
// shape_shape: Vec::new(),
// ball_ball_prox: Vec::new(),
@@ -73,45 +78,84 @@ impl NarrowPhase {
// &mut self.contact_graph.interactions
// }
pub(crate) fn remove_colliders(
/// Maintain the narrow-phase internal state by taking collider removal into account.
pub fn maintain(&mut self, colliders: &mut ColliderSet, bodies: &mut RigidBodySet) {
// Ensure we already subscribed.
if self.removed_colliders.is_none() {
self.removed_colliders = Some(colliders.removed_colliders.subscribe());
}
let mut cursor = self.removed_colliders.take().unwrap();
// TODO: avoid these hash-maps.
// They are necessary to handle the swap-remove done internally
// by the contact/proximity graphs when a node is removed.
let mut prox_id_remap = HashMap::new();
let mut contact_id_remap = HashMap::new();
for i in 0.. {
if let Some(collider) = colliders.removed_colliders.read_ith(&cursor, i) {
let proximity_graph_id = prox_id_remap
.get(&collider.handle)
.copied()
.unwrap_or(collider.proximity_graph_index);
let contact_graph_id = contact_id_remap
.get(&collider.handle)
.copied()
.unwrap_or(collider.contact_graph_index);
self.remove_collider(
proximity_graph_id,
contact_graph_id,
colliders,
bodies,
&mut prox_id_remap,
&mut contact_id_remap,
);
} else {
break;
}
}
colliders.removed_colliders.ack(&mut cursor);
self.removed_colliders = Some(cursor);
}
pub(crate) fn remove_collider<'a>(
&mut self,
handles: &[ColliderHandle],
proximity_graph_id: ColliderGraphIndex,
contact_graph_id: ColliderGraphIndex,
colliders: &mut ColliderSet,
bodies: &mut RigidBodySet,
prox_id_remap: &mut HashMap<ColliderHandle, ColliderGraphIndex>,
contact_id_remap: &mut HashMap<ColliderHandle, ColliderGraphIndex>,
) {
for handle in handles {
if let Some(collider) = colliders.get(*handle) {
let proximity_graph_id = collider.proximity_graph_index;
let contact_graph_id = collider.contact_graph_index;
// 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).map(|c| c.parent) {
bodies.wake_up(parent, true)
}
// 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).map(|c| c.parent) {
bodies.wake_up(parent, true)
}
if let Some(parent) = colliders.get(b).map(|c| c.parent) {
bodies.wake_up(parent, true)
}
}
if let Some(parent) = colliders.get(b).map(|c| c.parent) {
bodies.wake_up(parent, true)
}
}
// We have to manage the fact that one other collider will
// have its graph index changed because of the node's swap-remove.
if let Some(replacement) = self.proximity_graph.remove_node(proximity_graph_id) {
if let Some(replacement) = colliders.get_mut(replacement) {
replacement.proximity_graph_index = proximity_graph_id;
} else {
prox_id_remap.insert(replacement, proximity_graph_id);
}
}
// We have to manage the fact that one other collider will
// have its graph index changed because of the node's swap-remove.
if let Some(replacement) = self
.proximity_graph
.remove_node(proximity_graph_id)
.and_then(|h| colliders.get_mut(h))
{
replacement.proximity_graph_index = proximity_graph_id;
}
if let Some(replacement) = self
.contact_graph
.remove_node(contact_graph_id)
.and_then(|h| colliders.get_mut(h))
{
replacement.contact_graph_index = contact_graph_id;
}
if let Some(replacement) = self.contact_graph.remove_node(contact_graph_id) {
if let Some(replacement) = colliders.get_mut(replacement) {
replacement.contact_graph_index = contact_graph_id;
} else {
contact_id_remap.insert(replacement, contact_graph_id);
}
}
}

24
src/pipeline/collision_pipeline.rs
View File

@@ -84,28 +84,4 @@ impl CollisionPipeline {
bodies.modified_inactive_set.clear();
}
/// Remove a rigid-body and all its associated data.
pub fn remove_rigid_body(
&mut self,
handle: RigidBodyHandle,
broad_phase: &mut BroadPhase,
narrow_phase: &mut NarrowPhase,
bodies: &mut RigidBodySet,
colliders: &mut ColliderSet,
) -> Option<RigidBody> {
// Remove the body.
let body = bodies.remove_internal(handle)?;
// Remove this rigid-body from the broad-phase and narrow-phase.
broad_phase.remove_colliders(&body.colliders, colliders);
narrow_phase.remove_colliders(&body.colliders, colliders, bodies);
// Remove all colliders attached to this body.
for collider in &body.colliders {
colliders.remove_internal(*collider);
}
Some(body)
}
}

65
src/pipeline/physics_pipeline.rs
View File

@@ -1,6 +1,7 @@
//! Physics pipeline structures.
use crate::counters::Counters;
use crate::data::pubsub::PubSubCursor;
#[cfg(not(feature = "parallel"))]
use crate::dynamics::IslandSolver;
use crate::dynamics::{IntegrationParameters, JointSet, RigidBody, RigidBodyHandle, RigidBodySet};
@@ -8,7 +9,7 @@ use crate::dynamics::{IntegrationParameters, JointSet, RigidBody, RigidBodyHandl
use crate::dynamics::{JointGraphEdge, ParallelIslandSolver as IslandSolver};
use crate::geometry::{
BroadPhase, BroadPhasePairEvent, Collider, ColliderHandle, ColliderPair, ColliderSet,
ContactManifoldIndex, NarrowPhase,
ContactManifoldIndex, NarrowPhase, RemovedCollider,
};
use crate::math::Vector;
use crate::pipeline::EventHandler;
@@ -59,6 +60,18 @@ impl PhysicsPipeline {
}
}
/// Remove this.
pub fn maintain(
&mut self,
broad_phase: &mut BroadPhase,
narrow_phase: &mut NarrowPhase,
bodies: &mut RigidBodySet,
colliders: &mut ColliderSet,
) {
// broad_phase.maintain(colliders);
// narrow_phase.maintain(colliders, bodies);
}
/// Executes one timestep of the physics simulation.
pub fn step(
&mut self,
@@ -73,6 +86,7 @@ impl PhysicsPipeline {
) {
// println!("Step");
self.counters.step_started();
self.maintain(broad_phase, narrow_phase, bodies, colliders);
bodies.maintain_active_set();
// Update kinematic bodies velocities.
@@ -249,55 +263,6 @@ impl PhysicsPipeline {
bodies.modified_inactive_set.clear();
self.counters.step_completed();
}
/// Remove a collider and all its associated data.
pub fn remove_collider(
&mut self,
handle: ColliderHandle,
broad_phase: &mut BroadPhase,
narrow_phase: &mut NarrowPhase,
bodies: &mut RigidBodySet,
colliders: &mut ColliderSet,
) -> Option<Collider> {
broad_phase.remove_colliders(&[handle], colliders);
narrow_phase.remove_colliders(&[handle], colliders, bodies);
let collider = colliders.remove_internal(handle)?;
if let Some(parent) = bodies.get_mut_internal(collider.parent) {
parent.remove_collider_internal(handle, &collider);
bodies.wake_up(collider.parent, true);
}
Some(collider)
}
/// Remove a rigid-body and all its associated data.
pub fn remove_rigid_body(
&mut self,
handle: RigidBodyHandle,
broad_phase: &mut BroadPhase,
narrow_phase: &mut NarrowPhase,
bodies: &mut RigidBodySet,
colliders: &mut ColliderSet,
joints: &mut JointSet,
) -> Option<RigidBody> {
// Remove the body.
let body = bodies.remove_internal(handle)?;
// Remove this rigid-body from the broad-phase and narrow-phase.
broad_phase.remove_colliders(&body.colliders, colliders);
narrow_phase.remove_colliders(&body.colliders, colliders, bodies);
// Remove all joints attached to this body.
joints.remove_rigid_body(body.joint_graph_index, bodies);
// Remove all colliders attached to this body.
for collider in &body.colliders {
colliders.remove_internal(*collider);
}
Some(body)
}
}
#[cfg(test)]