From 4654845417e5b0a7400cd3855319d7daf3d9e5db Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?S=C3=A9bastien=20Crozet?= Date: Wed, 13 Nov 2024 12:03:14 +0100 Subject: [PATCH] Improve robustness of mesh/mesh intersection (#286) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit * fix: don’t insert duplicate triangles in mesh/mesh intersection * feat: make intersect_meshes_with_tolerances and MeshIntersectionTolerances public * fix some edge-cases of convex polygons intersections + rename its variables for better readability * chore: add docs to the MeshIntersectionError variants * chore: update changelog * feat: switch to a deterministic hashmap in the mesh intersection algorithm * chore: cargo fmt --- CHANGELOG.md | 24 +- .../mesh_intersection/mesh_intersection.rs | 125 ++++-- .../mesh_intersection_error.rs | 11 +- src/transformation/mesh_intersection/mod.rs | 4 +- .../triangle_triangle_intersection.rs | 137 +++++- src/transformation/mod.rs | 9 +- src/transformation/polygon_intersection.rs | 422 +++++++++++++----- 7 files changed, 554 insertions(+), 178 deletions(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index cbb3d64b..654f7ca0 100644 --- a/CHANGELOG.md +++ b/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 diff --git a/src/transformation/mesh_intersection/mesh_intersection.rs b/src/transformation/mesh_intersection/mesh_intersection.rs index 06b5cc48..b12db52a 100644 --- a/src/transformation/mesh_intersection/mesh_intersection.rs +++ b/src/transformation/mesh_intersection/mesh_intersection.rs @@ -4,16 +4,32 @@ use crate::query::point::point_query::PointQueryWithLocation; use crate::query::{visitors::BoundingVolumeIntersectionsSimultaneousVisitor, PointQuery}; use crate::shape::{TriMesh, Triangle}; use crate::utils; +use crate::utils::hashmap::HashMap; 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, 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 +41,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 +55,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 +94,7 @@ pub fn intersect_meshes_with_tolerances( pos2: &Isometry, mesh2: &TriMesh, flip2: bool, - meta_data: MeshIntersectionTolerances, + tolerances: MeshIntersectionTolerances, ) -> Result, MeshIntersectionError> { if cfg!(debug_assertions) { mesh1.assert_half_edge_topology_is_valid(); @@ -115,7 +134,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,21 +164,27 @@ 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::::new(); - let mut topology_indices = Vec::new(); + let mut topology_indices = HashMap::default(); { let mut insert_point = |position: Point3| { - 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 @@ -165,11 +192,12 @@ pub fn intersect_meshes_with_tolerances( 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 +212,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 +248,7 @@ pub fn intersect_meshes_with_tolerances( pos2, flip1, flip2, - &meta_data, + &tolerances, &mut point_set, &mut topology_indices, )?; @@ -232,7 +261,7 @@ pub fn intersect_meshes_with_tolerances( pos1, flip2, flip1, - &meta_data, + &tolerances, &mut point_set, &mut topology_indices, )?; @@ -243,7 +272,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 +590,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 +616,10 @@ fn merge_triangle_sets( flip2: bool, metadata: &MeshIntersectionTolerances, point_set: &mut RTree, - topology_indices: &mut Vec<[u32; 3]>, + topology_indices: &mut HashMap, ) -> 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 +662,53 @@ fn merge_triangle_sets( .0; if flip2 ^ (projection.is_inside_eps(¢er, 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(); + } + } + } } } } diff --git a/src/transformation/mesh_intersection/mesh_intersection_error.rs b/src/transformation/mesh_intersection/mesh_intersection_error.rs index 13904c3b..bae9ee15 100644 --- a/src/transformation/mesh_intersection/mesh_intersection_error.rs +++ b/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")] diff --git a/src/transformation/mesh_intersection/mod.rs b/src/transformation/mesh_intersection/mod.rs index 1a99d68a..a11d73b9 100644 --- a/src/transformation/mesh_intersection/mod.rs +++ b/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::*; diff --git a/src/transformation/mesh_intersection/triangle_triangle_intersection.rs b/src/transformation/mesh_intersection/triangle_triangle_intersection.rs index b787fa7e..7e3d6d01 100644 --- a/src/transformation/mesh_intersection/triangle_triangle_intersection.rs +++ b/src/transformation/mesh_intersection/triangle_triangle_intersection.rs @@ -1,9 +1,13 @@ use super::EPS; use crate::math::{Point, Real, Vector}; use crate::query; +use crate::query::PointQuery; use crate::shape::{FeatureId, Segment, Triangle}; -use crate::transformation::polygon_intersection::PolylinePointLocation; +use crate::transformation::polygon_intersection::{ + PolygonIntersectionTolerances, PolylinePointLocation, +}; use crate::utils::WBasis; +use na::Point2; #[derive(Copy, Clone, Debug, Default)] pub struct TriangleTriangleIntersectionPoint { @@ -28,9 +32,10 @@ impl Default for TriangleTriangleIntersection { } } -pub fn triangle_triangle_intersection( +pub(crate) fn triangle_triangle_intersection( tri1: &Triangle, tri2: &Triangle, + collinearity_epsilon: Real, ) -> Option { let normal1 = tri1.robust_normal(); let normal2 = tri2.robust_normal(); @@ -113,8 +118,7 @@ pub fn triangle_triangle_intersection( let unit_normal2 = normal2.normalize(); if (tri1.a - tri2.a).dot(&unit_normal2) < EPS { let basis = unit_normal2.orthonormal_basis(); - let proj = - |vect: Vector| na::Point2::new(vect.dot(&basis[0]), vect.dot(&basis[1])); + let proj = |vect: Vector| Point2::new(vect.dot(&basis[0]), vect.dot(&basis[1])); let mut intersections = vec![]; @@ -144,25 +148,39 @@ pub fn triangle_triangle_intersection( ), }; - crate::transformation::convex_polygons_intersection(&poly1, &poly2, |pt1, pt2| { - let intersection = match (pt1, pt2) { - (Some(loc1), Some(loc2)) => { - let (_f1, p1) = convert_loc(loc1, pts1); - let (_f2, _p2) = convert_loc(loc2, pts2); - TriangleTriangleIntersectionPoint { p1 } - } - (Some(loc1), None) => { - let (_f1, p1) = convert_loc(loc1, pts1); - TriangleTriangleIntersectionPoint { p1 } - } - (None, Some(loc2)) => { - let (_f2, p2) = convert_loc(loc2, pts2); - TriangleTriangleIntersectionPoint { p1: p2 } - } - (None, None) => unreachable!(), - }; - intersections.push(intersection); - }); + crate::transformation::convex_polygons_intersection_with_tolerances( + &poly1, + &poly2, + PolygonIntersectionTolerances { + collinearity_epsilon, + }, + |pt1, pt2| { + let intersection = match (pt1, pt2) { + (Some(loc1), Some(loc2)) => { + let (_f1, p1) = convert_loc(loc1, pts1); + let (_f2, _p2) = convert_loc(loc2, pts2); + TriangleTriangleIntersectionPoint { p1 } + } + (Some(loc1), None) => { + let (_f1, p1) = convert_loc(loc1, pts1); + TriangleTriangleIntersectionPoint { p1 } + } + (None, Some(loc2)) => { + let (_f2, p2) = convert_loc(loc2, pts2); + TriangleTriangleIntersectionPoint { p1: p2 } + } + (None, None) => unreachable!(), + }; + intersections.push(intersection); + }, + ); + + // NOTE: set this to `true` to automatically check if the computed intersection is + // valid, and print debug infos if it is not. + const DEBUG_INTERSECTIONS: bool = false; + if DEBUG_INTERSECTIONS { + debug_check_intersections(tri1, tri2, &basis, &poly1, &poly2, &intersections); + } Some(TriangleTriangleIntersection::Polygon(intersections)) } else { @@ -195,3 +213,76 @@ fn segment_plane_intersection( Some((segment.a + dir * time_of_impact, FeatureId::Edge(eid))) } } + +/// Prints debug information if the calulated intersection of two triangles is detected to be +/// invalid. +/// +/// If the intersection is valid, this prints nothing. If it isn’t valid, this will print a few +/// lines to copy/paste into the Desmos online graphing tool (for visual debugging), as well as +/// some rust code to add to the `tris` array in the `intersect_triangle_common_vertex` test for +/// regression checking. +fn debug_check_intersections( + tri1: &Triangle, + tri2: &Triangle, + basis: &[na::Vector3; 2], + poly1: &[Point2], // Projection of tri1 on the basis `basis1` with the origin at tri2.a. + poly2: &[Point2], // Projection of tri2 on the basis `basis2` with the origin at tri2.a. + intersections: &[TriangleTriangleIntersectionPoint], +) { + let proj = |vect: Vector| Point2::new(vect.dot(&basis[0]), vect.dot(&basis[1])); + let mut incorrect = false; + for pt in intersections { + if !tri1 + .project_local_point(&pt.p1, false) + .is_inside_eps(&pt.p1, 1.0e-5) + { + incorrect = true; + break; + } + + if !tri2 + .project_local_point(&pt.p1, false) + .is_inside_eps(&pt.p1, 1.0e-5) + { + incorrect = true; + break; + } + } + + if incorrect { + let proj_inter: Vec<_> = intersections.iter().map(|p| proj(p.p1 - tri2.a)).collect(); + println!("-------- (copy/paste the following on Desmos graphing)"); + println!("A=({:.2},{:.2})", poly1[0].x, poly1[0].y); + println!("B=({:.2},{:.2})", poly1[1].x, poly1[1].y); + println!("C=({:.2},{:.2})", poly1[2].x, poly1[2].y); + println!("D=({:.2},{:.2})", poly2[0].x, poly2[0].y); + println!("E=({:.2},{:.2})", poly2[1].x, poly2[1].y); + println!("F=({:.2},{:.2})", poly2[2].x, poly2[2].y); + + let lbls = ["G", "H", "I", "J", "K", "L", "M", "N", "O"]; + for (i, inter) in proj_inter.iter().enumerate() { + println!("{}=({:.2},{:.2})", lbls[i], inter.x, inter.y); + } + + // polygons + println!("X=polygon(A,B,C)"); + println!("Y=polygon(D,E,F)"); + print!("Z=polygon({}", lbls[0]); + for lbl in lbls.iter().skip(1) { + print!(",{}", lbl); + } + println!(")"); + + println!("~~~~~~~ (copy/paste the folliwing input in the `intersect_triangle_common_vertex` test)"); + println!("(Triangle::new("); + for pt1 in poly1 { + println!(" Point2::new({},{}),", pt1.x, pt1.y); + } + println!("),"); + println!("Triangle::new("); + for pt2 in poly2 { + println!(" Point2::new({},{}),", pt2.x, pt2.y); + } + println!("),),"); + } +} diff --git a/src/transformation/mod.rs b/src/transformation/mod.rs index ca974549..c79f5b3d 100644 --- a/src/transformation/mod.rs +++ b/src/transformation/mod.rs @@ -7,9 +7,14 @@ pub use self::convex_hull2::{convex_hull2 as convex_hull, convex_hull2_idx as co #[cfg(feature = "dim3")] pub use self::convex_hull3::{check_convex_hull, convex_hull, try_convex_hull, ConvexHullError}; #[cfg(feature = "dim3")] -pub use self::mesh_intersection::{intersect_meshes, MeshIntersectionError}; +pub use self::mesh_intersection::{ + intersect_meshes, intersect_meshes_with_tolerances, MeshIntersectionError, + MeshIntersectionTolerances, +}; pub use self::polygon_intersection::{ - convex_polygons_intersection, convex_polygons_intersection_points, polygons_intersection, + convex_polygons_intersection, convex_polygons_intersection_points, + convex_polygons_intersection_points_with_tolerances, + convex_polygons_intersection_with_tolerances, polygons_intersection, polygons_intersection_points, }; diff --git a/src/transformation/polygon_intersection.rs b/src/transformation/polygon_intersection.rs index 0619c6ac..a770ae4b 100644 --- a/src/transformation/polygon_intersection.rs +++ b/src/transformation/polygon_intersection.rs @@ -7,12 +7,28 @@ use crate::shape::{SegmentPointLocation, Triangle, TriangleOrientation}; use crate::utils::hashmap::HashMap; use crate::utils::{self, SegmentsIntersection}; -const EPS: Real = Real::EPSILON * 100.0; +#[derive(Copy, Clone, PartialEq, Debug)] +pub struct PolygonIntersectionTolerances { + /// The epsilon given to [`Triangle::orientation2d`] for detecting if three points are collinear. + /// + /// Three points forming a triangle with an area smaller than this value are considered collinear. + pub collinearity_epsilon: Real, +} + +impl Default for PolygonIntersectionTolerances { + fn default() -> Self { + Self { + collinearity_epsilon: Real::EPSILON * 100.0, + } + } +} #[derive(Copy, Clone, Debug, PartialEq, Eq)] enum InFlag { - PIn, - QIn, + // The current neighborhood of the traversed point on poly1 is inside poly2. + Poly1IsInside, + // The current neighborhood of the traversed point on poly2 is inside poly1. + Poly2IsInside, Unknown, } @@ -66,13 +82,27 @@ impl PolylinePointLocation { /// Computes the intersection points of two convex polygons. /// -/// The resulting polygon is output vertex-by-vertex to the `out` closure. +/// The resulting polygon is output vertex-by-vertex to the `out` vector. +/// This is the same as [`convex_polygons_intersection_points_with_tolerances`] with the tolerances +/// set to their default values. pub fn convex_polygons_intersection_points( poly1: &[Point2], poly2: &[Point2], out: &mut Vec>, ) { - convex_polygons_intersection(poly1, poly2, |loc1, loc2| { + convex_polygons_intersection_points_with_tolerances(poly1, poly2, Default::default(), out); +} + +/// Computes the intersection points of two convex polygons. +/// +/// The resulting polygon is output vertex-by-vertex to the `out` vector. +pub fn convex_polygons_intersection_points_with_tolerances( + poly1: &[Point2], + poly2: &[Point2], + tolerances: PolygonIntersectionTolerances, + out: &mut Vec>, +) { + convex_polygons_intersection_with_tolerances(poly1, poly2, tolerances, |loc1, loc2| { if let Some(loc1) = loc1 { out.push(loc1.to_point(poly1)) } else if let Some(loc2) = loc2 { @@ -84,78 +114,127 @@ pub fn convex_polygons_intersection_points( /// Computes the intersection of two convex polygons. /// /// The resulting polygon is output vertex-by-vertex to the `out` closure. +/// This is the same as [`convex_polygons_intersection_with_tolerances`] with the tolerances +/// set to their default values. pub fn convex_polygons_intersection( poly1: &[Point2], poly2: &[Point2], + out: impl FnMut(Option, Option), +) { + convex_polygons_intersection_with_tolerances(poly1, poly2, Default::default(), out) +} + +/// Computes the intersection of two convex polygons. +/// +/// The resulting polygon is output vertex-by-vertex to the `out` closure. +pub fn convex_polygons_intersection_with_tolerances( + poly1: &[Point2], + poly2: &[Point2], + tolerances: PolygonIntersectionTolerances, mut out: impl FnMut(Option, Option), ) { // TODO: this does not handle correctly the case where the // first triangle of the polygon is degenerate. let rev1 = poly1.len() > 2 - && Triangle::orientation2d(&poly1[0], &poly1[1], &poly1[2], EPS) - == TriangleOrientation::Clockwise; + && Triangle::orientation2d( + &poly1[0], + &poly1[1], + &poly1[2], + tolerances.collinearity_epsilon, + ) == TriangleOrientation::Clockwise; let rev2 = poly2.len() > 2 - && Triangle::orientation2d(&poly2[0], &poly2[1], &poly2[2], EPS) - == TriangleOrientation::Clockwise; - - let n = poly1.len(); - let m = poly2.len(); - - let mut a = 0; - let mut b = 0; - let mut aa = 0; - let mut ba = 0; + && Triangle::orientation2d( + &poly2[0], + &poly2[1], + &poly2[2], + tolerances.collinearity_epsilon, + ) == TriangleOrientation::Clockwise; + + let len1 = poly1.len(); + let len2 = poly2.len(); + + let mut i1 = 0; // Current index on the first polyline. + let mut i2 = 0; // Current index on the second polyline. + let mut nsteps1 = 0; // Number of times we advanced on the first polyline. + let mut nsteps2 = 0; // Number of times we advanced on the second polyline. let mut inflag = InFlag::Unknown; let mut first_point_found = false; // Quit when both adv. indices have cycled, or one has cycled twice. - while (aa < n || ba < m) && aa < 2 * n && ba < 2 * m { - let (a1, a2) = if rev1 { - ((n - a) % n, n - a - 1) + while (nsteps1 < len1 || nsteps2 < len2) && nsteps1 < 2 * len1 && nsteps2 < 2 * len2 { + let (a1, b1) = if rev1 { + ((len1 - i1) % len1, len1 - i1 - 1) } else { - ((a + n - 1) % n, a) + // Point before `i1`, and point at `i1`. + ((i1 + len1 - 1) % len1, i1) }; - let (b1, b2) = if rev2 { - ((m - b) % m, m - b - 1) + let (a2, b2) = if rev2 { + ((len2 - i2) % len2, len2 - i2 - 1) } else { - ((b + m - 1) % m, b) + // Point before `i2`, and point at `i2`. + ((i2 + len2 - 1) % len2, i2) }; - let dir_edge1 = poly1[a2] - poly1[a1]; - let dir_edge2 = poly2[b2] - poly2[b1]; + let dir_edge1 = poly1[b1] - poly1[a1]; + let dir_edge2 = poly2[b2] - poly2[a2]; + // If there is an intersection, this will determine if the edge from poly2 is transitioning + // Left -> Right (CounterClockwise) or Right -> Left (Clockwise) relative to the edge from + // poly1. let cross = Triangle::orientation2d( &Point2::origin(), &Point2::from(dir_edge1), &Point2::from(dir_edge2), - EPS, + tolerances.collinearity_epsilon, + ); + // Determines if b1 is left (CounterClockwise) or right (Clockwise) of [a2, b2]. + let a2_b2_b1 = Triangle::orientation2d( + &poly2[a2], + &poly2[b2], + &poly1[b1], + tolerances.collinearity_epsilon, + ); + // Determines if b2 is left (CounterClockwise) or right (Clockwise) of [a1, b1]. + let a1_b1_b2 = Triangle::orientation2d( + &poly1[a1], + &poly1[b1], + &poly2[b2], + tolerances.collinearity_epsilon, ); - let a_hb = Triangle::orientation2d(&poly2[b1], &poly2[b2], &poly1[a2], EPS); - let b_ha = Triangle::orientation2d(&poly1[a1], &poly1[a2], &poly2[b2], EPS); // If edge1 & edge2 intersect, update inflag. - if let Some(inter) = - utils::segments_intersection2d(&poly1[a1], &poly1[a2], &poly2[b1], &poly2[b2], EPS) - { + if let Some(inter) = utils::segments_intersection2d( + &poly1[a1], + &poly1[b1], + &poly2[a2], + &poly2[b2], + tolerances.collinearity_epsilon, + ) { match inter { SegmentsIntersection::Point { loc1, loc2 } => { - let loc1 = PolylinePointLocation::from_segment_point_location(a1, a2, loc1); - let loc2 = PolylinePointLocation::from_segment_point_location(b1, b2, loc2); - out(Some(loc1), Some(loc2)); - - if inflag == InFlag::Unknown && !first_point_found { - // This is the first point. - aa = 0; - ba = 0; - first_point_found = true; - } + if a2_b2_b1 != TriangleOrientation::Degenerate + && a1_b1_b2 != TriangleOrientation::Degenerate + { + let loc1 = PolylinePointLocation::from_segment_point_location(a1, b1, loc1); + let loc2 = PolylinePointLocation::from_segment_point_location(a2, b2, loc2); + out(Some(loc1), Some(loc2)); + + if inflag == InFlag::Unknown && !first_point_found { + // This is the first point, reset the number of steps since we are + // effectively starting the actual traversal now. + nsteps1 = 0; + nsteps2 = 0; + first_point_found = true; + } - // Update inflag. - if a_hb == TriangleOrientation::CounterClockwise { - inflag = InFlag::PIn; - } else if b_ha == TriangleOrientation::CounterClockwise { - inflag = InFlag::QIn; + if a2_b2_b1 == TriangleOrientation::CounterClockwise { + // The point b1 is left of [a2, b2] so it is inside poly2 ??? + inflag = InFlag::Poly1IsInside; + } else if a1_b1_b2 == TriangleOrientation::CounterClockwise { + // The point b2 is left of [a1, b1] so it is inside poly1 ??? + inflag = InFlag::Poly2IsInside; + } } } SegmentsIntersection::Segment { @@ -164,20 +243,21 @@ pub fn convex_polygons_intersection( second_loc1, second_loc2, } => { - // Special case: edge1 & edge2 overlap and oppositely oriented. if dir_edge1.dot(&dir_edge2) < 0.0 { + // Special case: edge1 & edge2 overlap and oppositely oriented. The + // intersection is degenerate (equals to a segment). Output + // the segment and exit. let loc1 = - PolylinePointLocation::from_segment_point_location(a1, a2, first_loc1); + PolylinePointLocation::from_segment_point_location(a1, b1, first_loc1); let loc2 = - PolylinePointLocation::from_segment_point_location(b1, b2, first_loc2); + PolylinePointLocation::from_segment_point_location(a2, b2, first_loc2); out(Some(loc1), Some(loc2)); let loc1 = - PolylinePointLocation::from_segment_point_location(a1, a2, second_loc1); + PolylinePointLocation::from_segment_point_location(a1, b1, second_loc1); let loc2 = - PolylinePointLocation::from_segment_point_location(b1, b2, second_loc2); + PolylinePointLocation::from_segment_point_location(a2, b2, second_loc2); out(Some(loc1), Some(loc2)); - return; } } @@ -186,48 +266,49 @@ pub fn convex_polygons_intersection( // Special case: edge1 & edge2 parallel and separated. if cross == TriangleOrientation::Degenerate - && a_hb == TriangleOrientation::Clockwise - && b_ha == TriangleOrientation::Clockwise + && a2_b2_b1 == TriangleOrientation::Clockwise + && a1_b1_b2 == TriangleOrientation::Clockwise + // TODO: should this also include any case where a2_b2_b1 and a1_b1_b2 are both different from Degenerate? { return; } // Special case: edge1 & edge2 collinear. else if cross == TriangleOrientation::Degenerate - && a_hb == TriangleOrientation::Degenerate - && b_ha == TriangleOrientation::Degenerate + && a2_b2_b1 == TriangleOrientation::Degenerate + && a1_b1_b2 == TriangleOrientation::Degenerate { // Advance but do not output point. - if inflag == InFlag::PIn { - b = advance(b, &mut ba, m); + if inflag == InFlag::Poly1IsInside { + i2 = advance(i2, &mut nsteps2, len2); } else { - a = advance(a, &mut aa, n); + i1 = advance(i1, &mut nsteps1, len1); } } // Generic cases. else if cross == TriangleOrientation::CounterClockwise { - if b_ha == TriangleOrientation::CounterClockwise { - if inflag == InFlag::PIn { - out(Some(PolylinePointLocation::OnVertex(a2)), None) + if a1_b1_b2 == TriangleOrientation::CounterClockwise { + if inflag == InFlag::Poly1IsInside { + out(Some(PolylinePointLocation::OnVertex(b1)), None) } - a = advance(a, &mut aa, n); + i1 = advance(i1, &mut nsteps1, len1); } else { - if inflag == InFlag::QIn { + if inflag == InFlag::Poly2IsInside { out(None, Some(PolylinePointLocation::OnVertex(b2))) } - b = advance(b, &mut ba, m); + i2 = advance(i2, &mut nsteps2, len2); } } else { // We have cross == TriangleOrientation::Clockwise. - if a_hb == TriangleOrientation::CounterClockwise { - if inflag == InFlag::QIn { + if a2_b2_b1 == TriangleOrientation::CounterClockwise { + if inflag == InFlag::Poly2IsInside { out(None, Some(PolylinePointLocation::OnVertex(b2))) } - b = advance(b, &mut ba, m); + i2 = advance(i2, &mut nsteps2, len2); } else { - if inflag == InFlag::PIn { - out(Some(PolylinePointLocation::OnVertex(a2)), None) + if inflag == InFlag::Poly1IsInside { + out(Some(PolylinePointLocation::OnVertex(b1)), None) } - a = advance(a, &mut aa, n); + i1 = advance(i1, &mut nsteps1, len1); } } } @@ -237,20 +318,31 @@ pub fn convex_polygons_intersection( let mut orient = TriangleOrientation::Degenerate; let mut ok = true; - for a in 0..n { - let a1 = (a + n - 1) % n; // a - 1 - let new_orient = Triangle::orientation2d(&poly1[a1], &poly1[a], &poly2[0], EPS); - - if orient == TriangleOrientation::Degenerate { - orient = new_orient - } else if new_orient != orient && new_orient != TriangleOrientation::Degenerate { - ok = false; - break; + // TODO: avoid the O(n²) complexity. + for a in 0..len1 { + for p2 in poly2 { + let a_minus_1 = (a + len1 - 1) % len1; // a - 1 + let new_orient = Triangle::orientation2d( + &poly1[a_minus_1], + &poly1[a], + p2, + tolerances.collinearity_epsilon, + ); + + if orient == TriangleOrientation::Degenerate { + orient = new_orient + } else if new_orient != orient && new_orient != TriangleOrientation::Degenerate { + ok = false; + break; + } } } if ok { - for b in 0..m { + for mut b in 0..len2 { + if rev2 { + b = len2 - b - 1; + } out(None, Some(PolylinePointLocation::OnVertex(b))) } } @@ -258,20 +350,31 @@ pub fn convex_polygons_intersection( let mut orient = TriangleOrientation::Degenerate; let mut ok = true; - for b in 0..m { - let b1 = (b + m - 1) % m; // b - 1 - let new_orient = Triangle::orientation2d(&poly2[b1], &poly2[b], &poly1[0], EPS); - - if orient == TriangleOrientation::Degenerate { - orient = new_orient - } else if new_orient != orient && new_orient != TriangleOrientation::Degenerate { - ok = false; - break; + // TODO: avoid the O(n²) complexity. + for b in 0..len2 { + for p1 in poly1 { + let b_minus_1 = (b + len2 - 1) % len2; // = b - 1 + let new_orient = Triangle::orientation2d( + &poly2[b_minus_1], + &poly2[b], + p1, + tolerances.collinearity_epsilon, + ); + + if orient == TriangleOrientation::Degenerate { + orient = new_orient + } else if new_orient != orient && new_orient != TriangleOrientation::Degenerate { + ok = false; + break; + } } } if ok { - for a in 0..n { + for mut a in 0..len1 { + if rev1 { + a = len1 - a - 1; + } out(Some(PolylinePointLocation::OnVertex(a)), None) } } @@ -334,13 +437,16 @@ pub fn polygons_intersection( poly2: &[Point2], mut out: impl FnMut(Option, Option), ) -> Result<(), PolygonsIntersectionError> { + let tolerances = PolygonIntersectionTolerances::default(); + #[derive(Debug)] struct ToTraverse { poly: usize, edge: EdgeId, } - let (intersections, num_intersections) = compute_sorted_edge_intersections(poly1, poly2); + let (intersections, num_intersections) = + compute_sorted_edge_intersections(poly1, poly2, tolerances.collinearity_epsilon); let mut visited = vec![false; num_intersections]; let segment = |eid: EdgeId, poly: &[Point2]| [poly[eid], poly[(eid + 1) % poly.len()]]; @@ -355,20 +461,26 @@ pub fn polygons_intersection( // between poly1 and poly2 when reaching an intersection. let [a1, b1] = segment(inter.edges[0], poly1); let [a2, b2] = segment(inter.edges[1], poly2); - let poly_to_traverse = match Triangle::orientation2d(&a1, &b1, &a2, EPS) { - TriangleOrientation::Clockwise => 1, - TriangleOrientation::CounterClockwise => 0, - TriangleOrientation::Degenerate => { - match Triangle::orientation2d(&a1, &b1, &b2, EPS) { - TriangleOrientation::Clockwise => 0, - TriangleOrientation::CounterClockwise => 1, - TriangleOrientation::Degenerate => { - log::debug!("Unhandled edge-edge overlap case."); - 0 + let poly_to_traverse = + match Triangle::orientation2d(&a1, &b1, &a2, tolerances.collinearity_epsilon) { + TriangleOrientation::Clockwise => 1, + TriangleOrientation::CounterClockwise => 0, + TriangleOrientation::Degenerate => { + match Triangle::orientation2d( + &a1, + &b1, + &b2, + tolerances.collinearity_epsilon, + ) { + TriangleOrientation::Clockwise => 0, + TriangleOrientation::CounterClockwise => 1, + TriangleOrientation::Degenerate => { + log::debug!("Unhandled edge-edge overlap case."); + 0 + } } } - } - }; + }; #[derive(Debug)] enum TraversalStatus { @@ -484,6 +596,7 @@ struct IntersectionPoint { fn compute_sorted_edge_intersections( poly1: &[Point2], poly2: &[Point2], + eps: Real, ) -> ([HashMap>; 2], usize) { let mut inter1: HashMap> = HashMap::default(); let mut inter2: HashMap> = HashMap::default(); @@ -498,7 +611,7 @@ fn compute_sorted_edge_intersections( let j2 = (i2 + 1) % poly2.len(); let Some(inter) = - utils::segments_intersection2d(&poly1[i1], &poly1[j1], &poly2[i2], &poly2[j2], EPS) + utils::segments_intersection2d(&poly1[i1], &poly1[j1], &poly2[i2], &poly2[j2], eps) else { continue; }; @@ -543,3 +656,100 @@ fn compute_sorted_edge_intersections( ([inter1, inter2], id) } + +#[cfg(all(test, feature = "dim2"))] +mod test { + use crate::query::PointQuery; + use crate::shape::Triangle; + use crate::transformation::convex_polygons_intersection_points_with_tolerances; + use crate::transformation::polygon_intersection::PolygonIntersectionTolerances; + use na::Point2; + + #[test] + fn intersect_triangle_common_vertex() { + let tris = [ + ( + Triangle::new( + Point2::new(-0.0008759537858568062, -2.0103871966663305), + Point2::new(0.3903908709629763, -1.3421764825890266), + Point2::new(1.3380817875388151, -2.0098007857739013), + ), + Triangle::new( + Point2::new(0.0, -0.0), + Point2::new(-0.0008759537858568062, -2.0103871966663305), + Point2::new(1.9991979155226394, -2.009511242880474), + ), + ), + ( + Triangle::new( + Point2::new(0.7319315811016305, -0.00004046981523721891), + Point2::new(2.0004914907008944, -0.00011061077714557787), + Point2::new(1.1848406021956144, -0.8155712451545468), + ), + Triangle::new( + Point2::new(0.0, 0.0), + Point2::new(0.00011061077714557787, -2.000024893134292), + Point2::new(2.0004914907008944, -0.00011061077714557787), + ), + ), + ( + Triangle::new( + Point2::new(-0.000049995168258705205, -0.9898801451981707), + Point2::new(0.0, -0.0), + Point2::new(0.583013294019752, -1.4170136900568633), + ), + Triangle::new( + Point2::new(0.0, -0.0), + Point2::new(-0.00010101395240669591, -2.000027389553894), + Point2::new(2.000372544168497, 0.00010101395240669591), + ), + ), + ( + Triangle::new( + Point2::new(-0.940565646581769, -0.939804943675256), + Point2::new(0.0, -0.0), + Point2::new(-0.001533592366792066, -0.9283586484736431), + ), + Triangle::new( + Point2::new(0.0, -0.0), + Point2::new(-2.00752629829582, -2.0059026672784825), + Point2::new(-0.0033081650580626698, -2.0025945022204197), + ), + ), + ]; + + for (tri1, tri2) in tris { + let mut inter = Vec::new(); + let tolerances = PolygonIntersectionTolerances { + collinearity_epsilon: 1.0e-5, + }; + convex_polygons_intersection_points_with_tolerances( + tri1.vertices(), + tri2.vertices(), + tolerances, + &mut inter, + ); + + println!("----------"); + println!("inter: {:?}", inter); + println!( + "tri1 is in tri2: {}", + tri1.vertices().iter().all(|pt| tri2 + .project_local_point(pt, false) + .is_inside_eps(pt, 1.0e-5)) + ); + println!( + "tri2 is in tri1: {}", + tri2.vertices().iter().all(|pt| tri1 + .project_local_point(pt, false) + .is_inside_eps(pt, 1.0e-5)) + ); + for pt in &inter { + let proj1 = tri1.project_local_point(&pt, false); + let proj2 = tri2.project_local_point(&pt, false); + assert!(proj1.is_inside_eps(&pt, 1.0e-5)); + assert!(proj2.is_inside_eps(&pt, 1.0e-5)); + } + } + } +}