Merge pull request #114 from EmbarkStudios/split-contacts-2

Split bouncy and resting contacts (take 2)

6 files changed, 38 insertions, 35 deletions

diff --git a/src/dynamics/integration_parameters.rs b/src/dynamics/integration_parameters.rs
index 628e07a..5d0d221 100644
--- a/src/dynamics/integration_parameters.rs
+++ b/src/dynamics/integration_parameters.rs
@@ -27,9 +27,6 @@ pub struct IntegrationParameters {
     /// Each cached impulse are multiplied by this coefficient in `[0, 1]`
     /// when they are re-used to initialize the solver (default `1.0`).
     pub warmstart_coeff: Real,
-    /// Contacts at points where the involved bodies have a relative
-    /// velocity smaller than this threshold wont be affected by the restitution force (default: `1.0`).
-    pub restitution_velocity_threshold: Real,
     /// Amount of penetration the engine wont attempt to correct (default: `0.005m`).
     pub allowed_linear_error: Real,
     /// The maximal distance separating two objects that will generate predictive contacts (default: `0.002`).
@@ -95,7 +92,7 @@ impl IntegrationParameters {
         erp: Real,
         joint_erp: Real,
         warmstart_coeff: Real,
-        restitution_velocity_threshold: Real,
+        _restitution_velocity_threshold: Real,
         allowed_linear_error: Real,
         allowed_angular_error: Real,
         max_linear_correction: Real,
@@ -116,7 +113,6 @@ impl IntegrationParameters {
             erp,
             joint_erp,
             warmstart_coeff,
-            restitution_velocity_threshold,
             allowed_linear_error,
             allowed_angular_error,
             max_linear_correction,
@@ -188,7 +184,6 @@ impl Default for IntegrationParameters {
             erp: 0.2,
             joint_erp: 0.2,
             warmstart_coeff: 1.0,
-            restitution_velocity_threshold: 1.0,
             allowed_linear_error: 0.005,
             prediction_distance: 0.002,
             allowed_angular_error: 0.001,
diff --git a/src/dynamics/solver/velocity_constraint.rs b/src/dynamics/solver/velocity_constraint.rs
index 514434b..2cac93d 100644
--- a/src/dynamics/solver/velocity_constraint.rs
+++ b/src/dynamics/solver/velocity_constraint.rs
@@ -239,13 +239,10 @@ impl VelocityConstraint {
                             + gcross1.gdot(gcross1)
                             + gcross2.gdot(gcross2));
 
-                    let mut rhs = (vel1 - vel2).dot(&force_dir1);
-
-                    if rhs <= -params.restitution_velocity_threshold {
-                        rhs += manifold_point.restitution * rhs
-                    }
-
-                    rhs += manifold_point.dist.max(0.0) * inv_dt;
+                    let is_bouncy = manifold_point.is_bouncy() as u32 as Real;
+                    let rhs = (1.0 + is_bouncy * manifold_point.restitution)
+                        * (vel1 - vel2).dot(&force_dir1)
+                        + manifold_point.dist.max(0.0) * inv_dt;
 
                     let impulse = manifold_point.data.impulse * warmstart_coeff;
 
diff --git a/src/dynamics/solver/velocity_constraint_wide.rs b/src/dynamics/solver/velocity_constraint_wide.rs
index a8384de..3e068e7 100644
--- a/src/dynamics/solver/velocity_constraint_wide.rs
+++ b/src/dynamics/solver/velocity_constraint_wide.rs
@@ -96,8 +96,6 @@ impl WVelocityConstraint {
         let mj_lambda1 = array![|ii| rbs1[ii].active_set_offset; SIMD_WIDTH];
         let mj_lambda2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH];
 
-        let restitution_velocity_threshold = SimdReal::splat(params.restitution_velocity_threshold);
-
         let warmstart_multiplier =
             SimdReal::from(array![|ii| manifolds[ii].data.warmstart_multiplier; SIMD_WIDTH]);
         let warmstart_coeff = warmstart_multiplier * SimdReal::splat(params.warmstart_coeff);
@@ -127,6 +125,9 @@ impl WVelocityConstraint {
                     SimdReal::from(array![|ii| manifold_points[ii][k].friction; SIMD_WIDTH]);
                 let restitution =
                     SimdReal::from(array![|ii| manifold_points[ii][k].restitution; SIMD_WIDTH]);
+                let is_bouncy = SimdReal::from(
+                    array![|ii| manifold_points[ii][k].is_bouncy() as u32 as Real; SIMD_WIDTH],
+                );
                 let point = Point::from(array![|ii| manifold_points[ii][k].point; SIMD_WIDTH]);
                 let dist = SimdReal::from(array![|ii| manifold_points[ii][k].dist; SIMD_WIDTH]);
 
@@ -148,11 +149,9 @@ impl WVelocityConstraint {
 
                     let r = SimdReal::splat(1.0)
                         / (im1 + im2 + gcross1.gdot(gcross1) + gcross2.gdot(gcross2));
-                    let mut rhs = (vel1 - vel2).dot(&force_dir1);
-                    let use_restitution = rhs.simd_le(-restitution_velocity_threshold);
-                    let rhs_with_restitution = rhs + rhs * restitution;
-                    rhs = rhs_with_restitution.select(use_restitution, rhs);
-                    rhs += dist.simd_max(SimdReal::zero()) * inv_dt;
+                    let projected_velocity = (vel1 - vel2).dot(&force_dir1);
+                    let rhs = (SimdReal::splat(1.0) + is_bouncy * restitution) * projected_velocity
+                        + dist.simd_max(SimdReal::zero()) * inv_dt;
 
                     constraint.elements[k].normal_part = WVelocityConstraintElementPart {
                         gcross1,
diff --git a/src/dynamics/solver/velocity_ground_constraint.rs b/src/dynamics/solver/velocity_ground_constraint.rs
index 20642ec..88f864a 100644
--- a/src/dynamics/solver/velocity_ground_constraint.rs
+++ b/src/dynamics/solver/velocity_ground_constraint.rs
@@ -154,15 +154,12 @@ impl VelocityGroundConstraint {
 
                     let r = 1.0 / (rb2.effective_inv_mass + gcross2.gdot(gcross2));
 
-                    let mut rhs = (vel1 - vel2).dot(&force_dir1);
+                    let is_bouncy = manifold_point.is_bouncy() as u32 as Real;
+                    let rhs = (1.0 + is_bouncy * manifold_point.restitution)
+                        * (vel1 - vel2).dot(&force_dir1)
+                        + manifold_point.dist.max(0.0) * inv_dt;
 
-                    if rhs <= -params.restitution_velocity_threshold {
-                        rhs += manifold_point.restitution * rhs
-                    }
-
-                    rhs += manifold_point.dist.max(0.0) * inv_dt;
-
-                    let impulse = manifold_points[k].data.impulse * warmstart_coeff;
+                    let impulse = manifold_point.data.impulse * warmstart_coeff;
 
                     constraint.elements[k].normal_part = VelocityGroundConstraintElementPart {
                         gcross2,
diff --git a/src/dynamics/solver/velocity_ground_constraint_wide.rs b/src/dynamics/solver/velocity_ground_constraint_wide.rs
index c2d2c4f..29ba5e9 100644
--- a/src/dynamics/solver/velocity_ground_constraint_wide.rs
+++ b/src/dynamics/solver/velocity_ground_constraint_wide.rs
@@ -95,8 +95,6 @@ impl WVelocityGroundConstraint {
 
         let mj_lambda2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH];
 
-        let restitution_velocity_threshold = SimdReal::splat(params.restitution_velocity_threshold);
-
         let warmstart_multiplier =
             SimdReal::from(array![|ii| manifolds[ii].data.warmstart_multiplier; SIMD_WIDTH]);
         let warmstart_coeff = warmstart_multiplier * SimdReal::splat(params.warmstart_coeff);
@@ -122,6 +120,9 @@ impl WVelocityGroundConstraint {
                     SimdReal::from(array![|ii| manifold_points[ii][k].friction; SIMD_WIDTH]);
                 let restitution =
                     SimdReal::from(array![|ii| manifold_points[ii][k].restitution; SIMD_WIDTH]);
+                let is_bouncy = SimdReal::from(
+                    array![|ii| manifold_points[ii][k].is_bouncy() as u32 as Real; SIMD_WIDTH],
+                );
                 let point = Point::from(array![|ii| manifold_points[ii][k].point; SIMD_WIDTH]);
                 let dist = SimdReal::from(array![|ii| manifold_points[ii][k].dist; SIMD_WIDTH]);
 
@@ -140,11 +141,9 @@ impl WVelocityGroundConstraint {
                     let gcross2 = ii2.transform_vector(dp2.gcross(-force_dir1));
 
                     let r = SimdReal::splat(1.0) / (im2 + gcross2.gdot(gcross2));
-                    let mut rhs = (vel1 - vel2).dot(&force_dir1);
-                    let use_restitution = rhs.simd_le(-restitution_velocity_threshold);
-                    let rhs_with_restitution = rhs + rhs * restitution;
-                    rhs = rhs_with_restitution.select(use_restitution, rhs);
-                    rhs += dist.simd_max(SimdReal::zero()) * inv_dt;
+                    let projected_velocity = (vel1 - vel2).dot(&force_dir1);
+                    let rhs = (SimdReal::splat(1.0) + is_bouncy * restitution) * projected_velocity
+                        + dist.simd_max(SimdReal::zero()) * inv_dt;
 
                     constraint.elements[k].normal_part = WVelocityGroundConstraintElementPart {
                         gcross2,
diff --git a/src/geometry/contact_pair.rs b/src/geometry/contact_pair.rs
index 47f6678..462d3ef 100644
--- a/src/geometry/contact_pair.rs
+++ b/src/geometry/contact_pair.rs
@@ -128,6 +128,22 @@ pub struct SolverContact {
     pub data: ContactData,
 }
 
+impl SolverContact {
+    /// Should we treat this contact as a bouncy contact?
+    /// If `true`, use [`Self::restitution`].
+    pub fn is_bouncy(&self) -> bool {
+        let is_new = self.data.impulse == 0.0;
+        if is_new {
+            // Treat new collisions as bouncing at first, unless we have zero restitution.
+            self.restitution > 0.0
+        } else {
+            // If the contact is still here one step later, it is now a resting contact.
+            // The exception is very high restitutions, which can never rest
+            self.restitution >= 1.0
+        }
+    }
+}
+
 impl Default for ContactManifoldData {
     fn default() -> Self {
         Self::new(