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') 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') 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') 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 c3a0c67272e09d3bb4f80aca145ff580d0172745 Mon Sep 17 00:00:00 2001 From: Crozet Sébastien Date: Mon, 29 Mar 2021 17:23:05 +0200 Subject: Add missing files. --- src/dynamics/ccd/ccd_solver.rs | 398 +++++++++++++++++++++++++++++++++++++++++ src/dynamics/ccd/mod.rs | 5 + 2 files changed, 403 insertions(+) create mode 100644 src/dynamics/ccd/ccd_solver.rs create mode 100644 src/dynamics/ccd/mod.rs (limited to 'src/dynamics/ccd') diff --git a/src/dynamics/ccd/ccd_solver.rs b/src/dynamics/ccd/ccd_solver.rs new file mode 100644 index 0000000..2d39956 --- /dev/null +++ b/src/dynamics/ccd/ccd_solver.rs @@ -0,0 +1,398 @@ +use super::TOIEntry; +use crate::dynamics::{RigidBodyHandle, RigidBodySet}; +use crate::geometry::{ColliderSet, IntersectionEvent}; +use crate::math::Real; +use crate::parry::utils::SortedPair; +use crate::pipeline::{EventHandler, QueryPipeline, QueryPipelineMode}; +use parry::query::{DefaultQueryDispatcher, QueryDispatcher}; +use parry::utils::hashmap::HashMap; +use std::collections::BinaryHeap; + +pub enum PredictedImpacts { + Impacts(HashMap), + ImpactsAfterEndTime(Real), + NoImpacts, +} + +/// Solver responsible for performing motion-clamping on fast-moving bodies. +pub struct CCDSolver { + query_pipeline: QueryPipeline, +} + +impl CCDSolver { + /// Initializes a new CCD solver + pub fn new() -> Self { + Self::with_query_dispatcher(DefaultQueryDispatcher) + } + + /// Initializes a CCD solver with a custom `QueryDispatcher` used for computing time-of-impacts. + /// + /// Use this constructor in order to use a custom `QueryDispatcher` that is aware of your own + /// user-defined shapes. + pub fn with_query_dispatcher(d: D) -> Self + where + D: 'static + QueryDispatcher, + { + CCDSolver { + query_pipeline: QueryPipeline::with_query_dispatcher(d), + } + } + + /// Apply motion-clamping to the bodies affected by the given `impacts`. + /// + /// The `impacts` should be the result of a previous call to `self.predict_next_impacts`. + pub fn clamp_motions(&self, dt: Real, bodies: &mut RigidBodySet, impacts: &PredictedImpacts) { + match impacts { + PredictedImpacts::Impacts(tois) => { + for (handle, toi) in tois { + if let Some(body) = bodies.get_mut_internal(*handle) { + let min_toi = + (body.ccd_thickness * 0.15 * crate::utils::inv(body.linvel.norm())) + .min(dt); + // println!("Min toi: {}, Toi: {}", min_toi, toi); + body.integrate_next_position(toi.max(min_toi), false); + } + } + } + _ => {} + } + } + + /// Updates the set of bodies that needs CCD to be resolved. + /// + /// Returns `true` if any rigid-body must have CCD resolved. + pub fn update_ccd_active_flags(&self, bodies: &mut RigidBodySet, dt: Real) -> bool { + let mut ccd_active = false; + + bodies.foreach_active_dynamic_body_mut_internal(|_, body| { + body.update_ccd_active_flag(dt); + ccd_active = ccd_active || body.is_ccd_active(); + }); + + ccd_active + } + + /// Find the first time a CCD-enabled body has a non-sensor collider hitting another non-sensor collider. + pub fn find_first_impact( + &mut self, + dt: Real, + bodies: &RigidBodySet, + colliders: &ColliderSet, + ) -> Option { + // Update the query pipeline. + self.query_pipeline.update_with_mode( + bodies, + colliders, + QueryPipelineMode::SweepTestWithPredictedPosition { dt }, + ); + + let mut pairs_seen = HashMap::default(); + let mut min_toi = dt; + + for (_, rb1) in bodies.iter_active_dynamic() { + if rb1.is_ccd_active() { + let predicted_body_pos1 = rb1.predict_position_using_velocity_and_forces(dt); + + for ch1 in &rb1.colliders { + let co1 = &colliders[*ch1]; + + if co1.is_sensor() { + continue; // Ignore sensors. + } + + let aabb1 = + co1.compute_swept_aabb(&(predicted_body_pos1 * co1.position_wrt_parent())); + + self.query_pipeline + .colliders_with_aabb_intersecting_aabb(&aabb1, |ch2| { + if *ch1 == *ch2 { + // Ignore self-intersection. + return true; + } + + if pairs_seen + .insert( + SortedPair::new(ch1.into_raw_parts().0, ch2.into_raw_parts().0), + (), + ) + .is_none() + { + let c1 = colliders.get(*ch1).unwrap(); + let c2 = colliders.get(*ch2).unwrap(); + let bh1 = c1.parent(); + let bh2 = c2.parent(); + + if bh1 == bh2 || (c1.is_sensor() || c2.is_sensor()) { + // Ignore self-intersection and sensors. + return true; + } + + let b1 = bodies.get(bh1).unwrap(); + let b2 = bodies.get(bh2).unwrap(); + + if let Some(toi) = TOIEntry::try_from_colliders( + self.query_pipeline.query_dispatcher(), + *ch1, + *ch2, + c1, + c2, + b1, + b2, + None, + None, + 0.0, + min_toi, + ) { + min_toi = min_toi.min(toi.toi); + } + } + + true + }); + } + } + } + + if min_toi < dt { + Some(min_toi) + } else { + None + } + } + + /// Outputs the set of bodies as well as their first time-of-impact event. + pub fn predict_impacts_at_next_positions( + &mut self, + dt: Real, + bodies: &RigidBodySet, + colliders: &ColliderSet, + events: &dyn EventHandler, + ) -> PredictedImpacts { + let mut frozen = HashMap::<_, Real>::default(); + let mut all_toi = BinaryHeap::new(); + let mut pairs_seen = HashMap::default(); + let mut min_overstep = dt; + + // Update the query pipeline. + self.query_pipeline.update_with_mode( + bodies, + colliders, + QueryPipelineMode::SweepTestWithNextPosition, + ); + + /* + * + * First, collect all TOIs. + * + */ + // TODO: don't iterate through all the colliders. + for (ch1, co1) in colliders.iter() { + let rb1 = &bodies[co1.parent()]; + if rb1.is_ccd_active() { + let aabb = co1.compute_swept_aabb(&(rb1.next_position * co1.position_wrt_parent())); + + self.query_pipeline + .colliders_with_aabb_intersecting_aabb(&aabb, |ch2| { + if ch1 == *ch2 { + // Ignore self-intersection. + return true; + } + + if pairs_seen + .insert( + SortedPair::new(ch1.into_raw_parts().0, ch2.into_raw_parts().0), + (), + ) + .is_none() + { + let c1 = colliders.get(ch1).unwrap(); + let c2 = colliders.get(*ch2).unwrap(); + let bh1 = c1.parent(); + let bh2 = c2.parent(); + + if bh1 == bh2 { + // Ignore self-intersection. + return true; + } + + let b1 = bodies.get(bh1).unwrap(); + let b2 = bodies.get(bh2).unwrap(); + + if let Some(toi) = TOIEntry::try_from_colliders( + self.query_pipeline.query_dispatcher(), + ch1, + *ch2, + c1, + c2, + b1, + b2, + None, + None, + 0.0, + // NOTE: we use dt here only once we know that + // there is at least one TOI before dt. + min_overstep, + ) { + if toi.toi > dt { + min_overstep = min_overstep.min(toi.toi); + } else { + min_overstep = dt; + all_toi.push(toi); + } + } + } + + true + }); + } + } + + /* + * + * If the smallest TOI is outside of the time interval, return. + * + */ + if min_overstep == dt && all_toi.is_empty() { + return PredictedImpacts::NoImpacts; + } else if min_overstep > dt { + return PredictedImpacts::ImpactsAfterEndTime(min_overstep); + } + + // NOTE: all static bodies (and kinematic bodies?) should be considered as "frozen", this + // may avoid some resweeps. + let mut intersections_to_check = vec![]; + + while let Some(toi) = all_toi.pop() { + assert!(toi.toi <= dt); + + let body1 = bodies.get(toi.b1).unwrap(); + let body2 = bodies.get(toi.b2).unwrap(); + + let mut colliders_to_check = Vec::new(); + let should_freeze1 = body1.is_ccd_active() && !frozen.contains_key(&toi.b1); + let should_freeze2 = body2.is_ccd_active() && !frozen.contains_key(&toi.b2); + + if !should_freeze1 && !should_freeze2 { + continue; + } + + if toi.is_intersection_test { + // NOTE: this test is rendundant with the previous `if !should_freeze && ...` + // but let's keep it to avoid tricky regressions if we end up swapping both + // `if` for some reasons in the future. + if should_freeze1 || should_freeze2 { + // This is only an intersection so we don't have to freeze and there is no + // need to resweep. However we will need to see if we have to generate + // intersection events, so push the TOI for further testing. + intersections_to_check.push(toi); + } + continue; + } + + if should_freeze1 { + let _ = frozen.insert(toi.b1, toi.toi); + colliders_to_check.extend_from_slice(&body1.colliders); + } + + if should_freeze2 { + let _ = frozen.insert(toi.b2, toi.toi); + colliders_to_check.extend_from_slice(&body2.colliders); + } + + let start_time = toi.toi; + + for ch1 in &colliders_to_check { + let co1 = &colliders[*ch1]; + let rb1 = &bodies[co1.parent]; + let aabb = co1.compute_swept_aabb(&(rb1.next_position * co1.position_wrt_parent())); + + self.query_pipeline + .colliders_with_aabb_intersecting_aabb(&aabb, |ch2| { + let c1 = colliders.get(*ch1).unwrap(); + let c2 = colliders.get(*ch2).unwrap(); + let bh1 = c1.parent(); + let bh2 = c2.parent(); + + if bh1 == bh2 { + // Ignore self-intersection. + return true; + } + + let frozen1 = frozen.get(&bh1); + let frozen2 = frozen.get(&bh2); + + let b1 = bodies.get(bh1).unwrap(); + let b2 = bodies.get(bh2).unwrap(); + + if (frozen1.is_some() || !b1.is_ccd_active()) + && (frozen2.is_some() || !b2.is_ccd_active()) + { + // We already did a resweep. + return true; + } + + if let Some(toi) = TOIEntry::try_from_colliders( + self.query_pipeline.query_dispatcher(), + *ch1, + *ch2, + c1, + c2, + b1, + b2, + frozen1.copied(), + frozen2.copied(), + start_time, + dt, + ) { + all_toi.push(toi); + } + + true + }); + } + } + + for toi in intersections_to_check { + // See if the intersection is still active once the bodies + // reach their final positions. + // - If the intersection is still active, don't report it yet. It will be + // reported by the narrow-phase at the next timestep/substep. + // - If the intersection isn't active anymore, and it wasn't intersecting + // before, then we need to generate one interaction-start and one interaction-stop + // events because it will never be detected by the narrow-phase because of tunneling. + let body1 = &bodies[toi.b1]; + let body2 = &bodies[toi.b2]; + let co1 = &colliders[toi.c1]; + let co2 = &colliders[toi.c2]; + let frozen1 = frozen.get(&toi.b1); + let frozen2 = frozen.get(&toi.b2); + let pos1 = frozen1 + .map(|t| body1.integrate_velocity(*t)) + .unwrap_or(body1.next_position); + let pos2 = frozen2 + .map(|t| body2.integrate_velocity(*t)) + .unwrap_or(body2.next_position); + + let prev_coll_pos12 = co1.position.inv_mul(&co2.position); + let next_coll_pos12 = + (pos1 * co1.position_wrt_parent()).inverse() * (pos2 * co2.position_wrt_parent()); + + let query_dispatcher = self.query_pipeline.query_dispatcher(); + let intersect_before = query_dispatcher + .intersection_test(&prev_coll_pos12, co1.shape(), co2.shape()) + .unwrap_or(false); + + let intersect_after = query_dispatcher + .intersection_test(&next_coll_pos12, co1.shape(), co2.shape()) + .unwrap_or(false); + + if !intersect_before && !intersect_after { + // Emit one intersection-started and one intersection-stopped event. + events.handle_intersection_event(IntersectionEvent::new(toi.c1, toi.c2, true)); + events.handle_intersection_event(IntersectionEvent::new(toi.c1, toi.c2, false)); + } + } + + PredictedImpacts::Impacts(frozen) + } +} diff --git a/src/dynamics/ccd/mod.rs b/src/dynamics/ccd/mod.rs new file mode 100644 index 0000000..84807fa --- /dev/null +++ b/src/dynamics/ccd/mod.rs @@ -0,0 +1,5 @@ +pub use self::ccd_solver::{CCDSolver, PredictedImpacts}; +pub use self::toi_entry::TOIEntry; + +mod ccd_solver; +mod toi_entry; -- 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/ccd_solver.rs | 44 ++++++++++++++++++++++++++++++++++++------ src/dynamics/ccd/toi_entry.rs | 29 +++++++++++++++++++++++----- 2 files changed, 62 insertions(+), 11 deletions(-) (limited to 'src/dynamics/ccd') diff --git a/src/dynamics/ccd/ccd_solver.rs b/src/dynamics/ccd/ccd_solver.rs index 2d39956..41b195c 100644 --- a/src/dynamics/ccd/ccd_solver.rs +++ b/src/dynamics/ccd/ccd_solver.rs @@ -1,6 +1,6 @@ use super::TOIEntry; use crate::dynamics::{RigidBodyHandle, RigidBodySet}; -use crate::geometry::{ColliderSet, IntersectionEvent}; +use crate::geometry::{ColliderSet, IntersectionEvent, NarrowPhase}; use crate::math::Real; use crate::parry::utils::SortedPair; use crate::pipeline::{EventHandler, QueryPipeline, QueryPipelineMode}; @@ -44,11 +44,13 @@ impl CCDSolver { pub fn clamp_motions(&self, dt: Real, bodies: &mut RigidBodySet, impacts: &PredictedImpacts) { match impacts { PredictedImpacts::Impacts(tois) => { + // println!("Num to clamp: {}", tois.len()); for (handle, toi) in tois { if let Some(body) = bodies.get_mut_internal(*handle) { - let min_toi = - (body.ccd_thickness * 0.15 * crate::utils::inv(body.linvel.norm())) - .min(dt); + let min_toi = (body.ccd_thickness + * 0.15 + * crate::utils::inv(body.max_point_velocity())) + .min(dt); // println!("Min toi: {}, Toi: {}", min_toi, toi); body.integrate_next_position(toi.max(min_toi), false); } @@ -61,11 +63,18 @@ impl CCDSolver { /// Updates the set of bodies that needs CCD to be resolved. /// /// Returns `true` if any rigid-body must have CCD resolved. - pub fn update_ccd_active_flags(&self, bodies: &mut RigidBodySet, dt: Real) -> bool { + pub fn update_ccd_active_flags( + &self, + bodies: &mut RigidBodySet, + dt: Real, + include_forces: bool, + ) -> bool { let mut ccd_active = false; + // println!("Checking CCD activation"); bodies.foreach_active_dynamic_body_mut_internal(|_, body| { - body.update_ccd_active_flag(dt); + body.update_ccd_active_flag(dt, include_forces); + // println!("CCD is active: {}, for {:?}", ccd_active, handle); ccd_active = ccd_active || body.is_ccd_active(); }); @@ -78,6 +87,7 @@ impl CCDSolver { dt: Real, bodies: &RigidBodySet, colliders: &ColliderSet, + narrow_phase: &NarrowPhase, ) -> Option { // Update the query pipeline. self.query_pipeline.update_with_mode( @@ -127,6 +137,12 @@ impl CCDSolver { return true; } + let smallest_dist = narrow_phase + .contact_pair(*ch1, *ch2) + .and_then(|p| p.find_deepest_contact()) + .map(|c| c.1.dist) + .unwrap_or(0.0); + let b1 = bodies.get(bh1).unwrap(); let b2 = bodies.get(bh2).unwrap(); @@ -142,6 +158,7 @@ impl CCDSolver { None, 0.0, min_toi, + smallest_dist, ) { min_toi = min_toi.min(toi.toi); } @@ -166,6 +183,7 @@ impl CCDSolver { dt: Real, bodies: &RigidBodySet, colliders: &ColliderSet, + narrow_phase: &NarrowPhase, events: &dyn EventHandler, ) -> PredictedImpacts { let mut frozen = HashMap::<_, Real>::default(); @@ -218,6 +236,12 @@ impl CCDSolver { let b1 = bodies.get(bh1).unwrap(); let b2 = bodies.get(bh2).unwrap(); + let smallest_dist = narrow_phase + .contact_pair(ch1, *ch2) + .and_then(|p| p.find_deepest_contact()) + .map(|c| c.1.dist) + .unwrap_or(0.0); + if let Some(toi) = TOIEntry::try_from_colliders( self.query_pipeline.query_dispatcher(), ch1, @@ -232,6 +256,7 @@ impl CCDSolver { // NOTE: we use dt here only once we know that // there is at least one TOI before dt. min_overstep, + smallest_dist, ) { if toi.toi > dt { min_overstep = min_overstep.min(toi.toi); @@ -331,6 +356,12 @@ impl CCDSolver { return true; } + let smallest_dist = narrow_phase + .contact_pair(*ch1, *ch2) + .and_then(|p| p.find_deepest_contact()) + .map(|c| c.1.dist) + .unwrap_or(0.0); + if let Some(toi) = TOIEntry::try_from_colliders( self.query_pipeline.query_dispatcher(), *ch1, @@ -343,6 +374,7 @@ impl CCDSolver { frozen2.copied(), start_time, dt, + smallest_dist, ) { all_toi.push(toi); } 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 From 0ecc302971e353f181c5319504124c3967c89d15 Mon Sep 17 00:00:00 2001 From: Crozet Sébastien Date: Thu, 1 Apr 2021 10:11:32 +0200 Subject: Some small performance improvements. --- src/dynamics/ccd/ccd_solver.rs | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'src/dynamics/ccd') diff --git a/src/dynamics/ccd/ccd_solver.rs b/src/dynamics/ccd/ccd_solver.rs index 41b195c..134b8a6 100644 --- a/src/dynamics/ccd/ccd_solver.rs +++ b/src/dynamics/ccd/ccd_solver.rs @@ -52,7 +52,7 @@ impl CCDSolver { * crate::utils::inv(body.max_point_velocity())) .min(dt); // println!("Min toi: {}, Toi: {}", min_toi, toi); - body.integrate_next_position(toi.max(min_toi), false); + body.integrate_next_position(toi.max(min_toi)); } } } -- cgit