Merge pull request #157 from dimforge/ccd

Implement Continuous Collision Detection

1 files changed, 150 insertions, 57 deletions

diff --git a/src/dynamics/rigid_body_set.rs b/src/dynamics/rigid_body_set.rs
index f459d4f..5a85ada 100644
--- a/src/dynamics/rigid_body_set.rs
+++ b/src/dynamics/rigid_body_set.rs
@@ -2,7 +2,7 @@
 use rayon::prelude::*;
 
 use crate::data::arena::Arena;
-use crate::dynamics::{Joint, JointSet, RigidBody, RigidBodyChanges};
+use crate::dynamics::{BodyStatus, Joint, JointSet, RigidBody, RigidBodyChanges};
 use crate::geometry::{ColliderSet, InteractionGraph, NarrowPhase};
 use parry::partitioning::IndexedData;
 use std::ops::{Index, IndexMut};
@@ -220,13 +220,17 @@ impl RigidBodySet {
     ///
     /// Using this is discouraged in favor of `self.get_mut(handle)` which does not
     /// suffer form the ABA problem.
+    #[cfg(not(feature = "dev-remove-slow-accessors"))]
     pub fn get_unknown_gen_mut(&mut self, i: usize) -> Option<(&mut RigidBody, RigidBodyHandle)> {
-        let result = self.bodies.get_unknown_gen_mut(i)?;
-        if !self.modified_all_bodies && !result.0.changes.contains(RigidBodyChanges::MODIFIED) {
-            result.0.changes = RigidBodyChanges::MODIFIED;
-            self.modified_bodies.push(RigidBodyHandle(result.1));
-        }
-        Some((result.0, RigidBodyHandle(result.1)))
+        let (rb, handle) = self.bodies.get_unknown_gen_mut(i)?;
+        let handle = RigidBodyHandle(handle);
+        Self::mark_as_modified(
+            handle,
+            rb,
+            &mut self.modified_bodies,
+            self.modified_all_bodies,
+        );
+        Some((rb, handle))
     }
 
     /// Gets the rigid-body with the given handle.
@@ -247,6 +251,7 @@ impl RigidBodySet {
     }
 
     /// Gets a mutable reference to the rigid-body with the given handle.
+    #[cfg(not(feature = "dev-remove-slow-accessors"))]
     pub fn get_mut(&mut self, handle: RigidBodyHandle) -> Option<&mut RigidBody> {
         let result = self.bodies.get_mut(handle.0)?;
         Self::mark_as_modified(
@@ -262,6 +267,22 @@ impl RigidBodySet {
         self.bodies.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`.
+    pub(crate) fn get_mut_internal_with_modification_tracking(
+        &mut self,
+        handle: RigidBodyHandle,
+    ) -> Option<&mut RigidBody> {
+        let result = self.bodies.get_mut(handle.0)?;
+        Self::mark_as_modified(
+            handle,
+            result,
+            &mut self.modified_bodies,
+            self.modified_all_bodies,
+        );
+        Some(result)
+    }
+
     pub(crate) fn get2_mut_internal(
         &mut self,
         h1: RigidBodyHandle,
@@ -276,6 +297,7 @@ impl RigidBodySet {
     }
 
     /// Iterates mutably through all the rigid-bodies on this set.
+    #[cfg(not(feature = "dev-remove-slow-accessors"))]
     pub fn iter_mut(&mut self) -> impl Iterator<Item = (RigidBodyHandle, &mut RigidBody)> {
         self.modified_bodies.clear();
         self.modified_all_bodies = true;
@@ -317,6 +339,7 @@ impl RigidBodySet {
     /// Applies the given function on all the active dynamic rigid-bodies
     /// contained by this set.
     #[inline(always)]
+    #[cfg(not(feature = "dev-remove-slow-accessors"))]
     pub fn foreach_active_dynamic_body_mut(
         &mut self,
         mut f: impl FnMut(RigidBodyHandle, &mut RigidBody),
@@ -430,71 +453,133 @@ impl RigidBodySet {
 
     // Utility function to avoid some borrowing issue in the `maintain` method.
     fn maintain_one(
+        bodies: &mut Arena<RigidBody>,
         colliders: &mut ColliderSet,
         handle: RigidBodyHandle,
-        rb: &mut RigidBody,
         modified_inactive_set: &mut Vec<RigidBodyHandle>,
         active_kinematic_set: &mut Vec<RigidBodyHandle>,
         active_dynamic_set: &mut Vec<RigidBodyHandle>,
     ) {
-        // Update the positions of the colliders.
-        if rb.changes.contains(RigidBodyChanges::POSITION)
-            || rb.changes.contains(RigidBodyChanges::COLLIDERS)
-        {
-            rb.update_colliders_positions(colliders);
-
-            if rb.is_static() {
-                modified_inactive_set.push(handle);
+        enum FinalAction {
+            UpdateActiveKinematicSetId,
+            UpdateActiveDynamicSetId,
+        }
+
+        if let Some(rb) = bodies.get_mut(handle.0) {
+            let mut final_action = None;
+
+            // The body's status changed. We need to make sure
+            // it is on the correct active set.
+            if rb.changes.contains(RigidBodyChanges::BODY_STATUS) {
+                match rb.body_status() {
+                    BodyStatus::Dynamic => {
+                        // Remove from the active kinematic set if it was there.
+                        if active_kinematic_set.get(rb.active_set_id) == Some(&handle) {
+                            active_kinematic_set.swap_remove(rb.active_set_id);
+                            final_action =
+                                Some((FinalAction::UpdateActiveKinematicSetId, rb.active_set_id));
+                        }
+
+                        // Add to the active dynamic set.
+                        rb.wake_up(true);
+                        // Make sure the sleep change flag is set (even if for some
+                        // reasons the rigid-body was already awake) to make
+                        // sure the code handling sleeping change adds the body to
+                        // the active_dynamic_set.
+                        rb.changes.set(RigidBodyChanges::SLEEP, true);
+                    }
+                    BodyStatus::Kinematic => {
+                        // Remove from the active dynamic set if it was there.
+                        if active_dynamic_set.get(rb.active_set_id) == Some(&handle) {
+                            active_dynamic_set.swap_remove(rb.active_set_id);
+                            final_action =
+                                Some((FinalAction::UpdateActiveDynamicSetId, rb.active_set_id));
+                        }
+
+                        // Add to the active kinematic set.
+                        if active_kinematic_set.get(rb.active_set_id) != Some(&handle) {
+                            rb.active_set_id = active_kinematic_set.len();
+                            active_kinematic_set.push(handle);
+                        }
+                    }
+                    BodyStatus::Static => {}
+                }
             }
 
-            if rb.is_kinematic() && active_kinematic_set.get(rb.active_set_id) != Some(&handle) {
-                rb.active_set_id = active_kinematic_set.len();
-                active_kinematic_set.push(handle);
+            // Update the positions of the colliders.
+            if rb.changes.contains(RigidBodyChanges::POSITION)
+                || rb.changes.contains(RigidBodyChanges::COLLIDERS)
+            {
+                rb.update_colliders_positions(colliders);
+
+                if rb.is_static() {
+                    modified_inactive_set.push(handle);
+                }
+
+                if rb.is_kinematic() && active_kinematic_set.get(rb.active_set_id) != Some(&handle)
+                {
+                    rb.active_set_id = active_kinematic_set.len();
+                    active_kinematic_set.push(handle);
+                }
             }
-        }
 
-        // Push the body to the active set if it is not
-        // sleeping and if it is not already inside of the active set.
-        if rb.changes.contains(RigidBodyChanges::SLEEP)
-            && !rb.is_sleeping() // May happen if the body was put to sleep manually.
-            && rb.is_dynamic() // Only dynamic bodies are in the active dynamic set.
-            && active_dynamic_set.get(rb.active_set_id) != Some(&handle)
-        {
-            rb.active_set_id = active_dynamic_set.len(); // This will handle the case where the activation_channel contains duplicates.
-            active_dynamic_set.push(handle);
-        }
+            // Push the body to the active set if it is not
+            // sleeping and if it is not already inside of the active set.
+            if rb.changes.contains(RigidBodyChanges::SLEEP)
+                && !rb.is_sleeping() // May happen if the body was put to sleep manually.
+                && rb.is_dynamic() // Only dynamic bodies are in the active dynamic set.
+                && active_dynamic_set.get(rb.active_set_id) != Some(&handle)
+            {
+                rb.active_set_id = active_dynamic_set.len(); // This will handle the case where the activation_channel contains duplicates.
+                active_dynamic_set.push(handle);
+            }
+
+            rb.changes = RigidBodyChanges::empty();
+
+            // Adjust some ids, if needed.
+            if let Some((action, id)) = final_action {
+                let active_set = match action {
+                    FinalAction::UpdateActiveKinematicSetId => active_kinematic_set,
+                    FinalAction::UpdateActiveDynamicSetId => active_dynamic_set,
+                };
 
-        rb.changes = RigidBodyChanges::empty();
+                if id < active_set.len() {
+                    if let Some(rb2) = bodies.get_mut(active_set[id].0) {
+                        rb2.active_set_id = id;
+                    }
+                }
+            }
+        }
     }
 
-    pub(crate) fn maintain(&mut self, colliders: &mut ColliderSet) {
+    pub(crate) fn handle_user_changes(&mut self, colliders: &mut ColliderSet) {
         if self.modified_all_bodies {
-            for (handle, rb) in self.bodies.iter_mut() {
-                Self::maintain_one(
-                    colliders,
-                    RigidBodyHandle(handle),
-                    rb,
-                    &mut self.modified_inactive_set,
-                    &mut self.active_kinematic_set,
-                    &mut self.active_dynamic_set,
-                )
+            // Unfortunately, we have to push all the bodies to `modified_bodies`
+            // instead of just calling `maintain_one` on each element i
+            // `self.bodies.iter_mut()` because otherwise it would be difficult to
+            // handle the final  change of active_set_id in Self::maintain_one
+            // (because it has  to modify another rigid-body because of the swap-remove.
+            // So this causes borrowing problems if we do this while iterating
+            // through self.bodies.iter_mut()).
+            for (handle, _) in self.bodies.iter_mut() {
+                self.modified_bodies.push(RigidBodyHandle(handle));
             }
+        }
 
-            self.modified_bodies.clear();
+        for handle in self.modified_bodies.drain(..) {
+            Self::maintain_one(
+                &mut self.bodies,
+                colliders,
+                handle,
+                &mut self.modified_inactive_set,
+                &mut self.active_kinematic_set,
+                &mut self.active_dynamic_set,
+            )
+        }
+
+        if self.modified_all_bodies {
+            self.modified_bodies.shrink_to_fit(); // save some memory.
             self.modified_all_bodies = false;
-        } else {
-            for handle in self.modified_bodies.drain(..) {
-                if let Some(rb) = self.bodies.get_mut(handle.0) {
-                    Self::maintain_one(
-                        colliders,
-                        handle,
-                        rb,
-                        &mut self.modified_inactive_set,
-                        &mut self.active_kinematic_set,
-                        &mut self.active_dynamic_set,
-                    )
-                }
-            }
         }
     }
 
@@ -651,8 +736,16 @@ impl Index<RigidBodyHandle> for RigidBodySet {
     }
 }
 
+#[cfg(not(feature = "dev-remove-slow-accessors"))]
 impl IndexMut<RigidBodyHandle> for RigidBodySet {
-    fn index_mut(&mut self, index: RigidBodyHandle) -> &mut RigidBody {
-        &mut self.bodies[index.0]
+    fn index_mut(&mut self, handle: RigidBodyHandle) -> &mut RigidBody {
+        let rb = &mut self.bodies[handle.0];
+        Self::mark_as_modified(
+            handle,
+            rb,
+            &mut self.modified_bodies,
+            self.modified_all_bodies,
+        );
+        rb
     }
 }