diff --git a/CHANGELOG.md b/CHANGELOG.md index ec4c721d..9c4120c5 100644 --- a/CHANGELOG.md +++ b/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 diff --git a/src/query/contact/contact_support_map_support_map.rs b/src/query/contact/contact_support_map_support_map.rs index 03d8516e..16568111 100644 --- a/src/query/contact/contact_support_map_support_map.rs +++ b/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); + } + } +} diff --git a/src/query/epa/epa2.rs b/src/query/epa/epa2.rs index 555280dd..e6a4a58f 100644 --- a/src/query/epa/epa2.rs +++ b/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 . + 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, ));