Improve convergence of epa algorithm in case it's stuck (#245)

CHANGELOG.md

@@ -1,5 +1,11 @@
 # Change Log
 
+## Unreleased
+
+### Modified
+
+- Improve convergence of epa algorithm in degenerate configurations.
+
 ## v0.17.0
 
 ### Added

crates/parry2d/Cargo.toml

@@ -90,7 +90,7 @@ simba = { version = "0.9", default-features = false }
 oorandom = "11"
 ptree = "0.4.0"
 rand = { version = "0.8" }
-macroquad = "0.4"
+macroquad = "0.4.12"
 
 [package.metadata.docs.rs]
 rustdoc-args = ["-Zunstable-options", "--generate-link-to-definition"]

crates/parry2d/tests/geometry/epa_convergence.rs

@@ -0,0 +1,42 @@
+use na::Vector2;
+use parry2d::{
+    math::{Isometry, Point, Real},
+    query,
+    shape::{Capsule, ConvexPolygon, SharedShape},
+};
+
+/// Original issue: https://github.com/dimforge/parry/issues/205
+#[test]
+fn capsule_convergence() {
+    let shape1 = Capsule::new_y(5.0, 10.0);
+    let mut vec = Vec::<Point<Real>>::with_capacity(3);
+    vec.push(Point::<Real>::new(64.0, 507.0));
+    vec.push(Point::<Real>::new(440.0, 326.0));
+    vec.push(Point::<Real>::new(1072.0, 507.0));
+    let shape2 = ConvexPolygon::from_convex_polyline(vec);
+    let shape2 = shape2.unwrap();
+    let transform1 = Isometry::new(Vector2::new(381.592, 348.491), 0.0);
+    let transform2 = Isometry::new(Vector2::new(0.0, 0.0), 0.0);
+
+    let _res = query::details::contact_support_map_support_map(
+        &transform1.inv_mul(&transform2),
+        &shape1,
+        &shape2,
+        10.0,
+    )
+    .expect("Penetration not found.");
+    let shared_shape1 = SharedShape::new(shape1);
+    let shared_shape2 = SharedShape::new(shape2);
+
+    if let Ok(Some(_contact)) = query::contact(
+        &transform1,
+        shared_shape1.as_ref(),
+        &transform2,
+        shared_shape2.as_ref(),
+        1.0,
+    ) {
+        println!("collision");
+    } else {
+        panic!("no collision");
+    }
+}

crates/parry2d/tests/geometry/mod.rs

@@ -2,5 +2,6 @@ mod aabb_scale;
 mod ball_ball_toi;
 mod ball_cuboid_contact;
 mod epa2;
+mod epa_convergence;
 mod ray_cast;
 mod time_of_impact2;

crates/parry3d/Cargo.toml

@@ -88,7 +88,7 @@ obj = { version = "0.10.2", optional = true }
 oorandom = "11"
 ptree = "0.4.0"
 rand = { version = "0.8" }
-macroquad = "0.4"
+macroquad = "0.4.12"
 
 [package.metadata.docs.rs]
 rustdoc-args = ["-Zunstable-options", "--generate-link-to-definition"]

crates/parry3d/examples/plane_intersection.rs

@@ -88,7 +88,8 @@ fn mquad_mesh_from_points(trimesh: &(Vec<Point3<Real>>, Vec<[u32; 3]>), camera_p
             .map(|p| Vertex {
                 position: mquad_from_na(*p),
                 uv: Vec2::new(p.x, p.y),
-                color: DARKGRAY,
+                color: DARKGRAY.into(),
+                normal: Vec4::ZERO,
             })
             .collect(),
         indices.iter().flatten().map(|v| *v as u16).collect(),
@@ -125,14 +126,15 @@ fn mquad_compute_normals(points: &Vec<Vertex>, indices: &Vec<u16>, cam_pos: Vec3
 
         for &i in indices.iter() {
             let mut color = points[i as usize].color;
-            color.r *= brightness_mod;
-            color.g *= brightness_mod;
-            color.b *= brightness_mod;
+            color[0] = (color[0] as f32 * brightness_mod) as u8;
+            color[1] = (color[1] as f32 * brightness_mod) as u8;
+            color[2] = (color[2] as f32 * brightness_mod) as u8;
 
             vertices.push(Vertex {
                 position: points[i as usize].position,
                 uv: Vec2::ZERO,
                 color: color,
+                normal: Vec4::ZERO,
             });
         }
     }

src/query/epa/epa2.rs

@@ -275,6 +275,7 @@ impl EPA {
         let mut niter = 0;
         let mut max_dist = Real::max_value();
         let mut best_face_id = *self.heap.peek().unwrap();
+        let mut old_dist = 0.0;
 
         /*
          * Run the expansion.
@@ -300,12 +301,18 @@ impl EPA {
 
             let curr_dist = -face_id.neg_dist;
 
-            if max_dist - curr_dist < _eps_tol {
+            if max_dist - curr_dist < _eps_tol ||
+                // Accept the intersection as the algorithm is stuck and no new points will be found
+                // This happens because of numerical stability issue
+                ((curr_dist - old_dist).abs() < _eps && candidate_max_dist < max_dist)
+            {
                 let best_face = &self.faces[best_face_id.id];
                 let cpts = best_face.closest_points(&self.vertices);
                 return Some((cpts.0, cpts.1, best_face.normal));
             }
 
+            old_dist = curr_dist;
+
             let pts1 = [face.pts[0], support_point_id];
             let pts2 = [support_point_id, face.pts[1]];
 
@@ -333,8 +340,10 @@ impl EPA {
             }
 
             niter += 1;
-            if niter > 10000 {
-                return None;
+            if niter > 100 {
+                // if we reached this point, our algorithm didn't converge to what precision we wanted.
+                // still return an intersection point, as it's probably close enough.
+                break;
             }
         }
 

src/query/epa/epa3.rs

@@ -315,6 +315,7 @@ impl EPA {
         let mut niter = 0;
         let mut max_dist = Real::max_value();
         let mut best_face_id = *self.heap.peek().unwrap();
+        let mut old_dist = 0.0;
 
         /*
          * Run the expansion.
@@ -340,12 +341,18 @@ impl EPA {
 
             let curr_dist = -face_id.neg_dist;
 
-            if max_dist - curr_dist < _eps_tol {
+            if max_dist - curr_dist < _eps_tol ||
+                // Accept the intersection as the algorithm is stuck and no new points will be found
+                // This happens because of numerical stability issue
+                ((curr_dist - old_dist).abs() < _eps && candidate_max_dist < max_dist)
+            {
                 let best_face = &self.faces[best_face_id.id];
                 let points = best_face.closest_points(&self.vertices);
                 return Some((points.0, points.1, best_face.normal));
             }
 
+            old_dist = curr_dist;
+
             self.faces[face_id.id].deleted = true;
 
             let adj_opp_pt_id1 = self.faces[face.adj[0]].next_ccw_pt_id(face.pts[0]);
@@ -411,8 +418,10 @@ impl EPA {
             // self.check_topology(); // NOTE: for debugging only.
 
             niter += 1;
-            if niter > 10000 {
-                return None;
+            if niter > 100 {
+                // if we reached this point, our algorithm didn't converge to what precision we wanted.
+                // still return an intersection point, as it's probably close enough.
+                break;
             }
         }
 

src/query/gjk/gjk.rs

@@ -175,7 +175,8 @@ where
             }
         }
         niter += 1;
-        if niter == 10000 {
+
+        if niter == 100 {
             return GJKResult::NoIntersection(Vector::x_axis());
         }
     }
@@ -362,7 +363,7 @@ where
         }
 
         niter += 1;
-        if niter == 10000 {
+        if niter == 100 {
             return None;
         }
     }