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

Start introducing SAP layers.

Browse Source
This commit is contained in:
Crozet Sébastien
2021-03-08 15:12:45 +01:00
parent 0b80bc827c
commit 7983c25606
6 changed files with 208 additions and 108 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
examples3d/all_examples3.rs
View File

@@ -16,6 +16,7 @@ mod convex_decomposition3;
mod convex_polyhedron3;
mod damping3;
mod debug_add_remove_collider3;
mod debug_big_colliders3;
mod debug_boxes3;
mod debug_cylinder3;
mod debug_dynamic_collider_add3;
@@ -95,6 +96,7 @@ pub fn main() {
"(Debug) add/rm collider",
debug_add_remove_collider3::init_world,
),
("(Debug) big colliders", debug_big_colliders3::init_world),
("(Debug) boxes", debug_boxes3::init_world),
(
"(Debug) dyn. coll. add",

53
examples3d/debug_big_colliders3.rs Normal file
View File

@@ -0,0 +1,53 @@
use na::Point3;
use rapier3d::dynamics::{JointSet, RigidBodyBuilder, RigidBodySet};
use rapier3d::geometry::{ColliderBuilder, ColliderSet};
use rapier_testbed3d::Testbed;
pub fn init_world(testbed: &mut Testbed) {
/*
* World
*/
let mut bodies = RigidBodySet::new();
let mut colliders = ColliderSet::new();
let joints = JointSet::new();
/*
* Ground
*/
let ground_size = 100.0;
let ground_height = 0.1;
let rigid_body = RigidBodyBuilder::new_static().build();
let handle = bodies.insert(rigid_body);
let collider = ColliderBuilder::cuboid(ground_size, ground_height, ground_size)
.friction(1.5)
.build();
colliders.insert(collider, handle, &mut bodies);
let mut curr_y = 0.0;
let mut curr_width = 1_000.0;
for _ in 0..6 {
curr_y += curr_width;
let rigid_body = RigidBodyBuilder::new_dynamic()
.translation(0.0, curr_y, 0.0)
.build();
let handle = bodies.insert(rigid_body);
let collider = ColliderBuilder::cuboid(curr_width, curr_width, curr_width).build();
colliders.insert(collider, handle, &mut bodies);
curr_width /= 10.0;
}
/*
* Set up the testbed.
*/
testbed.set_world(bodies, colliders, joints);
testbed.look_at(Point3::new(10.0, 10.0, 10.0), Point3::origin());
}
fn main() {
let testbed = Testbed::from_builders(0, vec![("Boxes", init_world)]);
testbed.run()
}

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

@@ -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<Point<i32>, SAPRegion>,
layers: Vec<SAPLayer>,
removed_colliders: Option<Subscription<RemovedCollider>>,
deleted_any: bool,
#[cfg_attr(feature = "serde-serialize", serde(skip))]
region_pool: Vec<SAPRegion>, // To avoid repeated allocations.
#[cfg_attr(feature = "serde-serialize", serde(skip))]
regions_to_remove: Vec<Point<i32>>, // 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);
}
layer
};
// 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);
}
}
}
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<BroadPhasePairEvent>) {
// 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()
// );
}
}

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

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

2
src/geometry/broad_phase_multi_sap/mod.rs
View File

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

131
src/geometry/broad_phase_multi_sap/sap_layer.rs Normal file
View File

@@ -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<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>>,
}
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<i32>) {}
pub fn preupdate_collider(
&mut self,
collider: &Collider,
aabb: &AABB,
pool: &mut Vec<SAPRegion>,
) {
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<SAPRegion>,
) {
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<SAPRegion>,
) {
if self.deleted_any {
self.update_regions(proxies, reporting, pool);
self.deleted_any = false;
}
}
}