phys-collision 2.0.1-beta.0

// Copyright (C) 2020-2025 phys-collision authors. All Rights Reserved.
//
// 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.

use super::triangle::Triangle;
use crate::bvh::bin_resource::*;
use crate::bvh::bvh::QuantizedBvh4;
use crate::traits::ComputeAabb3;
use crate::Aabb3;

pub struct QuantizedBvhMesh {
    bvh: QuantizedBvh4<u32>,
}

pub trait QueryTriangle {
    fn query_triangle(&self, visit: impl FnMut(u32, &Triangle));
}

impl QueryTriangle for Vec<(Triangle, u32)> {
    fn query_triangle(&self, mut visit: impl FnMut(u32, &Triangle)) {
        for (triangle, index) in self {
            visit(*index, triangle);
        }
    }
}

impl QuantizedBvhMesh {
    pub fn new<T>(triangles: &T, encode_scale: f32) -> Self
    where
        T: QueryTriangle,
    {
        let mut aabbs = vec![];
        triangles.query_triangle(
            #[inline]
            |id, triangle| {
                let aabb = triangle.compute_aabb();
                aabbs.push((aabb, id));
            },
        );

        Self {
            bvh: QuantizedBvh4::new_with_aabbs(
                &aabbs,
                encode_scale,
                &mut BinAuxiliaryResource::default(),
            ),
        }
    }

    #[inline]
    pub fn find_overlaps(&self, aabb: &Aabb3, mut visit: impl FnMut(u32)) {
        self.bvh.aabb_query(
            aabb,
            #[inline]
            |_, triangle_index| {
                visit(triangle_index);
                true
            },
        );
    }

    #[cfg(test)]
    #[inline]
    #[must_use]
    fn bvh(&self) -> &QuantizedBvh4<u32> {
        &self.bvh
    }
}

#[cfg(test)]
mod tests {
    use std::io::BufReader;

    use glam_det::{Point3, Vec3};
    use tobj::LoadError;

    use super::*;

    #[test]
    fn test_mesh_overlaps() {
        let _ = env_logger::builder().is_test(true).try_init();

        let file_input = include_bytes!("../../tests/reduced_bunny.obj");
        let mut buf = BufReader::new(file_input.as_slice());
        let (models, _) = tobj::load_obj_buf(&mut buf, &tobj::LoadOptions::default(), |_| {
            Err(LoadError::ReadError)
        })
        .expect("failed to load object");
        let mut triangles: Vec<(Triangle, u32)> = vec![];
        if let Some(model) = models.first() {
            let indices = &model.mesh.indices;
            let positions = &model.mesh.positions;

            for face in 0..indices.len() / 3 {
                let v0 = indices[face * 3] as usize;
                let v1 = indices[face * 3 + 1] as usize;
                let v2 = indices[face * 3 + 2] as usize;

                let p0 = Point3::new(
                    positions[v0 * 3],
                    positions[v0 * 3 + 1],
                    positions[v0 * 3 + 2],
                );
                let p1 = Point3::new(
                    positions[v1 * 3],
                    positions[v1 * 3 + 1],
                    positions[v1 * 3 + 2],
                );
                let p2 = Point3::new(
                    positions[v2 * 3],
                    positions[v2 * 3 + 1],
                    positions[v2 * 3 + 2],
                );
                triangles.push((Triangle::new(p0, p1, p2), face as u32));
            }

            let bvh_mesh = QuantizedBvhMesh::new(&triangles, 1000.0);
            let query_aabb_position = Point3::new(-1.639, 1.882, 0.862);

            let query_aabb_size = Vec3::new(0.1, 0.17, 0.2);
            let query_aabb = Aabb3::new(
                query_aabb_position - query_aabb_size * 0.5,
                query_aabb_position + query_aabb_size * 0.5,
            );

            let mut brute_force_results: Vec<u32> = Vec::new();
            let mut bvh_results: Vec<u32> = Vec::new();

            for (triangle, index) in &triangles {
                let aabb = triangle.compute_aabb();
                if aabb.overlap_test(&query_aabb) {
                    brute_force_results.push(*index);
                }
            }

            bvh_mesh.find_overlaps(&query_aabb, |leaf| {
                bvh_results.push(leaf);
            });

            for leaf_index in &brute_force_results {
                assert!(bvh_results.contains(leaf_index));
            }

            // Test whether the triangle and the leaf node are in one-to-one correspondence
            // If there are illegal triangles, they may be filtered
            let mut leaves: Vec<u32> = Vec::new();
            bvh_mesh.bvh().traverse_leaves(|triangle_index| {
                leaves.push(triangle_index);
            });
            leaves.sort_unstable();
            for (index, triangle_index) in leaves.iter().enumerate() {
                assert_eq!(*triangle_index, index as u32);
            }
        }
    }
}