diff options
Diffstat (limited to 'src')
| -rw-r--r-- | src/data/mod.rs | 1 | ||||
| -rw-r--r-- | src/data/pubsub.rs | 19 | ||||
| -rw-r--r-- | src/dynamics/rigid_body_set.rs | 34 | ||||
| -rw-r--r-- | src/geometry/broad_phase_multi_sap.rs | 79 | ||||
| -rw-r--r-- | src/geometry/collider_set.rs | 45 | ||||
| -rw-r--r-- | src/geometry/mod.rs | 1 | ||||
| -rw-r--r-- | src/geometry/narrow_phase.rs | 110 | ||||
| -rw-r--r-- | src/pipeline/collision_pipeline.rs | 24 | ||||
| -rw-r--r-- | src/pipeline/physics_pipeline.rs | 65 |
9 files changed, 226 insertions, 152 deletions
diff --git a/src/data/mod.rs b/src/data/mod.rs index 7c00442..5d3efa6 100644 --- a/src/data/mod.rs +++ b/src/data/mod.rs @@ -2,3 +2,4 @@ pub mod arena; pub(crate) mod graph; +pub mod pubsub; diff --git a/src/data/pubsub.rs b/src/data/pubsub.rs index b3b263c..1ac7498 100644 --- a/src/data/pubsub.rs +++ b/src/data/pubsub.rs @@ -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; diff --git a/src/dynamics/rigid_body_set.rs b/src/dynamics/rigid_body_set.rs index f54bc55..7906083 100644 --- a/src/dynamics/rigid_body_set.rs +++ b/src/dynamics/rigid_body_set.rs @@ -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 diff --git a/src/geometry/broad_phase_multi_sap.rs b/src/geometry/broad_phase_multi_sap.rs index 054fccf..5c80e0e 100644 --- a/src/geometry/broad_phase_multi_sap.rs +++ b/src/geometry/broad_phase_multi_sap.rs @@ -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 proxy = &mut self.proxies[collider.proxy_index]; + let mut cursor = self.removed_colliders.take().unwrap(); + for collider in colliders.removed_colliders.read(&cursor) { + self.remove_collider(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); + colliders.removed_colliders.ack(&mut cursor); + self.removed_colliders = Some(cursor); + } - #[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); - self.deleted_any = true; - } + 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; } } + } - #[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; - } + #[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; } } } - - self.proxies.remove(collider.proxy_index); } + + self.proxies.remove(proxy_index); } pub(crate) fn update_aabbs( diff --git a/src/geometry/collider_set.rs b/src/geometry/collider_set.rs index 22bba1b..3a259ae 100644 --- a/src/geometry/collider_set.rs +++ b/src/geometry/collider_set.rs @@ -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. diff --git a/src/geometry/mod.rs b/src/geometry/mod.rs index 406727f..4456961 100644 --- a/src/geometry/mod.rs +++ b/src/geometry/mod.rs @@ -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")] diff --git a/src/geometry/narrow_phase.rs b/src/geometry/narrow_phase.rs index 016da33..60c5e1a 100644 --- a/src/geometry/narrow_phase.rs +++ b/src/geometry/narrow_phase.rs @@ -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) - .and_then(|h| colliders.get_mut(h)) - { - replacement.proximity_graph_index = 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) { + if let Some(replacement) = colliders.get_mut(replacement) { + replacement.proximity_graph_index = proximity_graph_id; + } else { + prox_id_remap.insert(replacement, 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); } } } diff --git a/src/pipeline/collision_pipeline.rs b/src/pipeline/collision_pipeline.rs index f30dae7..2283fa7 100644 --- a/src/pipeline/collision_pipeline.rs +++ b/src/pipeline/collision_pipeline.rs @@ -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) - } } diff --git a/src/pipeline/physics_pipeline.rs b/src/pipeline/physics_pipeline.rs index 3fcd1af..7185c62 100644 --- a/src/pipeline/physics_pipeline.rs +++ b/src/pipeline/physics_pipeline.rs @@ -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)] |