Start implementing SAPLayer creation and insertion.

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