From 09af4313fa650664792d43def4dc8cda4efa9731 Mon Sep 17 00:00:00 2001 From: Sébastien Crozet Date: Sat, 23 Mar 2024 16:00:19 +0100 Subject: Fix reported contact impulse in the contact force event Address https://github.com/dimforge/rapier.js/issues/261 --- .../generic_one_body_constraint.rs | 2 +- .../generic_two_body_constraint.rs | 2 +- .../contact_constraint/one_body_constraint.rs | 14 +++++----- .../one_body_constraint_element.rs | 30 ++++++++++++++++++---- .../contact_constraint/one_body_constraint_simd.rs | 14 +++++----- .../contact_constraint/two_body_constraint.rs | 14 +++++----- .../two_body_constraint_element.rs | 30 ++++++++++++++++++---- .../contact_constraint/two_body_constraint_simd.rs | 14 +++++----- 8 files changed, 80 insertions(+), 40 deletions(-) (limited to 'src/dynamics') 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 { #[cfg(feature = "dim3")] pub impulse: na::Vector2, #[cfg(feature = "dim2")] - pub total_impulse: na::Vector1, + pub impulse_accumulator: na::Vector1, #[cfg(feature = "dim3")] - pub total_impulse: na::Vector2, + pub impulse_accumulator: na::Vector2, #[cfg(feature = "dim2")] pub r: [N; 1], #[cfg(feature = "dim3")] @@ -28,7 +28,7 @@ impl OneBodyConstraintTangentPart { 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 OneBodyConstraintTangentPart { } } + /// Total impulse applied across all the solver substeps. + #[inline] + #[cfg(feature = "dim2")] + pub fn total_impulse(&self) -> na::Vector1 { + self.impulse_accumulator + self.impulse + } + + /// Total impulse applied across all the solver substeps. + #[inline] + #[cfg(feature = "dim3")] + pub fn total_impulse(&self) -> na::Vector2 { + self.impulse_accumulator + self.impulse + } + #[inline] pub fn apply_limit( &mut self, @@ -137,7 +151,7 @@ pub(crate) struct OneBodyConstraintNormalPart { 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 OneBodyConstraintNormalPart { 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 { #[cfg(feature = "dim3")] pub impulse: na::Vector2, #[cfg(feature = "dim2")] - pub total_impulse: na::Vector1, + pub impulse_accumulator: na::Vector1, #[cfg(feature = "dim3")] - pub total_impulse: na::Vector2, + pub impulse_accumulator: na::Vector2, #[cfg(feature = "dim2")] pub r: [N; 1], #[cfg(feature = "dim3")] @@ -30,7 +30,7 @@ impl TwoBodyConstraintTangentPart { 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 TwoBodyConstraintTangentPart { } } + /// Total impulse applied across all the solver substeps. + #[inline] + #[cfg(feature = "dim2")] + pub fn total_impulse(&self) -> na::Vector1 { + self.impulse_accumulator + self.impulse + } + + /// Total impulse applied across all the solver substeps. + #[inline] + #[cfg(feature = "dim3")] + pub fn total_impulse(&self) -> na::Vector2 { + self.impulse_accumulator + self.impulse + } + #[inline] pub fn apply_limit( &mut self, @@ -166,7 +180,7 @@ pub(crate) struct TwoBodyConstraintNormalPart { 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 TwoBodyConstraintNormalPart { 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]]; -- cgit