diff --git a/CHANGELOG.md b/CHANGELOG.md index c7499975..970a072f 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -1,5 +1,11 @@ # Change Log +## Unreleased + +### Modified + +- Improve convergence of epa algorithm in degenerate configurations. + ## v0.17.0 ### Added diff --git a/crates/parry2d/Cargo.toml b/crates/parry2d/Cargo.toml index 76bf9277..eec11d6a 100644 --- a/crates/parry2d/Cargo.toml +++ b/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"] diff --git a/crates/parry2d/tests/geometry/epa_convergence.rs b/crates/parry2d/tests/geometry/epa_convergence.rs new file mode 100644 index 00000000..e2206104 --- /dev/null +++ b/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::>::with_capacity(3); + vec.push(Point::::new(64.0, 507.0)); + vec.push(Point::::new(440.0, 326.0)); + vec.push(Point::::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"); + } +} diff --git a/crates/parry2d/tests/geometry/mod.rs b/crates/parry2d/tests/geometry/mod.rs index d3ca21e6..05907e8a 100644 --- a/crates/parry2d/tests/geometry/mod.rs +++ b/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; diff --git a/crates/parry3d/Cargo.toml b/crates/parry3d/Cargo.toml index 3599e174..c4afd5c0 100644 --- a/crates/parry3d/Cargo.toml +++ b/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"] diff --git a/crates/parry3d/examples/plane_intersection.rs b/crates/parry3d/examples/plane_intersection.rs index cc73fc8b..c80fc0e5 100644 --- a/crates/parry3d/examples/plane_intersection.rs +++ b/crates/parry3d/examples/plane_intersection.rs @@ -88,7 +88,8 @@ fn mquad_mesh_from_points(trimesh: &(Vec>, 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, indices: &Vec, 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, }); } } diff --git a/src/query/epa/epa2.rs b/src/query/epa/epa2.rs index 26a06bbe..02357f08 100644 --- a/src/query/epa/epa2.rs +++ b/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; } } diff --git a/src/query/epa/epa3.rs b/src/query/epa/epa3.rs index 7c42ffb6..05afdd97 100644 --- a/src/query/epa/epa3.rs +++ b/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; } } diff --git a/src/query/gjk/gjk.rs b/src/query/gjk/gjk.rs index aab34fde..ebee30ae 100644 --- a/src/query/gjk/gjk.rs +++ b/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; } }