manifold_rust/

collider.rs

// Copyright 2026 Lars Brubaker
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//      http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

// Collider — BVH-based broadphase collision detection.
// Uses a radix tree built from Morton codes for O(n log n) queries,
// matching the C++ Manifold implementation.

use crate::impl_mesh::ManifoldImpl;
use crate::linalg::{cross, distance2, dot, Mat3x4, Vec3};
use crate::sort::{get_face_box_morton, morton_code};
use crate::types::Box as BBox;

// Node encoding (matches C++):
// - Even indices are leaf nodes: leaf i -> node 2*i
// - Odd indices are internal nodes: internal i -> node 2*i + 1
// - Root is always node index 1 (internal node 0)
const K_ROOT: i32 = 1;

fn leaf_to_node(leaf: i32) -> i32 {
    leaf * 2
}
fn node_to_leaf(node: i32) -> i32 {
    node / 2
}
fn internal_to_node(internal: i32) -> i32 {
    internal * 2 + 1
}
fn node_to_internal(node: i32) -> i32 {
    node / 2
}
fn is_leaf(node: i32) -> bool {
    node % 2 == 0
}
fn is_internal(node: i32) -> bool {
    node % 2 == 1
}

#[derive(Clone, Debug, Default)]
pub struct Collider {
    leaf_bbox: Vec<BBox>,
    leaf_morton: Vec<u32>,
    sorted_to_original: Vec<usize>, // Maps sorted leaf index → original index
    // BVH tree data (built on demand)
    node_bbox: Vec<BBox>,          // AABBs for all nodes (2*num_leaves - 1)
    internal_children: Vec<[i32; 2]>, // Child pairs for internal nodes (num_leaves - 1)
}

impl Collider {
    /// Create a new Collider from leaf bounding boxes and morton codes.
    /// Sorts leaves by morton code internally (required by radix tree algorithm).
    pub fn new(leaf_bbox: Vec<BBox>, leaf_morton: Vec<u32>) -> Self {
        debug_assert_eq!(leaf_bbox.len(), leaf_morton.len());
        let n = leaf_bbox.len();
        // Sort leaves by morton code (matching C++ SortGeometry ordering)
        let mut order: Vec<usize> = (0..n).collect();
        order.sort_by_key(|&i| leaf_morton[i]);
        let sorted_bbox: Vec<BBox> = order.iter().map(|&i| leaf_bbox[i]).collect();
        let sorted_morton: Vec<u32> = order.iter().map(|&i| leaf_morton[i]).collect();
        let sorted_to_original = order;

        let mut collider = Self {
            leaf_bbox: sorted_bbox,
            leaf_morton: sorted_morton,
            sorted_to_original,
            node_bbox: Vec::new(),
            internal_children: Vec::new(),
        };
        collider.build_bvh();
        collider
    }


    fn build_bvh(&mut self) {
        let num_leaves = self.leaf_bbox.len();
        if num_leaves == 0 {
            return;
        }
        if num_leaves == 1 {
            // Single leaf: root bbox = leaf bbox
            self.node_bbox = vec![BBox::default(); 2];
            self.node_bbox[0] = self.leaf_bbox[0]; // leaf 0 -> node 0
            self.node_bbox[1] = self.leaf_bbox[0]; // root node 1
            self.internal_children = vec![[0, 0]; 1]; // root points to leaf 0
            return;
        }

        let num_internal = num_leaves - 1;
        let num_nodes = 2 * num_leaves - 1;
        self.node_bbox = vec![BBox::default(); num_nodes];
        self.internal_children = vec![[-1, -1]; num_internal];

        // Copy leaf bboxes into node array
        for i in 0..num_leaves {
            self.node_bbox[leaf_to_node(i as i32) as usize] = self.leaf_bbox[i];
        }

        // Build radix tree structure
        self.create_radix_tree();

        // Build internal bounding boxes bottom-up
        self.build_internal_boxes();
    }

    /// Build radix tree from sorted Morton codes (matches C++ CreateRadixTree).
    fn create_radix_tree(&mut self) {
        let num_leaves = self.leaf_bbox.len();
        if num_leaves <= 1 {
            return;
        }
        let num_internal = num_leaves - 1;

        const K_INITIAL_LENGTH: i32 = 128;
        const K_LENGTH_MULTIPLE: i32 = 4;

        // Helper: count leading zeros of XOR of two morton codes
        let prefix_length = |i: i32, j: i32| -> i32 {
            if j < 0 || j >= num_leaves as i32 {
                return -1;
            }
            let mi = self.leaf_morton[i as usize];
            let mj = self.leaf_morton[j as usize];
            let xor = mi ^ mj;
            if xor == 0 {
                // Same morton code, use index as tiebreaker (matches C++ clz)
                32 + ((i as u32 ^ j as u32).leading_zeros() as i32)
            } else {
                xor.leading_zeros() as i32
            }
        };

        // RangeEnd: find the other end of the range for internal node i
        let range_end = |i: i32| -> i32 {
            let dir_val = prefix_length(i, i + 1) - prefix_length(i, i - 1);
            let dir = if dir_val > 0 { 1i32 } else if dir_val < 0 { -1i32 } else { -1i32 };
            let common_prefix = prefix_length(i, i - dir);
            let mut max_length = K_INITIAL_LENGTH;
            while prefix_length(i, i + dir * max_length) > common_prefix {
                max_length *= K_LENGTH_MULTIPLE;
            }
            let mut length = 0i32;
            let mut step = max_length / 2;
            while step > 0 {
                if prefix_length(i, i + dir * (length + step)) > common_prefix {
                    length += step;
                }
                step /= 2;
            }
            i + dir * length
        };

        // FindSplit: find where the split occurs within [first, last]
        let find_split = |first: i32, last: i32| -> i32 {
            let common_prefix = prefix_length(first, last);
            let mut split = first;
            let mut step = last - first;
            loop {
                step = (step + 1) >> 1; // divide by 2, rounding up
                let new_split = split + step;
                if new_split < last {
                    let split_prefix = prefix_length(first, new_split);
                    if split_prefix > common_prefix {
                        split = new_split;
                    }
                }
                if step <= 1 { break; }
            }
            split
        };

        // For each internal node, find its range and split point
        let mut node_parent = vec![-1i32; 2 * num_leaves - 1];

        for internal in 0..num_internal {
            let i = internal as i32;
            let mut first = i;
            let mut last = range_end(i);
            if first > last {
                std::mem::swap(&mut first, &mut last);
            }

            let split = find_split(first, last);

            // Assign children (matches C++ exactly)
            let child1 = if split == first {
                leaf_to_node(split)
            } else {
                internal_to_node(split)
            };
            // C++ increments split before computing child2
            let split2 = split + 1;
            let child2 = if split2 == last {
                leaf_to_node(split2)
            } else if (split2 as usize) < num_internal {
                internal_to_node(split2)
            } else {
                // Degenerate case: split2 exceeds internal node range.
                // This mirrors C++ UB when dir=0 in RangeEnd (clz(0) is UB).
                // Make child2 = child1 so traversal still works.
                child1
            };

            self.internal_children[internal] = [child1, child2];
            let node = internal_to_node(i);
            node_parent[child1 as usize] = node;
            node_parent[child2 as usize] = node;
        }

        // Build bboxes bottom-up using a counter-based approach
        // Process leaves and walk up to root
        let mut counter = vec![0u32; num_internal];

        for leaf in 0..num_leaves {
            let mut node = leaf_to_node(leaf as i32);
            loop {
                let parent = node_parent[node as usize];
                if parent < 0 {
                    break; // at root
                }
                let internal = node_to_internal(parent);
                if internal < 0 || internal >= num_internal as i32 {
                    break;
                }
                let idx = internal as usize;
                counter[idx] += 1;
                if counter[idx] < 2 {
                    break; // wait for second child
                }
                // Both children ready, compute union
                let [c1, c2] = self.internal_children[idx];
                let b1 = self.node_bbox[c1 as usize];
                let b2 = self.node_bbox[c2 as usize];
                self.node_bbox[parent as usize] = b1.union_box(&b2);
                node = parent;
            }
        }
    }

    /// Build internal bounding boxes (already done in create_radix_tree for sequential).
    fn build_internal_boxes(&mut self) {
        // Already built in create_radix_tree above.
        // The C++ separates these for GPU parallelism; we combine them.
    }

    /// Run a single query box against the BVH, invoking `record(query_idx,
    /// leaf_idx)` per overlap. Same traversal as `collisions_fn` for one
    /// index — the per-query entry point for parallel callers (`&self` only,
    /// so queries can run concurrently with thread-local recording).
    pub fn collisions_one<R: FnMut(usize, usize)>(
        &self,
        query: &BBox,
        query_idx: usize,
        mut record: R,
    ) {
        if query.is_empty() {
            return;
        }
        if self.internal_children.is_empty() {
            if self.leaf_bbox.len() == 1 {
                let original_idx = self.sorted_to_original[0];
                if query.does_overlap_box(&self.leaf_bbox[0]) {
                    record(query_idx, original_idx);
                }
            }
            return;
        }
        self.traverse_bvh(query, query_idx, false, &mut record);
    }

    /// BVH-accelerated collision query with function-generated query boxes.
    /// For each query index 0..n, calls query_box_fn(i) to get the query AABB,
    /// then traverses the BVH to find overlapping leaves.
    pub fn collisions_fn<F, R>(
        &self,
        query_box_fn: F,
        n: usize,
        mut record: R,
    ) where
        F: Fn(usize) -> BBox,
        R: FnMut(usize, usize),
    {
        if self.internal_children.is_empty() {
            // Fallback for 0-1 leaves
            if self.leaf_bbox.len() == 1 {
                let original_idx = self.sorted_to_original[0];
                for query_idx in 0..n {
                    let query = query_box_fn(query_idx);
                    if !query.is_empty() && query.does_overlap_box(&self.leaf_bbox[0]) {
                        record(query_idx, original_idx);
                    }
                }
            }
            return;
        }

        for query_idx in 0..n {
            let query = query_box_fn(query_idx);
            if query.is_empty() {
                continue;
            }
            self.traverse_bvh(&query, query_idx, false, &mut record);
        }
    }

    /// BVH-accelerated collision query with pre-computed query boxes.
    pub fn collisions_with_boxes<F: FnMut(usize, usize)>(
        &self,
        queries: &[BBox],
        self_collision: bool,
        mut record: F,
    ) {
        if self.internal_children.is_empty() {
            // Fallback for 0-1 leaves
            if self.leaf_bbox.len() == 1 {
                let original_idx = self.sorted_to_original[0];
                for (qi, q) in queries.iter().enumerate() {
                    if !(self_collision && qi == original_idx) && q.does_overlap_box(&self.leaf_bbox[0]) {
                        record(qi, original_idx);
                    }
                }
            }
            return;
        }

        for (query_idx, query) in queries.iter().enumerate() {
            if query.is_empty() {
                continue;
            }
            self.traverse_bvh(query, query_idx, self_collision, &mut record);
        }
    }

    /// Point-based collision query using BVH.
    pub fn collisions_point<F, R>(
        &self,
        point_fn: F,
        n: usize,
        mut record: R,
    ) where
        F: Fn(usize) -> Vec3,
        R: FnMut(usize, usize),
    {
        if self.internal_children.is_empty() {
            if self.leaf_bbox.len() == 1 {
                let original_idx = self.sorted_to_original[0];
                for query_idx in 0..n {
                    let pt = point_fn(query_idx);
                    let query = BBox::from_point(pt);
                    if query.does_overlap_box(&self.leaf_bbox[0]) {
                        record(query_idx, original_idx);
                    }
                }
            }
            return;
        }

        for query_idx in 0..n {
            let pt = point_fn(query_idx);
            let query = BBox::from_point(pt);
            self.traverse_bvh(&query, query_idx, false, &mut record);
        }
    }

    /// Stack-based depth-first BVH traversal (matches C++ FindCollision).
    fn traverse_bvh<F: FnMut(usize, usize)>(
        &self,
        query: &BBox,
        query_idx: usize,
        self_collision: bool,
        record: &mut F,
    ) {
        let mut stack = [0i32; 64];
        let mut top: i32 = -1;
        let mut node = K_ROOT;

        loop {
            let internal = node_to_internal(node);
            if internal < 0 || internal as usize >= self.internal_children.len() {
                if top < 0 { break; }
                node = stack[top as usize];
                top -= 1;
                continue;
            }
            let [child1, child2] = self.internal_children[internal as usize];

            let traverse1 = self.check_node(query, child1, query_idx, self_collision, record);
            let traverse2 = self.check_node(query, child2, query_idx, self_collision, record);

            if !traverse1 && !traverse2 {
                if top < 0 {
                    break;
                }
                node = stack[top as usize];
                top -= 1;
            } else {
                node = if traverse1 { child1 } else { child2 };
                if traverse1 && traverse2 {
                    top += 1;
                    debug_assert!((top as usize) < 64, "BVH stack overflow");
                    stack[top as usize] = child2;
                }
            }
        }
    }

    /// Check if a node's AABB overlaps the query. If it's a leaf, record the hit.
    /// Returns true if the node is internal and overlaps (should traverse deeper).
    #[inline]
    fn check_node<F: FnMut(usize, usize)>(
        &self,
        query: &BBox,
        node: i32,
        query_idx: usize,
        self_collision: bool,
        record: &mut F,
    ) -> bool {
        if node < 0 || node as usize >= self.node_bbox.len() {
            return false;
        }
        let node_box = &self.node_bbox[node as usize];
        let overlaps = query.does_overlap_box(node_box);
        if overlaps && is_leaf(node) {
            let sorted_idx = node_to_leaf(node) as usize;
            let original_idx = self.sorted_to_original[sorted_idx];
            if !self_collision || original_idx != query_idx {
                record(query_idx, original_idx);
            }
        }
        overlaps && is_internal(node)
    }

    pub fn update_boxes(&mut self, leaf_bbox: Vec<BBox>) {
        debug_assert_eq!(leaf_bbox.len(), self.leaf_bbox.len());
        // Reorder to sorted order (sorted_to_original maps sorted→original)
        // We need original→sorted, which is the inverse
        for (sorted_idx, &orig_idx) in self.sorted_to_original.iter().enumerate() {
            self.leaf_bbox[sorted_idx] = leaf_bbox[orig_idx];
        }
        // Rebuild BVH with new boxes (morton codes and sort order unchanged)
        self.build_bvh();
    }

    pub fn transform(&mut self, transform: &Mat3x4) {
        debug_assert!(Self::is_axis_aligned(transform));
        for bbox in &mut self.leaf_bbox {
            *bbox = bbox.transform(transform);
        }
        // Rebuild BVH after transform
        self.build_bvh();
    }

    pub fn leaf_count(&self) -> usize {
        self.leaf_bbox.len()
    }

    pub fn leaf_bbox(&self) -> &[BBox] {
        &self.leaf_bbox
    }

    pub fn morton_code(position: Vec3, bbox: &BBox) -> u32 {
        morton_code(position, bbox)
    }

    pub fn is_axis_aligned(transform: &Mat3x4) -> bool {
        for row in 0..3 {
            let mut zero_count = 0;
            for col in 0..3 {
                if transform[col][row] == 0.0 {
                    zero_count += 1;
                }
            }
            if zero_count != 2 {
                return false;
            }
        }
        true
    }

    pub fn leaf_morton(&self) -> &[u32] {
        &self.leaf_morton
    }
}

pub fn edge_edge_dist(p: Vec3, a: Vec3, q: Vec3, b: Vec3) -> (Vec3, Vec3) {
    let t_vec = q - p;
    let a_dot_a = dot(a, a);
    let b_dot_b = dot(b, b);
    let a_dot_b = dot(a, b);
    let a_dot_t = dot(a, t_vec);
    let b_dot_t = dot(b, t_vec);

    let denom = a_dot_a * b_dot_b - a_dot_b * a_dot_b;
    let mut t = if denom != 0.0 {
        ((a_dot_t * b_dot_b - b_dot_t * a_dot_b) / denom).clamp(0.0, 1.0)
    } else {
        0.0
    };

    let u = if b_dot_b != 0.0 {
        let u = (t * a_dot_b - b_dot_t) / b_dot_b;
        if u < 0.0 {
            t = if a_dot_a != 0.0 { (a_dot_t / a_dot_a).clamp(0.0, 1.0) } else { 0.0 };
            0.0
        } else if u > 1.0 {
            t = if a_dot_a != 0.0 {
                ((a_dot_b + a_dot_t) / a_dot_a).clamp(0.0, 1.0)
            } else {
                0.0
            };
            1.0
        } else {
            u
        }
    } else {
        t = if a_dot_a != 0.0 { (a_dot_t / a_dot_a).clamp(0.0, 1.0) } else { 0.0 };
        0.0
    };

    (p + a * t, q + b * u)
}

pub fn distance_triangle_triangle_squared(p: [Vec3; 3], q: [Vec3; 3]) -> f64 {
    let sv = [p[1] - p[0], p[2] - p[1], p[0] - p[2]];
    let tv = [q[1] - q[0], q[2] - q[1], q[0] - q[2]];

    let mut shown_disjoint = false;
    let mut mindd = f64::MAX;

    for i in 0..3 {
        for j in 0..3 {
            let (cp, cq) = edge_edge_dist(p[i], sv[i], q[j], tv[j]);
            let v = cq - cp;
            let dd = dot(v, v);

            if dd <= mindd {
                mindd = dd;

                let mut id = i + 2;
                if id >= 3 { id -= 3; }
                let z = p[id] - cp;
                let mut a = dot(z, v);

                id = j + 2;
                if id >= 3 { id -= 3; }
                let z = q[id] - cq;
                let mut b = dot(z, v);

                if a <= 0.0 && b >= 0.0 {
                    return dot(v, v);
                }

                if a <= 0.0 { a = 0.0; } else if b > 0.0 { b = 0.0; }

                if mindd - a + b > 0.0 {
                    shown_disjoint = true;
                }
            }
        }
    }

    let sn = cross(sv[0], sv[1]);
    let snl = dot(sn, sn);
    if snl > 1e-15 {
        let tp = Vec3::new(dot(p[0] - q[0], sn), dot(p[0] - q[1], sn), dot(p[0] - q[2], sn));
        let mut index = None;
        if tp.x > 0.0 && tp.y > 0.0 && tp.z > 0.0 {
            let mut idx = if tp.x < tp.y { 0 } else { 1 };
            if tp.z < tp[idx] { idx = 2; }
            index = Some(idx);
        } else if tp.x < 0.0 && tp.y < 0.0 && tp.z < 0.0 {
            let mut idx = if tp.x > tp.y { 0 } else { 1 };
            if tp.z > tp[idx] { idx = 2; }
            index = Some(idx);
        }

        if let Some(index) = index {
            shown_disjoint = true;
            let q_index = q[index];
            let v = q_index - p[0];
            let z = cross(sn, sv[0]);
            if dot(v, z) > 0.0 {
                let v = q_index - p[1];
                let z = cross(sn, sv[1]);
                if dot(v, z) > 0.0 {
                    let v = q_index - p[2];
                    let z = cross(sn, sv[2]);
                    if dot(v, z) > 0.0 {
                        let cp = q_index + sn * (tp[index] / snl);
                        let cq = q_index;
                        return dot(cp - cq, cp - cq);
                    }
                }
            }
        }
    }

    let tn = cross(tv[0], tv[1]);
    let tnl = dot(tn, tn);
    if tnl > 1e-15 {
        let sp = Vec3::new(dot(q[0] - p[0], tn), dot(q[0] - p[1], tn), dot(q[0] - p[2], tn));
        let mut index = None;
        if sp.x > 0.0 && sp.y > 0.0 && sp.z > 0.0 {
            let mut idx = if sp.x < sp.y { 0 } else { 1 };
            if sp.z < sp[idx] { idx = 2; }
            index = Some(idx);
        } else if sp.x < 0.0 && sp.y < 0.0 && sp.z < 0.0 {
            let mut idx = if sp.x > sp.y { 0 } else { 1 };
            if sp.z > sp[idx] { idx = 2; }
            index = Some(idx);
        }

        if let Some(index) = index {
            shown_disjoint = true;
            let p_index = p[index];
            let v = p_index - q[0];
            let z = cross(tn, tv[0]);
            if dot(v, z) > 0.0 {
                let v = p_index - q[1];
                let z = cross(tn, tv[1]);
                if dot(v, z) > 0.0 {
                    let v = p_index - q[2];
                    let z = cross(tn, tv[2]);
                    if dot(v, z) > 0.0 {
                        let cp = p_index;
                        let cq = p_index + tn * (sp[index] / tnl);
                        return dot(cp - cq, cp - cq);
                    }
                }
            }
        }
    }

    if shown_disjoint { mindd } else { 0.0 }
}

pub fn ray_triangle_intersection(
    origin: Vec3,
    direction: Vec3,
    tri: [Vec3; 3],
) -> Option<f64> {
    let eps = 1e-9;
    let edge1 = tri[1] - tri[0];
    let edge2 = tri[2] - tri[0];
    let h = cross(direction, edge2);
    let a = dot(edge1, h);
    if a.abs() < eps {
        return None;
    }
    let f = 1.0 / a;
    let s = origin - tri[0];
    let u = f * dot(s, h);
    if !(0.0..=1.0).contains(&u) {
        return None;
    }
    let q = cross(s, edge1);
    let v = f * dot(direction, q);
    if v < 0.0 || u + v > 1.0 {
        return None;
    }
    let t = f * dot(edge2, q);
    if t > eps { Some(t) } else { None }
}

impl ManifoldImpl {
    pub fn is_self_intersecting(&self) -> bool {
        let ep = 2.0 * self.epsilon;
        let epsilon_sq = ep * ep;
        let (face_box, face_morton) = get_face_box_morton(self);
        let collider = Collider::new(face_box.clone(), face_morton);
        let mut intersecting = false;

        collider.collisions_with_boxes(&face_box, true, |tri0, tri1| {
            if intersecting {
                return;
            }
            let tri_verts0 = self.face_triangle_vertices(tri0);
            let tri_verts1 = self.face_triangle_vertices(tri1);

            for a in &tri_verts0 {
                for b in &tri_verts1 {
                    if distance2(*a, *b) <= epsilon_sq {
                        return;
                    }
                }
            }

            if distance_triangle_triangle_squared(tri_verts0, tri_verts1) == 0.0 {
                let mut tmp0 = tri_verts0;
                let mut tmp1 = tri_verts1;
                for i in 0..3 {
                    tmp0[i] = tri_verts0[i] + self.face_normal[tri1] * ep;
                }
                if distance_triangle_triangle_squared(tmp0, tri_verts1) > 0.0 {
                    return;
                }
                for i in 0..3 {
                    tmp0[i] = tri_verts0[i] - self.face_normal[tri1] * ep;
                }
                if distance_triangle_triangle_squared(tmp0, tri_verts1) > 0.0 {
                    return;
                }
                for i in 0..3 {
                    tmp1[i] = tri_verts1[i] + self.face_normal[tri0] * ep;
                }
                if distance_triangle_triangle_squared(tri_verts0, tmp1) > 0.0 {
                    return;
                }
                for i in 0..3 {
                    tmp1[i] = tri_verts1[i] - self.face_normal[tri0] * ep;
                }
                if distance_triangle_triangle_squared(tri_verts0, tmp1) > 0.0 {
                    return;
                }
                intersecting = true;
            }
        });

        intersecting
    }

    pub fn min_gap(&self, other: &ManifoldImpl, search_length: f64) -> f64 {
        let (self_box, self_morton) = get_face_box_morton(self);
        let (mut other_box, _) = get_face_box_morton(other);
        for bbox in &mut other_box {
            bbox.min = bbox.min - Vec3::splat(search_length);
            bbox.max = bbox.max + Vec3::splat(search_length);
        }

        let collider = Collider::new(self_box, self_morton);
        let mut min_distance = f64::INFINITY;
        collider.collisions_with_boxes(&other_box, false, |tri_other, tri| {
            let p = self.face_triangle_vertices(tri);
            let q = other.face_triangle_vertices(tri_other);
            min_distance = min_distance.min(distance_triangle_triangle_squared(p, q));
        });

        min_distance.min(search_length * search_length).sqrt()
    }

    fn face_triangle_vertices(&self, tri: usize) -> [Vec3; 3] {
        [
            self.vert_pos[self.halfedge[3 * tri].start_vert as usize],
            self.vert_pos[self.halfedge[3 * tri + 1].start_vert as usize],
            self.vert_pos[self.halfedge[3 * tri + 2].start_vert as usize],
        ]
    }
}

#[cfg(test)]
mod tests {
    use super::*;
    use crate::linalg::{mat4_to_mat3x4, translation_matrix};

    #[test]
    fn test_collider_box_overlap() {
        let boxes = vec![
            BBox::from_points(Vec3::new(0.0, 0.0, 0.0), Vec3::new(1.0, 1.0, 1.0)),
            BBox::from_points(Vec3::new(2.0, 2.0, 2.0), Vec3::new(3.0, 3.0, 3.0)),
        ];
        let collider = Collider::new(boxes.clone(), vec![0, 1]);
        let mut hits = Vec::new();
        collider.collisions_with_boxes(&boxes, true, |a, b| hits.push((a, b)));
        assert!(hits.is_empty());

        let queries = vec![BBox::from_points(Vec3::new(0.5, 0.5, 0.5), Vec3::new(2.5, 2.5, 2.5))];
        collider.collisions_with_boxes(&queries, false, |a, b| hits.push((a, b)));
        assert_eq!(hits, vec![(0, 0), (0, 1)]);
    }

    #[test]
    fn test_ray_triangle_intersection() {
        let tri = [
            Vec3::new(0.0, 0.0, 0.0),
            Vec3::new(1.0, 0.0, 0.0),
            Vec3::new(0.0, 1.0, 0.0),
        ];
        let hit = ray_triangle_intersection(Vec3::new(0.25, 0.25, -1.0), Vec3::new(0.0, 0.0, 1.0), tri);
        assert!(hit.is_some());
    }

    #[test]
    fn test_triangle_triangle_distance_zero_for_intersection() {
        let a = [
            Vec3::new(0.0, 0.0, 0.0),
            Vec3::new(1.0, 0.0, 0.0),
            Vec3::new(0.0, 1.0, 0.0),
        ];
        let b = [
            Vec3::new(0.25, 0.25, -1.0),
            Vec3::new(0.25, 0.25, 1.0),
            Vec3::new(0.75, 0.25, 0.0),
        ];
        assert_eq!(distance_triangle_triangle_squared(a, b), 0.0);
    }

    #[test]
    fn test_cube_not_self_intersecting() {
        let m = ManifoldImpl::cube(&Mat3x4::identity());
        assert!(!m.is_self_intersecting());
    }

    #[test]
    fn test_min_gap_between_cubes() {
        let a = ManifoldImpl::cube(&Mat3x4::identity());
        let b = ManifoldImpl::cube(&mat4_to_mat3x4(translation_matrix(Vec3::new(2.0, 0.0, 0.0))));
        let gap = a.min_gap(&b, 5.0);
        assert!((gap - 1.0).abs() < 1e-8, "gap = {}", gap);
    }

    #[test]
    fn test_bvh_many_boxes() {
        // Create many non-overlapping boxes and verify BVH finds the right pairs
        let n = 100;
        let mut boxes = Vec::new();
        let mut mortons = Vec::new();
        for i in 0..n {
            let x = i as f64 * 3.0;
            boxes.push(BBox::from_points(
                Vec3::new(x, 0.0, 0.0),
                Vec3::new(x + 1.0, 1.0, 1.0),
            ));
            mortons.push(i as u32);
        }
        let collider = Collider::new(boxes.clone(), mortons);

        // Query that overlaps box 50
        let query = vec![BBox::from_points(
            Vec3::new(150.5, 0.5, 0.5),
            Vec3::new(150.6, 0.6, 0.6),
        )];
        let mut hits = Vec::new();
        collider.collisions_with_boxes(&query, false, |a, b| hits.push((a, b)));
        assert_eq!(hits, vec![(0, 50)]);
    }
}