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

Fix bug wher ethe broad-phase could miss some contacts pairs.

Browse Source
This commit is contained in:
Crozet Sébastien
2021-03-26 18:16:43 +01:00
parent 97157c9423
commit 710dd8d71e
4 changed files with 139 additions and 30 deletions
Download Patch File Download Diff File Expand all files Collapse all files

58
src/geometry/broad_phase_multi_sap/broad_phase.rs
View File

@@ -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();
}

74
src/geometry/broad_phase_multi_sap/sap_layer.rs
View File

@@ -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(&region_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
}

35
src/geometry/broad_phase_multi_sap/sap_region.rs
View File

@@ -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) {

2
src/geometry/broad_phase_multi_sap/sap_utils.rs
View File

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