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