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

Fix the last few bugs and unbounded memory usage.

Browse Source
This commit is contained in:
Crozet Sébastien
2021-03-17 09:34:56 +01:00
parent d82a675b46
commit 326469a1df
10 changed files with 211 additions and 135 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -213,7 +213,9 @@ impl BroadPhase {
*/
let cursor = self.removed_colliders.as_ref().unwrap();
for collider in colliders.removed_colliders.read(&cursor) {
self.proxies.remove(collider.proxy_index);
if collider.proxy_index != crate::INVALID_U32 {
self.proxies.remove(collider.proxy_index);
}
}
colliders.removed_colliders.ack(&cursor);
}
@@ -222,7 +224,7 @@ impl BroadPhase {
if let Some(larger_layer) = self.layers[layer_id as usize].larger_layer {
// Remove all the region endpoints from the larger layer.
// They will be automatically replaced by the new layer's regions.
self.layers[larger_layer as usize].delete_all_region_endpoints(&mut self.proxies);
self.layers[larger_layer as usize].unregister_all_subregions(&mut self.proxies);
}
if let Some(smaller_layer) = self.layers[layer_id as usize].smaller_layer {
@@ -305,6 +307,7 @@ impl BroadPhase {
}
}
/// Updates the broad-phase, taking into account the new collider positions.
pub fn update(
&mut self,
prediction_distance: Real,
@@ -324,7 +327,9 @@ impl BroadPhase {
{
for handle in &bodies[*body_handle].colliders {
let collider = &mut colliders[*handle];
let aabb = collider.compute_aabb().loosened(prediction_distance / 2.0);
let mut aabb = collider.compute_aabb().loosened(prediction_distance / 2.0);
aabb.mins = super::clamp_point(aabb.mins);
aabb.maxs = super::clamp_point(aabb.maxs);
let layer_id = if let Some(proxy) = self.proxies.get_mut(collider.proxy_index) {
proxy.aabb = aabb;
@@ -334,7 +339,7 @@ impl BroadPhase {
let layer_id = self.ensure_layer_exists(layer_depth);
// Create the proxy.
let proxy = SAPProxy::collider(*handle, aabb, layer_id);
let proxy = SAPProxy::collider(*handle, aabb, layer_id, layer_depth);
collider.proxy_index = self.proxies.insert(proxy);
layer_id
};
@@ -443,13 +448,19 @@ impl BroadPhase {
(SAPProxyData::Collider(_), SAPProxyData::Region(_)) => {
if *colliding {
// Add the collider to the subregion.
proxy2.data.as_region_mut().preupdate_proxy(*proxy_id1);
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);
proxy1
.data
.as_region_mut()
.preupdate_proxy(*proxy_id2, false);
}
}
(SAPProxyData::Region(_), SAPProxyData::Region(_)) => {

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

@@ -29,6 +29,13 @@ impl SAPAxis {
}
}
pub fn clear(&mut self) {
self.new_endpoints.clear();
self.endpoints.clear();
self.endpoints.push(SAPEndpoint::start_sentinel());
self.endpoints.push(SAPEndpoint::end_sentinel());
}
pub fn batch_insert(
&mut self,
dim: usize,
@@ -115,14 +122,29 @@ impl SAPAxis {
}
}
pub fn delete_out_of_bounds_proxies(&self, existing_proxies: &mut BitVec) -> usize {
let mut deleted = 0;
/// 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_idx = endpoint.proxy() as usize;
if endpoint.is_end() && existing_proxies[proxy_idx] {
existing_proxies.set(proxy_idx, false);
deleted += 1;
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;
@@ -131,17 +153,22 @@ impl SAPAxis {
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;
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;
}
}
deleted
(num_proxies_deleted, num_subproper_proxies_deleted)
}
pub fn delete_out_of_bounds_endpoints(&mut self, existing_proxies: &BitVec) {
@@ -149,26 +176,39 @@ impl SAPAxis {
.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_deleted = 0;
let mut num_subproper_proxies_deleted = 0;
self.endpoints.retain(|endpt| {
if !endpt.is_sentinel() && proxies[endpt.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);
num_deleted += 1;
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;
}
false
} else {
true
}
true
});
num_deleted
num_subproper_proxies_deleted
}
pub fn update_endpoints(
@@ -197,7 +237,8 @@ impl SAPAxis {
if endpoint_j.is_end() {
// Report start collision.
if aabb_i.intersects(&proxies[endpoint_j.proxy()].aabb) {
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);
}

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

@@ -38,10 +38,18 @@ impl SAPLayer {
}
}
pub fn delete_all_region_endpoints(&mut self, proxies: &mut SAPProxies) {
pub fn unregister_all_subregions(&mut self, proxies: &mut SAPProxies) {
for region_id in self.regions.values() {
if let Some(mut region) = proxies[*region_id].data.take_region() {
region.delete_all_region_endpoints(proxies);
for subregion in region.subregions.drain(..) {
proxies[subregion]
.data
.as_region_mut()
.id_in_parent_subregion = crate::INVALID_U32;
}
proxies[*region_id].data.set_region(region);
}
}
@@ -77,14 +85,17 @@ impl SAPLayer {
pool: &mut SAPRegionPool,
) {
if let Some(proxy) = proxies.get(proxy_id) {
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;
if proxy.data.as_region().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;
}
}
}
@@ -106,9 +117,10 @@ impl SAPLayer {
region.needs_update_after_subregion_removal = true;
}
region
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.
@@ -137,7 +149,8 @@ impl SAPLayer {
Entry::Vacant(vacant) => {
let region_bounds = super::region_aabb(region_key, self.region_width);
let region = SAPRegion::recycle_or_new(region_bounds, pool);
let region_proxy = SAPProxy::subregion(region, region_bounds, self.layer_id);
let region_proxy =
SAPProxy::subregion(region, region_bounds, self.layer_id, self.depth);
let region_proxy_id = proxies.insert(region_proxy);
self.created_regions.push(region_proxy_id as u32);
let _ = vacant.insert(region_proxy_id);
@@ -172,7 +185,7 @@ impl SAPLayer {
let region_key = Point::new(i, j, _k);
let region_id = self.ensure_region_exists(region_key, proxies, pool);
let region = proxies[region_id].data.as_region_mut();
region.preupdate_proxy(proxy_id);
region.preupdate_proxy(proxy_id, true);
}
}
}
@@ -222,14 +235,14 @@ impl SAPLayer {
for (point, region_id) in &self.regions {
if let Some(mut region) = proxies[*region_id].data.take_region() {
// Update the region.
region.update(proxies, reporting);
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.proxy_count == 0 {
if region.subproper_proxy_count == 0 {
self.regions_to_potentially_remove.push(*point);
}
@@ -257,10 +270,10 @@ impl SAPLayer {
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);
region.update_after_subregion_removal(proxies, self.depth);
// Check if we actually can delete this region.
if region.proxy_count == 0 {
// Check if we can actually delete this region.
if region.subproper_proxy_count == 0 {
let region_id = region_id.remove();
// We can delete this region. So we need to tell the larger

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

@@ -14,19 +14,19 @@ pub enum SAPProxyData {
}
impl SAPProxyData {
pub fn is_subregion(&self) -> bool {
pub fn is_region(&self) -> bool {
match self {
SAPProxyData::Region(_) => true,
_ => false,
}
}
// pub fn as_region(&self) -> &SAPRegion {
// match self {
// SAPProxyData::Region(r) => r.as_ref().unwrap(),
// _ => panic!("Invalid proxy type."),
// }
// }
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 {
@@ -53,25 +53,29 @@ pub struct SAPProxy {
pub data: SAPProxyData,
pub aabb: AABB,
pub next_free: SAPProxyIndex,
// 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) -> Self {
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) -> Self {
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,
}
}
}
@@ -92,7 +96,7 @@ impl SAPProxies {
}
pub fn insert(&mut self, proxy: SAPProxy) -> SAPProxyIndex {
if self.first_free != NEXT_FREE_SENTINEL {
let result = 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;
@@ -100,7 +104,9 @@ impl SAPProxies {
} else {
self.elements.push(proxy);
self.elements.len() as u32 - 1
}
};
result
}
pub fn remove(&mut self, proxy_id: SAPProxyIndex) {

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

@@ -18,7 +18,10 @@ pub struct SAPRegion {
pub id_in_parent_subregion: u32,
pub update_count: u8,
pub needs_update_after_subregion_removal: bool,
pub proxy_count: usize,
// Number of proxies (added to this region) that originates
// from the layer at depth <= the depth of the layer containing
// this region.
pub subproper_proxy_count: usize,
}
impl SAPRegion {
@@ -37,26 +40,27 @@ impl SAPRegion {
id_in_parent_subregion: crate::INVALID_U32,
update_count: 0,
needs_update_after_subregion_removal: false,
proxy_count: 0,
subproper_proxy_count: 0,
}
}
pub fn recycle(bounds: AABB, mut old: Box<Self>) -> Box<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;
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;
old.existing_proxies.clear();
old.id_in_parent_subregion = crate::INVALID_U32;
// The rest of the fields should be "empty"
assert_eq!(old.proxy_count, 0);
assert_eq!(old.subproper_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
}
@@ -75,7 +79,7 @@ impl SAPRegion {
axis.endpoints.retain(|e| {
if let Some(proxy) = proxies.get(e.proxy()) {
existing_proxies.set(e.proxy() as usize, false);
!proxy.data.is_subregion()
!proxy.data.is_region()
} else {
true
}
@@ -97,11 +101,11 @@ impl SAPRegion {
pub fn register_subregion(&mut self, proxy_id: SAPProxyIndex) -> usize {
let subregion_index = self.subregions.len();
self.subregions.push(proxy_id);
self.preupdate_proxy(proxy_id);
self.preupdate_proxy(proxy_id, true);
subregion_index
}
pub fn preupdate_proxy(&mut self, proxy_id: SAPProxyIndex) -> bool {
pub fn preupdate_proxy(&mut self, proxy_id: SAPProxyIndex, is_subproper_proxy: bool) -> bool {
let mask_len = self.existing_proxies.len();
if proxy_id as usize >= mask_len {
self.existing_proxies
@@ -111,7 +115,11 @@ impl SAPRegion {
if !self.existing_proxies[proxy_id as usize] {
self.to_insert.push(proxy_id);
self.existing_proxies.set(proxy_id as usize, true);
self.proxy_count += 1;
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
@@ -122,31 +130,41 @@ impl SAPRegion {
}
}
pub fn update_after_subregion_removal(&mut self, proxies: &SAPProxies) {
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 {
self.proxy_count -= axis
self.subproper_proxy_count -= axis
.delete_deleted_proxies_and_endpoints_after_subregion_removal(
proxies,
&mut self.existing_proxies,
layer_depth,
);
}
self.needs_update_after_subregion_removal = false;
}
}
pub fn update(&mut self, proxies: &SAPProxies, reporting: &mut HashMap<(u32, u32), bool>) {
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 deleted = 0;
let mut total_deleted = 0;
let mut total_deleted_subproper = 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);
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 deleted > 0 {
self.proxy_count -= deleted;
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);
}

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

@@ -5,6 +5,7 @@ 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;
@@ -18,6 +19,10 @@ pub(crate) fn sort2(a: u32, b: u32) -> (u32, u32) {
}
}
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<i32> {
(point / region_width)
.coords
@@ -32,7 +37,7 @@ pub(crate) fn region_aabb(index: Point<i32>, region_width: Real) -> AABB {
}
pub(crate) fn region_width(depth: i8) -> Real {
REGION_WIDTH_BASE * REGION_WIDTH_POWER_BASIS.powi(depth as i32)
(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.
@@ -49,8 +54,9 @@ pub(crate) fn layer_containing_aabb(aabb: &AABB) -> i8 {
const NUM_ELEMENTS_PER_DIMENSION: Real = 10.0;
let width = 2.0 * aabb.half_extents().norm() * NUM_ELEMENTS_PER_DIMENSION;
// TODO: handle overflows in the f32 -> i8 conversion.
(width / REGION_WIDTH_BASE)
.log(REGION_WIDTH_POWER_BASIS)
.round() as i8
.round()
.max(i8::MIN as Real)
.min(i8::MAX as Real) as i8
}