From 0b80bc827ce53b6e207f0de79f226245c1a9b735 Mon Sep 17 00:00:00 2001 From: Crozet Sébastien Date: Mon, 8 Mar 2021 11:53:21 +0100 Subject: Split the broad-phase code into multiple files. --- src/geometry/broad_phase_multi_sap/broad_phase.rs | 288 +++++++++++++++++++++ .../broad_phase_pair_event.rs | 41 +++ .../broad_phase_multi_sap/broad_phase_proxy.rs | 68 +++++ src/geometry/broad_phase_multi_sap/mod.rs | 16 ++ src/geometry/broad_phase_multi_sap/sap_axis.rs | 209 +++++++++++++++ src/geometry/broad_phase_multi_sap/sap_endpoint.rs | 60 +++++ src/geometry/broad_phase_multi_sap/sap_region.rs | 127 +++++++++ src/geometry/broad_phase_multi_sap/sap_utils.rs | 27 ++ 8 files changed, 836 insertions(+) create mode 100644 src/geometry/broad_phase_multi_sap/broad_phase.rs create mode 100644 src/geometry/broad_phase_multi_sap/broad_phase_pair_event.rs create mode 100644 src/geometry/broad_phase_multi_sap/broad_phase_proxy.rs create mode 100644 src/geometry/broad_phase_multi_sap/mod.rs create mode 100644 src/geometry/broad_phase_multi_sap/sap_axis.rs create mode 100644 src/geometry/broad_phase_multi_sap/sap_endpoint.rs create mode 100644 src/geometry/broad_phase_multi_sap/sap_region.rs create mode 100644 src/geometry/broad_phase_multi_sap/sap_utils.rs (limited to 'src/geometry/broad_phase_multi_sap') diff --git a/src/geometry/broad_phase_multi_sap/broad_phase.rs b/src/geometry/broad_phase_multi_sap/broad_phase.rs new file mode 100644 index 0000000..f7006d3 --- /dev/null +++ b/src/geometry/broad_phase_multi_sap/broad_phase.rs @@ -0,0 +1,288 @@ +use super::{ + BroadPhasePairEvent, BroadPhaseProxies, BroadPhaseProxy, ColliderPair, SAPRegion, + NEXT_FREE_SENTINEL, SENTINEL_VALUE, +}; +use crate::data::pubsub::Subscription; +use crate::dynamics::RigidBodySet; +use crate::geometry::{ColliderSet, RemovedCollider}; +use crate::math::{Point, Real}; +use parry::bounding_volume::BoundingVolume; +use parry::utils::hashmap::HashMap; + +/// A broad-phase based on multiple Sweep-and-Prune instances running of disjoint region of the 3D world. +#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] +#[derive(Clone)] +pub struct BroadPhase { + proxies: BroadPhaseProxies, + regions: HashMap, SAPRegion>, + removed_colliders: Option>, + deleted_any: bool, + #[cfg_attr(feature = "serde-serialize", serde(skip))] + region_pool: Vec, // To avoid repeated allocations. + #[cfg_attr(feature = "serde-serialize", serde(skip))] + regions_to_remove: Vec>, // Workspace + // 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 BroadPhase { + /// Create a new empty broad-phase. + pub fn new() -> Self { + BroadPhase { + removed_colliders: None, + proxies: BroadPhaseProxies::new(), + regions: HashMap::default(), + region_pool: Vec::new(), + regions_to_remove: Vec::new(), + reporting: HashMap::default(), + deleted_any: false, + } + } + + /// Maintain the broad-phase internal state by taking collider removal into account. + pub fn maintain(&mut self, colliders: &mut ColliderSet) { + // Ensure we already subscribed. + if self.removed_colliders.is_none() { + self.removed_colliders = Some(colliders.removed_colliders.subscribe()); + } + + let cursor = self.removed_colliders.take().unwrap(); + for collider in colliders.removed_colliders.read(&cursor) { + self.remove_collider(collider.proxy_index); + } + + colliders.removed_colliders.ack(&cursor); + self.removed_colliders = Some(cursor); + } + + fn remove_collider(&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]; + + // 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); + + #[cfg(feature = "dim2")] + for i in start.x..=end.x { + for j in start.y..=end.y { + if let Some(region) = self.regions.get_mut(&Point::new(i, j)) { + region.predelete_proxy(proxy_index); + self.deleted_any = true; + } + } + } + + #[cfg(feature = "dim3")] + for i in start.x..=end.x { + for j in start.y..=end.y { + for k in start.z..=end.z { + if let Some(region) = self.regions.get_mut(&Point::new(i, j, k)) { + region.predelete_proxy(proxy_index); + self.deleted_any = true; + } + } + } + } + + // Push the proxy to infinity, but not beyond the sentinels. + proxy.aabb.mins.coords.fill(SENTINEL_VALUE / 2.0); + proxy.aabb.maxs.coords.fill(SENTINEL_VALUE / 2.0); + self.proxies.remove(proxy_index); + } + + pub(crate) fn update_aabbs( + &mut self, + prediction_distance: Real, + bodies: &RigidBodySet, + colliders: &mut ColliderSet, + ) { + // First, if we have any pending removals we have + // to deal with them now because otherwise we will + // end up with an ABA problems when reusing proxy + // ids. + self.complete_removals(); + + for body_handle in bodies + .modified_inactive_set + .iter() + .chain(bodies.active_dynamic_set.iter()) + .chain(bodies.active_kinematic_set.iter()) + { + for handle in &bodies[*body_handle].colliders { + let collider = &mut colliders[*handle]; + let aabb = collider.compute_aabb().loosened(prediction_distance / 2.0); + + if let Some(proxy) = self.proxies.get_mut(collider.proxy_index) { + proxy.aabb = aabb; + } else { + let proxy = BroadPhaseProxy { + handle: *handle, + aabb, + next_free: NEXT_FREE_SENTINEL, + }; + collider.proxy_index = self.proxies.insert(proxy); + } + + // Discretize the aabb. + let proxy_id = collider.proxy_index; + // let start = Point::origin(); + // let end = Point::origin(); + let start = super::point_key(aabb.mins); + let end = super::point_key(aabb.maxs); + + let regions = &mut self.regions; + let pool = &mut self.region_pool; + + #[cfg(feature = "dim2")] + for i in start.x..=end.x { + for j in start.y..=end.y { + let region_key = Point::new(i, j); + let region_bounds = region_aabb(region_key); + let region = regions + .entry(region_key) + .or_insert_with(|| SAPRegion::recycle_or_new(region_bounds, pool)); + let _ = region.preupdate_proxy(proxy_id); + } + } + + #[cfg(feature = "dim3")] + for i in start.x..=end.x { + for j in start.y..=end.y { + for k in start.z..=end.z { + let region_key = Point::new(i, j, k); + let region_bounds = super::region_aabb(region_key); + let region = regions + .entry(region_key) + .or_insert_with(|| SAPRegion::recycle_or_new(region_bounds, pool)); + let _ = region.preupdate_proxy(proxy_id); + } + } + } + } + } + } + + fn update_regions(&mut self) { + for (point, region) in &mut self.regions { + region.update(&self.proxies, &mut self.reporting); + if region.proxy_count == 0 { + self.regions_to_remove.push(*point); + } + } + + // Remove all the empty regions and store them in the region pool + let regions = &mut self.regions; + self.region_pool.extend( + self.regions_to_remove + .drain(..) + .map(|p| regions.remove(&p).unwrap()), + ); + } + + pub(crate) fn complete_removals(&mut self) { + if self.deleted_any { + self.update_regions(); + + // NOTE: we don't care about reporting pairs. + self.reporting.clear(); + self.deleted_any = false; + } + } + + pub(crate) fn find_pairs(&mut self, out_events: &mut Vec) { + // println!("num regions: {}", self.regions.len()); + + self.reporting.clear(); + self.update_regions(); + + // Convert reports to broad phase events. + // let t = instant::now(); + // let mut num_add_events = 0; + // let mut num_delete_events = 0; + + 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, + ))); + // num_add_events += 1; + } else { + out_events.push(BroadPhasePairEvent::DeletePair(ColliderPair::new( + handle1, handle2, + ))); + // num_delete_events += 1; + } + } + + // println!( + // "Event conversion time: {}, add: {}/{}, delete: {}/{}", + // instant::now() - t, + // num_add_events, + // out_events.len(), + // num_delete_events, + // out_events.len() + // ); + } +} + +#[cfg(test)] +mod test { + use crate::dynamics::{JointSet, RigidBodyBuilder, RigidBodySet}; + use crate::geometry::{BroadPhase, ColliderBuilder, ColliderSet}; + + #[test] + fn test_add_update_remove() { + let mut broad_phase = BroadPhase::new(); + let mut bodies = RigidBodySet::new(); + let mut colliders = ColliderSet::new(); + let mut joints = JointSet::new(); + + let rb = RigidBodyBuilder::new_dynamic().build(); + let co = ColliderBuilder::ball(0.5).build(); + let hrb = bodies.insert(rb); + colliders.insert(co, hrb, &mut bodies); + + broad_phase.update_aabbs(0.0, &bodies, &mut colliders); + + bodies.remove(hrb, &mut colliders, &mut joints); + broad_phase.maintain(&mut colliders); + broad_phase.update_aabbs(0.0, &bodies, &mut colliders); + + // Create another body. + let rb = RigidBodyBuilder::new_dynamic().build(); + let co = ColliderBuilder::ball(0.5).build(); + let hrb = bodies.insert(rb); + colliders.insert(co, hrb, &mut bodies); + + // Make sure the proxy handles is recycled properly. + broad_phase.update_aabbs(0.0, &bodies, &mut colliders); + } +} diff --git a/src/geometry/broad_phase_multi_sap/broad_phase_pair_event.rs b/src/geometry/broad_phase_multi_sap/broad_phase_pair_event.rs new file mode 100644 index 0000000..c27917b --- /dev/null +++ b/src/geometry/broad_phase_multi_sap/broad_phase_pair_event.rs @@ -0,0 +1,41 @@ +use crate::geometry::ColliderHandle; + +#[derive(Copy, Clone, Debug, PartialEq, Eq, Hash)] +#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] +pub struct ColliderPair { + pub collider1: ColliderHandle, + pub collider2: ColliderHandle, +} + +impl ColliderPair { + pub fn new(collider1: ColliderHandle, collider2: ColliderHandle) -> Self { + ColliderPair { + collider1, + collider2, + } + } + + pub fn new_sorted(collider1: ColliderHandle, collider2: ColliderHandle) -> Self { + if collider1.into_raw_parts().0 <= collider2.into_raw_parts().0 { + Self::new(collider1, collider2) + } else { + Self::new(collider2, collider1) + } + } + + pub fn swap(self) -> Self { + Self::new(self.collider2, self.collider1) + } + + pub fn zero() -> Self { + Self { + collider1: ColliderHandle::from_raw_parts(0, 0), + collider2: ColliderHandle::from_raw_parts(0, 0), + } + } +} + +pub enum BroadPhasePairEvent { + AddPair(ColliderPair), + DeletePair(ColliderPair), +} diff --git a/src/geometry/broad_phase_multi_sap/broad_phase_proxy.rs b/src/geometry/broad_phase_multi_sap/broad_phase_proxy.rs new file mode 100644 index 0000000..9290ce8 --- /dev/null +++ b/src/geometry/broad_phase_multi_sap/broad_phase_proxy.rs @@ -0,0 +1,68 @@ +use super::NEXT_FREE_SENTINEL; +use crate::geometry::ColliderHandle; +use parry::bounding_volume::AABB; +use std::ops::{Index, IndexMut}; + +#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] +#[derive(Clone)] +pub(crate) struct BroadPhaseProxy { + pub handle: ColliderHandle, + pub aabb: AABB, + pub next_free: u32, +} + +#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] +#[derive(Clone)] +pub(crate) struct BroadPhaseProxies { + pub elements: Vec, + pub first_free: u32, +} + +impl BroadPhaseProxies { + pub fn new() -> Self { + Self { + elements: Vec::new(), + first_free: NEXT_FREE_SENTINEL, + } + } + + pub fn insert(&mut self, proxy: BroadPhaseProxy) -> usize { + if self.first_free != NEXT_FREE_SENTINEL { + let proxy_id = self.first_free as usize; + self.first_free = self.elements[proxy_id].next_free; + self.elements[proxy_id] = proxy; + proxy_id + } else { + self.elements.push(proxy); + self.elements.len() - 1 + } + } + + pub fn remove(&mut self, proxy_id: usize) { + self.elements[proxy_id].next_free = self.first_free; + 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) + } +} + +impl Index for BroadPhaseProxies { + type Output = BroadPhaseProxy; + fn index(&self, i: usize) -> &BroadPhaseProxy { + self.elements.index(i) + } +} + +impl IndexMut for BroadPhaseProxies { + fn index_mut(&mut self, i: usize) -> &mut BroadPhaseProxy { + self.elements.index_mut(i) + } +} diff --git a/src/geometry/broad_phase_multi_sap/mod.rs b/src/geometry/broad_phase_multi_sap/mod.rs new file mode 100644 index 0000000..849a325 --- /dev/null +++ b/src/geometry/broad_phase_multi_sap/mod.rs @@ -0,0 +1,16 @@ +pub use self::broad_phase::BroadPhase; +pub use self::broad_phase_pair_event::{BroadPhasePairEvent, ColliderPair}; + +pub(self) use self::broad_phase_proxy::*; +pub(self) use self::sap_axis::*; +pub(self) use self::sap_endpoint::*; +pub(self) use self::sap_region::*; +pub(self) use self::sap_utils::*; + +mod broad_phase; +mod broad_phase_pair_event; +mod broad_phase_proxy; +mod sap_axis; +mod sap_endpoint; +mod sap_region; +mod sap_utils; diff --git a/src/geometry/broad_phase_multi_sap/sap_axis.rs b/src/geometry/broad_phase_multi_sap/sap_axis.rs new file mode 100644 index 0000000..3d59c7e --- /dev/null +++ b/src/geometry/broad_phase_multi_sap/sap_axis.rs @@ -0,0 +1,209 @@ +use super::{BroadPhaseProxies, SAPEndpoint, NUM_SENTINELS}; +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)] +pub(crate) struct SAPAxis { + pub min_bound: Real, + pub max_bound: Real, + pub endpoints: Vec, + #[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 batch_insert( + &mut self, + dim: usize, + new_proxies: &[usize], + proxies: &BroadPhaseProxies, + 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); + assert!(proxy.aabb.maxs[dim] >= self.min_bound); + let start_endpoint = + SAPEndpoint::start_endpoint(proxy.aabb.mins[dim], *proxy_id as u32); + let end_endpoint = SAPEndpoint::end_endpoint(proxy.aabb.maxs[dim], *proxy_id as u32); + + 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() as usize]; + 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() as usize]; + + // 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); + } + } + } + } + } + } + + pub fn delete_out_of_bounds_proxies(&self, existing_proxies: &mut BitVec) -> usize { + let mut deleted = 0; + for endpoint in &self.endpoints { + if endpoint.value < self.min_bound { + let proxy_idx = endpoint.proxy() as usize; + if endpoint.is_end() && existing_proxies[proxy_idx] { + existing_proxies.set(proxy_idx, false); + deleted += 1; + } + } else { + break; + } + } + + for endpoint in self.endpoints.iter().rev() { + if endpoint.value > self.max_bound { + let proxy_idx = endpoint.proxy() as usize; + if endpoint.is_start() && existing_proxies[proxy_idx] { + existing_proxies.set(endpoint.proxy() as usize, false); + deleted += 1; + } + } else { + break; + } + } + + 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]) + } + + pub fn update_endpoints( + &mut self, + dim: usize, + proxies: &BroadPhaseProxies, + 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() as usize].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. + if aabb_i.intersects(&proxies[endpoint_j.proxy() as usize].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() as usize].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 + // ); + } +} diff --git a/src/geometry/broad_phase_multi_sap/sap_endpoint.rs b/src/geometry/broad_phase_multi_sap/sap_endpoint.rs new file mode 100644 index 0000000..c3e69b2 --- /dev/null +++ b/src/geometry/broad_phase_multi_sap/sap_endpoint.rs @@ -0,0 +1,60 @@ +use super::SENTINEL_VALUE; +use crate::math::Real; + +#[derive(Copy, Clone, Debug, PartialEq)] +#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] +pub(crate) 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 + } +} diff --git a/src/geometry/broad_phase_multi_sap/sap_region.rs b/src/geometry/broad_phase_multi_sap/sap_region.rs new file mode 100644 index 0000000..ba251c6 --- /dev/null +++ b/src/geometry/broad_phase_multi_sap/sap_region.rs @@ -0,0 +1,127 @@ +use super::{BroadPhaseProxies, SAPAxis}; +use crate::math::DIM; +use bit_vec::BitVec; +use parry::bounding_volume::AABB; +use parry::utils::hashmap::HashMap; + +#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] +#[derive(Clone)] +pub(crate) struct SAPRegion { + pub axes: [SAPAxis; DIM], + pub existing_proxies: BitVec, + #[cfg_attr(feature = "serde-serialize", serde(skip))] + pub to_insert: Vec, // Workspace + pub update_count: u8, + pub 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(), + update_count: 0, + proxy_count: 0, + } + } + + pub fn recycle(bounds: AABB, mut old: Self) -> Self { + // Correct the bounds + for (axis, &bound) in old.axes.iter_mut().zip(bounds.mins.iter()) { + axis.min_bound = bound; + } + for (axis, &bound) in old.axes.iter_mut().zip(bounds.maxs.iter()) { + axis.max_bound = bound; + } + + old.update_count = 0; + + // The rest of the fields should be "empty" + assert_eq!(old.proxy_count, 0); + assert!(old.to_insert.is_empty()); + debug_assert!(!old.existing_proxies.any()); + assert!(old.axes.iter().all(|ax| ax.endpoints.len() == 2)); + + old + } + + pub fn recycle_or_new(bounds: AABB, pool: &mut Vec) -> Self { + if let Some(old) = pool.pop() { + Self::recycle(bounds, old) + } else { + Self::new(bounds) + } + } + + 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 + // handled transparently during the next update. + self.update_count = 1; + } + + pub fn preupdate_proxy(&mut self, proxy_id: usize) -> bool { + let mask_len = self.existing_proxies.len(); + if proxy_id >= mask_len { + self.existing_proxies.grow(proxy_id + 1 - mask_len, false); + } + + if !self.existing_proxies[proxy_id] { + self.to_insert.push(proxy_id); + self.existing_proxies.set(proxy_id, true); + self.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( + &mut self, + proxies: &BroadPhaseProxies, + reporting: &mut HashMap<(u32, u32), bool>, + ) { + if self.update_count > 0 { + // Update endpoints. + let mut deleted = 0; + + for dim in 0..DIM { + self.axes[dim].update_endpoints(dim, proxies, reporting); + deleted += self.axes[dim].delete_out_of_bounds_proxies(&mut self.existing_proxies); + } + + if deleted > 0 { + self.proxy_count -= deleted; + 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; + } + } +} diff --git a/src/geometry/broad_phase_multi_sap/sap_utils.rs b/src/geometry/broad_phase_multi_sap/sap_utils.rs new file mode 100644 index 0000000..a32d974 --- /dev/null +++ b/src/geometry/broad_phase_multi_sap/sap_utils.rs @@ -0,0 +1,27 @@ +use crate::math::{Point, Real, Vector}; +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) fn sort2(a: u32, b: u32) -> (u32, u32) { + assert_ne!(a, b); + + if a < b { + (a, b) + } else { + (b, a) + } +} + +pub(crate) fn point_key(point: Point) -> Point { + (point / CELL_WIDTH).coords.map(|e| e.floor() as i32).into() +} + +pub(crate) fn region_aabb(index: Point) -> AABB { + let mins = index.coords.map(|i| i as Real * CELL_WIDTH).into(); + let maxs = mins + Vector::repeat(CELL_WIDTH); + AABB::new(mins, maxs) +} -- cgit From 7983c256064b021400a529be01bd092d87ed0e85 Mon Sep 17 00:00:00 2001 From: Crozet Sébastien Date: Mon, 8 Mar 2021 15:12:45 +0100 Subject: Start introducing SAP layers. --- src/geometry/broad_phase_multi_sap/broad_phase.rs | 127 +++----------------- .../broad_phase_multi_sap/broad_phase_proxy.rs | 1 + src/geometry/broad_phase_multi_sap/mod.rs | 2 + src/geometry/broad_phase_multi_sap/sap_layer.rs | 131 +++++++++++++++++++++ 4 files changed, 153 insertions(+), 108 deletions(-) create mode 100644 src/geometry/broad_phase_multi_sap/sap_layer.rs (limited to 'src/geometry/broad_phase_multi_sap') diff --git a/src/geometry/broad_phase_multi_sap/broad_phase.rs b/src/geometry/broad_phase_multi_sap/broad_phase.rs index f7006d3..fc6d8f6 100644 --- a/src/geometry/broad_phase_multi_sap/broad_phase.rs +++ b/src/geometry/broad_phase_multi_sap/broad_phase.rs @@ -1,11 +1,11 @@ use super::{ - BroadPhasePairEvent, BroadPhaseProxies, BroadPhaseProxy, ColliderPair, SAPRegion, + BroadPhasePairEvent, BroadPhaseProxies, BroadPhaseProxy, ColliderPair, SAPLayer, SAPRegion, NEXT_FREE_SENTINEL, SENTINEL_VALUE, }; use crate::data::pubsub::Subscription; use crate::dynamics::RigidBodySet; use crate::geometry::{ColliderSet, RemovedCollider}; -use crate::math::{Point, Real}; +use crate::math::Real; use parry::bounding_volume::BoundingVolume; use parry::utils::hashmap::HashMap; @@ -14,13 +14,11 @@ use parry::utils::hashmap::HashMap; #[derive(Clone)] pub struct BroadPhase { proxies: BroadPhaseProxies, - regions: HashMap, SAPRegion>, + layers: Vec, removed_colliders: Option>, deleted_any: bool, #[cfg_attr(feature = "serde-serialize", serde(skip))] region_pool: Vec, // To avoid repeated allocations. - #[cfg_attr(feature = "serde-serialize", serde(skip))] - regions_to_remove: Vec>, // Workspace // 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. @@ -48,9 +46,8 @@ impl BroadPhase { BroadPhase { removed_colliders: None, proxies: BroadPhaseProxies::new(), - regions: HashMap::default(), + layers: vec![SAPLayer::new(0)], region_pool: Vec::new(), - regions_to_remove: Vec::new(), reporting: HashMap::default(), deleted_any: false, } @@ -80,31 +77,8 @@ impl BroadPhase { let proxy = &mut self.proxies[proxy_index]; - // 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); - - #[cfg(feature = "dim2")] - for i in start.x..=end.x { - for j in start.y..=end.y { - if let Some(region) = self.regions.get_mut(&Point::new(i, j)) { - region.predelete_proxy(proxy_index); - self.deleted_any = true; - } - } - } - - #[cfg(feature = "dim3")] - for i in start.x..=end.x { - for j in start.y..=end.y { - for k in start.z..=end.z { - if let Some(region) = self.regions.get_mut(&Point::new(i, j, k)) { - region.predelete_proxy(proxy_index); - self.deleted_any = true; - } - } - } - } + let layer = &mut self.layers[proxy.layer as usize]; + layer.remove_collider(proxy, proxy_index); // Push the proxy to infinity, but not beyond the sentinels. proxy.aabb.mins.coords.fill(SENTINEL_VALUE / 2.0); @@ -134,93 +108,41 @@ impl BroadPhase { let collider = &mut colliders[*handle]; let aabb = collider.compute_aabb().loosened(prediction_distance / 2.0); - if let Some(proxy) = self.proxies.get_mut(collider.proxy_index) { + let layer = if let Some(proxy) = self.proxies.get_mut(collider.proxy_index) { proxy.aabb = aabb; + proxy.layer } else { + let layer = 0; // FIXME: compute the actual layer. let proxy = BroadPhaseProxy { handle: *handle, aabb, next_free: NEXT_FREE_SENTINEL, + layer, }; collider.proxy_index = self.proxies.insert(proxy); - } - - // Discretize the aabb. - let proxy_id = collider.proxy_index; - // let start = Point::origin(); - // let end = Point::origin(); - let start = super::point_key(aabb.mins); - let end = super::point_key(aabb.maxs); - - let regions = &mut self.regions; - let pool = &mut self.region_pool; - - #[cfg(feature = "dim2")] - for i in start.x..=end.x { - for j in start.y..=end.y { - let region_key = Point::new(i, j); - let region_bounds = region_aabb(region_key); - let region = regions - .entry(region_key) - .or_insert_with(|| SAPRegion::recycle_or_new(region_bounds, pool)); - let _ = region.preupdate_proxy(proxy_id); - } - } + layer + }; - #[cfg(feature = "dim3")] - for i in start.x..=end.x { - for j in start.y..=end.y { - for k in start.z..=end.z { - let region_key = Point::new(i, j, k); - let region_bounds = super::region_aabb(region_key); - let region = regions - .entry(region_key) - .or_insert_with(|| SAPRegion::recycle_or_new(region_bounds, pool)); - let _ = region.preupdate_proxy(proxy_id); - } - } - } + let layer = &mut self.layers[layer as usize]; + layer.preupdate_collider(collider, &aabb, &mut self.region_pool); } } } - fn update_regions(&mut self) { - for (point, region) in &mut self.regions { - region.update(&self.proxies, &mut self.reporting); - if region.proxy_count == 0 { - self.regions_to_remove.push(*point); - } - } - - // Remove all the empty regions and store them in the region pool - let regions = &mut self.regions; - self.region_pool.extend( - self.regions_to_remove - .drain(..) - .map(|p| regions.remove(&p).unwrap()), - ); - } - pub(crate) fn complete_removals(&mut self) { - if self.deleted_any { - self.update_regions(); - + for layer in &mut self.layers { + layer.complete_removals(&self.proxies, &mut self.reporting, &mut self.region_pool); // NOTE: we don't care about reporting pairs. self.reporting.clear(); - self.deleted_any = false; } } pub(crate) fn find_pairs(&mut self, out_events: &mut Vec) { - // println!("num regions: {}", self.regions.len()); - self.reporting.clear(); - self.update_regions(); - // Convert reports to broad phase events. - // let t = instant::now(); - // let mut num_add_events = 0; - // let mut num_delete_events = 0; + for layer in &mut self.layers { + layer.update_regions(&self.proxies, &mut self.reporting, &mut self.region_pool); + } for ((proxy1, proxy2), colliding) in &self.reporting { let proxy1 = &self.proxies[*proxy1 as usize]; @@ -233,23 +155,12 @@ impl BroadPhase { out_events.push(BroadPhasePairEvent::AddPair(ColliderPair::new( handle1, handle2, ))); - // num_add_events += 1; } else { out_events.push(BroadPhasePairEvent::DeletePair(ColliderPair::new( handle1, handle2, ))); - // num_delete_events += 1; } } - - // println!( - // "Event conversion time: {}, add: {}/{}, delete: {}/{}", - // instant::now() - t, - // num_add_events, - // out_events.len(), - // num_delete_events, - // out_events.len() - // ); } } 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 9290ce8..ccc0f9c 100644 --- a/src/geometry/broad_phase_multi_sap/broad_phase_proxy.rs +++ b/src/geometry/broad_phase_multi_sap/broad_phase_proxy.rs @@ -9,6 +9,7 @@ pub(crate) struct BroadPhaseProxy { pub handle: ColliderHandle, pub aabb: AABB, pub next_free: u32, + pub layer: u8, } #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] diff --git a/src/geometry/broad_phase_multi_sap/mod.rs b/src/geometry/broad_phase_multi_sap/mod.rs index 849a325..9706cf0 100644 --- a/src/geometry/broad_phase_multi_sap/mod.rs +++ b/src/geometry/broad_phase_multi_sap/mod.rs @@ -4,6 +4,7 @@ pub use self::broad_phase_pair_event::{BroadPhasePairEvent, ColliderPair}; pub(self) use self::broad_phase_proxy::*; pub(self) use self::sap_axis::*; pub(self) use self::sap_endpoint::*; +pub(self) use self::sap_layer::*; pub(self) use self::sap_region::*; pub(self) use self::sap_utils::*; @@ -12,5 +13,6 @@ mod broad_phase_pair_event; mod broad_phase_proxy; mod sap_axis; mod sap_endpoint; +mod sap_layer; mod sap_region; mod sap_utils; diff --git a/src/geometry/broad_phase_multi_sap/sap_layer.rs b/src/geometry/broad_phase_multi_sap/sap_layer.rs new file mode 100644 index 0000000..71a97e2 --- /dev/null +++ b/src/geometry/broad_phase_multi_sap/sap_layer.rs @@ -0,0 +1,131 @@ +use super::{BroadPhaseProxies, SAPRegion}; +use crate::geometry::broad_phase_multi_sap::BroadPhaseProxy; +use crate::geometry::{Collider, AABB}; +use crate::math::{Point, Real}; +use parry::utils::hashmap::{Entry, HashMap}; + +#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] +#[derive(Clone)] +pub(crate) struct SAPLayer { + depth: i8, + region_width: Real, + regions: HashMap, SAPRegion>, + deleted_any: bool, + #[cfg_attr(feature = "serde-serialize", serde(skip))] + regions_to_remove: Vec>, // Workspace + #[cfg_attr(feature = "serde-serialize", serde(skip))] + created_regions: Vec>, +} + +impl SAPLayer { + pub fn new(depth: i8) -> Self { + Self { + depth, + region_width: super::CELL_WIDTH, // FIXME + regions: HashMap::default(), + deleted_any: false, + regions_to_remove: vec![], + created_regions: vec![], + } + } + + pub fn insert_subregion(&mut self, sub_key: &Point) {} + + pub fn preupdate_collider( + &mut self, + collider: &Collider, + aabb: &AABB, + pool: &mut Vec, + ) { + let proxy_id = collider.proxy_index; + let start = super::point_key(aabb.mins); + let end = super::point_key(aabb.maxs); + + // 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_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); + 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) { + // 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); + + #[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) = self.regions.get_mut(&key) { + region.predelete_proxy(proxy_index); + self.deleted_any = true; + } + } + } + } + } + + pub fn update_regions( + &mut self, + proxies: &BroadPhaseProxies, + reporting: &mut HashMap<(u32, u32), bool>, + pool: &mut Vec, + ) { + for (point, region) in &mut self.regions { + region.update(proxies, reporting); + if region.proxy_count == 0 { + self.regions_to_remove.push(*point); + } + } + + // Remove all the empty regions and store them in the region pool + let regions = &mut self.regions; + pool.extend( + self.regions_to_remove + .drain(..) + .map(|p| regions.remove(&p).unwrap()), + ); + } + + pub fn complete_removals( + &mut self, + proxies: &BroadPhaseProxies, + reporting: &mut HashMap<(u32, u32), bool>, + pool: &mut Vec, + ) { + if self.deleted_any { + self.update_regions(proxies, reporting, pool); + self.deleted_any = false; + } + } +} -- cgit From a967ace7d426eea11b4132328574cc3a48790ea5 Mon Sep 17 00:00:00 2001 From: Crozet Sébastien Date: Mon, 8 Mar 2021 18:27:06 +0100 Subject: Start implementing SAPLayer creation and insertion. --- src/geometry/broad_phase_multi_sap/broad_phase.rs | 184 +++++++++++++++++---- .../broad_phase_multi_sap/broad_phase_proxy.rs | 26 ++- src/geometry/broad_phase_multi_sap/sap_layer.rs | 39 +++-- src/geometry/broad_phase_multi_sap/sap_region.rs | 13 ++ src/geometry/broad_phase_multi_sap/sap_utils.rs | 26 ++- 5 files changed, 231 insertions(+), 57 deletions(-) (limited to 'src/geometry/broad_phase_multi_sap') 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, + smallest_layer: u8, + largest_layer: u8, removed_colliders: Option>, 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) { - 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), +} + +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, + pub larger_layer: Option, region_width: Real, regions: HashMap, SAPRegion>, deleted_any: bool, #[cfg_attr(feature = "serde-serialize", serde(skip))] regions_to_remove: Vec>, // Workspace #[cfg_attr(feature = "serde-serialize", serde(skip))] - created_regions: Vec>, + pub created_regions: Vec>, } impl SAPLayer { - pub fn new(depth: i8) -> Self { + pub fn new(depth: i8, smaller_layer: Option, bigger_layer: Option) -> 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) {} + 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) { + 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, ) { 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