fast_mesh_scene/
contacts.rs

1//! Broadphase and contacts between the meshes of a scene.
2
3use fast_mesh_store::{StoreError, TriangleId};
4
5use fast_mesh_query::Rigid;
6
7use crate::placement::relative_rigid;
8use crate::query::{Aabb64, MARGIN, cached_world_box, check_bound, pair_pad, round_up, try_vec};
9use crate::{MeshId, Result, Scene, SceneError};
10
11/// Two meshes within the margin of [`Scene::find_contacts`].
12#[derive(Debug, Clone, PartialEq)]
13pub struct Contact {
14    /// The mesh with the lower index.
15    pub a: MeshId,
16    /// The mesh with the higher index.
17    pub b: MeshId,
18    /// Whether the two meshes intersect or touch, decided exactly.
19    pub intersecting: bool,
20    /// The distance between the two surfaces; 0 when `intersecting`.
21    pub distance: f64,
22    /// The triangles of `a` within the margin of `b`, ascending.
23    pub triangles_a: Vec<TriangleId>,
24    /// The triangles of `b` within the margin of `a`, ascending.
25    pub triangles_b: Vec<TriangleId>,
26}
27
28/// Which mesh pairs [`Scene::find_contacts`] looks at.
29#[derive(Debug, Clone, PartialEq, Eq, Default)]
30pub struct ContactFilter {
31    /// Only these pairs, in any order and either way round; `None` means
32    /// every pair of meshes.
33    pub pairs: Option<Vec<(MeshId, MeshId)>>,
34}
35
36impl Scene {
37    /// Appends a [`Contact`] for every mesh pair (of `filter`, or of the whole
38    /// scene) whose surfaces are within `margin` of each other as placed in
39    /// the world, sorted by `(a, b)`, and returns how many were appended.
40    /// Elements already in `out` stay in front.
41    ///
42    /// The broadphase keeps the pairs whose world boxes (BVH root boxes
43    /// placed by the rigid transforms) are within `margin`, padded so that it
44    /// never drops a pair Phase 3b would report. For each, in A's space:
45    /// `intersecting` comes from [`fast_mesh_query::intersects`]; `distance`
46    /// is 0 when intersecting, else from [`fast_mesh_query::mesh_distance`]
47    /// with `margin` rounded up to `f32`, and a pair whose distance exceeds
48    /// the exact `margin` is not reported; the triangle lists come from
49    /// [`fast_mesh_query::triangles_near`] with the rounded-up margin (see
50    /// [`Scene::triangles_near`]).
51    ///
52    /// With `filter.pairs`, each pair may be given either way round and more
53    /// than once; only the listed meshes' BVHs are brought up to date.
54    /// Without, every Lazy BVH is (§15.4).
55    ///
56    /// # Errors
57    /// [`SceneError::InvalidInput`] for a negative or NaN `margin` or a pair
58    /// naming one mesh twice; [`SceneError::UnknownMesh`] for a stale id in
59    /// the filter; then a BVH or memory error from a Lazy update,
60    /// [`SceneError::StaleBvh`] for the lowest stale Manual mesh involved, or
61    /// a memory error while collecting. On any error `out` keeps its length.
62    pub fn find_contacts(
63        &mut self,
64        margin: f64,
65        filter: &ContactFilter,
66        out: &mut Vec<Contact>,
67    ) -> Result<usize> {
68        self.drop_field_unit();
69        let start = out.len();
70        let found = self.find_contacts_into(margin, filter, out);
71        if found.is_err() {
72            out.truncate(start);
73        }
74        found
75    }
76
77    fn find_contacts_into(
78        &mut self,
79        margin: f64,
80        filter: &ContactFilter,
81        out: &mut Vec<Contact>,
82    ) -> Result<usize> {
83        check_bound(margin, MARGIN)?;
84        // The meshes involved, ascending, and the pairs as indices into them.
85        let (ids, pairs) = match &filter.pairs {
86            Some(list) => {
87                let mut pairs = try_vec(list.len())?;
88                for &(a, b) in list {
89                    self.entry(a)?;
90                    self.entry(b)?;
91                    if a == b {
92                        return Err(SceneError::InvalidInput(
93                            "a contact pair needs two different meshes",
94                        ));
95                    }
96                    pairs.push((a.min(b), a.max(b)));
97                }
98                pairs.sort_unstable();
99                pairs.dedup();
100                let mut ids = try_vec(2 * pairs.len())?;
101                for &(a, b) in &pairs {
102                    ids.push(a);
103                    ids.push(b);
104                }
105                ids.sort_unstable();
106                ids.dedup();
107                self.prepare(Some(&ids))?;
108                (ids, Some(pairs))
109            }
110            None => {
111                self.prepare(None)?;
112                let mut ids = try_vec(self.entries.iter().count())?;
113                ids.extend(self.ids());
114                (ids, None)
115            }
116        };
117        let mut boxes = try_vec(ids.len())?;
118        for &id in &ids {
119            boxes.push(cached_world_box(self.entry_mut(id)?).0);
120        }
121        let start = out.len();
122        match pairs {
123            Some(pairs) => {
124                for (a, b) in pairs {
125                    // Both are in `ids`, which is sorted.
126                    let i = ids.binary_search(&a).map_err(|_| SceneError::Corrupted)?;
127                    let j = ids.binary_search(&b).map_err(|_| SceneError::Corrupted)?;
128                    self.contact((a, &boxes[i]), (b, &boxes[j]), margin, out)?;
129                }
130            }
131            None => {
132                for i in 0..ids.len() {
133                    for j in i + 1..ids.len() {
134                        self.contact((ids[i], &boxes[i]), (ids[j], &boxes[j]), margin, out)?;
135                    }
136                }
137            }
138        }
139        Ok(out.len() - start)
140    }
141
142    /// Runs the broadphase test and, if it passes, the narrow phase for one
143    /// pair (`a` below `b`), appending its contact if within `margin`.
144    fn contact(
145        &self,
146        (a, wa): (MeshId, &Aabb64),
147        (b, wb): (MeshId, &Aabb64),
148        margin: f64,
149        out: &mut Vec<Contact>,
150    ) -> Result<()> {
151        if wa.is_empty() || wb.is_empty() {
152            return Ok(());
153        }
154        let (ea, eb) = (self.entry(a)?, self.entry(b)?);
155        if wa.distance_to_box(wb) > margin + pair_pad(ea, wa, eb, wb) {
156            return Ok(());
157        }
158        #[cfg(test)]
159        crate::scene::fault::before_contact()?;
160        if crate::placement::is_relative_rigid(&ea.placement, &eb.placement) {
161            let xf = relative_rigid(
162                &ea.placement,
163                ea.mesh.origin(),
164                &eb.placement,
165                eb.mesh.origin(),
166            );
167            return self.contact_rigid(a, b, &xf, margin, out);
168        }
169        self.contact_scaled(a, b, margin, out)
170    }
171
172    /// The narrow phase for one pair with the relative transform `xf`: the
173    /// rigid path of [`Scene::contact`].
174    fn contact_rigid(
175        &self,
176        a: MeshId,
177        b: MeshId,
178        xf: &Rigid,
179        margin: f64,
180        out: &mut Vec<Contact>,
181    ) -> Result<()> {
182        let (ea, eb) = (self.entry(a)?, self.entry(b)?);
183        let (ma, ba, mb, bb) = (&ea.mesh, &ea.bvh, &eb.mesh, &eb.bvh);
184        let m32 = round_up(margin);
185        let intersecting = fast_mesh_query::intersects(ma, ba, mb, bb, xf)?;
186        let distance = if intersecting {
187            0.0
188        } else {
189            match fast_mesh_query::mesh_distance(ma, ba, mb, bb, xf, m32)? {
190                Some(d) if f64::from(d.distance) <= margin => f64::from(d.distance),
191                _ => return Ok(()),
192            }
193        };
194        let (mut triangles_a, mut triangles_b) = (Vec::new(), Vec::new());
195        fast_mesh_query::triangles_near(
196            ma,
197            ba,
198            mb,
199            bb,
200            xf,
201            m32,
202            &mut triangles_a,
203            &mut triangles_b,
204        )?;
205        out.try_reserve(1)
206            .map_err(|_| SceneError::Store(StoreError::OutOfMemory))?;
207        out.push(Contact {
208            a,
209            b,
210            intersecting,
211            distance,
212            triangles_a,
213            triangles_b,
214        });
215        Ok(())
216    }
217
218    /// The narrow phase for one scaled pair: on a copy of B baked into A's
219    /// local space (or of both into the world when A is non-uniformly
220    /// scaled; see [`Scene::bake_pair_inner`]). Frame distances map to the
221    /// world times the frame's factor.
222    fn contact_scaled(
223        &self,
224        a: MeshId,
225        b: MeshId,
226        margin: f64,
227        out: &mut Vec<Contact>,
228    ) -> Result<()> {
229        let (ta, tb, s) = self.bake_pair_inner(a, b)?;
230        let ea = self.entry(a)?;
231        let (ma, ba) = match &ta {
232            None => (&ea.mesh, &ea.bvh),
233            Some((m, bvh)) => (m, bvh),
234        };
235        let (mb, bb) = (&tb.0, &tb.1);
236        let xf = Rigid::IDENTITY;
237        let m32 = round_up(margin / s);
238        let intersecting = fast_mesh_query::intersects(ma, ba, mb, bb, &xf)?;
239        let distance = if intersecting {
240            0.0
241        } else {
242            match fast_mesh_query::mesh_distance(ma, ba, mb, bb, &xf, m32)? {
243                Some(d) if s * f64::from(d.distance) <= margin => s * f64::from(d.distance),
244                _ => return Ok(()),
245            }
246        };
247        let (mut triangles_a, mut triangles_b) = (Vec::new(), Vec::new());
248        fast_mesh_query::triangles_near(
249            ma,
250            ba,
251            mb,
252            bb,
253            &xf,
254            m32,
255            &mut triangles_a,
256            &mut triangles_b,
257        )?;
258        out.try_reserve(1)
259            .map_err(|_| SceneError::Store(StoreError::OutOfMemory))?;
260        out.push(Contact {
261            a,
262            b,
263            intersecting,
264            distance,
265            triangles_a,
266            triangles_b,
267        });
268        Ok(())
269    }
270}
271
272#[cfg(test)]
273mod tests {
274    use super::*;
275    use crate::placement::relative_rigid;
276    use crate::query::tests::{RX90, RZ90, cube, place};
277    use crate::query::world_box;
278    use crate::{MeshOptions, Placement, SceneOptions, UpdateMode};
279
280    /// Every pair `a < b` by calling Phase 3b directly.
281    fn brute(scene: &Scene, margin: f64, pairs: &[(MeshId, MeshId)]) -> Vec<Contact> {
282        let m32 = margin as f32;
283        assert_eq!(f64::from(m32), margin, "test margins are exact in f32");
284        let mut out = Vec::new();
285        for &(a, b) in pairs {
286            let (ea, eb) = (scene.entries.get(a).unwrap(), scene.entries.get(b).unwrap());
287            let xf = relative_rigid(
288                &ea.placement,
289                ea.mesh.origin(),
290                &eb.placement,
291                eb.mesh.origin(),
292            );
293            let (ma, ba, mb, bb) = (&ea.mesh, &ea.bvh, &eb.mesh, &eb.bvh);
294            let intersecting = fast_mesh_query::intersects(ma, ba, mb, bb, &xf).unwrap();
295            let distance = if intersecting {
296                0.0
297            } else {
298                match fast_mesh_query::mesh_distance(ma, ba, mb, bb, &xf, m32).unwrap() {
299                    Some(d) => f64::from(d.distance),
300                    None => continue,
301                }
302            };
303            let (mut ta, mut tb) = (Vec::new(), Vec::new());
304            fast_mesh_query::triangles_near(ma, ba, mb, bb, &xf, m32, &mut ta, &mut tb).unwrap();
305            out.push(Contact {
306                a,
307                b,
308                intersecting,
309                distance,
310                triangles_a: ta,
311                triangles_b: tb,
312            });
313        }
314        out
315    }
316
317    fn all_pairs(scene: &Scene) -> Vec<(MeshId, MeshId)> {
318        let ids: Vec<MeshId> = scene.ids().collect();
319        let mut pairs = Vec::new();
320        for (i, &a) in ids.iter().enumerate() {
321            for &b in &ids[i + 1..] {
322                pairs.push((a, b));
323            }
324        }
325        pairs
326    }
327
328    /// A rotation by 45 degrees about z.
329    fn rz45() -> [[f64; 3]; 3] {
330        let (s, c) = 45f64.to_radians().sin_cos();
331        [[c, -s, 0.0], [s, c, 0.0], [0.0, 0.0, 1.0]]
332    }
333
334    fn scene() -> (Scene, Vec<MeshId>) {
335        let mut scene = Scene::new(SceneOptions::default());
336        let identity = Placement::IDENTITY.rotation;
337        let ids = vec![
338            // [0, 2]^3.
339            scene
340                .add_mesh(cube([0.0; 3], 2.0), MeshOptions::default())
341                .unwrap(),
342            // x in [1.5, 2.5]: crosses the first.
343            scene
344                .add_mesh(cube([0.0; 3], 1.0), place(RZ90, [2.5, 0.5, 0.5]))
345                .unwrap(),
346            // y in [2.25, 3.25]: 0.25 from the first.
347            scene
348                .add_mesh(cube([0.0; 3], 1.0), place(RX90, [0.5, 3.25, 1.0]))
349                .unwrap(),
350            // Far from everything.
351            scene
352                .add_mesh(cube([0.0; 3], 1.0), place(identity, [10.0, 10.0, 10.0]))
353                .unwrap(),
354            // Turned 45 degrees, off the first's edge x = 0, y = 2: its box
355            // is about 0.41 away, its surface about 0.91.
356            scene
357                .add_mesh(
358                    cube([-0.5, -0.5, 0.0], 1.0),
359                    place(rz45(), [-1.0, 3.0, 0.5]),
360                )
361                .unwrap(),
362            // 0.6 beyond the first along +z: outside a 0.5 margin.
363            scene
364                .add_mesh(cube([0.0; 3], 1.0), place(identity, [0.5, 0.5, 2.6]))
365                .unwrap(),
366        ];
367        (scene, ids)
368    }
369
370    #[test]
371    fn find_contacts_matches_all_pairs() {
372        let (mut scene, ids) = scene();
373        let pairs = all_pairs(&scene);
374        for margin in [0.0, 0.25, 0.5, 1.0, 4.0, f64::INFINITY] {
375            let want = if margin.is_finite() {
376                brute(&scene, margin, &pairs)
377            } else {
378                brute(&scene, f64::from(f32::MAX), &pairs)
379            };
380            let mut out = Vec::new();
381            let n = scene
382                .find_contacts(margin, &ContactFilter::default(), &mut out)
383                .unwrap();
384            assert_eq!(n, out.len());
385            assert_eq!(out, want, "margin {margin}");
386            if !margin.is_finite() {
387                assert_eq!(out.len(), pairs.len());
388            }
389        }
390        let mut out = Vec::new();
391        scene
392            .find_contacts(0.5, &ContactFilter::default(), &mut out)
393            .unwrap();
394        let found: Vec<_> = out
395            .iter()
396            .map(|c| (c.a, c.b, c.intersecting, c.distance))
397            .collect();
398        // The turned cube passes the broadphase but not the narrow phase.
399        let (e0, e4) = (
400            scene.entries.get(ids[0]).unwrap(),
401            scene.entries.get(ids[4]).unwrap(),
402        );
403        let box_gap = world_box(e0).distance_to_box(&world_box(e4));
404        assert!((0.4..0.45).contains(&box_gap), "{box_gap}");
405        assert!(out.iter().all(|c| (c.a, c.b) != (ids[0], ids[4])));
406        let d = scene.mesh_distance(ids[0], ids[4], 1.0).unwrap().unwrap();
407        assert!((0.9..0.95).contains(&d.distance), "{}", d.distance);
408        assert!(found.contains(&(ids[0], ids[1], true, 0.0)));
409        assert!(found.contains(&(ids[0], ids[2], false, 0.25)));
410        assert!(out.iter().all(|c| c.a != ids[3] && c.b != ids[3]));
411        assert!(out.iter().all(|c| c.b != ids[5] || c.a != ids[0]));
412        assert!(out.windows(2).all(|w| (w[0].a, w[0].b) < (w[1].a, w[1].b)));
413    }
414
415    #[test]
416    fn find_contacts_with_a_pair_filter() {
417        let (mut scene, ids) = scene();
418        let filter = ContactFilter {
419            pairs: Some(vec![
420                (ids[2], ids[0]),
421                (ids[0], ids[2]),
422                (ids[3], ids[1]),
423                (ids[1], ids[0]),
424            ]),
425        };
426        let mut out = vec![];
427        let n = scene.find_contacts(0.5, &filter, &mut out).unwrap();
428        let want = brute(&scene, 0.5, &[(ids[0], ids[1]), (ids[0], ids[2])]);
429        assert_eq!(n, 2);
430        assert_eq!(out, want);
431        // Existing elements stay in front.
432        let n = scene.find_contacts(0.5, &filter, &mut out).unwrap();
433        assert_eq!(n, 2);
434        assert_eq!(out[..2], out[2..]);
435    }
436
437    #[test]
438    fn find_contacts_staleness_and_errors() {
439        let (mut scene, ids) = scene();
440        let keep = || {
441            vec![Contact {
442                a: ids[0],
443                b: ids[1],
444                intersecting: false,
445                distance: 9.0,
446                triangles_a: vec![],
447                triangles_b: vec![],
448            }]
449        };
450        // A stale Manual mesh fails a search over all pairs, and one naming
451        // it; `out` is left as it was.
452        scene.set_mode(ids[3], UpdateMode::Manual).unwrap();
453        scene
454            .edit(ids[3], |tx| tx.set_positions(&[0], &[[0.0, 0.0, -0.5]]))
455            .unwrap();
456        let lazy = ids[4];
457        scene.set_mode(lazy, UpdateMode::Lazy).unwrap();
458        scene
459            .edit(lazy, |tx| tx.set_positions(&[0], &[[-0.5, -0.5, -0.25]]))
460            .unwrap();
461        let mut out = keep();
462        let all = ContactFilter::default();
463        assert_eq!(
464            scene.find_contacts(0.5, &all, &mut out),
465            Err(SceneError::StaleBvh(ids[3]))
466        );
467        assert_eq!(out, keep());
468        let with = ContactFilter {
469            pairs: Some(vec![(ids[0], ids[3])]),
470        };
471        assert_eq!(
472            scene.find_contacts(0.5, &with, &mut out),
473            Err(SceneError::StaleBvh(ids[3]))
474        );
475        assert_eq!(out, keep());
476        // The search over all pairs still updated the Lazy mesh first.
477        assert_eq!(scene.is_current(lazy), Ok(true));
478        scene
479            .edit(lazy, |tx| tx.set_positions(&[0], &[[-0.5, -0.5, -0.3]]))
480            .unwrap();
481        // A filter that leaves it out runs, and updates only what it names.
482        scene
483            .edit(ids[2], |tx| tx.set_positions(&[0], &[[0.0, 0.0, -0.1]]))
484            .unwrap();
485        scene.set_mode(ids[2], UpdateMode::Lazy).unwrap();
486        scene
487            .edit(ids[2], |tx| tx.set_positions(&[0], &[[0.0, 0.0, -0.2]]))
488            .unwrap();
489        let without = ContactFilter {
490            pairs: Some(vec![(ids[0], ids[2])]),
491        };
492        assert_eq!(scene.find_contacts(0.5, &without, &mut out), Ok(1));
493        assert_eq!(scene.is_current(ids[2]), Ok(true));
494        assert_eq!(scene.is_current(lazy), Ok(false));
495        out.truncate(1);
496        // Bad inputs.
497        let same = ContactFilter {
498            pairs: Some(vec![(ids[0], ids[0])]),
499        };
500        assert!(matches!(
501            scene.find_contacts(0.5, &same, &mut out),
502            Err(SceneError::InvalidInput(_))
503        ));
504        for margin in [-1.0, f64::NAN] {
505            assert!(matches!(
506                scene.find_contacts(margin, &without, &mut out),
507                Err(SceneError::InvalidInput(_))
508            ));
509        }
510        let gone = ids[5];
511        scene.remove_mesh(gone).unwrap();
512        let unknown = ContactFilter {
513            pairs: Some(vec![(ids[0], ids[2]), (gone, ids[0])]),
514        };
515        assert_eq!(
516            scene.find_contacts(0.5, &unknown, &mut out),
517            Err(SceneError::UnknownMesh)
518        );
519        assert_eq!(out, keep());
520    }
521
522    #[test]
523    fn find_contacts_truncates_after_a_midway_failure() {
524        let (mut scene, ids) = scene();
525        let keep = || {
526            vec![Contact {
527                a: ids[0],
528                b: ids[1],
529                intersecting: false,
530                distance: 9.0,
531                triangles_a: vec![],
532                triangles_b: vec![],
533            }]
534        };
535        // The second pair to reach the narrow phase fails, after the first
536        // was appended: the error leaves `out` as it was.
537        let mut out = keep();
538        crate::scene::fault::fail_contact_on(2);
539        assert_eq!(
540            scene.find_contacts(0.5, &ContactFilter::default(), &mut out),
541            Err(SceneError::Store(StoreError::OutOfMemory))
542        );
543        assert_eq!(out, keep());
544        // The hook fires once: the search works again afterwards.
545        let n = scene
546            .find_contacts(0.5, &ContactFilter::default(), &mut out)
547            .unwrap();
548        assert!(n >= 2, "{n}");
549        assert_eq!(out.len(), 1 + n);
550    }
551}
fast_mesh_scene/contacts.rs

fast_mesh_scene/
contacts.rs
