Merge pull request #427 from dimforge/disable

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

9 files changed, 163 insertions, 26 deletions

diff --git a/src/dynamics/ccd/ccd_solver.rs b/src/dynamics/ccd/ccd_solver.rs
index 9e0ab8e..9b06a80 100644
--- a/src/dynamics/ccd/ccd_solver.rs
+++ b/src/dynamics/ccd/ccd_solver.rs
@@ -123,7 +123,6 @@ impl CCDSolver {
     ) -> Option<Real> {
         // Update the query pipeline.
         self.query_pipeline.update_with_mode(
-            islands,
             bodies,
             colliders,
             QueryPipelineMode::SweepTestWithPredictedPosition { dt },
@@ -245,7 +244,6 @@ impl CCDSolver {
 
         // Update the query pipeline.
         self.query_pipeline.update_with_mode(
-            islands,
             bodies,
             colliders,
             QueryPipelineMode::SweepTestWithNextPosition,
diff --git a/src/dynamics/island_manager.rs b/src/dynamics/island_manager.rs
index 1295f14..e196503 100644
--- a/src/dynamics/island_manager.rs
+++ b/src/dynamics/island_manager.rs
@@ -1,6 +1,6 @@
 use crate::dynamics::{
-    ImpulseJointSet, MultibodyJointSet, RigidBodyActivation, RigidBodyColliders, RigidBodyHandle,
-    RigidBodyIds, RigidBodySet, RigidBodyType, RigidBodyVelocity,
+    ImpulseJointSet, MultibodyJointSet, RigidBodyActivation, RigidBodyChanges, RigidBodyColliders,
+    RigidBodyHandle, RigidBodyIds, RigidBodySet, RigidBodyType, RigidBodyVelocity,
 };
 use crate::geometry::{ColliderSet, NarrowPhase};
 use crate::math::Real;
@@ -96,11 +96,18 @@ impl IslandManager {
         //       attempting to wake-up a rigid-body that has already been deleted.
         if bodies.get(handle).map(|rb| rb.body_type()) == Some(RigidBodyType::Dynamic) {
             let rb = bodies.index_mut_internal(handle);
-            rb.activation.wake_up(strong);
 
-            if self.active_dynamic_set.get(rb.ids.active_set_id) != Some(&handle) {
-                rb.ids.active_set_id = self.active_dynamic_set.len();
-                self.active_dynamic_set.push(handle);
+            // Check that the user didn’t change the sleeping state explicitly, in which
+            // case we don’t overwrite it.
+            if !rb.changes.contains(RigidBodyChanges::SLEEP) {
+                rb.activation.wake_up(strong);
+
+                if rb.is_enabled()
+                    && self.active_dynamic_set.get(rb.ids.active_set_id) != Some(&handle)
+                {
+                    rb.ids.active_set_id = self.active_dynamic_set.len();
+                    self.active_dynamic_set.push(handle);
+                }
             }
         }
     }
@@ -256,12 +263,12 @@ impl IslandManager {
             // in contact or joined with this collider.
             push_contacting_bodies(&rb.colliders, colliders, narrow_phase, &mut self.stack);
 
-            for inter in impulse_joints.attached_joints(handle) {
+            for inter in impulse_joints.attached_enabled_joints(handle) {
                 let other = crate::utils::select_other((inter.0, inter.1), handle);
                 self.stack.push(other);
             }
 
-            for other in multibody_joints.attached_bodies(handle) {
+            for other in multibody_joints.bodies_attached_with_enabled_joint(handle) {
                 self.stack.push(other);
             }
 
diff --git a/src/dynamics/joint/generic_joint.rs b/src/dynamics/joint/generic_joint.rs
index bb1598d..cbd9649 100644
--- a/src/dynamics/joint/generic_joint.rs
+++ b/src/dynamics/joint/generic_joint.rs
@@ -182,6 +182,19 @@ impl JointMotor {
     }
 }
 
+#[derive(Copy, Clone, Debug, PartialEq, Eq, Hash)]
+#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
+/// Enum indicating whether or not a joint is enabled.
+pub enum JointEnabled {
+    /// The joint is enabled.
+    Enabled,
+    /// The joint wasn’t disabled by the user explicitly but it is attached to
+    /// a disabled rigid-body.
+    DisabledByAttachedBody,
+    /// The joint is disabled by the user explicitly.
+    Disabled,
+}
+
 #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
 #[derive(Copy, Clone, Debug, PartialEq)]
 /// A generic joint.
@@ -208,6 +221,8 @@ pub struct GenericJoint {
     pub motors: [JointMotor; SPATIAL_DIM],
     /// Are contacts between the attached rigid-bodies enabled?
     pub contacts_enabled: bool,
+    /// Whether or not the joint is enabled.
+    pub enabled: JointEnabled,
 }
 
 impl Default for GenericJoint {
@@ -222,6 +237,7 @@ impl Default for GenericJoint {
             limits: [JointLimits::default(); SPATIAL_DIM],
             motors: [JointMotor::default(); SPATIAL_DIM],
             contacts_enabled: true,
+            enabled: JointEnabled::Enabled,
         }
     }
 }
@@ -260,6 +276,27 @@ impl GenericJoint {
         }
     }
 
+    /// Is this joint enabled?
+    pub fn is_enabled(&self) -> bool {
+        self.enabled == JointEnabled::Enabled
+    }
+
+    /// Set whether this joint is enabled or not.
+    pub fn set_enabled(&mut self, enabled: bool) {
+        match self.enabled {
+            JointEnabled::Enabled | JointEnabled::DisabledByAttachedBody => {
+                if !enabled {
+                    self.enabled = JointEnabled::Disabled;
+                }
+            }
+            JointEnabled::Disabled => {
+                if enabled {
+                    self.enabled = JointEnabled::Enabled;
+                }
+            }
+        }
+    }
+
     /// Add the specified axes to the set of axes locked by this joint.
     pub fn lock_axes(&mut self, axes: JointAxesMask) -> &mut Self {
         self.locked_axes |= axes;
diff --git a/src/dynamics/joint/impulse_joint/impulse_joint_set.rs b/src/dynamics/joint/impulse_joint/impulse_joint_set.rs
index 7409172..f21f950 100644
--- a/src/dynamics/joint/impulse_joint/impulse_joint_set.rs
+++ b/src/dynamics/joint/impulse_joint/impulse_joint_set.rs
@@ -72,11 +72,11 @@ impl ImpulseJointSet {
     }
 
     /// Iterates through all the joints between two rigid-bodies.
-    pub fn joints_between<'a>(
-        &'a self,
+    pub fn joints_between(
+        &self,
         body1: RigidBodyHandle,
         body2: RigidBodyHandle,
-    ) -> impl Iterator<Item = (ImpulseJointHandle, &'a ImpulseJoint)> {
+    ) -> impl Iterator<Item = (ImpulseJointHandle, &ImpulseJoint)> {
         self.rb_graph_ids
             .get(body1.0)
             .zip(self.rb_graph_ids.get(body2.0))
@@ -86,15 +86,15 @@ impl ImpulseJointSet {
     }
 
     /// Iterates through all the impulse joints attached to the given rigid-body.
-    pub fn attached_joints<'a>(
-        &'a self,
+    pub fn attached_joints(
+        &self,
         body: RigidBodyHandle,
     ) -> impl Iterator<
         Item = (
             RigidBodyHandle,
             RigidBodyHandle,
             ImpulseJointHandle,
-            &'a ImpulseJoint,
+            &ImpulseJoint,
         ),
     > {
         self.rb_graph_ids
@@ -104,6 +104,35 @@ impl ImpulseJointSet {
             .map(|inter| (inter.0, inter.1, inter.2.handle, inter.2))
     }
 
+    /// Iterates through all the impulse joints attached to the given rigid-body.
+    pub fn map_attached_joints_mut<'a>(
+        &'a mut self,
+        body: RigidBodyHandle,
+        mut f: impl FnMut(RigidBodyHandle, RigidBodyHandle, ImpulseJointHandle, &mut ImpulseJoint),
+    ) {
+        self.rb_graph_ids.get(body.0).into_iter().for_each(|id| {
+            for inter in self.joint_graph.interactions_with_mut(*id) {
+                (f)(inter.0, inter.1, inter.3.handle, inter.3)
+            }
+        })
+    }
+
+    /// Iterates through all the enabled impulse joints attached to the given rigid-body.
+    pub fn attached_enabled_joints(
+        &self,
+        body: RigidBodyHandle,
+    ) -> impl Iterator<
+        Item = (
+            RigidBodyHandle,
+            RigidBodyHandle,
+            ImpulseJointHandle,
+            &ImpulseJoint,
+        ),
+    > {
+        self.attached_joints(body)
+            .filter(|inter| inter.3.data.is_enabled())
+    }
+
     /// Is the given joint handle valid?
     pub fn contains(&self, handle: ImpulseJointHandle) -> bool {
         self.joint_ids.contains(handle.0)
@@ -246,7 +275,7 @@ impl ImpulseJointSet {
         ImpulseJointHandle(handle)
     }
 
-    /// Retrieve all the impulse_joints happening between two active bodies.
+    /// Retrieve all the enabled impulse joints happening between two active bodies.
     // NOTE: this is very similar to the code from NarrowPhase::select_active_interactions.
     pub(crate) fn select_active_interactions(
         &self,
@@ -264,7 +293,8 @@ impl ImpulseJointSet {
             let rb1 = &bodies[joint.body1];
             let rb2 = &bodies[joint.body2];
 
-            if (rb1.is_dynamic() || rb2.is_dynamic())
+            if joint.data.is_enabled()
+                && (rb1.is_dynamic() || rb2.is_dynamic())
                 && (!rb1.is_dynamic() || !rb1.is_sleeping())
                 && (!rb2.is_dynamic() || !rb2.is_sleeping())
             {
diff --git a/src/dynamics/joint/multibody_joint/multibody_joint_set.rs b/src/dynamics/joint/multibody_joint/multibody_joint_set.rs
index c25de73..a4b338a 100644
--- a/src/dynamics/joint/multibody_joint/multibody_joint_set.rs
+++ b/src/dynamics/joint/multibody_joint/multibody_joint_set.rs
@@ -372,7 +372,7 @@ impl MultibodyJointSet {
     }
 
     /// Iterate through the handles of all the rigid-bodies attached to this rigid-body
-    /// by an multibody_joint.
+    /// by a multibody_joint.
     pub fn attached_bodies<'a>(
         &'a self,
         body: RigidBodyHandle,
@@ -384,6 +384,21 @@ impl MultibodyJointSet {
             .map(move |inter| crate::utils::select_other((inter.0, inter.1), body))
     }
 
+    /// Iterate through the handles of all the rigid-bodies attached to this rigid-body
+    /// by an enabled multibody_joint.
+    pub fn bodies_attached_with_enabled_joint<'a>(
+        &'a self,
+        body: RigidBodyHandle,
+    ) -> impl Iterator<Item = RigidBodyHandle> + 'a {
+        self.attached_bodies(body).filter(move |other| {
+            if let Some((_, _, link)) = self.joint_between(body, *other) {
+                link.joint.data.is_enabled()
+            } else {
+                false
+            }
+        })
+    }
+
     /// Iterates through all the multibodies on this set.
     pub fn multibodies(&self) -> impl Iterator<Item = &Multibody> {
         self.multibodies.iter().map(|e| e.1)
diff --git a/src/dynamics/rigid_body.rs b/src/dynamics/rigid_body.rs
index 9c38eee..bb1e9bf 100644
--- a/src/dynamics/rigid_body.rs
+++ b/src/dynamics/rigid_body.rs
@@ -35,6 +35,7 @@ pub struct RigidBody {
     pub(crate) body_type: RigidBodyType,
     /// The dominance group this rigid-body is part of.
     pub(crate) dominance: RigidBodyDominance,
+    pub(crate) enabled: bool,
     /// User-defined data associated to this rigid-body.
     pub user_data: u128,
 }
@@ -61,6 +62,7 @@ impl RigidBody {
             changes: RigidBodyChanges::all(),
             body_type: RigidBodyType::Dynamic,
             dominance: RigidBodyDominance::default(),
+            enabled: true,
             user_data: 0,
         }
     }
@@ -81,6 +83,28 @@ impl RigidBody {
         &mut self.activation
     }
 
+    /// Is this rigid-body enabled?
+    pub fn is_enabled(&self) -> bool {
+        self.enabled
+    }
+
+    /// Sets whether this rigid-body is enabled or not.
+    pub fn set_enabled(&mut self, enabled: bool) {
+        if enabled != self.enabled {
+            if enabled {
+                // NOTE: this is probably overkill, but it makes sure we don’t
+                // forget anything that needs to be updated because the rigid-body
+                // was basically interpreted as if it was removed while it was
+                // disabled.
+                self.changes = RigidBodyChanges::all();
+            } else {
+                self.changes |= RigidBodyChanges::ENABLED_OR_DISABLED;
+            }
+
+            self.enabled = enabled;
+        }
+    }
+
     /// The linear damping coefficient of this rigid-body.
     #[inline]
     pub fn linear_damping(&self) -> Real {
@@ -111,7 +135,7 @@ impl RigidBody {
     }
 
     /// Sets the type of this rigid-body.
-    pub fn set_body_type(&mut self, status: RigidBodyType) {
+    pub fn set_body_type(&mut self, status: RigidBodyType, wake_up: bool) {
         if status != self.body_type {
             self.changes.insert(RigidBodyChanges::TYPE);
             self.body_type = status;
@@ -119,9 +143,19 @@ impl RigidBody {
             if status == RigidBodyType::Fixed {
                 self.vels = RigidBodyVelocity::zero();
             }
+
+            if self.is_dynamic() && wake_up {
+                self.wake_up(true);
+            }
         }
     }
 
+    /// The world-space center-of-mass of this rigid-body.
+    #[inline]
+    pub fn center_of_mass(&self) -> &Point<Real> {
+        &self.mprops.world_com
+    }
+
     /// The mass-properties of this rigid-body.
     #[inline]
     pub fn mass_properties(&self) -> &MassProperties {
@@ -963,6 +997,8 @@ pub struct RigidBodyBuilder {
     pub ccd_enabled: bool,
     /// The dominance group of the rigid-body to be built.
     pub dominance_group: i8,
+    /// Will the rigid-body being built be enabled?
+    pub enabled: bool,
     /// An arbitrary user-defined 128-bit integer associated to the rigid-bodies built by this builder.
     pub user_data: u128,
 }
@@ -984,6 +1020,7 @@ impl RigidBodyBuilder {
             sleeping: false,
             ccd_enabled: false,
             dominance_group: 0,
+            enabled: true,
             user_data: 0,
         }
     }
@@ -1230,6 +1267,12 @@ impl RigidBodyBuilder {
         self
     }
 
+    /// Enable or disable the rigid-body after its creation.
+    pub fn enabled(mut self, enabled: bool) -> Self {
+        self.enabled = enabled;
+        self
+    }
+
     /// Build a new rigid-body with the parameters configured with this builder.
     pub fn build(&self) -> RigidBody {
         let mut rb = RigidBody::new();
@@ -1252,6 +1295,7 @@ impl RigidBodyBuilder {
         rb.damping.angular_damping = self.angular_damping;
         rb.forces.gravity_scale = self.gravity_scale;
         rb.dominance = RigidBodyDominance(self.dominance_group);
+        rb.enabled = self.enabled;
         rb.enable_ccd(self.ccd_enabled);
 
         if self.can_sleep && self.sleeping {
diff --git a/src/dynamics/rigid_body_components.rs b/src/dynamics/rigid_body_components.rs
index cabf6ca..eb94c6b 100644
--- a/src/dynamics/rigid_body_components.rs
+++ b/src/dynamics/rigid_body_components.rs
@@ -112,6 +112,8 @@ bitflags::bitflags! {
         const DOMINANCE   = 1 << 5;
         /// Flag indicating that the local mass-properties of this rigid-body must be recomputed.
         const LOCAL_MASS_PROPERTIES = 1 << 6;
+        /// Flag indicating that the rigid-body was enabled or disabled.
+        const ENABLED_OR_DISABLED = 1 << 7;
     }
 }
 
@@ -329,12 +331,14 @@ impl RigidBodyMassProps {
 
         for handle in &attached_colliders.0 {
             if let Some(co) = colliders.get(*handle) {
-                if let Some(co_parent) = co.parent {
-                    let to_add = co
-                        .mprops
-                        .mass_properties(&*co.shape)
-                        .transform_by(&co_parent.pos_wrt_parent);
-                    self.local_mprops += to_add;
+                if co.is_enabled() {
+                    if let Some(co_parent) = co.parent {
+                        let to_add = co
+                            .mprops
+                            .mass_properties(&*co.shape)
+                            .transform_by(&co_parent.pos_wrt_parent);
+                        self.local_mprops += to_add;
+                    }
                 }
             }
         }
diff --git a/src/dynamics/solver/velocity_constraint.rs b/src/dynamics/solver/velocity_constraint.rs
index 7d2294e..a6b7384 100644
--- a/src/dynamics/solver/velocity_constraint.rs
+++ b/src/dynamics/solver/velocity_constraint.rs
@@ -214,6 +214,7 @@ impl VelocityConstraint {
             let constraint = if insert_at.is_none() {
                 let new_len = out_constraints.len() + 1;
                 unsafe {
+                    #[allow(invalid_value)]
                     out_constraints.resize_with(new_len, || {
                         AnyVelocityConstraint::Nongrouped(
                             std::mem::MaybeUninit::uninit().assume_init(),
diff --git a/src/dynamics/solver/velocity_ground_constraint.rs b/src/dynamics/solver/velocity_ground_constraint.rs
index 19bf7e6..83bbd7e 100644
--- a/src/dynamics/solver/velocity_ground_constraint.rs
+++ b/src/dynamics/solver/velocity_ground_constraint.rs
@@ -105,6 +105,7 @@ impl VelocityGroundConstraint {
             let constraint = if insert_at.is_none() {
                 let new_len = out_constraints.len() + 1;
                 unsafe {
+                    #[allow(invalid_value)]
                     out_constraints.resize_with(new_len, || {
                         AnyVelocityConstraint::NongroupedGround(
                             std::mem::MaybeUninit::uninit().assume_init(),