more generic approach to colinear points in project_local_point

Author: ThierryBerger

src/query/epa/mod.rs

@@ -58,21 +58,22 @@ mod test {
             point_dim(100.0, 0.0),
             point_dim(160.0, 0.0),
         );
+        // point considered "inside" (on the line)
         let res = triangle.project_point_and_get_location(
             &Isometry::identity(),
             &point_dim(10.0, 0.0),
             false,
         );
         assert!(res.0.is_inside);
         res.0.point.iter().for_each(|p| assert!(p.is_finite()));
+
+        // point outside
         let res = triangle.project_point_and_get_location(
             &Isometry::identity(),
             &point_dim(10.0, 10.0),
             false,
         );
-        // FIXME: This assert is currently failing !
-        // False negative for inside on edge may be understandable due to floating points imprecisions,
-        // But false positives for points outside is a problem. (maybe related to triangle orientation?)
+
         assert!(res.0.is_inside == false);
         res.0.point.iter().for_each(|p| assert!(p.is_finite()));
     }

src/query/point/point_triangle.rs

@@ -1,5 +1,3 @@
-use std::dbg;
-
 use crate::math::{Point, Real, Vector, DIM};
 use crate::query::{PointProjection, PointQuery, PointQueryWithLocation};
 use crate::shape::{FeatureId, Segment, Triangle, TrianglePointLocation};
@@ -57,60 +55,19 @@ impl PointQueryWithLocation for Triangle {
     type Location = TrianglePointLocation;
 
     #[inline]
-    fn project_local_point_and_get_location(
-        &self,
-        pt: &Point<Real>,
-        solid: bool,
-    ) -> (PointProjection, Self::Location) {
+    fn triangle(&self, pt: &Point<Real>, solid: bool) -> (PointProjection, Self::Location) {
         // To understand the ideas, consider reading the slides below
         // https://box2d.org/files/ErinCatto_GJK_GDC2010.pdf
 
         let a = self.a;
         let b = self.b;
         let c = self.c;
 
-        // Additional checks for same point triangles
-        // TODO: Check for flat triangles (collinear points) ?
-        if a == b || b == c {
-            let pos = Segment::new(a, c).project_local_point_and_get_location(pt, solid);
-
-            return (
-                pos.0,
-                match pos.1 {
-                    crate::shape::SegmentPointLocation::OnVertex(v) => {
-                        TrianglePointLocation::OnVertex(if v == 1 { 2 } else { 0 })
-                    }
-                    crate::shape::SegmentPointLocation::OnEdge(v) => TrianglePointLocation::OnEdge(
-                        // could be 0 or 1, but we're on a flat triangle which is a bit degenerate.
-                        1, v,
-                    ),
-                },
-            );
-        }
-        if a == c {
-            let pos = Segment::new(a, b).project_local_point_and_get_location(pt, solid);
-
-            return (
-                pos.0,
-                match pos.1 {
-                    crate::shape::SegmentPointLocation::OnVertex(v) => {
-                        TrianglePointLocation::OnVertex(v)
-                    }
-                    crate::shape::SegmentPointLocation::OnEdge(v) => TrianglePointLocation::OnEdge(
-                        // could be 1 or 2, but we're on a flat triangle which is a bit degenerate.
-                        0, v,
-                    ),
-                },
-            );
-        }
-
         let ab = b - a;
         let ac = c - a;
         let ap = pt - a;
-        std::dbg!(ab, ac, ap);
         let ab_ap = ab.dot(&ap);
         let ac_ap = ac.dot(&ap);
-        std::dbg!(ab_ap, ac_ap);
 
         if ab_ap <= 0.0 && ac_ap <= 0.0 {
             // Voronoï region of `a`.
@@ -120,7 +77,6 @@ impl PointQueryWithLocation for Triangle {
         let bp = pt - b;
         let ab_bp = ab.dot(&bp);
         let ac_bp = ac.dot(&bp);
-        std::dbg!(bp, ab_bp, ac_bp);
 
         if ab_bp >= 0.0 && ac_bp <= ab_bp {
             // Voronoï region of `b`.
@@ -130,7 +86,6 @@ impl PointQueryWithLocation for Triangle {
         let cp = pt - c;
         let ab_cp = ab.dot(&cp);
         let ac_cp = ac.dot(&cp);
-        std::dbg!(cp, ab_cp, ac_cp);
 
         if ac_cp >= 0.0 && ab_cp <= ac_cp {
             // Voronoï region of `c`.
@@ -256,23 +211,73 @@ impl PointQueryWithLocation for Triangle {
                 );
             }
             ProjectionInfo::OnFace(face_side, va, vb, vc) => {
-                // Voronoï region of the face.
-                if DIM != 2 {
-                    // NOTE: in some cases, numerical instability
-                    // may result in the denominator being zero
-                    // when the triangle is nearly degenerate.
-                    if va + vb + vc != 0.0 {
-                        let denom = 1.0 / (va + vb + vc);
-                        let v = vb * denom;
-                        let w = vc * denom;
-                        let bcoords = [1.0 - v - w, v, w];
-                        let res = a + ab * v + ac * w;
-
+                let inv_denom = va + vb + vc;
+                if inv_denom == 0.0 {
+                    // The triangle is degenerate (the three points are collinear).
+                    // So we find the longest segment and project point on it.
+
+                    // TODO: this can probably be optimized using data we already have ?
+                    let length_sq_ab = ab.magnitude_squared();
+                    let length_sq_ac = ac.magnitude_squared();
+                    let length_sq_bc = bc.magnitude_squared();
+
+                    if length_sq_ab >= length_sq_ac && length_sq_ab >= length_sq_bc {
+                        // AB is longest edge.
+                        let pos =
+                            Segment::new(a, b).project_local_point_and_get_location(pt, solid);
+                        return (
+                            pos.0,
+                            match pos.1 {
+                                crate::shape::SegmentPointLocation::OnVertex(v) => {
+                                    TrianglePointLocation::OnVertex(v)
+                                }
+                                crate::shape::SegmentPointLocation::OnEdge(v) => {
+                                    TrianglePointLocation::OnEdge(0, v)
+                                }
+                            },
+                        );
+                    } else if length_sq_ac >= length_sq_ab && length_sq_ac >= length_sq_bc {
+                        // AC is longest edge.
+                        let pos =
+                            Segment::new(a, c).project_local_point_and_get_location(pt, solid);
                         return (
-                            compute_result(pt, res),
-                            TrianglePointLocation::OnFace(face_side as u32, bcoords),
+                            pos.0,
+                            match pos.1 {
+                                crate::shape::SegmentPointLocation::OnVertex(v) => {
+                                    TrianglePointLocation::OnVertex(v)
+                                }
+                                crate::shape::SegmentPointLocation::OnEdge(v) => {
+                                    TrianglePointLocation::OnEdge(1, v)
+                                }
+                            },
+                        );
+                    } else {
+                        // BC is longest edge.
+                        let pos =
+                            Segment::new(b, c).project_local_point_and_get_location(pt, solid);
+                        return (
+                            pos.0,
+                            match pos.1 {
+                                crate::shape::SegmentPointLocation::OnVertex(v) => {
+                                    TrianglePointLocation::OnVertex(v)
+                                }
+                                crate::shape::SegmentPointLocation::OnEdge(v) => {
+                                    TrianglePointLocation::OnEdge(2, v)
+                                }
+                            },
                         );
                     }
+                } else if DIM != 2 {
+                    let denom = 1.0 / inv_denom;
+                    let v = vb * denom;
+                    let w = vc * denom;
+                    let bcoords = [1.0 - v - w, v, w];
+                    let res = a + ab * v + ac * w;
+
+                    return (
+                        compute_result(pt, res),
+                        TrianglePointLocation::OnFace(face_side as u32, bcoords),
+                    );
                 }
             }
         }
@@ -286,11 +291,11 @@ impl PointQueryWithLocation for Triangle {
             )
         } else {
             // We have to project on the closest edge.
+
             // TODO: this might be optimizable.
             // TODO: be careful with numerical errors.
             let v = ab_ap / (ab_ap - ab_bp); // proj on ab = a + ab * v
             let w = ac_ap / (ac_ap - ac_cp); // proj on ac = a + ac * w
-            dbg!((ac_bp - ab_bp) / (ac_bp - ab_bp + ab_cp - ac_cp));
             let u = (ac_bp - ab_bp) / (ac_bp - ab_bp + ab_cp - ac_cp); // proj on bc = b + bc * u
 
             let bc = c - b;