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

Start implementing SAPLayer creation and insertion.

Browse Source
This commit is contained in:
Crozet Sébastien
2021-03-08 18:27:06 +01:00
parent 7983c25606
commit a967ace7d4
7 changed files with 235 additions and 57 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -1,6 +1,6 @@
use super::{
BroadPhasePairEvent, BroadPhaseProxies, BroadPhaseProxy, ColliderPair, SAPLayer, SAPRegion,
NEXT_FREE_SENTINEL, SENTINEL_VALUE,
BroadPhasePairEvent, BroadPhaseProxies, BroadPhaseProxy, BroadPhaseProxyData, ColliderPair,
SAPLayer, SAPRegion, NEXT_FREE_SENTINEL, SENTINEL_VALUE,
};
use crate::data::pubsub::Subscription;
use crate::dynamics::RigidBodySet;
@@ -15,6 +15,8 @@ use parry::utils::hashmap::HashMap;
pub struct BroadPhase {
proxies: BroadPhaseProxies,
layers: Vec<SAPLayer>,
smallest_layer: u8,
largest_layer: u8,
removed_colliders: Option<Subscription<RemovedCollider>>,
deleted_any: bool,
#[cfg_attr(feature = "serde-serialize", serde(skip))]
@@ -46,7 +48,9 @@ impl BroadPhase {
BroadPhase {
removed_colliders: None,
proxies: BroadPhaseProxies::new(),
layers: vec![SAPLayer::new(0)],
layers: Vec::new(),
smallest_layer: 0,
largest_layer: 0,
region_pool: Vec::new(),
reporting: HashMap::default(),
deleted_any: false,
@@ -62,23 +66,22 @@ impl BroadPhase {
let cursor = self.removed_colliders.take().unwrap();
for collider in colliders.removed_colliders.read(&cursor) {
self.remove_collider(collider.proxy_index);
self.remove_proxy(collider.proxy_index);
}
colliders.removed_colliders.ack(&cursor);
self.removed_colliders = Some(cursor);
}
fn remove_collider(&mut self, proxy_index: usize) {
fn remove_proxy(&mut self, proxy_index: usize) {
if proxy_index == crate::INVALID_USIZE {
// This collider has not been added to the broad-phase yet.
return;
}
let proxy = &mut self.proxies[proxy_index];
let layer = &mut self.layers[proxy.layer as usize];
layer.remove_collider(proxy, proxy_index);
let layer = &mut self.layers[proxy.layer_id as usize];
layer.remove_proxy(proxy, proxy_index);
// Push the proxy to infinity, but not beyond the sentinels.
proxy.aabb.mins.coords.fill(SENTINEL_VALUE / 2.0);
@@ -86,6 +89,82 @@ impl BroadPhase {
self.proxies.remove(proxy_index);
}
fn finalize_layer_insertion(&mut self, layer_id: u8) {
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 region.
self.layers[larger_layer as usize].delete_all_region_endpoints();
}
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.
for smaller_region in smaller_layer.regions.iter() {
let smaller_region_key = ???;
new_layer.preupdate_proxy_in_region(smaller_region.proxy_id, smaller_region_key);
}
}
}
fn ensure_layer_exists(&mut self, new_depth: i8) -> u8 {
// Special case: we don't have any layers yet.
if self.layers.is_empty() {
self.layers.push(SAPLayer::new(new_depth));
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[layer as usize].larger_layer;
}
match larger_layer_id {
None => {
let new_layer_id = self.layers.len() as u8;
self.layers[self.largest_layer as usize].larger_layer = Some(new_layer_id);
self.largest_layer = new_layer_id;
self.layers
.push(SAPLayer::new(new_depth, Some(self.largest_layer), None));
self.finalize_layer_insertion(new_layer_id);
new_layer_id
}
Some(larger_layer_id) => {
if self.layers[larger_layer_id].depth == new_depth {
// Found it! The layer already exists.
larger_layer_id
} else {
// The layer does not exist yet. Create it.
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);
}
self.layers.push(SAPLayer::new(
new_depth,
smaller_layer_id,
Some(larger_layer_id),
));
self.finalize_layer_insertion(new_layer_id);
new_layer_id
}
}
}
}
pub(crate) fn update_aabbs(
&mut self,
prediction_distance: Real,
@@ -108,22 +187,27 @@ impl BroadPhase {
let collider = &mut colliders[*handle];
let aabb = collider.compute_aabb().loosened(prediction_distance / 2.0);
let layer = if let Some(proxy) = self.proxies.get_mut(collider.proxy_index) {
let layer_id = if let Some(proxy) = self.proxies.get_mut(collider.proxy_index) {
proxy.aabb = aabb;
proxy.layer
proxy.layer_id
} else {
let layer = 0; // FIXME: compute the actual layer.
let layer_depth = super::layer_containing_aabb(&aabb);
let layer_id = self.ensure_layer_exists(layer_depth);
// Create the proxy.
let proxy = BroadPhaseProxy {
handle: *handle,
data: BroadPhaseProxyData::Collider(*handle),
aabb,
next_free: NEXT_FREE_SENTINEL,
layer,
layer_id,
};
collider.proxy_index = self.proxies.insert(proxy);
layer
layer_id
};
let layer = &mut self.layers[layer as usize];
let layer = &mut self.layers[layer_id as usize];
// Preupdate the collider in the layer.
layer.preupdate_collider(collider, &aabb, &mut self.region_pool);
}
}
@@ -138,28 +222,62 @@ impl BroadPhase {
}
pub(crate) fn find_pairs(&mut self, out_events: &mut Vec<BroadPhasePairEvent>) {
self.reporting.clear();
let mut layer_id = 0;
for layer in &mut self.layers {
while let Some(layer) = self.layers.get_mut(layer_id as usize) {
layer.update_regions(&self.proxies, &mut self.reporting, &mut self.region_pool);
}
layer_id = layer.prev_layer; // this MUST be done before the `for` loop because we use the prev_layer index there.
for ((proxy1, proxy2), colliding) in &self.reporting {
let proxy1 = &self.proxies[*proxy1 as usize];
let proxy2 = &self.proxies[*proxy2 as usize];
for ((proxy_id1, proxy_id2), colliding) in &self.reporting {
let proxy1 = &self.proxies[*proxy_id1 as usize];
let proxy2 = &self.proxies[*proxy_id2 as usize];
let handle1 = proxy1.handle;
let handle2 = proxy2.handle;
let handle1 = proxy1.handle;
let handle2 = proxy2.handle;
if *colliding {
out_events.push(BroadPhasePairEvent::AddPair(ColliderPair::new(
handle1, handle2,
)));
} else {
out_events.push(BroadPhasePairEvent::DeletePair(ColliderPair::new(
handle1, handle2,
)));
match (&handle1, &handle2) {
(
BroadPhaseProxyData::Collider(handle1),
BroadPhaseProxyData::Collider(handle2),
) => {
if *colliding {
out_events.push(BroadPhasePairEvent::AddPair(ColliderPair::new(
*handle1, *handle2,
)));
} else {
out_events.push(BroadPhasePairEvent::DeletePair(ColliderPair::new(
*handle1, *handle2,
)));
}
}
(
BroadPhaseProxyData::Collider(_),
BroadPhaseProxyData::Subregion(region_key),
) => {
if *colliding {
// Add the collider to the subregion.
let sublayer = &mut self.layers[layer_id as usize];
sublayer.preupdate_proxy_in_region(*proxy_id1, region_key);
}
}
(
BroadPhaseProxyData::Subregion(region_key),
BroadPhaseProxyData::Collider(_),
) => {
if *colliding {
// Add the collider to the subregion.
let sublayer = &mut self.layers[layer_id as usize];
sublayer.preupdate_proxy_in_region(*proxy_id2, region_key);
}
}
(BroadPhaseProxyData::Subregion(_), BroadPhaseProxyData::Subregion(_)) => {
// This will only happen between two adjacent subregions because
// they share some of the same bounds. So this case does not matter.
}
}
}
self.reporting.clear();
}
}
}