nichkara/rapier
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Fix reported contact impulse in the contact force event

Browse Source 
Address https://github.com/dimforge/rapier.js/issues/261
This commit is contained in:
Sébastien Crozet
2024-03-23 16:00:19 +01:00
committed by Sébastien Crozet
parent 59dc9bfe00
commit 09af4313fa
8 changed files with 80 additions and 40 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
src/dynamics/solver/contact_constraint/generic_one_body_constraint.rs
View File

@@ -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,
};
}

2
src/dynamics/solver/contact_constraint/generic_two_body_constraint.rs
View File

@@ -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,
};

14
src/dynamics/solver/contact_constraint/one_body_constraint.rs
View File

@@ -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();
}
}
}

30
src/dynamics/solver/contact_constraint/one_body_constraint_element.rs
View File

@@ -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,

14
src/dynamics/solver/contact_constraint/one_body_constraint_simd.rs
View File

@@ -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]];

14
src/dynamics/solver/contact_constraint/two_body_constraint.rs
View File

@@ -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();
}
}
}

30
src/dynamics/solver/contact_constraint/two_body_constraint_element.rs
View File

@@ -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,

14
src/dynamics/solver/contact_constraint/two_body_constraint_simd.rs
View File

@@ -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]];