diff options
| -rw-r--r-- | src/geometry/broad_phase_multi_sap/broad_phase.rs | 58 | ||||
| -rw-r--r-- | src/geometry/broad_phase_multi_sap/sap_layer.rs | 74 | ||||
| -rw-r--r-- | src/geometry/broad_phase_multi_sap/sap_region.rs | 35 | ||||
| -rw-r--r-- | src/geometry/broad_phase_multi_sap/sap_utils.rs | 2 |
4 files changed, 139 insertions, 30 deletions
diff --git a/src/geometry/broad_phase_multi_sap/broad_phase.rs b/src/geometry/broad_phase_multi_sap/broad_phase.rs index 18d394f..c97c737 100644 --- a/src/geometry/broad_phase_multi_sap/broad_phase.rs +++ b/src/geometry/broad_phase_multi_sap/broad_phase.rs @@ -119,6 +119,10 @@ impl BroadPhase { } /// Maintain the broad-phase internal state by taking collider removal into account. + /// + /// 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() { @@ -141,13 +145,13 @@ impl BroadPhase { self.removed_colliders = Some(cursor); } - /// Removes a proxy from this broad-phase. + /// Pre-deletes a proxy from this broad-phase. /// /// The removal of a proxy is a semi-lazy process. It will mark /// the proxy as predeleted, and will set its AABB as +infinity. /// After this method has been called with all the proxies to /// remove, the `complete_removal` method MUST be called to - /// complete the removal of these proxies, by removing them + /// complete the removal of these proxies, by actually removing them /// from all the relevant layers/regions/axes. fn predelete_proxy(&mut self, proxy_index: SAPProxyIndex) { if proxy_index == crate::INVALID_U32 { @@ -220,20 +224,27 @@ impl BroadPhase { colliders.removed_colliders.ack(&cursor); } + /// Finalize the insertion of the layer identified by `layer_id`. + /// + /// This will: + /// - Remove all the subregion proxies from the larger layer. + /// - Pre-insert all the smaller layer's region proxies into this layer. fn finalize_layer_insertion(&mut self, layer_id: u8) { + // Remove all the region endpoints from the larger layer. + // They will be automatically replaced by the new layer's regions. if let Some(larger_layer) = self.layers[layer_id as usize].larger_layer { - // Remove all the region endpoints from the larger layer. - // They will be automatically replaced by the new layer's regions. self.layers[larger_layer as usize].unregister_all_subregions(&mut self.proxies); } + // Add all the regions from the smaller layer to the new layer. + // This will result in new regions to be created in the new layer. + // These new regions will automatically propagate to the larger layers in + // the Phase 3 of `Self::update`. if let Some(smaller_layer) = self.layers[layer_id as usize].smaller_layer { let (smaller_layer, new_layer) = self .layers .index_mut2(smaller_layer as usize, layer_id as usize); - // Add all the regions from the smaller layer to the new layer. - // This will propagate to the bigger layers automatically. smaller_layer.propagate_existing_regions( new_layer, &mut self.proxies, @@ -242,6 +253,17 @@ impl BroadPhase { } } + /// Ensures that a given layer exists. + /// + /// If the layer does not exist then: + /// 1. It is created and added to `self.layers`. + /// 2. The smaller/larger layer indices are updated to order them + /// properly depending on their depth. + /// 3. All the subregion proxies from the larger layer are deleted: + /// they will be replaced by this new layer's regions later in + /// the `update` function. + /// 4. All the regions from the smaller layer are added to that new + /// layer. fn ensure_layer_exists(&mut self, new_depth: i8) -> u8 { // Special case: we don't have any layers yet. if self.layers.is_empty() { @@ -264,6 +286,9 @@ impl BroadPhase { match larger_layer_id { None => { + // The layer we are currently creating is the new largest layer. So + // we need to update `self.largest_layer` accordingly then call + // `self.finalize_layer_insertion. assert_ne!(self.layers.len() as u8, u8::MAX, "Not yet implemented."); let new_layer_id = self.layers.len() as u8; self.layers[self.largest_layer as usize].larger_layer = Some(new_layer_id); @@ -283,6 +308,10 @@ impl BroadPhase { larger_layer_id } else { // The layer does not exist yet. Create it. + // And we found another layer that is larger than this one. + // So we need to adjust the smaller/larger layer indices too + // keep the list sorted, and then call `self.finalize_layer_insertion` + // to deal with region propagation. let new_layer_id = self.layers.len() as u8; let smaller_layer_id = self.layers[larger_layer_id as usize].smaller_layer; self.layers[larger_layer_id as usize].smaller_layer = Some(new_layer_id); @@ -315,10 +344,12 @@ impl BroadPhase { colliders: &mut ColliderSet, events: &mut Vec<BroadPhasePairEvent>, ) { + // Phase 1: pre-delete the collisions that have been deleted. self.handle_removed_colliders(colliders); let mut need_region_propagation = false; + // Phase 2: pre-delete the collisions that have been deleted. for body_handle in bodies .modified_inactive_set .iter() @@ -353,23 +384,23 @@ impl BroadPhase { } } - // Bottom-up pass to propagate regions from smaller layers to larger layers. + // Phase 3: bottom-up pass to propagate new regions from smaller layers to larger layers. if need_region_propagation { self.propagate_created_regions(); } - // Top-down pass to propagate proxies from larger layers to smaller layers. + // Phase 4: top-down pass to propagate proxies from larger layers to smaller layers. self.update_layers_and_find_pairs(events); - // 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. + // 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); } - /// Propagate regions from the smaller layers up to the larger layers. + /// Propagate regions from the smallest layers up to the larger layers. /// /// Whenever a region is created on a layer `n`, then its AABB must be - /// added to all the larger layers so we can detect whaen a object + /// added to its larger layer so we can detect when an object /// in a larger layer may start interacting with objects in a smaller /// layer. fn propagate_created_regions(&mut self) { @@ -389,8 +420,9 @@ impl BroadPhase { &mut self.proxies, &mut self.region_pool, ); + layer.created_regions.clear(); } else { - // Always clear the set of created regions, even else if + // Always clear the set of created regions, even if // there is no larger layer. layer.created_regions.clear(); } diff --git a/src/geometry/broad_phase_multi_sap/sap_layer.rs b/src/geometry/broad_phase_multi_sap/sap_layer.rs index 8242cca..1ee4468 100644 --- a/src/geometry/broad_phase_multi_sap/sap_layer.rs +++ b/src/geometry/broad_phase_multi_sap/sap_layer.rs @@ -38,23 +38,39 @@ impl SAPLayer { } } + /// Deletes from all the regions of this layer, all the endpoints corresponding + /// to subregions. Clears the arrays of subregions indices from all the regions of + /// this layer. pub fn unregister_all_subregions(&mut self, proxies: &mut SAPProxies) { for region_id in self.regions.values() { - if let Some(mut region) = proxies[*region_id].data.take_region() { - region.delete_all_region_endpoints(proxies); + // Extract the region to make the borrow-checker happy. + let mut region = proxies[*region_id] + .data + .take_region() + .expect("Should be a region proxy."); - for subregion in region.subregions.drain(..) { - proxies[subregion] - .data - .as_region_mut() - .id_in_parent_subregion = crate::INVALID_U32; - } + // Delete the endpoints. + region.delete_all_region_endpoints(proxies); - proxies[*region_id].data.set_region(region); + // Clear the subregions vec and reset the subregions parent ids. + for subregion in region.subregions.drain(..) { + proxies[subregion] + .data + .as_region_mut() + .id_in_parent_subregion = crate::INVALID_U32; } + + // Re set the region to make the borrow-checker happy. + proxies[*region_id].data.set_region(region); } } + /// Register into `larger_layer` all the region proxies of the recently-created regions + /// contained by `self`. + /// + /// This method must be called in a bottom-up loop, propagating new regions from the + /// smallest layer, up to the largest layer. That loop is done by the Phase 3 of the + /// BroadPhase::update. pub fn propagate_created_regions( &mut self, larger_layer: &mut Self, @@ -66,6 +82,7 @@ impl SAPLayer { } } + /// Register into `larger_layer` all the region proxies of the region contained in `self`. pub fn propagate_existing_regions( &mut self, larger_layer: &mut Self, @@ -77,7 +94,12 @@ impl SAPLayer { } } - // Preupdates the proxy of a subregion. + /// Registers a subregion of this layer. + /// + /// The subregion proxy will be added to the region of `self` that contains + /// that subregion center. Because the hierarchical grid cells have aligned boundaries + /// at each depth, we have the guarantee that a given subregion will only be part of + /// one region on its parent "larger" layer. fn register_subregion( &mut self, proxy_id: SAPProxyIndex, @@ -85,7 +107,9 @@ impl SAPLayer { pool: &mut SAPRegionPool, ) { if let Some(proxy) = proxies.get(proxy_id) { - if proxy.data.as_region().id_in_parent_subregion == crate::INVALID_U32 { + let curr_id_in_parent_subregion = proxy.data.as_region().id_in_parent_subregion; + + if curr_id_in_parent_subregion == crate::INVALID_U32 { let region_key = super::point_key(proxy.aabb.center(), self.region_width); let region_id = self.ensure_region_exists(region_key, proxies, pool); let region = proxies[region_id].data.as_region_mut(); @@ -95,6 +119,20 @@ impl SAPLayer { .data .as_region_mut() .id_in_parent_subregion = id_in_parent_subregion as u32; + } else { + // NOTE: all the following are just assertions to make sure the + // region ids are correctly wired. If this piece of code causes + // any performance problem, it can be deleted completely without + // hesitation. + if curr_id_in_parent_subregion != crate::INVALID_U32 { + let region_key = super::point_key(proxy.aabb.center(), self.region_width); + let region_id = self.regions.get(®ion_key).unwrap(); + let region = proxies[*region_id].data.as_region_mut(); + assert_eq!( + region.subregions[curr_id_in_parent_subregion as usize], + proxy_id + ); + } } } } @@ -138,6 +176,15 @@ impl SAPLayer { } } + /// Ensures a given region exists in this layer. + /// + /// If the region with the given region key does not exist yet, it is created. + /// When a region is created, it creates a new proxy for that region, and its + /// proxy ID is added to `self.created_region` so it can be propagated during + /// the Phase 3 of `BroadPhase::update`. + /// + /// This returns the proxy ID of the already existing region if it existed, or + /// of the new region if it did not exist and has been created by this method. pub fn ensure_region_exists( &mut self, region_key: Point<i32>, @@ -145,14 +192,19 @@ impl SAPLayer { pool: &mut SAPRegionPool, ) -> SAPProxyIndex { match self.regions.entry(region_key) { + // Yay, the region already exists! Entry::Occupied(occupied) => *occupied.get(), + // The region does not exist, create it. Entry::Vacant(vacant) => { let region_bounds = super::region_aabb(region_key, self.region_width); let region = SAPRegion::recycle_or_new(region_bounds, pool); + // Create a new proxy for that region. let region_proxy = SAPProxy::subregion(region, region_bounds, self.layer_id, self.depth); let region_proxy_id = proxies.insert(region_proxy); + // Push this region's proxy ID to the set of created regions. self.created_regions.push(region_proxy_id as u32); + // Insert the new region to this layer's region hashmap. let _ = vacant.insert(region_proxy_id); region_proxy_id } diff --git a/src/geometry/broad_phase_multi_sap/sap_region.rs b/src/geometry/broad_phase_multi_sap/sap_region.rs index 3d0834c..1c3036d 100644 --- a/src/geometry/broad_phase_multi_sap/sap_region.rs +++ b/src/geometry/broad_phase_multi_sap/sap_region.rs @@ -73,18 +73,43 @@ impl SAPRegion { } } + /// Deletes from the axes of this region all the endpoints that point + /// to a region. pub fn delete_all_region_endpoints(&mut self, proxies: &SAPProxies) { - for axis in &mut self.axes { + let mut num_deleted_subregion_endpoints = [0; DIM]; + + for (i, axis) in self.axes.iter_mut().enumerate() { let existing_proxies = &mut self.existing_proxies; axis.endpoints.retain(|e| { + // NOTE: we use `if let` instead of `unwrap` because no + // proxy will be found for the sentinels. if let Some(proxy) = proxies.get(e.proxy()) { - existing_proxies.set(e.proxy() as usize, false); - !proxy.data.is_region() - } else { - true + if proxy.data.is_region() { + existing_proxies.set(e.proxy() as usize, false); + num_deleted_subregion_endpoints[i] += 1; + return false; + } } + + true }); } + + // All axes should have deleted the same number of region endpoints. + for k in 1..DIM { + assert_eq!( + num_deleted_subregion_endpoints[0], + num_deleted_subregion_endpoints[k] + ); + } + + // The number of deleted endpoints should be even because there + // are two endpoints per proxy on each axes. + assert_eq!(num_deleted_subregion_endpoints[0] % 2, 0); + + // All the region endpoints are subproper proxies. + // So update the subproper proxy count accordingly. + self.subproper_proxy_count -= num_deleted_subregion_endpoints[0] / 2; } pub fn predelete_proxy(&mut self, _proxy_id: SAPProxyIndex) { diff --git a/src/geometry/broad_phase_multi_sap/sap_utils.rs b/src/geometry/broad_phase_multi_sap/sap_utils.rs index ac84926..43a8bb4 100644 --- a/src/geometry/broad_phase_multi_sap/sap_utils.rs +++ b/src/geometry/broad_phase_multi_sap/sap_utils.rs @@ -44,7 +44,7 @@ pub(crate) fn region_width(depth: i8) -> Real { /// /// The idea here is that an AABB should be part of a layer which has /// regions large enough so that one AABB doesn't crosses too many -/// regions. But the regions must also not bee too large, otherwise +/// regions. But the regions must also not be too large, otherwise /// we are loosing the benefits of Multi-SAP. /// /// If the code bellow, we select a layer such that each region can |