Start implementing SAPLayer creation and insertion.

src/geometry/broad_phase_multi_sap/broad_phase.rs

@@ -1,6 +1,6 @@
 use super::{
-    BroadPhasePairEvent, BroadPhaseProxies, BroadPhaseProxy, ColliderPair, SAPLayer, SAPRegion,
+    BroadPhasePairEvent, BroadPhaseProxies, BroadPhaseProxy, BroadPhaseProxyData, ColliderPair,
-    NEXT_FREE_SENTINEL, SENTINEL_VALUE,
+    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_id as usize];
-        let layer = &mut self.layers[proxy.layer as usize];
+        layer.remove_proxy(proxy, proxy_index);
-        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);
@@ -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 {
+            for ((proxy_id1, proxy_id2), colliding) in &self.reporting {
-            let proxy1 = &self.proxies[*proxy1 as usize];
+                let proxy1 = &self.proxies[*proxy_id1 as usize];
-            let proxy2 = &self.proxies[*proxy2 as usize];
+                let proxy2 = &self.proxies[*proxy_id2 as usize];
 
-            let handle1 = proxy1.handle;
+                let handle1 = proxy1.handle;
-            let handle2 = proxy2.handle;
+                let handle2 = proxy2.handle;
 
-            if *colliding {
+                match (&handle1, &handle2) {
-                out_events.push(BroadPhasePairEvent::AddPair(ColliderPair::new(
+                    (
-                    handle1, handle2,
+                        BroadPhaseProxyData::Collider(handle1),
-                )));
+                        BroadPhaseProxyData::Collider(handle2),
-            } else {
+                    ) => {
-                out_events.push(BroadPhasePairEvent::DeletePair(ColliderPair::new(
+                        if *colliding {
-                    handle1, handle2,
+                            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();
         }
     }
 }

src/geometry/broad_phase_multi_sap/broad_phase_proxy.rs

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

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 start = super::point_key(aabb.mins, self.region_width);
-        let end = super::point_key(aabb.maxs);
+        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 start = super::point_key(proxy.aabb.mins, self.region_width);
-        let end = super::point_key(proxy.aabb.maxs);
+        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);
             }

src/geometry/broad_phase_multi_sap/sap_region.rs

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

src/geometry/broad_phase_multi_sap/sap_utils.rs

@@ -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> {
+pub(crate) fn point_key(point: Point<Real>, region_width: Real) -> Point<i32> {
-    (point / CELL_WIDTH).coords.map(|e| e.floor() as i32).into()
+    (point / region_width)
+        .coords
+        .map(|e| e.floor() as i32)
+        .into()
 }
 
-pub(crate) fn region_aabb(index: Point<i32>) -> AABB {
+pub(crate) fn region_aabb(index: Point<i32>, region_width: Real) -> AABB {
-    let mins = index.coords.map(|i| i as Real * CELL_WIDTH).into();
+    let mins = index.coords.map(|i| i as Real * region_width).into();
-    let maxs = mins + Vector::repeat(CELL_WIDTH);
+    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
+}

src_testbed/nphysics_backend.rs

@@ -228,7 +228,11 @@ fn nphysics_collider_from_rapier_collider(
         }
     };
 
-    let density = if is_dynamic { collider.density() } else { 0.0 };
+    let density = if is_dynamic {
+        collider.density().unwrap_or(0.0)
+    } else {
+        0.0
+    };
 
     Some(
         ColliderDesc::new(shape)

src_testbed/physx_backend.rs

@@ -237,7 +237,7 @@ impl PhysxWorld {
                 let densities: Vec<_> = rb
                     .colliders()
                     .iter()
-                    .map(|h| colliders[*h].density())
+                    .map(|h| colliders[*h].density().unwrap_or(0.0))
                     .collect();
 
                 unsafe {