Fix reported contact impulse in the contact force event

Address https://github.com/dimforge/rapier.js/issues/261

8 files changed, 80 insertions, 40 deletions

diff --git a/src/dynamics/solver/contact_constraint/generic_one_body_constraint.rs b/src/dynamics/solver/contact_constraint/generic_one_body_constraint.rs
index 7b1f8ea..cfdff55 100644
--- a/src/dynamics/solver/contact_constraint/generic_one_body_constraint.rs
+++ b/src/dynamics/solver/contact_constraint/generic_one_body_constraint.rs
@@ -153,7 +153,7 @@ impl GenericOneBodyConstraintBuilder {
                         rhs: na::zero(),
                         rhs_wo_bias: na::zero(),
                         impulse: na::zero(),
-                        total_impulse: na::zero(),
+                        impulse_accumulator: na::zero(),
                         r,
                     };
                 }
diff --git a/src/dynamics/solver/contact_constraint/generic_two_body_constraint.rs b/src/dynamics/solver/contact_constraint/generic_two_body_constraint.rs
index 073f585..f0a7d64 100644
--- a/src/dynamics/solver/contact_constraint/generic_two_body_constraint.rs
+++ b/src/dynamics/solver/contact_constraint/generic_two_body_constraint.rs
@@ -201,7 +201,7 @@ impl GenericTwoBodyConstraintBuilder {
                         gcross2,
                         rhs: na::zero(),
                         rhs_wo_bias: na::zero(),
-                        total_impulse: na::zero(),
+                        impulse_accumulator: na::zero(),
                         impulse: na::zero(),
                         r,
                     };
diff --git a/src/dynamics/solver/contact_constraint/one_body_constraint.rs b/src/dynamics/solver/contact_constraint/one_body_constraint.rs
index be108a4..c9b35f3 100644
--- a/src/dynamics/solver/contact_constraint/one_body_constraint.rs
+++ b/src/dynamics/solver/contact_constraint/one_body_constraint.rs
@@ -149,7 +149,7 @@ impl OneBodyConstraintBuilder {
                         rhs: na::zero(),
                         rhs_wo_bias: na::zero(),
                         impulse: na::zero(),
-                        total_impulse: na::zero(),
+                        impulse_accumulator: na::zero(),
                         r: projected_mass,
                     };
                 }
@@ -270,18 +270,17 @@ impl OneBodyConstraintBuilder {
                     * (dist + params.allowed_linear_error)
                         .clamp(-params.max_penetration_correction, 0.0);
                 let new_rhs = rhs_wo_bias + rhs_bias;
-                let total_impulse = element.normal_part.total_impulse + element.normal_part.impulse;
                 is_fast_contact = is_fast_contact || (-new_rhs * params.dt > ccd_thickness * 0.5);
 
                 element.normal_part.rhs_wo_bias = rhs_wo_bias;
                 element.normal_part.rhs = new_rhs;
-                element.normal_part.total_impulse = total_impulse;
+                element.normal_part.impulse_accumulator += element.normal_part.impulse;
                 element.normal_part.impulse = na::zero();
             }
 
             // Tangent part.
             {
-                element.tangent_part.total_impulse += element.tangent_part.impulse;
+                element.tangent_part.impulse_accumulator += element.tangent_part.impulse;
                 element.tangent_part.impulse = na::zero();
 
                 for j in 0..DIM - 1 {
@@ -359,15 +358,16 @@ impl OneBodyConstraint {
         for k in 0..self.num_contacts as usize {
             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.impulse = self.elements[k].normal_part.total_impulse();
 
             #[cfg(feature = "dim2")]
             {
-                active_contact.data.tangent_impulse = self.elements[k].tangent_part.impulse[0];
+                active_contact.data.tangent_impulse =
+                    self.elements[k].tangent_part.total_impulse()[0];
             }
             #[cfg(feature = "dim3")]
             {
-                active_contact.data.tangent_impulse = self.elements[k].tangent_part.impulse;
+                active_contact.data.tangent_impulse = self.elements[k].tangent_part.total_impulse();
             }
         }
     }
diff --git a/src/dynamics/solver/contact_constraint/one_body_constraint_element.rs b/src/dynamics/solver/contact_constraint/one_body_constraint_element.rs
index d9ff7f4..79b207b 100644
--- a/src/dynamics/solver/contact_constraint/one_body_constraint_element.rs
+++ b/src/dynamics/solver/contact_constraint/one_body_constraint_element.rs
@@ -12,9 +12,9 @@ pub(crate) struct OneBodyConstraintTangentPart<N: SimdRealCopy> {
     #[cfg(feature = "dim3")]
     pub impulse: na::Vector2<N>,
     #[cfg(feature = "dim2")]
-    pub total_impulse: na::Vector1<N>,
+    pub impulse_accumulator: na::Vector1<N>,
     #[cfg(feature = "dim3")]
-    pub total_impulse: na::Vector2<N>,
+    pub impulse_accumulator: na::Vector2<N>,
     #[cfg(feature = "dim2")]
     pub r: [N; 1],
     #[cfg(feature = "dim3")]
@@ -28,7 +28,7 @@ impl<N: SimdRealCopy> OneBodyConstraintTangentPart<N> {
             rhs: [na::zero(); DIM - 1],
             rhs_wo_bias: [na::zero(); DIM - 1],
             impulse: na::zero(),
-            total_impulse: na::zero(),
+            impulse_accumulator: na::zero(),
             #[cfg(feature = "dim2")]
             r: [na::zero(); 1],
             #[cfg(feature = "dim3")]
@@ -36,6 +36,20 @@ impl<N: SimdRealCopy> OneBodyConstraintTangentPart<N> {
         }
     }
 
+    /// Total impulse applied across all the solver substeps.
+    #[inline]
+    #[cfg(feature = "dim2")]
+    pub fn total_impulse(&self) -> na::Vector1<N> {
+        self.impulse_accumulator + self.impulse
+    }
+
+    /// Total impulse applied across all the solver substeps.
+    #[inline]
+    #[cfg(feature = "dim3")]
+    pub fn total_impulse(&self) -> na::Vector2<N> {
+        self.impulse_accumulator + self.impulse
+    }
+
     #[inline]
     pub fn apply_limit(
         &mut self,
@@ -137,7 +151,7 @@ pub(crate) struct OneBodyConstraintNormalPart<N: SimdRealCopy> {
     pub rhs: N,
     pub rhs_wo_bias: N,
     pub impulse: N,
-    pub total_impulse: N,
+    pub impulse_accumulator: N,
     pub r: N,
 }
 
@@ -148,11 +162,17 @@ impl<N: SimdRealCopy> OneBodyConstraintNormalPart<N> {
             rhs: na::zero(),
             rhs_wo_bias: na::zero(),
             impulse: na::zero(),
-            total_impulse: na::zero(),
+            impulse_accumulator: na::zero(),
             r: na::zero(),
         }
     }
 
+    /// Total impulse applied across all the solver substeps.
+    #[inline]
+    pub fn total_impulse(&self) -> N {
+        self.impulse_accumulator + self.impulse
+    }
+
     #[inline]
     pub fn solve(
         &mut self,
diff --git a/src/dynamics/solver/contact_constraint/one_body_constraint_simd.rs b/src/dynamics/solver/contact_constraint/one_body_constraint_simd.rs
index cd2ea56..03c1abe 100644
--- a/src/dynamics/solver/contact_constraint/one_body_constraint_simd.rs
+++ b/src/dynamics/solver/contact_constraint/one_body_constraint_simd.rs
@@ -154,7 +154,7 @@ impl SimdOneBodyConstraintBuilder {
                         rhs: na::zero(),
                         rhs_wo_bias: na::zero(),
                         impulse: na::zero(),
-                        total_impulse: na::zero(),
+                        impulse_accumulator: na::zero(),
                         r: projected_mass,
                     };
                 }
@@ -259,19 +259,18 @@ impl SimdOneBodyConstraintBuilder {
                     .simd_clamp(-max_penetration_correction, SimdReal::zero())
                     * erp_inv_dt;
                 let new_rhs = rhs_wo_bias + rhs_bias;
-                let total_impulse = element.normal_part.total_impulse + element.normal_part.impulse;
                 is_fast_contact =
                     is_fast_contact | (-new_rhs * dt).simd_gt(ccd_thickness * SimdReal::splat(0.5));
 
                 element.normal_part.rhs_wo_bias = rhs_wo_bias;
                 element.normal_part.rhs = new_rhs;
-                element.normal_part.total_impulse = total_impulse;
+                element.normal_part.impulse_accumulator += element.normal_part.impulse;
                 element.normal_part.impulse = na::zero();
             }
 
             // tangent parts.
             {
-                element.tangent_part.total_impulse += element.tangent_part.impulse;
+                element.tangent_part.impulse_accumulator += element.tangent_part.impulse;
                 element.tangent_part.impulse = na::zero();
 
                 for j in 0..DIM - 1 {
@@ -334,11 +333,12 @@ impl OneBodyConstraintSimd {
     // FIXME: duplicated code. This is exactly the same as in the two-body velocity constraint.
     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 impulses: [_; SIMD_WIDTH] = self.elements[k].normal_part.total_impulse().into();
             #[cfg(feature = "dim2")]
-            let tangent_impulses: [_; SIMD_WIDTH] = self.elements[k].tangent_part.impulse[0].into();
+            let tangent_impulses: [_; SIMD_WIDTH] =
+                self.elements[k].tangent_part.total_impulse()[0].into();
             #[cfg(feature = "dim3")]
-            let tangent_impulses = self.elements[k].tangent_part.impulse;
+            let tangent_impulses = self.elements[k].tangent_part.total_impulse();
 
             for ii in 0..SIMD_WIDTH {
                 let manifold = &mut manifolds_all[self.manifold_id[ii]];
diff --git a/src/dynamics/solver/contact_constraint/two_body_constraint.rs b/src/dynamics/solver/contact_constraint/two_body_constraint.rs
index 0a0ebd6..8abf5c5 100644
--- a/src/dynamics/solver/contact_constraint/two_body_constraint.rs
+++ b/src/dynamics/solver/contact_constraint/two_body_constraint.rs
@@ -223,7 +223,7 @@ impl TwoBodyConstraintBuilder {
                         rhs: na::zero(),
                         rhs_wo_bias: na::zero(),
                         impulse: na::zero(),
-                        total_impulse: na::zero(),
+                        impulse_accumulator: na::zero(),
                         r: projected_mass,
                     };
                 }
@@ -348,18 +348,17 @@ impl TwoBodyConstraintBuilder {
                     * (dist + params.allowed_linear_error)
                         .clamp(-params.max_penetration_correction, 0.0);
                 let new_rhs = rhs_wo_bias + rhs_bias;
-                let total_impulse = element.normal_part.total_impulse + element.normal_part.impulse;
                 is_fast_contact = is_fast_contact || (-new_rhs * params.dt > ccd_thickness * 0.5);
 
                 element.normal_part.rhs_wo_bias = rhs_wo_bias;
                 element.normal_part.rhs = new_rhs;
-                element.normal_part.total_impulse = total_impulse;
+                element.normal_part.impulse_accumulator += element.normal_part.impulse;
                 element.normal_part.impulse = na::zero();
             }
 
             // Tangent part.
             {
-                element.tangent_part.total_impulse += element.tangent_part.impulse;
+                element.tangent_part.impulse_accumulator += element.tangent_part.impulse;
                 element.tangent_part.impulse = na::zero();
 
                 for j in 0..DIM - 1 {
@@ -408,15 +407,16 @@ impl TwoBodyConstraint {
         for k in 0..self.num_contacts as usize {
             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.impulse = self.elements[k].normal_part.total_impulse();
 
             #[cfg(feature = "dim2")]
             {
-                active_contact.data.tangent_impulse = self.elements[k].tangent_part.impulse[0];
+                active_contact.data.tangent_impulse =
+                    self.elements[k].tangent_part.total_impulse()[0];
             }
             #[cfg(feature = "dim3")]
             {
-                active_contact.data.tangent_impulse = self.elements[k].tangent_part.impulse;
+                active_contact.data.tangent_impulse = self.elements[k].tangent_part.total_impulse();
             }
         }
     }
diff --git a/src/dynamics/solver/contact_constraint/two_body_constraint_element.rs b/src/dynamics/solver/contact_constraint/two_body_constraint_element.rs
index 8c720b9..75fc739 100644
--- a/src/dynamics/solver/contact_constraint/two_body_constraint_element.rs
+++ b/src/dynamics/solver/contact_constraint/two_body_constraint_element.rs
@@ -13,9 +13,9 @@ pub(crate) struct TwoBodyConstraintTangentPart<N: SimdRealCopy> {
     #[cfg(feature = "dim3")]
     pub impulse: na::Vector2<N>,
     #[cfg(feature = "dim2")]
-    pub total_impulse: na::Vector1<N>,
+    pub impulse_accumulator: na::Vector1<N>,
     #[cfg(feature = "dim3")]
-    pub total_impulse: na::Vector2<N>,
+    pub impulse_accumulator: na::Vector2<N>,
     #[cfg(feature = "dim2")]
     pub r: [N; 1],
     #[cfg(feature = "dim3")]
@@ -30,7 +30,7 @@ impl<N: SimdRealCopy> TwoBodyConstraintTangentPart<N> {
             rhs: [na::zero(); DIM - 1],
             rhs_wo_bias: [na::zero(); DIM - 1],
             impulse: na::zero(),
-            total_impulse: na::zero(),
+            impulse_accumulator: na::zero(),
             #[cfg(feature = "dim2")]
             r: [na::zero(); 1],
             #[cfg(feature = "dim3")]
@@ -38,6 +38,20 @@ impl<N: SimdRealCopy> TwoBodyConstraintTangentPart<N> {
         }
     }
 
+    /// Total impulse applied across all the solver substeps.
+    #[inline]
+    #[cfg(feature = "dim2")]
+    pub fn total_impulse(&self) -> na::Vector1<N> {
+        self.impulse_accumulator + self.impulse
+    }
+
+    /// Total impulse applied across all the solver substeps.
+    #[inline]
+    #[cfg(feature = "dim3")]
+    pub fn total_impulse(&self) -> na::Vector2<N> {
+        self.impulse_accumulator + self.impulse
+    }
+
     #[inline]
     pub fn apply_limit(
         &mut self,
@@ -166,7 +180,7 @@ pub(crate) struct TwoBodyConstraintNormalPart<N: SimdRealCopy> {
     pub rhs: N,
     pub rhs_wo_bias: N,
     pub impulse: N,
-    pub total_impulse: N,
+    pub impulse_accumulator: N,
     pub r: N,
 }
 
@@ -178,11 +192,17 @@ impl<N: SimdRealCopy> TwoBodyConstraintNormalPart<N> {
             rhs: na::zero(),
             rhs_wo_bias: na::zero(),
             impulse: na::zero(),
-            total_impulse: na::zero(),
+            impulse_accumulator: na::zero(),
             r: na::zero(),
         }
     }
 
+    /// Total impulse applied across all the solver substeps.
+    #[inline]
+    pub fn total_impulse(&self) -> N {
+        self.impulse_accumulator + self.impulse
+    }
+
     #[inline]
     pub fn solve(
         &mut self,
diff --git a/src/dynamics/solver/contact_constraint/two_body_constraint_simd.rs b/src/dynamics/solver/contact_constraint/two_body_constraint_simd.rs
index ee176d6..2de6ee2 100644
--- a/src/dynamics/solver/contact_constraint/two_body_constraint_simd.rs
+++ b/src/dynamics/solver/contact_constraint/two_body_constraint_simd.rs
@@ -138,7 +138,7 @@ impl TwoBodyConstraintBuilderSimd {
                         rhs: na::zero(),
                         rhs_wo_bias: na::zero(),
                         impulse: SimdReal::splat(0.0),
-                        total_impulse: SimdReal::splat(0.0),
+                        impulse_accumulator: SimdReal::splat(0.0),
                         r: projected_mass,
                     };
                 }
@@ -241,19 +241,18 @@ impl TwoBodyConstraintBuilderSimd {
                     .simd_clamp(-max_penetration_correction, SimdReal::zero())
                     * erp_inv_dt;
                 let new_rhs = rhs_wo_bias + rhs_bias;
-                let total_impulse = element.normal_part.total_impulse + element.normal_part.impulse;
                 is_fast_contact =
                     is_fast_contact | (-new_rhs * dt).simd_gt(ccd_thickness * SimdReal::splat(0.5));
 
                 element.normal_part.rhs_wo_bias = rhs_wo_bias;
                 element.normal_part.rhs = new_rhs;
-                element.normal_part.total_impulse = total_impulse;
+                element.normal_part.impulse_accumulator += element.normal_part.impulse;
                 element.normal_part.impulse = na::zero();
             }
 
             // tangent parts.
             {
-                element.tangent_part.total_impulse += element.tangent_part.impulse;
+                element.tangent_part.impulse_accumulator += element.tangent_part.impulse;
                 element.tangent_part.impulse = na::zero();
 
                 for j in 0..DIM - 1 {
@@ -328,11 +327,12 @@ impl TwoBodyConstraintSimd {
 
     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 impulses: [_; SIMD_WIDTH] = self.elements[k].normal_part.total_impulse().into();
             #[cfg(feature = "dim2")]
-            let tangent_impulses: [_; SIMD_WIDTH] = self.elements[k].tangent_part.impulse[0].into();
+            let tangent_impulses: [_; SIMD_WIDTH] =
+                self.elements[k].tangent_part.total_impulse()[0].into();
             #[cfg(feature = "dim3")]
-            let tangent_impulses = self.elements[k].tangent_part.impulse;
+            let tangent_impulses = self.elements[k].tangent_part.total_impulse();
 
             for ii in 0..SIMD_WIDTH {
                 let manifold = &mut manifolds_all[self.manifold_id[ii]];