parry3d/transformation/mesh_intersection/
mesh_intersection.rs

1use super::{MeshIntersectionError, TriangleTriangleIntersection};
2use crate::bounding_volume::BoundingVolume;
3use crate::math::{Isometry, Real};
4use crate::partitioning::BvhNode;
5use crate::query::point::point_query::PointQueryWithLocation;
6use crate::query::PointQuery;
7use crate::shape::{TriMesh, Triangle};
8use crate::utils;
9use crate::utils::hashmap::Entry;
10use crate::utils::hashmap::HashMap;
11use crate::utils::hashset::HashSet;
12use alloc::collections::BTreeMap;
13use alloc::{vec, vec::Vec};
14#[cfg(not(feature = "std"))]
15use na::ComplexField;
16use na::{Point3, Vector3};
17use rstar::RTree;
18use spade::{ConstrainedDelaunayTriangulation, InsertionError, Triangulation as _};
19#[cfg(feature = "wavefront")]
20use std::path::PathBuf;
21
22/// A triangle with indices sorted in increasing order for deduplication in a hashmap.
23///
24/// Note that when converting a `[u32; 3]` into a `HashableTriangleIndices`, the result’s orientation
25/// might not match the input’s.
26#[derive(Copy, Clone, PartialEq, Eq, Hash)]
27struct HashableTriangleIndices([u32; 3]);
28
29impl From<[u32; 3]> for HashableTriangleIndices {
30    fn from([a, b, c]: [u32; 3]) -> Self {
31        let (sa, sb, sc) = utils::sort3(&a, &b, &c);
32        HashableTriangleIndices([*sa, *sb, *sc])
33    }
34}
35
36/// Metadata that specifies thresholds to use when making construction choices
37/// in mesh intersections.
38#[derive(Copy, Clone, PartialEq, Debug)]
39pub struct MeshIntersectionTolerances {
40    /// The smallest angle (in radians) that will be tolerated. A triangle with
41    /// a smaller angle is considered degenerate and will be deleted.
42    pub angle_epsilon: Real,
43    /// The maximum distance at which two points are considered to overlap in space.
44    /// If `||p1 - p2|| < global_insertion_epsilon` then p1 and p2 are considered
45    /// to be the same point.
46    pub global_insertion_epsilon: Real,
47    /// A multiplier coefficient to scale [`Self::global_insertion_epsilon`] when checking for
48    /// point duplication within a single triangle.
49    ///
50    /// Inside an individual triangle the distance at which two points are considered
51    /// to be the same is `global_insertion_epsilon * local_insertion_epsilon_mod`.
52    pub local_insertion_epsilon_scale: Real,
53    /// Three points forming a triangle with an area smaller than this epsilon are considered collinear.
54    pub collinearity_epsilon: Real,
55}
56
57impl Default for MeshIntersectionTolerances {
58    fn default() -> Self {
59        Self {
60            #[expect(clippy::unnecessary_cast)]
61            angle_epsilon: (0.005 as Real).to_radians(), // 0.005 degrees
62            global_insertion_epsilon: Real::EPSILON * 100.0,
63            local_insertion_epsilon_scale: 10.,
64            collinearity_epsilon: Real::EPSILON * 100.0,
65        }
66    }
67}
68
69/// Computes the intersection of two meshes.
70///
71/// The meshes must be oriented, have their half-edge topology computed, and must not be self-intersecting.
72/// The result mesh vertex coordinates are given in the local-space of `mesh1`.
73pub fn intersect_meshes(
74    pos1: &Isometry<Real>,
75    mesh1: &TriMesh,
76    flip1: bool,
77    pos2: &Isometry<Real>,
78    mesh2: &TriMesh,
79    flip2: bool,
80) -> Result<Option<TriMesh>, MeshIntersectionError> {
81    intersect_meshes_with_tolerances(
82        pos1,
83        mesh1,
84        flip1,
85        pos2,
86        mesh2,
87        flip2,
88        MeshIntersectionTolerances::default(),
89    )
90}
91
92/// Similar to `intersect_meshes`.
93///
94/// It allows to specify epsilons for how the algorithm will behave.
95/// See `MeshIntersectionTolerances` for details.
96pub fn intersect_meshes_with_tolerances(
97    pos1: &Isometry<Real>,
98    mesh1: &TriMesh,
99    flip1: bool,
100    pos2: &Isometry<Real>,
101    mesh2: &TriMesh,
102    flip2: bool,
103    tolerances: MeshIntersectionTolerances,
104) -> Result<Option<TriMesh>, MeshIntersectionError> {
105    if cfg!(debug_assertions) {
106        mesh1.assert_half_edge_topology_is_valid();
107        mesh2.assert_half_edge_topology_is_valid();
108    }
109
110    if mesh1.topology().is_none() || mesh2.topology().is_none() {
111        return Err(MeshIntersectionError::MissingTopology);
112    }
113
114    if mesh1.pseudo_normals_if_oriented().is_none() || mesh2.pseudo_normals_if_oriented().is_none()
115    {
116        return Err(MeshIntersectionError::MissingPseudoNormals);
117    }
118
119    let pos12 = pos1.inv_mul(pos2);
120
121    // 1: collect all the potential triangle-triangle intersections.
122    let intersection_candidates: Vec<_> = mesh1
123        .bvh()
124        .leaf_pairs(mesh2.bvh(), |node1: &BvhNode, node2: &BvhNode| {
125            let aabb1 = node1.aabb();
126            let aabb2 = node2.aabb().transform_by(&pos12);
127            aabb1.intersects(&aabb2)
128        })
129        .collect();
130
131    let mut deleted_faces1: HashSet<u32> = HashSet::default();
132    let mut deleted_faces2: HashSet<u32> = HashSet::default();
133    let mut new_indices1 = vec![];
134    let mut new_indices2 = vec![];
135
136    // 2: Identify all triangles that do actually intersect.
137    let mut intersections = vec![];
138    for (fid1, fid2) in &intersection_candidates {
139        let tri1 = mesh1.triangle(*fid1);
140        let tri2 = mesh2.triangle(*fid2).transformed(&pos12);
141
142        if super::triangle_triangle_intersection(&tri1, &tri2, tolerances.collinearity_epsilon)
143            .is_some()
144        {
145            intersections.push((*fid1, *fid2));
146            let _ = deleted_faces1.insert(*fid1);
147            let _ = deleted_faces2.insert(*fid2);
148        }
149    }
150
151    // 3: Grab all triangles that are inside the other mesh but do not intersect it.
152    extract_connected_components(
153        &pos12,
154        mesh1,
155        mesh2,
156        flip2,
157        &deleted_faces1,
158        &mut new_indices1,
159    );
160    extract_connected_components(
161        &pos12.inverse(),
162        mesh2,
163        mesh1,
164        flip1,
165        &deleted_faces2,
166        &mut new_indices2,
167    );
168
169    // 4: Initialize a new mesh by inserting points into a set. Duplicate points should
170    // hash to the same index.
171    let mut point_set = RTree::<TreePoint, _>::new();
172    let mut topology_indices = HashMap::default();
173    {
174        let mut insert_point = |position: Point3<Real>| {
175            insert_into_set(
176                position,
177                &mut point_set,
178                tolerances.global_insertion_epsilon,
179            ) as u32
180        };
181        // Add the inside vertices and triangles from mesh1
182        for mut face in new_indices1 {
183            if flip1 {
184                face.swap(0, 1);
185            }
186
187            let idx = [
188                insert_point(pos1 * mesh1.vertices()[face[0] as usize]),
189                insert_point(pos1 * mesh1.vertices()[face[1] as usize]),
190                insert_point(pos1 * mesh1.vertices()[face[2] as usize]),
191            ];
192
193            if !is_topologically_degenerate(idx) {
194                insert_topology_indices(&mut topology_indices, idx);
195            }
196        }
197
198        // Add the inside vertices and triangles from mesh2
199        for mut face in new_indices2 {
200            if flip2 {
201                face.swap(0, 1);
202            }
203            let idx = [
204                insert_point(pos2 * mesh2.vertices()[face[0] as usize]),
205                insert_point(pos2 * mesh2.vertices()[face[1] as usize]),
206                insert_point(pos2 * mesh2.vertices()[face[2] as usize]),
207            ];
208
209            if !is_topologically_degenerate(idx) {
210                insert_topology_indices(&mut topology_indices, idx);
211            }
212        }
213    }
214
215    // 5: Associate constraint edges generated by a triangle-triangle intersection
216    // to each intersecting triangle where they occur.
217    let mut constraints1 = BTreeMap::<_, Vec<_>>::new();
218    let mut constraints2 = BTreeMap::<_, Vec<_>>::new();
219    for (fid1, fid2) in &intersections {
220        let tri1 = mesh1.triangle(*fid1).transformed(pos1);
221        let tri2 = mesh2.triangle(*fid2).transformed(pos2);
222
223        let list1 = constraints1.entry(fid1).or_default();
224        let list2 = constraints2.entry(fid2).or_default();
225
226        let intersection =
227            super::triangle_triangle_intersection(&tri1, &tri2, tolerances.collinearity_epsilon);
228        if let Some(intersection) = intersection {
229            match intersection {
230                TriangleTriangleIntersection::Segment { a, b } => {
231                    // For both triangles, add the points in the intersection
232                    // and their associated edge to the set.
233                    // Note this necessarily introduces duplicate points to the
234                    // set that need to be filtered out.
235                    list1.push([a.p1, b.p1]);
236                    list2.push([a.p1, b.p1]);
237                }
238                TriangleTriangleIntersection::Polygon(polygon) => {
239                    for i in 0..polygon.len() {
240                        let a = polygon[i];
241                        let b = polygon[(i + 1) % polygon.len()];
242
243                        // Triangles overlap in space, so only one constraint is needed.
244                        list1.push([a.p1, b.p1]);
245                    }
246                }
247            }
248        }
249    }
250
251    // 6: Collect all triangles that intersect and their associated constraint edges.
252    // For each such triangle, compute a CDT of its constraints. For each face in this CDT,
253    // if the face is contained in the opposite mesh, add it to the intersection mesh.
254    merge_triangle_sets(
255        mesh1,
256        mesh2,
257        &constraints1,
258        pos1,
259        pos2,
260        flip1,
261        flip2,
262        &tolerances,
263        &mut point_set,
264        &mut topology_indices,
265    )?;
266
267    merge_triangle_sets(
268        mesh2,
269        mesh1,
270        &constraints2,
271        pos2,
272        pos1,
273        flip2,
274        flip1,
275        &tolerances,
276        &mut point_set,
277        &mut topology_indices,
278    )?;
279
280    // 7: Sort the output points by insertion order.
281    let mut vertices: Vec<_> = point_set.iter().copied().collect();
282    vertices.sort_by(|a, b| a.id.cmp(&b.id));
283    let vertices: Vec<_> = vertices.iter().map(|p| Point3::from(p.point)).collect();
284
285    if !topology_indices.is_empty() {
286        Ok(Some(TriMesh::new(
287            vertices,
288            topology_indices.into_values().collect(),
289        )?))
290    } else {
291        Ok(None)
292    }
293}
294
295fn extract_connected_components(
296    pos12: &Isometry<Real>,
297    mesh1: &TriMesh,
298    mesh2: &TriMesh,
299    flip2: bool,
300    deleted_faces1: &HashSet<u32>,
301    new_indices1: &mut Vec<[u32; 3]>,
302) {
303    let topo1 = mesh1.topology().unwrap();
304    let mut visited: HashSet<u32> = HashSet::default();
305    let mut to_visit = vec![];
306    let mut visited_conn_comp = if let Some(cc) = mesh1.connected_components() {
307        vec![false; cc.ranges.len()] // TODO: use a Vob instead?
308    } else {
309        vec![]
310    };
311
312    for face in deleted_faces1 {
313        if let Some(cc) = mesh1.connected_components() {
314            visited_conn_comp[cc.face_colors[*face as usize] as usize] = true;
315        }
316
317        let eid = topo1.faces[*face as usize].half_edge;
318        let edge_a = &topo1.half_edges[eid as usize];
319        let edge_b = &topo1.half_edges[edge_a.next as usize];
320        let edge_c = &topo1.half_edges[edge_b.next as usize];
321        let edges = [edge_a, edge_b, edge_c];
322
323        for edge in edges {
324            if let Some(twin) = topo1.half_edges.get(edge.twin as usize) {
325                if !deleted_faces1.contains(&twin.face) {
326                    let tri1 = mesh1.triangle(twin.face);
327
328                    if flip2
329                        ^ mesh2.contains_local_point(&pos12.inverse_transform_point(&tri1.center()))
330                    {
331                        to_visit.push(twin.face);
332                    }
333                }
334            }
335        }
336    }
337
338    // Propagate.
339    while let Some(face) = to_visit.pop() {
340        if !visited.insert(face) {
341            continue; // Already visited.
342        }
343
344        new_indices1.push(mesh1.indices()[face as usize]);
345
346        let eid = topo1.faces[face as usize].half_edge;
347        let edge_a = &topo1.half_edges[eid as usize];
348        let edge_b = &topo1.half_edges[edge_a.next as usize];
349        let edge_c = &topo1.half_edges[edge_b.next as usize];
350        let edges = [edge_a, edge_b, edge_c];
351
352        for edge in edges {
353            if let Some(twin) = topo1.half_edges.get(edge.twin as usize) {
354                if !deleted_faces1.contains(&twin.face) {
355                    to_visit.push(twin.face);
356                }
357            }
358        }
359    }
360
361    /*
362     * Deal with connected components that don’t intersect the other mesh.
363     */
364    if let Some(cc) = mesh1.connected_components() {
365        for (i, range) in cc.ranges.windows(2).enumerate() {
366            if !visited_conn_comp[i] {
367                // This connected component doesn’t intersect the second mesh.
368                // Classify one of its face (the "representative face", can be any
369                // face of the connected component) to determine
370                // if the whole thing is inside or outside.
371                let repr_face = cc.grouped_faces[range[0]];
372                let repr_pt = mesh1.triangle(repr_face).center();
373                let indices = mesh1.indices();
374
375                if flip2 ^ mesh2.contains_local_point(&pos12.inverse_transform_point(&repr_pt)) {
376                    new_indices1.extend(
377                        cc.grouped_faces[range[0]..range[1]]
378                            .iter()
379                            .map(|fid| indices[*fid as usize]),
380                    )
381                }
382            }
383        }
384    } else if deleted_faces1.is_empty() {
385        // Deal with the case where there is no intersection between the meshes.
386        let repr_pt = mesh1.triangle(0).center();
387
388        if flip2 ^ mesh2.contains_local_point(&pos12.inverse_transform_point(&repr_pt)) {
389            new_indices1.extend_from_slice(mesh1.indices());
390        }
391    }
392}
393
394fn triangulate_constraints_and_merge_duplicates(
395    tri: &Triangle,
396    constraints: &[[Point3<Real>; 2]],
397    epsilon: Real,
398) -> Result<
399    (
400        ConstrainedDelaunayTriangulation<spade::Point2<Real>>,
401        Vec<Point3<Real>>,
402    ),
403    InsertionError,
404> {
405    let mut constraints = constraints.to_vec();
406    // Add the triangle points to the triangulation.
407    let mut point_set = RTree::<TreePoint, _>::new();
408    let _ = insert_into_set(tri.a, &mut point_set, epsilon);
409    let _ = insert_into_set(tri.b, &mut point_set, epsilon);
410    let _ = insert_into_set(tri.c, &mut point_set, epsilon);
411
412    // Sometimes, points on the edge of a triangle are slightly off, and this makes
413    // spade think that there is a super thin triangle. Project points close to an edge
414    // onto the edge to get better results.
415    let tri_vtx = tri.vertices();
416    for point_pair in constraints.iter_mut() {
417        let p1 = point_pair[0];
418        let p2 = point_pair[1];
419
420        for i in 0..3 {
421            let q1 = tri_vtx[i];
422            let q2 = tri_vtx[(i + 1) % 3];
423
424            let proj1 = project_point_to_segment(&p1, &[q1, q2]);
425            if (p1 - proj1).norm() < epsilon {
426                point_pair[0] = Point3::from(proj1);
427            }
428
429            let proj2 = project_point_to_segment(&p2, &[q1, q2]);
430            if (p2 - proj2).norm() < epsilon {
431                point_pair[1] = Point3::from(proj2);
432            }
433        }
434    }
435
436    // Generate edge, taking care to merge duplicate vertices.
437    let mut edges = Vec::new();
438    for point_pair in constraints {
439        let p1_id = insert_into_set(point_pair[0], &mut point_set, epsilon);
440        let p2_id = insert_into_set(point_pair[1], &mut point_set, epsilon);
441
442        edges.push([p1_id, p2_id]);
443    }
444
445    let mut points: Vec<_> = point_set.iter().cloned().collect();
446    points.sort_by(|a, b| a.id.cmp(&b.id));
447
448    let tri_points = tri.vertices();
449    let best_source = tri.angle_closest_to_90();
450    let d1 = tri_points[(best_source + 2) % 3] - tri_points[(best_source + 1) % 3];
451    let d2 = tri_points[best_source] - tri_points[(best_source + 1) % 3];
452    let (e1, e2) = planar_gram_schmidt(d1, d2);
453
454    let project = |p: &Point3<Real>| spade::Point2::new(e1.dot(&p.coords), e2.dot(&p.coords));
455
456    // Project points into 2D and triangulate the resulting set.
457    let planar_points: Vec<_> = points
458        .iter()
459        .copied()
460        .map(|point| {
461            let point_proj = project(&point.point);
462            utils::sanitize_spade_point(point_proj)
463        })
464        .collect();
465    let cdt_triangulation =
466        ConstrainedDelaunayTriangulation::bulk_load_cdt_stable(planar_points, edges)?;
467    debug_assert!(cdt_triangulation.vertices().len() == points.len());
468
469    let points = points.into_iter().map(|p| Point3::from(p.point)).collect();
470    Ok((cdt_triangulation, points))
471}
472
473// We heavily recommend that this is left here in case one needs to debug the above code.
474#[cfg(feature = "wavefront")]
475fn _points_to_obj(mesh: &[Point3<Real>], path: &PathBuf) {
476    use std::io::Write;
477    let mut file = std::fs::File::create(path).unwrap();
478
479    for p in mesh {
480        writeln!(file, "v {} {} {}", p.x, p.y, p.z).unwrap();
481    }
482}
483
484// We heavily recommend that this is left here in case one needs to debug the above code.
485#[cfg(feature = "wavefront")]
486fn _points_and_edges_to_obj(mesh: &[Point3<Real>], edges: &[[usize; 2]], path: &PathBuf) {
487    use std::io::Write;
488    let mut file = std::fs::File::create(path).unwrap();
489
490    for p in mesh {
491        writeln!(file, "v {} {} {}", p.x, p.y, p.z).unwrap();
492    }
493
494    for e in edges {
495        writeln!(file, "l {} {}", e[0] + 1, e[1] + 1).unwrap();
496    }
497}
498
499#[derive(Copy, Clone, PartialEq, Debug, Default)]
500struct TreePoint {
501    point: Point3<Real>,
502    id: usize,
503}
504
505impl rstar::Point for TreePoint {
506    type Scalar = Real;
507    const DIMENSIONS: usize = 3;
508
509    fn generate(mut generator: impl FnMut(usize) -> Self::Scalar) -> Self {
510        TreePoint {
511            point: Point3::new(generator(0), generator(1), generator(2)),
512            id: usize::MAX,
513        }
514    }
515
516    fn nth(&self, index: usize) -> Self::Scalar {
517        self.point[index]
518    }
519
520    fn nth_mut(&mut self, index: usize) -> &mut Self::Scalar {
521        &mut self.point[index]
522    }
523}
524
525fn insert_into_set(
526    position: Point3<Real>,
527    point_set: &mut RTree<TreePoint>,
528    epsilon: Real,
529) -> usize {
530    let point_count = point_set.size();
531    let point_to_insert = TreePoint {
532        point: position,
533        id: point_count,
534    };
535
536    match point_set.nearest_neighbor(&point_to_insert) {
537        Some(tree_point) => {
538            if (tree_point.point - position).norm_squared() <= epsilon {
539                tree_point.id
540            } else {
541                point_set.insert(point_to_insert);
542                debug_assert!(point_set.size() == point_count + 1);
543                point_count
544            }
545        }
546        None => {
547            point_set.insert(point_to_insert);
548            debug_assert!(point_set.size() == point_count + 1);
549            point_count
550        }
551    }
552}
553
554fn smallest_angle(points: &[Point3<Real>]) -> Real {
555    let n = points.len();
556
557    let mut worst_cos: Real = -2.0;
558    for i in 0..points.len() {
559        let d1 = (points[i] - points[(i + 1) % n]).normalize();
560        let d2 = (points[(i + 2) % n] - points[(i + 1) % n]).normalize();
561
562        let cos = d1.dot(&d2);
563        if cos > worst_cos {
564            worst_cos = cos;
565        }
566    }
567
568    worst_cos.acos()
569}
570
571fn planar_gram_schmidt(v1: Vector3<Real>, v2: Vector3<Real>) -> (Vector3<Real>, Vector3<Real>) {
572    let u1 = v1;
573    let u2 = v2 - (v2.dot(&u1) / u1.norm_squared()) * u1;
574
575    let e1 = u1.normalize();
576    let e2 = u2.normalize();
577
578    (e1, e2)
579}
580
581fn project_point_to_segment(point: &Point3<Real>, segment: &[Point3<Real>; 2]) -> Point3<Real> {
582    let dir = segment[1] - segment[0];
583    let local = point - segment[0];
584
585    let norm = dir.norm();
586    // Restrict the result to the segment portion of the line.
587    let coeff = (dir.dot(&local) / norm).clamp(0., norm);
588
589    segment[0] + coeff * dir.normalize()
590}
591
592/// No matter how smart we are about computing intersections. It is always possible
593/// to create ultra thin triangles when a point lies on an edge of a triangle. These
594/// are degenerate and need to be removed.
595fn is_triangle_degenerate(
596    triangle: &[Point3<Real>; 3],
597    epsilon_angle: Real,
598    epsilon_distance: Real,
599) -> bool {
600    if smallest_angle(triangle) < epsilon_angle {
601        return true;
602    }
603
604    for i in 0..3 {
605        let mut dir = triangle[(i + 1) % 3] - triangle[(i + 2) % 3];
606        if dir.normalize_mut() < epsilon_distance {
607            return true;
608        }
609
610        let proj = triangle[(i + 2) % 3] + (triangle[i] - triangle[(i + 2) % 3]).dot(&dir) * dir;
611
612        if (proj - triangle[i]).norm() < epsilon_distance {
613            return true;
614        }
615    }
616
617    false
618}
619
620fn merge_triangle_sets(
621    mesh1: &TriMesh,
622    mesh2: &TriMesh,
623    triangle_constraints: &BTreeMap<&u32, Vec<[Point3<Real>; 2]>>,
624    pos1: &Isometry<Real>,
625    pos2: &Isometry<Real>,
626    flip1: bool,
627    flip2: bool,
628    metadata: &MeshIntersectionTolerances,
629    point_set: &mut RTree<TreePoint>,
630    topology_indices: &mut HashMap<HashableTriangleIndices, [u32; 3]>,
631) -> Result<(), MeshIntersectionError> {
632    // For each triangle, and each constraint edge associated to that triangle,
633    // make a triangulation of the face and sort whether each generated
634    // sub-triangle is part of the intersection.
635    // For each sub-triangle that is part of the intersection, add them to the
636    // output mesh.
637    for (triangle_id, constraints) in triangle_constraints.iter() {
638        let tri = mesh1.triangle(**triangle_id).transformed(pos1);
639
640        let (delaunay, points) = triangulate_constraints_and_merge_duplicates(
641            &tri,
642            constraints,
643            metadata.global_insertion_epsilon * metadata.local_insertion_epsilon_scale,
644        )
645        .or(Err(MeshIntersectionError::TriangulationError))?;
646
647        for face in delaunay.inner_faces() {
648            let verts = face.vertices();
649            let p1 = points[verts[0].index()];
650            let p2 = points[verts[1].index()];
651            let p3 = points[verts[2].index()];
652
653            // Sometimes the triangulation is messed up due to numerical errors. If
654            // a triangle does not survive this test it should be deleted.
655            if is_triangle_degenerate(
656                &[p1, p2, p3],
657                metadata.angle_epsilon,
658                metadata.global_insertion_epsilon,
659            ) {
660                continue;
661            }
662
663            let center = Triangle {
664                a: p1,
665                b: p2,
666                c: p3,
667            }
668            .center();
669
670            let epsilon = metadata.global_insertion_epsilon;
671            let projection = mesh2
672                .project_local_point_and_get_location(&pos2.inverse_transform_point(&center), true)
673                .0;
674
675            if flip2 ^ (projection.is_inside_eps(&center, epsilon)) {
676                let mut new_tri_idx = [
677                    insert_into_set(p1, point_set, epsilon) as u32,
678                    insert_into_set(p2, point_set, epsilon) as u32,
679                    insert_into_set(p3, point_set, epsilon) as u32,
680                ];
681
682                if flip1 {
683                    new_tri_idx.swap(0, 1)
684                }
685
686                // This should *never* trigger. If it does
687                // it means the code has created a triangle with duplicate vertices,
688                // which means we encountered an unaccounted for edge case.
689                if is_topologically_degenerate(new_tri_idx) {
690                    return Err(MeshIntersectionError::DuplicateVertices);
691                }
692
693                insert_topology_indices(topology_indices, new_tri_idx);
694            }
695        }
696    }
697
698    Ok(())
699}
700
701// Insert in the hashmap with sorted indices to avoid adding duplicates.
702//
703// We also check if we don’t keep pairs of triangles that have the same
704// set of indices but opposite orientations. If this happens, both the new triangle, and the one it
705// matched with are removed (because they describe a degenerate piece of volume).
706fn insert_topology_indices(
707    topology_indices: &mut HashMap<HashableTriangleIndices, [u32; 3]>,
708    new_tri_idx: [u32; 3],
709) {
710    match topology_indices.entry(new_tri_idx.into()) {
711        Entry::Vacant(e) => {
712            let _ = e.insert(new_tri_idx);
713        }
714        Entry::Occupied(e) => {
715            fn same_orientation(a: &[u32; 3], b: &[u32; 3]) -> bool {
716                let ib = if a[0] == b[0] {
717                    0
718                } else if a[0] == b[1] {
719                    1
720                } else {
721                    2
722                };
723                a[1] == b[(ib + 1) % 3]
724            }
725
726            if !same_orientation(e.get(), &new_tri_idx) {
727                // If we are inserting two identical triangles but with mismatching
728                // orientations, we can just ignore both because they cover a degenerate
729                // 2D plane.
730                #[cfg(feature = "enhanced-determinism")]
731                let _ = e.swap_remove();
732                #[cfg(not(feature = "enhanced-determinism"))]
733                let _ = e.remove();
734            }
735        }
736    }
737}
738
739fn is_topologically_degenerate(tri_idx: [u32; 3]) -> bool {
740    tri_idx[0] == tri_idx[1] || tri_idx[0] == tri_idx[2] || tri_idx[1] == tri_idx[2]
741}
742
743#[cfg(feature = "wavefront")]
744#[cfg(test)]
745mod tests {
746    use super::*;
747    use crate::shape::{Ball, Cuboid, TriMeshFlags};
748    use obj::Obj;
749    use obj::ObjData;
750
751    #[test]
752    fn test_same_mesh_intersection() {
753        let Obj {
754            data: ObjData {
755                position, objects, ..
756            },
757            ..
758        } = Obj::load("../../assets/tests/low_poly_bunny.obj").unwrap();
759
760        let mesh = TriMesh::with_flags(
761            position
762                .iter()
763                .map(|v| Point3::new(v[0] as Real, v[1] as Real, v[2] as Real))
764                .collect::<Vec<_>>(),
765            objects[0].groups[0]
766                .polys
767                .iter()
768                .map(|p| [p.0[0].0 as u32, p.0[1].0 as u32, p.0[2].0 as u32])
769                .collect::<Vec<_>>(),
770            TriMeshFlags::all(),
771        )
772        .unwrap();
773
774        let _ = intersect_meshes(
775            &Isometry::identity(),
776            &mesh,
777            false,
778            &Isometry::identity(),
779            &mesh,
780            false,
781        )
782        .unwrap()
783        .unwrap();
784
785        let _ = mesh.to_obj_file(&PathBuf::from("same_test.obj"));
786    }
787
788    #[test]
789    fn test_non_origin_pos1_pos2_intersection() {
790        let ball = Ball::new(2f32 as Real).to_trimesh(10, 10);
791        let cuboid = Cuboid::new(Vector3::new(2.0, 1.0, 1.0)).to_trimesh();
792        let mut sphere_mesh = TriMesh::new(ball.0, ball.1).unwrap();
793        sphere_mesh.set_flags(TriMeshFlags::all()).unwrap();
794        let mut cuboid_mesh = TriMesh::new(cuboid.0, cuboid.1).unwrap();
795        cuboid_mesh.set_flags(TriMeshFlags::all()).unwrap();
796
797        let res = intersect_meshes(
798            &Isometry::translation(1.0, 0.0, 0.0),
799            &cuboid_mesh,
800            false,
801            &Isometry::translation(2.0, 0.0, 0.0),
802            &sphere_mesh,
803            false,
804        )
805        .unwrap()
806        .unwrap();
807
808        let _ = res.to_obj_file(&PathBuf::from("test_non_origin_pos1_pos2_intersection.obj"));
809
810        let bounding_sphere = res.local_bounding_sphere();
811        assert!(bounding_sphere.center == Point3::new(1.5, 0.0, 0.0));
812        assert_relative_eq!(2.0615528, bounding_sphere.radius, epsilon = 1.0e-5);
813    }
814
815    #[test]
816    fn test_offset_cylinder_intersection() {
817        let Obj {
818            data: ObjData {
819                position, objects, ..
820            },
821            ..
822        } = Obj::load("../../assets/tests/offset_cylinder.obj").unwrap();
823
824        let offset_mesh = TriMesh::with_flags(
825            position
826                .iter()
827                .map(|v| Point3::new(v[0] as Real, v[1] as Real, v[2] as Real))
828                .collect::<Vec<_>>(),
829            objects[0].groups[0]
830                .polys
831                .iter()
832                .map(|p| [p.0[0].0 as u32, p.0[1].0 as u32, p.0[2].0 as u32])
833                .collect::<Vec<_>>(),
834            TriMeshFlags::all(),
835        )
836        .unwrap();
837
838        let Obj {
839            data: ObjData {
840                position, objects, ..
841            },
842            ..
843        } = Obj::load("../../assets/tests/center_cylinder.obj").unwrap();
844
845        let center_mesh = TriMesh::with_flags(
846            position
847                .iter()
848                .map(|v| Point3::new(v[0] as Real, v[1] as Real, v[2] as Real))
849                .collect::<Vec<_>>(),
850            objects[0].groups[0]
851                .polys
852                .iter()
853                .map(|p| [p.0[0].0 as u32, p.0[1].0 as u32, p.0[2].0 as u32])
854                .collect::<Vec<_>>(),
855            TriMeshFlags::all(),
856        )
857        .unwrap();
858
859        let res = intersect_meshes(
860            &Isometry::identity(),
861            &center_mesh,
862            false,
863            &Isometry::identity(),
864            &offset_mesh,
865            false,
866        )
867        .unwrap()
868        .unwrap();
869
870        let _ = res.to_obj_file(&PathBuf::from("offset_test.obj"));
871    }
872
873    #[test]
874    fn test_stair_bar_intersection() {
875        let Obj {
876            data: ObjData {
877                position, objects, ..
878            },
879            ..
880        } = Obj::load("../../assets/tests/stairs.obj").unwrap();
881
882        let stair_mesh = TriMesh::with_flags(
883            position
884                .iter()
885                .map(|v| Point3::new(v[0] as Real, v[1] as Real, v[2] as Real))
886                .collect::<Vec<_>>(),
887            objects[0].groups[0]
888                .polys
889                .iter()
890                .map(|p| [p.0[0].0 as u32, p.0[1].0 as u32, p.0[2].0 as u32])
891                .collect::<Vec<_>>(),
892            TriMeshFlags::all(),
893        )
894        .unwrap();
895
896        let Obj {
897            data: ObjData {
898                position, objects, ..
899            },
900            ..
901        } = Obj::load("../../assets/tests/bar.obj").unwrap();
902
903        let bar_mesh = TriMesh::with_flags(
904            position
905                .iter()
906                .map(|v| Point3::new(v[0] as Real, v[1] as Real, v[2] as Real))
907                .collect::<Vec<_>>(),
908            objects[0].groups[0]
909                .polys
910                .iter()
911                .map(|p| [p.0[0].0 as u32, p.0[1].0 as u32, p.0[2].0 as u32])
912                .collect::<Vec<_>>(),
913            TriMeshFlags::all(),
914        )
915        .unwrap();
916
917        let res = intersect_meshes(
918            &Isometry::identity(),
919            &stair_mesh,
920            false,
921            &Isometry::identity(),
922            &bar_mesh,
923            false,
924        )
925        .unwrap()
926        .unwrap();
927
928        let _ = res.to_obj_file(&PathBuf::from("stair_test.obj"));
929    }
930
931    #[test]
932    fn test_complex_intersection() {
933        let Obj {
934            data: ObjData {
935                position, objects, ..
936            },
937            ..
938        } = Obj::load("../../assets/tests/low_poly_bunny.obj").unwrap();
939
940        let bunny_mesh = TriMesh::with_flags(
941            position
942                .iter()
943                .map(|v| Point3::new(v[0] as Real, v[1] as Real, v[2] as Real))
944                .collect::<Vec<_>>(),
945            objects[0].groups[0]
946                .polys
947                .iter()
948                .map(|p| [p.0[0].0 as u32, p.0[1].0 as u32, p.0[2].0 as u32])
949                .collect::<Vec<_>>(),
950            TriMeshFlags::all(),
951        )
952        .unwrap();
953
954        let Obj {
955            data: ObjData {
956                position, objects, ..
957            },
958            ..
959        } = Obj::load("../../assets/tests/poly_cylinder.obj").unwrap();
960
961        let cylinder_mesh = TriMesh::with_flags(
962            position
963                .iter()
964                .map(|v| Point3::new(v[0] as Real, v[1] as Real, v[2] as Real))
965                .collect::<Vec<_>>(),
966            objects[0].groups[0]
967                .polys
968                .iter()
969                .map(|p| [p.0[0].0 as u32, p.0[1].0 as u32, p.0[2].0 as u32])
970                .collect::<Vec<_>>(),
971            TriMeshFlags::all(),
972        )
973        .unwrap();
974
975        let res = intersect_meshes(
976            &Isometry::identity(),
977            &bunny_mesh,
978            false,
979            &Isometry::identity(),
980            &cylinder_mesh,
981            true,
982        )
983        .unwrap()
984        .unwrap();
985
986        let _ = res.to_obj_file(&PathBuf::from("complex_test.obj"));
987    }
988}