del-msh-cpu 0.1.49

use num_traits::{AsPrimitive, PrimInt};

/// build aabb for uniform mesh
/// if 'elem2vtx' is None, bvh stores the vertex index directly
/// if 'vtx2xyz1' is Some, compute AABB for Continuous-Collision Detection (CCD)
pub fn update_for_points_with_bvh<Index, Real>(
    bvhnode2aabb: &mut [[Real; 6]],
    i_bvhnode: usize,
    bvhnodes: &[[Index; 3]],
    vtx2xyz0: &[[Real; 3]],
    vtx2xyz1: Option<&[[Real; 3]]>,
) where
    Real: num_traits::Float,
    Index: PrimInt + AsPrimitive<usize>,
{
    // aabbs.resize();
    assert_eq!(bvhnode2aabb.len(), bvhnodes.len());
    assert!(i_bvhnode < bvhnodes.len());
    assert!(if let Some(vtx2xyz1) = vtx2xyz1 {
        vtx2xyz1.len() == vtx2xyz0.len()
    } else {
        true
    });
    if bvhnodes[i_bvhnode][2] == Index::max_value() {
        // leaf node
        let i_elem: usize = bvhnodes[i_bvhnode][1].as_();
        let aabb0 = crate::vtx2xyz::to_xyz(vtx2xyz0.as_flattened(), i_elem).aabb();
        let aabb = if let Some(vtx2xyz1) = vtx2xyz1 {
            let aabb1 = crate::vtx2xyz::to_xyz(vtx2xyz1.as_flattened(), i_elem).aabb();
            del_geo_core::aabb3::from_two_aabbs(&aabb0, &aabb1)
        } else {
            aabb0
        };
        bvhnode2aabb[i_bvhnode] = aabb;
    } else {
        let i_bvhnode_child0: usize = bvhnodes[i_bvhnode][1].as_();
        let i_bvhnode_child1: usize = bvhnodes[i_bvhnode][2].as_();
        // branch node
        assert_eq!(bvhnodes[i_bvhnode_child0][0].as_(), i_bvhnode);
        assert_eq!(bvhnodes[i_bvhnode_child1][0].as_(), i_bvhnode);
        // build right tree
        update_for_points_with_bvh::<Index, Real>(
            bvhnode2aabb,
            i_bvhnode_child0,
            bvhnodes,
            vtx2xyz0,
            vtx2xyz1,
        );
        // build left tree
        update_for_points_with_bvh::<Index, Real>(
            bvhnode2aabb,
            i_bvhnode_child1,
            bvhnodes,
            vtx2xyz0,
            vtx2xyz1,
        );
        let aabb = del_geo_core::aabb3::from_two_aabbs(
            &bvhnode2aabb[i_bvhnode_child0],
            &bvhnode2aabb[i_bvhnode_child1],
        );
        bvhnode2aabb[i_bvhnode] = aabb;
    }
}

/// build aabb for uniform mesh
/// if 'elem2vtx' is None, bvh stores the vertex index directly
/// if 'vtx2xyz1' is Some, compute AABB for Continuous-Collision Detection (CCD)
pub fn update_for_uniform_mesh_with_bvh<Index, Real, const NNOEL: usize>(
    bvhnode2aabb: &mut [[Real; 6]],
    i_bvhnode: usize,
    bvhnodes: &[[Index; 3]],
    elem2vtx: &[[Index; NNOEL]],
    vtx2xyz0: &[[Real; 3]],
    vtx2xyz1: Option<&[[Real; 3]]>,
) where
    Real: num_traits::Float,
    Index: PrimInt + AsPrimitive<usize>,
{
    // aabbs.resize();
    assert_eq!(bvhnode2aabb.len(), bvhnodes.len());
    assert!(i_bvhnode < bvhnodes.len());
    assert!(if let Some(vtx2xyz1) = vtx2xyz1 {
        vtx2xyz1.len() == vtx2xyz0.len()
    } else {
        true
    });
    if bvhnodes[i_bvhnode][2] == Index::max_value() {
        // leaf node
        let i_elem: usize = bvhnodes[i_bvhnode][1].as_();
        let aabb = {
            // element index is provided
            let aabb0 =
                crate::vtx2xyz::aabb3_indexed(elem2vtx[i_elem].as_slice(), vtx2xyz0, Real::zero());
            if let Some(vtx2xyz1) = vtx2xyz1 {
                let aabb1 = crate::vtx2xyz::aabb3_indexed(
                    elem2vtx[i_elem].as_slice(),
                    vtx2xyz1,
                    Real::zero(),
                );
                del_geo_core::aabb3::from_two_aabbs(&aabb0, &aabb1)
            } else {
                aabb0
            }
        };
        bvhnode2aabb[i_bvhnode] = aabb;
    } else {
        let i_bvhnode_child0: usize = bvhnodes[i_bvhnode][1].as_();
        let i_bvhnode_child1: usize = bvhnodes[i_bvhnode][2].as_();
        // branch node
        assert_eq!(bvhnodes[i_bvhnode_child0][0].as_(), i_bvhnode);
        assert_eq!(bvhnodes[i_bvhnode_child1][0].as_(), i_bvhnode);
        // build right tree
        update_for_uniform_mesh_with_bvh::<Index, Real, NNOEL>(
            bvhnode2aabb,
            i_bvhnode_child0,
            bvhnodes,
            elem2vtx,
            vtx2xyz0,
            vtx2xyz1,
        );
        // build left tree
        update_for_uniform_mesh_with_bvh::<Index, Real, NNOEL>(
            bvhnode2aabb,
            i_bvhnode_child1,
            bvhnodes,
            elem2vtx,
            vtx2xyz0,
            vtx2xyz1,
        );
        let aabb = del_geo_core::aabb3::from_two_aabbs(
            &bvhnode2aabb[i_bvhnode_child0],
            &bvhnode2aabb[i_bvhnode_child1],
        );
        bvhnode2aabb[i_bvhnode] = aabb;
    }
}

/// Build 3D Axis-Aligned Bounding Boxes (AABBs) for all nodes in a BVH tree structure.
///
/// Generates a complete array of 3D AABBs for a BVH tree built over uniform mesh elements.
/// Each AABB encloses the 3D geometry of one BVH node. Recursively computes AABBs from
/// leaf nodes (containing elements/vertices) up to the root.
///
/// # Arguments
/// * `i_bvhnode` - Root BVH node index to start building from (typically 0)
/// * `bvhnodes` - BVH tree structure (3 indices per node: parent, child0, child1)
/// * `elem2vtx` - Optional element-to-vertex mapping (vertex_indices, num_nodes_per_element).
///   If None, BVH directly stores vertex indices (no element indirection)
/// * `vtx2xyz0` - Vertex coordinates at time t=0 (required, 3 values per vertex)
/// * `vtx2xyz1` - Optional vertex coordinates at time t=1 for Continuous-Collision Detection (CCD).
///   If provided, AABBs will enclose geometry swept through the 3D motion
///
/// # Returns
/// * `Vec<Real>` - Flattened 3D AABB array where each node i has 6 values at indices [6*i..6*i+6]
///   representing [x_min, y_min, z_min, x_max, y_max, z_max]
///
/// # Notes
/// * AABBs are computed bottom-up: leaf nodes first, then branch nodes merge child AABBs
/// * For CCD, the AABB represents the union of 3D geometry at both start and end positions
/// * All nodes are processed, even if not reachable from i_bvhnode (complete tree coverage)
/// * This is the 3D variant; see `bvhnode2aabb2.rs` for 2D AABBs
pub fn from_uniform_mesh_with_bvh<Index, Real, const NNOEL: usize>(
    i_bvhnode: usize,
    bvhnodes: &[[Index; 3]],
    elem2vtx: &[[Index; NNOEL]],
    vtx2xyz0: &[[Real; 3]],
    vtx2xyz1: Option<&[[Real; 3]]>,
) -> Vec<[Real; 6]>
where
    Real: num_traits::Float,
    Index: PrimInt + AsPrimitive<usize>,
{
    // Calculate total number of BVH nodes (each node stores 3 indices)
    let num_bvhnode = bvhnodes.len();
    // Allocate 3D AABB storage: 6 values per node (x_min, y_min, z_min, x_max, y_max, z_max)
    let mut bvhnode2aabb = vec![[Real::zero(); 6]; num_bvhnode];
    // Recursively compute AABBs for entire tree from root node
    update_for_uniform_mesh_with_bvh::<Index, Real, NNOEL>(
        &mut bvhnode2aabb,
        i_bvhnode,
        bvhnodes,
        elem2vtx,
        vtx2xyz0,
        vtx2xyz1,
    );
    bvhnode2aabb
}

pub fn update_for_polygon_polyhedron_mesh_with_bvh<Index, Real>(
    bvhnode2aabb: &mut [[Real; 6]],
    i_bvhnode: usize,
    bvhnodes: &[[Index; 3]],
    elem2idx: &[Index],
    idx2vtx: &[Index],
    vtx2xyz: &[[Real; 3]],
) where
    Real: num_traits::Float,
    Index: PrimInt + AsPrimitive<usize>,
{
    // aabbs.resize();
    assert_eq!(bvhnode2aabb.len(), bvhnodes.len());
    assert!(i_bvhnode < bvhnodes.len());
    if bvhnodes[i_bvhnode][2] == Index::max_value() {
        // leaf node
        let i_elem: usize = bvhnodes[i_bvhnode][1].as_();
        let aabb = {
            let zero = Real::zero();
            let idx0: usize = elem2idx[i_elem].as_();
            let idx1: usize = elem2idx[i_elem + 1].as_();
            let p0 = &vtx2xyz[idx2vtx[idx0].as_()];
            let mut aabb3 = [zero; 6];
            del_geo_core::aabb3::set_as_cube(&mut aabb3, p0, zero);
            for &i_vtx in &idx2vtx[idx0 + 1..idx1] {
                del_geo_core::aabb3::add_point(&mut aabb3, &vtx2xyz[i_vtx.as_()], zero);
            }
            aabb3
        };
        bvhnode2aabb[i_bvhnode] = aabb;
    } else {
        let i_bvhnode_child0: usize = bvhnodes[i_bvhnode][1].as_();
        let i_bvhnode_child1: usize = bvhnodes[i_bvhnode][2].as_();
        // branch node
        assert_eq!(bvhnodes[i_bvhnode_child0][0].as_(), i_bvhnode);
        assert_eq!(bvhnodes[i_bvhnode_child1][0].as_(), i_bvhnode);
        // build right tree
        update_for_polygon_polyhedron_mesh_with_bvh::<Index, Real>(
            bvhnode2aabb,
            i_bvhnode_child0,
            bvhnodes,
            elem2idx,
            idx2vtx,
            vtx2xyz,
        );
        // build left tree
        update_for_polygon_polyhedron_mesh_with_bvh::<Index, Real>(
            bvhnode2aabb,
            i_bvhnode_child1,
            bvhnodes,
            elem2idx,
            idx2vtx,
            vtx2xyz,
        );
        let aabb = del_geo_core::aabb3::from_two_aabbs(
            &bvhnode2aabb[i_bvhnode_child0],
            &bvhnode2aabb[i_bvhnode_child1],
        );
        bvhnode2aabb[i_bvhnode] = aabb;
    }
}

pub fn from_polygon_polyhedron_mesh_with_bvh<Index, Real>(
    i_bvhnode: usize,
    bvhnodes: &[[Index; 3]],
    elem2idx: &[Index],
    idx2vtx: &[Index],
    vtx2xyz: &[[Real; 3]],
) -> Vec<[Real; 6]>
where
    Real: num_traits::Float,
    Index: PrimInt + AsPrimitive<usize>,
{
    let num_bvhnode = bvhnodes.len();
    let mut bvhnode2aabb = vec![[Real::zero(); 6]; num_bvhnode];
    update_for_polygon_polyhedron_mesh_with_bvh(
        &mut bvhnode2aabb,
        i_bvhnode,
        bvhnodes,
        elem2idx,
        idx2vtx,
        vtx2xyz,
    );
    bvhnode2aabb
}