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        if p1_id != p2_id {
443            edges.push([p1_id, p2_id]);
444        }
445    }
446
447    let mut points: Vec<_> = point_set.iter().cloned().collect();
448    points.sort_by(|a, b| a.id.cmp(&b.id));
449
450    let tri_points = tri.vertices();
451    let best_source = tri.angle_closest_to_90();
452    let d1 = tri_points[(best_source + 2) % 3] - tri_points[(best_source + 1) % 3];
453    let d2 = tri_points[best_source] - tri_points[(best_source + 1) % 3];
454    let (e1, e2) = planar_gram_schmidt(d1, d2);
455
456    let project = |p: &Point3<Real>| spade::Point2::new(e1.dot(&p.coords), e2.dot(&p.coords));
457
458    // Project points into 2D and triangulate the resulting set.
459    let planar_points: Vec<_> = points
460        .iter()
461        .copied()
462        .map(|point| {
463            let point_proj = project(&point.point);
464            utils::sanitize_spade_point(point_proj)
465        })
466        .collect();
467    let cdt_triangulation = ConstrainedDelaunayTriangulation::bulk_load_cdt(planar_points, edges)?;
468    debug_assert!(cdt_triangulation.vertices().len() == points.len());
469
470    let points = points.into_iter().map(|p| Point3::from(p.point)).collect();
471    Ok((cdt_triangulation, points))
472}
473
474// We heavily recommend that this is left here in case one needs to debug the above code.
475#[cfg(feature = "wavefront")]
476fn _points_to_obj(mesh: &[Point3<Real>], path: &PathBuf) {
477    use std::io::Write;
478    let mut file = std::fs::File::create(path).unwrap();
479
480    for p in mesh {
481        writeln!(file, "v {} {} {}", p.x, p.y, p.z).unwrap();
482    }
483}
484
485// We heavily recommend that this is left here in case one needs to debug the above code.
486#[cfg(feature = "wavefront")]
487fn _points_and_edges_to_obj(mesh: &[Point3<Real>], edges: &[[usize; 2]], path: &PathBuf) {
488    use std::io::Write;
489    let mut file = std::fs::File::create(path).unwrap();
490
491    for p in mesh {
492        writeln!(file, "v {} {} {}", p.x, p.y, p.z).unwrap();
493    }
494
495    for e in edges {
496        writeln!(file, "l {} {}", e[0] + 1, e[1] + 1).unwrap();
497    }
498}
499
500#[derive(Copy, Clone, PartialEq, Debug, Default)]
501struct TreePoint {
502    point: Point3<Real>,
503    id: usize,
504}
505
506impl rstar::Point for TreePoint {
507    type Scalar = Real;
508    const DIMENSIONS: usize = 3;
509
510    fn generate(mut generator: impl FnMut(usize) -> Self::Scalar) -> Self {
511        TreePoint {
512            point: Point3::new(generator(0), generator(1), generator(2)),
513            id: usize::MAX,
514        }
515    }
516
517    fn nth(&self, index: usize) -> Self::Scalar {
518        self.point[index]
519    }
520
521    fn nth_mut(&mut self, index: usize) -> &mut Self::Scalar {
522        &mut self.point[index]
523    }
524}
525
526fn insert_into_set(
527    position: Point3<Real>,
528    point_set: &mut RTree<TreePoint>,
529    epsilon: Real,
530) -> usize {
531    let point_count = point_set.size();
532    let point_to_insert = TreePoint {
533        point: position,
534        id: point_count,
535    };
536
537    match point_set.nearest_neighbor(&point_to_insert) {
538        Some(tree_point) => {
539            if (tree_point.point - position).norm_squared() <= epsilon {
540                tree_point.id
541            } else {
542                point_set.insert(point_to_insert);
543                debug_assert!(point_set.size() == point_count + 1);
544                point_count
545            }
546        }
547        None => {
548            point_set.insert(point_to_insert);
549            debug_assert!(point_set.size() == point_count + 1);
550            point_count
551        }
552    }
553}
554
555fn smallest_angle(points: &[Point3<Real>]) -> Real {
556    let n = points.len();
557
558    let mut worst_cos: Real = -2.0;
559    for i in 0..points.len() {
560        let d1 = (points[i] - points[(i + 1) % n]).normalize();
561        let d2 = (points[(i + 2) % n] - points[(i + 1) % n]).normalize();
562
563        let cos = d1.dot(&d2);
564        if cos > worst_cos {
565            worst_cos = cos;
566        }
567    }
568
569    worst_cos.acos()
570}
571
572fn planar_gram_schmidt(v1: Vector3<Real>, v2: Vector3<Real>) -> (Vector3<Real>, Vector3<Real>) {
573    let u1 = v1;
574    let u2 = v2 - (v2.dot(&u1) / u1.norm_squared()) * u1;
575
576    let e1 = u1.normalize();
577    let e2 = u2.normalize();
578
579    (e1, e2)
580}
581
582fn project_point_to_segment(point: &Point3<Real>, segment: &[Point3<Real>; 2]) -> Point3<Real> {
583    let dir = segment[1] - segment[0];
584    let local = point - segment[0];
585
586    let norm = dir.norm();
587    // Restrict the result to the segment portion of the line.
588    let coeff = (dir.dot(&local) / norm).clamp(0., norm);
589
590    segment[0] + coeff * dir.normalize()
591}
592
593/// No matter how smart we are about computing intersections. It is always possible
594/// to create ultra thin triangles when a point lies on an edge of a triangle. These
595/// are degenerate and need to be removed.
596fn is_triangle_degenerate(
597    triangle: &[Point3<Real>; 3],
598    epsilon_angle: Real,
599    epsilon_distance: Real,
600) -> bool {
601    if smallest_angle(triangle) < epsilon_angle {
602        return true;
603    }
604
605    for i in 0..3 {
606        let mut dir = triangle[(i + 1) % 3] - triangle[(i + 2) % 3];
607        if dir.normalize_mut() < epsilon_distance {
608            return true;
609        }
610
611        let proj = triangle[(i + 2) % 3] + (triangle[i] - triangle[(i + 2) % 3]).dot(&dir) * dir;
612
613        if (proj - triangle[i]).norm() < epsilon_distance {
614            return true;
615        }
616    }
617
618    false
619}
620
621fn merge_triangle_sets(
622    mesh1: &TriMesh,
623    mesh2: &TriMesh,
624    triangle_constraints: &BTreeMap<&u32, Vec<[Point3<Real>; 2]>>,
625    pos1: &Isometry<Real>,
626    pos2: &Isometry<Real>,
627    flip1: bool,
628    flip2: bool,
629    metadata: &MeshIntersectionTolerances,
630    point_set: &mut RTree<TreePoint>,
631    topology_indices: &mut HashMap<HashableTriangleIndices, [u32; 3]>,
632) -> Result<(), MeshIntersectionError> {
633    // For each triangle, and each constraint edge associated to that triangle,
634    // make a triangulation of the face and sort whether each generated
635    // sub-triangle is part of the intersection.
636    // For each sub-triangle that is part of the intersection, add them to the
637    // output mesh.
638    for (triangle_id, constraints) in triangle_constraints.iter() {
639        let tri = mesh1.triangle(**triangle_id).transformed(pos1);
640
641        let (delaunay, points) = triangulate_constraints_and_merge_duplicates(
642            &tri,
643            constraints,
644            metadata.global_insertion_epsilon * metadata.local_insertion_epsilon_scale,
645        )
646        .or(Err(MeshIntersectionError::TriangulationError))?;
647
648        for face in delaunay.inner_faces() {
649            let verts = face.vertices();
650            let p1 = points[verts[0].index()];
651            let p2 = points[verts[1].index()];
652            let p3 = points[verts[2].index()];
653
654            // Sometimes the triangulation is messed up due to numerical errors. If
655            // a triangle does not survive this test it should be deleted.
656            if is_triangle_degenerate(
657                &[p1, p2, p3],
658                metadata.angle_epsilon,
659                metadata.global_insertion_epsilon,
660            ) {
661                continue;
662            }
663
664            let center = Triangle {
665                a: p1,
666                b: p2,
667                c: p3,
668            }
669            .center();
670
671            let epsilon = metadata.global_insertion_epsilon;
672            let projection = mesh2
673                .project_local_point_and_get_location(&pos2.inverse_transform_point(&center), true)
674                .0;
675
676            if flip2 ^ (projection.is_inside_eps(&center, epsilon)) {
677                let mut new_tri_idx = [
678                    insert_into_set(p1, point_set, epsilon) as u32,
679                    insert_into_set(p2, point_set, epsilon) as u32,
680                    insert_into_set(p3, point_set, epsilon) as u32,
681                ];
682
683                if flip1 {
684                    new_tri_idx.swap(0, 1)
685                }
686
687                // This should *never* trigger. If it does
688                // it means the code has created a triangle with duplicate vertices,
689                // which means we encountered an unaccounted for edge case.
690                if is_topologically_degenerate(new_tri_idx) {
691                    return Err(MeshIntersectionError::DuplicateVertices);
692                }
693
694                insert_topology_indices(topology_indices, new_tri_idx);
695            }
696        }
697    }
698
699    Ok(())
700}
701
702// Insert in the hashmap with sorted indices to avoid adding duplicates.
703//
704// We also check if we don’t keep pairs of triangles that have the same
705// set of indices but opposite orientations. If this happens, both the new triangle, and the one it
706// matched with are removed (because they describe a degenerate piece of volume).
707fn insert_topology_indices(
708    topology_indices: &mut HashMap<HashableTriangleIndices, [u32; 3]>,
709    new_tri_idx: [u32; 3],
710) {
711    match topology_indices.entry(new_tri_idx.into()) {
712        Entry::Vacant(e) => {
713            let _ = e.insert(new_tri_idx);
714        }
715        Entry::Occupied(e) => {
716            fn same_orientation(a: &[u32; 3], b: &[u32; 3]) -> bool {
717                let ib = if a[0] == b[0] {
718                    0
719                } else if a[0] == b[1] {
720                    1
721                } else {
722                    2
723                };
724                a[1] == b[(ib + 1) % 3]
725            }
726
727            if !same_orientation(e.get(), &new_tri_idx) {
728                // If we are inserting two identical triangles but with mismatching
729                // orientations, we can just ignore both because they cover a degenerate
730                // 2D plane.
731                #[cfg(feature = "enhanced-determinism")]
732                let _ = e.swap_remove();
733                #[cfg(not(feature = "enhanced-determinism"))]
734                let _ = e.remove();
735            }
736        }
737    }
738}
739
740fn is_topologically_degenerate(tri_idx: [u32; 3]) -> bool {
741    tri_idx[0] == tri_idx[1] || tri_idx[0] == tri_idx[2] || tri_idx[1] == tri_idx[2]
742}
743
744#[cfg(feature = "wavefront")]
745#[cfg(test)]
746mod tests {
747    use super::*;
748    use crate::shape::{Ball, Cuboid, TriMeshFlags};
749    use obj::Obj;
750    use obj::ObjData;
751
752    #[test]
753    fn test_same_mesh_intersection() {
754        let Obj {
755            data: ObjData {
756                position, objects, ..
757            },
758            ..
759        } = Obj::load("../../assets/tests/low_poly_bunny.obj").unwrap();
760
761        let mesh = TriMesh::with_flags(
762            position
763                .iter()
764                .map(|v| Point3::new(v[0] as Real, v[1] as Real, v[2] as Real))
765                .collect::<Vec<_>>(),
766            objects[0].groups[0]
767                .polys
768                .iter()
769                .map(|p| [p.0[0].0 as u32, p.0[1].0 as u32, p.0[2].0 as u32])
770                .collect::<Vec<_>>(),
771            TriMeshFlags::all(),
772        )
773        .unwrap();
774
775        let _ = intersect_meshes(
776            &Isometry::identity(),
777            &mesh,
778            false,
779            &Isometry::identity(),
780            &mesh,
781            false,
782        )
783        .unwrap()
784        .unwrap();
785
786        let _ = mesh.to_obj_file(&PathBuf::from("same_test.obj"));
787    }
788
789    #[test]
790    fn test_non_origin_pos1_pos2_intersection() {
791        let ball = Ball::new(2f32 as Real).to_trimesh(10, 10);
792        let cuboid = Cuboid::new(Vector3::new(2.0, 1.0, 1.0)).to_trimesh();
793        let mut sphere_mesh = TriMesh::new(ball.0, ball.1).unwrap();
794        sphere_mesh.set_flags(TriMeshFlags::all()).unwrap();
795        let mut cuboid_mesh = TriMesh::new(cuboid.0, cuboid.1).unwrap();
796        cuboid_mesh.set_flags(TriMeshFlags::all()).unwrap();
797
798        let res = intersect_meshes(
799            &Isometry::translation(1.0, 0.0, 0.0),
800            &cuboid_mesh,
801            false,
802            &Isometry::translation(2.0, 0.0, 0.0),
803            &sphere_mesh,
804            false,
805        )
806        .unwrap()
807        .unwrap();
808
809        let _ = res.to_obj_file(&PathBuf::from("test_non_origin_pos1_pos2_intersection.obj"));
810
811        let bounding_sphere = res.local_bounding_sphere();
812        assert!(bounding_sphere.center == Point3::new(1.5, 0.0, 0.0));
813        assert_relative_eq!(2.0615528, bounding_sphere.radius, epsilon = 1.0e-5);
814    }
815
816    #[test]
817    fn test_offset_cylinder_intersection() {
818        let Obj {
819            data: ObjData {
820                position, objects, ..
821            },
822            ..
823        } = Obj::load("../../assets/tests/offset_cylinder.obj").unwrap();
824
825        let offset_mesh = TriMesh::with_flags(
826            position
827                .iter()
828                .map(|v| Point3::new(v[0] as Real, v[1] as Real, v[2] as Real))
829                .collect::<Vec<_>>(),
830            objects[0].groups[0]
831                .polys
832                .iter()
833                .map(|p| [p.0[0].0 as u32, p.0[1].0 as u32, p.0[2].0 as u32])
834                .collect::<Vec<_>>(),
835            TriMeshFlags::all(),
836        )
837        .unwrap();
838
839        let Obj {
840            data: ObjData {
841                position, objects, ..
842            },
843            ..
844        } = Obj::load("../../assets/tests/center_cylinder.obj").unwrap();
845
846        let center_mesh = TriMesh::with_flags(
847            position
848                .iter()
849                .map(|v| Point3::new(v[0] as Real, v[1] as Real, v[2] as Real))
850                .collect::<Vec<_>>(),
851            objects[0].groups[0]
852                .polys
853                .iter()
854                .map(|p| [p.0[0].0 as u32, p.0[1].0 as u32, p.0[2].0 as u32])
855                .collect::<Vec<_>>(),
856            TriMeshFlags::all(),
857        )
858        .unwrap();
859
860        let res = intersect_meshes(
861            &Isometry::identity(),
862            &center_mesh,
863            false,
864            &Isometry::identity(),
865            &offset_mesh,
866            false,
867        )
868        .unwrap()
869        .unwrap();
870
871        let _ = res.to_obj_file(&PathBuf::from("offset_test.obj"));
872    }
873
874    #[test]
875    fn test_stair_bar_intersection() {
876        let Obj {
877            data: ObjData {
878                position, objects, ..
879            },
880            ..
881        } = Obj::load("../../assets/tests/stairs.obj").unwrap();
882
883        let stair_mesh = TriMesh::with_flags(
884            position
885                .iter()
886                .map(|v| Point3::new(v[0] as Real, v[1] as Real, v[2] as Real))
887                .collect::<Vec<_>>(),
888            objects[0].groups[0]
889                .polys
890                .iter()
891                .map(|p| [p.0[0].0 as u32, p.0[1].0 as u32, p.0[2].0 as u32])
892                .collect::<Vec<_>>(),
893            TriMeshFlags::all(),
894        )
895        .unwrap();
896
897        let Obj {
898            data: ObjData {
899                position, objects, ..
900            },
901            ..
902        } = Obj::load("../../assets/tests/bar.obj").unwrap();
903
904        let bar_mesh = TriMesh::with_flags(
905            position
906                .iter()
907                .map(|v| Point3::new(v[0] as Real, v[1] as Real, v[2] as Real))
908                .collect::<Vec<_>>(),
909            objects[0].groups[0]
910                .polys
911                .iter()
912                .map(|p| [p.0[0].0 as u32, p.0[1].0 as u32, p.0[2].0 as u32])
913                .collect::<Vec<_>>(),
914            TriMeshFlags::all(),
915        )
916        .unwrap();
917
918        let res = intersect_meshes(
919            &Isometry::identity(),
920            &stair_mesh,
921            false,
922            &Isometry::identity(),
923            &bar_mesh,
924            false,
925        )
926        .unwrap()
927        .unwrap();
928
929        let _ = res.to_obj_file(&PathBuf::from("stair_test.obj"));
930    }
931
932    #[test]
933    fn test_complex_intersection() {
934        let Obj {
935            data: ObjData {
936                position, objects, ..
937            },
938            ..
939        } = Obj::load("../../assets/tests/low_poly_bunny.obj").unwrap();
940
941        let bunny_mesh = TriMesh::with_flags(
942            position
943                .iter()
944                .map(|v| Point3::new(v[0] as Real, v[1] as Real, v[2] as Real))
945                .collect::<Vec<_>>(),
946            objects[0].groups[0]
947                .polys
948                .iter()
949                .map(|p| [p.0[0].0 as u32, p.0[1].0 as u32, p.0[2].0 as u32])
950                .collect::<Vec<_>>(),
951            TriMeshFlags::all(),
952        )
953        .unwrap();
954
955        let Obj {
956            data: ObjData {
957                position, objects, ..
958            },
959            ..
960        } = Obj::load("../../assets/tests/poly_cylinder.obj").unwrap();
961
962        let cylinder_mesh = TriMesh::with_flags(
963            position
964                .iter()
965                .map(|v| Point3::new(v[0] as Real, v[1] as Real, v[2] as Real))
966                .collect::<Vec<_>>(),
967            objects[0].groups[0]
968                .polys
969                .iter()
970                .map(|p| [p.0[0].0 as u32, p.0[1].0 as u32, p.0[2].0 as u32])
971                .collect::<Vec<_>>(),
972            TriMeshFlags::all(),
973        )
974        .unwrap();
975
976        let res = intersect_meshes(
977            &Isometry::identity(),
978            &bunny_mesh,
979            false,
980            &Isometry::identity(),
981            &cylinder_mesh,
982            true,
983        )
984        .unwrap()
985        .unwrap();
986
987        let _ = res.to_obj_file(&PathBuf::from("complex_test.obj"));
988    }
989}