Merge pull request #148 from Ralith/robust-ball-manifold

Fix ball-convex manifolds missing contacts in corner cases
dimforge · Oct 1, 2023 · af38912 · af38912
2 parents ac33c4a + a487a09
commit af38912
Showing 1 changed file with 39 additions and 32 deletions.
diff --git a/src/query/contact_manifolds/contact_manifolds_convex_ball.rs b/src/query/contact_manifolds/contact_manifolds_convex_ball.rs
@@ -1,4 +1,4 @@
-use crate::math::{Isometry, Point, Real};
+use crate::math::{Isometry, Point, Real, Vector};
 use crate::query::{ContactManifold, TrackedContact};
 use crate::shape::{Ball, PackedFeatureId, Shape};
 use na::Unit;
@@ -36,41 +36,48 @@ pub fn contact_manifold_convex_ball<'a, ManifoldData, ContactData, S1>(
     let (proj, fid1) = shape1.project_local_point_and_get_feature(&local_p2_1);
     let dpos = local_p2_1 - proj.point;
 
-    if let Some((mut local_n1, mut dist)) = Unit::try_new_and_get(dpos, 0.0) {
-        if proj.is_inside {
-            local_n1 = -local_n1;
-            dist = -dist;
-        }
+    // local_n1 points from the surface towards our origin if defined, otherwise from the other
+    // shape's origin towards our origin if defined, otherwise towards +x
+    let (mut local_n1, mut dist) = Unit::try_new_and_get(dpos, 0.0).unwrap_or_else(|| {
+        (
+            Unit::try_new(pos12.translation.vector, 0.0).unwrap_or_else(|| Vector::x_axis()),
+            0.0,
+        )
+    });
 
-        if dist <= ball2.radius + prediction {
-            let local_n2 = pos12.inverse_transform_vector(&-*local_n1);
-            let local_p2 = (local_n2 * ball2.radius).into();
-            let contact_point = TrackedContact::flipped(
-                proj.point,
-                local_p2,
-                fid1.into(),
-                PackedFeatureId::face(0),
-                dist - ball2.radius,
-                flipped,
-            );
+    if proj.is_inside {
+        local_n1 = -local_n1;
+        dist = -dist;
+    }
 
-            if manifold.points.len() != 1 {
-                manifold.clear();
-                manifold.points.push(contact_point);
-            } else {
-                // Copy only the geometry so we keep the warmstart impulses.
-                manifold.points[0].copy_geometry_from(contact_point);
-            }
+    if dist <= ball2.radius + prediction {
+        let local_n2 = pos12.inverse_transform_vector(&-*local_n1);
+        let local_p2 = (local_n2 * ball2.radius).into();
+        let contact_point = TrackedContact::flipped(
+            proj.point,
+            local_p2,
+            fid1.into(),
+            PackedFeatureId::face(0),
+            dist - ball2.radius,
+            flipped,
+        );
 
-            if flipped {
-                manifold.local_n1 = local_n2;
-                manifold.local_n2 = *local_n1;
-            } else {
-                manifold.local_n1 = *local_n1;
-                manifold.local_n2 = local_n2;
-            }
-        } else {
+        if manifold.points.len() != 1 {
             manifold.clear();
+            manifold.points.push(contact_point);
+        } else {
+            // Copy only the geometry so we keep the warmstart impulses.
+            manifold.points[0].copy_geometry_from(contact_point);
+        }
+
+        if flipped {
+            manifold.local_n1 = local_n2;
+            manifold.local_n2 = *local_n1;
+        } else {
+            manifold.local_n1 = *local_n1;
+            manifold.local_n2 = local_n2;
         }
+    } else {
+        manifold.clear();
     }
 }