diff options
| author | Crozet Sébastien <developer@crozet.re> | 2021-03-28 11:26:53 +0200 |
|---|---|---|
| committer | Crozet Sébastien <developer@crozet.re> | 2021-03-28 11:27:07 +0200 |
| commit | 7306821c460ca3f77e697c89a79393e61c126624 (patch) | |
| tree | c8aa2a4d7d2c381706ee7edb60245bfd7bac7a07 /src | |
| parent | 710dd8d71ed53d2f52f15cdd19ee2f1248b62a96 (diff) | |
| download | rapier-7306821c460ca3f77e697c89a79393e61c126624.tar.gz rapier-7306821c460ca3f77e697c89a79393e61c126624.tar.bz2 rapier-7306821c460ca3f77e697c89a79393e61c126624.zip | |
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.
Diffstat (limited to 'src')
| -rw-r--r-- | src/dynamics/ccd/toi_entry.rs | 39 | ||||
| -rw-r--r-- | src/dynamics/integration_parameters.rs | 3 | ||||
| -rw-r--r-- | src/dynamics/rigid_body.rs | 14 | ||||
| -rw-r--r-- | src/dynamics/solver/velocity_constraint.rs | 17 | ||||
| -rw-r--r-- | src/dynamics/solver/velocity_constraint_wide.rs | 27 | ||||
| -rw-r--r-- | src/dynamics/solver/velocity_ground_constraint.rs | 16 | ||||
| -rw-r--r-- | src/dynamics/solver/velocity_ground_constraint_wide.rs | 26 | ||||
| -rw-r--r-- | src/geometry/contact_pair.rs | 20 | ||||
| -rw-r--r-- | src/geometry/narrow_phase.rs | 4 |
9 files changed, 112 insertions, 54 deletions
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<QD: ?Sized + PersistentQueryDispatcher<(), ()>>( + pub fn try_from_colliders<QD: ?Sized + QueryDispatcher>( params: &IntegrationParameters, query_dispatcher: &QD, ch1: ColliderHandle, @@ -54,7 +48,6 @@ impl TOIEntry { frozen2: Option<Real>, start_time: Real, end_time: Real, - body_params: &Coarena<CCDData>, ) -> Option<Self> { 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<Real> { - 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<Real>, + /// 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<Real>, - /// 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<Real>, + /// 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); |
