use crate::math::{Aabb, Point3, Transform, Vec3};
use super::Mesh;
pub(crate) struct Bvh {
boxes: Vec<Aabb>,
nodes: Vec<(Aabb, Node)>,
order: Vec<usize>,
}
enum Node {
Inner(usize, usize),
Leaf(usize, usize),
}
impl Bvh {
pub(crate) fn new(mesh: &Mesh) -> Bvh {
let boxes: Vec<Aabb> = mesh
.triangles
.iter()
.map(|t| {
let mut b = Aabb::EMPTY;
for &i in t {
b.add(mesh.positions[i as usize]);
}
b
})
.collect();
Bvh::of_boxes(boxes)
}
pub(crate) fn of_boxes(boxes: Vec<Aabb>) -> Bvh {
let mut bvh = Bvh {
order: (0..boxes.len()).collect(),
boxes,
nodes: Vec::new(),
};
if !bvh.order.is_empty() {
bvh.split(0, bvh.order.len());
}
bvh
}
fn split(&mut self, lo: usize, hi: usize) -> usize {
let mut bb = Aabb::EMPTY;
for &i in &self.order[lo..hi] {
bb.union(&self.boxes[i]);
}
let at = self.nodes.len();
if hi - lo <= 4 {
self.nodes.push((bb, Node::Leaf(lo, hi)));
return at;
}
self.nodes.push((bb, Node::Leaf(lo, hi)));
let ext = bb.max - bb.min;
let axis = if ext.x >= ext.y && ext.x >= ext.z {
0
} else if ext.y >= ext.z {
1
} else {
2
};
let key = |b: &Aabb| {
let c = (b.min + b.max) * 0.5;
[c.x, c.y, c.z][axis]
};
let mid = (lo + hi) / 2;
let boxes = &self.boxes;
self.order[lo..hi].sort_by(|&i, &j| key(&boxes[i]).total_cmp(&key(&boxes[j])));
let l = self.split(lo, mid);
let r = self.split(mid, hi);
self.nodes[at].1 = Node::Inner(l, r);
at
}
pub(crate) fn along_line(&self, q: &Aabb, d: Vec3, out: &mut Vec<usize>) {
out.clear();
if self.nodes.is_empty() {
return;
}
let mut stack = vec![0];
while let Some(n) = stack.pop() {
let (bb, node) = &self.nodes[n];
if !line_meets(bb, q, d) {
continue;
}
match *node {
Node::Inner(l, r) => {
stack.push(l);
stack.push(r);
}
Node::Leaf(lo, hi) => out.extend(
self.order[lo..hi]
.iter()
.copied()
.filter(|&i| line_meets(&self.boxes[i], q, d)),
),
}
}
}
pub(crate) fn within(&self, q: &Aabb, d: f64, out: &mut Vec<usize>) {
out.clear();
if self.nodes.is_empty() {
return;
}
let mut stack = vec![0];
while let Some(n) = stack.pop() {
let (bb, node) = &self.nodes[n];
if box_gap(bb, q) > d {
continue;
}
match *node {
Node::Inner(l, r) => {
stack.push(l);
stack.push(r);
}
Node::Leaf(lo, hi) => out.extend(
self.order[lo..hi]
.iter()
.copied()
.filter(|&i| box_gap(&self.boxes[i], q) <= d),
),
}
}
}
pub(crate) fn along(
&self,
o: Point3,
d: Vec3,
(t0, t1): (f64, f64),
pad: f64,
out: &mut Vec<usize>,
) {
out.clear();
if self.nodes.is_empty() {
return;
}
let meets = |b: &Aabb| {
let (mut lo, mut hi) = (t0, t1);
for (o, d, a, z) in [
(o.x, d.x, b.min.x - pad, b.max.x + pad),
(o.y, d.y, b.min.y - pad, b.max.y + pad),
(o.z, d.z, b.min.z - pad, b.max.z + pad),
] {
if d.abs() < 1e-300 {
if o < a || o > z {
return false;
}
continue;
}
let (s0, s1) = ((a - o) / d, (z - o) / d);
lo = lo.max(s0.min(s1));
hi = hi.min(s0.max(s1));
if lo > hi {
return false;
}
}
true
};
let mut stack = vec![0];
while let Some(n) = stack.pop() {
let (bb, node) = &self.nodes[n];
if !meets(bb) {
continue;
}
match *node {
Node::Inner(l, r) => {
stack.push(l);
stack.push(r);
}
Node::Leaf(lo, hi) => out.extend(
self.order[lo..hi]
.iter()
.copied()
.filter(|&i| meets(&self.boxes[i])),
),
}
}
}
pub(crate) fn any_within(
&self,
other: &Bvh,
t: &Transform,
d: f64,
mut near: impl FnMut(usize, usize) -> bool,
) -> bool {
if self.nodes.is_empty() || other.nodes.is_empty() {
return false;
}
let moved = |b: &Aabb| moved_box(b, t);
let mut stack = vec![(0usize, 0usize)];
while let Some((a, b)) = stack.pop() {
let (ba, na) = &self.nodes[a];
let (bb, nb) = &other.nodes[b];
let bb = moved(bb);
if box_gap(ba, &bb) > d {
continue;
}
match (na, nb) {
(Node::Leaf(alo, ahi), Node::Leaf(blo, bhi)) => {
for &j in &other.order[*blo..*bhi] {
let bj = moved(&other.boxes[j]);
for &i in &self.order[*alo..*ahi] {
if box_gap(&self.boxes[i], &bj) <= d && near(i, j) {
return true;
}
}
}
}
(Node::Inner(l, r), Node::Leaf(..)) => {
stack.push((*l, b));
stack.push((*r, b));
}
(Node::Leaf(..), Node::Inner(l, r)) => {
stack.push((a, *l));
stack.push((a, *r));
}
(Node::Inner(al, ar), Node::Inner(bl, br)) => {
let ea = ba.max - ba.min;
let eb = bb.max - bb.min;
if ea.dot(ea) >= eb.dot(eb) {
stack.push((*al, b));
stack.push((*ar, b));
} else {
stack.push((a, *bl));
stack.push((a, *br));
}
}
}
}
false
}
pub(crate) fn query(&self, q: &Aabb, out: &mut Vec<usize>) {
out.clear();
if self.nodes.is_empty() {
return;
}
let meets = |b: &Aabb| {
b.min.x <= q.max.x
&& q.min.x <= b.max.x
&& b.min.y <= q.max.y
&& q.min.y <= b.max.y
&& b.min.z <= q.max.z
&& q.min.z <= b.max.z
};
let mut stack = vec![0];
while let Some(n) = stack.pop() {
let (bb, node) = &self.nodes[n];
if !meets(bb) {
continue;
}
match *node {
Node::Inner(l, r) => {
stack.push(l);
stack.push(r);
}
Node::Leaf(lo, hi) => out.extend(
self.order[lo..hi]
.iter()
.copied()
.filter(|&i| meets(&self.boxes[i])),
),
}
}
}
}
fn line_meets(b: &Aabb, q: &Aabb, d: Vec3) -> bool {
let (mut t0, mut t1) = (f64::NEG_INFINITY, f64::INFINITY);
for k in 0..3 {
let (lo, hi, step) = (q.min[k] - b.max[k], q.max[k] - b.min[k], d[k]);
if step.abs() < 1e-12 {
if lo > 0.0 || hi < 0.0 {
return false;
}
continue;
}
let (x, y) = (lo / step, hi / step);
t0 = t0.max(x.min(y));
t1 = t1.min(x.max(y));
}
t0 <= t1
}
pub(crate) fn box_gap(a: &Aabb, b: &Aabb) -> f64 {
let gap = |lo1: f64, hi1: f64, lo2: f64, hi2: f64| (lo2 - hi1).max(lo1 - hi2).max(0.0);
let (x, y, z) = (
gap(a.min.x, a.max.x, b.min.x, b.max.x),
gap(a.min.y, a.max.y, b.min.y, b.max.y),
gap(a.min.z, a.max.z, b.min.z, b.max.z),
);
(x * x + y * y + z * z).sqrt()
}
pub(crate) fn moved_box(b: &Aabb, t: &Transform) -> Aabb {
let mut out = Aabb::EMPTY;
for &x in &[b.min.x, b.max.x] {
for &y in &[b.min.y, b.max.y] {
for &z in &[b.min.z, b.max.z] {
out.add(t.point(Point3::new(x, y, z)));
}
}
}
out
}
pub(crate) fn triangles(a: &[Point3; 3], b: &[Point3; 3]) -> f64 {
let mut best = f64::INFINITY;
for (s, t) in [(a, b), (b, a)] {
for k in 0..3 {
best = best.min(segment_triangle(s[k], s[(k + 1) % 3], t).0);
if best == 0.0 {
return 0.0;
}
}
}
best
}
pub(crate) fn ray_triangle(p: Point3, d: Vec3, a: Point3, b: Point3, c: Point3) -> Option<f64> {
let (side_b, side_c) = (b - a, c - a);
let normal = side_b.cross(side_c);
let approach = d.dot(normal);
let flat = 1e-12 * d.norm() * side_b.norm() * side_c.norm();
if approach.abs() <= flat || approach.abs() < 1e-300 {
return None;
}
let t = (a - p).dot(normal) / approach;
let from_a = p + d * t - a;
let area = normal.dot(normal);
let u = from_a.cross(side_c).dot(normal) / area;
let v = side_b.cross(from_a).dot(normal) / area;
(u >= 0.0 && v >= 0.0 && u + v <= 1.0).then_some(t)
}
pub(crate) fn on_segment(p: Point3, a: Point3, b: Point3) -> Point3 {
let d = b - a;
let l = d.dot(d);
if l <= 0.0 {
return a;
}
a + d * ((p - a).dot(d) / l).clamp(0.0, 1.0)
}
pub(crate) fn segments(p1: Point3, q1: Point3, p2: Point3, q2: Point3) -> (f64, Point3, Point3) {
let (d1, d2, apart) = (q1 - p1, q2 - p2, p1 - p2);
let (l1, l2, lean) = (d1.dot(d1), d2.dot(d2), d1.dot(d2));
let mut best = (f64::INFINITY, p1, p2);
let mut offer = |c1: Point3, c2: Point3| {
let d = c1.dist(c2);
if d < best.0 {
best = (d, c1, c2);
}
};
offer(p1, on_segment(p1, p2, q2));
offer(q1, on_segment(q1, p2, q2));
offer(on_segment(p2, p1, q1), p2);
offer(on_segment(q2, p1, q1), q2);
let det = l1 * l2 - lean * lean;
if det > 0.0 {
let (pull1, pull2) = (d1.dot(apart), d2.dot(apart));
let s = (lean * pull2 - pull1 * l2) / det;
let t = (l1 * pull2 - lean * pull1) / det;
if (0.0..=1.0).contains(&s) && (0.0..=1.0).contains(&t) {
offer(p1 + d1 * s, p2 + d2 * t);
}
}
best
}
pub(crate) fn on_triangle(p: Point3, t: &[Point3; 3]) -> Point3 {
let [a, b, c] = *t;
let (side_b, side_c, from_a) = (b - a, c - a, p - a);
let (bb, bc, cc) = (side_b.dot(side_b), side_b.dot(side_c), side_c.dot(side_c));
let det = bb * cc - bc * bc;
if det > 0.0 {
let (pb, pc) = (from_a.dot(side_b), from_a.dot(side_c));
let u = (cc * pb - bc * pc) / det;
let v = (bb * pc - bc * pb) / det;
if u >= 0.0 && v >= 0.0 && u + v <= 1.0 {
return a + side_b * u + side_c * v;
}
}
let mut best = on_segment(p, a, b);
for (from, to) in [(b, c), (c, a)] {
let q = on_segment(p, from, to);
if q.dist(p) < best.dist(p) {
best = q;
}
}
best
}
pub(crate) fn segment_triangle(p: Point3, q: Point3, t: &[Point3; 3]) -> (f64, Point3, Point3) {
if let Some(x) = crossing(p, q, t) {
return (0.0, x, x);
}
let mut best = (f64::INFINITY, p, t[0]);
for e in [p, q] {
let c = on_triangle(e, t);
let d = e.dist(c);
if d < best.0 {
best = (d, e, c);
}
}
for k in 0..3 {
let x = segments(p, q, t[k], t[(k + 1) % 3]);
if x.0 < best.0 {
best = x;
}
}
best
}
pub(crate) fn crossing(p: Point3, q: Point3, t: &[Point3; 3]) -> Option<Point3> {
let d = q - p;
let s = ray_triangle(p, d, t[0], t[1], t[2])?;
(0.0..=1.0).contains(&s).then(|| p + d * s)
}
#[cfg(test)]
mod tests {
use super::*;
#[test]
fn a_segment_in_a_tilted_triangle_s_plane_does_not_cross_it_from_afar() {
let tri = [
Vec3::new(14.508628775221682, -16.768516875320916, -3.068077540287854),
Vec3::new(23.328340064120123, -3.3624006213025357, 4.255717148417697),
Vec3::new(15.411143728305284, -16.19447442838339, -2.754476722249861),
];
let other = [
Vec3::new(55.55756088708297, -0.7777271174955624, 5.667730718980721),
Vec3::new(55.55756088708297, -3.2222728825044435, 4.3322692810192756),
Vec3::new(28.176531602590885, -3.004352420640489, 4.451319771923463),
];
for (s, t) in [(&tri, &other), (&other, &tri)] {
for k in 0..3 {
let (p, q) = (s[k], s[(k + 1) % 3]);
assert!(crossing(p, q, t).is_none(), "side {k} crosses");
assert!(segment_triangle(p, q, t).0 > 4.0, "side {k} is near");
}
}
}
}