Fix the last few bugs and unbounded memory usage.

examples3d/debug_big_colliders3.rs

@@ -1,6 +1,6 @@
-use na::Point3;
+use na::{Point3, Vector3};
 use rapier3d::dynamics::{JointSet, RigidBodyBuilder, RigidBodySet};
-use rapier3d::geometry::{ColliderBuilder, ColliderSet};
+use rapier3d::geometry::{ColliderBuilder, ColliderSet, HalfSpace, SharedShape};
 use rapier_testbed3d::Testbed;
 
 pub fn init_world(testbed: &mut Testbed) {
@@ -14,30 +14,27 @@ pub fn init_world(testbed: &mut Testbed) {
     /*
      * 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)
+    let halfspace = SharedShape::new(HalfSpace::new(Vector3::y_axis()));
-        .friction(1.5)
+    let collider = ColliderBuilder::new(halfspace).build();
-        .build();
     colliders.insert(collider, handle, &mut bodies);
 
     let mut curr_y = 0.0;
-    let mut curr_width = 1_000.0;
+    let mut curr_width = 10_000.0;
 
-    for _ in 0..6 {
+    for _ in 0..12 {
-        curr_y += curr_width;
+        let curr_height = 0.1f32.min(curr_width);
+        curr_y += curr_height * 4.0;
 
         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();
+        let collider = ColliderBuilder::cuboid(curr_width, curr_height, curr_width).build();
         colliders.insert(collider, handle, &mut bodies);
 
-        curr_width /= 10.0;
+        curr_width /= 5.0;
     }
 
     /*

examples3d/debug_infinite_fall3.rs

@@ -1,3 +1,4 @@
+use na::Point3;
 use rapier3d::dynamics::{JointSet, RigidBodyBuilder, RigidBodySet};
 use rapier3d::geometry::{ColliderBuilder, ColliderSet};
 use rapier_testbed3d::Testbed;
@@ -17,7 +18,7 @@ pub fn init_world(testbed: &mut Testbed) {
     let ground_height = 2.1;
 
     let rigid_body = RigidBodyBuilder::new_static()
-        .translation(0.0, -ground_height, 0.0)
+        .translation(0.0, 4.0, 0.0)
         .build();
     let handle = bodies.insert(rigid_body);
     let collider = ColliderBuilder::cuboid(ground_size, ground_height, ground_size).build();
@@ -26,7 +27,7 @@ pub fn init_world(testbed: &mut Testbed) {
     let rad = 1.0;
     // Build the dynamic box rigid body.
     let rigid_body = RigidBodyBuilder::new_dynamic()
-        .translation(0.0, 3.0 * rad, 0.0)
+        .translation(0.0, 7.0 * rad, 0.0)
         .can_sleep(false)
         .build();
     let handle = bodies.insert(rigid_body);
@@ -34,7 +35,7 @@ pub fn init_world(testbed: &mut Testbed) {
     colliders.insert(collider, handle, &mut bodies);
 
     let rigid_body = RigidBodyBuilder::new_dynamic()
-        .translation(0.0, 5.0 * rad, 0.0)
+        .translation(0.0, 2.0 * rad, 0.0)
         .can_sleep(false)
         .build();
     let handle = bodies.insert(rigid_body);
@@ -44,6 +45,7 @@ pub fn init_world(testbed: &mut Testbed) {
     /*
      * Set up the testbed.
      */
+    testbed.look_at(Point3::new(100.0, -10.0, 100.0), Point3::origin());
     testbed.set_world(bodies, colliders, joints);
 }
 

examples3d/fountain3.rs

@@ -15,7 +15,7 @@ pub fn init_world(testbed: &mut Testbed) {
      * Ground
      */
     let ground_size = 100.1;
-    let ground_height = 2.1;
+    let ground_height = 2.1; // 16.0;
 
     let rigid_body = RigidBodyBuilder::new_static()
         .translation(0.0, -ground_height, 0.0)
@@ -64,10 +64,6 @@ pub fn init_world(testbed: &mut Testbed) {
                 physics
                     .bodies
                     .remove(*handle, &mut physics.colliders, &mut physics.joints);
-                physics.broad_phase.maintain(&mut physics.colliders);
-                physics
-                    .narrow_phase
-                    .maintain(&mut physics.colliders, &mut physics.bodies);
 
                 if let (Some(graphics), Some(window)) = (&mut graphics, &mut window) {
                     graphics.remove_body_nodes(window, *handle);
@@ -80,10 +76,10 @@ pub fn init_world(testbed: &mut Testbed) {
      * Set up the testbed.
      */
     testbed.set_world(bodies, colliders, joints);
-    testbed
+    // testbed
-        .physics_state_mut()
+    //     .physics_state_mut()
-        .integration_parameters
+    //     .integration_parameters
-        .velocity_based_erp = 0.2;
+    //     .velocity_based_erp = 0.2;
     testbed.look_at(Point3::new(-30.0, 4.0, -30.0), Point3::new(0.0, 1.0, 0.0));
 }
 

examples3d/primitives3.rs

@@ -11,6 +11,19 @@ pub fn init_world(testbed: &mut Testbed) {
     let mut colliders = ColliderSet::new();
     let joints = JointSet::new();
 
+    /*
+     * Ground
+     */
+    let ground_size = 100.1;
+    let ground_height = 2.1;
+
+    let rigid_body = RigidBodyBuilder::new_static()
+        .translation(0.0, -ground_height, 0.0)
+        .build();
+    let handle = bodies.insert(rigid_body);
+    let collider = ColliderBuilder::cuboid(ground_size, ground_height, ground_size).build();
+    colliders.insert(collider, handle, &mut bodies);
+
     /*
      * Create the cubes
      */
@@ -26,11 +39,11 @@ pub fn init_world(testbed: &mut Testbed) {
 
     let mut offset = -(num as f32) * (rad * 2.0 + rad) * 0.5;
 
-    for j in 0usize..1 {
+    for j in 0usize..20 {
-        for i in 0..1 {
+        for i in 0..num {
-            for k in 0usize..1 {
+            for k in 0usize..num {
                 let x = i as f32 * shiftx - centerx + offset;
-                let y = j as f32 * shifty + centery - rad;
+                let y = j as f32 * shifty + centery + 3.0;
                 let z = k as f32 * shiftz - centerz + offset;
 
                 // Build the rigid body.
@@ -54,37 +67,10 @@ pub fn init_world(testbed: &mut Testbed) {
         offset -= 0.05 * rad * (num as f32 - 1.0);
     }
 
-    /*
-     * Ground
-     */
-    testbed.add_callback(
-        move |mut window, mut graphics, physics, events, run_state| {
-            if run_state.timestep_id == 10 {
-                let ground_size = 100.1;
-                let ground_height = 2.1;
-
-                let rigid_body = RigidBodyBuilder::new_static()
-                    .translation(0.0, -ground_height, 0.0)
-                    .build();
-                let handle = physics.bodies.insert(rigid_body);
-                let collider =
-                    ColliderBuilder::cuboid(ground_size, ground_height, ground_size).build();
-                physics
-                    .colliders
-                    .insert(collider, handle, &mut physics.bodies);
-
-                if let (Some(graphics), Some(window)) = (&mut graphics, &mut window) {
-                    graphics.add(window, handle, &physics.bodies, &physics.colliders);
-                }
-            }
-        },
-    );
-
     /*
      * Set up the testbed.
      */
     testbed.set_world(bodies, colliders, joints);
-    testbed.physics_state_mut().gravity.fill(0.0);
     testbed.look_at(Point3::new(100.0, 100.0, 100.0), Point3::origin());
 }
 

src/geometry/broad_phase_multi_sap/broad_phase.rs

@@ -213,7 +213,9 @@ impl BroadPhase {
          */
         let cursor = self.removed_colliders.as_ref().unwrap();
         for collider in colliders.removed_colliders.read(&cursor) {
-            self.proxies.remove(collider.proxy_index);
+            if collider.proxy_index != crate::INVALID_U32 {
+                self.proxies.remove(collider.proxy_index);
+            }
         }
         colliders.removed_colliders.ack(&cursor);
     }
@@ -222,7 +224,7 @@ impl BroadPhase {
         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 regions.
-            self.layers[larger_layer as usize].delete_all_region_endpoints(&mut self.proxies);
+            self.layers[larger_layer as usize].unregister_all_subregions(&mut self.proxies);
         }
 
         if let Some(smaller_layer) = self.layers[layer_id as usize].smaller_layer {
@@ -305,6 +307,7 @@ impl BroadPhase {
         }
     }
 
+    /// Updates the broad-phase, taking into account the new collider positions.
     pub fn update(
         &mut self,
         prediction_distance: Real,
@@ -324,7 +327,9 @@ impl BroadPhase {
         {
             for handle in &bodies[*body_handle].colliders {
                 let collider = &mut colliders[*handle];
-                let aabb = collider.compute_aabb().loosened(prediction_distance / 2.0);
+                let mut aabb = collider.compute_aabb().loosened(prediction_distance / 2.0);
+                aabb.mins = super::clamp_point(aabb.mins);
+                aabb.maxs = super::clamp_point(aabb.maxs);
 
                 let layer_id = if let Some(proxy) = self.proxies.get_mut(collider.proxy_index) {
                     proxy.aabb = aabb;
@@ -334,7 +339,7 @@ impl BroadPhase {
                     let layer_id = self.ensure_layer_exists(layer_depth);
 
                     // Create the proxy.
-                    let proxy = SAPProxy::collider(*handle, aabb, layer_id);
+                    let proxy = SAPProxy::collider(*handle, aabb, layer_id, layer_depth);
                     collider.proxy_index = self.proxies.insert(proxy);
                     layer_id
                 };
@@ -443,13 +448,19 @@ impl BroadPhase {
                     (SAPProxyData::Collider(_), SAPProxyData::Region(_)) => {
                         if *colliding {
                             // Add the collider to the subregion.
-                            proxy2.data.as_region_mut().preupdate_proxy(*proxy_id1);
+                            proxy2
+                                .data
+                                .as_region_mut()
+                                .preupdate_proxy(*proxy_id1, false);
                         }
                     }
                     (SAPProxyData::Region(_), SAPProxyData::Collider(_)) => {
                         if *colliding {
                             // Add the collider to the subregion.
-                            proxy1.data.as_region_mut().preupdate_proxy(*proxy_id2);
+                            proxy1
+                                .data
+                                .as_region_mut()
+                                .preupdate_proxy(*proxy_id2, false);
                         }
                     }
                     (SAPProxyData::Region(_), SAPProxyData::Region(_)) => {

src/geometry/broad_phase_multi_sap/sap_axis.rs

@@ -29,6 +29,13 @@ impl SAPAxis {
         }
     }
 
+    pub fn clear(&mut self) {
+        self.new_endpoints.clear();
+        self.endpoints.clear();
+        self.endpoints.push(SAPEndpoint::start_sentinel());
+        self.endpoints.push(SAPEndpoint::end_sentinel());
+    }
+
     pub fn batch_insert(
         &mut self,
         dim: usize,
@@ -115,14 +122,29 @@ impl SAPAxis {
         }
     }
 
-    pub fn delete_out_of_bounds_proxies(&self, existing_proxies: &mut BitVec) -> usize {
+    /// Removes from this axis all the endpoints that are out of bounds from this axis.
-        let mut deleted = 0;
+    ///
+    /// Returns the number of deleted proxies as well as the number of proxies deleted
+    /// such that `proxy.layer_depth <= layer_depth`.
+    pub fn delete_out_of_bounds_proxies(
+        &self,
+        proxies: &SAPProxies,
+        existing_proxies: &mut BitVec,
+        layer_depth: i8,
+    ) -> (usize, usize) {
+        let mut num_subproper_proxies_deleted = 0;
+        let mut num_proxies_deleted = 0;
         for endpoint in &self.endpoints {
             if endpoint.value < self.min_bound {
-                let proxy_idx = endpoint.proxy() as usize;
+                let proxy_id = endpoint.proxy();
-                if endpoint.is_end() && existing_proxies[proxy_idx] {
+                if endpoint.is_end() && existing_proxies[proxy_id as usize] {
-                    existing_proxies.set(proxy_idx, false);
+                    existing_proxies.set(proxy_id as usize, false);
-                    deleted += 1;
+
+                    if proxies[proxy_id].layer_depth <= layer_depth {
+                        num_subproper_proxies_deleted += 1;
+                    }
+
+                    num_proxies_deleted += 1;
                 }
             } else {
                 break;
@@ -131,17 +153,22 @@ impl SAPAxis {
 
         for endpoint in self.endpoints.iter().rev() {
             if endpoint.value > self.max_bound {
-                let proxy_idx = endpoint.proxy() as usize;
+                let proxy_id = endpoint.proxy();
-                if endpoint.is_start() && existing_proxies[proxy_idx] {
+                if endpoint.is_start() && existing_proxies[proxy_id as usize] {
-                    existing_proxies.set(endpoint.proxy() as usize, false);
+                    existing_proxies.set(proxy_id as usize, false);
-                    deleted += 1;
+
+                    if proxies[proxy_id].layer_depth <= layer_depth {
+                        num_subproper_proxies_deleted += 1;
+                    }
+
+                    num_proxies_deleted += 1;
                 }
             } else {
                 break;
             }
         }
 
-        deleted
+        (num_proxies_deleted, num_subproper_proxies_deleted)
     }
 
     pub fn delete_out_of_bounds_endpoints(&mut self, existing_proxies: &BitVec) {
@@ -149,26 +176,39 @@ impl SAPAxis {
             .retain(|endpt| endpt.is_sentinel() || existing_proxies[endpt.proxy() as usize])
     }
 
+    /// Removes from this axis all the endpoints corresponding to a proxy with an AABB mins/maxs values
+    /// equal to DELETED_AABB_VALUE, indicating that the endpoints should be deleted.
+    ///
+    /// Returns the number of deleted proxies such that `proxy.layer_depth <= layer_depth`.
     pub fn delete_deleted_proxies_and_endpoints_after_subregion_removal(
         &mut self,
         proxies: &SAPProxies,
         existing_proxies: &mut BitVec,
+        layer_depth: i8,
     ) -> usize {
-        let mut num_deleted = 0;
+        let mut num_subproper_proxies_deleted = 0;
 
         self.endpoints.retain(|endpt| {
-            if !endpt.is_sentinel() && proxies[endpt.proxy()].aabb.mins.x == DELETED_AABB_VALUE {
+            if !endpt.is_sentinel() {
-                if existing_proxies.get(endpt.proxy() as usize) == Some(true) {
+                let proxy = &proxies[endpt.proxy()];
-                    existing_proxies.set(endpt.proxy() as usize, false);
+
-                    num_deleted += 1;
+                if proxy.aabb.mins.x == DELETED_AABB_VALUE {
+                    if existing_proxies.get(endpt.proxy() as usize) == Some(true) {
+                        existing_proxies.set(endpt.proxy() as usize, false);
+
+                        if proxy.layer_depth <= layer_depth {
+                            num_subproper_proxies_deleted += 1;
+                        }
+                    }
+
+                    return false;
                 }
-                false
-            } else {
-                true
             }
+
+            true
         });
 
-        num_deleted
+        num_subproper_proxies_deleted
     }
 
     pub fn update_endpoints(
@@ -197,7 +237,8 @@ impl SAPAxis {
 
                     if endpoint_j.is_end() {
                         // Report start collision.
-                        if aabb_i.intersects(&proxies[endpoint_j.proxy()].aabb) {
+                        let proxy_j = &proxies[endpoint_j.proxy()];
+                        if aabb_i.intersects(&proxy_j.aabb) {
                             let pair = super::sort2(endpoint_i.proxy(), endpoint_j.proxy());
                             reporting.insert(pair, true);
                         }

src/geometry/broad_phase_multi_sap/sap_layer.rs

@@ -38,10 +38,18 @@ impl SAPLayer {
         }
     }
 
-    pub fn delete_all_region_endpoints(&mut self, proxies: &mut SAPProxies) {
+    pub fn unregister_all_subregions(&mut self, proxies: &mut SAPProxies) {
         for region_id in self.regions.values() {
             if let Some(mut region) = proxies[*region_id].data.take_region() {
                 region.delete_all_region_endpoints(proxies);
+
+                for subregion in region.subregions.drain(..) {
+                    proxies[subregion]
+                        .data
+                        .as_region_mut()
+                        .id_in_parent_subregion = crate::INVALID_U32;
+                }
+
                 proxies[*region_id].data.set_region(region);
             }
         }
@@ -77,14 +85,17 @@ impl SAPLayer {
         pool: &mut SAPRegionPool,
     ) {
         if let Some(proxy) = proxies.get(proxy_id) {
-            let region_key = super::point_key(proxy.aabb.center(), self.region_width);
+            if proxy.data.as_region().id_in_parent_subregion == crate::INVALID_U32 {
-            let region_id = self.ensure_region_exists(region_key, proxies, pool);
+                let region_key = super::point_key(proxy.aabb.center(), self.region_width);
-            let region = proxies[region_id].data.as_region_mut();
+                let region_id = self.ensure_region_exists(region_key, proxies, pool);
-            let id_in_parent_subregion = region.register_subregion(proxy_id);
+                let region = proxies[region_id].data.as_region_mut();
-            proxies[proxy_id]
+
-                .data
+                let id_in_parent_subregion = region.register_subregion(proxy_id);
-                .as_region_mut()
+                proxies[proxy_id]
-                .id_in_parent_subregion = id_in_parent_subregion as u32;
+                    .data
+                    .as_region_mut()
+                    .id_in_parent_subregion = id_in_parent_subregion as u32;
+            }
         }
     }
 
@@ -106,9 +117,10 @@ impl SAPLayer {
                     region.needs_update_after_subregion_removal = true;
                 }
 
-                region
+                let removed = region
                     .subregions
                     .swap_remove(id_in_parent_subregion as usize); // Remove the subregion index from the subregion list.
+                assert_eq!(removed, proxy_id);
 
                 // Re-adjust the id_in_parent_subregion of the subregion that was swapped in place
                 // of the deleted one.
@@ -137,7 +149,8 @@ impl SAPLayer {
             Entry::Vacant(vacant) => {
                 let region_bounds = super::region_aabb(region_key, self.region_width);
                 let region = SAPRegion::recycle_or_new(region_bounds, pool);
-                let region_proxy = SAPProxy::subregion(region, region_bounds, self.layer_id);
+                let region_proxy =
+                    SAPProxy::subregion(region, region_bounds, self.layer_id, self.depth);
                 let region_proxy_id = proxies.insert(region_proxy);
                 self.created_regions.push(region_proxy_id as u32);
                 let _ = vacant.insert(region_proxy_id);
@@ -172,7 +185,7 @@ impl SAPLayer {
                     let region_key = Point::new(i, j, _k);
                     let region_id = self.ensure_region_exists(region_key, proxies, pool);
                     let region = proxies[region_id].data.as_region_mut();
-                    region.preupdate_proxy(proxy_id);
+                    region.preupdate_proxy(proxy_id, true);
                 }
             }
         }
@@ -222,14 +235,14 @@ impl SAPLayer {
         for (point, region_id) in &self.regions {
             if let Some(mut region) = proxies[*region_id].data.take_region() {
                 // Update the region.
-                region.update(proxies, reporting);
+                region.update(proxies, self.depth, reporting);
 
                 // Mark all subregions as to-be-updated.
                 for subregion_id in &region.subregions {
                     proxies[*subregion_id].data.as_region_mut().mark_as_dirty();
                 }
 
-                if region.proxy_count == 0 {
+                if region.subproper_proxy_count == 0 {
                     self.regions_to_potentially_remove.push(*point);
                 }
 
@@ -257,10 +270,10 @@ impl SAPLayer {
                 if let Some(proxy) = proxies.get_mut(*region_id.get()) {
                     // First, we need to remove the endpoints of the deleted subregions.
                     let mut region = proxy.data.take_region().unwrap();
-                    region.update_after_subregion_removal(proxies);
+                    region.update_after_subregion_removal(proxies, self.depth);
 
-                    // Check if we actually can delete this region.
+                    // Check if we can actually delete this region.
-                    if region.proxy_count == 0 {
+                    if region.subproper_proxy_count == 0 {
                         let region_id = region_id.remove();
 
                         // We can delete this region. So we need to tell the larger

src/geometry/broad_phase_multi_sap/sap_proxy.rs

@@ -14,19 +14,19 @@ pub enum SAPProxyData {
 }
 
 impl SAPProxyData {
-    pub fn is_subregion(&self) -> bool {
+    pub fn is_region(&self) -> bool {
         match self {
             SAPProxyData::Region(_) => true,
             _ => false,
         }
     }
 
-    // pub fn as_region(&self) -> &SAPRegion {
+    pub fn as_region(&self) -> &SAPRegion {
-    //     match self {
+        match self {
-    //         SAPProxyData::Region(r) => r.as_ref().unwrap(),
+            SAPProxyData::Region(r) => r.as_ref().unwrap(),
-    //         _ => panic!("Invalid proxy type."),
+            _ => panic!("Invalid proxy type."),
-    //     }
+        }
-    // }
+    }
 
     pub fn as_region_mut(&mut self) -> &mut SAPRegion {
         match self {
@@ -53,25 +53,29 @@ pub struct SAPProxy {
     pub data: SAPProxyData,
     pub aabb: AABB,
     pub next_free: SAPProxyIndex,
+    // TODO: pack the layer_id and layer_depth into a single u16?
     pub layer_id: u8,
+    pub layer_depth: i8,
 }
 
 impl SAPProxy {
-    pub fn collider(handle: ColliderHandle, aabb: AABB, layer_id: u8) -> Self {
+    pub fn collider(handle: ColliderHandle, aabb: AABB, layer_id: u8, layer_depth: i8) -> Self {
         Self {
             data: SAPProxyData::Collider(handle),
             aabb,
             next_free: NEXT_FREE_SENTINEL,
             layer_id,
+            layer_depth,
         }
     }
 
-    pub fn subregion(subregion: Box<SAPRegion>, aabb: AABB, layer_id: u8) -> Self {
+    pub fn subregion(subregion: Box<SAPRegion>, aabb: AABB, layer_id: u8, layer_depth: i8) -> Self {
         Self {
             data: SAPProxyData::Region(Some(subregion)),
             aabb,
             next_free: NEXT_FREE_SENTINEL,
             layer_id,
+            layer_depth,
         }
     }
 }
@@ -92,7 +96,7 @@ impl SAPProxies {
     }
 
     pub fn insert(&mut self, proxy: SAPProxy) -> SAPProxyIndex {
-        if self.first_free != NEXT_FREE_SENTINEL {
+        let result = if self.first_free != NEXT_FREE_SENTINEL {
             let proxy_id = self.first_free;
             self.first_free = self.elements[proxy_id as usize].next_free;
             self.elements[proxy_id as usize] = proxy;
@@ -100,7 +104,9 @@ impl SAPProxies {
         } else {
             self.elements.push(proxy);
             self.elements.len() as u32 - 1
-        }
+        };
+
+        result
     }
 
     pub fn remove(&mut self, proxy_id: SAPProxyIndex) {

src/geometry/broad_phase_multi_sap/sap_region.rs

@@ -18,7 +18,10 @@ pub struct SAPRegion {
     pub id_in_parent_subregion: u32,
     pub update_count: u8,
     pub needs_update_after_subregion_removal: bool,
-    pub proxy_count: usize,
+    // Number of proxies (added to this region) that originates
+    // from the layer at depth <= the depth of the layer containing
+    // this region.
+    pub subproper_proxy_count: usize,
 }
 
 impl SAPRegion {
@@ -37,26 +40,27 @@ impl SAPRegion {
             id_in_parent_subregion: crate::INVALID_U32,
             update_count: 0,
             needs_update_after_subregion_removal: false,
-            proxy_count: 0,
+            subproper_proxy_count: 0,
         }
     }
 
     pub fn recycle(bounds: AABB, mut old: Box<Self>) -> Box<Self> {
         // Correct the bounds
-        for (axis, &bound) in old.axes.iter_mut().zip(bounds.mins.iter()) {
+        for i in 0..DIM {
-            axis.min_bound = bound;
+            // Make sure the axis is empty (it may still contain
-        }
+            // some old endpoints from non-proper proxies.
-        for (axis, &bound) in old.axes.iter_mut().zip(bounds.maxs.iter()) {
+            old.axes[i].clear();
-            axis.max_bound = bound;
+            old.axes[i].min_bound = bounds.mins[i];
+            old.axes[i].max_bound = bounds.maxs[i];
         }
 
         old.update_count = 0;
+        old.existing_proxies.clear();
+        old.id_in_parent_subregion = crate::INVALID_U32;
 
         // The rest of the fields should be "empty"
-        assert_eq!(old.proxy_count, 0);
+        assert_eq!(old.subproper_proxy_count, 0);
         assert!(old.to_insert.is_empty());
-        debug_assert!(!old.existing_proxies.any());
-        assert!(old.axes.iter().all(|ax| ax.endpoints.len() == 2));
 
         old
     }
@@ -75,7 +79,7 @@ impl SAPRegion {
             axis.endpoints.retain(|e| {
                 if let Some(proxy) = proxies.get(e.proxy()) {
                     existing_proxies.set(e.proxy() as usize, false);
-                    !proxy.data.is_subregion()
+                    !proxy.data.is_region()
                 } else {
                     true
                 }
@@ -97,11 +101,11 @@ impl SAPRegion {
     pub fn register_subregion(&mut self, proxy_id: SAPProxyIndex) -> usize {
         let subregion_index = self.subregions.len();
         self.subregions.push(proxy_id);
-        self.preupdate_proxy(proxy_id);
+        self.preupdate_proxy(proxy_id, true);
         subregion_index
     }
 
-    pub fn preupdate_proxy(&mut self, proxy_id: SAPProxyIndex) -> bool {
+    pub fn preupdate_proxy(&mut self, proxy_id: SAPProxyIndex, is_subproper_proxy: bool) -> bool {
         let mask_len = self.existing_proxies.len();
         if proxy_id as usize >= mask_len {
             self.existing_proxies
@@ -111,7 +115,11 @@ impl SAPRegion {
         if !self.existing_proxies[proxy_id as usize] {
             self.to_insert.push(proxy_id);
             self.existing_proxies.set(proxy_id as usize, true);
-            self.proxy_count += 1;
+
+            if is_subproper_proxy {
+                self.subproper_proxy_count += 1;
+            }
+
             false
         } else {
             // Here we need a second update if all proxies exit this region. In this case, we need
@@ -122,31 +130,41 @@ impl SAPRegion {
         }
     }
 
-    pub fn update_after_subregion_removal(&mut self, proxies: &SAPProxies) {
+    pub fn update_after_subregion_removal(&mut self, proxies: &SAPProxies, layer_depth: i8) {
         if self.needs_update_after_subregion_removal {
             for axis in &mut self.axes {
-                self.proxy_count -= axis
+                self.subproper_proxy_count -= axis
                     .delete_deleted_proxies_and_endpoints_after_subregion_removal(
                         proxies,
                         &mut self.existing_proxies,
+                        layer_depth,
                     );
             }
             self.needs_update_after_subregion_removal = false;
         }
     }
 
-    pub fn update(&mut self, proxies: &SAPProxies, reporting: &mut HashMap<(u32, u32), bool>) {
+    pub fn update(
+        &mut self,
+        proxies: &SAPProxies,
+        layer_depth: i8,
+        reporting: &mut HashMap<(u32, u32), bool>,
+    ) {
         if self.update_count > 0 {
             // Update endpoints.
-            let mut deleted = 0;
+            let mut total_deleted = 0;
+            let mut total_deleted_subproper = 0;
 
             for dim in 0..DIM {
                 self.axes[dim].update_endpoints(dim, proxies, reporting);
-                deleted += self.axes[dim].delete_out_of_bounds_proxies(&mut self.existing_proxies);
+                let (num_deleted, num_deleted_subproper) = self.axes[dim]
+                    .delete_out_of_bounds_proxies(proxies, &mut self.existing_proxies, layer_depth);
+                total_deleted += num_deleted;
+                total_deleted_subproper += num_deleted_subproper;
             }
 
-            if deleted > 0 {
+            if total_deleted > 0 {
-                self.proxy_count -= deleted;
+                self.subproper_proxy_count -= total_deleted_subproper;
                 for dim in 0..DIM {
                     self.axes[dim].delete_out_of_bounds_endpoints(&self.existing_proxies);
                 }

src/geometry/broad_phase_multi_sap/sap_utils.rs

@@ -5,6 +5,7 @@ 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 DELETED_AABB_VALUE: Real = SENTINEL_VALUE / 2.0;
+pub(crate) const MAX_AABB_EXTENT: Real = SENTINEL_VALUE / 4.0;
 pub(crate) const REGION_WIDTH_BASE: Real = 1.0;
 pub(crate) const REGION_WIDTH_POWER_BASIS: Real = 5.0;
 
@@ -18,6 +19,10 @@ pub(crate) fn sort2(a: u32, b: u32) -> (u32, u32) {
     }
 }
 
+pub(crate) fn clamp_point(point: Point<Real>) -> Point<Real> {
+    point.map(|e| na::clamp(e, -MAX_AABB_EXTENT, MAX_AABB_EXTENT))
+}
+
 pub(crate) fn point_key(point: Point<Real>, region_width: Real) -> Point<i32> {
     (point / region_width)
         .coords
@@ -32,7 +37,7 @@ pub(crate) fn region_aabb(index: Point<i32>, region_width: Real) -> AABB {
 }
 
 pub(crate) fn region_width(depth: i8) -> Real {
-    REGION_WIDTH_BASE * REGION_WIDTH_POWER_BASIS.powi(depth as i32)
+    (REGION_WIDTH_BASE * REGION_WIDTH_POWER_BASIS.powi(depth as i32)).min(MAX_AABB_EXTENT)
 }
 
 /// Computes the depth of the layer the given AABB should be part of.
@@ -49,8 +54,9 @@ pub(crate) fn layer_containing_aabb(aabb: &AABB) -> i8 {
     const NUM_ELEMENTS_PER_DIMENSION: Real = 10.0;
 
     let width = 2.0 * aabb.half_extents().norm() * NUM_ELEMENTS_PER_DIMENSION;
-    // TODO: handle overflows in the f32 -> i8 conversion.
     (width / REGION_WIDTH_BASE)
         .log(REGION_WIDTH_POWER_BASIS)
-        .round() as i8
+        .round()
+        .max(i8::MIN as Real)
+        .min(i8::MAX as Real) as i8
 }