diff --git a/src/query/contact/contact_support_map_support_map.rs b/src/query/contact/contact_support_map_support_map.rs index 831ba246..aae2ace5 100644 --- a/src/query/contact/contact_support_map_support_map.rs +++ b/src/query/contact/contact_support_map_support_map.rs @@ -4,6 +4,7 @@ use crate::query::gjk::{self, CSOPoint, GJKResult, VoronoiSimplex}; use crate::query::Contact; use crate::shape::SupportMap; +use log::warn; use na::Unit; /// Contact between support-mapped shapes (`Cuboid`, `ConvexHull`, etc.) @@ -19,12 +20,16 @@ where { let simplex = &mut VoronoiSimplex::new(); match contact_support_map_support_map_with_params(pos12, g1, g2, prediction, simplex, None) { - GJKResult::ClosestPoints(point1, point2_1, normal1) => { + GJKResult::ClosestPoints(point1, point2_1, Ok(normal1)) => { let dist = (point2_1 - point1).dot(&normal1); let point2 = pos12.inverse_transform_point(&point2_1); let normal2 = pos12.inverse_transform_unit_vector(&-normal1); Some(Contact::new(point1, point2, normal1, normal2, dist)) } + GJKResult::ClosestPoints(_, _, Err(_)) => { + warn!("`contact_support_map_support_map` found the closest points on a degenerate face: verify your shapes' correctness."); + None + } GJKResult::NoIntersection(_) => None, GJKResult::Intersection => unreachable!(), GJKResult::Proximity(_) => unreachable!(), @@ -68,6 +73,7 @@ where // The point is inside of the CSO: use the fallback algorithm let mut epa = EPA::new(); + if let Some((p1, p2, n)) = epa.closest_points(pos12, g1, g2, simplex) { return GJKResult::ClosestPoints(p1, p2, n); } diff --git a/src/query/contact_manifolds/contact_manifolds_pfm_pfm.rs b/src/query/contact_manifolds/contact_manifolds_pfm_pfm.rs index bb13a838..20058300 100644 --- a/src/query/contact_manifolds/contact_manifolds_pfm_pfm.rs +++ b/src/query/contact_manifolds/contact_manifolds_pfm_pfm.rs @@ -79,7 +79,7 @@ pub fn contact_manifold_pfm_pfm<'a, ManifoldData, ContactData, S1, S2>( manifold.clear(); match contact { - GJKResult::ClosestPoints(p1, p2_1, dir) => { + GJKResult::ClosestPoints(p1, p2_1, Ok(dir)) => { let mut local_n1 = dir; let mut local_n2 = pos12.inverse_transform_unit_vector(&-dir); let dist = (p2_1 - p1).dot(&local_n1); diff --git a/src/query/epa/epa2.rs b/src/query/epa/epa2.rs index 02357f08..2ceb89c3 100644 --- a/src/query/epa/epa2.rs +++ b/src/query/epa/epa2.rs @@ -8,6 +8,7 @@ use num::Bounded; use crate::math::{Isometry, Point, Real, Vector}; use crate::query::gjk::{self, CSOPoint, ConstantOrigin, VoronoiSimplex}; +use crate::query::FaceDegenerate; use crate::shape::SupportMap; use crate::utils; @@ -52,7 +53,7 @@ impl Ord for FaceId { #[derive(Clone, Debug)] struct Face { pts: [usize; 2], - normal: Unit>, + normal: Result>, FaceDegenerate>, proj: Point, bcoords: [Real; 2], deleted: bool, @@ -83,10 +84,10 @@ impl Face { if let Some(n) = utils::ccw_face_normal([&vertices[pts[0]].point, &vertices[pts[1]].point]) { - normal = n; + normal = Ok(n); deleted = false; } else { - normal = Unit::new_unchecked(na::zero()); + normal = Err(FaceDegenerate); deleted = true; } @@ -158,7 +159,11 @@ impl EPA { g1: &G1, g2: &G2, simplex: &VoronoiSimplex, - ) -> Option<(Point, Point, Unit>)> + ) -> Option<( + Point, + Point, + Result>, FaceDegenerate>, + )> where G1: ?Sized + SupportMap, G2: ?Sized + SupportMap, @@ -213,7 +218,7 @@ impl EPA { } } - return Some((Point::origin(), Point::origin(), n)); + return Some((Point::origin(), Point::origin(), Ok(n))); } else if simplex.dimension() == 2 { let dp1 = self.vertices[1] - self.vertices[0]; let dp2 = self.vertices[2] - self.vertices[0]; @@ -235,18 +240,24 @@ impl EPA { self.faces.push(face3); if proj_is_inside1 { - let dist1 = self.faces[0].normal.dot(&self.vertices[0].point.coords); - self.heap.push(FaceId::new(0, -dist1)?); + if let Ok(normal) = self.faces[0].normal { + let dist1 = normal.dot(&self.vertices[0].point.coords); + self.heap.push(FaceId::new(0, -dist1)?); + } } if proj_is_inside2 { - let dist2 = self.faces[1].normal.dot(&self.vertices[1].point.coords); - self.heap.push(FaceId::new(1, -dist2)?); + if let Ok(normal) = self.faces[1].normal { + let dist2 = normal.dot(&self.vertices[1].point.coords); + self.heap.push(FaceId::new(1, -dist2)?); + } } if proj_is_inside3 { - let dist3 = self.faces[2].normal.dot(&self.vertices[2].point.coords); - self.heap.push(FaceId::new(2, -dist3)?); + if let Ok(normal) = self.faces[2].normal { + let dist3 = normal.dot(&self.vertices[2].point.coords); + self.heap.push(FaceId::new(2, -dist3)?); + } } } else { let pts1 = [0, 1]; @@ -265,8 +276,17 @@ impl EPA { pts2, )); - let dist1 = self.faces[0].normal.dot(&self.vertices[0].point.coords); - let dist2 = self.faces[1].normal.dot(&self.vertices[1].point.coords); + let dist1 = self.faces[0] + .normal + .as_ref() + .map(|normal| normal.dot(&self.vertices[0].point.coords)) + .unwrap_or(0.0); + + let dist2 = self.faces[1] + .normal + .as_ref() + .map(|normal| normal.dot(&self.vertices[1].point.coords)) + .unwrap_or(0.0); self.heap.push(FaceId::new(0, dist1)?); self.heap.push(FaceId::new(1, dist2)?); @@ -287,12 +307,15 @@ impl EPA { if face.deleted { continue; } + let Ok(face_normal) = face.normal else { + continue; + }; - let cso_point = CSOPoint::from_shapes(pos12, g1, g2, &face.normal); + let cso_point = CSOPoint::from_shapes(pos12, g1, g2, &face_normal); let support_point_id = self.vertices.len(); self.vertices.push(cso_point); - let candidate_max_dist = cso_point.point.coords.dot(&face.normal); + let candidate_max_dist = cso_point.point.coords.dot(&face_normal); if candidate_max_dist < max_dist { best_face_id = face_id; @@ -308,7 +331,7 @@ impl EPA { { 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)); + return Some((cpts.0, cpts.1, best_face.normal.clone())); } old_dist = curr_dist; @@ -323,12 +346,17 @@ impl EPA { for f in new_faces.iter() { if f.1 { - let dist = f.0.normal.dot(&f.0.proj.coords); + let new_face_normal = + f.0.normal + .clone() + .unwrap_or(Unit::new_unchecked(Vector::zeros())); + + let dist = new_face_normal.dot(&f.0.proj.coords); if dist < curr_dist { // TODO: if we reach this point, there were issues due to // numerical errors. let cpts = f.0.closest_points(&self.vertices); - return Some((cpts.0, cpts.1, f.0.normal)); + return Some((cpts.0, cpts.1, Ok(new_face_normal))); } if !f.0.deleted { @@ -349,7 +377,7 @@ impl EPA { let best_face = &self.faces[best_face_id.id]; let cpts = best_face.closest_points(&self.vertices); - Some((cpts.0, cpts.1, best_face.normal)) + Some((cpts.0, cpts.1, best_face.normal.clone())) } } diff --git a/src/query/epa/epa3.rs b/src/query/epa/epa3.rs index 9f8bdc90..c195a1c2 100644 --- a/src/query/epa/epa3.rs +++ b/src/query/epa/epa3.rs @@ -2,7 +2,7 @@ use crate::math::{Isometry, Point, Real, Vector}; use crate::query::gjk::{self, CSOPoint, ConstantOrigin, VoronoiSimplex}; -use crate::query::PointQueryWithLocation; +use crate::query::{FaceDegenerate, PointQueryWithLocation}; use crate::shape::{SupportMap, Triangle, TrianglePointLocation}; use crate::utils; use na::{self, Unit}; @@ -52,7 +52,7 @@ impl Ord for FaceId { struct Face { pts: [usize; 3], adj: [usize; 3], - normal: Unit>, + normal: Result>, FaceDegenerate>, bcoords: [Real; 3], deleted: bool, } @@ -71,13 +71,9 @@ impl Face { &vertices[pts[1]].point, &vertices[pts[2]].point, ]) { - normal = n; + normal = Ok(n); } else { - // This is a bit of a hack for degenerate faces. - // TODO: It will work OK with our current code, though - // we should do this in another way to avoid any risk - // of misusing the face normal in the future. - normal = Unit::new_unchecked(na::zero()); + normal = Err(FaceDegenerate); } Face { @@ -149,8 +145,13 @@ impl Face { // have a zero normal, causing the dot product to be zero. // So return true for these case will let us skip the triangle // during silhouette computation. - (*pt - *p0).dot(&self.normal) >= -gjk::eps_tol() - || Triangle::new(*p1, *p2, *pt).is_affinely_dependent() + match &self.normal { + Ok(normal) => { + (*pt - *p0).dot(normal) >= -gjk::eps_tol() + || Triangle::new(*p1, *p2, *pt).is_affinely_dependent() + } + Err(_) => true, + } } } @@ -215,7 +216,11 @@ impl EPA { g1: &G1, g2: &G2, simplex: &VoronoiSimplex, - ) -> Option<(Point, Point, Unit>)> + ) -> Option<( + Point, + Point, + Result>, FaceDegenerate>, + )> where G1: ?Sized + SupportMap, G2: ?Sized + SupportMap, @@ -235,7 +240,7 @@ impl EPA { if simplex.dimension() == 0 { let mut n: Vector = na::zero(); n[1] = 1.0; - return Some((Point::origin(), Point::origin(), Unit::new_unchecked(n))); + return Some((Point::origin(), Point::origin(), Ok(Unit::new_unchecked(n)))); } else if simplex.dimension() == 3 { let dp1 = self.vertices[1] - self.vertices[0]; let dp2 = self.vertices[2] - self.vertices[0]; @@ -266,23 +271,31 @@ impl EPA { self.faces.push(face4); if proj_inside1 { - let dist1 = self.faces[0].normal.dot(&self.vertices[0].point.coords); - self.heap.push(FaceId::new(0, -dist1)?); + if let Ok(normal) = self.faces[0].normal { + let dist1 = normal.dot(&self.vertices[0].point.coords); + self.heap.push(FaceId::new(0, -dist1)?); + } } if proj_inside2 { - let dist2 = self.faces[1].normal.dot(&self.vertices[1].point.coords); - self.heap.push(FaceId::new(1, -dist2)?); + if let Ok(normal) = self.faces[1].normal { + let dist2 = normal.dot(&self.vertices[1].point.coords); + self.heap.push(FaceId::new(1, -dist2)?); + } } if proj_inside3 { - let dist3 = self.faces[2].normal.dot(&self.vertices[2].point.coords); - self.heap.push(FaceId::new(2, -dist3)?); + if let Ok(normal) = self.faces[2].normal { + let dist3 = normal.dot(&self.vertices[2].point.coords); + self.heap.push(FaceId::new(2, -dist3)?); + } } if proj_inside4 { - let dist4 = self.faces[3].normal.dot(&self.vertices[3].point.coords); - self.heap.push(FaceId::new(3, -dist4)?); + if let Ok(normal) = self.faces[3].normal { + let dist4 = normal.dot(&self.vertices[3].point.coords); + self.heap.push(FaceId::new(3, -dist4)?); + } } } else { if simplex.dimension() == 1 { @@ -323,17 +336,17 @@ impl EPA { while let Some(face_id) = self.heap.pop() { // Create new faces. let face = self.faces[face_id.id].clone(); - if face.deleted { continue; } - - let cso_point = CSOPoint::from_shapes(pos12, g1, g2, &face.normal); + let Ok(face_normal) = &face.normal else { + continue; + }; + let cso_point = CSOPoint::from_shapes(pos12, g1, g2, face_normal); + let candidate_max_dist = cso_point.point.coords.dot(face_normal); let support_point_id = self.vertices.len(); self.vertices.push(cso_point); - let candidate_max_dist = cso_point.point.coords.dot(&face.normal); - if candidate_max_dist < max_dist { best_face_id = face_id; max_dist = candidate_max_dist; @@ -347,8 +360,9 @@ impl EPA { ((curr_dist - old_dist).abs() < _eps && candidate_max_dist < max_dist) { let best_face = &self.faces[best_face_id.id]; + let best_face_normal = face.normal.unwrap_or(Unit::new_unchecked(Vector::zeros())); let points = best_face.closest_points(&self.vertices); - return Some((points.0, points.1, best_face.normal)); + return Some((points.0, points.1, Ok(best_face_normal))); } old_dist = curr_dist; @@ -388,12 +402,16 @@ impl EPA { if new_face.1 { let pt = self.vertices[self.faces[new_face_id].pts[0]].point.coords; - let dist = self.faces[new_face_id].normal.dot(&pt); + let new_face_normal = self.faces[new_face_id] + .normal + .clone() + .unwrap_or(Unit::new_unchecked(Vector::zeros())); + let dist = new_face_normal.dot(&pt); if dist < curr_dist { // TODO: if we reach this point, there were issues due to // numerical errors. let points = face.closest_points(&self.vertices); - return Some((points.0, points.1, face.normal)); + return Some((points.0, points.1, Ok(new_face_normal))); } self.heap.push(FaceId::new(new_face_id, -dist)?); @@ -423,7 +441,7 @@ impl EPA { let best_face = &self.faces[best_face_id.id]; let points = best_face.closest_points(&self.vertices); - Some((points.0, points.1, best_face.normal)) + Some((points.0, points.1, best_face.normal.clone())) } fn compute_silhouette(&mut self, point: usize, id: usize, opp_pt_id: usize) { diff --git a/src/query/gjk/gjk.rs b/src/query/gjk/gjk.rs index ebee30ae..88d4d89a 100644 --- a/src/query/gjk/gjk.rs +++ b/src/query/gjk/gjk.rs @@ -3,6 +3,7 @@ use na::{self, ComplexField, Unit}; use crate::query::gjk::{CSOPoint, ConstantOrigin, VoronoiSimplex}; +use crate::query::FaceDegenerate; use crate::shape::SupportMap; // use query::Proximity; use crate::math::{Isometry, Point, Real, Vector, DIM}; @@ -19,7 +20,11 @@ pub enum GJKResult { /// /// Both points and vector are expressed in the local-space of the first geometry involved /// in the GJK execution. - ClosestPoints(Point, Point, Unit>), + ClosestPoints( + Point, + Point, + Result>, FaceDegenerate>, + ), /// Result of the GJK algorithm when the origin is too close to the polytope but not inside of it. /// /// The returned vector is expressed in the local-space of the first geometry involved in the @@ -125,7 +130,7 @@ where if max_bound >= old_max_bound { if exact_dist { let (p1, p2) = result(simplex, true); - return GJKResult::ClosestPoints(p1, p2, old_dir); // upper bounds inconsistencies + return GJKResult::ClosestPoints(p1, p2, Ok(old_dir)); // upper bounds inconsistencies } else { return GJKResult::Proximity(old_dir); } @@ -143,7 +148,7 @@ where } else if max_bound - min_bound <= _eps_rel * max_bound { if exact_dist { let (p1, p2) = result(simplex, false); - return GJKResult::ClosestPoints(p1, p2, dir); // the distance found has a good enough precision + return GJKResult::ClosestPoints(p1, p2, Ok(dir)); // the distance found has a good enough precision } else { return GJKResult::Proximity(dir); } @@ -152,7 +157,7 @@ where if !simplex.add_point(cso_point) { if exact_dist { let (p1, p2) = result(simplex, false); - return GJKResult::ClosestPoints(p1, p2, dir); + return GJKResult::ClosestPoints(p1, p2, Ok(dir)); } else { return GJKResult::Proximity(dir); } @@ -165,7 +170,7 @@ where if min_bound >= _eps_tol { if exact_dist { let (p1, p2) = result(simplex, true); - return GJKResult::ClosestPoints(p1, p2, old_dir); + return GJKResult::ClosestPoints(p1, p2, Ok(old_dir)); } else { // NOTE: previous implementation used old_proj here. return GJKResult::Proximity(old_dir); diff --git a/src/query/mod.rs b/src/query/mod.rs index ed6e1537..aa6cfaa4 100644 --- a/src/query/mod.rs +++ b/src/query/mod.rs @@ -80,3 +80,7 @@ pub mod details { pub use super::ray::*; pub use super::shape_cast::*; } + +/// Represents a degenerate Face normal. +#[derive(Clone, Debug, PartialEq, Eq)] +pub struct FaceDegenerate; diff --git a/src/shape/convex_polyhedron.rs b/src/shape/convex_polyhedron.rs index da03d7ec..a357ca8f 100644 --- a/src/shape/convex_polyhedron.rs +++ b/src/shape/convex_polyhedron.rs @@ -117,7 +117,7 @@ impl ConvexPolyhedron { /// This explicitly computes the convex hull of the given set of points. Use /// Returns `None` if the convex hull computation failed. pub fn from_convex_hull(points: &[Point]) -> Option { - let (vertices, indices) = crate::transformation::convex_hull(points); + let (vertices, indices) = crate::transformation::try_convex_hull(points).ok()?; Self::from_convex_mesh(vertices, &indices) }