feat: implement collision skin

5 files changed, 66 insertions, 23 deletions

diff --git a/src/geometry/broad_phase_multi_sap/broad_phase_multi_sap.rs b/src/geometry/broad_phase_multi_sap/broad_phase_multi_sap.rs
index fb5d407..d20ed31 100644
--- a/src/geometry/broad_phase_multi_sap/broad_phase_multi_sap.rs
+++ b/src/geometry/broad_phase_multi_sap/broad_phase_multi_sap.rs
@@ -2,7 +2,7 @@ use super::{
     BroadPhasePairEvent, ColliderPair, SAPLayer, SAPProxies, SAPProxy, SAPProxyData, SAPRegionPool,
 };
 use crate::geometry::{
-    BroadPhaseProxyIndex, ColliderBroadPhaseData, ColliderChanges, ColliderHandle,
+    BroadPhaseProxyIndex, Collider, ColliderBroadPhaseData, ColliderChanges, ColliderHandle,
     ColliderPosition, ColliderSet, ColliderShape,
 };
 use crate::math::{Isometry, Real};
@@ -353,19 +353,16 @@ impl BroadPhaseMultiSap {
         prediction_distance: Real,
         handle: ColliderHandle,
         proxy_index: &mut u32,
-        collider: (&ColliderPosition, &ColliderShape, &ColliderChanges),
+        collider: &Collider,
         next_position: Option<&Isometry<Real>>,
     ) -> bool {
-        let (co_pos, co_shape, co_changes) = collider;
-
-        let mut aabb = co_shape
-            .compute_aabb(co_pos)
-            .loosened(prediction_distance / 2.0);
+        let mut aabb = collider.compute_collision_aabb(prediction_distance / 2.0);
 
         if let Some(next_position) = next_position {
-            let next_aabb = co_shape
+            let next_aabb = collider
+                .shape
                 .compute_aabb(next_position)
-                .loosened(prediction_distance / 2.0);
+                .loosened(collider.collision_skin() + prediction_distance / 2.0);
             aabb.merge(&next_aabb);
         }
 
@@ -386,7 +383,7 @@ impl BroadPhaseMultiSap {
             prev_aabb = proxy.aabb;
             proxy.aabb = aabb;
 
-            if co_changes.contains(ColliderChanges::SHAPE) {
+            if collider.changes.contains(ColliderChanges::SHAPE) {
                 // If the shape was changed, then we need to see if this proxy should be
                 // migrated to a larger layer. Indeed, if the shape was replaced by
                 // a much larger shape, we need to promote the proxy to a bigger layer
@@ -609,7 +606,7 @@ impl BroadPhase for BroadPhaseMultiSap {
                     prediction_distance,
                     *handle,
                     &mut new_proxy_id,
-                    (&co.pos, &co.shape, &co.changes),
+                    &co,
                     next_pos.as_ref(),
                 ) {
                     need_region_propagation = true;
diff --git a/src/geometry/broad_phase_qbvh.rs b/src/geometry/broad_phase_qbvh.rs
index 56c2b6f..be8c53b 100644
--- a/src/geometry/broad_phase_qbvh.rs
+++ b/src/geometry/broad_phase_qbvh.rs
@@ -59,7 +59,7 @@ impl BroadPhaseQbvh {
                 colliders.iter().map(|(handle, collider)| {
                     (
                         handle,
-                        collider.compute_aabb().loosened(prediction_distance / 2.0),
+                        collider.compute_collision_aabb(prediction_distance / 2.0),
                     )
                 }),
                 margin,
@@ -76,9 +76,7 @@ impl BroadPhaseQbvh {
             }
 
             let _ = self.qbvh.refit(margin, &mut self.workspace, |handle| {
-                colliders[*handle]
-                    .compute_aabb()
-                    .loosened(prediction_distance / 2.0)
+                colliders[*handle].compute_collision_aabb(prediction_distance / 2.0)
             });
             self.qbvh
                 .traverse_modified_bvtt_with_stack(&self.qbvh, &mut visitor, &mut self.stack);
diff --git a/src/geometry/collider.rs b/src/geometry/collider.rs
index c83535e..617881a 100644
--- a/src/geometry/collider.rs
+++ b/src/geometry/collider.rs
@@ -9,7 +9,7 @@ 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::bounding_volume::{Aabb, BoundingVolume};
 use parry::shape::{Shape, TriMeshFlags};
 
 #[cfg(feature = "dim3")]
@@ -30,6 +30,7 @@ pub struct Collider {
     pub(crate) material: ColliderMaterial,
     pub(crate) flags: ColliderFlags,
     pub(crate) bf_data: ColliderBroadPhaseData,
+    collision_skin: Real,
     contact_force_event_threshold: Real,
     /// User-defined data associated to this collider.
     pub user_data: u128,
@@ -109,6 +110,7 @@ impl Collider {
             bf_data: _bf_data, // Internal ids must not be overwritten.
             contact_force_event_threshold,
             user_data,
+            collision_skin,
         } = other;
 
         if self.parent.is_none() {
@@ -123,6 +125,7 @@ impl Collider {
         self.user_data = *user_data;
         self.flags = *flags;
         self.changes = ColliderChanges::all();
+        self.collision_skin = *collision_skin;
     }
 
     /// The physics hooks enabled for this collider.
@@ -155,6 +158,16 @@ impl Collider {
         self.flags.active_collision_types = active_collision_types;
     }
 
+    /// The collision skin of this collider.
+    pub fn collision_skin(&self) -> Real {
+        self.collision_skin
+    }
+
+    /// Sets the collision skin of this collider.
+    pub fn set_collision_skin(&mut self, skin_thickness: Real) {
+        self.collision_skin = skin_thickness;
+    }
+
     /// The friction coefficient of this collider.
     pub fn friction(&self) -> Real {
         self.material.friction
@@ -437,10 +450,21 @@ impl Collider {
     }
 
     /// Compute the axis-aligned bounding box of this collider.
+    ///
+    /// This AABB doesn’t take into account the collider’s collision skin.
+    /// [`Collider::collision_skin`].
     pub fn compute_aabb(&self) -> Aabb {
         self.shape.compute_aabb(&self.pos)
     }
 
+    /// Compute the axis-aligned bounding box of this collider, taking into account the
+    /// [`Collider::collision_skin`] and prediction distance.
+    pub fn compute_collision_aabb(&self, prediction: Real) -> Aabb {
+        self.shape
+            .compute_aabb(&self.pos)
+            .loosened(self.collision_skin + prediction)
+    }
+
     /// Compute the axis-aligned bounding box of this collider moving from its current position
     /// to the given `next_position`
     pub fn compute_swept_aabb(&self, next_position: &Isometry<Real>) -> Aabb {
@@ -495,6 +519,8 @@ pub struct ColliderBuilder {
     pub enabled: bool,
     /// The total force magnitude beyond which a contact force event can be emitted.
     pub contact_force_event_threshold: Real,
+    /// An extract thickness around the collider shape to keep them further apart when in collision.
+    pub collision_skin: Real,
 }
 
 impl ColliderBuilder {
@@ -517,6 +543,7 @@ impl ColliderBuilder {
             active_events: ActiveEvents::empty(),
             enabled: true,
             contact_force_event_threshold: 0.0,
+            collision_skin: 0.0,
         }
     }
 
@@ -946,6 +973,20 @@ impl ColliderBuilder {
         self
     }
 
+    /// Sets the collision skin of the collider.
+    ///
+    /// The collision skin acts as if the collider was enlarged with a skin of width `skin_thickness`
+    /// around it, keeping objects further apart when colliding.
+    ///
+    /// A non-zero collision skin can increase performance, and in some cases, stability. However
+    /// it creates a small gap between colliding object (equal to the sum of their skin). If the
+    /// skin is sufficiently small, this might not be visually significant or can be hidden by the
+    /// rendering assets.
+    pub fn collision_skin(mut self, skin_thickness: Real) -> Self {
+        self.collision_skin = skin_thickness;
+        self
+    }
+
     /// Enable or disable the collider after its creation.
     pub fn enabled(mut self, enabled: bool) -> Self {
         self.enabled = enabled;
@@ -993,6 +1034,7 @@ impl ColliderBuilder {
             flags,
             coll_type,
             contact_force_event_threshold: self.contact_force_event_threshold,
+            collision_skin: self.collision_skin,
             user_data: self.user_data,
         }
     }
diff --git a/src/geometry/contact_pair.rs b/src/geometry/contact_pair.rs
index 5db95c0..55fec2b 100644
--- a/src/geometry/contact_pair.rs
+++ b/src/geometry/contact_pair.rs
@@ -116,6 +116,9 @@ pub struct ContactPair {
     /// The set of contact manifolds between the two colliders.
     ///
     /// All contact manifold contain themselves contact points between the colliders.
+    /// Note that contact points in the contact manifold do not take into account the
+    /// [`Collider::collision_skin`] which only affects the constraint solver and the
+    /// [`SolverContact`].
     pub manifolds: Vec<ContactManifold>,
     /// Is there any active contact in this contact pair?
     pub has_any_active_contact: bool,
diff --git a/src/geometry/narrow_phase.rs b/src/geometry/narrow_phase.rs
index 39ed253..bd2256b 100644
--- a/src/geometry/narrow_phase.rs
+++ b/src/geometry/narrow_phase.rs
@@ -898,11 +898,12 @@ impl NarrowPhase {
 
                 let pos12 = co1.pos.inv_mul(&co2.pos);
 
+                let collision_skin_sum = co1.collision_skin() + co2.collision_skin();
                 let soft_ccd_prediction1 = rb1.map(|rb| rb.soft_ccd_prediction()).unwrap_or(0.0);
                 let soft_ccd_prediction2 = rb2.map(|rb| rb.soft_ccd_prediction()).unwrap_or(0.0);
                 let effective_prediction_distance = if soft_ccd_prediction1 > 0.0 || soft_ccd_prediction2 > 0.0 {
-                        let aabb1 = co1.compute_aabb();
-                        let aabb2 = co2.compute_aabb();
+                        let aabb1 = co1.compute_collision_aabb(0.0);
+                        let aabb2 = co2.compute_collision_aabb(0.0);
                         let inv_dt = crate::utils::inv(dt);
 
                         let linvel1 = rb1.map(|rb| rb.linvel()
@@ -917,9 +918,9 @@ impl NarrowPhase {
 
 
                     prediction_distance.max(
-                        dt * (linvel1 - linvel2).norm())
+                        dt * (linvel1 - linvel2).norm()) + collision_skin_sum
                 } else {
-                    prediction_distance
+                    prediction_distance + collision_skin_sum
                 };
 
                 let _ = query_dispatcher.contact_manifolds(
@@ -968,12 +969,14 @@ impl NarrowPhase {
                             break;
                         }
 
-                        let keep_solver_contact = contact.dist < prediction_distance || {
+                        let effective_contact_dist = contact.dist - co1.collision_skin() - co2.collision_skin();
+
+                        let keep_solver_contact = effective_contact_dist < prediction_distance || {
                             let world_pt1 = world_pos1 * contact.local_p1;
                             let world_pt2 = world_pos2 * contact.local_p2;
                             let vel1 = rb1.map(|rb| rb.velocity_at_point(&world_pt1)).unwrap_or_default();
                             let vel2 = rb2.map(|rb| rb.velocity_at_point(&world_pt2)).unwrap_or_default();
-                            contact.dist + (vel2 - vel1).dot(&manifold.data.normal) * dt < prediction_distance
+                            effective_contact_dist + (vel2 - vel1).dot(&manifold.data.normal) * dt < prediction_distance
                         };
 
                         if keep_solver_contact {
@@ -985,7 +988,7 @@ impl NarrowPhase {
                             let solver_contact = SolverContact {
                                 contact_id: contact_id as u8,
                                 point: effective_point,
-                                dist: contact.dist,
+                                dist: effective_contact_dist,
                                 friction,
                                 restitution,
                                 tangent_velocity: Vector::zeros(),