epa::Face::normal Degenerate variant.

src/query/contact/contact_support_map_support_map.rs

@@ -19,12 +19,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(_)) => {
+            // TODO: propagate the error.
+            None
+        }
         GJKResult::NoIntersection(_) => None,
         GJKResult::Intersection => unreachable!(),
         GJKResult::Proximity(_) => unreachable!(),
@@ -68,6 +72,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);
     }

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);

src/query/epa/epa2.rs

@@ -49,10 +49,14 @@ impl Ord for FaceId {
     }
 }
 
+/// Represents a degenerate [`Face`] normal.
+#[derive(Clone, Debug, PartialEq, Eq)]
+pub struct Degenerate;
+
 #[derive(Clone, Debug)]
 struct Face {
     pts: [usize; 2],
-    normal: Unit<Vector<Real>>,
+    normal: Result<Unit<Vector<Real>>, Degenerate>,
     proj: Point<Real>,
     bcoords: [Real; 2],
     deleted: bool,
@@ -83,10 +87,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(Degenerate);
             deleted = true;
         }
 
@@ -158,7 +162,11 @@ impl EPA {
         g1: &G1,
         g2: &G2,
         simplex: &VoronoiSimplex,
-    ) -> Option<(Point<Real>, Point<Real>, Unit<Vector<Real>>)>
+    ) -> Option<(
+        Point<Real>,
+        Point<Real>,
+        Result<Unit<Vector<Real>>, Degenerate>,
+    )>
     where
         G1: ?Sized + SupportMap,
         G2: ?Sized + SupportMap,
@@ -213,7 +221,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 +243,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 +279,16 @@ 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()
+                .unwrap_or(&Unit::new_unchecked(Vector::zeros()))
+                .dot(&self.vertices[0].point.coords);
+            let dist2 = self.faces[1]
+                .normal
+                .as_ref()
+                .unwrap_or(&Unit::new_unchecked(Vector::zeros()))
+                .dot(&self.vertices[1].point.coords);
 
             self.heap.push(FaceId::new(0, dist1)?);
             self.heap.push(FaceId::new(1, dist2)?);
@@ -287,12 +309,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 +333,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 +348,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 +379,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()))
     }
 }
 

src/query/epa/epa3.rs

@@ -48,11 +48,15 @@ impl Ord for FaceId {
     }
 }
 
+/// Represents a degenerate [`Face`] normal.
+#[derive(Clone, Debug, PartialEq, Eq)]
+pub struct Degenerate;
+
 #[derive(Clone, Debug)]
 struct Face {
     pts: [usize; 3],
     adj: [usize; 3],
-    normal: Unit<Vector<Real>>,
+    normal: Result<Unit<Vector<Real>>, Degenerate>,
     bcoords: [Real; 3],
     deleted: bool,
 }
@@ -71,13 +75,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(Degenerate);
         }
 
         Face {
@@ -149,8 +149,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 +220,11 @@ impl EPA {
         g1: &G1,
         g2: &G2,
         simplex: &VoronoiSimplex,
-    ) -> Option<(Point<Real>, Point<Real>, Unit<Vector<Real>>)>
+    ) -> Option<(
+        Point<Real>,
+        Point<Real>,
+        Result<Unit<Vector<Real>>, Degenerate>,
+    )>
     where
         G1: ?Sized + SupportMap,
         G2: ?Sized + SupportMap,
@@ -235,7 +244,7 @@ impl EPA {
         if simplex.dimension() == 0 {
             let mut n: Vector<Real> = 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 +275,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 {
@@ -327,13 +344,15 @@ impl EPA {
             if face.deleted {
                 continue;
             }
-
-            let cso_point = CSOPoint::from_shapes(pos12, g1, g2, &face.normal);
+            let face_normal = &face
+                .normal
+                .clone()
+                .unwrap_or(Unit::new_unchecked(Vector::zeros()));
+            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 +366,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 +408,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 +447,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) {

src/query/epa/mod.rs

@@ -1,9 +1,9 @@
 //! The EPA algorithm for penetration depth computation.
 //!
 #[cfg(feature = "dim2")]
-pub use self::epa2::EPA;
+pub use self::epa2::{Degenerate, EPA};
 #[cfg(feature = "dim3")]
-pub use self::epa3::EPA;
+pub use self::epa3::{Degenerate, EPA};
 
 #[cfg(feature = "dim2")]
 pub mod epa2;