Allow the PhysicsPipeline and CollisionPipeline to update the QueryPipeline incrementally

1 files changed, 88 insertions, 0 deletions

diff --git a/src/geometry/broad_phase_qbvh.rs b/src/geometry/broad_phase_qbvh.rs
new file mode 100644
index 0000000..22ca562
--- /dev/null
+++ b/src/geometry/broad_phase_qbvh.rs
@@ -0,0 +1,88 @@
+use crate::geometry::{BroadPhasePairEvent, ColliderHandle, ColliderPair, ColliderSet};
+use parry::bounding_volume::BoundingVolume;
+use parry::math::Real;
+use parry::partitioning::Qbvh;
+use parry::partitioning::QbvhUpdateWorkspace;
+use parry::query::visitors::BoundingVolumeIntersectionsSimultaneousVisitor;
+
+#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
+#[derive(Clone)]
+pub struct BroadPhase {
+    qbvh: Qbvh<ColliderHandle>,
+    stack: Vec<(u32, u32)>,
+    #[cfg_attr(feature = "serde-serialize", serde(skip))]
+    workspace: QbvhUpdateWorkspace,
+}
+
+impl Default for BroadPhase {
+    fn default() -> Self {
+        Self::new()
+    }
+}
+
+impl BroadPhase {
+    pub fn new() -> Self {
+        Self {
+            qbvh: Qbvh::new(),
+            stack: vec![],
+            workspace: QbvhUpdateWorkspace::default(),
+        }
+    }
+
+    #[allow(dead_code)] // This broad-phase is just experimental right now.
+    pub fn update(
+        &mut self,
+        prediction_distance: Real,
+        colliders: &ColliderSet,
+        modified_colliders: &[ColliderHandle],
+        removed_colliders: &[ColliderHandle],
+        events: &mut Vec<BroadPhasePairEvent>,
+    ) {
+        let margin = 0.01;
+
+        if modified_colliders.is_empty() {
+            return;
+        }
+
+        // Visitor to find collision pairs.
+        let mut visitor = BoundingVolumeIntersectionsSimultaneousVisitor::new(
+            |co1: &ColliderHandle, co2: &ColliderHandle| {
+                events.push(BroadPhasePairEvent::AddPair(ColliderPair::new(*co1, *co2)));
+                true
+            },
+        );
+
+        let full_rebuild = self.qbvh.raw_nodes().is_empty();
+
+        if full_rebuild {
+            self.qbvh.clear_and_rebuild(
+                colliders.iter().map(|(handle, collider)| {
+                    (
+                        handle,
+                        collider.compute_aabb().loosened(prediction_distance / 2.0),
+                    )
+                }),
+                margin,
+            );
+            self.qbvh
+                .traverse_bvtt_with_stack(&self.qbvh, &mut visitor, &mut self.stack);
+        } else {
+            for modified in modified_colliders {
+                self.qbvh.pre_update_or_insert(*modified);
+            }
+
+            for removed in removed_colliders {
+                self.qbvh.remove(*removed);
+            }
+
+            let _ = self.qbvh.refit(margin, &mut self.workspace, |handle| {
+                colliders[*handle]
+                    .compute_aabb()
+                    .loosened(prediction_distance / 2.0)
+            });
+            self.qbvh
+                .traverse_modified_bvtt_with_stack(&self.qbvh, &mut visitor, &mut self.stack);
+            self.qbvh.rebalance(margin, &mut self.workspace);
+        }
+    }
+}