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

Attenuate the warmstart impulse for CCD contacts.

Browse Source 
CCD contacts result in very strong, instantaneous, impulses. So it is preferable to attenuate their contribution to subsequent timesteps to avoid overshooting.
This commit is contained in:
Crozet Sébastien
2021-03-28 11:26:53 +02:00
parent 710dd8d71e
commit 7306821c46
10 changed files with 114 additions and 62 deletions
Download Patch File Download Diff File Expand all files Collapse all files

27
src/dynamics/solver/velocity_constraint_wide.rs
View File

@@ -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")]