Attenuate the warmstart impulse for CCD contacts.

CCD contacts result in very strong, instantaneous, impulses. So it is preferable to attenuate their contribution to subsequent timesteps to avoid overshooting.

4 files changed, 64 insertions, 22 deletions

diff --git a/src/dynamics/solver/velocity_constraint.rs b/src/dynamics/solver/velocity_constraint.rs
index 3e8cb61..c339ce4 100644
--- a/src/dynamics/solver/velocity_constraint.rs
+++ b/src/dynamics/solver/velocity_constraint.rs
@@ -208,6 +208,8 @@ impl VelocityConstraint {
                 let vel1 = rb1.linvel + rb1.angvel.gcross(dp1);
                 let vel2 = rb2.linvel + rb2.angvel.gcross(dp2);
 
+                let warmstart_correction;
+
                 constraint.limit = manifold_point.friction;
                 constraint.manifold_contact_id[k] = manifold_point.contact_id;
 
@@ -234,12 +236,15 @@ impl VelocityConstraint {
                     rhs += manifold_point.dist.max(0.0) * inv_dt;
                     rhs *= is_bouncy + is_resting * params.velocity_solve_fraction;
                     rhs += is_resting * velocity_based_erp_inv_dt * manifold_point.dist.min(0.0);
+                    warmstart_correction = (params.warmstart_correction_slope
+                        / (rhs - manifold_point.prev_rhs).abs())
+                    .min(warmstart_coeff);
 
                     constraint.elements[k].normal_part = VelocityConstraintNormalPart {
                         gcross1,
                         gcross2,
                         rhs,
-                        impulse: manifold_point.data.impulse * warmstart_coeff,
+                        impulse: manifold_point.warmstart_impulse * warmstart_correction,
                         r,
                     };
                 }
@@ -247,10 +252,12 @@ impl VelocityConstraint {
                 // Tangent parts.
                 {
                     #[cfg(feature = "dim3")]
-                    let impulse =
-                        tangent_rot1 * manifold_points[k].data.tangent_impulse * warmstart_coeff;
+                    let impulse = tangent_rot1
+                        * manifold_points[k].warmstart_tangent_impulse
+                        * warmstart_correction;
                     #[cfg(feature = "dim2")]
-                    let impulse = [manifold_points[k].data.tangent_impulse * warmstart_coeff];
+                    let impulse =
+                        [manifold_points[k].warmstart_tangent_impulse * warmstart_correction];
                     constraint.elements[k].tangent_part.impulse = impulse;
 
                     for j in 0..DIM - 1 {
@@ -332,6 +339,8 @@ impl VelocityConstraint {
             let contact_id = self.manifold_contact_id[k];
             let active_contact = &mut manifold.points[contact_id as usize];
             active_contact.data.impulse = self.elements[k].normal_part.impulse;
+            active_contact.data.rhs = self.elements[k].normal_part.rhs;
+
             #[cfg(feature = "dim2")]
             {
                 active_contact.data.tangent_impulse = self.elements[k].tangent_part.impulse[0];
diff --git a/src/dynamics/solver/velocity_constraint_wide.rs b/src/dynamics/solver/velocity_constraint_wide.rs
index 673af54..7d5f468 100644
--- a/src/dynamics/solver/velocity_constraint_wide.rs
+++ b/src/dynamics/solver/velocity_constraint_wide.rs
@@ -9,6 +9,7 @@ use crate::math::{
 #[cfg(feature = "dim2")]
 use crate::utils::WBasis;
 use crate::utils::{WAngularInertia, WCross, WDot};
+use na::SimdComplexField;
 use num::Zero;
 use simba::simd::{SimdPartialOrd, SimdValue};
 
@@ -44,6 +45,7 @@ impl WVelocityConstraint {
         }
 
         let inv_dt = SimdReal::splat(params.inv_dt());
+        let warmstart_correction_slope = SimdReal::splat(params.warmstart_correction_slope);
         let velocity_solve_fraction = SimdReal::splat(params.velocity_solve_fraction);
         let velocity_based_erp_inv_dt = SimdReal::splat(params.velocity_based_erp_inv_dt());
 
@@ -123,8 +125,11 @@ impl WVelocityConstraint {
                 let tangent_velocity =
                     Vector::from(array![|ii| manifold_points[ii][k].tangent_velocity; SIMD_WIDTH]);
 
-                let impulse =
-                    SimdReal::from(array![|ii| manifold_points[ii][k].data.impulse; SIMD_WIDTH]);
+                let impulse = SimdReal::from(
+                    array![|ii| manifold_points[ii][k].warmstart_impulse; SIMD_WIDTH],
+                );
+                let prev_rhs =
+                    SimdReal::from(array![|ii| manifold_points[ii][k].prev_rhs; SIMD_WIDTH]);
 
                 let dp1 = point - world_com1;
                 let dp2 = point - world_com2;
@@ -132,6 +137,8 @@ impl WVelocityConstraint {
                 let vel1 = linvel1 + angvel1.gcross(dp1);
                 let vel2 = linvel2 + angvel2.gcross(dp2);
 
+                let warmstart_correction;
+
                 constraint.limit = friction;
                 constraint.manifold_contact_id[k] =
                     array![|ii| manifold_points[ii][k].contact_id; SIMD_WIDTH];
@@ -150,12 +157,15 @@ impl WVelocityConstraint {
                     rhs *= is_bouncy + is_resting * velocity_solve_fraction;
                     rhs +=
                         dist.simd_min(SimdReal::zero()) * (velocity_based_erp_inv_dt * is_resting);
+                    warmstart_correction = (warmstart_correction_slope
+                        / (rhs - prev_rhs).simd_abs())
+                    .simd_min(warmstart_coeff);
 
                     constraint.elements[k].normal_part = VelocityConstraintNormalPart {
                         gcross1,
                         gcross2,
                         rhs,
-                        impulse: impulse * warmstart_coeff,
+                        impulse: impulse * warmstart_correction,
                         r,
                     };
                 }
@@ -163,14 +173,15 @@ impl WVelocityConstraint {
                 // tangent parts.
                 #[cfg(feature = "dim2")]
                 let impulse = [SimdReal::from(
-                    array![|ii| manifold_points[ii][k].data.tangent_impulse; SIMD_WIDTH],
-                )];
+                    array![|ii| manifold_points[ii][k].warmstart_tangent_impulse; SIMD_WIDTH],
+                ) * warmstart_correction];
 
                 #[cfg(feature = "dim3")]
                 let impulse = tangent_rot1
                     * na::Vector2::from(
-                        array![|ii| manifold_points[ii][k].data.tangent_impulse; SIMD_WIDTH],
-                    );
+                        array![|ii| manifold_points[ii][k].warmstart_tangent_impulse; SIMD_WIDTH],
+                    )
+                    * warmstart_correction;
 
                 constraint.elements[k].tangent_part.impulse = impulse;
 
@@ -281,6 +292,7 @@ impl WVelocityConstraint {
     pub fn writeback_impulses(&self, manifolds_all: &mut [&mut ContactManifold]) {
         for k in 0..self.num_contacts as usize {
             let impulses: [_; SIMD_WIDTH] = self.elements[k].normal_part.impulse.into();
+            let rhs: [_; SIMD_WIDTH] = self.elements[k].normal_part.rhs.into();
             #[cfg(feature = "dim2")]
             let tangent_impulses: [_; SIMD_WIDTH] = self.elements[k].tangent_part.impulse[0].into();
             #[cfg(feature = "dim3")]
@@ -292,6 +304,7 @@ impl WVelocityConstraint {
                 let manifold = &mut manifolds_all[self.manifold_id[ii]];
                 let contact_id = self.manifold_contact_id[k][ii];
                 let active_contact = &mut manifold.points[contact_id as usize];
+                active_contact.data.rhs = rhs[ii];
                 active_contact.data.impulse = impulses[ii];
 
                 #[cfg(feature = "dim2")]
diff --git a/src/dynamics/solver/velocity_ground_constraint.rs b/src/dynamics/solver/velocity_ground_constraint.rs
index b9c5236..0e195d5 100644
--- a/src/dynamics/solver/velocity_ground_constraint.rs
+++ b/src/dynamics/solver/velocity_ground_constraint.rs
@@ -133,6 +133,7 @@ impl VelocityGroundConstraint {
                 let dp1 = manifold_point.point - rb1.world_com;
                 let vel1 = rb1.linvel + rb1.angvel.gcross(dp1);
                 let vel2 = rb2.linvel + rb2.angvel.gcross(dp2);
+                let warmstart_correction;
 
                 constraint.limit = manifold_point.friction;
                 constraint.manifold_contact_id[k] = manifold_point.contact_id;
@@ -153,11 +154,14 @@ impl VelocityGroundConstraint {
                     rhs += manifold_point.dist.max(0.0) * inv_dt;
                     rhs *= is_bouncy + is_resting * params.velocity_solve_fraction;
                     rhs += is_resting * velocity_based_erp_inv_dt * manifold_point.dist.min(0.0);
+                    warmstart_correction = (params.warmstart_correction_slope
+                        / (rhs - manifold_point.prev_rhs).abs())
+                    .min(warmstart_coeff);
 
                     constraint.elements[k].normal_part = VelocityGroundConstraintNormalPart {
                         gcross2,
                         rhs,
-                        impulse: manifold_point.data.impulse * warmstart_coeff,
+                        impulse: manifold_point.warmstart_impulse * warmstart_correction,
                         r,
                     };
                 }
@@ -165,10 +169,12 @@ impl VelocityGroundConstraint {
                 // Tangent parts.
                 {
                     #[cfg(feature = "dim3")]
-                    let impulse =
-                        tangent_rot1 * manifold_points[k].data.tangent_impulse * warmstart_coeff;
+                    let impulse = tangent_rot1
+                        * manifold_points[k].warmstart_tangent_impulse
+                        * warmstart_correction;
                     #[cfg(feature = "dim2")]
-                    let impulse = [manifold_points[k].data.tangent_impulse * warmstart_coeff];
+                    let impulse =
+                        [manifold_points[k].warmstart_tangent_impulse * warmstart_correction];
                     constraint.elements[k].tangent_part.impulse = impulse;
 
                     for j in 0..DIM - 1 {
@@ -237,6 +243,8 @@ impl VelocityGroundConstraint {
             let contact_id = self.manifold_contact_id[k];
             let active_contact = &mut manifold.points[contact_id as usize];
             active_contact.data.impulse = self.elements[k].normal_part.impulse;
+            active_contact.data.rhs = self.elements[k].normal_part.rhs;
+
             #[cfg(feature = "dim2")]
             {
                 active_contact.data.tangent_impulse = self.elements[k].tangent_part.impulse[0];
diff --git a/src/dynamics/solver/velocity_ground_constraint_wide.rs b/src/dynamics/solver/velocity_ground_constraint_wide.rs
index ba1c46a..4237d99 100644
--- a/src/dynamics/solver/velocity_ground_constraint_wide.rs
+++ b/src/dynamics/solver/velocity_ground_constraint_wide.rs
@@ -10,6 +10,7 @@ use crate::math::{
 #[cfg(feature = "dim2")]
 use crate::utils::WBasis;
 use crate::utils::{WAngularInertia, WCross, WDot};
+use na::SimdComplexField;
 use num::Zero;
 use simba::simd::{SimdPartialOrd, SimdValue};
 
@@ -77,6 +78,7 @@ impl WVelocityGroundConstraint {
         let warmstart_multiplier =
             SimdReal::from(array![|ii| manifolds[ii].data.warmstart_multiplier; SIMD_WIDTH]);
         let warmstart_coeff = warmstart_multiplier * SimdReal::splat(params.warmstart_coeff);
+        let warmstart_correction_slope = SimdReal::splat(params.warmstart_correction_slope);
         let num_active_contacts = manifolds[0].data.num_active_contacts();
 
         #[cfg(feature = "dim2")]
@@ -118,13 +120,17 @@ impl WVelocityGroundConstraint {
                 let tangent_velocity =
                     Vector::from(array![|ii| manifold_points[ii][k].tangent_velocity; SIMD_WIDTH]);
 
-                let impulse =
-                    SimdReal::from(array![|ii| manifold_points[ii][k].data.impulse; SIMD_WIDTH]);
+                let impulse = SimdReal::from(
+                    array![|ii| manifold_points[ii][k].warmstart_impulse; SIMD_WIDTH],
+                );
+                let prev_rhs =
+                    SimdReal::from(array![|ii| manifold_points[ii][k].prev_rhs; SIMD_WIDTH]);
                 let dp1 = point - world_com1;
                 let dp2 = point - world_com2;
 
                 let vel1 = linvel1 + angvel1.gcross(dp1);
                 let vel2 = linvel2 + angvel2.gcross(dp2);
+                let warmstart_correction;
 
                 constraint.limit = friction;
                 constraint.manifold_contact_id[k] =
@@ -142,11 +148,14 @@ impl WVelocityGroundConstraint {
                     rhs *= is_bouncy + is_resting * velocity_solve_fraction;
                     rhs +=
                         dist.simd_min(SimdReal::zero()) * (velocity_based_erp_inv_dt * is_resting);
+                    warmstart_correction = (warmstart_correction_slope
+                        / (rhs - prev_rhs).simd_abs())
+                    .simd_min(warmstart_coeff);
 
                     constraint.elements[k].normal_part = VelocityGroundConstraintNormalPart {
                         gcross2,
                         rhs,
-                        impulse: impulse * warmstart_coeff,
+                        impulse: impulse * warmstart_correction,
                         r,
                     };
                 }
@@ -154,13 +163,14 @@ impl WVelocityGroundConstraint {
                 // tangent parts.
                 #[cfg(feature = "dim2")]
                 let impulse = [SimdReal::from(
-                    array![|ii| manifold_points[ii][k].data.tangent_impulse; SIMD_WIDTH],
-                )];
+                    array![|ii| manifold_points[ii][k].warmstart_tangent_impulse; SIMD_WIDTH],
+                ) * warmstart_correction];
                 #[cfg(feature = "dim3")]
                 let impulse = tangent_rot1
                     * na::Vector2::from(
-                        array![|ii| manifold_points[ii][k].data.tangent_impulse; SIMD_WIDTH],
-                    );
+                        array![|ii| manifold_points[ii][k].warmstart_tangent_impulse; SIMD_WIDTH],
+                    )
+                    * warmstart_correction;
                 constraint.elements[k].tangent_part.impulse = impulse;
 
                 for j in 0..DIM - 1 {
@@ -237,6 +247,7 @@ impl WVelocityGroundConstraint {
     // FIXME: duplicated code. This is exactly the same as in the non-ground velocity constraint.
     pub fn writeback_impulses(&self, manifolds_all: &mut [&mut ContactManifold]) {
         for k in 0..self.num_contacts as usize {
+            let rhs: [_; SIMD_WIDTH] = self.elements[k].normal_part.rhs.into();
             let impulses: [_; SIMD_WIDTH] = self.elements[k].normal_part.impulse.into();
             #[cfg(feature = "dim2")]
             let tangent_impulses: [_; SIMD_WIDTH] = self.elements[k].tangent_part.impulse[0].into();
@@ -249,6 +260,7 @@ impl WVelocityGroundConstraint {
                 let manifold = &mut manifolds_all[self.manifold_id[ii]];
                 let contact_id = self.manifold_contact_id[k][ii];
                 let active_contact = &mut manifold.points[contact_id as usize];
+                active_contact.data.rhs = rhs[ii];
                 active_contact.data.impulse = impulses[ii];
 
                 #[cfg(feature = "dim2")]