Improve robustness of mesh/mesh intersection

CHANGELOG.md

@@ -5,17 +5,28 @@
 ### Added
 
 - Implement `::to_trimesh` in 2d for `Cuboid` and `Aabb`.
-- Implement `Shape::feature_normal_at_point` for `TriMesh` to retrieve the normal of a face, when passing a `FeatureId::Face`.
+- Implement `Shape::feature_normal_at_point` for `TriMesh` to retrieve the normal of a face, when passing a
+  `FeatureId::Face`.
+- Add `convex_polygons_intersection_points_with_tolerances`, `convex_polygons_intersection_with_tolerances`, and
+  `intersect_meshes_with_tolerances` that let the user specify tolerances value for the collinearity check.
 
 ### Modified
 
 - Propagate error information while creating a mesh and using functions making use of it (See #262):
-  - `TriMesh::new`
-  - `TriMesh::intersection_with_aabb`
-  - `SharedShape::trimesh`
-  - `SharedShape::trimesh_with_flags`
+    - `TriMesh::new`
+    - `TriMesh::intersection_with_aabb`
+    - `SharedShape::trimesh`
+    - `SharedShape::trimesh_with_flags`
 - `point_cloud_bounding_sphere_with_center` now returns a `BoundingSphere`.
 
+### Fixed
+
+- Fixed some robustness issues in mesh/mesh intersection when parts of both meshes overlap perfectly.
+- Improve robustness of convex polygons intersections when all the vertices of one polygon are located in either the
+  edges or vertices of the other polygon.
+- Fix incorrect orientation sometimes given to the polygon output by the convex polygon intersections when one of the
+  polygon is completely inside the other.
+
 ## v0.17.1
 
 ### Modified
@@ -293,7 +304,8 @@ This version was yanked. See the release notes for 0.13.3 instead.
   for most shapes.
 - Add the `parallel` feature that enables methods for the parallel traversal of Qbvh
   trees: `Qbvh::traverse_bvtt_parallel`,
-  `Qbvh::traverse_bvtt_node_parallel`, `Qbvh::traverse_depth_first_parallel`, `Qbvh::traverse_depth_first_node_parallel`.
+  `Qbvh::traverse_bvtt_node_parallel`, `Qbvh::traverse_depth_first_parallel`,
+  `Qbvh::traverse_depth_first_node_parallel`.
 
 ### Fixed
 

src/transformation/mesh_intersection/mesh_intersection.rs

@@ -8,12 +8,27 @@ use na::{Point3, Vector3};
 use rstar::RTree;
 use spade::{ConstrainedDelaunayTriangulation, InsertionError, Triangulation as _};
 use std::collections::BTreeMap;
-use std::collections::HashSet;
+use std::collections::{hash_map::Entry, HashMap, HashSet};
 #[cfg(feature = "wavefront")]
 use std::path::PathBuf;
 
+/// A triangle with indices sorted in increasing order for deduplication in a hashmap.
+///
+/// Note that when converting a `[u32; 3]` into a `HashableTriangleIndices`, the result’s orientation
+/// might not match the input’s.
+#[derive(Copy, Clone, PartialEq, Eq, Hash)]
+struct HashableTriangleIndices([u32; 3]);
+
+impl From<[u32; 3]> for HashableTriangleIndices {
+    fn from([a, b, c]: [u32; 3]) -> Self {
+        let (sa, sb, sc) = utils::sort3(&a, &b, &c);
+        HashableTriangleIndices([*sa, *sb, *sc])
+    }
+}
+
 /// Metadata that specifies thresholds to use when making construction choices
 /// in mesh intersections.
+#[derive(Copy, Clone, PartialEq, Debug)]
 pub struct MeshIntersectionTolerances {
     /// The smallest angle (in radians) that will be tolerated. A triangle with
     /// a smaller angle is considered degenerate and will be deleted.
@@ -25,9 +40,11 @@ pub struct MeshIntersectionTolerances {
     /// A multiplier coefficient to scale [`Self::global_insertion_epsilon`] when checking for
     /// point duplication within a single triangle.
     ///
-    /// Inside of an individual triangle the distance at which two points are considered
+    /// Inside an individual triangle the distance at which two points are considered
     /// to be the same is `global_insertion_epsilon * local_insertion_epsilon_mod`.
     pub local_insertion_epsilon_scale: Real,
+    /// Three points forming a triangle with an area smaller than this epsilon are considered collinear.
+    pub collinearity_epsilon: Real,
 }
 
 impl Default for MeshIntersectionTolerances {
@@ -37,6 +54,7 @@ impl Default for MeshIntersectionTolerances {
             angle_epsilon: (0.005 as Real).to_radians(), // 0.005 degrees
             global_insertion_epsilon: Real::EPSILON * 100.0,
             local_insertion_epsilon_scale: 10.,
+            collinearity_epsilon: Real::EPSILON * 100.0,
         }
     }
 }
@@ -75,7 +93,7 @@ pub fn intersect_meshes_with_tolerances(
     pos2: &Isometry<Real>,
     mesh2: &TriMesh,
     flip2: bool,
-    meta_data: MeshIntersectionTolerances,
+    tolerances: MeshIntersectionTolerances,
 ) -> Result<Option<TriMesh>, MeshIntersectionError> {
     if cfg!(debug_assertions) {
         mesh1.assert_half_edge_topology_is_valid();
@@ -115,7 +133,9 @@ pub fn intersect_meshes_with_tolerances(
         let tri1 = mesh1.triangle(*fid1);
         let tri2 = mesh2.triangle(*fid2).transformed(&pos12);
 
-        if super::triangle_triangle_intersection(&tri1, &tri2).is_some() {
+        if super::triangle_triangle_intersection(&tri1, &tri2, tolerances.collinearity_epsilon)
+            .is_some()
+        {
             intersections.push((*fid1, *fid2));
             let _ = deleted_faces1.insert(*fid1);
             let _ = deleted_faces2.insert(*fid2);
@@ -143,33 +163,40 @@ pub fn intersect_meshes_with_tolerances(
     // 4: Initialize a new mesh by inserting points into a set. Duplicate points should
     // hash to the same index.
     let mut point_set = RTree::<TreePoint, _>::new();
-    let mut topology_indices = Vec::new();
+    let mut topology_indices = HashMap::new();
     {
         let mut insert_point = |position: Point3<Real>| {
-            insert_into_set(position, &mut point_set, meta_data.global_insertion_epsilon) as u32
+            insert_into_set(
+                position,
+                &mut point_set,
+                tolerances.global_insertion_epsilon,
+            ) as u32
         };
         // Add the inside vertices and triangles from mesh1
         for mut face in new_indices1 {
             if flip1 {
                 face.swap(0, 1);
             }
-            topology_indices.push([
+
+            let idx = [
                 insert_point(pos1 * mesh1.vertices()[face[0] as usize]),
                 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);
         }
 
         // Add the inside vertices and triangles from mesh2
         for mut face in new_indices2 {
             if flip2 {
                 face.swap(0, 1);
             }
-            topology_indices.push([
+            let idx = [
                 insert_point(pos2 * mesh2.vertices()[face[0] as usize]),
                 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);
         }
     }
 
@@ -184,7 +211,8 @@ pub fn intersect_meshes_with_tolerances(
         let list1 = constraints1.entry(fid1).or_default();
         let list2 = constraints2.entry(fid2).or_default();
 
-        let intersection = super::triangle_triangle_intersection(&tri1, &tri2);
+        let intersection =
+            super::triangle_triangle_intersection(&tri1, &tri2, tolerances.collinearity_epsilon);
         if let Some(intersection) = intersection {
             match intersection {
                 TriangleTriangleIntersection::Segment { a, b } => {
@@ -219,7 +247,7 @@ pub fn intersect_meshes_with_tolerances(
         pos2,
         flip1,
         flip2,
-        &meta_data,
+        &tolerances,
         &mut point_set,
         &mut topology_indices,
     )?;
@@ -232,7 +260,7 @@ pub fn intersect_meshes_with_tolerances(
         pos1,
         flip2,
         flip1,
-        &meta_data,
+        &tolerances,
         &mut point_set,
         &mut topology_indices,
     )?;
@@ -243,7 +271,10 @@ pub fn intersect_meshes_with_tolerances(
     let vertices: Vec<_> = vertices.iter().map(|p| Point3::from(p.point)).collect();
 
     if !topology_indices.is_empty() {
-        Ok(Some(TriMesh::new(vertices, topology_indices)?))
+        Ok(Some(TriMesh::new(
+            vertices,
+            topology_indices.into_values().collect(),
+        )?))
     } else {
         Ok(None)
     }
@@ -558,28 +589,20 @@ fn is_triangle_degenerate(
         return true;
     }
 
-    let mut shortest_side = Real::MAX;
-    for i in 0..3 {
-        let p1 = triangle[i];
-        let p2 = triangle[(i + 1) % 3];
-
-        shortest_side = shortest_side.min((p1 - p2).norm());
-    }
-
-    let mut worse_projection_distance = Real::MAX;
     for i in 0..3 {
-        let dir = triangle[(i + 1) % 3] - triangle[(i + 2) % 3];
-        if dir.norm() < epsilon_distance {
+        let mut dir = triangle[(i + 1) % 3] - triangle[(i + 2) % 3];
+        if dir.normalize_mut() < epsilon_distance {
             return true;
         }
 
-        let dir = dir.normalize();
         let proj = triangle[(i + 2) % 3] + (triangle[i] - triangle[(i + 2) % 3]).dot(&dir) * dir;
 
-        worse_projection_distance = worse_projection_distance.min((proj - triangle[i]).norm());
+        if (proj - triangle[i]).norm() < epsilon_distance {
+            return true;
+        }
     }
 
-    worse_projection_distance < epsilon_distance
+    false
 }
 
 fn merge_triangle_sets(
@@ -592,10 +615,10 @@ fn merge_triangle_sets(
     flip2: bool,
     metadata: &MeshIntersectionTolerances,
     point_set: &mut RTree<TreePoint>,
-    topology_indices: &mut Vec<[u32; 3]>,
+    topology_indices: &mut HashMap<HashableTriangleIndices, [u32; 3]>,
 ) -> Result<(), MeshIntersectionError> {
     // For each triangle, and each constraint edge associated to that triangle,
-    // make a triangulation of the face and sort whether or not each generated
+    // make a triangulation of the face and sort whether each generated
     // sub-triangle is part of the intersection.
     // For each sub-triangle that is part of the intersection, add them to the
     // output mesh.
@@ -638,24 +661,53 @@ fn merge_triangle_sets(
                 .0;
 
             if flip2 ^ (projection.is_inside_eps(&center, epsilon)) {
-                topology_indices.push([
+                let mut new_tri_idx = [
                     insert_into_set(p1, point_set, epsilon) as u32,
                     insert_into_set(p2, point_set, epsilon) as u32,
                     insert_into_set(p3, point_set, epsilon) as u32,
-                ]);
+                ];
 
                 if flip1 {
-                    topology_indices.last_mut().unwrap().swap(0, 1)
+                    new_tri_idx.swap(0, 1)
                 }
 
-                let [id1, id2, id3] = topology_indices.last().unwrap();
-
                 // 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 id1 == id2 || id1 == id3 || id2 == id3 {
+                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]
+                {
                     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.
+                            let _ = e.remove();
+                        }
+                    }
+                }
             }
         }
     }

src/transformation/mesh_intersection/mesh_intersection_error.rs

@@ -1,13 +1,16 @@
 use crate::shape::TriMeshBuilderError;
 
+#[cfg(doc)]
+use crate::shape::{TriMesh, TriMeshFlags};
+
 /// Error indicating that a query is not supported between certain shapes
 #[derive(thiserror::Error, Debug, Copy, Clone, Eq, PartialEq)]
 pub enum MeshIntersectionError {
-    /// At least one of the meshes is missing its topology information. Call `mesh.compute_topology` on the mesh
-    #[error("at least one of the meshes is missing its topology information. Call `mesh.compute_topology` on the mesh")]
+    /// At least one of the meshes is missing its topology information. Ensure that the [`TriMeshFlags::ORIENTED`] flag is enabled on both meshes.
+    #[error("at least one of the meshes is missing its topology information. Ensure that the `TriMeshFlags::ORIENTED` flag is enabled on both meshes.")]
     MissingTopology,
-    /// At least one of the meshes is missing its pseudo-normals. Call `mesh.compute_pseudo_normals` on the mesh
-    #[error("at least one of the meshes is missing its pseudo-normals. Call `mesh.compute_pseudo_normals` on the mesh")]
+    /// At least one of the meshes is missing its pseudo-normals. Ensure that the [`TriMeshFlags::ORIENTED`] flag is enabled on both meshes.
+    #[error("at least one of the meshes is missing its pseudo-normals. Ensure that the `TriMeshFlags::ORIENTED` flag is enabled on both meshes.")]
     MissingPseudoNormals,
     /// Internal failure while intersecting two triangles
     #[error("internal failure while intersecting two triangles")]

src/transformation/mesh_intersection/mod.rs

@@ -1,4 +1,6 @@
-pub use self::mesh_intersection::intersect_meshes;
+pub use self::mesh_intersection::{
+    intersect_meshes, intersect_meshes_with_tolerances, MeshIntersectionTolerances,
+};
 pub use self::mesh_intersection_error::MeshIntersectionError;
 use triangle_triangle_intersection::*;