From 97157c9423f3360c5e941b4065377689221014ae Mon Sep 17 00:00:00 2001 From: Crozet Sébastien Date: Fri, 26 Mar 2021 18:16:27 +0100 Subject: First working version of non-linear CCD based on single-substep motion-clamping. --- src/dynamics/ccd/toi_entry.rs | 147 ++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 147 insertions(+) create mode 100644 src/dynamics/ccd/toi_entry.rs (limited to 'src/dynamics/ccd/toi_entry.rs') diff --git a/src/dynamics/ccd/toi_entry.rs b/src/dynamics/ccd/toi_entry.rs new file mode 100644 index 0000000..20f5268 --- /dev/null +++ b/src/dynamics/ccd/toi_entry.rs @@ -0,0 +1,147 @@ +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}; + +#[derive(Copy, Clone, Debug)] +pub struct TOIEntry { + pub toi: Real, + pub c1: ColliderHandle, + pub b1: RigidBodyHandle, + pub c2: ColliderHandle, + pub b2: RigidBodyHandle, + pub is_intersection_test: bool, + pub timestamp: usize, +} + +impl TOIEntry { + fn new( + toi: Real, + c1: ColliderHandle, + b1: RigidBodyHandle, + c2: ColliderHandle, + b2: RigidBodyHandle, + is_intersection_test: bool, + timestamp: usize, + ) -> Self { + Self { + toi, + c1, + b1, + c2, + b2, + is_intersection_test, + timestamp, + } + } + + pub fn try_from_colliders>( + params: &IntegrationParameters, + query_dispatcher: &QD, + ch1: ColliderHandle, + ch2: ColliderHandle, + c1: &Collider, + c2: &Collider, + b1: &RigidBody, + b2: &RigidBody, + frozen1: Option, + frozen2: Option, + start_time: Real, + end_time: Real, + body_params: &Coarena, + ) -> Option { + assert!(start_time <= end_time); + + let linvel1 = frozen1.is_none() as u32 as Real * b1.linvel; + 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()); + + if params.dt * vel12.norm() < thickness { + return None; + } + + 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); + + if let Some(t) = frozen1 { + motion1.freeze(t); + } + + if let Some(t) = frozen2 { + motion2.freeze(t); + } + + let mut toi; + let motion_c1 = motion1.prepend(*c1.position_wrt_parent()); + let motion_c2 = motion2.prepend(*c2.position_wrt_parent()); + + // println!("start_time: {}", start_time); + + // If this is just an intersection test (i.e. with sensors) + // then we can stop the TOI search immediately if it starts with + // a penetration because we don't care about the whether the velocity + // at the impact is a separating velocity or not. + // If the TOI search involves two non-sensor colliders then + // we don't want to stop the TOI search at the first penetration + // because the colliders may be in a separating trajectory. + let stop_at_penetration = is_intersection_test; + + let res_toi = query_dispatcher + .nonlinear_time_of_impact( + &motion_c1, + c1.shape(), + &motion_c2, + c2.shape(), + start_time, + end_time, + stop_at_penetration, + ) + .ok(); + + toi = res_toi??; + + Some(Self::new( + toi.toi, + ch1, + c1.parent(), + ch2, + c2.parent(), + is_intersection_test, + 0, + )) + } +} + +impl PartialOrd for TOIEntry { + fn partial_cmp(&self, other: &Self) -> Option { + (-self.toi).partial_cmp(&(-other.toi)) + } +} + +impl Ord for TOIEntry { + fn cmp(&self, other: &Self) -> std::cmp::Ordering { + self.partial_cmp(other).unwrap() + } +} + +impl PartialEq for TOIEntry { + fn eq(&self, other: &Self) -> bool { + self.toi == other.toi + } +} + +impl Eq for TOIEntry {} -- cgit 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 +++++++++++++++++++++------------------ 1 file changed, 21 insertions(+), 18 deletions(-) (limited to 'src/dynamics/ccd/toi_entry.rs') 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 { -- cgit From a733f97028f5cd532212572f9561ab64e09f002b Mon Sep 17 00:00:00 2001 From: Crozet Sébastien Date: Mon, 29 Mar 2021 17:21:49 +0200 Subject: Implement the ability to run multiple CCD substeps. --- src/dynamics/ccd/toi_entry.rs | 16 +++++----------- 1 file changed, 5 insertions(+), 11 deletions(-) (limited to 'src/dynamics/ccd/toi_entry.rs') diff --git a/src/dynamics/ccd/toi_entry.rs b/src/dynamics/ccd/toi_entry.rs index 6679a91..3916a4b 100644 --- a/src/dynamics/ccd/toi_entry.rs +++ b/src/dynamics/ccd/toi_entry.rs @@ -1,4 +1,4 @@ -use crate::dynamics::{IntegrationParameters, RigidBody, RigidBodyHandle}; +use crate::dynamics::{RigidBody, RigidBodyHandle}; use crate::geometry::{Collider, ColliderHandle}; use crate::math::Real; use parry::query::{NonlinearRigidMotion, QueryDispatcher}; @@ -36,7 +36,6 @@ impl TOIEntry { } pub fn try_from_colliders( - params: &IntegrationParameters, query_dispatcher: &QD, ch1: ColliderHandle, ch2: ColliderHandle, @@ -56,16 +55,11 @@ impl TOIEntry { let vel12 = linvel2 - linvel1; let thickness = c1.shape().ccd_thickness() + c2.shape().ccd_thickness(); - - if params.dt * vel12.norm() < thickness { - return None; - } - let is_intersection_test = c1.is_sensor() || c2.is_sensor(); // Compute the TOI. - let mut motion1 = Self::body_motion(params.dt, b1); - let mut motion2 = Self::body_motion(params.dt, b2); + let mut motion1 = Self::body_motion(b1); + let mut motion2 = Self::body_motion(b2); if let Some(t) = frozen1 { motion1.freeze(t); @@ -114,8 +108,8 @@ impl TOIEntry { )) } - fn body_motion(dt: Real, body: &RigidBody) -> NonlinearRigidMotion { - if body.should_resolve_ccd(dt) { + fn body_motion(body: &RigidBody) -> NonlinearRigidMotion { + if body.is_ccd_active() { NonlinearRigidMotion::new( 0.0, body.position, -- cgit From d2ee6420538d7ee524f2096995d4f44fcfef4551 Mon Sep 17 00:00:00 2001 From: Crozet Sébastien Date: Tue, 30 Mar 2021 17:08:51 +0200 Subject: CCD: take angular motion and penetration depth into account in various thresholds. --- src/dynamics/ccd/toi_entry.rs | 29 ++++++++++++++++++++++++----- 1 file changed, 24 insertions(+), 5 deletions(-) (limited to 'src/dynamics/ccd/toi_entry.rs') diff --git a/src/dynamics/ccd/toi_entry.rs b/src/dynamics/ccd/toi_entry.rs index 3916a4b..cc6773c 100644 --- a/src/dynamics/ccd/toi_entry.rs +++ b/src/dynamics/ccd/toi_entry.rs @@ -47,16 +47,35 @@ impl TOIEntry { frozen2: Option, start_time: Real, end_time: Real, + smallest_contact_dist: Real, ) -> Option { assert!(start_time <= end_time); - let linvel1 = frozen1.is_none() as u32 as Real * b1.linvel; - 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 linvel1 = frozen1.is_none() as u32 as Real * b1.linvel(); + let linvel2 = frozen2.is_none() as u32 as Real * b2.linvel(); + let angvel1 = frozen1.is_none() as u32 as Real * b1.angvel(); + let angvel2 = frozen2.is_none() as u32 as Real * b2.angvel(); + + #[cfg(feature = "dim2")] + let vel12 = (linvel2 - linvel1).norm() + + angvel1.abs() * b1.ccd_max_dist + + angvel2.abs() * b2.ccd_max_dist; + #[cfg(feature = "dim3")] + let vel12 = (linvel2 - linvel1).norm() + + angvel1.norm() * b1.ccd_max_dist + + angvel2.norm() * b2.ccd_max_dist; + + // We may be slightly over-conservative by taking the `max(0.0)` here. + // But removing the `max` doesn't really affect performances so let's + // keep it since more conservatism is good at this stage. + let thickness = (c1.shape().ccd_thickness() + c2.shape().ccd_thickness()) + + smallest_contact_dist.max(0.0); let is_intersection_test = c1.is_sensor() || c2.is_sensor(); + if (end_time - start_time) * vel12 < thickness { + return None; + } + // Compute the TOI. let mut motion1 = Self::body_motion(b1); let mut motion2 = Self::body_motion(b2); -- cgit