From e66f019597d44cc905fb87ab865b64bf246ffd61 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?S=C3=A9bastien=20Crozet?= Date: Sun, 28 Apr 2024 15:00:27 +0200 Subject: [PATCH] feat: add a penetration distance cutoff on corrected contacts --- .../contact_manifolds_cuboid_triangle.rs | 17 ++++++++++++++++- .../contact_manifolds_pfm_pfm.rs | 11 +++++++++++ 2 files changed, 27 insertions(+), 1 deletion(-) diff --git a/src/query/contact_manifolds/contact_manifolds_cuboid_triangle.rs b/src/query/contact_manifolds/contact_manifolds_cuboid_triangle.rs index 63e39080..2574760b 100644 --- a/src/query/contact_manifolds/contact_manifolds_cuboid_triangle.rs +++ b/src/query/contact_manifolds/contact_manifolds_cuboid_triangle.rs @@ -104,22 +104,26 @@ pub fn contact_manifold_cuboid_triangle<'a, ManifoldData, ContactData>( * */ let mut normal1 = sep1.1; + let mut dist = sep1.0; if sep2.0 > sep1.0 && sep2.0 > sep3.0 { normal1 = pos12 * -sep2.1; + dist = sep2.0; } else if sep3.0 > sep1.0 { normal1 = sep3.1; + dist = sep3.0; } // Apply any normal constraint to the separating axis. let mut normal2 = pos21 * -normal1; + if !(normal_constraints1, normal_constraints2).project_local_normals( pos12, &mut normal1, &mut normal2, ) { manifold.clear(); - return; // THe contact got completely discarded by normal correction. + return; // The contact got completely discarded by normal correction. } let feature1; @@ -145,6 +149,17 @@ pub fn contact_manifold_cuboid_triangle<'a, ManifoldData, ContactData>( pos12, pos21, &normal1, &normal2, &feature1, &feature2, manifold, flipped, ); + + if normal_constraints1.is_some() || normal_constraints2.is_some() { + // HACK: some normal correction can lead to very incorrect penetration + // depth, e.g., if the other object extends very far toward that direction. + // This is caused by the locality of the convex/convex check. + // I haven’t found a good mathematically robust approach to account for + // that locally, so for now, we eliminate points that are large divergence + // relative to the unconstrained penetration distance. + manifold.points.retain(|pt| dist >= 0.0 || pt.dist >= 0.0 || pt.dist >= dist * 5.0); + } + if flipped { manifold.local_n1 = normal2; manifold.local_n2 = normal1; diff --git a/src/query/contact_manifolds/contact_manifolds_pfm_pfm.rs b/src/query/contact_manifolds/contact_manifolds_pfm_pfm.rs index a6d946c3..55fcbfdf 100644 --- a/src/query/contact_manifolds/contact_manifolds_pfm_pfm.rs +++ b/src/query/contact_manifolds/contact_manifolds_pfm_pfm.rs @@ -82,6 +82,7 @@ pub fn contact_manifold_pfm_pfm<'a, ManifoldData, ContactData, S1, S2>( GJKResult::ClosestPoints(p1, p2_1, dir) => { let mut local_n1 = dir; let mut local_n2 = pos12.inverse_transform_unit_vector(&-dir); + let dist = (p2_1 - p1).dot(&local_n1); if !(normal_constraints1, normal_constraints2).project_local_normals( pos12, @@ -122,6 +123,16 @@ pub fn contact_manifold_pfm_pfm<'a, ManifoldData, ContactData, S1, S2>( manifold.points.push(contact); } + if normal_constraints1.is_some() || normal_constraints2.is_some() { + // HACK: some normal correction can lead to very incorrect penetration + // depth, e.g., if the other object extends very far toward that direction. + // This is caused by the locality of the convex/convex check. + // I haven’t found a good mathematically robust approach to account for + // that locally, so for now, we eliminate points that are large divergence + // relative to the unconstrained penetration distance. + manifold.points.retain(|pt| dist >= 0.0 || pt.dist >= 0.0 || pt.dist >= dist * 5.0); + } + // Adjust points to take the radius into account. if border_radius1 != 0.0 || border_radius2 != 0.0 { for contact in &mut manifold.points {