More mesh/mesh intersection and point-in-poly fixes

CHANGELOG.md

@@ -5,6 +5,8 @@
 ### Added
 
 - Implement `::to_trimesh` in 2d for `Cuboid` and `Aabb`.
+- Fix some edge-cases in `point_in_poly2d` for self-intersecting polygons.
+- Fix some edge-cases in mesh/mesh intersection that could result in degenerate triangles being generated.
 
 ### Modified
 

crates/parry2d/examples/point_in_poly2d.rs

@@ -10,27 +10,35 @@ const RENDER_SCALE: f32 = 30.0;
 #[macroquad::main("parry2d::utils::point_in_poly2d")]
 async fn main() {
     let mut spikes = spikes_polygon();
+    let mut squares = squares_polygon();
     let test_points = grid_points();
 
     let animation_rotation = UnitComplex::new(0.02);
-    let spikes_render_pos = Point2::new(screen_width() / 2.0, screen_height() / 2.0);
+    let polygon_render_pos = Point2::new(screen_width() / 2.0, screen_height() / 2.0);
+
+    for i in 0.. {
+        let polygon = if (i / 350) % 2 == 0 {
+            &mut spikes
+        } else {
+            &mut squares
+        };
 
-    loop {
         clear_background(BLACK);
 
-        spikes
+        polygon
             .iter_mut()
             .for_each(|pt| *pt = animation_rotation * *pt);
-        draw_polygon(&spikes, RENDER_SCALE, spikes_render_pos, BLUE);
+
+        draw_polygon(&polygon, RENDER_SCALE, polygon_render_pos, BLUE);
 
         /*
          * Compute polygon intersections.
          */
         for point in &test_points {
-            if point_in_poly2d(point, &spikes) {
-                draw_point(*point, RENDER_SCALE, spikes_render_pos, RED);
+            if point_in_poly2d(point, &polygon) {
+                draw_point(*point, RENDER_SCALE, polygon_render_pos, RED);
             } else {
-                draw_point(*point, RENDER_SCALE, spikes_render_pos, GREEN);
+                draw_point(*point, RENDER_SCALE, polygon_render_pos, GREEN);
             }
         }
 
@@ -60,6 +68,21 @@ fn spikes_polygon() -> Vec<Point2<f32>> {
     polygon
 }
 
+fn squares_polygon() -> Vec<Point2<f32>> {
+    let scale = 3.0;
+    [
+        Point2::new(-1.0, -1.0) * scale,
+        Point2::new(0.0, -1.0) * scale,
+        Point2::new(0.0, 1.0) * scale,
+        Point2::new(-2.0, 1.0) * scale,
+        Point2::new(-2.0, -2.0) * scale,
+        Point2::new(1.0, -2.0) * scale,
+        Point2::new(1.0, 2.0) * scale,
+        Point2::new(-1.0, 2.0) * scale,
+    ]
+    .to_vec()
+}
+
 fn grid_points() -> Vec<Point2<f32>> {
     let count = 40;
     let spacing = 0.6;

src/shape/trimesh.rs

@@ -10,7 +10,7 @@ use {crate::shape::Cuboid, crate::utils::SortedPair, na::Unit};
 use {
     crate::shape::composite_shape::SimdCompositeShape,
     crate::utils::hashmap::{Entry, HashMap},
-    std::collections::HashSet,
+    crate::utils::hashset::HashSet,
 };
 
 #[cfg(feature = "dim2")]
@@ -542,7 +542,7 @@ impl TriMesh {
         let mut vtx_to_id = HashMap::default();
         let mut new_vertices = Vec::with_capacity(self.vertices.len());
         let mut new_indices = Vec::with_capacity(self.indices.len());
-        let mut triangle_set = HashSet::new();
+        let mut triangle_set = HashSet::default();
 
         fn resolve_coord_id(
             coord: &Point<Real>,
@@ -693,7 +693,7 @@ impl TriMesh {
     }
 
     fn delete_bad_topology_triangles(&mut self) {
-        let mut half_edge_set = HashSet::new();
+        let mut half_edge_set = HashSet::default();
         let mut deleted_any = false;
 
         // First, create three half-edges for each face.

src/transformation/mesh_intersection/mesh_intersection.rs

@@ -6,11 +6,11 @@ use crate::shape::{TriMesh, Triangle};
 use crate::utils;
 use crate::utils::hashmap::Entry;
 use crate::utils::hashmap::HashMap;
+use crate::utils::hashset::HashSet;
 use na::{Point3, Vector3};
 use rstar::RTree;
 use spade::{ConstrainedDelaunayTriangulation, InsertionError, Triangulation as _};
 use std::collections::BTreeMap;
-use std::collections::HashSet;
 #[cfg(feature = "wavefront")]
 use std::path::PathBuf;
 
@@ -185,7 +185,10 @@ pub fn intersect_meshes_with_tolerances(
                 insert_point(pos1 * mesh1.vertices()[face[1] as usize]),
                 insert_point(pos1 * mesh1.vertices()[face[2] as usize]),
             ];
-            let _ = topology_indices.insert(idx.into(), idx);
+
+            if !is_topologically_degenerate(idx) {
+                insert_topology_indices(&mut topology_indices, idx);
+            }
         }
 
         // Add the inside vertices and triangles from mesh2
@@ -198,7 +201,10 @@ pub fn intersect_meshes_with_tolerances(
                 insert_point(pos2 * mesh2.vertices()[face[1] as usize]),
                 insert_point(pos2 * mesh2.vertices()[face[2] as usize]),
             ];
-            let _ = topology_indices.insert(idx.into(), idx);
+
+            if !is_topologically_degenerate(idx) {
+                insert_topology_indices(&mut topology_indices, idx);
+            }
         }
     }
 
@@ -676,50 +682,60 @@ fn merge_triangle_sets(
                 // This should *never* trigger. If it does
                 // it means the code has created a triangle with duplicate vertices,
                 // which means we encountered an unaccounted for edge case.
-                if new_tri_idx[0] == new_tri_idx[1]
-                    || new_tri_idx[0] == new_tri_idx[2]
-                    || new_tri_idx[1] == new_tri_idx[2]
-                {
+                if is_topologically_degenerate(new_tri_idx) {
                     return Err(MeshIntersectionError::DuplicateVertices);
                 }
 
-                // Insert in the hashmap with sorted indices to avoid adding duplicates.
-                // We also check if we don’t keep pairs of triangles that have the same
-                // set of indices but opposite orientations.
-                match topology_indices.entry(new_tri_idx.into()) {
-                    Entry::Vacant(e) => {
-                        let _ = e.insert(new_tri_idx);
-                    }
-                    Entry::Occupied(e) => {
-                        fn same_orientation(a: &[u32; 3], b: &[u32; 3]) -> bool {
-                            let ib = if a[0] == b[0] {
-                                0
-                            } else if a[0] == b[1] {
-                                1
-                            } else {
-                                2
-                            };
-                            a[1] == b[(ib + 1) % 3]
-                        }
-
-                        if !same_orientation(e.get(), &new_tri_idx) {
-                            // If we are inserting two identical triangles but with mismatching
-                            // orientations, we can just ignore both because they cover a degenerate
-                            // 2D plane.
-                            #[cfg(feature = "enhanced-determinism")]
-                            let _ = e.swap_remove();
-                            #[cfg(not(feature = "enhanced-determinism"))]
-                            let _ = e.remove();
-                        }
-                    }
-                }
+                insert_topology_indices(topology_indices, new_tri_idx);
             }
         }
     }
 
     Ok(())
 }
 
+// Insert in the hashmap with sorted indices to avoid adding duplicates.
+//
+// We also check if we don’t keep pairs of triangles that have the same
+// set of indices but opposite orientations. If this happens, both the new triangle, and the one it
+// matched with are removed (because they describe a degenerate piece of volume).
+fn insert_topology_indices(
+    topology_indices: &mut HashMap<HashableTriangleIndices, [u32; 3]>,
+    new_tri_idx: [u32; 3],
+) {
+    match topology_indices.entry(new_tri_idx.into()) {
+        Entry::Vacant(e) => {
+            let _ = e.insert(new_tri_idx);
+        }
+        Entry::Occupied(e) => {
+            fn same_orientation(a: &[u32; 3], b: &[u32; 3]) -> bool {
+                let ib = if a[0] == b[0] {
+                    0
+                } else if a[0] == b[1] {
+                    1
+                } else {
+                    2
+                };
+                a[1] == b[(ib + 1) % 3]
+            }
+
+            if !same_orientation(e.get(), &new_tri_idx) {
+                // If we are inserting two identical triangles but with mismatching
+                // orientations, we can just ignore both because they cover a degenerate
+                // 2D plane.
+                #[cfg(feature = "enhanced-determinism")]
+                let _ = e.swap_remove();
+                #[cfg(not(feature = "enhanced-determinism"))]
+                let _ = e.remove();
+            }
+        }
+    }
+}
+
+fn is_topologically_degenerate(tri_idx: [u32; 3]) -> bool {
+    tri_idx[0] == tri_idx[1] || tri_idx[0] == tri_idx[2] || tri_idx[1] == tri_idx[2]
+}
+
 #[cfg(feature = "wavefront")]
 #[cfg(test)]
 mod tests {

src/utils/point_in_poly2d.rs

@@ -50,18 +50,35 @@ pub fn point_in_poly2d(pt: &Point2<Real>, poly: &[Point2<Real>]) -> bool {
         let perp = dpt.perp(&seg_dir);
         winding += match (dpt.y >= 0.0, b.y > pt.y) {
             (true, true) if perp < 0.0 => 1,
-            (false, false) if perp > 0.0 => -1,
+            (false, false) if perp > 0.0 => 1,
             _ => 0,
         };
     }
 
-    winding != 0
+    winding % 2 == 1
 }
 
 #[cfg(test)]
 mod tests {
     use super::*;
 
+    #[test]
+    fn point_in_poly2d_self_intersecting() {
+        let poly = [
+            [-1.0, -1.0],
+            [0.0, -1.0],
+            [0.0, 1.0],
+            [-2.0, 1.0],
+            [-2.0, -2.0],
+            [1.0, -2.0],
+            [1.0, 2.0],
+            [-1.0, 2.0],
+        ]
+        .map(Point2::from);
+        assert!(!point_in_poly2d(&[-0.5, -0.5].into(), &poly));
+        assert!(point_in_poly2d(&[0.5, -0.5].into(), &poly));
+    }
+
     #[test]
     fn point_in_poly2d_concave() {
         let poly = [