parry3d_f64/transformation/mesh_intersection/
triangle_triangle_intersection.rs

1use super::EPS;
2use crate::math::{Point, Real, Vector};
3use crate::query;
4use crate::query::PointQuery;
5use crate::shape::{FeatureId, Segment, Triangle};
6use crate::transformation::polygon_intersection::{
7    PolygonIntersectionTolerances, PolylinePointLocation,
8};
9use crate::utils::WBasis;
10use alloc::{vec, vec::Vec};
11use na::Point2;
12
13#[derive(Copy, Clone, Debug, Default)]
14pub struct TriangleTriangleIntersectionPoint {
15    pub p1: Point<Real>,
16}
17
18#[derive(Clone, Debug)]
19pub enum TriangleTriangleIntersection {
20    Segment {
21        a: TriangleTriangleIntersectionPoint,
22        b: TriangleTriangleIntersectionPoint,
23    },
24    Polygon(Vec<TriangleTriangleIntersectionPoint>),
25}
26
27impl Default for TriangleTriangleIntersection {
28    fn default() -> Self {
29        Self::Segment {
30            a: Default::default(),
31            b: Default::default(),
32        }
33    }
34}
35
36pub(crate) fn triangle_triangle_intersection(
37    tri1: &Triangle,
38    tri2: &Triangle,
39    collinearity_epsilon: Real,
40) -> Option<TriangleTriangleIntersection> {
41    let normal1 = tri1.robust_normal();
42    let normal2 = tri2.robust_normal();
43
44    if let Some(intersection_dir) = normal1.cross(&normal2).try_normalize(1.0e-6) {
45        let mut range1 = [
46            (Real::MAX, Point::origin(), FeatureId::Unknown),
47            (-Real::MAX, Point::origin(), FeatureId::Unknown),
48        ];
49        let mut range2 = [
50            (Real::MAX, Point::origin(), FeatureId::Unknown),
51            (-Real::MAX, Point::origin(), FeatureId::Unknown),
52        ];
53
54        let hits1 = [
55            segment_plane_intersection(&tri2.a, &normal2, &Segment::new(tri1.a, tri1.b), 0, (0, 1))
56                .map(|(p, feat)| (intersection_dir.dot(&p.coords), p, feat)),
57            segment_plane_intersection(&tri2.a, &normal2, &Segment::new(tri1.b, tri1.c), 1, (1, 2))
58                .map(|(p, feat)| (intersection_dir.dot(&p.coords), p, feat)),
59            segment_plane_intersection(&tri2.a, &normal2, &Segment::new(tri1.c, tri1.a), 2, (2, 0))
60                .map(|(p, feat)| (intersection_dir.dot(&p.coords), p, feat)),
61        ];
62
63        for hit1 in hits1.into_iter().flatten() {
64            if hit1.0 < range1[0].0 {
65                range1[0] = hit1;
66            }
67            if hit1.0 > range1[1].0 {
68                range1[1] = hit1;
69            }
70        }
71
72        if range1[0].0 >= range1[1].0 {
73            // The first triangle doesn’t intersect the second plane.
74            return None;
75        }
76
77        let hits2 = [
78            segment_plane_intersection(&tri1.a, &normal1, &Segment::new(tri2.a, tri2.b), 0, (0, 1))
79                .map(|(p, feat)| (intersection_dir.dot(&p.coords), p, feat)),
80            segment_plane_intersection(&tri1.a, &normal1, &Segment::new(tri2.b, tri2.c), 1, (1, 2))
81                .map(|(p, feat)| (intersection_dir.dot(&p.coords), p, feat)),
82            segment_plane_intersection(&tri1.a, &normal1, &Segment::new(tri2.c, tri2.a), 2, (2, 0))
83                .map(|(p, feat)| (intersection_dir.dot(&p.coords), p, feat)),
84        ];
85
86        for hit2 in hits2.into_iter().flatten() {
87            if hit2.0 < range2[0].0 {
88                range2[0] = hit2;
89            }
90            if hit2.0 > range2[1].0 {
91                range2[1] = hit2;
92            }
93        }
94
95        if range2[0].0 >= range2[1].0 {
96            // The second triangle doesn’t intersect the first plane.
97            return None;
98        }
99
100        if range1[1].0 <= range2[0].0 + EPS || range2[1].0 <= range1[0].0 + EPS {
101            // The two triangles intersect each others’ plane, but these intersections are disjoint.
102            return None;
103        }
104
105        let a = if range2[0].0 > range1[0].0 + EPS {
106            TriangleTriangleIntersectionPoint { p1: range2[0].1 }
107        } else {
108            TriangleTriangleIntersectionPoint { p1: range1[0].1 }
109        };
110
111        let b = if range2[1].0 < range1[1].0 - EPS {
112            TriangleTriangleIntersectionPoint { p1: range2[1].1 }
113        } else {
114            TriangleTriangleIntersectionPoint { p1: range1[1].1 }
115        };
116
117        Some(TriangleTriangleIntersection::Segment { a, b })
118    } else {
119        let unit_normal2 = normal2.normalize();
120        if (tri1.a - tri2.a).dot(&unit_normal2) < EPS {
121            let basis = unit_normal2.orthonormal_basis();
122            let proj = |vect: Vector<Real>| Point2::new(vect.dot(&basis[0]), vect.dot(&basis[1]));
123
124            let mut intersections = vec![];
125
126            let pts1 = tri1.vertices();
127            let pts2 = tri2.vertices();
128            let poly1 = [
129                proj(tri1.a - tri2.a),
130                proj(tri1.b - tri2.a),
131                proj(tri1.c - tri2.a),
132            ];
133            let poly2 = [
134                proj(Vector::zeros()), // = proj(tri2.a - tri2.a)
135                proj(tri2.b - tri2.a),
136                proj(tri2.c - tri2.a),
137            ];
138
139            let convert_loc = |loc, pts: &[Point<Real>; 3]| match loc {
140                PolylinePointLocation::OnVertex(vid) => (FeatureId::Vertex(vid as u32), pts[vid]),
141                PolylinePointLocation::OnEdge(vid1, vid2, bcoords) => (
142                    match (vid1, vid2) {
143                        (0, 1) | (1, 0) => FeatureId::Edge(0),
144                        (1, 2) | (2, 1) => FeatureId::Edge(1),
145                        (2, 0) | (0, 2) => FeatureId::Edge(2),
146                        _ => unreachable!(),
147                    },
148                    pts[vid1] * bcoords[0] + pts[vid2].coords * bcoords[1],
149                ),
150            };
151
152            crate::transformation::convex_polygons_intersection_with_tolerances(
153                &poly1,
154                &poly2,
155                PolygonIntersectionTolerances {
156                    collinearity_epsilon,
157                },
158                |pt1, pt2| {
159                    let intersection = match (pt1, pt2) {
160                        (Some(loc1), Some(loc2)) => {
161                            let (_f1, p1) = convert_loc(loc1, pts1);
162                            let (_f2, _p2) = convert_loc(loc2, pts2);
163                            TriangleTriangleIntersectionPoint { p1 }
164                        }
165                        (Some(loc1), None) => {
166                            let (_f1, p1) = convert_loc(loc1, pts1);
167                            TriangleTriangleIntersectionPoint { p1 }
168                        }
169                        (None, Some(loc2)) => {
170                            let (_f2, p2) = convert_loc(loc2, pts2);
171                            TriangleTriangleIntersectionPoint { p1: p2 }
172                        }
173                        (None, None) => unreachable!(),
174                    };
175                    intersections.push(intersection);
176                },
177            );
178
179            #[cfg(feature = "std")]
180            {
181                // NOTE: set this to `true` to automatically check if the computed intersection is
182                //       valid, and print debug infos if it is not.
183                const DEBUG_INTERSECTIONS: bool = false;
184                if DEBUG_INTERSECTIONS {
185                    debug_check_intersections(tri1, tri2, &basis, &poly1, &poly2, &intersections);
186                }
187            }
188
189            Some(TriangleTriangleIntersection::Polygon(intersections))
190        } else {
191            None
192        }
193    }
194}
195
196fn segment_plane_intersection(
197    plane_center: &Point<Real>,
198    plane_normal: &Vector<Real>,
199    segment: &Segment,
200    eid: u32,
201    vids: (u32, u32),
202) -> Option<(Point<Real>, FeatureId)> {
203    let dir = segment.b - segment.a;
204    let dir_norm = dir.norm();
205
206    let time_of_impact =
207        query::details::line_toi_with_halfspace(plane_center, plane_normal, &segment.a, &dir)?;
208    let scaled_toi = time_of_impact * dir_norm;
209
210    if scaled_toi < -EPS || scaled_toi > dir_norm + EPS {
211        None
212    } else if scaled_toi <= EPS {
213        Some((segment.a, FeatureId::Vertex(vids.0)))
214    } else if scaled_toi >= dir_norm - EPS {
215        Some((segment.b, FeatureId::Vertex(vids.1)))
216    } else {
217        Some((segment.a + dir * time_of_impact, FeatureId::Edge(eid)))
218    }
219}
220
221/// Prints debug information if the calculated intersection of two triangles is detected to be
222/// invalid.
223///
224/// If the intersection is valid, this prints nothing. If it isn't valid, this will print a few
225/// lines to copy/paste into the Desmos online graphing tool (for visual debugging), as well as
226/// some rust code to add to the `tris` array in the `intersect_triangle_common_vertex` test for
227/// regression checking.
228#[cfg(feature = "std")]
229fn debug_check_intersections(
230    tri1: &Triangle,
231    tri2: &Triangle,
232    basis: &[na::Vector3<Real>; 2],
233    poly1: &[Point2<Real>], // Projection of tri1 on the basis `basis1` with the origin at tri2.a.
234    poly2: &[Point2<Real>], // Projection of tri2 on the basis `basis2` with the origin at tri2.a.
235    intersections: &[TriangleTriangleIntersectionPoint],
236) {
237    use std::{print, println};
238
239    let proj = |vect: Vector<Real>| Point2::new(vect.dot(&basis[0]), vect.dot(&basis[1]));
240    let mut incorrect = false;
241    for pt in intersections {
242        if !tri1
243            .project_local_point(&pt.p1, false)
244            .is_inside_eps(&pt.p1, 1.0e-5)
245        {
246            incorrect = true;
247            break;
248        }
249
250        if !tri2
251            .project_local_point(&pt.p1, false)
252            .is_inside_eps(&pt.p1, 1.0e-5)
253        {
254            incorrect = true;
255            break;
256        }
257    }
258
259    if incorrect {
260        let proj_inter: Vec<_> = intersections.iter().map(|p| proj(p.p1 - tri2.a)).collect();
261        println!("-------- (copy/paste the following on Desmos graphing)");
262        println!("A=({:.2},{:.2})", poly1[0].x, poly1[0].y);
263        println!("B=({:.2},{:.2})", poly1[1].x, poly1[1].y);
264        println!("C=({:.2},{:.2})", poly1[2].x, poly1[2].y);
265        println!("D=({:.2},{:.2})", poly2[0].x, poly2[0].y);
266        println!("E=({:.2},{:.2})", poly2[1].x, poly2[1].y);
267        println!("F=({:.2},{:.2})", poly2[2].x, poly2[2].y);
268
269        let lbls = ["G", "H", "I", "J", "K", "L", "M", "N", "O"];
270        for (i, inter) in proj_inter.iter().enumerate() {
271            println!("{}=({:.2},{:.2})", lbls[i], inter.x, inter.y);
272        }
273
274        // polygons
275        println!("X=polygon(A,B,C)");
276        println!("Y=polygon(D,E,F)");
277        print!("Z=polygon({}", lbls[0]);
278        for lbl in lbls.iter().skip(1) {
279            print!(",{lbl}");
280        }
281        println!(")");
282
283        println!("~~~~~~~ (copy/paste the following input in the `intersect_triangle_common_vertex` test)");
284        println!("(Triangle::new(");
285        for pt1 in poly1 {
286            println!("    Point2::new({},{}),", pt1.x, pt1.y);
287        }
288        println!("),");
289        println!("Triangle::new(");
290        for pt2 in poly2 {
291            println!("    Point2::new({},{}),", pt2.x, pt2.y);
292        }
293        println!("),),");
294    }
295}
parry3d_f64/transformation/mesh_intersection/triangle_triangle_intersection.rs

parry3d_f64/transformation/mesh_intersection/
triangle_triangle_intersection.rs
