From 7306821c460ca3f77e697c89a79393e61c126624 Mon Sep 17 00:00:00 2001 From: Crozet Sébastien Date: Sun, 28 Mar 2021 11:26:53 +0200 Subject: 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. --- src/dynamics/solver/velocity_constraint.rs | 17 ++++++++++---- src/dynamics/solver/velocity_constraint_wide.rs | 27 ++++++++++++++++------ src/dynamics/solver/velocity_ground_constraint.rs | 16 +++++++++---- .../solver/velocity_ground_constraint_wide.rs | 26 +++++++++++++++------ 4 files changed, 64 insertions(+), 22 deletions(-) (limited to 'src/dynamics/solver') 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")] -- cgit