Merge pull request #427 from dimforge/disable

Add enable/disable, incremental query pipeline, and vehicle character contoller

7 files changed, 199 insertions, 6 deletions

diff --git a/src/geometry/broad_phase_multi_sap/broad_phase.rs b/src/geometry/broad_phase_multi_sap/broad_phase.rs
index 915017d..0667184 100644
--- a/src/geometry/broad_phase_multi_sap/broad_phase.rs
+++ b/src/geometry/broad_phase_multi_sap/broad_phase.rs
@@ -176,7 +176,11 @@ impl BroadPhase {
     /// This method will actually remove from the proxy list all the proxies
     /// marked as deletable by `self.predelete_proxy`, making their proxy
     /// handles re-usable by new proxies.
-    fn complete_removals(&mut self, removed_colliders: &[ColliderHandle]) {
+    fn complete_removals(
+        &mut self,
+        colliders: &mut ColliderSet,
+        removed_colliders: &[ColliderHandle],
+    ) {
         // If there is no layer, there is nothing to remove.
         if self.layers.is_empty() {
             return;
@@ -224,6 +228,11 @@ impl BroadPhase {
                     self.proxies.remove(proxy_id);
                 }
             }
+
+            if let Some(co) = colliders.get_mut_internal(*removed) {
+                // Reset the proxy index.
+                co.bf_data.proxy_index = crate::INVALID_U32;
+            }
         }
     }
 
@@ -460,9 +469,10 @@ impl BroadPhase {
             // NOTE: we use `get` because the collider may no longer
             //       exist if it has been removed.
             if let Some(co) = colliders.get_mut_internal(*handle) {
-                if !co.changes.needs_broad_phase_update() {
+                if !co.is_enabled() || !co.changes.needs_broad_phase_update() {
                     continue;
                 }
+
                 let mut new_proxy_id = co.bf_data.proxy_index;
 
                 if self.handle_modified_collider(
@@ -496,7 +506,7 @@ impl BroadPhase {
 
         // Phase 5: bottom-up pass to remove proxies, and propagate region removed from smaller
         // layers to possible remove regions from larger layers that would become empty that way.
-        self.complete_removals(removed_colliders);
+        self.complete_removals(colliders, removed_colliders);
     }
 
     /// Propagate regions from the smallest layers up to the larger layers.
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);
+        }
+    }
+}
diff --git a/src/geometry/collider.rs b/src/geometry/collider.rs
index 95ae273..6759b81 100644
--- a/src/geometry/collider.rs
+++ b/src/geometry/collider.rs
@@ -7,6 +7,7 @@ use crate::geometry::{
 use crate::math::{AngVector, Isometry, Point, Real, Rotation, Vector, DIM};
 use crate::parry::transformation::vhacd::VHACDParameters;
 use crate::pipeline::{ActiveEvents, ActiveHooks};
+use crate::prelude::ColliderEnabled;
 use na::Unit;
 use parry::bounding_volume::Aabb;
 use parry::shape::{Shape, TriMeshFlags};
@@ -154,6 +155,32 @@ impl Collider {
         }
     }
 
+    /// Is this collider enabled?
+    pub fn is_enabled(&self) -> bool {
+        match self.flags.enabled {
+            ColliderEnabled::Enabled => true,
+            _ => false,
+        }
+    }
+
+    /// Sets whether or not this collider is enabled.
+    pub fn set_enabled(&mut self, enabled: bool) {
+        match self.flags.enabled {
+            ColliderEnabled::Enabled | ColliderEnabled::DisabledByParent => {
+                if !enabled {
+                    self.changes.insert(ColliderChanges::ENABLED_OR_DISABLED);
+                    self.flags.enabled = ColliderEnabled::Disabled;
+                }
+            }
+            ColliderEnabled::Disabled => {
+                if enabled {
+                    self.changes.insert(ColliderChanges::ENABLED_OR_DISABLED);
+                    self.flags.enabled = ColliderEnabled::Enabled;
+                }
+            }
+        }
+    }
+
     /// Sets the translational part of this collider's position.
     pub fn set_translation(&mut self, translation: Vector<Real>) {
         self.changes.insert(ColliderChanges::POSITION);
@@ -402,6 +429,8 @@ pub struct ColliderBuilder {
     pub collision_groups: InteractionGroups,
     /// The solver groups for the collider being built.
     pub solver_groups: InteractionGroups,
+    /// Will the collider being built be enabled?
+    pub enabled: bool,
     /// The total force magnitude beyond which a contact force event can be emitted.
     pub contact_force_event_threshold: Real,
 }
@@ -424,6 +453,7 @@ impl ColliderBuilder {
             active_collision_types: ActiveCollisionTypes::default(),
             active_hooks: ActiveHooks::empty(),
             active_events: ActiveEvents::empty(),
+            enabled: true,
             contact_force_event_threshold: 0.0,
         }
     }
@@ -834,6 +864,12 @@ impl ColliderBuilder {
         self
     }
 
+    /// Enable or disable the collider after its creation.
+    pub fn enabled(mut self, enabled: bool) -> Self {
+        self.enabled = enabled;
+        self
+    }
+
     /// Builds a new collider attached to the given rigid-body.
     pub fn build(&self) -> Collider {
         let shape = self.shape.clone();
@@ -849,6 +885,11 @@ impl ColliderBuilder {
             active_collision_types: self.active_collision_types,
             active_hooks: self.active_hooks,
             active_events: self.active_events,
+            enabled: if self.enabled {
+                ColliderEnabled::Enabled
+            } else {
+                ColliderEnabled::Disabled
+            },
         };
         let changes = ColliderChanges::all();
         let pos = ColliderPosition(self.position);
diff --git a/src/geometry/collider_components.rs b/src/geometry/collider_components.rs
index 29a24b5..0e65bac 100644
--- a/src/geometry/collider_components.rs
+++ b/src/geometry/collider_components.rs
@@ -64,6 +64,8 @@ bitflags::bitflags! {
         /// This flags is automatically set by the `PhysicsPipeline` when the `RigidBodyChanges::DOMINANCE`
         /// or `RigidBodyChanges::TYPE` of the parent rigid-body of this collider is detected.
         const PARENT_EFFECTIVE_DOMINANCE = 1 << 7; // NF update.
+        /// Flag indicating that whether or not the collider is enabled was changed.
+        const ENABLED_OR_DISABLED = 1 << 8; // BF & NF updates.
     }
 }
 
@@ -374,6 +376,19 @@ impl Default for ActiveCollisionTypes {
 
 #[derive(Copy, Clone, Debug, PartialEq, Eq, Hash)]
 #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
+/// Enum indicating whether or not a collider is enabled.
+pub enum ColliderEnabled {
+    /// The collider is enabled.
+    Enabled,
+    /// The collider wasn’t disabled by the user explicitly but it is attached to
+    /// a disabled rigid-body.
+    DisabledByParent,
+    /// The collider is disabled by the user explicitly.
+    Disabled,
+}
+
+#[derive(Copy, Clone, Debug, PartialEq, Eq, Hash)]
+#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
 /// A set of flags for controlling collision/intersection filtering, modification, and events.
 pub struct ColliderFlags {
     /// Controls whether collision-detection happens between two colliders depending on
@@ -389,6 +404,8 @@ pub struct ColliderFlags {
     pub active_hooks: ActiveHooks,
     /// The events enabled for this collider.
     pub active_events: ActiveEvents,
+    /// Whether or not the collider is enabled.
+    pub enabled: ColliderEnabled,
 }
 
 impl Default for ColliderFlags {
@@ -399,6 +416,7 @@ impl Default for ColliderFlags {
             solver_groups: InteractionGroups::all(),
             active_hooks: ActiveHooks::empty(),
             active_events: ActiveEvents::empty(),
+            enabled: ColliderEnabled::Enabled,
         }
     }
 }
diff --git a/src/geometry/collider_set.rs b/src/geometry/collider_set.rs
index b141445..4bc2a03 100644
--- a/src/geometry/collider_set.rs
+++ b/src/geometry/collider_set.rs
@@ -41,6 +41,14 @@ impl ColliderSet {
         self.colliders.iter().map(|(h, c)| (ColliderHandle(h), c))
     }
 
+    /// Iterate through all the enabled colliders on this set.
+    pub fn iter_enabled(&self) -> impl Iterator<Item = (ColliderHandle, &Collider)> {
+        self.colliders
+            .iter()
+            .map(|(h, c)| (ColliderHandle(h), c))
+            .filter(|(_, c)| c.is_enabled())
+    }
+
     /// Iterates mutably through all the colliders on this set.
     #[cfg(not(feature = "dev-remove-slow-accessors"))]
     pub fn iter_mut(&mut self) -> impl Iterator<Item = (ColliderHandle, &mut Collider)> {
@@ -52,6 +60,12 @@ impl ColliderSet {
         })
     }
 
+    /// Iterates mutably through all the enabled colliders on this set.
+    #[cfg(not(feature = "dev-remove-slow-accessors"))]
+    pub fn iter_enabled_mut(&mut self) -> impl Iterator<Item = (ColliderHandle, &mut Collider)> {
+        self.iter_mut().filter(|(_, c)| c.is_enabled())
+    }
+
     /// The number of colliders on this set.
     pub fn len(&self) -> usize {
         self.colliders.len()
@@ -268,6 +282,18 @@ impl ColliderSet {
     pub(crate) fn get_mut_internal(&mut self, handle: ColliderHandle) -> Option<&mut Collider> {
         self.colliders.get_mut(handle.0)
     }
+
+    // Just a very long name instead of `.get_mut` to make sure
+    // this is really the method we wanted to use instead of `get_mut_internal`.
+    #[allow(dead_code)]
+    pub(crate) fn get_mut_internal_with_modification_tracking(
+        &mut self,
+        handle: ColliderHandle,
+    ) -> Option<&mut Collider> {
+        let result = self.colliders.get_mut(handle.0)?;
+        Self::mark_as_modified(handle, result, &mut self.modified_colliders);
+        Some(result)
+    }
 }
 
 impl Index<crate::data::Index> for ColliderSet {
diff --git a/src/geometry/mod.rs b/src/geometry/mod.rs
index 4f3ada8..a91c4b9 100644
--- a/src/geometry/mod.rs
+++ b/src/geometry/mod.rs
@@ -1,6 +1,9 @@
 //! Structures related to geometry: colliders, shapes, etc.
 
-pub use self::broad_phase_multi_sap::{BroadPhase, BroadPhasePairEvent, ColliderPair};
+pub use self::broad_phase_multi_sap::{BroadPhasePairEvent, ColliderPair};
+
+pub use self::broad_phase_multi_sap::BroadPhase;
+// pub use self::broad_phase_qbvh::BroadPhase;
 pub use self::collider_components::*;
 pub use self::contact_pair::{
     ContactData, ContactManifoldData, ContactPair, IntersectionPair, SolverContact, SolverFlags,
@@ -199,5 +202,6 @@ mod interaction_graph;
 mod interaction_groups;
 mod narrow_phase;
 
+mod broad_phase_qbvh;
 mod collider;
 mod collider_set;
diff --git a/src/geometry/narrow_phase.rs b/src/geometry/narrow_phase.rs
index ecd1623..dacca9e 100644
--- a/src/geometry/narrow_phase.rs
+++ b/src/geometry/narrow_phase.rs
@@ -299,7 +299,13 @@ impl NarrowPhase {
             }
         }
 
-        self.handle_modified_colliders(islands, modified_colliders, colliders, bodies, events);
+        self.handle_user_changes_on_colliders(
+            islands,
+            modified_colliders,
+            colliders,
+            bodies,
+            events,
+        );
     }
 
     pub(crate) fn remove_collider(
@@ -393,7 +399,7 @@ impl NarrowPhase {
         }
     }
 
-    pub(crate) fn handle_modified_colliders(
+    pub(crate) fn handle_user_changes_on_colliders(
         &mut self,
         mut islands: Option<&mut IslandManager>,
         modified_colliders: &[ColliderHandle],