brep_kernel/geometry/

spatial.rs

//! Shared axis-aligned bounding boxes and a static bounding-volume
//! hierarchy over per-face boxes.
//!
//! Rational surfaces and curves with positive weights lie inside the convex
//! hull of their control points, so a control-point box is a conservative
//! bound for every point on the exact geometry. Queries expand boxes by the
//! caller's tolerance, which keeps BVH pruning conservative for the same
//! tolerance the narrow phase uses.

use crate::surface::NurbsSurface;
use crate::Vec3;

#[derive(Clone, Copy, Debug)]
pub struct Aabb {
    pub minimum: Vec3,
    pub maximum: Vec3,
}

fn axis(point: Vec3, index: usize) -> f64 {
    match index {
        0 => point.x,
        1 => point.y,
        _ => point.z,
    }
}

impl Aabb {
    pub fn empty() -> Self {
        Self {
            minimum: Vec3::new(f64::INFINITY, f64::INFINITY, f64::INFINITY),
            maximum: Vec3::new(f64::NEG_INFINITY, f64::NEG_INFINITY, f64::NEG_INFINITY),
        }
    }

    pub fn include_point(&mut self, point: Vec3) {
        self.minimum.x = self.minimum.x.min(point.x);
        self.minimum.y = self.minimum.y.min(point.y);
        self.minimum.z = self.minimum.z.min(point.z);
        self.maximum.x = self.maximum.x.max(point.x);
        self.maximum.y = self.maximum.y.max(point.y);
        self.maximum.z = self.maximum.z.max(point.z);
    }

    pub fn include(&mut self, other: Self) {
        self.include_point(other.minimum);
        self.include_point(other.maximum);
    }

    pub fn from_surface_controls(surface: &NurbsSurface) -> Result<Self, String> {
        let mut bounds = Self::empty();
        for control in surface.control_points.iter().flatten() {
            bounds.include_point(control.point()?);
        }
        Ok(bounds)
    }

    pub fn expanded(self, amount: f64) -> Self {
        let delta = Vec3::new(amount, amount, amount);
        Self {
            minimum: self.minimum.sub(delta),
            maximum: self.maximum.add(delta),
        }
    }

    pub fn contains(self, point: Vec3) -> bool {
        point.x >= self.minimum.x
            && point.x <= self.maximum.x
            && point.y >= self.minimum.y
            && point.y <= self.maximum.y
            && point.z >= self.minimum.z
            && point.z <= self.maximum.z
    }

    pub fn intersects(self, other: Self, tolerance: f64) -> bool {
        self.minimum.x - tolerance <= other.maximum.x
            && self.maximum.x + tolerance >= other.minimum.x
            && self.minimum.y - tolerance <= other.maximum.y
            && self.maximum.y + tolerance >= other.minimum.y
            && self.minimum.z - tolerance <= other.maximum.z
            && self.maximum.z + tolerance >= other.minimum.z
    }

    pub fn diagonal(self) -> f64 {
        self.maximum.sub(self.minimum).length()
    }

    fn center_along(self, index: usize) -> f64 {
        (axis(self.minimum, index) + axis(self.maximum, index)) / 2.0
    }

    fn intersects_segment(self, start: Vec3, delta: Vec3, tolerance: f64) -> bool {
        let mut enter = 0.0f64;
        let mut exit = 1.0f64;
        for index in 0..3 {
            let origin = axis(start, index);
            let direction = axis(delta, index);
            let minimum = axis(self.minimum, index) - tolerance;
            let maximum = axis(self.maximum, index) + tolerance;
            if direction.abs() < 1e-300 {
                if origin < minimum || origin > maximum {
                    return false;
                }
                continue;
            }
            let inverse = 1.0 / direction;
            let mut near = (minimum - origin) * inverse;
            let mut far = (maximum - origin) * inverse;
            if near > far {
                std::mem::swap(&mut near, &mut far);
            }
            enter = enter.max(near);
            exit = exit.min(far);
            if enter > exit {
                return false;
            }
        }
        true
    }
}

const LEAF_SIZE: usize = 4;

struct Node {
    bounds: Aabb,
    /// Leaf: start index into `order`. Internal: index of the left child
    /// node (the right child immediately follows the left subtree).
    left: u32,
    /// Leaf: end index (exclusive) into `order`. Internal: index of the
    /// right child node.
    right: u32,
    leaf: bool,
}

/// Static BVH over per-item boxes; queries return indices into the slice
/// the tree was built from.
pub struct Bvh {
    nodes: Vec<Node>,
    order: Vec<u32>,
    boxes: Vec<Aabb>,
}

impl Bvh {
    pub fn build(boxes: &[Aabb]) -> Self {
        let mut order: Vec<u32> = (0..boxes.len() as u32).collect();
        let mut nodes = Vec::new();
        if !boxes.is_empty() {
            let count = order.len();
            build_node(boxes, &mut order, 0, count, &mut nodes);
        }
        Self {
            nodes,
            order,
            boxes: boxes.to_vec(),
        }
    }

    /// Collect indices whose box overlaps `query` expanded by `tolerance`.
    pub fn overlapping(&self, query: Aabb, tolerance: f64, out: &mut Vec<usize>) {
        self.visit(|bounds| bounds.intersects(query, tolerance), out);
    }

    /// Collect indices whose box (expanded by `tolerance`) intersects the
    /// segment from `start` to `end`.
    pub fn intersecting_segment(
        &self,
        start: Vec3,
        end: Vec3,
        tolerance: f64,
        out: &mut Vec<usize>,
    ) {
        let delta = end.sub(start);
        self.visit(
            |bounds| bounds.intersects_segment(start, delta, tolerance),
            out,
        );
    }

    /// Collect indices whose box (expanded by `tolerance`) contains `point`.
    pub fn containing_point(&self, point: Vec3, tolerance: f64, out: &mut Vec<usize>) {
        self.visit(|bounds| bounds.expanded(tolerance).contains(point), out);
    }

    fn visit(&self, hit: impl Fn(Aabb) -> bool, out: &mut Vec<usize>) {
        if self.nodes.is_empty() {
            return;
        }
        let mut stack = vec![0usize];
        while let Some(index) = stack.pop() {
            let node = &self.nodes[index];
            if !hit(node.bounds) {
                continue;
            }
            if node.leaf {
                for &item in &self.order[node.left as usize..node.right as usize] {
                    if hit(self.boxes[item as usize]) {
                        out.push(item as usize);
                    }
                }
            } else {
                stack.push(node.left as usize);
                stack.push(node.right as usize);
            }
        }
    }
}

fn build_node(
    boxes: &[Aabb],
    order: &mut [u32],
    start: usize,
    end: usize,
    nodes: &mut Vec<Node>,
) -> usize {
    let mut bounds = Aabb::empty();
    for &item in &order[start..end] {
        bounds.include(boxes[item as usize]);
    }
    let index = nodes.len();
    if end - start <= LEAF_SIZE {
        nodes.push(Node {
            bounds,
            left: start as u32,
            right: end as u32,
            leaf: true,
        });
        return index;
    }
    let extent = bounds.maximum.sub(bounds.minimum);
    let split_axis = if extent.x >= extent.y && extent.x >= extent.z {
        0
    } else if extent.y >= extent.z {
        1
    } else {
        2
    };
    let middle = start + (end - start) / 2;
    order[start..end].select_nth_unstable_by(middle - start, |&a, &b| {
        boxes[a as usize]
            .center_along(split_axis)
            .total_cmp(&boxes[b as usize].center_along(split_axis))
    });
    nodes.push(Node {
        bounds,
        left: 0,
        right: 0,
        leaf: false,
    });
    let left = build_node(boxes, order, start, middle, nodes);
    let right = build_node(boxes, order, middle, end, nodes);
    nodes[index].left = left as u32;
    nodes[index].right = right as u32;
    index
}

#[cfg(test)]
mod tests {
    use super::*;

    fn unit_box_at(x: f64, y: f64, z: f64) -> Aabb {
        Aabb {
            minimum: Vec3::new(x, y, z),
            maximum: Vec3::new(x + 1.0, y + 1.0, z + 1.0),
        }
    }

    fn brute_overlap(boxes: &[Aabb], query: Aabb, tolerance: f64) -> Vec<usize> {
        boxes
            .iter()
            .enumerate()
            .filter(|(_, bounds)| bounds.intersects(query, tolerance))
            .map(|(index, _)| index)
            .collect()
    }

    #[test]
    fn bvh_overlap_matches_brute_force() {
        let mut boxes = Vec::new();
        for i in 0..7 {
            for j in 0..5 {
                for k in 0..3 {
                    boxes.push(unit_box_at(i as f64 * 1.5, j as f64 * 1.5, k as f64 * 1.5));
                }
            }
        }
        let bvh = Bvh::build(&boxes);
        for query in [
            unit_box_at(0.0, 0.0, 0.0),
            unit_box_at(3.2, 1.4, 0.7),
            unit_box_at(100.0, 100.0, 100.0),
            Aabb {
                minimum: Vec3::new(-1.0, -1.0, -1.0),
                maximum: Vec3::new(20.0, 20.0, 20.0),
            },
        ] {
            for tolerance in [0.0, 0.25] {
                let mut found = Vec::new();
                bvh.overlapping(query, tolerance, &mut found);
                found.sort_unstable();
                assert_eq!(found, brute_overlap(&boxes, query, tolerance));
            }
        }
    }

    #[test]
    fn bvh_segment_query_matches_brute_force() {
        let mut boxes = Vec::new();
        for i in 0..20 {
            boxes.push(unit_box_at(i as f64 * 2.0, (i % 4) as f64, 0.0));
        }
        let bvh = Bvh::build(&boxes);
        let start = Vec3::new(-1.0, 0.5, 0.5);
        let end = Vec3::new(41.0, 2.5, 0.5);
        let mut found = Vec::new();
        bvh.intersecting_segment(start, end, 1e-9, &mut found);
        found.sort_unstable();
        let delta = end.sub(start);
        let expected: Vec<usize> = boxes
            .iter()
            .enumerate()
            .filter(|(_, bounds)| bounds.intersects_segment(start, delta, 1e-9))
            .map(|(index, _)| index)
            .collect();
        assert_eq!(found, expected);
        assert!(!found.is_empty());
        // A segment that stops short of the far boxes must not report them.
        let mut short = Vec::new();
        bvh.intersecting_segment(start, Vec3::new(5.0, 0.75, 0.5), 1e-9, &mut short);
        assert!(short.iter().all(|&index| index <= 3));
    }

    #[test]
    fn bvh_point_query_and_empty_tree() {
        let empty = Bvh::build(&[]);
        let mut out = Vec::new();
        empty.containing_point(Vec3::new(0.0, 0.0, 0.0), 1.0, &mut out);
        assert!(out.is_empty());

        let boxes = vec![unit_box_at(0.0, 0.0, 0.0), unit_box_at(5.0, 0.0, 0.0)];
        let bvh = Bvh::build(&boxes);
        let mut found = Vec::new();
        bvh.containing_point(Vec3::new(0.5, 0.5, 0.5), 0.0, &mut found);
        assert_eq!(found, vec![0]);
        found.clear();
        bvh.containing_point(Vec3::new(4.9, 0.5, 0.5), 0.2, &mut found);
        assert_eq!(found, vec![1]);
    }
}