Start introducing SAP layers.

6 files changed, 208 insertions, 108 deletions

diff --git a/examples3d/all_examples3.rs b/examples3d/all_examples3.rs
index 724aa45..a8c38c6 100644
--- a/examples3d/all_examples3.rs
+++ b/examples3d/all_examples3.rs
@@ -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",
diff --git a/examples3d/debug_big_colliders3.rs b/examples3d/debug_big_colliders3.rs
new file mode 100644
index 0000000..c2b62e2
--- /dev/null
+++ b/examples3d/debug_big_colliders3.rs
@@ -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()
+}
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<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);
-                }
-
-                // 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<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()
-        // );
     }
 }
 
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<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;
+        }
+    }
+}