axiolid_measure/
mesh_proximity.rs

1//! Mesh-level proximity: distance with witnesses, and proximity components.
2//!
3//! # What the kernel decides and what it does not
4//!
5//! The kernel owns the distance, the witnesses, and the component
6//! decomposition. It does not own the verdict: whether a separation is a
7//! clash, a tolerance violation, or acceptable is caller policy.
8//!
9//! # Surface contact is not solid penetration
10//!
11//! These routines measure SURFACE separation. Two solids whose boundaries
12//! touch report distance zero; so do two that interpenetrate deeply, because
13//! their surfaces cross. A contact area and an interpenetration depth are
14//! different measurements and must not be conflated behind one number, so
15//! neither is inferred here: [`MeshDistance::surfaces_cross`] reports the fact
16//! and leaves the interpretation to the caller.
17
18use axiolid_core::Point3;
19use axiolid_mesh::TriangleMeshView;
20
21use crate::proximity::{closest_points_on_triangles, ClosestPoints3, ProximityError};
22
23/// Why a mesh proximity query could not be answered.
24#[derive(Debug, Clone, Copy, PartialEq, Eq)]
25#[non_exhaustive]
26pub enum MeshProximityError {
27    /// A mesh had no usable triangles.
28    ///
29    /// Distance to nothing is undefined; returning infinity would let a
30    /// caller read an empty mesh as "very far away" rather than "no input".
31    EmptyMesh,
32    /// A triangle referenced a position the mesh does not have.
33    IndexOutOfRange,
34    /// The threshold was negative or not finite.
35    InvalidThreshold,
36    /// A primitive query rejected its input.
37    Primitive(ProximityError),
38}
39
40impl From<ProximityError> for MeshProximityError {
41    fn from(error: ProximityError) -> Self {
42        Self::Primitive(error)
43    }
44}
45
46/// The separation between two meshes and the witness pair realising it.
47#[derive(Debug, Clone, Copy, PartialEq)]
48pub struct MeshDistance {
49    /// Witness on the first mesh.
50    pub point_a: Point3,
51    /// Witness on the second mesh.
52    pub point_b: Point3,
53    /// Squared separation of the witnesses.
54    ///
55    /// Squared to stay exact for the comparisons callers make with it; take
56    /// the square root only for display.
57    pub distance_squared: f64,
58    /// Index of the first mesh triangle carrying the witness.
59    pub triangle_a: usize,
60    /// Index of the second mesh triangle carrying the witness.
61    pub triangle_b: usize,
62    /// The surfaces meet or cross at the witness.
63    ///
64    /// True exactly when the separation is zero. This is a SURFACE fact and
65    /// says nothing about penetration depth or contact area: grazing contact
66    /// and deep interpenetration both report zero here.
67    pub surfaces_cross: bool,
68}
69
70/// A region where two meshes approach within a threshold.
71#[derive(Debug, Clone, PartialEq)]
72pub struct ProximityComponent {
73    /// The closest approach within this component.
74    pub witness: MeshDistance,
75    /// First-mesh triangles participating, ascending.
76    pub triangles_a: Vec<usize>,
77    /// Second-mesh triangles participating, ascending.
78    pub triangles_b: Vec<usize>,
79}
80
81/// Distance between two meshes, with the witness pair realising it.
82///
83/// Exhaustive over triangle pairs: correct by construction, and the reference
84/// the broad-phase-accelerated path is checked against.
85///
86/// Ties break on the lowest `(triangle_a, triangle_b)` pair, so the witness is
87/// reproducible rather than dependent on iteration order.
88pub fn mesh_distance<A, B>(first: &A, second: &B) -> Result<MeshDistance, MeshProximityError>
89where
90    A: TriangleMeshView + ?Sized,
91    B: TriangleMeshView + ?Sized,
92{
93    let (triangles_a, _) = collect(first)?;
94    let (triangles_b, _) = collect(second)?;
95
96    let mut best: Option<MeshDistance> = None;
97    for (index_a, tri_a) in triangles_a.iter().enumerate() {
98        for (index_b, tri_b) in triangles_b.iter().enumerate() {
99            let pair = closest_points_on_triangles(*tri_a, *tri_b)?;
100            let candidate = to_distance(pair, index_a, index_b);
101            // Strictly less, so the first pair in index order wins a tie.
102            if best.is_none_or(|current| candidate.distance_squared < current.distance_squared) {
103                best = Some(candidate);
104            }
105        }
106    }
107
108    best.ok_or(MeshProximityError::EmptyMesh)
109}
110
111fn to_distance(pair: ClosestPoints3, triangle_a: usize, triangle_b: usize) -> MeshDistance {
112    MeshDistance {
113        point_a: pair.point_a,
114        point_b: pair.point_b,
115        distance_squared: pair.distance_squared,
116        triangle_a,
117        triangle_b,
118        surfaces_cross: pair.distance_squared == 0.0,
119    }
120}
121
122type Collected = (Vec<[Point3; 3]>, Vec<[usize; 3]>);
123
124fn collect<M: TriangleMeshView + ?Sized>(mesh: &M) -> Result<Collected, MeshProximityError> {
125    let positions = mesh.position_count();
126    let mut triangles = Vec::with_capacity(mesh.triangle_count());
127    let mut corner_indices = Vec::with_capacity(mesh.triangle_count());
128    for index in 0..mesh.triangle_count() {
129        let corners = mesh.triangle(index);
130        let mut points = [Point3::ZERO; 3];
131        let mut corner_ids = [0usize; 3];
132        for (slot, corner) in corners.iter().enumerate() {
133            let corner =
134                usize::try_from(*corner).map_err(|_| MeshProximityError::IndexOutOfRange)?;
135            if corner >= positions {
136                return Err(MeshProximityError::IndexOutOfRange);
137            }
138            points[slot] = mesh.position(corner);
139            corner_ids[slot] = corner;
140        }
141        triangles.push(points);
142        corner_indices.push(corner_ids);
143    }
144    if triangles.is_empty() {
145        return Err(MeshProximityError::EmptyMesh);
146    }
147    Ok((triangles, corner_indices))
148}
149
150/// Disjoint regions where two meshes approach within `threshold`.
151///
152/// Two close pairs belong to the same component when they share a triangle on
153/// either mesh; connectivity is transitive through that relation. This groups
154/// one physical approach into one component instead of reporting every
155/// triangle pair separately.
156///
157/// Components are ordered by closest approach, nearest first, with ties broken
158/// on the witness triangle indices so the order is reproducible.
159pub fn proximity_components<A, B>(
160    first: &A,
161    second: &B,
162    threshold: f64,
163) -> Result<Vec<ProximityComponent>, MeshProximityError>
164where
165    A: TriangleMeshView + ?Sized,
166    B: TriangleMeshView + ?Sized,
167{
168    if !threshold.is_finite() || threshold < 0.0 {
169        return Err(MeshProximityError::InvalidThreshold);
170    }
171    let (triangles_a, corners_a) = collect(first)?;
172    let (triangles_b, corners_b) = collect(second)?;
173    let limit = threshold * threshold;
174
175    // Close pairs first; the union-find below groups them.
176    let mut close = Vec::new();
177    for (index_a, tri_a) in triangles_a.iter().enumerate() {
178        for (index_b, tri_b) in triangles_b.iter().enumerate() {
179            let pair = closest_points_on_triangles(*tri_a, *tri_b)?;
180            if pair.distance_squared <= limit {
181                close.push(to_distance(pair, index_a, index_b));
182            }
183        }
184    }
185    if close.is_empty() {
186        return Ok(Vec::new());
187    }
188
189    // Two close pairs belong to the same approach when their triangles are
190    // CONNECTED on both meshes: same triangle, or sharing a vertex with it.
191    //
192    // Weaker rules were tried and are wrong. Sharing a triangle on either mesh
193    // alone merges everything one long triangle touches, so a bar spanning two
194    // separated squares reports a single approach. Witness distance alone
195    // splits one approach into several, because witnesses on adjacent
196    // triangles of the same flat face sit a whole triangle apart, which says
197    // nothing about the approach's extent. Adjacency is the relation that
198    // actually tracks "same piece of surface".
199    let mut parent: Vec<usize> = (0..close.len()).collect();
200    for i in 0..close.len() {
201        for j in i + 1..close.len() {
202            let linked_a = adjacent(&corners_a, close[i].triangle_a, close[j].triangle_a);
203            let linked_b = adjacent(&corners_b, close[i].triangle_b, close[j].triangle_b);
204            if linked_a && linked_b {
205                union(&mut parent, i, j);
206            }
207        }
208    }
209
210    let mut groups: Vec<(usize, Vec<usize>)> = Vec::new();
211    for index in 0..close.len() {
212        let root = find(&mut parent, index);
213        match groups.iter_mut().find(|(key, _)| *key == root) {
214            Some((_, members)) => members.push(index),
215            None => groups.push((root, vec![index])),
216        }
217    }
218
219    let mut components: Vec<ProximityComponent> = groups
220        .into_iter()
221        .map(|(_, members)| build_component(&close, &members))
222        .collect();
223    components.sort_by(|a, b| {
224        a.witness
225            .distance_squared
226            .total_cmp(&b.witness.distance_squared)
227            .then(a.witness.triangle_a.cmp(&b.witness.triangle_a))
228            .then(a.witness.triangle_b.cmp(&b.witness.triangle_b))
229    });
230    Ok(components)
231}
232
233fn build_component(close: &[MeshDistance], members: &[usize]) -> ProximityComponent {
234    let mut witness = close[members[0]];
235    let mut triangles_a = Vec::new();
236    let mut triangles_b = Vec::new();
237    for &index in members {
238        let entry = close[index];
239        if entry.distance_squared < witness.distance_squared {
240            witness = entry;
241        }
242        triangles_a.push(entry.triangle_a);
243        triangles_b.push(entry.triangle_b);
244    }
245    triangles_a.sort_unstable();
246    triangles_a.dedup();
247    triangles_b.sort_unstable();
248    triangles_b.dedup();
249    ProximityComponent {
250        witness,
251        triangles_a,
252        triangles_b,
253    }
254}
255
256fn find(parent: &mut [usize], mut node: usize) -> usize {
257    while parent[node] != node {
258        parent[node] = parent[parent[node]];
259        node = parent[node];
260    }
261    node
262}
263
264fn union(parent: &mut [usize], a: usize, b: usize) {
265    let (root_a, root_b) = (find(parent, a), find(parent, b));
266    if root_a != root_b {
267        parent[root_b] = root_a;
268    }
269}
270
271/// Two triangles are the same or share at least one vertex.
272fn adjacent(corners: &[[usize; 3]], first: usize, second: usize) -> bool {
273    first == second
274        || corners[first]
275            .iter()
276            .any(|a| corners[second].iter().any(|b| a == b))
277}
axiolid_measure/mesh_proximity.rs

axiolid_measure/
mesh_proximity.rs
