Merge pull request #157 from dimforge/ccd

Implement Continuous Collision Detection

1 files changed, 136 insertions, 12 deletions

diff --git a/src/geometry/collider.rs b/src/geometry/collider.rs
index 593c53c..529d2f5 100644
--- a/src/geometry/collider.rs
+++ b/src/geometry/collider.rs
@@ -1,8 +1,9 @@
 use crate::dynamics::{CoefficientCombineRule, MassProperties, RigidBodyHandle};
-use crate::geometry::{InteractionGroups, SharedShape, SolverFlags};
+use crate::geometry::{InteractionGroups, SAPProxyIndex, SharedShape, SolverFlags};
 use crate::math::{AngVector, Isometry, Point, Real, Rotation, Vector, DIM};
 use crate::parry::transformation::vhacd::VHACDParameters;
-use parry::bounding_volume::AABB;
+use na::Unit;
+use parry::bounding_volume::{BoundingVolume, AABB};
 use parry::shape::Shape;
 
 bitflags::bitflags! {
@@ -49,6 +50,34 @@ enum MassInfo {
     MassProperties(Box<MassProperties>),
 }
 
+bitflags::bitflags! {
+    #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
+    /// Flags describing how the collider has been modified by the user.
+    pub(crate) struct ColliderChanges: u32 {
+        const MODIFIED             = 1 << 0;
+        const POSITION_WRT_PARENT  = 1 << 1; // => BF & NF updates.
+        const POSITION             = 1 << 2; // => BF & NF updates.
+        const COLLISION_GROUPS     = 1 << 3; // => NF update.
+        const SOLVER_GROUPS        = 1 << 4; // => NF update.
+        const SHAPE                = 1 << 5; // => BF & NF update. NF pair workspace invalidation.
+        const SENSOR               = 1 << 6; // => NF update. NF pair invalidation.
+    }
+}
+
+impl ColliderChanges {
+    pub fn needs_broad_phase_update(self) -> bool {
+        self.intersects(
+            ColliderChanges::POSITION_WRT_PARENT
+                | ColliderChanges::POSITION
+                | ColliderChanges::SHAPE,
+        )
+    }
+
+    pub fn needs_narrow_phase_update(self) -> bool {
+        self.bits() > 1
+    }
+}
+
 #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
 #[derive(Clone)]
 /// A geometric entity that can be attached to a body so it can be affected by contacts and proximity queries.
@@ -59,17 +88,17 @@ pub struct Collider {
     mass_info: MassInfo,
     pub(crate) flags: ColliderFlags,
     pub(crate) solver_flags: SolverFlags,
+    pub(crate) changes: ColliderChanges,
     pub(crate) parent: RigidBodyHandle,
     pub(crate) delta: Isometry<Real>,
     pub(crate) position: Isometry<Real>,
-    pub(crate) predicted_position: Isometry<Real>,
     /// The friction coefficient of this collider.
     pub friction: Real,
     /// The restitution coefficient of this collider.
     pub restitution: Real,
     pub(crate) collision_groups: InteractionGroups,
     pub(crate) solver_groups: InteractionGroups,
-    pub(crate) proxy_index: usize,
+    pub(crate) proxy_index: SAPProxyIndex,
     /// User-defined data associated to this rigid-body.
     pub user_data: u128,
 }
@@ -77,7 +106,8 @@ pub struct Collider {
 impl Collider {
     pub(crate) fn reset_internal_references(&mut self) {
         self.parent = RigidBodyHandle::invalid();
-        self.proxy_index = crate::INVALID_USIZE;
+        self.proxy_index = crate::INVALID_U32;
+        self.changes = ColliderChanges::empty();
     }
 
     /// The rigid body this collider is attached to.
@@ -90,6 +120,42 @@ impl Collider {
         self.flags.is_sensor()
     }
 
+    /// The combine rule used by this collider to combine its friction
+    /// coefficient with the friction coefficient of the other collider it
+    /// is in contact with.
+    pub fn friction_combine_rule(&self) -> CoefficientCombineRule {
+        CoefficientCombineRule::from_value(self.flags.friction_combine_rule_value())
+    }
+
+    /// Sets the combine rule used by this collider to combine its friction
+    /// coefficient with the friction coefficient of the other collider it
+    /// is in contact with.
+    pub fn set_friction_combine_rule(&mut self, rule: CoefficientCombineRule) {
+        self.flags = self.flags.with_friction_combine_rule(rule);
+    }
+
+    /// The combine rule used by this collider to combine its restitution
+    /// coefficient with the restitution coefficient of the other collider it
+    /// is in contact with.
+    pub fn restitution_combine_rule(&self) -> CoefficientCombineRule {
+        CoefficientCombineRule::from_value(self.flags.restitution_combine_rule_value())
+    }
+
+    /// Sets the combine rule used by this collider to combine its restitution
+    /// coefficient with the restitution coefficient of the other collider it
+    /// is in contact with.
+    pub fn set_restitution_combine_rule(&mut self, rule: CoefficientCombineRule) {
+        self.flags = self.flags.with_restitution_combine_rule(rule)
+    }
+
+    /// Sets whether or not this is a sensor collider.
+    pub fn set_sensor(&mut self, is_sensor: bool) {
+        if is_sensor != self.is_sensor() {
+            self.changes.insert(ColliderChanges::SENSOR);
+            self.flags.set(ColliderFlags::SENSOR, is_sensor);
+        }
+    }
+
     #[doc(hidden)]
     pub fn set_position_debug(&mut self, position: Isometry<Real>) {
         self.position = position;
@@ -106,21 +172,49 @@ impl Collider {
         &self.position
     }
 
+    /// Sets the position of this collider wrt. its parent rigid-body.
+    pub(crate) fn set_position(&mut self, position: Isometry<Real>) {
+        self.changes.insert(ColliderChanges::POSITION);
+        self.position = position;
+    }
+
     /// The position of this collider wrt the body it is attached to.
     pub fn position_wrt_parent(&self) -> &Isometry<Real> {
         &self.delta
     }
 
+    /// Sets the position of this collider wrt. its parent rigid-body.
+    pub fn set_position_wrt_parent(&mut self, position: Isometry<Real>) {
+        self.changes.insert(ColliderChanges::POSITION_WRT_PARENT);
+        self.delta = position;
+    }
+
     /// The collision groups used by this collider.
     pub fn collision_groups(&self) -> InteractionGroups {
         self.collision_groups
     }
 
+    /// Sets the collision groups of this collider.
+    pub fn set_collision_groups(&mut self, groups: InteractionGroups) {
+        if self.collision_groups != groups {
+            self.changes.insert(ColliderChanges::COLLISION_GROUPS);
+            self.collision_groups = groups;
+        }
+    }
+
     /// The solver groups used by this collider.
     pub fn solver_groups(&self) -> InteractionGroups {
         self.solver_groups
     }
 
+    /// Sets the solver groups of this collider.
+    pub fn set_solver_groups(&mut self, groups: InteractionGroups) {
+        if self.solver_groups != groups {
+            self.changes.insert(ColliderChanges::SOLVER_GROUPS);
+            self.solver_groups = groups;
+        }
+    }
+
     /// The density of this collider, if set.
     pub fn density(&self) -> Option<Real> {
         match &self.mass_info {
@@ -134,16 +228,32 @@ impl Collider {
         &*self.shape.0
     }
 
+    /// A mutable reference to the geometric shape of this collider.
+    ///
+    /// If that shape is shared by multiple colliders, it will be
+    /// cloned first so that `self` contains a unique copy of that
+    /// shape that you can modify.
+    pub fn shape_mut(&mut self) -> &mut dyn Shape {
+        self.shape.make_mut()
+    }
+
+    /// Sets the shape of this collider.
+    pub fn set_shape(&mut self, shape: SharedShape) {
+        self.changes.insert(ColliderChanges::SHAPE);
+        self.shape = shape;
+    }
+
     /// Compute the axis-aligned bounding box of this collider.
     pub fn compute_aabb(&self) -> AABB {
         self.shape.compute_aabb(&self.position)
     }
 
-    // pub(crate) fn compute_aabb_with_prediction(&self) -> AABB {
-    //     let aabb1 = self.shape.compute_aabb(&self.position);
-    //     let aabb2 = self.shape.compute_aabb(&self.predicted_position);
-    //     aabb1.merged(&aabb2)
-    // }
+    /// Compute the axis-aligned bounding box of this collider.
+    pub fn compute_swept_aabb(&self, next_position: &Isometry<Real>) -> AABB {
+        let aabb1 = self.shape.compute_aabb(&self.position);
+        let aabb2 = self.shape.compute_aabb(next_position);
+        aabb1.merged(&aabb2)
+    }
 
     /// Compute the local-space mass properties of this collider.
     pub fn mass_properties(&self) -> MassProperties {
@@ -218,6 +328,12 @@ impl ColliderBuilder {
         Self::new(SharedShape::ball(radius))
     }
 
+    /// Initialize a new collider build with a half-space shape defined by the outward normal
+    /// of its planar boundary.
+    pub fn halfspace(outward_normal: Unit<Vector<Real>>) -> Self {
+        Self::new(SharedShape::halfspace(outward_normal))
+    }
+
     /// Initialize a new collider builder with a cylindrical shape defined by its half-height
     /// (along along the y axis) and its radius.
     #[cfg(feature = "dim3")]
@@ -553,6 +669,14 @@ impl ColliderBuilder {
 
     /// Sets the initial position (translation and orientation) of the collider to be created,
     /// relative to the rigid-body it is attached to.
+    pub fn position_wrt_parent(mut self, pos: Isometry<Real>) -> Self {
+        self.delta = pos;
+        self
+    }
+
+    /// Sets the initial position (translation and orientation) of the collider to be created,
+    /// relative to the rigid-body it is attached to.
+    #[deprecated(note = "Use `.position_wrt_parent` instead.")]
     pub fn position(mut self, pos: Isometry<Real>) -> Self {
         self.delta = pos;
         self
@@ -594,10 +718,10 @@ impl ColliderBuilder {
             delta: self.delta,
             flags,
             solver_flags,
+            changes: ColliderChanges::all(),
             parent: RigidBodyHandle::invalid(),
             position: Isometry::identity(),
-            predicted_position: Isometry::identity(),
-            proxy_index: crate::INVALID_USIZE,
+            proxy_index: crate::INVALID_U32,
             collision_groups: self.collision_groups,
             solver_groups: self.solver_groups,
             user_data: self.user_data,