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

feat: switch to the new Bvh from parry for the broad-phase (#853)

Browse Source 
* feat: switch to the new Bvh from parry for the broad-phase

* chore: cargo fmt + update testbed

* chore: remove the multi-grid SAP broad-phase

* fix soft-ccd handling in broad-phase

* Fix contact cleanup in broad-phase after collider removal

* chore: clippy fixes

* fix CCD regression

* chore: update changelog

* fix build with the parallel feature enabled

* chore: remove the now useless broad-phase proxy index from colliders

* fix tests
This commit is contained in:
Sébastien Crozet
2025-07-11 22:36:40 +02:00
committed by GitHub
parent 86a257d4f1
commit 95bd6fcfeb
212 changed files with 2140 additions and 3953 deletions
Download Patch File Download Diff File Expand all files Collapse all files

18
src/geometry/broad_phase.rs
View File

@@ -1,9 +1,7 @@
use crate::dynamics::RigidBodySet;
use crate::geometry::{BroadPhasePairEvent, ColliderHandle, ColliderSet};
use parry::math::Real;
/// An internal index stored in colliders by some broad-phase algorithms.
pub type BroadPhaseProxyIndex = u32;
use crate::prelude::IntegrationParameters;
use downcast_rs::DowncastSync;
/// Trait implemented by broad-phase algorithms supported by Rapier.
///
@@ -12,7 +10,7 @@ pub type BroadPhaseProxyIndex = u32;
/// two objects don’t actually touch, but it is incorrect to remove a pair between two objects
/// that are still touching. In other words, it can have false-positive (though these induce
/// some computational overhead on the narrow-phase), but cannot have false-negative.
pub trait BroadPhase: Send + Sync + 'static {
pub trait BroadPhase: Send + Sync + 'static + DowncastSync {
/// Updates the broad-phase.
///
/// The results must be output through the `events` struct. The broad-phase algorithm is only
@@ -25,8 +23,7 @@ pub trait BroadPhase: Send + Sync + 'static {
/// **not** be modified during the broad-phase update.
///
/// # Parameters
/// - `prediction_distance`: colliders that are not exactly touching, but closer to this
/// distance must form a collision pair.
/// - `params`: the integration parameters governing the simulation.
/// - `colliders`: the set of colliders. Change detection with `collider.needs_broad_phase_update()`
/// can be relied on at this stage.
/// - `modified_colliders`: colliders that are know to be modified since the last update.
@@ -39,12 +36,13 @@ pub trait BroadPhase: Send + Sync + 'static {
/// are still touching or closer than `prediction_distance`.
fn update(
&mut self,
dt: Real,
prediction_distance: Real,
colliders: &mut ColliderSet,
params: &IntegrationParameters,
colliders: &ColliderSet,
bodies: &RigidBodySet,
modified_colliders: &[ColliderHandle],
removed_colliders: &[ColliderHandle],
events: &mut Vec<BroadPhasePairEvent>,
);
}
downcast_rs::impl_downcast!(sync BroadPhase);

270
src/geometry/broad_phase_bvh.rs Normal file
View File

@@ -0,0 +1,270 @@
use crate::dynamics::{IntegrationParameters, RigidBodySet};
use crate::geometry::{BroadPhase, BroadPhasePairEvent, ColliderHandle, ColliderPair, ColliderSet};
use parry::bounding_volume::BoundingVolume;
use parry::partitioning::{Bvh, BvhWorkspace};
use parry::utils::hashmap::{Entry, HashMap};
/// A broad-phase based on parry’s [`Bvh`] data structure.
#[derive(Default, Clone)]
#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
pub struct BroadPhaseBvh {
pub(crate) tree: Bvh,
#[cfg_attr(feature = "serde-serialize", serde(skip))]
workspace: BvhWorkspace,
pairs: HashMap<(ColliderHandle, ColliderHandle), u32>,
frame_index: u32,
optimization_strategy: BvhOptimizationStrategy,
}
// TODO: would be interesting to try out:
// "Fast Insertion-Based Optimization of Bounding Volume Hierarchies"
// by Bittner et al.
/// Selection of strategies to maintain through time the broad-phase BVH in shape that remains
/// efficient for collision-detection and scene queries.
#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
#[derive(Default, PartialEq, Eq, Copy, Clone)]
pub enum BvhOptimizationStrategy {
/// Different sub-trees of the BVH will be optimized at each frame.
#[default]
SubtreeOptimizer,
/// Disables incremental BVH optimization (discouraged).
///
/// This should not be used except for debugging purpose.
None,
}
const ENABLE_TREE_VALIDITY_CHECK: bool = false;
impl BroadPhaseBvh {
/// Initializes a new empty broad-phase.
pub fn new() -> Self {
Self::default()
}
/// Initializes a new empty broad-phase with the specified strategy for incremental
/// BVH optimization.
pub fn with_optimization_strategy(optimization_strategy: BvhOptimizationStrategy) -> Self {
Self {
optimization_strategy,
..Default::default()
}
}
fn update_with_strategy(
&mut self,
params: &IntegrationParameters,
colliders: &ColliderSet,
bodies: &RigidBodySet,
modified_colliders: &[ColliderHandle],
removed_colliders: &[ColliderHandle],
events: &mut Vec<BroadPhasePairEvent>,
strategy: BvhOptimizationStrategy,
) {
const CHANGE_DETECTION_ENABLED: bool = true;
self.frame_index = self.frame_index.overflowing_add(1).0;
// Removals must be handled first, in case another collider in
// `modified_colliders` shares the same index.
for handle in removed_colliders {
self.tree.remove(handle.into_raw_parts().0);
}
if modified_colliders.is_empty() {
return;
}
let first_pass = self.tree.is_empty();
// let t0 = std::time::Instant::now();
for modified in modified_colliders {
if let Some(collider) = colliders.get(*modified) {
if !collider.is_enabled() || !collider.changes.needs_broad_phase_update() {
continue;
}
// Take soft-ccd into account by growing the aabb.
let next_pose = collider.parent.and_then(|p| {
let parent = bodies.get(p.handle)?;
(parent.soft_ccd_prediction() > 0.0).then(|| {
parent.predict_position_using_velocity_and_forces_with_max_dist(
params.dt,
parent.soft_ccd_prediction(),
) * p.pos_wrt_parent
})
});
let prediction_distance = params.prediction_distance();
let mut aabb = collider.compute_collision_aabb(prediction_distance / 2.0);
if let Some(next_pose) = next_pose {
let next_aabb = collider
.shape
.compute_aabb(&next_pose)
.loosened(collider.contact_skin() + prediction_distance / 2.0);
aabb.merge(&next_aabb);
}
let change_detection_skin = if CHANGE_DETECTION_ENABLED {
1.0e-2 * params.length_unit
} else {
0.0
};
self.tree.insert_or_update_partially(
aabb,
modified.into_raw_parts().0,
change_detection_skin,
);
}
}
if ENABLE_TREE_VALIDITY_CHECK {
if first_pass {
self.tree.assert_well_formed();
}
self.tree.assert_well_formed_topology_only();
}
// let t0 = std::time::Instant::now();
match strategy {
BvhOptimizationStrategy::SubtreeOptimizer => {
self.tree.optimize_incremental(&mut self.workspace);
}
BvhOptimizationStrategy::None => {}
};
// println!(
// "Incremental optimization: {}",
// t0.elapsed().as_secs_f32() * 1000.0
// );
// NOTE: we run refit after optimization so we can skip updating internal nodes during
// optimization, and so we can reorder the tree in memory (in depth-first order)
// to make it more cache friendly after the rebuild shuffling everything around.
// let t0 = std::time::Instant::now();
self.tree.refit(&mut self.workspace);
if ENABLE_TREE_VALIDITY_CHECK {
self.tree.assert_well_formed();
}
// println!("Refit: {}", t0.elapsed().as_secs_f32() * 1000.0);
// println!(
// "leaf count: {}/{} (changed: {})",
// self.tree.leaf_count(),
// self.tree.reachable_leaf_count(0),
// self.tree.changed_leaf_count(0),
// );
// self.tree.assert_is_depth_first();
// self.tree.assert_well_formed();
// println!(
// "Is well formed. Tree height: {}",
// self.tree.subtree_height(0),
// );
// // println!("Tree quality: {}", self.tree.quality_metric());
let mut pairs_collector = |co1: u32, co2: u32| {
assert_ne!(co1, co2);
let Some((_, mut handle1)) = colliders.get_unknown_gen(co1) else {
return;
};
let Some((_, mut handle2)) = colliders.get_unknown_gen(co2) else {
return;
};
if co1 > co2 {
std::mem::swap(&mut handle1, &mut handle2);
}
match self.pairs.entry((handle1, handle2)) {
Entry::Occupied(e) => *e.into_mut() = self.frame_index,
Entry::Vacant(e) => {
e.insert(self.frame_index);
events.push(BroadPhasePairEvent::AddPair(ColliderPair::new(
handle1, handle2,
)));
}
}
};
// let t0 = std::time::Instant::now();
self.tree
.traverse_bvtt_single_tree::<CHANGE_DETECTION_ENABLED>(
&mut self.workspace,
&mut pairs_collector,
);
// println!("Detection: {}", t0.elapsed().as_secs_f32() * 1000.0);
// println!(">>>>>> Num events: {}", events.iter().len());
// Find outdated entries.
// TODO PERF:
// Currently, the narrow-phase isn’t capable of removing its own outdated
// collision pairs. So we need to run a pass here to find aabbs that are
// no longer overlapping. This, and the pair deduplication happening in
// the `pairs_collector` is expensive and should be done more efficiently
// by the narrow-phase itself (or islands) once we rework it.
//
// let t0 = std::time::Instant::now();
self.pairs.retain(|(h0, h1), timestamp| {
if *timestamp != self.frame_index {
if !colliders.contains(*h0) || !colliders.contains(*h1) {
// At least one of the colliders no longer exist, don’t retain the pair.
return false;
}
let Some(node0) = self.tree.leaf_node(h0.into_raw_parts().0) else {
return false;
};
let Some(node1) = self.tree.leaf_node(h1.into_raw_parts().0) else {
return false;
};
if (!CHANGE_DETECTION_ENABLED || node0.is_changed() || node1.is_changed())
&& !node0.intersects(node1)
{
events.push(BroadPhasePairEvent::DeletePair(ColliderPair::new(*h0, *h1)));
false
} else {
true
}
} else {
// If the timestamps match, we already saw this pair during traversal.
// There can be rare occurrences where the timestamp will be equal
// even though we didn’t see the pair during traversal. This happens
// if the frame index overflowed. But this is fine, we’ll catch it
// in another frame.
true
}
});
// println!(
// "Post-filtering: {} (added pairs: {}, removed pairs: {})",
// t0.elapsed().as_secs_f32() * 1000.0,
// added_pairs,
// removed_pairs
// );
}
}
impl BroadPhase for BroadPhaseBvh {
fn update(
&mut self,
params: &IntegrationParameters,
colliders: &ColliderSet,
bodies: &RigidBodySet,
modified_colliders: &[ColliderHandle],
removed_colliders: &[ColliderHandle],
events: &mut Vec<BroadPhasePairEvent>,
) {
self.update_with_strategy(
params,
colliders,
bodies,
modified_colliders,
removed_colliders,
events,
self.optimization_strategy,
);
}
}

702
src/geometry/broad_phase_multi_sap/broad_phase_multi_sap.rs
View File

@@ -1,702 +0,0 @@
use super::{
BroadPhasePairEvent, ColliderPair, SAPLayer, SAPProxies, SAPProxy, SAPProxyData, SAPRegionPool,
};
use crate::geometry::{
BroadPhaseProxyIndex, Collider, ColliderBroadPhaseData, ColliderChanges, ColliderHandle,
ColliderSet,
};
use crate::math::{Isometry, Real};
use crate::prelude::{BroadPhase, RigidBodySet};
use crate::utils::IndexMut2;
use parry::bounding_volume::BoundingVolume;
use parry::utils::hashmap::HashMap;
/// A broad-phase combining a Hierarchical Grid and Sweep-and-Prune.
///
/// The basic Sweep-and-Prune (SAP) algorithm has one significant flaws:
/// the interactions between far-away objects. This means that objects
/// that are very far away will still have some of their endpoints swapped
/// within the SAP data-structure. This results in poor scaling because this
/// results in lots of swapping between endpoints of Aabbs that won't ever
/// actually interact.
///
/// The first optimization to address this problem is to use the Multi-SAP
/// method. This basically combines an SAP with a grid. The grid subdivides
/// the spaces into equally-sized subspaces (grid cells). Each subspace, which we call
/// a "region" contains an SAP instance (i.e. there SAP axes responsible for
/// collecting endpoints and swapping them when they move to detect interaction pairs).
/// Each Aabb is inserted in all the regions it intersects.
/// This prevents the far-away problem because two objects that are far away will
/// be located on different regions. So their endpoints will never meet.
///
/// However, the Multi-SAP approach has one notable problem: the region size must
/// be chosen wisely. It could be user-defined, but that's makes it more difficult
/// to use (for the end-user). Or it can be given a fixed value. Using a fixed
/// value may result in large objects intersecting lots of regions, resulting in
/// poor performances and very high memory usage.
///
/// So a solution to that large-objects problem is the Multi-SAP approach is to
/// replace the grid by a hierarchical grid. A hierarchical grid is composed of
/// several layers. And each layer have different region sizes. For example all
/// the regions on layer 0 will have the size 1x1x1. All the regions on the layer
/// 1 will have the size 10x10x10, etc. That way, a given Aabb will be inserted
/// on the layer that has regions big enough to avoid the large-object problem.
/// For example a 20x20x20 object will be inserted in the layer with region
/// of size 10x10x10, resulting in only 8 regions being intersect by the Aabb.
/// (If it was inserted in the layer with regions of size 1x1x1, it would have intersected
/// 8000 regions, which is a problem performance-wise.)
///
/// We call this new method the Hierarchical-SAP.
///
/// Now with the Hierarchical-SAP, we can update each layer independently from one another.
/// However, objects belonging to different layers will never be detected as intersecting that
/// way. So we need a way to do inter-layer interference detection. There is a lot ways of doing
/// this: performing inter-layer Multi-Box-Pruning passes is one example (but this is not what we do).
/// In our implementation, we do the following:
/// - The Aabb bounds of each region of the layer `n` are inserted into the corresponding larger region
/// of the layer `n + 1`.
/// - When an Aabb in the region of the layer `n + 1` intersects the Aabb corresponding to one of the
/// regions at the smaller layer `n`, we add that Aabb to that smaller region.
///
/// So in the end it means that a given Aabb will be inserted into all the region it intersects at
/// the layer `n`. And it will also be inserted into all the regions it intersects at the smaller layers
/// (the layers `< n`), but only for the regions that already exist (so we don't have to discretize
/// our Aabb into the layers `< n`). This involves a fair amount of bookkeeping unfortunately, but
/// this has the benefit of keep the overall complexity of the algorithm O(1) in the typical specially
/// coherent scenario.
///
/// From an implementation point-of-view, our hierarchical SAP is implemented with the following structures:
/// - There is one `SAPLayer` per layer of the hierarchical grid.
/// - Each `SAPLayer` contains multiple `SAPRegion` (each being a region of the grid represented by that layer).
/// - Each `SAPRegion` contains three `SAPAxis`, representing the "classical" SAP algorithm running on this region.
/// - Each `SAPAxis` maintains a sorted list of `SAPEndpoints` representing the endpoints of the Aabbs intersecting
/// the bounds on the `SAPRegion` containing this `SAPAxis`.
/// - A set of `SAPProxy` are maintained separately. It contains the Aabbs of all the colliders managed by this
/// broad-phase, as well as the Aabbs of all the regions part of this broad-phase.
#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
#[derive(Clone)]
pub struct BroadPhaseMultiSap {
proxies: SAPProxies,
layers: Vec<SAPLayer>,
smallest_layer: u8,
largest_layer: u8,
// 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.
#[cfg_attr(
feature = "serde-serialize",
serde(
serialize_with = "crate::utils::serde::serialize_to_vec_tuple",
deserialize_with = "crate::utils::serde::deserialize_from_vec_tuple"
)
)]
colliders_proxy_ids: HashMap<ColliderHandle, BroadPhaseProxyIndex>,
#[cfg_attr(feature = "serde-serialize", serde(skip))]
region_pool: SAPRegionPool, // To avoid repeated allocations.
// We could think serializing this workspace is useless.
// It turns out is is important to serialize at least its capacity
// and restore this capacity when deserializing the hashmap.
// This is because the order of future elements inserted into the
// hashmap depends on its capacity (because the internal bucket indices
// depend on this capacity). So not restoring this capacity may alter
// the order at which future elements are reported. This will in turn
// alter the order at which the pairs are registered in the narrow-phase,
// thus altering the order of the contact manifold. In the end, this
// alters the order of the resolution of contacts, resulting in
// diverging simulation after restoration of a snapshot.
#[cfg_attr(
feature = "serde-serialize",
serde(
serialize_with = "parry::utils::hashmap::serialize_hashmap_capacity",
deserialize_with = "parry::utils::hashmap::deserialize_hashmap_capacity"
)
)]
reporting: HashMap<(u32, u32), bool>, // Workspace
}
impl Default for BroadPhaseMultiSap {
fn default() -> Self {
Self::new()
}
}
impl BroadPhaseMultiSap {
/// Create a new empty broad-phase.
pub fn new() -> Self {
BroadPhaseMultiSap {
proxies: SAPProxies::new(),
layers: Vec::new(),
smallest_layer: 0,
largest_layer: 0,
region_pool: Vec::new(),
reporting: HashMap::default(),
colliders_proxy_ids: HashMap::default(),
}
}
/// 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 `BroadPhaseMultiSap::update`.
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);
}
}
}
/// 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 actually removing them
/// from all the relevant layers/regions/axes.
fn predelete_proxy(&mut self, proxy_index: BroadPhaseProxyIndex) {
if proxy_index == crate::INVALID_U32 {
// This collider has not been added to the broad-phase yet.
return;
}
let proxy = &mut self.proxies[proxy_index];
let layer = &mut self.layers[proxy.layer_id as usize];
// Let the layer know that the proxy is being deleted.
layer.predelete_proxy(&mut self.proxies, proxy_index);
}
/// Completes the removal of the deleted proxies.
///
/// If `self.predelete_proxy` was called, then this `complete_removals`
/// method must be called to complete the removals.
///
/// 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,
removed_colliders: &[ColliderHandle],
) {
// If there is no layer, there is nothing to remove.
if self.layers.is_empty() {
return;
}
// This is a bottom-up pass:
// - Complete the removal on the layer `n`. This may cause so regions to be deleted.
// - Continue with the layer `n + 1`. This will delete from `n + 1` all the proxies
// of the regions originating from `n`.
// This bottom-up approach will propagate region removal from the smallest layer up
// to the largest layer.
let mut curr_layer_id = self.smallest_layer;
loop {
let curr_layer = &mut self.layers[curr_layer_id as usize];
if let Some(larger_layer_id) = curr_layer.larger_layer {
let (curr_layer, larger_layer) = self
.layers
.index_mut2(curr_layer_id as usize, larger_layer_id as usize);
curr_layer.complete_removals(
Some(larger_layer),
&mut self.proxies,
&mut self.region_pool,
);
// NOTE: we don't care about reporting pairs.
self.reporting.clear();
curr_layer_id = larger_layer_id;
} else {
curr_layer.complete_removals(None, &mut self.proxies, &mut self.region_pool);
// NOTE: we don't care about reporting pairs.
self.reporting.clear();
break;
}
}
/*
* Actually remove the colliders proxies.
*/
for removed in removed_colliders {
#[cfg(feature = "enhanced-determinism")]
let proxy_id = self.colliders_proxy_ids.swap_remove(removed);
#[cfg(not(feature = "enhanced-determinism"))]
let proxy_id = self.colliders_proxy_ids.remove(removed);
if let Some(proxy_id) = proxy_id {
if proxy_id != crate::INVALID_U32 {
self.proxies.remove(proxy_id);
}
}
if let Some(co) = colliders.get_mut_internal(*removed) {
// Reset the proxy index.
co.bf_data.proxy_index = crate::INVALID_U32;
}
}
}
/// 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.
#[profiling::function]
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 {
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);
smaller_layer.propagate_existing_regions(
new_layer,
&mut self.proxies,
&mut self.region_pool,
);
}
}
/// 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.
#[profiling::function]
fn ensure_layer_exists(&mut self, new_depth: i8) -> u8 {
// Special case: we don't have any layers yet.
if self.layers.is_empty() {
let layer_id = self.layers.len() as u8; // TODO: check overflow.
self.layers
.push(SAPLayer::new(new_depth, layer_id, None, None));
return 0;
}
// Find the first layer with a depth larger or equal to new_depth.
let mut larger_layer_id = Some(self.smallest_layer);
while let Some(curr_layer_id) = larger_layer_id {
if self.layers[curr_layer_id as usize].depth >= new_depth {
break;
}
larger_layer_id = self.layers[curr_layer_id as usize].larger_layer;
}
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);
self.layers.push(SAPLayer::new(
new_depth,
new_layer_id,
Some(self.largest_layer),
None,
));
self.largest_layer = new_layer_id;
self.finalize_layer_insertion(new_layer_id);
new_layer_id
}
Some(larger_layer_id) => {
if self.layers[larger_layer_id as usize].depth == new_depth {
// Found it! The layer already exists.
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);
if let Some(smaller_layer_id) = smaller_layer_id {
self.layers[smaller_layer_id as usize].larger_layer = Some(new_layer_id);
} else {
self.smallest_layer = new_layer_id;
}
self.layers.push(SAPLayer::new(
new_depth,
new_layer_id,
smaller_layer_id,
Some(larger_layer_id),
));
self.finalize_layer_insertion(new_layer_id);
new_layer_id
}
}
}
}
fn handle_modified_collider(
&mut self,
prediction_distance: Real,
handle: ColliderHandle,
proxy_index: &mut u32,
collider: &Collider,
next_position: Option<&Isometry<Real>>,
) -> bool {
let mut aabb = collider.compute_collision_aabb(prediction_distance / 2.0);
if let Some(next_position) = next_position {
let next_aabb = collider
.shape
.compute_aabb(next_position)
.loosened(collider.contact_skin() + prediction_distance / 2.0);
aabb.merge(&next_aabb);
}
if aabb.mins.coords.iter().any(|e| !e.is_finite())
|| aabb.maxs.coords.iter().any(|e| !e.is_finite())
{
// Reject Aabbs with non-finite values.
return false;
}
aabb.mins = super::clamp_point(aabb.mins);
aabb.maxs = super::clamp_point(aabb.maxs);
let prev_aabb;
let layer_id = if let Some(proxy) = self.proxies.get_mut(*proxy_index) {
let mut layer_id = proxy.layer_id;
prev_aabb = proxy.aabb;
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, *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;
self.proxies[*proxy_index].layer_depth = new_layer_depth;
}
}
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);
prev_aabb = aabb;
*proxy_index = self.proxies.insert(proxy);
layer_id
};
let layer = &mut self.layers[layer_id as usize];
// Preupdate the collider in the layer.
// We need to use both the prev Aabb and the new Aabb for this update, to
// handle special cases where one Aabb has left a region that doesn’t contain
// any other modified Aabbs.
// If the combination of both previous and new aabbs isn’t more than 25% bigger
// than the new Aabb, we just merge them to save some computation times (to avoid
// discretizing twice the area at their intersection. If it’s bigger than 25% then
// we discretize both aabbs individually.
let merged_aabbs = prev_aabb.merged(&aabb);
if merged_aabbs.volume() > aabb.volume() * 1.25 {
layer.preupdate_collider(
*proxy_index,
&aabb,
None,
&mut self.proxies,
&mut self.region_pool,
);
layer.preupdate_collider(
*proxy_index,
&prev_aabb,
Some(&aabb),
&mut self.proxies,
&mut self.region_pool,
);
} else {
layer.preupdate_collider(
*proxy_index,
&merged_aabbs,
Some(&aabb),
&mut self.proxies,
&mut self.region_pool,
);
}
// Returns true if propagation is needed.
!layer.created_regions.is_empty()
}
/// 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 its larger layer so we can detect when an object
/// in a larger layer may start interacting with objects in a smaller
/// layer.
#[profiling::function]
fn propagate_created_regions(&mut self) {
let mut curr_layer = Some(self.smallest_layer);
while let Some(curr_layer_id) = curr_layer {
let layer = &mut self.layers[curr_layer_id as usize];
let larger_layer = layer.larger_layer;
if !layer.created_regions.is_empty() {
if let Some(larger_layer) = larger_layer {
let (layer, larger_layer) = self
.layers
.index_mut2(curr_layer_id as usize, larger_layer as usize);
layer.propagate_created_regions(
larger_layer,
&mut self.proxies,
&mut self.region_pool,
);
layer.created_regions.clear();
} else {
// Always clear the set of created regions, even if
// there is no larger layer.
layer.created_regions.clear();
}
}
curr_layer = larger_layer;
}
}
#[profiling::function]
fn update_layers_and_find_pairs(&mut self, out_events: &mut Vec<BroadPhasePairEvent>) {
if self.layers.is_empty() {
return;
}
// This is a top-down operation: we start by updating the largest
// layer, and continue down to the smallest layer.
//
// This must be top-down because:
// 1. If a non-region proxy from the layer `n` interacts with one of
// the regions from the layer `n - 1`, it must be added to that
// smaller layer before that smaller layer is updated.
// 2. If a region has been updated, then all its subregions at the
// layer `n - 1` must be marked as "needs-to-be-updated" too, in
// order to account for the fact that a big proxy moved.
// NOTE: this 2nd point could probably be improved: instead of updating
// all the subregions, we could perhaps just update the subregions
// that crosses the boundary of the Aabb of the big proxies that
// moved in they layer `n`.
let mut layer_id = Some(self.largest_layer);
while let Some(curr_layer_id) = layer_id {
self.layers[curr_layer_id as usize]
.update_regions(&mut self.proxies, &mut self.reporting);
layer_id = self.layers[curr_layer_id as usize].smaller_layer;
for ((proxy_id1, proxy_id2), colliding) in &self.reporting {
let (proxy1, proxy2) = self
.proxies
.elements
.index_mut2(*proxy_id1 as usize, *proxy_id2 as usize);
match (&mut proxy1.data, &mut proxy2.data) {
(SAPProxyData::Collider(handle1), SAPProxyData::Collider(handle2)) => {
if *colliding {
out_events.push(BroadPhasePairEvent::AddPair(ColliderPair::new(
*handle1, *handle2,
)));
} else {
out_events.push(BroadPhasePairEvent::DeletePair(ColliderPair::new(
*handle1, *handle2,
)));
}
}
(SAPProxyData::Collider(_), SAPProxyData::Region(_)) => {
if *colliding {
// Add the collider to the subregion.
proxy2
.data
.as_region_mut()
.preupdate_proxy(*proxy_id1, false);
}
}
(SAPProxyData::Region(_), SAPProxyData::Collider(_)) => {
if *colliding {
// Add the collider to the subregion.
proxy1
.data
.as_region_mut()
.preupdate_proxy(*proxy_id2, false);
}
}
(SAPProxyData::Region(_), SAPProxyData::Region(_)) => {
// This will only happen between two adjacent subregions because
// they share some identical bounds. So this case does not matter.
}
}
}
self.reporting.clear();
}
}
}
impl BroadPhase for BroadPhaseMultiSap {
/// Updates the broad-phase, taking into account the new collider positions.
#[profiling::function]
fn update(
&mut self,
dt: Real,
prediction_distance: Real,
colliders: &mut ColliderSet,
bodies: &RigidBodySet,
modified_colliders: &[ColliderHandle],
removed_colliders: &[ColliderHandle],
events: &mut Vec<BroadPhasePairEvent>,
) {
// Phase 1: pre-delete the collisions that have been deleted.
self.handle_removed_colliders(removed_colliders);
let mut need_region_propagation = false;
// Phase 2: pre-delete the collisions that have been deleted.
for handle in modified_colliders {
// NOTE: we use `get` because the collider may no longer
// exist if it has been removed.
if let Some(co) = colliders.get_mut_internal(*handle) {
if !co.is_enabled() || !co.changes.needs_broad_phase_update() {
continue;
}
let mut new_proxy_id = co.bf_data.proxy_index;
let next_pos = co.parent.and_then(|p| {
let parent = bodies.get(p.handle)?;
(parent.soft_ccd_prediction() > 0.0).then(|| {
parent.predict_position_using_velocity_and_forces_with_max_dist(
dt,
parent.soft_ccd_prediction(),
) * p.pos_wrt_parent
})
});
if self.handle_modified_collider(
prediction_distance,
*handle,
&mut new_proxy_id,
co,
next_pos.as_ref(),
) {
need_region_propagation = true;
}
if co.bf_data.proxy_index != new_proxy_id {
self.colliders_proxy_ids.insert(*handle, new_proxy_id);
// Make sure we have the new proxy index in case
// the collider was added for the first time.
co.bf_data = 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 {
self.propagate_created_regions();
}
// Phase 4: top-down pass to propagate proxies from larger layers to smaller layers.
self.update_layers_and_find_pairs(events);
// 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, removed_colliders);
}
}
#[cfg(test)]
mod test {
use crate::dynamics::{
ImpulseJointSet, IslandManager, MultibodyJointSet, RigidBodyBuilder, RigidBodySet,
};
use crate::geometry::{BroadPhase, BroadPhaseMultiSap, ColliderBuilder, ColliderSet};
#[test]
fn test_add_update_remove() {
let mut broad_phase = BroadPhaseMultiSap::new();
let mut bodies = RigidBodySet::new();
let mut colliders = ColliderSet::new();
let mut impulse_joints = ImpulseJointSet::new();
let mut multibody_joints = MultibodyJointSet::new();
let mut islands = IslandManager::new();
let rb = RigidBodyBuilder::dynamic().build();
let co = ColliderBuilder::ball(0.5).build();
let hrb = bodies.insert(rb);
let coh = colliders.insert_with_parent(co, hrb, &mut bodies);
let mut events = Vec::new();
broad_phase.update(0.0, 0.0, &mut colliders, &bodies, &[coh], &[], &mut events);
bodies.remove(
hrb,
&mut islands,
&mut colliders,
&mut impulse_joints,
&mut multibody_joints,
true,
);
broad_phase.update(0.0, 0.0, &mut colliders, &bodies, &[], &[coh], &mut events);
// Create another body.
let rb = RigidBodyBuilder::dynamic().build();
let co = ColliderBuilder::ball(0.5).build();
let hrb = bodies.insert(rb);
let coh = colliders.insert_with_parent(co, hrb, &mut bodies);
// Make sure the proxy handles is recycled properly.
broad_phase.update(0.0, 0.0, &mut colliders, &bodies, &[coh], &[], &mut events);
}
}

18
src/geometry/broad_phase_multi_sap/mod.rs
View File

@@ -1,18 +0,0 @@
pub use self::broad_phase_multi_sap::BroadPhaseMultiSap;
pub use self::broad_phase_pair_event::{BroadPhasePairEvent, ColliderPair};
use self::sap_axis::*;
use self::sap_endpoint::*;
use self::sap_layer::*;
use self::sap_proxy::*;
use self::sap_region::*;
use self::sap_utils::*;
mod broad_phase_multi_sap;
mod broad_phase_pair_event;
mod sap_axis;
mod sap_endpoint;
mod sap_layer;
mod sap_proxy;
mod sap_region;
mod sap_utils;

286
src/geometry/broad_phase_multi_sap/sap_axis.rs
View File

@@ -1,286 +0,0 @@
use super::{SAPEndpoint, SAPProxies, NUM_SENTINELS};
use crate::geometry::broad_phase_multi_sap::DELETED_AABB_VALUE;
use crate::geometry::BroadPhaseProxyIndex;
use crate::math::Real;
use bit_vec::BitVec;
use parry::bounding_volume::BoundingVolume;
use parry::utils::hashmap::HashMap;
use std::cmp::Ordering;
#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
#[derive(Clone, Debug)]
pub struct SAPAxis {
pub min_bound: Real,
pub max_bound: Real,
pub endpoints: Vec<SAPEndpoint>,
#[cfg_attr(feature = "serde-serialize", serde(skip))]
pub new_endpoints: Vec<(SAPEndpoint, usize)>, // Workspace
}
impl SAPAxis {
pub fn new(min_bound: Real, max_bound: Real) -> Self {
assert!(min_bound <= max_bound);
Self {
min_bound,
max_bound,
endpoints: vec![SAPEndpoint::start_sentinel(), SAPEndpoint::end_sentinel()],
new_endpoints: Vec::new(),
}
}
pub fn clear(&mut self) {
self.new_endpoints.clear();
self.endpoints.clear();
self.endpoints.push(SAPEndpoint::start_sentinel());
self.endpoints.push(SAPEndpoint::end_sentinel());
}
#[profiling::function]
pub fn batch_insert(
&mut self,
dim: usize,
new_proxies: &[BroadPhaseProxyIndex],
proxies: &SAPProxies,
reporting: Option<&mut HashMap<(u32, u32), bool>>,
) {
if new_proxies.is_empty() {
return;
}
self.new_endpoints.clear();
for proxy_id in new_proxies {
let proxy = &proxies[*proxy_id];
assert!(
proxy.aabb.mins[dim] <= self.max_bound,
"proxy.aabb.mins {} (in {:?}) <= max_bound {}",
proxy.aabb.mins[dim],
proxy.aabb,
self.max_bound
);
assert!(
proxy.aabb.maxs[dim] >= self.min_bound,
"proxy.aabb.maxs {} (in {:?}) >= min_bound {}",
proxy.aabb.maxs[dim],
proxy.aabb,
self.min_bound
);
let start_endpoint = SAPEndpoint::start_endpoint(proxy.aabb.mins[dim], *proxy_id);
let end_endpoint = SAPEndpoint::end_endpoint(proxy.aabb.maxs[dim], *proxy_id);
self.new_endpoints.push((start_endpoint, 0));
self.new_endpoints.push((end_endpoint, 0));
}
self.new_endpoints
.sort_by(|a, b| a.0.value.partial_cmp(&b.0.value).unwrap_or(Ordering::Equal));
let mut curr_existing_index = self.endpoints.len() - NUM_SENTINELS - 1;
let new_num_endpoints = self.endpoints.len() + self.new_endpoints.len();
self.endpoints
.resize(new_num_endpoints, SAPEndpoint::end_sentinel());
let mut curr_shift_index = new_num_endpoints - NUM_SENTINELS - 1;
// Sort the endpoints.
// TODO: specialize for the case where this is the
// first time we insert endpoints to this axis?
for new_endpoint in self.new_endpoints.iter_mut().rev() {
loop {
let existing_endpoint = self.endpoints[curr_existing_index];
if existing_endpoint.value <= new_endpoint.0.value {
break;
}
self.endpoints[curr_shift_index] = existing_endpoint;
curr_shift_index -= 1;
curr_existing_index -= 1;
}
self.endpoints[curr_shift_index] = new_endpoint.0;
new_endpoint.1 = curr_shift_index;
curr_shift_index -= 1;
}
// Report pairs using a single mbp pass on each new endpoint.
let endpoints_wo_last_sentinel = &self.endpoints[..self.endpoints.len() - 1];
if let Some(reporting) = reporting {
for (endpoint, endpoint_id) in self.new_endpoints.drain(..).filter(|e| e.0.is_start()) {
let proxy1 = &proxies[endpoint.proxy()];
let min = endpoint.value;
let max = proxy1.aabb.maxs[dim];
for endpoint2 in &endpoints_wo_last_sentinel[endpoint_id + 1..] {
if endpoint2.proxy() == endpoint.proxy() {
continue;
}
let proxy2 = &proxies[endpoint2.proxy()];
// NOTE: some pairs with equal aabb.mins[dim] may end up being reported twice.
if (endpoint2.is_start() && endpoint2.value < max)
|| (endpoint2.is_end() && proxy2.aabb.mins[dim] <= min)
{
// Report pair.
if proxy1.aabb.intersects(&proxy2.aabb) {
// Report pair.
let pair = super::sort2(endpoint.proxy(), endpoint2.proxy());
reporting.insert(pair, true);
}
}
}
}
}
}
/// Removes from this axis all the endpoints that are out of bounds from this axis.
///
/// Returns the number of deleted proxies as well as the number of proxies deleted
/// such that `proxy.layer_depth <= layer_depth`.
pub fn delete_out_of_bounds_proxies(
&self,
proxies: &SAPProxies,
existing_proxies: &mut BitVec,
layer_depth: i8,
) -> (usize, usize) {
let mut num_subproper_proxies_deleted = 0;
let mut num_proxies_deleted = 0;
for endpoint in &self.endpoints {
if endpoint.value < self.min_bound {
let proxy_id = endpoint.proxy();
if endpoint.is_end() && existing_proxies[proxy_id as usize] {
existing_proxies.set(proxy_id as usize, false);
if proxies[proxy_id].layer_depth <= layer_depth {
num_subproper_proxies_deleted += 1;
}
num_proxies_deleted += 1;
}
} else {
break;
}
}
for endpoint in self.endpoints.iter().rev() {
if endpoint.value > self.max_bound {
let proxy_id = endpoint.proxy();
if endpoint.is_start() && existing_proxies[proxy_id as usize] {
existing_proxies.set(proxy_id as usize, false);
if proxies[proxy_id].layer_depth <= layer_depth {
num_subproper_proxies_deleted += 1;
}
num_proxies_deleted += 1;
}
} else {
break;
}
}
(num_proxies_deleted, num_subproper_proxies_deleted)
}
pub fn delete_out_of_bounds_endpoints(&mut self, existing_proxies: &BitVec) {
self.endpoints
.retain(|endpt| endpt.is_sentinel() || existing_proxies[endpt.proxy() as usize])
}
/// Removes from this axis all the endpoints corresponding to a proxy with an Aabb mins/maxs values
/// equal to DELETED_AABB_VALUE, indicating that the endpoints should be deleted.
///
/// Returns the number of deleted proxies such that `proxy.layer_depth <= layer_depth`.
pub fn delete_deleted_proxies_and_endpoints_after_subregion_removal(
&mut self,
proxies: &SAPProxies,
existing_proxies: &mut BitVec,
layer_depth: i8,
) -> usize {
let mut num_subproper_proxies_deleted = 0;
self.endpoints.retain(|endpt| {
if !endpt.is_sentinel() {
let proxy = &proxies[endpt.proxy()];
if proxy.aabb.mins.x == DELETED_AABB_VALUE {
if existing_proxies.get(endpt.proxy() as usize) == Some(true) {
existing_proxies.set(endpt.proxy() as usize, false);
if proxy.layer_depth <= layer_depth {
num_subproper_proxies_deleted += 1;
}
}
return false;
}
}
true
});
num_subproper_proxies_deleted
}
pub fn update_endpoints(
&mut self,
dim: usize,
proxies: &SAPProxies,
reporting: &mut HashMap<(u32, u32), bool>,
) {
let last_endpoint = self.endpoints.len() - NUM_SENTINELS;
for i in NUM_SENTINELS..last_endpoint {
let mut endpoint_i = self.endpoints[i];
let aabb_i = proxies[endpoint_i.proxy()].aabb;
if endpoint_i.is_start() {
endpoint_i.value = aabb_i.mins[dim];
} else {
endpoint_i.value = aabb_i.maxs[dim];
}
let mut j = i;
if endpoint_i.is_start() {
while endpoint_i.value < self.endpoints[j - 1].value {
let endpoint_j = self.endpoints[j - 1];
self.endpoints[j] = endpoint_j;
if endpoint_j.is_end() {
// Report start collision.
let proxy_j = &proxies[endpoint_j.proxy()];
if aabb_i.intersects(&proxy_j.aabb) {
let pair = super::sort2(endpoint_i.proxy(), endpoint_j.proxy());
reporting.insert(pair, true);
}
}
j -= 1;
}
} else {
while endpoint_i.value < self.endpoints[j - 1].value {
let endpoint_j = self.endpoints[j - 1];
self.endpoints[j] = endpoint_j;
if endpoint_j.is_start() {
// Report end collision.
if !aabb_i.intersects(&proxies[endpoint_j.proxy()].aabb) {
let pair = super::sort2(endpoint_i.proxy(), endpoint_j.proxy());
reporting.insert(pair, false);
}
}
j -= 1;
}
}
self.endpoints[j] = endpoint_i;
}
// println!(
// "Num start swaps: {}, end swaps: {}, dim: {}",
// num_start_swaps, num_end_swaps, dim
// );
}
}

60
src/geometry/broad_phase_multi_sap/sap_endpoint.rs
View File

@@ -1,60 +0,0 @@
use super::SENTINEL_VALUE;
use crate::math::Real;
#[derive(Copy, Clone, Debug, PartialEq)]
#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
pub struct SAPEndpoint {
pub value: Real,
pub packed_flag_proxy: u32,
}
const START_FLAG_MASK: u32 = 0b1 << 31;
const PROXY_MASK: u32 = u32::MAX ^ START_FLAG_MASK;
const START_SENTINEL_TAG: u32 = u32::MAX;
const END_SENTINEL_TAG: u32 = u32::MAX ^ START_FLAG_MASK;
impl SAPEndpoint {
pub fn start_endpoint(value: Real, proxy: u32) -> Self {
Self {
value,
packed_flag_proxy: proxy | START_FLAG_MASK,
}
}
pub fn end_endpoint(value: Real, proxy: u32) -> Self {
Self {
value,
packed_flag_proxy: proxy & PROXY_MASK,
}
}
pub fn start_sentinel() -> Self {
Self {
value: -SENTINEL_VALUE,
packed_flag_proxy: START_SENTINEL_TAG,
}
}
pub fn end_sentinel() -> Self {
Self {
value: SENTINEL_VALUE,
packed_flag_proxy: END_SENTINEL_TAG,
}
}
pub fn is_sentinel(self) -> bool {
self.packed_flag_proxy & PROXY_MASK == PROXY_MASK
}
pub fn proxy(self) -> u32 {
self.packed_flag_proxy & PROXY_MASK
}
pub fn is_start(self) -> bool {
(self.packed_flag_proxy & START_FLAG_MASK) != 0
}
pub fn is_end(self) -> bool {
(self.packed_flag_proxy & START_FLAG_MASK) == 0
}
}

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

@@ -1,409 +0,0 @@
use super::{RegionKey, SAPProxies, SAPProxy, SAPRegion, SAPRegionPool};
use crate::geometry::broad_phase_multi_sap::DELETED_AABB_VALUE;
use crate::geometry::{Aabb, BroadPhaseProxyIndex};
use crate::math::{Point, Real};
use parry::bounding_volume::BoundingVolume;
use parry::utils::hashmap::{Entry, HashMap};
#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
#[derive(Clone)]
pub(crate) struct SAPLayer {
pub depth: i8,
pub layer_id: u8,
pub smaller_layer: Option<u8>,
pub larger_layer: Option<u8>,
region_width: Real,
#[cfg_attr(
feature = "serde-serialize",
serde(
serialize_with = "crate::utils::serde::serialize_to_vec_tuple",
deserialize_with = "crate::utils::serde::deserialize_from_vec_tuple"
)
)]
pub regions: HashMap<Point<RegionKey>, BroadPhaseProxyIndex>,
#[cfg_attr(feature = "serde-serialize", serde(skip))]
regions_to_potentially_remove: Vec<Point<RegionKey>>, // Workspace
#[cfg_attr(feature = "serde-serialize", serde(skip))]
pub created_regions: Vec<BroadPhaseProxyIndex>,
}
impl SAPLayer {
pub fn new(
depth: i8,
layer_id: u8,
smaller_layer: Option<u8>,
larger_layer: Option<u8>,
) -> Self {
Self {
depth,
smaller_layer,
larger_layer,
layer_id,
region_width: super::region_width(depth),
regions: HashMap::default(),
regions_to_potentially_remove: vec![],
created_regions: vec![],
}
}
/// 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.
#[profiling::function]
pub fn unregister_all_subregions(&mut self, proxies: &mut SAPProxies) {
for region_id in self.regions.values() {
// Extract the region to make the borrow-checker happy.
let mut region = proxies[*region_id]
.data
.take_region()
.expect("Should be a region proxy.");
// Delete the endpoints.
region.delete_all_region_endpoints(proxies);
// 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
/// BroadPhaseMultiSap::update.
pub fn propagate_created_regions(
&mut self,
larger_layer: &mut Self,
proxies: &mut SAPProxies,
pool: &mut SAPRegionPool,
) {
for proxy_id in self.created_regions.drain(..) {
larger_layer.register_subregion(proxy_id, proxies, pool)
}
}
/// 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,
proxies: &mut SAPProxies,
pool: &mut SAPRegionPool,
) {
for proxy_id in self.regions.values() {
larger_layer.register_subregion(*proxy_id, proxies, pool)
}
}
/// 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.
#[profiling::function]
fn register_subregion(
&mut self,
proxy_id: BroadPhaseProxyIndex,
proxies: &mut SAPProxies,
pool: &mut SAPRegionPool,
) {
if let Some(proxy) = proxies.get(proxy_id) {
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();
let id_in_parent_subregion = region.register_subregion(proxy_id);
proxies[proxy_id]
.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
);
}
}
}
}
#[profiling::function]
fn unregister_subregion(
&mut self,
proxy_id: BroadPhaseProxyIndex,
proxy_region: &SAPRegion,
proxies: &mut SAPProxies,
) {
if let Some(proxy) = proxies.get(proxy_id) {
let id_in_parent_subregion = proxy_region.id_in_parent_subregion;
let region_key = super::point_key(proxy.aabb.center(), self.region_width);
if let Some(region_id) = self.regions.get(&region_key) {
let proxy = &mut proxies[*region_id];
let region = proxy.data.as_region_mut();
if !region.needs_update_after_subregion_removal {
self.regions_to_potentially_remove.push(region_key);
region.needs_update_after_subregion_removal = true;
}
let removed = region
.subregions
.swap_remove(id_in_parent_subregion as usize); // Remove the subregion index from the subregion list.
assert_eq!(removed, proxy_id);
// Re-adjust the id_in_parent_subregion of the subregion that was swapped in place
// of the deleted one.
if let Some(subregion_to_update) = region
.subregions
.get(id_in_parent_subregion as usize)
.copied()
{
proxies[subregion_to_update]
.data
.as_region_mut()
.id_in_parent_subregion = id_in_parent_subregion;
}
}
}
}
/// 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 `BroadPhaseMultiSap::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<RegionKey>,
proxies: &mut SAPProxies,
pool: &mut SAPRegionPool,
) -> BroadPhaseProxyIndex {
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
}
}
}
pub fn preupdate_collider(
&mut self,
proxy_id: u32,
aabb_to_discretize: &Aabb,
actual_aabb: Option<&Aabb>,
proxies: &mut SAPProxies,
pool: &mut SAPRegionPool,
) {
let start = super::point_key(aabb_to_discretize.mins, self.region_width);
let end = super::point_key(aabb_to_discretize.maxs, self.region_width);
// Discretize the aabb.
#[cfg(feature = "dim2")]
let k_range = 0..1;
#[cfg(feature = "dim3")]
let k_range = start.z..=end.z;
for i in start.x..=end.x {
for j in start.y..=end.y {
for _k in k_range.clone() {
#[cfg(feature = "dim2")]
let region_key = Point::new(i, j);
#[cfg(feature = "dim3")]
let region_key = Point::new(i, j, _k);
let region_id = self.ensure_region_exists(region_key, proxies, pool);
let region_proxy = &mut proxies[region_id];
let region = region_proxy.data.as_region_mut();
// NOTE: sometimes, rounding errors will generate start/end indices
// that lie outside of the actual region’s Aabb.
// TODO: is there a smarter, more efficient way of dealing with this?
if !region_proxy.aabb.intersects(aabb_to_discretize) {
continue;
}
if let Some(actual_aabb) = actual_aabb {
// NOTE: if the actual Aabb doesn't intersect the
// region’s Aabb, then we need to delete the
// proxy from that region because it means that
// during the last update the proxy intersected
// that region, but it doesn't intersect it any
// more during the current update.
if !region_proxy.aabb.intersects(actual_aabb) {
region.predelete_proxy(proxy_id);
continue;
}
}
region.preupdate_proxy(proxy_id, true);
}
}
}
}
#[profiling::function]
pub fn predelete_proxy(&mut self, proxies: &mut SAPProxies, proxy_index: BroadPhaseProxyIndex) {
// Discretize the Aabb to find the regions that need to be invalidated.
let proxy_aabb = &mut proxies[proxy_index].aabb;
let start = super::point_key(proxy_aabb.mins, self.region_width);
let end = super::point_key(proxy_aabb.maxs, self.region_width);
// Set the Aabb of the proxy to a very large value.
proxy_aabb.mins.coords.fill(DELETED_AABB_VALUE);
proxy_aabb.maxs.coords.fill(DELETED_AABB_VALUE);
#[cfg(feature = "dim2")]
let k_range = 0..1;
#[cfg(feature = "dim3")]
let k_range = start.z..=end.z;
for i in start.x..=end.x {
for j in start.y..=end.y {
for _k in k_range.clone() {
#[cfg(feature = "dim2")]
let key = Point::new(i, j);
#[cfg(feature = "dim3")]
let key = Point::new(i, j, _k);
if let Some(region_id) = self.regions.get(&key) {
let region = proxies[*region_id].data.as_region_mut();
region.predelete_proxy(proxy_index);
}
}
}
}
}
pub fn update_regions(
&mut self,
proxies: &mut SAPProxies,
reporting: &mut HashMap<(u32, u32), bool>,
) {
// println!(
// "Num regions at depth {}: {}",
// self.depth,
// self.regions.len(),
// );
for (point, region_id) in &self.regions {
if let Some(mut region) = proxies[*region_id].data.take_region() {
// Update the region.
region.update(proxies, self.depth, reporting);
// Mark all subregions as to-be-updated.
for subregion_id in &region.subregions {
proxies[*subregion_id].data.as_region_mut().mark_as_dirty();
}
if !region.contains_subproper_proxies() {
self.regions_to_potentially_remove.push(*point);
}
proxies[*region_id].data.set_region(region);
}
}
}
/// Complete the removals of empty regions on this layer.
///
/// This method must be called in a bottom-up fashion, i.e.,
/// by starting with the smallest layer up to the larger layer.
/// This is needed in order to properly propagate the removal
/// of a region (which is a subregion registered into the larger
/// layer).
pub fn complete_removals(
&mut self,
mut larger_layer: Option<&mut Self>,
proxies: &mut SAPProxies,
pool: &mut SAPRegionPool,
) {
// Delete all the empty regions and store them in the region pool.
for region_to_remove in self.regions_to_potentially_remove.drain(..) {
if let Entry::Occupied(region_id) = self.regions.entry(region_to_remove) {
if let Some(proxy) = proxies.get_mut(*region_id.get()) {
// First, we need to remove the endpoints of the deleted subregions.
let mut region = proxy.data.take_region().unwrap();
region.update_after_subregion_removal(proxies, self.depth);
// Check if we can actually delete this region.
if !region.contains_subproper_proxies() {
#[cfg(feature = "enhanced-determinism")]
let region_id = region_id.swap_remove();
#[cfg(not(feature = "enhanced-determinism"))]
let region_id = region_id.remove();
// We can delete this region. So we need to tell the larger
// layer that one of its subregion is being deleted.
// The next call to `complete_removals` on the larger layer
// will take care of updating its own regions by taking into
// account this deleted subregion.
if let Some(larger_layer) = &mut larger_layer {
larger_layer.unregister_subregion(region_id, &region, proxies);
}
// Move the proxy to infinity.
let proxy = &mut proxies[region_id];
proxy.aabb.mins.coords.fill(DELETED_AABB_VALUE);
proxy.aabb.maxs.coords.fill(DELETED_AABB_VALUE);
// Mark the proxy as deleted.
proxies.remove(region_id);
pool.push(region);
} else {
proxies[*region_id.get()].data.set_region(region);
}
}
}
}
}
pub fn proper_proxy_moved_to_bigger_layer(
&mut self,
proxies: &mut SAPProxies,
proxy_id: BroadPhaseProxyIndex,
) {
for (point, region_id) in &self.regions {
let region = &mut proxies[*region_id].data.as_region_mut();
let region_contains_proxy = region.proper_proxy_moved_to_a_bigger_layer(proxy_id);
// If that proper proxy was the last one keeping that region
// alive, mark the region as potentially removable.
if region_contains_proxy && !region.contains_subproper_proxies() {
self.regions_to_potentially_remove.push(*point);
}
}
}
}

138
src/geometry/broad_phase_multi_sap/sap_proxy.rs
View File

@@ -1,138 +0,0 @@
use super::NEXT_FREE_SENTINEL;
use crate::geometry::broad_phase_multi_sap::SAPRegion;
use crate::geometry::{BroadPhaseProxyIndex, ColliderHandle};
use parry::bounding_volume::Aabb;
use std::ops::{Index, IndexMut};
#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
#[derive(Clone)]
pub enum SAPProxyData {
Collider(ColliderHandle),
Region(Option<Box<SAPRegion>>),
}
impl SAPProxyData {
pub fn is_region(&self) -> bool {
matches!(self, SAPProxyData::Region(_))
}
pub fn as_region(&self) -> &SAPRegion {
match self {
SAPProxyData::Region(r) => r.as_ref().unwrap(),
_ => panic!("Invalid proxy type."),
}
}
pub fn as_region_mut(&mut self) -> &mut SAPRegion {
match self {
SAPProxyData::Region(r) => r.as_mut().unwrap(),
_ => panic!("Invalid proxy type."),
}
}
pub fn take_region(&mut self) -> Option<Box<SAPRegion>> {
match self {
SAPProxyData::Region(r) => r.take(),
_ => None,
}
}
pub fn set_region(&mut self, region: Box<SAPRegion>) {
*self = SAPProxyData::Region(Some(region));
}
}
#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
#[derive(Clone)]
pub struct SAPProxy {
pub data: SAPProxyData,
pub aabb: Aabb,
pub next_free: BroadPhaseProxyIndex,
// TODO: pack the layer_id and layer_depth into a single u16?
pub layer_id: u8,
pub layer_depth: i8,
}
impl SAPProxy {
pub fn collider(handle: ColliderHandle, aabb: Aabb, layer_id: u8, layer_depth: i8) -> Self {
Self {
data: SAPProxyData::Collider(handle),
aabb,
next_free: NEXT_FREE_SENTINEL,
layer_id,
layer_depth,
}
}
pub fn subregion(subregion: Box<SAPRegion>, aabb: Aabb, layer_id: u8, layer_depth: i8) -> Self {
Self {
data: SAPProxyData::Region(Some(subregion)),
aabb,
next_free: NEXT_FREE_SENTINEL,
layer_id,
layer_depth,
}
}
}
#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
#[derive(Clone)]
pub struct SAPProxies {
pub elements: Vec<SAPProxy>,
pub first_free: BroadPhaseProxyIndex,
}
impl Default for SAPProxies {
fn default() -> Self {
Self::new()
}
}
impl SAPProxies {
pub fn new() -> Self {
Self {
elements: Vec::new(),
first_free: NEXT_FREE_SENTINEL,
}
}
pub fn insert(&mut self, proxy: SAPProxy) -> BroadPhaseProxyIndex {
if self.first_free != NEXT_FREE_SENTINEL {
let proxy_id = self.first_free;
self.first_free = self.elements[proxy_id as usize].next_free;
self.elements[proxy_id as usize] = proxy;
proxy_id
} else {
self.elements.push(proxy);
self.elements.len() as u32 - 1
}
}
pub fn remove(&mut self, proxy_id: BroadPhaseProxyIndex) {
let proxy = &mut self.elements[proxy_id as usize];
proxy.next_free = self.first_free;
self.first_free = proxy_id;
}
// NOTE: this must not take holes into account.
pub fn get_mut(&mut self, i: BroadPhaseProxyIndex) -> Option<&mut SAPProxy> {
self.elements.get_mut(i as usize)
}
// NOTE: this must not take holes into account.
pub fn get(&self, i: BroadPhaseProxyIndex) -> Option<&SAPProxy> {
self.elements.get(i as usize)
}
}
impl Index<BroadPhaseProxyIndex> for SAPProxies {
type Output = SAPProxy;
fn index(&self, i: BroadPhaseProxyIndex) -> &SAPProxy {
self.elements.index(i as usize)
}
}
impl IndexMut<BroadPhaseProxyIndex> for SAPProxies {
fn index_mut(&mut self, i: BroadPhaseProxyIndex) -> &mut SAPProxy {
self.elements.index_mut(i as usize)
}
}

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

@@ -1,260 +0,0 @@
use super::{SAPAxis, SAPProxies};
use crate::geometry::BroadPhaseProxyIndex;
use crate::math::DIM;
use bit_vec::BitVec;
use parry::bounding_volume::Aabb;
use parry::utils::hashmap::HashMap;
pub type SAPRegionPool = Vec<Box<SAPRegion>>;
#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
#[derive(Clone)]
pub struct SAPRegion {
pub axes: [SAPAxis; DIM],
pub existing_proxies: BitVec,
#[cfg_attr(feature = "serde-serialize", serde(skip))]
pub to_insert: Vec<BroadPhaseProxyIndex>, // Workspace
pub subregions: Vec<BroadPhaseProxyIndex>,
pub id_in_parent_subregion: u32,
pub update_count: u8,
pub needs_update_after_subregion_removal: bool,
// Number of proxies (added to this region) that originates
// from the layer at depth <= the depth of the layer containing
// this region.
subproper_proxy_count: usize,
}
impl SAPRegion {
pub fn new(bounds: Aabb) -> Self {
let axes = [
SAPAxis::new(bounds.mins.x, bounds.maxs.x),
SAPAxis::new(bounds.mins.y, bounds.maxs.y),
#[cfg(feature = "dim3")]
SAPAxis::new(bounds.mins.z, bounds.maxs.z),
];
SAPRegion {
axes,
existing_proxies: BitVec::new(),
to_insert: Vec::new(),
subregions: Vec::new(),
id_in_parent_subregion: crate::INVALID_U32,
update_count: 0,
needs_update_after_subregion_removal: false,
subproper_proxy_count: 0,
}
}
pub fn recycle(bounds: Aabb, mut old: Box<Self>) -> Box<Self> {
// Correct the bounds
for i in 0..DIM {
// Make sure the axis is empty (it may still contain
// some old endpoints from non-proper proxies.
old.axes[i].clear();
old.axes[i].min_bound = bounds.mins[i];
old.axes[i].max_bound = bounds.maxs[i];
}
old.update_count = 0;
if cfg!(feature = "enhanced-determinism") {
old.existing_proxies = BitVec::new();
} else {
old.existing_proxies.clear();
}
old.id_in_parent_subregion = crate::INVALID_U32;
old.subregions.clear();
old.needs_update_after_subregion_removal = false;
// The rest of the fields should be "empty"
assert_eq!(old.subproper_proxy_count, 0);
assert!(old.to_insert.is_empty());
old
}
#[allow(clippy::vec_box)] // PERF: see if Box actually makes it faster (due to less copying).
pub fn recycle_or_new(bounds: Aabb, pool: &mut Vec<Box<Self>>) -> Box<Self> {
if let Some(old) = pool.pop() {
Self::recycle(bounds, old)
} else {
Box::new(Self::new(bounds))
}
}
/// Does this region still contain endpoints of subproper proxies?
pub fn contains_subproper_proxies(&self) -> bool {
self.subproper_proxy_count > 0
}
/// If this region contains the given proxy, this will decrement this region's proxy count.
///
/// Returns `true` if this region contained the proxy. Returns `false` otherwise.
pub fn proper_proxy_moved_to_a_bigger_layer(&mut self, proxy_id: BroadPhaseProxyIndex) -> bool {
if self.existing_proxies.get(proxy_id as usize) == Some(true) {
// NOTE: we are just registering the fact that that proxy isn't a
// subproper proxy anymore. But it is still part of this region
// so we must not set `self.existing_proxies[proxy_id] = false`
// nor delete its endpoints.
self.subproper_proxy_count -= 1;
true
} else {
false
}
}
/// 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) {
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()) {
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: BroadPhaseProxyIndex) {
// We keep the proxy_id as argument for uniformity with the "preupdate"
// method. However we don't actually need it because the deletion will be
// handled transparently during the next update.
self.update_count = self.update_count.max(1);
}
pub fn mark_as_dirty(&mut self) {
self.update_count = self.update_count.max(1);
}
pub fn register_subregion(&mut self, proxy_id: BroadPhaseProxyIndex) -> usize {
let subregion_index = self.subregions.len();
self.subregions.push(proxy_id);
self.preupdate_proxy(proxy_id, true);
subregion_index
}
pub fn preupdate_proxy(
&mut self,
proxy_id: BroadPhaseProxyIndex,
is_subproper_proxy: bool,
) -> bool {
let mask_len = self.existing_proxies.len();
if proxy_id as usize >= mask_len {
self.existing_proxies
.grow(proxy_id as usize + 1 - mask_len, false);
}
if !self.existing_proxies[proxy_id as usize] {
self.to_insert.push(proxy_id);
self.existing_proxies.set(proxy_id as usize, true);
if is_subproper_proxy {
self.subproper_proxy_count += 1;
}
false
} else {
// Here we need a second update if all proxies exit this region. In this case, we need
// to delete the final proxy, but the region may not have Aabbs overlapping it, so it
// wouldn't get an update otherwise.
self.update_count = 2;
true
}
}
pub fn update_after_subregion_removal(&mut self, proxies: &SAPProxies, layer_depth: i8) {
if self.needs_update_after_subregion_removal {
for axis in &mut self.axes {
let removed_count = axis
.delete_deleted_proxies_and_endpoints_after_subregion_removal(
proxies,
&mut self.existing_proxies,
layer_depth,
);
if removed_count > self.subproper_proxy_count {
log::debug!(
"Reached unexpected state: attempted to remove more sub-proper proxies than added (removing: {}, total: {}).",
removed_count,
self.subproper_proxy_count
);
self.subproper_proxy_count = 0;
} else {
self.subproper_proxy_count -= removed_count;
}
}
self.needs_update_after_subregion_removal = false;
}
}
#[profiling::function]
pub fn update(
&mut self,
proxies: &SAPProxies,
layer_depth: i8,
reporting: &mut HashMap<(u32, u32), bool>,
) {
if self.update_count > 0 {
// Update endpoints.
let mut total_deleted = 0;
let mut total_deleted_subproper = 0;
for dim in 0..DIM {
self.axes[dim].update_endpoints(dim, proxies, reporting);
let (num_deleted, num_deleted_subproper) = self.axes[dim]
.delete_out_of_bounds_proxies(proxies, &mut self.existing_proxies, layer_depth);
total_deleted += num_deleted;
total_deleted_subproper += num_deleted_subproper;
}
if total_deleted > 0 {
self.subproper_proxy_count -= total_deleted_subproper;
for dim in 0..DIM {
self.axes[dim].delete_out_of_bounds_endpoints(&self.existing_proxies);
}
}
self.update_count -= 1;
}
if !self.to_insert.is_empty() {
// Insert new proxies.
for dim in 1..DIM {
self.axes[dim].batch_insert(dim, &self.to_insert, proxies, None);
}
self.axes[0].batch_insert(0, &self.to_insert, proxies, Some(reporting));
self.to_insert.clear();
// In the rare event that all proxies leave this region in the next step, we need an
// update to remove them.
self.update_count = 1;
}
}
}

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

@@ -1,72 +0,0 @@
use crate::math::{Point, Real, Vector};
use parry::bounding_volume::Aabb;
#[cfg(feature = "f32")]
pub type RegionKey = i32;
#[cfg(feature = "f64")]
pub type RegionKey = i64;
pub(crate) const NUM_SENTINELS: usize = 1;
pub(crate) const NEXT_FREE_SENTINEL: u32 = u32::MAX;
pub(crate) const SENTINEL_VALUE: Real = Real::MAX;
pub(crate) const DELETED_AABB_VALUE: Real = SENTINEL_VALUE / 2.0;
pub(crate) const MAX_AABB_EXTENT: Real = SENTINEL_VALUE / 4.0;
pub(crate) const REGION_WIDTH_BASE: Real = 1.0;
pub(crate) const REGION_WIDTH_POWER_BASIS: Real = 5.0;
pub(crate) fn sort2(a: u32, b: u32) -> (u32, u32) {
assert_ne!(a, b);
if a < b {
(a, b)
} else {
(b, a)
}
}
pub(crate) fn clamp_point(point: Point<Real>) -> Point<Real> {
point.map(|e| na::clamp(e, -MAX_AABB_EXTENT, MAX_AABB_EXTENT))
}
pub(crate) fn point_key(point: Point<Real>, region_width: Real) -> Point<RegionKey> {
(point / region_width)
.coords
.map(|e| {
// If the region is outside this range, the region keys will overlap
assert!(e.floor() < RegionKey::MAX as Real);
assert!(e.floor() > RegionKey::MIN as Real);
e.floor() as RegionKey
})
.into()
}
pub(crate) fn region_aabb(index: Point<RegionKey>, region_width: Real) -> Aabb {
let mins = index.coords.map(|i| i as Real * region_width).into();
let maxs = mins + Vector::repeat(region_width);
Aabb::new(mins, maxs)
}
pub(crate) fn region_width(depth: i8) -> Real {
(REGION_WIDTH_BASE * REGION_WIDTH_POWER_BASIS.powi(depth as i32)).min(MAX_AABB_EXTENT)
}
/// Computes the depth of the layer the given [`Aabb`] should be part of.
///
/// 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 be too large, otherwise
/// we are loosing the benefits of Multi-SAP.
///
/// If the code below, we select a layer such that each region can
/// contain at least a chain of 10 contiguous objects with that [`Aabb`].
pub(crate) fn layer_containing_aabb(aabb: &Aabb) -> i8 {
// Max number of elements of this size we would like one region to be able to contain.
const NUM_ELEMENTS_PER_DIMENSION: Real = 10.0;
let width = 2.0 * aabb.half_extents().norm() * NUM_ELEMENTS_PER_DIMENSION;
(width / REGION_WIDTH_BASE)
.log(REGION_WIDTH_POWER_BASIS)
.round()
.max(i8::MIN as Real)
.min(i8::MAX as Real) as i8
}

0
src/geometry/broad_phase_multi_sap/broad_phase_pair_event.rs → src/geometry/broad_phase_pair_event.rs
View File

93
src/geometry/broad_phase_qbvh.rs
View File

@@ -1,93 +0,0 @@
use crate::dynamics::RigidBodySet;
use crate::geometry::{BroadPhase, BroadPhasePairEvent, ColliderHandle, ColliderPair, ColliderSet};
use parry::math::Real;
use parry::partitioning::Qbvh;
use parry::partitioning::QbvhUpdateWorkspace;
use parry::query::visitors::BoundingVolumeIntersectionsSimultaneousVisitor;
#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
#[derive(Clone)]
pub struct BroadPhaseQbvh {
qbvh: Qbvh<ColliderHandle>,
stack: Vec<(u32, u32)>,
#[cfg_attr(feature = "serde-serialize", serde(skip))]
workspace: QbvhUpdateWorkspace,
}
impl Default for BroadPhaseQbvh {
fn default() -> Self {
Self::new()
}
}
impl BroadPhaseQbvh {
pub fn new() -> Self {
Self {
qbvh: Qbvh::new(),
stack: vec![],
workspace: QbvhUpdateWorkspace::default(),
}
}
}
impl BroadPhase for BroadPhaseQbvh {
fn update(
&mut self,
_dt: Real,
prediction_distance: Real,
colliders: &mut ColliderSet,
_bodies: &RigidBodySet,
modified_colliders: &[ColliderHandle],
removed_colliders: &[ColliderHandle],
events: &mut Vec<BroadPhasePairEvent>,
) {
let margin = 0.01;
if modified_colliders.is_empty() {
return;
}
// Visitor to find collision pairs.
let mut visitor = BoundingVolumeIntersectionsSimultaneousVisitor::new(
|co1: &ColliderHandle, co2: &ColliderHandle| {
if *co1 != *co2 {
events.push(BroadPhasePairEvent::AddPair(ColliderPair::new(*co1, *co2)));
}
true
},
);
let full_rebuild = self.qbvh.raw_nodes().is_empty();
if full_rebuild {
self.qbvh.clear_and_rebuild(
colliders.iter().map(|(handle, collider)| {
(
handle,
collider.compute_collision_aabb(prediction_distance / 2.0),
)
}),
margin,
);
self.qbvh
.traverse_bvtt_with_stack(&self.qbvh, &mut visitor, &mut self.stack);
} else {
for modified in modified_colliders {
self.qbvh.pre_update_or_insert(*modified);
}
for removed in removed_colliders {
self.qbvh.remove(*removed);
}
let _ = self.qbvh.refit(margin, &mut self.workspace, |handle| {
colliders[*handle].compute_collision_aabb(prediction_distance / 2.0)
});
// self.qbvh
// .traverse_bvtt_with_stack(&self.qbvh, &mut visitor, &mut self.stack);
self.qbvh
.traverse_modified_bvtt_with_stack(&self.qbvh, &mut visitor, &mut self.stack);
self.qbvh.rebalance(margin, &mut self.workspace);
}
}
}

28
src/geometry/collider.rs
View File

@@ -2,11 +2,11 @@ use crate::dynamics::{CoefficientCombineRule, MassProperties, RigidBodyHandle};
#[cfg(feature = "dim3")]
use crate::geometry::HeightFieldFlags;
use crate::geometry::{
ActiveCollisionTypes, BroadPhaseProxyIndex, ColliderBroadPhaseData, ColliderChanges,
ColliderFlags, ColliderMassProps, ColliderMaterial, ColliderParent, ColliderPosition,
ColliderShape, ColliderType, InteractionGroups, MeshConverter, MeshConverterError, SharedShape,
ActiveCollisionTypes, ColliderChanges, ColliderFlags, ColliderMassProps, ColliderMaterial,
ColliderParent, ColliderPosition, ColliderShape, ColliderType, InteractionGroups,
MeshConverter, MeshConverterError, SharedShape,
};
use crate::math::{AngVector, Isometry, Point, Real, Rotation, Vector, DIM};
use crate::math::{AngVector, DIM, Isometry, Point, Real, Rotation, Vector};
use crate::parry::transformation::vhacd::VHACDParameters;
use crate::pipeline::{ActiveEvents, ActiveHooks};
use crate::prelude::ColliderEnabled;
@@ -29,7 +29,6 @@ pub struct Collider {
pub(crate) pos: ColliderPosition,
pub(crate) material: ColliderMaterial,
pub(crate) flags: ColliderFlags,
pub(crate) bf_data: ColliderBroadPhaseData,
contact_skin: Real,
contact_force_event_threshold: Real,
/// User-defined data associated to this collider.
@@ -38,7 +37,6 @@ pub struct Collider {
impl Collider {
pub(crate) fn reset_internal_references(&mut self) {
self.bf_data.proxy_index = crate::INVALID_U32;
self.changes = ColliderChanges::all();
}
@@ -54,21 +52,6 @@ impl Collider {
}
}
/// An internal index associated to this collider by the broad-phase algorithm.
pub fn internal_broad_phase_proxy_index(&self) -> BroadPhaseProxyIndex {
self.bf_data.proxy_index
}
/// Sets the internal index associated to this collider by the broad-phase algorithm.
///
/// This must **not** be called, unless you are implementing your own custom broad-phase
/// that require storing an index in the collider struct.
/// Modifying that index outside of a custom broad-phase code will most certainly break
/// the physics engine.
pub fn set_internal_broad_phase_proxy_index(&mut self, id: BroadPhaseProxyIndex) {
self.bf_data.proxy_index = id;
}
/// The rigid body this collider is attached to.
pub fn parent(&self) -> Option<RigidBodyHandle> {
self.parent.map(|parent| parent.handle)
@@ -107,7 +90,6 @@ impl Collider {
pos,
material,
flags,
bf_data: _bf_data, // Internal ids must not be overwritten.
contact_force_event_threshold,
user_data,
contact_skin,
@@ -1079,7 +1061,6 @@ impl ColliderBuilder {
};
let changes = ColliderChanges::all();
let pos = ColliderPosition(self.position);
let bf_data = ColliderBroadPhaseData::default();
let coll_type = if self.is_sensor {
ColliderType::Sensor
} else {
@@ -1093,7 +1074,6 @@ impl ColliderBuilder {
parent: None,
changes,
pos,
bf_data,
flags,
coll_type,
contact_force_event_threshold: self.contact_force_event_threshold,

28
src/geometry/collider_components.rs
View File

@@ -1,7 +1,6 @@
use crate::dynamics::{CoefficientCombineRule, MassProperties, RigidBodyHandle, RigidBodyType};
use crate::geometry::{BroadPhaseProxyIndex, InteractionGroups, Shape, SharedShape};
use crate::geometry::{InteractionGroups, Shape, SharedShape};
use crate::math::{Isometry, Real};
use crate::parry::partitioning::IndexedData;
use crate::pipeline::{ActiveEvents, ActiveHooks};
use std::ops::{Deref, DerefMut};
@@ -31,16 +30,6 @@ impl ColliderHandle {
}
}
impl IndexedData for ColliderHandle {
fn default() -> Self {
Self(IndexedData::default())
}
fn index(&self) -> usize {
self.0.index()
}
}
bitflags::bitflags! {
#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
#[derive(Copy, Clone, PartialEq, Eq, Debug)]
@@ -115,21 +104,6 @@ impl ColliderType {
}
}
#[derive(Copy, Clone, Debug, PartialEq, Eq, Hash)]
#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
/// Data associated to a collider that takes part to a broad-phase algorithm.
pub struct ColliderBroadPhaseData {
pub(crate) proxy_index: BroadPhaseProxyIndex,
}
impl Default for ColliderBroadPhaseData {
fn default() -> Self {
ColliderBroadPhaseData {
proxy_index: crate::INVALID_U32,
}
}
}
/// The shape of a collider.
pub type ColliderShape = SharedShape;

3
src/geometry/mesh_converter.rs
View File

@@ -75,7 +75,8 @@ impl MeshConverter {
SharedShape::new(cuboid)
}
MeshConverter::Aabb => {
let aabb = bounding_volume::details::local_point_cloud_aabb(&vertices);
let aabb =
bounding_volume::details::local_point_cloud_aabb(vertices.iter().copied());
let cuboid = Cuboid::new(aabb.half_extents());
transform = Isometry::from(aabb.center().coords);
SharedShape::new(cuboid)

21
src/geometry/mod.rs
View File

@@ -1,7 +1,8 @@
//! Structures related to geometry: colliders, shapes, etc.
pub use self::broad_phase::BroadPhase;
pub use self::broad_phase_multi_sap::{BroadPhaseMultiSap, BroadPhasePairEvent, ColliderPair};
pub use self::broad_phase_bvh::{BroadPhaseBvh, BvhOptimizationStrategy};
pub use self::broad_phase_pair_event::{BroadPhasePairEvent, ColliderPair};
pub use self::collider::{Collider, ColliderBuilder};
pub use self::collider_components::*;
pub use self::collider_set::ColliderSet;
@@ -16,6 +17,7 @@ pub use self::mesh_converter::{MeshConverter, MeshConverterError};
pub use self::narrow_phase::NarrowPhase;
pub use parry::bounding_volume::BoundingVolume;
pub use parry::partitioning::{Bvh, BvhBuildStrategy};
pub use parry::query::{PointQuery, PointQueryWithLocation, RayCast, TrackedContact};
pub use parry::shape::{SharedShape, VoxelState, VoxelType, Voxels};
@@ -54,7 +56,7 @@ pub type PointProjection = parry::query::PointProjection;
/// The result of a shape-cast between two shapes.
pub type ShapeCastHit = parry::query::ShapeCastHit;
/// The default broad-phase implementation recommended for general-purpose usage.
pub type DefaultBroadPhase = BroadPhaseMultiSap;
pub type DefaultBroadPhase = BroadPhaseBvh;
bitflags::bitflags! {
/// Flags providing more information regarding a collision event.
@@ -181,24 +183,16 @@ impl ContactForceEvent {
}
}
pub(crate) use self::broad_phase::BroadPhaseProxyIndex;
pub(crate) use self::collider_set::ModifiedColliders;
pub(crate) use self::narrow_phase::ContactManifoldIndex;
pub(crate) use parry::partitioning::Qbvh;
pub use parry::shape::*;
#[cfg(feature = "serde-serialize")]
pub(crate) fn default_persistent_query_dispatcher(
) -> std::sync::Arc<dyn parry::query::PersistentQueryDispatcher<ContactManifoldData, ContactData>> {
pub(crate) fn default_persistent_query_dispatcher()
-> std::sync::Arc<dyn parry::query::PersistentQueryDispatcher<ContactManifoldData, ContactData>> {
std::sync::Arc::new(parry::query::DefaultQueryDispatcher)
}
#[cfg(feature = "serde-serialize")]
pub(crate) fn default_query_dispatcher() -> std::sync::Arc<dyn parry::query::QueryDispatcher> {
std::sync::Arc::new(parry::query::DefaultQueryDispatcher)
}
mod broad_phase_multi_sap;
mod collider_components;
mod contact_pair;
mod interaction_graph;
@@ -206,7 +200,8 @@ mod interaction_groups;
mod narrow_phase;
mod broad_phase;
mod broad_phase_qbvh;
mod broad_phase_bvh;
mod broad_phase_pair_event;
mod collider;
mod collider_set;
mod mesh_converter;

14
src/geometry/narrow_phase.rs
View File

@@ -1,8 +1,8 @@
#[cfg(feature = "parallel")]
use rayon::prelude::*;
use crate::data::graph::EdgeIndex;
use crate::data::Coarena;
use crate::data::graph::EdgeIndex;
use crate::dynamics::{
CoefficientCombineRule, ImpulseJointSet, IslandManager, RigidBodyDominance, RigidBodySet,
RigidBodyType,
@@ -89,7 +89,7 @@ impl NarrowPhase {
}
/// The query dispatcher used by this narrow-phase to select the right collision-detection
/// algorithms depending of the shape types.
/// algorithms depending on the shape types.
pub fn query_dispatcher(
&self,
) -> &dyn PersistentQueryDispatcher<ContactManifoldData, ContactData> {
@@ -1189,7 +1189,7 @@ mod test {
use crate::prelude::{
CCDSolver, ColliderBuilder, DefaultBroadPhase, IntegrationParameters, PhysicsPipeline,
QueryPipeline, RigidBodyBuilder,
RigidBodyBuilder,
};
use super::*;
@@ -1237,7 +1237,6 @@ mod test {
let mut impulse_joint_set = ImpulseJointSet::new();
let mut multibody_joint_set = MultibodyJointSet::new();
let mut ccd_solver = CCDSolver::new();
let mut query_pipeline = QueryPipeline::new();
let physics_hooks = ();
let event_handler = ();
@@ -1252,7 +1251,6 @@ mod test {
&mut impulse_joint_set,
&mut multibody_joint_set,
&mut ccd_solver,
Some(&mut query_pipeline),
&physics_hooks,
&event_handler,
);
@@ -1288,7 +1286,6 @@ mod test {
&mut impulse_joint_set,
&mut multibody_joint_set,
&mut ccd_solver,
Some(&mut query_pipeline),
&physics_hooks,
&event_handler,
);
@@ -1317,7 +1314,6 @@ mod test {
&mut impulse_joint_set,
&mut multibody_joint_set,
&mut ccd_solver,
Some(&mut query_pipeline),
&physics_hooks,
&event_handler,
);
@@ -1385,7 +1381,6 @@ mod test {
let mut impulse_joint_set = ImpulseJointSet::new();
let mut multibody_joint_set = MultibodyJointSet::new();
let mut ccd_solver = CCDSolver::new();
let mut query_pipeline = QueryPipeline::new();
let physics_hooks = ();
let event_handler = ();
@@ -1400,7 +1395,6 @@ mod test {
&mut impulse_joint_set,
&mut multibody_joint_set,
&mut ccd_solver,
Some(&mut query_pipeline),
&physics_hooks,
&event_handler,
);
@@ -1435,7 +1429,6 @@ mod test {
&mut impulse_joint_set,
&mut multibody_joint_set,
&mut ccd_solver,
Some(&mut query_pipeline),
&physics_hooks,
&event_handler,
);
@@ -1462,7 +1455,6 @@ mod test {
&mut impulse_joint_set,
&mut multibody_joint_set,
&mut ccd_solver,
Some(&mut query_pipeline),
&physics_hooks,
&event_handler,
);