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/ccd/toi_entry.rs | 39 ++++++++++++---------- src/dynamics/integration_parameters.rs | 3 ++ src/dynamics/rigid_body.rs | 14 +++----- 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 +++++++++++---- src/geometry/contact_pair.rs | 20 ++++++++--- src/geometry/narrow_phase.rs | 4 ++- 9 files changed, 112 insertions(+), 54 deletions(-) (limited to 'src') diff --git a/src/dynamics/ccd/toi_entry.rs b/src/dynamics/ccd/toi_entry.rs index 20f5268..6679a91 100644 --- a/src/dynamics/ccd/toi_entry.rs +++ b/src/dynamics/ccd/toi_entry.rs @@ -1,13 +1,7 @@ -use crate::data::Coarena; -use crate::dynamics::ccd::ccd_solver::CCDContact; -use crate::dynamics::ccd::CCDData; use crate::dynamics::{IntegrationParameters, RigidBody, RigidBodyHandle}; use crate::geometry::{Collider, ColliderHandle}; -use crate::math::{Isometry, Real}; -use crate::parry::query::PersistentQueryDispatcher; -use crate::utils::WCross; -use na::{RealField, Unit}; -use parry::query::{NonlinearRigidMotion, QueryDispatcher, TOI}; +use crate::math::Real; +use parry::query::{NonlinearRigidMotion, QueryDispatcher}; #[derive(Copy, Clone, Debug)] pub struct TOIEntry { @@ -41,7 +35,7 @@ impl TOIEntry { } } - pub fn try_from_colliders>( + pub fn try_from_colliders( params: &IntegrationParameters, query_dispatcher: &QD, ch1: ColliderHandle, @@ -54,7 +48,6 @@ impl TOIEntry { frozen2: Option, start_time: Real, end_time: Real, - body_params: &Coarena, ) -> Option { assert!(start_time <= end_time); @@ -62,7 +55,7 @@ impl TOIEntry { let linvel2 = frozen2.is_none() as u32 as Real * b2.linvel; let vel12 = linvel2 - linvel1; - let thickness = (c1.shape().ccd_thickness() + c2.shape().ccd_thickness()); + let thickness = c1.shape().ccd_thickness() + c2.shape().ccd_thickness(); if params.dt * vel12.norm() < thickness { return None; @@ -70,12 +63,9 @@ impl TOIEntry { let is_intersection_test = c1.is_sensor() || c2.is_sensor(); - let body_params1 = body_params.get(c1.parent.0)?; - let body_params2 = body_params.get(c2.parent.0)?; - // Compute the TOI. - let mut motion1 = body_params1.motion(params.dt, b1, 0.0); - let mut motion2 = body_params2.motion(params.dt, b2, 0.0); + let mut motion1 = Self::body_motion(params.dt, b1); + let mut motion2 = Self::body_motion(params.dt, b2); if let Some(t) = frozen1 { motion1.freeze(t); @@ -85,7 +75,6 @@ impl TOIEntry { motion2.freeze(t); } - let mut toi; let motion_c1 = motion1.prepend(*c1.position_wrt_parent()); let motion_c2 = motion2.prepend(*c2.position_wrt_parent()); @@ -112,7 +101,7 @@ impl TOIEntry { ) .ok(); - toi = res_toi??; + let toi = res_toi??; Some(Self::new( toi.toi, @@ -124,6 +113,20 @@ impl TOIEntry { 0, )) } + + fn body_motion(dt: Real, body: &RigidBody) -> NonlinearRigidMotion { + if body.should_resolve_ccd(dt) { + NonlinearRigidMotion::new( + 0.0, + body.position, + body.mass_properties.local_com, + body.linvel, + body.angvel, + ) + } else { + NonlinearRigidMotion::constant_position(body.next_position) + } + } } impl PartialOrd for TOIEntry { diff --git a/src/dynamics/integration_parameters.rs b/src/dynamics/integration_parameters.rs index 8c0f26c..136e345 100644 --- a/src/dynamics/integration_parameters.rs +++ b/src/dynamics/integration_parameters.rs @@ -27,6 +27,8 @@ pub struct IntegrationParameters { /// Each cached impulse are multiplied by this coefficient in `[0, 1]` /// when they are re-used to initialize the solver (default `1.0`). pub warmstart_coeff: Real, + /// Correction factor to avoid large warmstart impulse after a strong impact. + pub warmstart_correction_slope: Real, /// 0-1: how much of the velocity to dampen out in the constraint solver? /// (default `1.0`). @@ -200,6 +202,7 @@ impl Default for IntegrationParameters { velocity_solve_fraction: 1.0, velocity_based_erp: 0.0, warmstart_coeff: 1.0, + warmstart_correction_slope: 1.0, allowed_linear_error: 0.005, prediction_distance: 0.002, allowed_angular_error: 0.001, diff --git a/src/dynamics/rigid_body.rs b/src/dynamics/rigid_body.rs index 9ac4763..e557df0 100644 --- a/src/dynamics/rigid_body.rs +++ b/src/dynamics/rigid_body.rs @@ -204,8 +204,12 @@ impl RigidBody { self.flags.contains(RigidBodyFlags::CCD_ENABLED) } + pub fn is_moving_fast(&self, dt: Real) -> bool { + self.is_dynamic() && self.linvel.norm() * dt > self.ccd_thickness + } + pub(crate) fn should_resolve_ccd(&self, dt: Real) -> bool { - self.is_ccd_enabled() && self.is_dynamic() && self.linvel.norm() * dt > self.ccd_thickness + self.is_ccd_enabled() && self.is_moving_fast(dt) } /// Sets the rigid-body's mass properties. @@ -371,10 +375,6 @@ impl RigidBody { shift * Isometry::new(self.linvel * dt, self.angvel * dt) * shift.inverse() } - pub(crate) fn position_at_time(&self, dt: Real) -> Isometry { - self.integrate_velocity(dt) * self.position - } - pub(crate) fn integrate_next_position(&mut self, dt: Real, apply_damping: bool) { // TODO: do we want to apply damping before or after the velocity integration? if apply_damping { @@ -504,10 +504,6 @@ impl RigidBody { self.linvel = dpos.translation.vector * inv_dt; } - pub(crate) fn update_next_position(&mut self, dt: Real) { - self.next_position = self.integrate_velocity(dt) * self.position; - } - pub(crate) fn update_world_mass_properties(&mut self) { self.world_com = self.mass_properties.world_com(&self.position); self.effective_inv_mass = self.mass_properties.inv_mass; 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")] diff --git a/src/geometry/contact_pair.rs b/src/geometry/contact_pair.rs index f156db5..ffd5d7f 100644 --- a/src/geometry/contact_pair.rs +++ b/src/geometry/contact_pair.rs @@ -38,6 +38,8 @@ pub struct ContactData { /// collider's rigid-body. #[cfg(feature = "dim3")] pub tangent_impulse: na::Vector2, + /// The target velocity correction at the contact point. + pub rhs: Real, } impl Default for ContactData { @@ -45,6 +47,7 @@ impl Default for ContactData { Self { impulse: 0.0, tangent_impulse: na::zero(), + rhs: 0.0, } } } @@ -143,16 +146,25 @@ pub struct SolverContact { /// This is set to zero by default. Set to a non-zero value to /// simulate, e.g., conveyor belts. pub tangent_velocity: Vector, - /// Associated contact data used to warm-start the constraints - /// solver. - pub data: ContactData, + /// The warmstart impulse, along the contact normal, applied by this contact to the first collider's rigid-body. + pub warmstart_impulse: Real, + /// The warmstart friction impulse along the vector orthonormal to the contact normal, applied to the first + /// collider's rigid-body. + #[cfg(feature = "dim2")] + pub warmstart_tangent_impulse: Real, + /// The warmstart friction impulses along the basis orthonormal to the contact normal, applied to the first + /// collider's rigid-body. + #[cfg(feature = "dim3")] + pub warmstart_tangent_impulse: na::Vector2, + /// The last velocity correction targeted by this contact. + pub prev_rhs: Real, } impl SolverContact { /// Should we treat this contact as a bouncy contact? /// If `true`, use [`Self::restitution`]. pub fn is_bouncy(&self) -> bool { - let is_new = self.data.impulse == 0.0; + let is_new = self.warmstart_impulse == 0.0; if is_new { // Treat new collisions as bouncing at first, unless we have zero restitution. self.restitution > 0.0 diff --git a/src/geometry/narrow_phase.rs b/src/geometry/narrow_phase.rs index 92cf57d..372d056 100644 --- a/src/geometry/narrow_phase.rs +++ b/src/geometry/narrow_phase.rs @@ -584,7 +584,9 @@ impl NarrowPhase { friction, restitution, tangent_velocity: Vector::zeros(), - data: contact.data, + warmstart_impulse: contact.data.impulse, + warmstart_tangent_impulse: contact.data.tangent_impulse, + prev_rhs: contact.data.rhs, }; manifold.data.solver_contacts.push(solver_contact); -- cgit