From c32da78f2a6014c491aa3e975fb83ddb7c80610e Mon Sep 17 00:00:00 2001 From: Crozet Sébastien Date: Mon, 26 Apr 2021 17:59:25 +0200 Subject: Split rigid-bodies and colliders into multiple components --- src/geometry/broad_phase_multi_sap/broad_phase.rs | 220 ++++++++++++++-------- src/geometry/broad_phase_multi_sap/sap_layer.rs | 5 +- 2 files changed, 142 insertions(+), 83 deletions(-) (limited to 'src/geometry/broad_phase_multi_sap') diff --git a/src/geometry/broad_phase_multi_sap/broad_phase.rs b/src/geometry/broad_phase_multi_sap/broad_phase.rs index fc79d6d..890d851 100644 --- a/src/geometry/broad_phase_multi_sap/broad_phase.rs +++ b/src/geometry/broad_phase_multi_sap/broad_phase.rs @@ -1,15 +1,17 @@ use super::{ BroadPhasePairEvent, ColliderPair, SAPLayer, SAPProxies, SAPProxy, SAPProxyData, SAPRegionPool, }; -use crate::data::pubsub::Subscription; use crate::geometry::broad_phase_multi_sap::SAPProxyIndex; -use crate::geometry::collider::ColliderChanges; -use crate::geometry::{ColliderSet, RemovedCollider}; +use crate::geometry::{ + ColliderBroadPhaseData, ColliderChanges, ColliderHandle, ColliderPosition, ColliderShape, +}; use crate::math::Real; use crate::utils::IndexMut2; use parry::bounding_volume::BoundingVolume; use parry::utils::hashmap::HashMap; +use crate::data::{BundleSet, ComponentSet, ComponentSetMut}; + /// A broad-phase combining a Hierarchical Grid and Sweep-and-Prune. /// /// The basic Sweep-and-Prune (SAP) algorithm has one significant flaws: @@ -78,8 +80,19 @@ pub struct BroadPhase { layers: Vec, smallest_layer: u8, largest_layer: u8, - removed_colliders: Option>, deleted_any: bool, + // NOTE: we maintain this hashmap to simplify collider removal. + // This information is also present in the ColliderProxyId + // component. However if that component is removed, we need + // a way to access it to do some cleanup. + // Note that we could just remove the ColliderProxyId component + // altogether but that would be slow because of the need to + // always access this hashmap. Instead, we access this hashmap + // only when the collider has been added/removed. + // Another alternative would be to remove ColliderProxyId and + // just use a Coarena. But this seems like it could use too + // much memory. + colliders_proxy_ids: HashMap, #[cfg_attr(feature = "serde-serialize", serde(skip))] region_pool: SAPRegionPool, // To avoid repeated allocations. // We could think serializing this workspace is useless. @@ -107,13 +120,13 @@ impl BroadPhase { /// Create a new empty broad-phase. pub fn new() -> Self { BroadPhase { - removed_colliders: None, proxies: SAPProxies::new(), layers: Vec::new(), smallest_layer: 0, largest_layer: 0, region_pool: Vec::new(), reporting: HashMap::default(), + colliders_proxy_ids: HashMap::default(), deleted_any: false, } } @@ -123,26 +136,13 @@ impl BroadPhase { /// For each colliders marked as removed, we make their containing layer mark /// its proxy as pre-deleted. The actual proxy removal will happen at the end /// of the `BroadPhase::update`. - fn handle_removed_colliders(&mut self, colliders: &mut ColliderSet) { - // Ensure we already subscribed the collider-removed events. - if self.removed_colliders.is_none() { - self.removed_colliders = Some(colliders.removed_colliders.subscribe()); - } - - // Extract the cursor to avoid borrowing issues. - let cursor = self.removed_colliders.take().unwrap(); - - // Read all the collider-removed events, and remove the corresponding proxy. - for collider in colliders.removed_colliders.read(&cursor) { - self.predelete_proxy(collider.proxy_index); + fn handle_removed_colliders(&mut self, removed_colliders: &[ColliderHandle]) { + // For each removed collider, remove the corresponding proxy. + for removed in removed_colliders { + if let Some(proxy_id) = self.colliders_proxy_ids.get(removed).copied() { + self.predelete_proxy(proxy_id); + } } - - // NOTE: We don't acknowledge the cursor just yet because we need - // to traverse the set of removed colliders one more time after - // the broad-phase update. - - // Re-insert the cursor we extracted to avoid borrowing issues. - self.removed_colliders = Some(cursor); } /// Pre-deletes a proxy from this broad-phase. @@ -173,7 +173,7 @@ impl BroadPhase { /// This method will actually remove from the proxy list all the proxies /// marked as deletable by `self.predelete_proxy`, making their proxy /// handles re-usable by new proxies. - fn complete_removals(&mut self, colliders: &mut ColliderSet) { + fn complete_removals(&mut self, removed_colliders: &[ColliderHandle]) { // If there is no layer, there is nothing to remove. if self.layers.is_empty() { return; @@ -215,13 +215,13 @@ impl BroadPhase { /* * Actually remove the colliders proxies. */ - let cursor = self.removed_colliders.as_ref().unwrap(); - for collider in colliders.removed_colliders.read(&cursor) { - if collider.proxy_index != crate::INVALID_U32 { - self.proxies.remove(collider.proxy_index); + for removed in removed_colliders { + if let Some(proxy_id) = self.colliders_proxy_ids.remove(removed) { + if proxy_id != crate::INVALID_U32 { + self.proxies.remove(proxy_id); + } } } - colliders.removed_colliders.ack(&cursor); } /// Finalize the insertion of the layer identified by `layer_id`. @@ -336,67 +336,127 @@ impl BroadPhase { } } + fn handle_modified_collider( + &mut self, + prediction_distance: Real, + handle: ColliderHandle, + proxy_index: &mut u32, + collider: (&ColliderPosition, &ColliderShape, &ColliderChanges), + ) -> bool { + let (co_pos, co_shape, co_changes) = collider; + + let mut aabb = co_shape + .compute_aabb(co_pos) + .loosened(prediction_distance / 2.0); + + aabb.mins = super::clamp_point(aabb.mins); + aabb.maxs = super::clamp_point(aabb.maxs); + + let layer_id = if let Some(proxy) = self.proxies.get_mut(*proxy_index) { + let mut layer_id = proxy.layer_id; + proxy.aabb = aabb; + + if co_changes.contains(ColliderChanges::SHAPE) { + // If the shape was changed, then we need to see if this proxy should be + // migrated to a larger layer. Indeed, if the shape was replaced by + // a much larger shape, we need to promote the proxy to a bigger layer + // to avoid the O(n²) discretization problem. + let new_layer_depth = super::layer_containing_aabb(&aabb); + if new_layer_depth > proxy.layer_depth { + self.layers[proxy.layer_id as usize] + .proper_proxy_moved_to_bigger_layer(&mut self.proxies, *proxy_index); + + // We need to promote the proxy to the bigger layer. + layer_id = self.ensure_layer_exists(new_layer_depth); + self.proxies[*proxy_index].layer_id = layer_id; + } + } + + layer_id + } else { + let layer_depth = super::layer_containing_aabb(&aabb); + let layer_id = self.ensure_layer_exists(layer_depth); + + // Create the proxy. + let proxy = SAPProxy::collider(handle, aabb, layer_id, layer_depth); + *proxy_index = self.proxies.insert(proxy); + layer_id + }; + + let layer = &mut self.layers[layer_id as usize]; + + // Preupdate the collider in the layer. + layer.preupdate_collider( + *proxy_index, + &aabb, + &mut self.proxies, + &mut self.region_pool, + ); + let need_region_propagation = !layer.created_regions.is_empty(); + + need_region_propagation + } + /// Updates the broad-phase, taking into account the new collider positions. - pub fn update( + pub fn update( &mut self, prediction_distance: Real, - colliders: &mut ColliderSet, + colliders: &mut Colliders, + modified_colliders: &[ColliderHandle], + removed_colliders: &[ColliderHandle], events: &mut Vec, - ) { + ) where + Colliders: ComponentSetMut + + ComponentSet + + ComponentSet + + ComponentSet, + { // Phase 1: pre-delete the collisions that have been deleted. - self.handle_removed_colliders(colliders); + self.handle_removed_colliders(removed_colliders); let mut need_region_propagation = false; // Phase 2: pre-delete the collisions that have been deleted. - colliders.foreach_modified_colliders_mut_internal(|handle, collider| { - if !collider.changes.needs_broad_phase_update() { - return; - } - - let mut aabb = collider.compute_aabb().loosened(prediction_distance / 2.0); - aabb.mins = super::clamp_point(aabb.mins); - aabb.maxs = super::clamp_point(aabb.maxs); - - let layer_id = if let Some(proxy) = self.proxies.get_mut(collider.proxy_index) { - let mut layer_id = proxy.layer_id; - proxy.aabb = aabb; - - if collider.changes.contains(ColliderChanges::SHAPE) { - // If the shape was changed, then we need to see if this proxy should be - // migrated to a larger layer. Indeed, if the shape was replaced by - // a much larger shape, we need to promote the proxy to a bigger layer - // to avoid the O(n²) discretization problem. - let new_layer_depth = super::layer_containing_aabb(&aabb); - if new_layer_depth > proxy.layer_depth { - self.layers[proxy.layer_id as usize].proper_proxy_moved_to_bigger_layer( - &mut self.proxies, - collider.proxy_index, - ); - - // We need to promote the proxy to the bigger layer. - layer_id = self.ensure_layer_exists(new_layer_depth); - self.proxies[collider.proxy_index].layer_id = layer_id; - } + for handle in modified_colliders { + // NOTE: we use `get` because the collider may no longer + // exist if it has been removed. + let co_changes: Option<&ColliderChanges> = colliders.get(handle.0); + + if let Some(co_changes) = co_changes { + let (co_bf_data, co_pos, co_shape): ( + &ColliderBroadPhaseData, + &ColliderPosition, + &ColliderShape, + ) = colliders.index_bundle(handle.0); + + if !co_changes.needs_broad_phase_update() { + return; + } + let mut new_proxy_id = co_bf_data.proxy_index; + + if self.handle_modified_collider( + prediction_distance, + *handle, + &mut new_proxy_id, + (co_pos, co_shape, co_changes), + ) { + need_region_propagation = true; } - layer_id - } else { - let layer_depth = super::layer_containing_aabb(&aabb); - let layer_id = self.ensure_layer_exists(layer_depth); - - // Create the proxy. - let proxy = SAPProxy::collider(handle, aabb, layer_id, layer_depth); - collider.proxy_index = self.proxies.insert(proxy); - layer_id - }; - - let layer = &mut self.layers[layer_id as usize]; + if co_bf_data.proxy_index != new_proxy_id { + self.colliders_proxy_ids.insert(*handle, new_proxy_id); - // Preupdate the collider in the layer. - layer.preupdate_collider(collider, &aabb, &mut self.proxies, &mut self.region_pool); - need_region_propagation = need_region_propagation || !layer.created_regions.is_empty(); - }); + // Make sure we have the new proxy index in case + // the collider was added for the first time. + colliders.set_internal( + handle.0, + ColliderBroadPhaseData { + proxy_index: new_proxy_id, + }, + ); + } + } + } // Phase 3: bottom-up pass to propagate new regions from smaller layers to larger layers. if need_region_propagation { @@ -408,7 +468,7 @@ impl BroadPhase { // Phase 5: bottom-up pass to remove proxies, and propagate region removed from smaller // layers to possible remove regions from larger layers that would become empty that way. - self.complete_removals(colliders); + self.complete_removals(removed_colliders); } /// Propagate regions from the smallest layers up to the larger layers. diff --git a/src/geometry/broad_phase_multi_sap/sap_layer.rs b/src/geometry/broad_phase_multi_sap/sap_layer.rs index 9a9c296..e4a9f42 100644 --- a/src/geometry/broad_phase_multi_sap/sap_layer.rs +++ b/src/geometry/broad_phase_multi_sap/sap_layer.rs @@ -1,6 +1,6 @@ use super::{SAPProxies, SAPProxy, SAPRegion, SAPRegionPool}; use crate::geometry::broad_phase_multi_sap::DELETED_AABB_VALUE; -use crate::geometry::{Collider, SAPProxyIndex, AABB}; +use crate::geometry::{SAPProxyIndex, AABB}; use crate::math::{Point, Real}; use parry::utils::hashmap::{Entry, HashMap}; @@ -213,12 +213,11 @@ impl SAPLayer { pub fn preupdate_collider( &mut self, - collider: &Collider, + proxy_id: u32, aabb: &AABB, proxies: &mut SAPProxies, pool: &mut SAPRegionPool, ) { - let proxy_id = collider.proxy_index; let start = super::point_key(aabb.mins, self.region_width); let end = super::point_key(aabb.maxs, self.region_width); -- cgit