Viewport picking: a BVH per body mesh and a GPU pick buffer for visible edges (#1776)

Thanks @oursland.

crates/geom/src/bvh.rs

@@ -0,0 +1,270 @@
+//! A bounding volume hierarchy over boxes: the nearest hit along a ray, or every item whose box
+//! passes a test, without looking at each item. Built once (median split on the widest axis of
+//! the box centres, a few items per leaf) and only read afterwards. [`MeshIndex`] holds one over
+//! a mesh's triangles and one over its edge segments, for picking.
+
+use crate::{Aabb3, Mesh, Vec3};
+
+/// Most items in a leaf.
+const LEAF: usize = 4;
+
+#[derive(Clone, Copy, Debug)]
+struct Node {
+    bounds: Aabb3,
+    /// A leaf: the first of its `count` items in [`Bvh::order`]. Otherwise its children are
+    /// `first` and `first + 1`.
+    first: u32,
+    count: u32,
+}
+
+/// A hierarchy over boxes; items are the boxes' indices.
+#[derive(Clone, Debug, Default)]
+pub struct Bvh {
+    nodes: Vec<Node>,
+    order: Vec<u32>,
+}
+
+impl Bvh {
+    /// The hierarchy over `boxes` (empty boxes are left out).
+    pub fn new(boxes: &[Aabb3]) -> Bvh {
+        let mut order: Vec<u32> =
+            (0..boxes.len().min(u32::MAX as usize)).filter(|&i| boxes.get(i).is_some_and(|b| !b.is_empty() && finite(b))).map(|i| i as u32).collect();
+        if order.is_empty() {
+            return Bvh::default();
+        }
+        let item = |i: u32| boxes.get(i as usize).copied().unwrap_or(Aabb3::EMPTY);
+        let centre = |i: u32| item(i).center();
+        let mut nodes = vec![Node { bounds: Aabb3::EMPTY, first: 0, count: 0 }];
+        // (node, start, end) in `order` still to be filled in.
+        let mut todo = vec![(0usize, 0usize, order.len())];
+        while let Some((n, start, end)) = todo.pop() {
+            let Some(span) = order.get_mut(start..end) else { continue };
+            let mut bounds = Aabb3::EMPTY;
+            let mut centres = Aabb3::EMPTY;
+            for &i in span.iter() {
+                bounds = bounds.union(&item(i));
+                centres.add(centre(i));
+            }
+            let size = centres.size();
+            let axis = if size.x >= size.y && size.x >= size.z {
+                0
+            } else if size.y >= size.z {
+                1
+            } else {
+                2
+            };
+            let extent = [size.x, size.y, size.z][axis];
+            if span.len() <= LEAF || !(extent > 0.0) {
+                if let Some(node) = nodes.get_mut(n) {
+                    *node = Node { bounds, first: start as u32, count: span.len() as u32 };
+                }
+                continue;
+            }
+            let key = |i: &u32| {
+                let c = centre(*i);
+                [c.x, c.y, c.z][axis]
+            };
+            let mid = span.len() / 2;
+            span.select_nth_unstable_by(mid, |a, b| key(a).total_cmp(&key(b)));
+            let left = nodes.len();
+            nodes.push(Node { bounds: Aabb3::EMPTY, first: 0, count: 0 });
+            nodes.push(Node { bounds: Aabb3::EMPTY, first: 0, count: 0 });
+            if let Some(node) = nodes.get_mut(n) {
+                *node = Node { bounds, first: left as u32, count: 0 };
+            }
+            todo.push((left, start, start + mid));
+            todo.push((left + 1, start + mid, end));
+        }
+        Bvh { nodes, order }
+    }
+
+    /// The box around everything (empty for no items).
+    pub fn bounds(&self) -> Aabb3 {
+        self.nodes.first().map_or(Aabb3::EMPTY, |n| n.bounds)
+    }
+
+    /// Calls `item` with every item in a leaf whose box, and every box above it, `enter` accepts.
+    pub fn visit(&self, mut enter: impl FnMut(&Aabb3) -> bool, mut item: impl FnMut(u32)) {
+        let mut stack = vec![0usize];
+        while let Some(n) = stack.pop() {
+            let Some(node) = self.nodes.get(n) else { continue };
+            if !enter(&node.bounds) {
+                continue;
+            }
+            if node.count > 0 {
+                let (a, b) = (node.first as usize, node.first as usize + node.count as usize);
+                for &i in self.order.get(a..b).unwrap_or(&[]) {
+                    item(i);
+                }
+            } else {
+                stack.push(node.first as usize + 1);
+                stack.push(node.first as usize);
+            }
+        }
+    }
+
+    /// The nearest item along the ray from `o` along `d`: `hit` gives an item's distance along
+    /// `d` (`None`: missed). Equal distances go to the lowest item, as a scan in item order
+    /// keeping strictly nearer hits would answer. Boxes are entered nearest first, and one that
+    /// starts past the best hit so far is skipped.
+    pub fn nearest(&self, o: Vec3, d: Vec3, mut hit: impl FnMut(u32) -> Option<f64>) -> Option<(f64, u32)> {
+        if ![o.x, o.y, o.z, d.x, d.y, d.z].iter().all(|v| v.is_finite()) {
+            return None;
+        }
+        let inv = Vec3::new(1.0 / d.x, 1.0 / d.y, 1.0 / d.z);
+        let mut best: Option<(f64, u32)> = None;
+        let root = self.nodes.first().and_then(|n| ray_box(o, d, inv, &n.bounds, 0.0))?;
+        let mut stack = vec![(0usize, root)];
+        while let Some((n, t0)) = stack.pop() {
+            if best.is_some_and(|(bt, _)| t0 > bt) {
+                continue;
+            }
+            let Some(node) = self.nodes.get(n) else { continue };
+            if node.count > 0 {
+                let (a, b) = (node.first as usize, node.first as usize + node.count as usize);
+                for &i in self.order.get(a..b).unwrap_or(&[]) {
+                    if let Some(t) = hit(i)
+                        && best.is_none_or(|(bt, bi)| t < bt || (t == bt && i < bi))
+                    {
+                        best = Some((t, i));
+                    }
+                }
+                continue;
+            }
+            let (l, r) = (node.first as usize, node.first as usize + 1);
+            let tl = self.nodes.get(l).and_then(|c| ray_box(o, d, inv, &c.bounds, 0.0));
+            let tr = self.nodes.get(r).and_then(|c| ray_box(o, d, inv, &c.bounds, 0.0));
+            // The nearer child is popped first.
+            match (tl, tr) {
+                (Some(a), Some(b)) if a <= b => stack.extend([(r, b), (l, a)]),
+                (Some(a), Some(b)) => stack.extend([(l, a), (r, b)]),
+                (Some(a), None) => stack.push((l, a)),
+                (None, Some(b)) => stack.push((r, b)),
+                (None, None) => {}
+            }
+        }
+        best
+    }
+}
+
+fn finite(b: &Aabb3) -> bool {
+    [b.min.x, b.min.y, b.min.z, b.max.x, b.max.y, b.max.z].iter().all(|v| v.is_finite())
+}
+
+/// Where the ray from `o` along `d` (`inv` = 1 / `d` per axis) enters box `b` grown by `pad`,
+/// from 0 on; `None` when it misses or the box lies behind.
+fn ray_box(o: Vec3, d: Vec3, inv: Vec3, b: &Aabb3, pad: f64) -> Option<f64> {
+    let (mut t0, mut t1) = (0.0f64, f64::INFINITY);
+    for (o, d, inv, lo, hi) in [(o.x, d.x, inv.x, b.min.x, b.max.x), (o.y, d.y, inv.y, b.min.y, b.max.y), (o.z, d.z, inv.z, b.min.z, b.max.z)] {
+        let (lo, hi) = (lo - pad, hi + pad);
+        if d == 0.0 || !inv.is_finite() {
+            if o < lo || o > hi {
+                return None;
+            }
+            continue;
+        }
+        let (a, b) = ((lo - o) * inv, (hi - o) * inv);
+        let (a, b) = if a <= b { (a, b) } else { (b, a) };
+        t0 = t0.max(a);
+        t1 = t1.min(b);
+        if t0 > t1 {
+            return None;
+        }
+    }
+    (t0 <= t1).then_some(t0)
+}
+
+/// The farthest any corner of `b` lies along `d` from `o` (0 when the box is behind).
+fn farthest_along(o: Vec3, d: Vec3, b: &Aabb3) -> f64 {
+    let along = |lo: f64, hi: f64, o: f64, d: f64| if d >= 0.0 { (hi - o) * d } else { (lo - o) * d };
+    (along(b.min.x, b.max.x, o.x, d.x) + along(b.min.y, b.max.y, o.y, d.y) + along(b.min.z, b.max.z, o.z, d.z)).max(0.0)
+}
+
+/// A mesh's triangles and edge segments in hierarchies, for picking: the nearest triangle along
+/// a ray, and the edge segments near one. Built from one mesh and answers only for it.
+#[derive(Clone, Debug, Default)]
+pub struct MeshIndex {
+    triangles: Bvh,
+    triangle_count: usize,
+    segments: Bvh,
+    /// Per segment item: its edge, and its first point in that edge's polyline.
+    segment_of: Vec<(u32, u32)>,
+}
+
+impl MeshIndex {
+    /// The index of `mesh`.
+    pub fn new(mesh: &Mesh) -> MeshIndex {
+        let pad = |mut b: Aabb3| {
+            // Flat boxes (a triangle square to an axis) are grown a little, so a ray through
+            // them is never lost to rounding in the box test.
+            let scale = [b.min.x, b.min.y, b.min.z, b.max.x, b.max.y, b.max.z].iter().fold(1.0f64, |m, v| m.max(v.abs()));
+            let e = scale * 1e-9;
+            b.min -= Vec3::new(e, e, e);
+            b.max += Vec3::new(e, e, e);
+            b
+        };
+        let boxes: Vec<Aabb3> = mesh
+            .triangles
+            .iter()
+            .map(|t| {
+                let mut b = Aabb3::EMPTY;
+                for p in mesh.tri(t).into_iter().flatten() {
+                    b.add(p);
+                }
+                if b.is_empty() { b } else { pad(b) }
+            })
+            .collect();
+        let mut segment_of = Vec::new();
+        let mut seg_boxes = Vec::new();
+        for (ei, e) in mesh.edges.iter().enumerate() {
+            // A one-point edge still has its vertex: it stands as a segment from it to itself.
+            let n = if e.len() == 1 { 1 } else { e.len().saturating_sub(1) };
+            for si in 0..n {
+                let (Some(a), Some(b)) = (e.get(si), e.get(si + 1).or(e.get(si))) else { continue };
+                let mut bx = Aabb3::EMPTY;
+                bx.add(*a);
+                bx.add(*b);
+                seg_boxes.push(pad(bx));
+                segment_of.push((ei as u32, si as u32));
+            }
+        }
+        MeshIndex { triangles: Bvh::new(&boxes), triangle_count: mesh.triangles.len(), segments: Bvh::new(&seg_boxes), segment_of }
+    }
+
+    /// [`Mesh::raycast`] through the index: the same answer, including which triangle wins a tie.
+    /// For a mesh other than the one indexed, the plain scan answers.
+    pub fn raycast(&self, mesh: &Mesh, o: Vec3, dir: Vec3) -> Option<(f64, usize)> {
+        if mesh.triangles.len() != self.triangle_count {
+            return mesh.raycast(o, dir);
+        }
+        let frame = crate::mesh::RayFrame::new(o, dir)?;
+        self.triangles
+            .nearest(o, dir, |i| mesh.triangles.get(i as usize).and_then(|t| mesh.tri(t)).and_then(|p| frame.hit(p)))
+            .map(|(t, i)| (t, i as usize))
+    }
+
+    /// Calls `f(edge, first point)` for every edge segment that may come within `a + b·t` of the
+    /// ray from `o` along unit `d`, `t` being the distance along it (each at most once). A
+    /// constant `a` suits an orthographic view; a perspective view widens with `b`.
+    pub fn segments_near(&self, o: Vec3, d: Vec3, a: f64, b: f64, mut f: impl FnMut(usize, usize)) {
+        if ![a, b, o.x, o.y, o.z, d.x, d.y, d.z].iter().all(|v| v.is_finite()) {
+            return;
+        }
+        let inv = Vec3::new(1.0 / d.x, 1.0 / d.y, 1.0 / d.z);
+        self.segments.visit(
+            |bx| {
+                let reach = a.max(0.0) + b.max(0.0) * farthest_along(o, d, bx);
+                ray_box(o, d, inv, bx, reach).is_some()
+            },
+            |i| {
+                if let Some((e, s)) = self.segment_of.get(i as usize) {
+                    f(*e as usize, *s as usize);
+                }
+            },
+        );
+    }
+}
+
+#[cfg(test)]
+#[path = "bvh_tests.rs"]
+mod tests;