fix(epa2): use the origin's barycentric coordinates on a segment simplex

CHANGELOG.md

@@ -6,6 +6,9 @@
   algorithm no longer fails with `"Cannot deserialize custom shape."`. It now yields the new
   `UnknownContactManifoldsWorkspace`, which the relevant algorithm can replace with a workspace
   of the correct type at its next execution.
+- Fix the 2D EPA returning a contact point snapped to a support vertex when the shapes are exactly
+  touching and the GJK simplex is a segment through the origin
+  ([#378](https://github.com/dimforge/parry/issues/378)).
 
 ## 0.31.1
 

src/query/contact/contact_support_map_support_map.rs

@@ -108,3 +108,34 @@ mod test {
         );
     }
 }
+
+#[cfg(all(test, feature = "dim2"))]
+mod test_touching_contact_points {
+    use super::*;
+    use crate::shape::{Capsule, Cuboid};
+
+    // Regression test for https://github.com/dimforge/parry/issues/378: when the shapes
+    // are exactly touching, EPA starts from a segment simplex and the contact point on the
+    // second shape was snapped to a support vertex (the cuboid corner).
+    #[test]
+    fn touching_capsule_cuboid_contact_points_are_consistent() {
+        let capsule = Capsule::new(Vector::new(0.0, -0.25), Vector::new(0.0, 0.25), 0.5);
+        let cuboid = Cuboid::new(Vector::new(4.0, 0.5));
+
+        for y in [2.749, 2.75, 2.751] {
+            let pos12 = Pose::from_translation(Vector::new(4.0, 1.5) - Vector::new(6.0373735, y));
+            let contact = contact_support_map_support_map(&pos12, &capsule, &cuboid, 0.002)
+                .expect("the shapes are within the prediction distance");
+
+            // The contact point on the cuboid must lie under the capsule, not at its corner.
+            let point1_in_2 = pos12.inverse_transform_point(contact.point1);
+            assert!(
+                (contact.point2.x - point1_in_2.x).abs() < 1.0e-2,
+                "y = {y}: point2 = {:?}, point1 in cuboid frame = {:?}",
+                contact.point2,
+                point1_in_2
+            );
+            assert!((contact.point2.y - 0.5).abs() < 1.0e-4);
+        }
+    }
+}

src/query/epa/epa2.rs

@@ -464,16 +464,23 @@ impl EPA {
             let pts1 = [0, 1];
             let pts2 = [1, 0];
 
+            // The origin lies on the segment: use its actual barycentric coordinates so the
+            // contact points are interpolated at the origin rather than snapped to vertex 0.
+            // See <https://github.com/dimforge/parry/issues/378>.
+            let bcoords = project_origin(self.vertices[0].point, self.vertices[1].point)
+                .map(|(_, bcoords)| bcoords)
+                .unwrap_or([1.0, 0.0]);
+
             self.faces.push(Face::new_with_proj(
                 &self.vertices,
                 Vector::ZERO,
-                [1.0, 0.0],
+                bcoords,
                 pts1,
             ));
             self.faces.push(Face::new_with_proj(
                 &self.vertices,
                 Vector::ZERO,
-                [1.0, 0.0],
+                [bcoords[1], bcoords[0]],
                 pts2,
             ));