diff options
Diffstat (limited to 'src')
| -rw-r--r-- | src/geometry/broad_phase_multi_sap/broad_phase.rs | 184 | ||||
| -rw-r--r-- | src/geometry/broad_phase_multi_sap/broad_phase_proxy.rs | 26 | ||||
| -rw-r--r-- | src/geometry/broad_phase_multi_sap/sap_layer.rs | 39 | ||||
| -rw-r--r-- | src/geometry/broad_phase_multi_sap/sap_region.rs | 13 | ||||
| -rw-r--r-- | src/geometry/broad_phase_multi_sap/sap_utils.rs | 26 |
5 files changed, 231 insertions, 57 deletions
diff --git a/src/geometry/broad_phase_multi_sap/broad_phase.rs b/src/geometry/broad_phase_multi_sap/broad_phase.rs index fc6d8f6..f0ae554 100644 --- a/src/geometry/broad_phase_multi_sap/broad_phase.rs +++ b/src/geometry/broad_phase_multi_sap/broad_phase.rs @@ -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]; - - 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, - ))); + 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; + + 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(); } } } diff --git a/src/geometry/broad_phase_multi_sap/broad_phase_proxy.rs b/src/geometry/broad_phase_multi_sap/broad_phase_proxy.rs index ccc0f9c..33da5ef 100644 --- a/src/geometry/broad_phase_multi_sap/broad_phase_proxy.rs +++ b/src/geometry/broad_phase_multi_sap/broad_phase_proxy.rs @@ -1,15 +1,32 @@ use super::NEXT_FREE_SENTINEL; use crate::geometry::ColliderHandle; +use crate::math::Point; use parry::bounding_volume::AABB; use std::ops::{Index, IndexMut}; #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] +#[derive(Copy, Clone, Debug)] +pub(crate) enum BroadPhaseProxyData { + Collider(ColliderHandle), + Subregion(Point<i32>), +} + +impl BroadPhaseProxyData { + pub fn is_subregion(&self) -> bool { + match self { + BroadPhaseProxyData::Subregion(_) => true, + _ => false, + } + } +} + +#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] #[derive(Clone)] pub(crate) struct BroadPhaseProxy { - pub handle: ColliderHandle, + pub data: BroadPhaseProxyData, pub aabb: AABB, pub next_free: u32, - pub layer: u8, + pub layer_id: u8, } #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] @@ -44,11 +61,6 @@ impl BroadPhaseProxies { self.first_free = proxy_id as u32; } - // // FIXME: take holes into account? - // pub fn get(&self, i: usize) -> Option<&BroadPhaseProxy> { - // self.elements.get(i) - // } - // FIXME: take holes into account? pub fn get_mut(&mut self, i: usize) -> Option<&mut BroadPhaseProxy> { self.elements.get_mut(i) diff --git a/src/geometry/broad_phase_multi_sap/sap_layer.rs b/src/geometry/broad_phase_multi_sap/sap_layer.rs index 71a97e2..31b8297 100644 --- a/src/geometry/broad_phase_multi_sap/sap_layer.rs +++ b/src/geometry/broad_phase_multi_sap/sap_layer.rs @@ -7,21 +7,25 @@ use parry::utils::hashmap::{Entry, HashMap}; #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] #[derive(Clone)] pub(crate) struct SAPLayer { - depth: i8, + pub depth: i8, + pub smaller_layer: Option<u8>, + pub larger_layer: Option<u8>, region_width: Real, regions: HashMap<Point<i32>, SAPRegion>, deleted_any: bool, #[cfg_attr(feature = "serde-serialize", serde(skip))] regions_to_remove: Vec<Point<i32>>, // Workspace #[cfg_attr(feature = "serde-serialize", serde(skip))] - created_regions: Vec<Point<i32>>, + pub created_regions: Vec<Point<i32>>, } impl SAPLayer { - pub fn new(depth: i8) -> Self { + pub fn new(depth: i8, smaller_layer: Option<u8>, bigger_layer: Option<u8>) -> Self { Self { depth, - region_width: super::CELL_WIDTH, // FIXME + smaller_layer, + larger_layer, + region_width: super::region_width(depth), regions: HashMap::default(), deleted_any: false, regions_to_remove: vec![], @@ -29,7 +33,18 @@ impl SAPLayer { } } - pub fn insert_subregion(&mut self, sub_key: &Point<i32>) {} + pub fn delete_all_region_endpoints(&mut self) { + for region in &mut self.regions { + region.0.delete_all_region_endpoints(); + } + } + + pub fn preupdate_proxy_in_region(&mut self, proxy: u32, region: &Point<i32>) { + self.regions + .get_mut(®ion) + .unwrap() + .preupdate_proxy(proxy as usize); + } pub fn preupdate_collider( &mut self, @@ -38,8 +53,8 @@ impl SAPLayer { pool: &mut Vec<SAPRegion>, ) { let proxy_id = collider.proxy_index; - let start = super::point_key(aabb.mins); - let end = super::point_key(aabb.maxs); + let start = super::point_key(aabb.mins, self.region_width); + let end = super::point_key(aabb.maxs, self.region_width); // Discretize the aabb. #[cfg(feature = "dim2")] @@ -54,25 +69,26 @@ impl SAPLayer { let region_key = Point::new(i, j); #[cfg(feature = "dim3")] let region_key = Point::new(i, j, _k); - let region_bounds = super::region_aabb(region_key); let region = match self.regions.entry(region_key) { Entry::Occupied(occupied) => occupied.into_mut(), Entry::Vacant(vacant) => { self.created_regions.push(region_key); + let region_bounds = super::region_aabb(region_key, self.region_width); vacant.insert(SAPRegion::recycle_or_new(region_bounds, pool)) } }; + let _ = region.preupdate_proxy(proxy_id); } } } } - pub fn remove_collider(&mut self, proxy: &BroadPhaseProxy, proxy_index: usize) { + pub fn remove_proxy(&mut self, proxy: &BroadPhaseProxy, proxy_index: usize) { // Discretize the AABB to find the regions that need to be invalidated. - let start = super::point_key(proxy.aabb.mins); - let end = super::point_key(proxy.aabb.maxs); + let start = super::point_key(proxy.aabb.mins, self.region_width); + let end = super::point_key(proxy.aabb.maxs, self.region_width); #[cfg(feature = "dim2")] let k_range = 0..1; @@ -103,6 +119,7 @@ impl SAPLayer { ) { for (point, region) in &mut self.regions { region.update(proxies, reporting); + if region.proxy_count == 0 { self.regions_to_remove.push(*point); } diff --git a/src/geometry/broad_phase_multi_sap/sap_region.rs b/src/geometry/broad_phase_multi_sap/sap_region.rs index ba251c6..6cd2ce0 100644 --- a/src/geometry/broad_phase_multi_sap/sap_region.rs +++ b/src/geometry/broad_phase_multi_sap/sap_region.rs @@ -60,6 +60,19 @@ impl SAPRegion { } } + pub fn delete_all_region_endpoints(&mut self, proxies: &BroadPhaseProxies) { + for axis in &mut self.axes { + axis.endpoints.retain(|e| { + if let Some(proxy) = proxies.get(e.proxy() as usize) { + self.existing_proxies.set(e.proxy() as usize, false); + !proxy.data.is_subregion() + } else { + true + } + }); + } + } + pub fn predelete_proxy(&mut self, _proxy_id: usize) { // 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 diff --git a/src/geometry/broad_phase_multi_sap/sap_utils.rs b/src/geometry/broad_phase_multi_sap/sap_utils.rs index a32d974..76e6a77 100644 --- a/src/geometry/broad_phase_multi_sap/sap_utils.rs +++ b/src/geometry/broad_phase_multi_sap/sap_utils.rs @@ -4,7 +4,8 @@ use parry::bounding_volume::AABB; 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 CELL_WIDTH: Real = 20.0; +pub(crate) const REGION_WIDTH_BASE: Real = 20.0; +pub(crate) const REGION_WIDTH_POWER_BASIS: Real = 8.0; pub(crate) fn sort2(a: u32, b: u32) -> (u32, u32) { assert_ne!(a, b); @@ -16,12 +17,25 @@ pub(crate) fn sort2(a: u32, b: u32) -> (u32, u32) { } } -pub(crate) fn point_key(point: Point<Real>) -> Point<i32> { - (point / CELL_WIDTH).coords.map(|e| e.floor() as i32).into() +pub(crate) fn point_key(point: Point<Real>, region_width: Real) -> Point<i32> { + (point / region_width) + .coords + .map(|e| e.floor() as i32) + .into() } -pub(crate) fn region_aabb(index: Point<i32>) -> AABB { - let mins = index.coords.map(|i| i as Real * CELL_WIDTH).into(); - let maxs = mins + Vector::repeat(CELL_WIDTH); +pub(crate) fn region_aabb(index: Point<i32>, 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) +} + +pub(crate) fn layer_containing_aabb(aabb: &AABB) -> i8 { + let width = 2.0 * aabb.half_extents().norm(); + // TODO: handle overflows in the f32 -> i8 conversion. + (width / REGION_WIDTH_BASE).log(REGION_WIDTH_POWER_BASIS) as i8 +} |