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();
}
}
}

26
src/geometry/broad_phase_multi_sap/broad_phase_proxy.rs
View File

@@ -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)

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

@@ -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(&region)
.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);
}

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

@@ -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

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

@@ -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
}