WQuadtree: fix stack overflow caused by more than 4 AABB with the same center.

2 files changed, 48 insertions, 17 deletions

diff --git a/examples3d/debug_boxes3.rs b/examples3d/debug_boxes3.rs
index 7237fd9..919cdd6 100644
--- a/examples3d/debug_boxes3.rs
+++ b/examples3d/debug_boxes3.rs
@@ -17,22 +17,26 @@ pub fn init_world(testbed: &mut Testbed) {
     let ground_size = 100.1;
     let ground_height = 0.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);
+    for _ in 0..6 {
+        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);
+    }
 
     // Build the dynamic box rigid body.
-    let rigid_body = RigidBodyBuilder::new_dynamic()
-        .translation(1.1, 0.0, 0.0)
-        .rotation(Vector3::new(0.8, 0.2, 0.1))
-        .can_sleep(false)
-        .build();
-    let handle = bodies.insert(rigid_body);
-    let collider = ColliderBuilder::cuboid(2.0, 0.1, 1.0).build();
-    colliders.insert(collider, handle, &mut bodies);
+    for _ in 0..6 {
+        let rigid_body = RigidBodyBuilder::new_dynamic()
+            .translation(1.1, 0.0, 0.0)
+            .rotation(Vector3::new(0.8, 0.2, 0.1))
+            .can_sleep(false)
+            .build();
+        let handle = bodies.insert(rigid_body);
+        let collider = ColliderBuilder::cuboid(2.0, 0.1, 1.0).build();
+        colliders.insert(collider, handle, &mut bodies);
+    }
 
     /*
      * Set up the testbed.
diff --git a/src/geometry/wquadtree.rs b/src/geometry/wquadtree.rs
index fce04eb..deab4a2 100644
--- a/src/geometry/wquadtree.rs
+++ b/src/geometry/wquadtree.rs
@@ -539,7 +539,7 @@ fn split_indices_wrt_dim<'a>(
     dim: usize,
 ) -> (&'a mut [usize], &'a mut [usize]) {
     let mut icurr = 0;
-    let mut ilast = indices.len() - 1;
+    let mut ilast = indices.len();
 
     // The loop condition we can just do 0..indices.len()
     // instead of the test icurr < ilast because we know
@@ -549,12 +549,39 @@ fn split_indices_wrt_dim<'a>(
         let center = aabbs[i].center();
 
         if center[dim] > split_point[dim] {
-            indices.swap(icurr, ilast);
             ilast -= 1;
+            indices.swap(icurr, ilast);
         } else {
             icurr += 1;
         }
     }
 
-    indices.split_at_mut(icurr)
+    if icurr == 0 || icurr == indices.len() {
+        // We don't want to return one empty set. But
+        // this can happen if all the coordinates along the
+        // given dimension are equal.
+        // In this is the case, we just split in the middle.
+        let half = indices.len() / 2;
+        indices.split_at_mut(half)
+    } else {
+        indices.split_at_mut(icurr)
+    }
+}
+
+#[cfg(test)]
+mod test {
+    use crate::geometry::{WQuadtree, AABB};
+    use crate::math::{Point, Vector};
+
+    #[test]
+    fn multiple_identical_AABB_stack_overflow() {
+        // A stack overflow was caused during the construction of the
+        // WAABB tree with more than four AABB with the same center.
+        let aabb = AABB::new(Point::origin(), Vector::repeat(1.0).into());
+
+        for k in 0..20 {
+            let mut tree = WQuadtree::new();
+            tree.clear_and_rebuild((0..k).map(|i| (i, aabb)), 0.0);
+        }
+    }
 }