From 97145f2214f2b0f17006388b77b986c97994f72e Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?S=C3=A9bastien=20Crozet?= <sebcrozet@dimforge.com>
Date: Sat, 23 Mar 2024 21:13:18 +0100
Subject: [PATCH] fix: force a complete QBVH rebuild when it gets too
 imbalanced.

Fix https://github.com/dimforge/rapier/issues/466
Fix https://github.com/dimforge/parry/issues/146
Fix https://github.com/dimforge/parry/pull/169
Fix https://github.com/dimforge/rapier/issues/532
Fix https://github.com/dimforge/rapier/issues/558
Fix https://github.com/dimforge/rapier/issues/450
---
 src/partitioning/qbvh/update.rs | 23 +++++++++++++++++++++++
 1 file changed, 23 insertions(+)

diff --git a/src/partitioning/qbvh/update.rs b/src/partitioning/qbvh/update.rs
index 7c948432..e7fa8ac1 100644
--- a/src/partitioning/qbvh/update.rs
+++ b/src/partitioning/qbvh/update.rs
@@ -339,6 +339,11 @@ impl<LeafData: IndexedData> Qbvh<LeafData> {
 
         const MIN_CHANGED_DEPTH: u8 = 5; // TODO: find a good value
 
+        // PERF: if we have modifications past this depth, the QBVH has become very
+        //       unbalanced and a full rebuild is warranted.
+        // TODO: work on a way to reduce the risks of imbalance.
+        const FULL_REBUILD_DEPTH: u8 = 15;
+
         for root_child in self.nodes[0].children {
             if root_child < self.nodes.len() as u32 {
                 workspace.stack.push((root_child, 1));
@@ -346,7 +351,13 @@ impl<LeafData: IndexedData> Qbvh<LeafData> {
         }
 
         // Collect empty slots and pseudo-leaves to sort.
+        let mut force_full_rebuild = false;
         while let Some((id, depth)) = workspace.stack.pop() {
+            if depth > FULL_REBUILD_DEPTH {
+                force_full_rebuild = true;
+                break;
+            }
+
             let node = &mut self.nodes[id as usize];
 
             if node.is_leaf() {
@@ -374,6 +385,18 @@ impl<LeafData: IndexedData> Qbvh<LeafData> {
             }
         }
 
+        if force_full_rebuild {
+            workspace.clear();
+            let all_leaves: Vec<_> = self
+                .proxies
+                .iter()
+                .filter_map(|proxy| self.node_aabb(proxy.node).map(|aabb| (proxy.data, aabb)))
+                .collect();
+
+            self.clear_and_rebuild(all_leaves.into_iter(), 0.0);
+            return;
+        }
+
         workspace.to_sort.extend(0..workspace.orig_ids.len());
         let root_id = NodeIndex::new(0, 0);