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.

#[cfg(not(feature = "qhull"))]
use chull_adapt::Config;
use chull_adapt::{ConvexHullConstructError, ConvexHullConstructState};
use glam_det::nums::num_traits::{Bool, Num, NumConstEx, PartialOrdEx, Signed};
use glam_det::nums::{bool32x4, f32x4, i32x4, u32x4};
use glam_det::{
    Cross, Dot, DotClamp, Isometry3, Point3, Point3x4, UnitVec3, UnitVec3x4, Vec3, Vec3x4, Vec4,
};
use glamdet_na_conv::ConvTo;
use phys_geom::math::Point3Ext;

use super::traits::{ComplexShapeTrait, ShapePlugin};
use super::ComputeVolume;
use crate::ray::{Raycast, RaycastHitResult};
use crate::shapes::container::ComplexShapeId as ConvexHullId;
use crate::traits::{
    ArrayGetter, BaseShapeWide, CreateShapeWide, Expansion, MinkowskiSupport,
    MinkowskiSupportResult, MinkowskiSupportResultWide, MinkowskiSupportWide,
    SignedDistanceToPoint,
};
use crate::{Aabb3, ComputeAabb3, ContainsPoint, ContainsResult, Ray, ShapeContainer};

/// max number of vertices on one face of convex hull
pub const GENERAL_FACE_VERTICES_RESTRICTIONS: usize = 32;
const MIN_DERTA: f32 = 1.5e-6;
/// the points must be pre quick convex
/// `face_to_point_indices_start` is the index of `face_vertex_indices`
/// for example, if `face_to_point_indices_start[0] = 0,face_to_point_indices_start[1] = 3`,
/// it is means the first face has 3 vertices,and the index of the `face_vertex_indices` is 0,1,2
#[derive(Clone, Debug)]
pub struct ConvexHull {
    points: Vec<Point3x4>,
    bounding_planes: Vec<HullPlaneWide>,
    face_vertex_indices: Vec<BundleIndex>,
    face_indices_start: Vec<u32>,
    center: Vec3,
    /// The offset vector applied to all points if align-to-center is enabled.
    ///
    /// - If `None`, align-to-center is disabled, and points remain at their original positions.
    /// - If `Some(Vec3)`, align-to-center is enabled, and points are shifted by the given vector,
    ///   meaning the original positions are at `pos + shift_center`.
    shift_center: Option<Vec3>,
    volume: f32,
    total_area: f32,
    #[cfg(feature = "contact_debug")]
    debug: ConvexHullDebugInfo,
}
#[cfg(feature = "contact_debug")]
#[derive(Default, Debug, Clone)]
pub struct ConvexHullDebugInfo {
    neighbours_index: Vec<u32>,
    /// every face is represented in `face_indices_start`,and it's neighbours represented in
    /// `neighbours_index_start` and `neighbours_index`
    neighbours_index_start: Vec<u32>,
}
impl ConvexHull {
    pub const PLUGIN_ID: u64 = 0x621fad2a;
}
impl ConvexHull {
    /// Creates a convex hull from the given points.
    /// These points do not need to be pre-convexed, as the system will automatically convex them
    /// internally. However, redundant vertices will still be stored in memory
    ///
    /// # Parameters
    /// - `points`: The input points to build the convex hull from.
    /// - `align_to_center`: Whether to shift points so that the AABB center aligns with the convex
    ///   hull's local origin. This reduces the magnitude of vertex coordinates relative to the
    ///   origin, improving numerical precision.
    ///
    /// # Note
    /// If construction fails, the value of `ConvexHullConstructState` will not be Success. However,
    /// it will still attempt to construct a convex hull using the mesh's AABB. If the state is
    /// Degenerate, construction will be abandoned, and the value will then be set to Success.
    ///
    /// `align_to_center` will be unused when `qhull` feature is open
    ///
    /// # Errors
    /// Returns `ConvexHullConstructError` if the convex hull cannot be constructed.
    #[inline]
    #[allow(clippy::new_ret_no_self)]
    pub fn new(
        points: &[Point3],
        #[allow(unused)] align_to_center: bool,
    ) -> Result<(ConvexHull, ConvexHullConstructState), ConvexHullConstructError> {
        quick_convex_hull(points, align_to_center, false)
    }

    pub(crate) fn empty() -> Self {
        Self {
            points: vec![],
            bounding_planes: vec![],
            face_vertex_indices: vec![],
            face_indices_start: vec![],
            center: Vec3::default(),
            volume: 1.0,
            total_area: 1.0,
            shift_center: None,
            #[cfg(feature = "contact_debug")]
            debug: ConvexHullDebugInfo::default(),
        }
    }

    #[inline]
    #[cfg(test)]
    pub(crate) fn new_unchecked(points: &[Point3]) -> Self {
        let result = quick_convex_hull(points, false, true);
        debug_assert!(result.is_ok(), "error occurs in convex hull creation");
        result.map_or(ConvexHull::empty(), |(hull, _)| hull)
    }
}

#[cfg(all(feature = "contact_debug", feature = "qhull"))]
impl ConvexHull {
    #[must_use]
    pub fn get_neighbours(&self, face_index: usize) -> &[u32] {
        let start = self.debug.neighbours_index_start[face_index] as usize;
        let next_index = face_index + 1;
        let end = if next_index < self.debug.neighbours_index_start.len() {
            self.debug.neighbours_index_start[next_index] as usize
        } else {
            self.face_vertex_indices.len()
        };
        &self.debug.neighbours_index[start..end]
    }
}
#[inline]
fn shift_point_back(point: Point3, shift_center: Vec3) -> Point3 {
    point + shift_center
}

fn quick_convex_hull(
    points: &[Point3],
    #[allow(unused)] align_to_center: bool,
    #[allow(unused)] uncheck: bool,
) -> Result<(ConvexHull, ConvexHullConstructState), ConvexHullConstructError> {
    #[cfg(feature = "qhull")]
    let (convex_hull, state) = chull_adapt::ConvexHull::new(points)?;
    #[cfg(not(feature = "qhull"))]
    let (convex_hull, state) = {
        let mut config = if uncheck {
            Config::new(25500, 65536, 1e-6, true, false)
        } else {
            Config::default()
        };
        config.set_shift_point_align_aabb_center(align_to_center);
        chull_adapt::ConvexHull::new(points, &config)?
    };

    let last_index = convex_hull.points.len() - 1;
    let point_bundle_count = get_bundle_count(convex_hull.points.len());
    let face_bundle_count = get_bundle_count(convex_hull.bounding_planes.len());
    let mut hull = ConvexHull::empty();
    for bundle_index in 0..point_bundle_count {
        let mut points_bundle: [Point3; 4] = [Point3::ZERO; 4];
        for (i, point_bundle) in points_bundle.iter_mut().enumerate() {
            let mut original_index = bundle_index * 4 + i;
            if original_index > last_index {
                // issue (#1332)
                original_index = last_index;
            }
            *point_bundle = convex_hull.points[original_index];
        }
        let point_bundle = Point3x4::compose_soa(&points_bundle);
        hull.points.push(point_bundle);
    }

    let mut iter = convex_hull.bounding_planes.iter();
    for _ in 0..face_bundle_count {
        let mut normal_bundle = [Vec3::ZERO; 4];
        let mut offset_bundle = [0.0f32; 4];
        let mut center_bundle = [Point3::ZERO; 4];
        for ((normal, offset), center) in normal_bundle
            .iter_mut()
            .zip(offset_bundle.iter_mut())
            .zip(center_bundle.iter_mut())
        {
            if let Some(plane) = iter.next() {
                *normal = plane.normal.as_vec3();
                *offset = plane.offset;
                *center = plane.center;
            } else {
                *normal = Vec3::default();
                *offset = f32::MIN;
                *center = Point3::default();
            }
        }
        hull.bounding_planes.push(HullPlaneWide {
            center: Point3x4::compose_soa(&center_bundle),
            normal: Vec3x4::compose_soa(&normal_bundle),
            offset: f32x4::from(offset_bundle),
        });
    }
    hull.face_indices_start
        .extend_from_slice(&convex_hull.face_indices_start);
    for &index in &convex_hull.face_vertex_indices {
        let b = BundleIndex::new(index);
        hull.face_vertex_indices.push(b);
    }
    hull.volume = convex_hull.volume;
    hull.total_area = convex_hull.total_area;
    hull.center = convex_hull.center;
    hull.shift_center = convex_hull.shift_center;
    #[cfg(feature = "contact_debug")]
    if let Some(debug) = convex_hull.debug {
        hull.debug.neighbours_index = debug.neighbours_index;
        hull.debug.neighbours_index_start = debug.neighbours_index_start;
    }
    Ok((hull, state))
}
#[derive(Default, Debug, Clone)]
struct HullPlaneWide {
    center: Point3x4,
    normal: Vec3x4,
    // TODO: check if should use UnitVec3x4 (issue #1365)
    offset: f32x4,
}

impl HullPlaneWide {
    /// distance from point to plane
    #[inline]
    pub fn signed_distance_to_point(&self, point: &Point3x4) -> f32x4 {
        let normal = self.normal.as_unit_vec3x4_unchecked();
        let plane_origin = normal * self.offset;
        let origin_to_point = point.as_vec3x4() - plane_origin;
        origin_to_point.dot(normal)
    }
}

#[derive(Clone, Copy, Eq, PartialEq, Debug)]
pub struct BundleIndex {
    bundle_index: u16,
    inner_index: u16,
}

impl BundleIndex {
    #[inline]
    pub fn new(index: u32) -> Self {
        Self {
            bundle_index: (index >> 2) as u16,
            inner_index: (index & 3) as u16,
        }
    }

    #[inline]
    pub fn index(self) -> u32 {
        u32::from(self.bundle_index) << 2 | u32::from(self.inner_index)
    }

    #[inline]
    pub fn bundle_index(self) -> usize {
        self.bundle_index as usize
    }

    #[inline]
    pub fn inner_index(self) -> usize {
        self.inner_index as usize
    }
}

#[derive(Debug)]
pub struct PickedFace {
    pub face_index: usize,
    pub normal: UnitVec3,
}

/// An iterator over each vertex in the convex hull.
/// Notice,the vertex has almost 3 extra data for wide structure
/// Each vertex of the triangle is in "shifted space".
/// Use the `shift_back` function to convert vertices to local space if needed.
pub struct ConvexHullPointsIter<'a> {
    hull: &'a ConvexHull,
    bundle_index: BundleIndex,
}
impl<'a> ConvexHullPointsIter<'a> {
    #[inline]
    #[must_use]
    pub fn new(hull: &'a ConvexHull) -> Self {
        Self {
            hull,
            bundle_index: BundleIndex::new(0),
        }
    }
}
impl Iterator for ConvexHullPointsIter<'_> {
    type Item = Point3;

    #[inline]
    fn next(&mut self) -> Option<Self::Item> {
        if self.bundle_index.bundle_index as usize >= self.hull.points.len() {
            None
        } else {
            let result = self.hull.get_point(self.bundle_index);
            self.bundle_index.inner_index += 1;
            let lane_count = i32x4::lanes();
            if self.bundle_index.inner_index as usize >= lane_count {
                self.bundle_index.inner_index = 0;
                self.bundle_index.bundle_index += 1;
            }
            Some(result)
        }
    }
}

/// An iterator over each triangle index in the convex hull.
pub struct ConvexHullTriangleIndexIter<'a> {
    hull: &'a ConvexHull,
    face_index: usize,
    sub_triangle_index: usize,
}
impl<'a> ConvexHullTriangleIndexIter<'a> {
    #[inline]
    #[must_use]
    pub fn new(hull: &'a ConvexHull) -> Self {
        Self {
            hull,
            face_index: 0,
            sub_triangle_index: 2,
        }
    }
}

impl Iterator for ConvexHullTriangleIndexIter<'_> {
    type Item = [usize; 3];

    #[inline]
    fn next(&mut self) -> Option<Self::Item> {
        if self.face_index < self.hull.face_indices_start.len() {
            let faces = self.hull.get_vertex_indices(self.face_index);
            let mut triangle = [usize::MAX; 3];
            triangle[0] = faces[0].index() as usize;
            triangle[1] = faces[self.sub_triangle_index].index() as usize;
            triangle[2] = faces[self.sub_triangle_index - 1].index() as usize;
            self.sub_triangle_index += 1;
            if self.sub_triangle_index == faces.len() {
                self.face_index += 1;
                self.sub_triangle_index = 2;
            }
            Some(triangle)
        } else {
            None
        }
    }
}
/// An iterator over each triangle in the convex hull.
/// Each vertex of the triangle is in "shifted space".
/// Use the `shift_back` function to convert vertices to local space if needed.
pub struct ConvexHullTriangleIter<'a> {
    hull: &'a ConvexHull,
    face_index: usize,
    sub_triangle_index: usize,
}

impl Iterator for ConvexHullTriangleIter<'_> {
    type Item = [Point3; 3];

    fn next(&mut self) -> Option<Self::Item> {
        if self.face_index < self.hull.face_indices_start.len() {
            let faces = self.hull.get_vertex_indices(self.face_index);
            let mut triangle = [Point3::ZERO; 3];
            triangle[0] = self.hull.get_point(faces[0]);
            triangle[1] = self.hull.get_point(faces[self.sub_triangle_index]);
            triangle[2] = self.hull.get_point(faces[self.sub_triangle_index - 1]);
            self.sub_triangle_index += 1;
            if self.sub_triangle_index == faces.len() {
                self.face_index += 1;
                self.sub_triangle_index = 2;
            }
            Some(triangle)
        } else {
            None
        }
    }
}

impl<'a> ConvexHullTriangleIter<'a> {
    #[inline]
    #[must_use]
    pub fn new(hull: &'a ConvexHull) -> Self {
        Self {
            hull,
            face_index: 0,
            sub_triangle_index: 2,
        }
    }
}

#[derive(Clone, Copy)]
enum PickFacePolicy {
    Depth,
    #[allow(dead_code)]
    Normal,
}

impl ConvexHull {
    #[inline]
    #[must_use]
    #[allow(dead_code)]
    fn get_bounding_plane(&self, index: BundleIndex) -> [f32; 4] {
        let bounding_planes = &self.bounding_planes[index.bundle_index()];
        let normal = bounding_planes.normal.extract_lane(index.inner_index());
        let offset = bounding_planes.offset.extract(index.inner_index());
        [normal.x, normal.y, normal.z, offset]
    }

    pub(crate) fn support_point_local_narrow(&self, dir: Vec3) -> MinkowskiSupportResult {
        let dir = Vec3x4::new(
            f32x4::splat(dir.x),
            f32x4::splat(dir.y),
            f32x4::splat(dir.z),
        );
        let first_point = self.points[0];
        let mut max_dot = dir.dot(first_point.as_vec3x4());
        let mut max_point = first_point;
        let base_indices = u32x4::from([0, 1, 2, 3]);
        let mut best_indices = base_indices;
        for i in 1..self.points.len() {
            let point = self.points[i];
            let dot = dir.dot(point.as_vec3x4());
            let greater = dot.gt(max_dot);
            max_dot = dot.select(greater, max_dot);
            max_point = Point3x4::lane_select(greater, point, max_point);
            best_indices =
                best_indices.select(greater, base_indices + u32x4::splat((i << 2) as u32));
        }
        let mut best_lane = 0;
        let mut best_dot = max_dot.extract(0);
        for i in 1..4 {
            let dot = max_dot.extract(i);
            if dot > best_dot {
                best_dot = dot;
                best_lane = i;
            }
        }
        let best_index = best_indices.extract(best_lane) as usize;
        let best_point = max_point.extract_lane(best_lane);
        MinkowskiSupportResult {
            point: best_point,
            point_index: best_index,
        }
    }

    #[inline]
    #[must_use]
    pub fn get_point(&self, index: BundleIndex) -> Point3 {
        self.points[index.bundle_index()].extract_lane(index.inner_index())
    }

    #[inline]
    #[must_use]
    pub fn shift_center(&self) -> Option<Vec3> {
        self.shift_center
    }

    /// The geometric centroid of the vertex set points in `shifted_space`
    #[inline]
    #[must_use]
    pub fn centroid(&self) -> Vec3 {
        self.center
    }

    #[inline]
    #[must_use]
    pub fn get_vertex_indices(&self, face_index: usize) -> &[BundleIndex] {
        let start = self.face_indices_start[face_index] as usize;
        let next_index = face_index + 1;
        let end = if next_index < self.face_indices_start.len() {
            self.face_indices_start[next_index] as usize
        } else {
            self.face_vertex_indices.len()
        };
        &self.face_vertex_indices[start..end]
    }

    #[cfg(all(test, feature = "serde"))]
    pub(crate) fn get_face_count(&self) -> usize {
        self.face_indices_start.len()
    }

    #[cfg(all(test, feature = "serde"))]
    pub(crate) fn get_bounding_plane_wide_count(&self) -> usize {
        self.bounding_planes.len()
    }

    #[cfg(all(test, feature = "serde"))]
    pub(crate) fn get_bounding_plane_offset(&self, index: usize) -> f32 {
        let face_index = BundleIndex::new(index as u32);
        self.bounding_planes[face_index.bundle_index()]
            .offset
            .extract(face_index.inner_index())
    }

    #[inline]
    #[must_use]
    pub fn cal_estimate_epsilon_scale(&self) -> f32 {
        let point = self.points[0].extract_lane(0) - self.center;
        (point.x.absf() + point.y.absf() + point.z.absf()) * 3f32.recip()
    }

    fn is_in_face_wide(&self, indices: u32x4, test_point: Point3) -> bool32x4 {
        let mut inside = [true; 4];
        for (lane, value) in inside.iter_mut().enumerate() {
            let face_index_u32 = indices.as_ref()[lane];
            let is_inside = self.is_in_face_scalar(test_point, face_index_u32);
            *value = is_inside;
        }
        bool32x4::from(inside)
    }

    fn is_in_face_scalar(&self, test_point: Point3, face_index_u32: u32) -> bool {
        let face_index = face_index_u32 as usize;
        let face_vertex_indices = self.get_vertex_indices(face_index);
        assert!(face_vertex_indices.len() >= 3);
        // Get face normal from bounding plane
        let bounding_plane = self.get_bounding_plane(BundleIndex::new(face_index_u32));
        let normal = UnitVec3::from_array_unchecked([
            bounding_plane[0],
            bounding_plane[1],
            bounding_plane[2],
        ]);
        // Iterate edges; require point to be on the inside side for all edges
        let mut prev = self.get_point(face_vertex_indices[face_vertex_indices.len() - 1]);
        let mut is_inside = true;
        for &vertex_index in face_vertex_indices {
            let curr = self.get_point(vertex_index);
            let edge_dir = curr - prev;
            let outward = edge_dir.cross(normal);
            let to_point = test_point - prev;
            let side = outward.dot(to_point);
            if side > 0.0 {
                is_inside = false;
                break;
            }
            prev = curr;
        }
        is_inside
    }

    #[must_use]
    pub fn pick_representative_face(
        &self,
        direction: UnitVec3,
        closest_point: Point3,
        bounding_plane_eps: f32,
    ) -> PickedFace {
        let policy = PickFacePolicy::Depth;
        assert!(!self.bounding_planes.is_empty());
        let direction = UnitVec3x4::splat_soa(direction);
        let closest_point_wide = Point3x4::splat_soa(closest_point);
        let bounding_plane_eps_wide = f32x4::splat(bounding_plane_eps);
        let neg_bounding_plane_eps_wide = -bounding_plane_eps_wide;
        let mut iter = self.bounding_planes.iter().enumerate();
        let first_face = iter.next().unwrap();
        let mut best_hull_dot_direction = first_face.1.normal.dot(direction);
        let mut closest_point_dot_hull = first_face.1.normal.dot(closest_point_wide.as_vec3x4());
        let mut best_errors = (closest_point_dot_hull - first_face.1.offset).absf();
        let base_indices = u32x4::from([0, 1, 2, 3]);
        let mut best_center_distances = closest_point_wide.distance(first_face.1.center);
        let mut best_indices = base_indices;
        for (i, hull_plane) in iter {
            let indices = base_indices + u32x4::splat((i << 2) as u32);
            let dot = hull_plane.normal.dot(direction);
            let center_distance = closest_point_wide.distance(hull_plane.center);
            closest_point_dot_hull = hull_plane.normal.dot(closest_point_wide.as_vec3x4());
            let errors = (closest_point_dot_hull - hull_plane.offset).absf();
            let error_improvement = best_errors - errors;

            let is_better = self.is_better_face(
                bounding_plane_eps_wide,
                neg_bounding_plane_eps_wide,
                best_hull_dot_direction,
                dot,
                error_improvement,
                policy,
                indices,
                closest_point,
            );

            best_hull_dot_direction = dot.select(is_better, best_hull_dot_direction);
            best_errors = errors.select(is_better, best_errors);
            best_indices = indices.select(is_better, best_indices);
            best_center_distances = center_distance.select(is_better, best_center_distances);
        }

        let mut best_dot = best_hull_dot_direction.extract(0);
        let mut best_error = best_errors.extract(0);
        let mut best_index = best_indices.extract(0);
        let neg_bounding_plane_eps = -bounding_plane_eps;
        for i in 1..4 {
            let dot = best_hull_dot_direction.extract(i);
            let error = best_errors.extract(i);
            let improvement = best_error - error;
            let index = best_indices.extract(i);
            let is_better = self.is_better_face_scalar(
                bounding_plane_eps,
                best_dot,
                neg_bounding_plane_eps,
                dot,
                improvement,
                policy,
                index,
                closest_point,
            );
            if is_better {
                best_dot = dot;
                best_error = error;
                best_index = best_indices.extract(i);
            }
        }
        let face_index = BundleIndex::new(best_index);
        let normal = self.bounding_planes[face_index.bundle_index()]
            .normal
            .extract_lane(face_index.inner_index());

        PickedFace {
            face_index: best_index as usize,
            normal: normal.normalize_to_unit(),
        }
    }

    #[inline]
    #[allow(clippy::too_many_arguments)]
    fn is_better_face_scalar(
        &self,
        bounding_plane_eps: f32,
        best_dot: f32,
        neg_bounding_plane_eps: f32,
        dot: f32,
        improvement: f32,
        policy: PickFacePolicy,
        index: u32,
        closest_point: Point3,
    ) -> bool {
        match policy {
            PickFacePolicy::Depth => {
                let depth_better = improvement >= bounding_plane_eps;
                let normal_better = improvement >= neg_bounding_plane_eps && dot > best_dot;
                let in_face = (improvement >= neg_bounding_plane_eps)
                    && (best_dot - dot).absf() < 1.5e-6
                    && self.is_in_face_scalar(closest_point, index);
                depth_better || normal_better || in_face
            }
            PickFacePolicy::Normal => {
                // let eps = bounding_plane_eps * 1000.0;
                // improvement >= eps || dot > best_dot

                dot > best_dot
            }
        }
    }

    #[inline]
    #[allow(clippy::too_many_arguments)]
    fn is_better_face(
        &self,
        bounding_plane_eps_wide: f32x4,
        neg_bounding_plane_eps_wide: f32x4,
        best_hull_dot_direction: f32x4,
        dot: f32x4,
        error_improvement: f32x4,
        policy: PickFacePolicy,
        indices: u32x4,
        closest_point: Point3,
    ) -> bool32x4 {
        let indices = u32x4::min(
            indices,
            u32x4::splat((self.face_indices_start.len() - 1) as u32),
        );
        match policy {
            PickFacePolicy::Depth => {
                let is_better_depth = error_improvement.ge(bounding_plane_eps_wide);
                let error_valid = error_improvement.gt(neg_bounding_plane_eps_wide);
                let is_better_normal = error_valid & dot.gt(best_hull_dot_direction);
                let error_valid_distance = error_improvement.gt(3.0 * neg_bounding_plane_eps_wide);
                let is_in_face = error_valid_distance
                    & ((best_hull_dot_direction - dot)
                        .absf()
                        .lt(f32x4::const_splat(MIN_DERTA)))
                    & self.is_in_face_wide(indices, closest_point);
                is_better_depth | is_better_normal | is_in_face
            }
            PickFacePolicy::Normal => {
                // let is_better_normal = dot.gt(best_hull_dot_direction);
                // let is_better_depth = error_improvement.ge(bounding_plane_eps_wide *  1000.0);
                // let is_better = is_better_depth | is_better_normal;
                // is_better
                dot.gt(best_hull_dot_direction)
            }
        }
    }

    #[inline]
    pub(crate) fn face_vertex_indices(&self) -> &[BundleIndex] {
        &self.face_vertex_indices
    }
}

impl ComputeAabb3 for ConvexHull {
    fn compute_aabb(&self) -> Aabb3 {
        let mut min_wide = Point3x4::splat(f32x4::splat(f32::MAX));
        let mut max_wide = Point3x4::splat(f32x4::splat(f32::MIN));
        for point in &self.points {
            min_wide = min_wide.min(*point);
            max_wide = max_wide.max(*point);
        }
        let min = (1..4).fold(
            min_wide.extract_lane(0),
            #[inline]
            |value, i| {
                let lane = min_wide.extract_lane(i);
                value.min(lane)
            },
        );
        let max = (1..4).fold(
            max_wide.extract_lane(0),
            #[inline]
            |value, i| {
                let lane = max_wide.extract_lane(i);
                value.max(lane)
            },
        );
        let expand = 0.01f32;
        let expand = Vec3::new(expand, expand, expand);
        let p1 = min - expand;
        let p2 = max + expand;
        let (p1, p2) = match self.shift_center {
            Some(shift_center) => (
                shift_point_back(p1, shift_center),
                shift_point_back(p2, shift_center),
            ),
            None => (p1, p2),
        };
        Aabb3::new_unchecked([p1.x, p1.y, p1.z], [p2.x, p2.y, p2.z])
    }
}

impl ConvexHull {
    #[must_use]
    pub fn raycast(
        &self,
        local_ray: Ray,
        max_distance: f32,
        discard_inside_hit: bool,
    ) -> std::option::Option<RaycastHitResult> {
        let ray_dir = local_ray.direction.conv_to();
        let ray_origin: Vec3 = local_ray.origin.to_array().into();
        let ray_direction = UnitVec3x4::splat_soa(ray_dir);
        let shift_center = self.shift_center().unwrap_or(Vec3::ZERO);
        // Shift the ray's origin by the convex hull's center shift to ensure correct raycasting
        // results when iterating over all faces.
        let shifted_origin = Point3x4::splat_soa(Point3::from_vec3(ray_origin - shift_center));

        let mut is_origin_inside = true;
        let mut nearest_exit_dist_wide = f32x4::const_splat(f32::MAX);
        let mut furthest_enter_dist = f32::MIN;
        let mut result_normal: Option<UnitVec3> = None;

        let valid_face_num = self.face_indices_start.len();
        let mut first_face_index_in_bundle = 0_usize;
        let mut new_last_plane_wide;

        for plane in &self.bounding_planes {
            // process invalid face wide lane
            let plane_valid = if first_face_index_in_bundle + 3 < valid_face_num {
                plane
            } else {
                let valid_num_in_last_bundle = valid_face_num - first_face_index_in_bundle; // 1,2 or 3
                let copied_plane_lane_index = valid_num_in_last_bundle - 1;
                let covered_lanes_index = valid_num_in_last_bundle..=3_usize;
                new_last_plane_wide = plane.clone();
                let last_plane_normal = new_last_plane_wide
                    .normal
                    .extract_lane(copied_plane_lane_index);
                let last_plane_offset = new_last_plane_wide.offset.extract(copied_plane_lane_index);
                for i in covered_lanes_index {
                    new_last_plane_wide
                        .normal
                        .replace_lane(i, last_plane_normal);
                    new_last_plane_wide.offset.replace(i, last_plane_offset);
                }

                &new_last_plane_wide
            };
            first_face_index_in_bundle += 4;

            let ray_origin_to_plane = plane_valid.signed_distance_to_point(&shifted_origin);
            let plane_normal = plane_valid.normal.normalize_to_unit();
            let ray_plane_verticality = plane_normal.dot_clamp(ray_direction);

            let ray_origin_to_plane_along_ray = -ray_origin_to_plane / ray_plane_verticality;
            let origin_on_positive_side = bool32x4::select(
                bool32x4::TRUE,
                ray_origin_to_plane.gt(f32x4::ZERO),
                bool32x4::FALSE,
            );

            // track if ray origin inside convex hull
            if is_origin_inside && origin_on_positive_side.any() {
                is_origin_inside = false;
            }

            let ray_from_plane_back = ray_plane_verticality.gt(f32x4::EPSILON);
            let ray_from_plane_front = ray_plane_verticality.lt(-f32x4::EPSILON);
            let ray_parallel_with_one_plane = !(ray_from_plane_back | ray_from_plane_front);

            // if ray is parallel with one plane and origin is on the positive side of the plane,
            // then the ray is outside the convex hull
            if (ray_parallel_with_one_plane & origin_on_positive_side).any() {
                return None;
            }

            // track the nearest exit
            // Although these planes are infinite planes, in the convex hull, the exit position of
            // the ray and convex hull must be the exit point nearest to the orign.
            nearest_exit_dist_wide = nearest_exit_dist_wide
                .min(ray_origin_to_plane_along_ray)
                .select(ray_from_plane_back, nearest_exit_dist_wide);

            let work_lanes = ray_from_plane_front
                & ray_origin_to_plane_along_ray.gt(f32x4::const_splat(furthest_enter_dist));
            let work_lanes_array = <[bool; 4]>::from(work_lanes);
            let ray_origin_to_plane_along_ray_array =
                <[f32; 4]>::from(ray_origin_to_plane_along_ray);
            let mut new_normal: Option<usize> = None;
            for i in 0..4_usize {
                if work_lanes_array[i]
                    && ray_origin_to_plane_along_ray_array[i] > furthest_enter_dist
                {
                    // track the furthest enter
                    // In the convex hull, the enter position of the ray and convex hull must be the
                    // enter point furthest from the orign.
                    furthest_enter_dist = ray_origin_to_plane_along_ray_array[i];
                    new_normal = Some(i);
                }
            }
            if let Some(i) = new_normal {
                result_normal = Some(plane_normal.extract_lane(i));
            }
        }

        let nearest_exit_dist = Vec4::from(<[f32; 4]>::from(nearest_exit_dist_wide)).min_element();

        if is_origin_inside {
            if discard_inside_hit {
                return None;
            }
            return Some(RaycastHitResult {
                distance: 0.,
                normal: -local_ray.direction,
            });
        }

        // If the nearest exit point is closer than the farthest entrance point, then the ray does
        // not intersect the convex hull
        if furthest_enter_dist < nearest_exit_dist
            && furthest_enter_dist > 0.
            && furthest_enter_dist < max_distance
        {
            return Some(RaycastHitResult {
                distance: furthest_enter_dist,
                normal: result_normal.map_or_else(
                    || panic!("error occurs in convex hull raycast."),
                    ConvTo::conv_to,
                ),
            });
        }

        None
    }
}
///`get_bundle_count` is disabled when qhull is turned on
#[inline]
#[allow(unused)]
fn get_bundle_count(count: usize) -> usize {
    (count + 3) >> 2
}

impl Expansion for ConvexHull {
    fn max_radius_and_max_angular_expansion(&self) -> (f32, f32) {
        // TODO: implement this in issue (#1673)
        (0.0, 0.0)
    }
}

impl ComputeVolume for ConvexHull {
    #[inline]
    fn compute_volume(&self) -> f32 {
        self.volume
    }
}

impl MinkowskiSupport for ConvexHull {
    fn support_point(&self, direction: Vec3, transform: &Isometry3) -> MinkowskiSupportResult {
        assert!(!self.points.is_empty());
        let dir = transform.inverse().transform_vector3(direction);

        let result = self.support_point_local_narrow(dir);
        MinkowskiSupportResult {
            point: transform.transform_point3(result.point),
            point_index: result.point_index,
        }
    }
}

impl ShapePlugin for ConvexHull {
    const PLUGIN_ID: u64 = ConvexHull::PLUGIN_ID;
}

impl Raycast for ConvexHull {
    fn raycast(
        &self,
        local_ray: Ray,
        max_distance: f32,
        discard_inside_hit: bool,
    ) -> Option<RaycastHitResult> {
        self.raycast(local_ray, max_distance, discard_inside_hit)
    }
}
impl ComplexShapeTrait for ConvexHull {}
#[derive(Default)]
pub struct ConvexHullWide {
    hulls: [ConvexHullId; 4],
}

impl ConvexHullWide {
    #[inline]
    pub fn iter_take(&self, count: usize) -> impl Iterator<Item = &ConvexHullId> {
        self.hulls.iter().take(count)
    }

    #[inline]
    #[must_use]
    pub fn get_id(&self, index: usize) -> ConvexHullId {
        self.hulls[index]
    }

    #[inline]
    #[must_use]
    pub fn estimate_epsilon_scale(
        &self,
        inactive_lanes: bool32x4,
        container: &ShapeContainer,
    ) -> f32x4 {
        let mut hull_epsilon_scale = f32x4::const_splat(0f32);
        for i in 0..4 {
            if inactive_lanes.extract(i) {
                continue;
            }
            hull_epsilon_scale.replace(
                i,
                container
                    .get::<ConvexHull>(self.get_id(i))
                    .expect("hull must exist")
                    .cal_estimate_epsilon_scale(),
            );
        }
        hull_epsilon_scale
    }

    pub(crate) fn get_convexhull_shift_wide(
        &self,
        container: &ShapeContainer,
        pair_count: usize,
    ) -> Vec3x4 {
        let mut shift_wide = Vec3x4::ZERO;
        for (i, shape_id) in self.iter_take(pair_count).enumerate() {
            let shape_b = container
                .get::<ConvexHull>(*shape_id)
                .expect("invalid shape id");
            let shift = shape_b.shift_center().unwrap_or(Vec3::ZERO);
            shift_wide.replace_lane(i, shift);
        }
        shift_wide
    }

    pub(crate) fn get_center(&self, container: &ShapeContainer, pair_count: usize) -> Vec3x4 {
        let mut center_wide = Vec3x4::ZERO;
        for (i, shape_id) in self.iter_take(pair_count).enumerate() {
            let shape_b = container
                .get::<ConvexHull>(*shape_id)
                .expect("invalid shape id");
            center_wide.replace_lane(i, shape_b.centroid());
        }
        center_wide
    }
}

impl MinkowskiSupportWide for ConvexHullWide {
    fn support_point_local(
        &self,
        local_direction: Vec3x4,
        container: Option<&ShapeContainer>,
    ) -> MinkowskiSupportResultWide {
        let container =
            container.expect("ConvexHullWide::support_point_local called without a container");
        let mut results = [MinkowskiSupportResult::default(); 4];
        self.hulls
            .iter()
            .map(
                #[inline]
                |&id| {
                    if id.is_valid() {
                        Some(container.get::<ConvexHull>(id).expect("hull must exist"))
                    } else {
                        None
                    }
                },
            )
            .enumerate()
            .for_each(
                #[inline]
                |(i, hull)| {
                    if let Some(convex_hull) = hull {
                        results[i] =
                            convex_hull.support_point_local_narrow(local_direction.extract_lane(i));
                    } else {
                        // slot values are invalid, so we can just leave the default value.
                    }
                },
            );
        MinkowskiSupportResultWide {
            point: ArrayGetter::<4>::get_point3x4_from_array4(
                ArrayGetter::<4>::get_array4_from_iter(
                    results.iter().map(
                        #[inline]
                        |result| result.point,
                    ),
                    Point3::default(),
                ),
            ),
            point_index: u32x4::from(ArrayGetter::<4>::get_array4_from_iter(
                results.iter().map(
                    #[inline]
                    |result| result.point_index as u32,
                ),
                0_u32,
            )),
        }
    }
}

impl ContainsPoint for ConvexHull {
    #[inline]
    fn contains_point_with_threshold(&self, local_point: Point3, threshold: f32) -> ContainsResult {
        let local_point_wide = Point3x4::splat_soa(local_point);
        for plane in self.bounding_planes.iter().as_ref() {
            let distances = plane.signed_distance_to_point(&local_point_wide);
            if distances.le(f32x4::const_splat(-threshold)).all() {
                continue;
            }
            if distances.ge(f32x4::const_splat(threshold)).any() {
                return ContainsResult::Outside;
            }
            let is_zero = distances.absf().lt(f32x4::splat(threshold));
            if is_zero.any() {
                for i in 0..4 {
                    let is_zero = is_zero.extract(i);
                    if is_zero && plane.offset.extract(i) != f32::MIN {
                        return ContainsResult::Surface;
                    }
                }
            }
        }
        ContainsResult::Inside
    }
}

impl SignedDistanceToPoint for ConvexHull {
    fn signed_distance_to_point(&self, _local_point: Point3) -> f32 {
        todo!("issue #1433");
    }
}

macro_rules! impl_convex_hull_wide {
    ($($num:tt),*) => {
        $(
          impl CreateShapeWide<$num> for ConvexHullWide {
            fn create<'a>(iter: impl Iterator<Item=&'a Self::TShape> + Clone) -> Self where Self::TShape: 'a {
                let hulls = ArrayGetter::<$num>::get_array4_from_iter(iter.map(#[inline]|&shape|{shape}),ConvexHullId::default());
                Self {
                    hulls,
                }
            }
          }
        )*
    }

}

impl BaseShapeWide for ConvexHullWide {
    type TShape = ConvexHullId;
}

impl_convex_hull_wide!(1, 2, 3, 4);

#[cfg(test)]
mod tests {
    use approx_det::assert_relative_eq;
    #[cfg(not(feature = "qhull"))]
    use chull_adapt::Config;
    #[cfg(not(feature = "qhull"))]
    use chull_adapt::ConvexHullConstructState::Success;
    use glam::nums::u32x4;
    use glam::UnitVec3;
    use glam_det::nums::{f32x4, Bool, NumConstEx};
    use glam_det::{Isometry3, Point3, Point3x4, UnitQuat, Vec3, Vec3x4};
    use wasm_bindgen_test::{wasm_bindgen_test_configure, *};

    #[cfg(feature = "contact_debug")]
    use super::ConvexHullDebugInfo;
    use super::{ConvexHullPointsIter, ConvexHullTriangleIndexIter, HullPlaneWide};
    use crate::ray::RaycastHitResult;
    use crate::shapes::{BundleIndex, CuboidExt};
    use crate::traits::{ArrayGetter, Expansion, MinkowskiSupport};
    use crate::{ComputeAabb3, ContainsResult, ConvexHull, Cuboid, Ray, Shape, ShapeContainer};

    wasm_bindgen_test_configure!(run_in_browser);

    impl ConvexHull {
        #[cfg(not(feature = "qhull"))]
        pub(crate) fn get_point_raw(&self, index: BundleIndex) -> Point3 {
            self.points[index.bundle_index()].extract_lane(index.inner_index())
        }

        #[must_use]
        pub fn new_unchecked_and_shift(points: &[Point3]) -> Self {
            #[cfg(not(feature = "qhull"))]
            let result = crate::shapes::convex_hull::quick_convex_hull(points, true, true);

            #[cfg(feature = "qhull")]
            let result = crate::shapes::convex_hull::quick_convex_hull(points, true, true);
            debug_assert!(result.is_ok(), "error occurs in convex hull creation");
            result.map_or(ConvexHull::empty(), |(hull, _)| hull)
        }

        #[must_use]
        pub fn generate_displacement_cuboid_vertex(
            length: Vec3,
            displacement_vec: Vec3,
        ) -> Vec<Point3> {
            let cuboid = Cuboid::new(length);
            (0..8)
                .map(|i| cuboid.get_vertex(i) + displacement_vec)
                .collect::<Vec<_>>()
        }
    }

    #[test]
    #[cfg(feature = "serde")]
    fn test_generate_shifted_cuboid_vertex() {
        use std::iter::zip;

        let points_expect = vec![
            Point3::new(0.5, 1.5, 0.5),
            Point3::new(1.5, 1.5, 0.5),
            Point3::new(0.5, 0.5, 0.5),
            Point3::new(1.5, 0.5, 0.5),
            Point3::new(0.5, 1.5, 1.5),
            Point3::new(1.5, 1.5, 1.5),
            Point3::new(0.5, 0.5, 1.5),
            Point3::new(1.5, 0.5, 1.5),
        ];
        let points_actual = ConvexHull::generate_displacement_cuboid_vertex(Vec3::ONE, Vec3::ONE);

        assert_eq!(points_expect.len(), points_actual.len());
        for (expect, actual) in zip(points_expect, points_actual) {
            assert_relative_eq!(expect, actual);
        }
    }

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

        let cuboid = Cuboid::UNIT;
        let points_0 = [
            cuboid.get_vertex(0),
            cuboid.get_vertex(1),
            cuboid.get_vertex(2),
            cuboid.get_vertex(3),
        ];
        let points_1 = [
            cuboid.get_vertex(4),
            cuboid.get_vertex(5),
            cuboid.get_vertex(6),
            cuboid.get_vertex(7),
        ];
        let wide_point0 = ArrayGetter::<4>::get_point3x4_from_array4(points_0);
        let wide_point1 = ArrayGetter::<4>::get_point3x4_from_array4(points_1);

        let convex_hull = ConvexHull {
            points: vec![wide_point0, wide_point1],
            bounding_planes: vec![],
            face_vertex_indices: vec![],
            face_indices_start: vec![],
            center: Vec3::default(),
            volume: 0.0,
            total_area: 0.0,
            shift_center: None,
            #[cfg(feature = "contact_debug")]
            debug: ConvexHullDebugInfo::default(),
        };
        let bound0 = convex_hull.compute_aabb();
        let bound1 = cuboid.compute_aabb();
        let expand = phys_geom::math::Vec3::repeat(0.01);
        assert_eq!(bound0.min(), bound1.min() - expand);
        assert_eq!(bound0.max(), bound1.max() + expand);
    }

    #[test]
    #[wasm_bindgen_test]
    #[cfg(feature = "serde")]
    fn shifted_convex_hull_compute_bound() {
        let _ = env_logger::builder().is_test(true).try_init();

        let cuboid = Cuboid::UNIT;
        let shift = Vec3::new(1., 2., 3.);
        let points_0 = [
            (cuboid.get_vertex(0)),
            (cuboid.get_vertex(1)),
            (cuboid.get_vertex(2)),
            (cuboid.get_vertex(3)),
        ];
        let points_1 = [
            (cuboid.get_vertex(4)),
            (cuboid.get_vertex(5)),
            (cuboid.get_vertex(6)),
            (cuboid.get_vertex(7)),
        ];
        let wide_point0 = ArrayGetter::<4>::get_point3x4_from_array4(points_0);
        let wide_point1 = ArrayGetter::<4>::get_point3x4_from_array4(points_1);

        let convex_hull = ConvexHull {
            points: vec![wide_point0, wide_point1],
            bounding_planes: vec![],
            face_vertex_indices: vec![],
            face_indices_start: vec![],
            center: Vec3::default(),
            volume: 0.0,
            total_area: 0.0,
            shift_center: Some(shift),
            #[cfg(feature = "contact_debug")]
            debug: ConvexHullDebugInfo::default(),
        };
        let bound0 = convex_hull.compute_aabb();
        let bound1 = cuboid.compute_aabb();
        let expand = phys_geom::math::Vec3::repeat(0.01f32);
        let shift: phys_geom::math::Vec3 = shift.to_array().into();
        assert_eq!(bound0.min(), bound1.min() - expand + shift);
        assert_eq!(bound0.max(), bound1.max() + expand + shift);
    }

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

        let cuboid = Cuboid::UNIT;
        let transform_a = Isometry3::from_rotation_translation(
            UnitQuat::from_euler_default(
                45f32.to_radians(),
                45f32.to_radians(),
                45f32.to_radians(),
            ),
            Vec3::new(1.2, 0.0, 0.0),
        );
        let v0 = cuboid.get_vertex(0);
        let v1 = cuboid.get_vertex(1);
        let v2 = cuboid.get_vertex(2);
        let v3 = cuboid.get_vertex(3);
        let v4 = cuboid.get_vertex(4);
        let v5 = cuboid.get_vertex(5);
        let v6 = cuboid.get_vertex(6);
        let v7 = cuboid.get_vertex(7);

        let points = vec![v0, v1, v2, v3, v4, v5, v6, v7];
        let convex_hull = ConvexHull::new_unchecked(&points);
        let dir = -Vec3::X;
        let support_a = convex_hull.support_point(dir, &transform_a);
        let support_b = cuboid.support_point(dir, &transform_a);
        assert_eq!(support_a.point, support_b.point);
    }

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

        let point_0 = Point3x4::splat_soa(Point3::new(1., 2., 3.));
        let point_1 = Point3x4::splat_soa(Point3::new(-1., -2., -3.));
        let plane_0 = HullPlaneWide {
            center: Point3x4::default(),
            normal: Vec3x4::X,
            offset: f32x4::ZERO,
        };
        let plane_1 = HullPlaneWide {
            center: Point3x4::default(),
            normal: Vec3x4::Y,
            offset: f32x4::ZERO,
        };
        let plane_2 = HullPlaneWide {
            center: Point3x4::default(),
            normal: Vec3x4::Z,
            offset: f32x4::ZERO,
        };
        let plane_3 = HullPlaneWide {
            center: Point3x4::default(),
            normal: Vec3x4::X,
            offset: -f32x4::ONE,
        };

        assert!((plane_0.signed_distance_to_point(&point_0) == f32x4::ONE).all());
        assert!((plane_0.signed_distance_to_point(&point_1) == -f32x4::ONE).all());
        assert!((plane_1.signed_distance_to_point(&point_0) == f32x4::TWO).all());
        assert!((plane_1.signed_distance_to_point(&point_1) == -f32x4::TWO).all());
        assert!((plane_2.signed_distance_to_point(&point_0) == f32x4::ONE + f32x4::TWO).all());
        assert!((plane_2.signed_distance_to_point(&point_1) == -f32x4::ONE - f32x4::TWO).all());
        assert!((plane_3.signed_distance_to_point(&point_0) == f32x4::TWO).all());
        assert!((plane_3.signed_distance_to_point(&point_1) == f32x4::ZERO).all());
    }

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

        let cuboid = Cuboid::new(Vec3::ONE * 2.0);
        let vertice_indexs = 0..8_usize;
        let vertices: Vec<Point3> = vertice_indexs
            .into_iter()
            .map(|i| cuboid.get_vertex(i))
            .collect();
        let convex_hull = ConvexHull::new_unchecked(&vertices);
        let ray_z = Ray::new_with_vec3([0.0, 0.0, 2.0], [0., 0., -1.]);
        assert_eq!(
            convex_hull.raycast(ray_z, 5.0, false),
            Some(RaycastHitResult::new(1.0, [0.0, 0.0, 1.0]))
        );

        assert_eq!(convex_hull.raycast(ray_z, 0.5, false), None);

        assert_eq!(
            convex_hull.raycast(
                Ray::new_with_vec3([2.0, 0.0, 0.0], [-1., 0., 0.]),
                5.0,
                false,
            ),
            Some(RaycastHitResult::new(1.0, [1.0, 0.0, 0.0]))
        );

        assert_eq!(
            convex_hull.raycast(
                Ray::new_with_vec3([0.0, 2.0, 0.0], [0.0, -1.0, 0.0]),
                5.0,
                false,
            ),
            Some(RaycastHitResult::new(1.0, [0.0, 1.0, 0.0]))
        );

        {
            let ray = Ray::new_with_vec3([0.0, 0.0, 2.0], [3.0, 0.0, -2.0]);
            assert_eq!(convex_hull.raycast(ray, 10.0, false), None);
        }

        {
            let ray = Ray::new_with_vec3([0.0, 0.0, 1.001], [1.0, 0.0, 0.0]);
            assert_eq!(convex_hull.raycast(ray, 10.0, false), None);
        }

        {
            let ray = Ray::new_with_vec3([0.0, 0.0, 0.999], [1.0, 0.0, 0.0]);
            assert_eq!(
                convex_hull.raycast(ray, 10.0, false),
                Some(RaycastHitResult::new(0.0, [-1.0, 0.0, 0.0]))
            );
        }

        {
            let ray = Ray::new_with_vec3([0.0, 0.0, 0.999], [1.0, 0.0, 0.0]);
            assert_eq!(convex_hull.raycast(ray, 10.0, true), None);
        }

        let shifted_vertices =
            ConvexHull::generate_displacement_cuboid_vertex(Vec3::ONE * 2.0, Vec3::ONE);
        let shifted_convexhull = ConvexHull::new_unchecked_and_shift(&shifted_vertices);
        {
            let ray = Ray::new_with_vec3([1.0, 1.0, -5.0], [0.0, 0.0, 1.0]);
            assert_eq!(
                shifted_convexhull.raycast(ray, 10.0, true),
                Some(RaycastHitResult::new(5.0, [0.0, 0.0, -1.0]))
            );
        }
        {
            let ray = Ray::new_with_vec3([-1.0, 1.0, -5.0], [0.0, 0.0, 1.0]);
            assert_eq!(shifted_convexhull.raycast(ray, 10.0, true), None);
        }
        {
            let ray = Ray::new_with_vec3([1.0, -1.0, -5.0], [0.0, 0.0, 1.0]);
            assert_eq!(shifted_convexhull.raycast(ray, 10.0, true), None);
        }
        {
            let ray = Ray::new_with_vec3([-1.0, -1.0, -5.0], [0.0, 0.0, 1.0]);
            assert_eq!(shifted_convexhull.raycast(ray, 10.0, true), None);
        }
    }

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

        let cuboid = Cuboid::new(Vec3::ONE * 2.0);
        let vertice_indexs = 0..8_usize;
        let vertices: Vec<Point3> = vertice_indexs
            .into_iter()
            .map(|i| cuboid.get_vertex(i))
            .collect();
        let convex_hull = ConvexHull::new_unchecked(&vertices);
        let ray = Ray::new_with_vec3([0.0, 0.0, 1.0], [0.0, 0.0, 1.0]);
        assert_eq!(convex_hull.raycast(ray, 10.0, true), None);
        assert_eq!(
            convex_hull.raycast(ray, 10.0, false),
            Some(RaycastHitResult {
                distance: 0.0,
                normal: -ray.direction,
            })
        );
    }

    // The origin of the ray is inside the cube.
    #[test]
    #[wasm_bindgen_test]
    fn test_raycast_inner() {
        let _ = env_logger::builder().is_test(true).try_init();

        let cuboid = Cuboid::new(Vec3::ONE * 2.0);
        let vertice_indexs = 0..8_usize;
        let vertices: Vec<Point3> = vertice_indexs
            .into_iter()
            .map(|i| cuboid.get_vertex(i))
            .collect();
        let convex_hull = ConvexHull::new_unchecked(&vertices);
        let ray = Ray::new_with_vec3([0.0, 0.0, 0.0], [0.0, 0.0, -1.0]);
        assert_eq!(
            convex_hull.raycast(ray, 5.0, false),
            Some(RaycastHitResult {
                distance: 0.0,
                normal: -ray.direction,
            })
        );

        assert_eq!(convex_hull.raycast(ray, 5.0, true), None);
    }
    #[test]
    #[wasm_bindgen_test]
    fn test_contain_point() {
        let _ = env_logger::builder().is_test(true).try_init();

        let mut container = ShapeContainer::default();
        let cuboid = Cuboid::new(Vec3::ONE * 2.0);
        let vertices: Vec<Point3> = (0..8_usize)
            .map(
                #[inline]
                |i| cuboid.get_vertex(i),
            )
            .collect();
        let id = container.add(ConvexHull::new_unchecked(&vertices));

        let convex_hull = Shape::ConvexHull(id).into_shape_ref(&container);
        let is_inner = convex_hull
            .contains_point(Point3::new(0.0, 0.0, 0.0))
            .eq(&ContainsResult::Inside);
        assert!(is_inner);
        let is_surface = convex_hull
            .contains_point(Point3::new(0.0, 0.0, 1.0))
            .eq(&ContainsResult::Surface);
        assert!(is_surface);
        let is_outer = convex_hull
            .contains_point(Point3::new(0.0, 0.0, 1.5))
            .eq(&ContainsResult::Outside);
        assert!(is_outer);
    }

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

        let convex_hull = {
            let cuboid = Cuboid::new(Vec3::ONE * 2.0);
            let vertices: Vec<Point3> = (0..8_usize)
                .map(
                    #[inline]
                    |i| cuboid.get_vertex(i),
                )
                .collect();
            ConvexHull::new_unchecked(&vertices)
        };
        let (max_radius, max_angular_expansion) =
            convex_hull.max_radius_and_max_angular_expansion();
        // just for test coverage
        assert_relative_eq!(max_radius, 0.0f32);
        assert_relative_eq!(max_angular_expansion, 0.0f32);
    }
    #[test]
    #[wasm_bindgen_test]
    #[cfg(not(feature = "qhull"))]
    fn test_build_shifted_convex_face() {
        let points = ConvexHull::generate_displacement_cuboid_vertex(
            Vec3::new(1., 2., 3.),
            Vec3::new(4., 5., 6.),
        );
        let config = Config::new(0xFF, 0x10000, 1e-6, true, true);
        let result = chull_adapt::ConvexHull::new(&points, &config);
        let result = result.unwrap();
        let state = result.1;
        assert_eq!(state, Success);
    }
    #[test]
    #[wasm_bindgen_test]
    fn test_points_iter() {
        let convex_hull = {
            let cuboid = Cuboid::new(Vec3::ONE * 2.0);
            let vertices: Vec<Point3> = (0..8_usize)
                .map(
                    #[inline]
                    |i| cuboid.get_vertex(i),
                )
                .collect();
            ConvexHull::new_unchecked(&vertices)
        };
        let iter = ConvexHullPointsIter::new(&convex_hull);
        let count = iter.count();
        assert_eq!(count, 8);
        let iter = ConvexHullTriangleIndexIter::new(&convex_hull);
        let count = iter.count();
        assert_eq!(count, 12);
    }

    fn cube_points() -> Vec<Point3> {
        let coords = [-1.0_f32, 1.0_f32];
        let mut pts = Vec::new();
        for &x in &coords {
            for &y in &coords {
                for &z in &coords {
                    pts.push(Point3::new(x, y, z));
                }
            }
        }
        pts
    }

    use glam_det::Dot;
    #[test]
    fn test_is_in_face_wide_inside_outside() {
        // Build a cube hull
        let points = cube_points();
        let hull = ConvexHull::new_unchecked(&points);

        // Find a face whose normal is approximately +Z
        let mut chosen_face_index: Option<usize> = None;
        let up = UnitVec3::from_array_unchecked([0.0, 0.0, 1.0]);
        let face_count = hull.face_indices_start.len();
        for i in 0..face_count {
            let bi = BundleIndex::new(i as u32);
            let plane = hull.get_bounding_plane(bi);
            let n = UnitVec3::from_array_unchecked([plane[0], plane[1], plane[2]]);
            if n.dot(up.as_vec3()) > 0.9 {
                chosen_face_index = Some(i);
                break;
            }
        }
        let fi = chosen_face_index.expect("should find top face");

        // Compute the face center as average of its vertices
        let face_vertex_indices = hull.get_vertex_indices(fi);
        assert!(face_vertex_indices.len() >= 3);
        let mut center = Vec3::ZERO;
        for &vidx in face_vertex_indices {
            let p = hull.get_point(vidx);
            let indices_wide = u32x4::from([fi as u32, fi as u32, fi as u32, fi as u32]);

            // Point clearly inside: face center
            let inside_mask = hull.is_in_face_wide(indices_wide, p);
            assert!(
                inside_mask.all(),
                "expected point at face center to be inside for all lanes"
            );
            center += p.as_vec3();
        }
        center /= face_vertex_indices.len() as f32;
        let center_p = Point3::from_vec3(center);

        // Indices wide: same face in all lanes
        let indices_wide = u32x4::from([fi as u32, fi as u32, fi as u32, fi as u32]);

        // Point clearly inside: face center
        let inside_mask = hull.is_in_face_wide(indices_wide, center_p);
        assert!(
            inside_mask.all(),
            "expected point at face center to be inside for all lanes"
        );

        // Point clearly outside: shift along X beyond face extent
        let outside_p = Point3::from_vec3(center + Vec3::new(5.0, 0.0, 0.0));
        let outside_mask = hull.is_in_face_wide(indices_wide, outside_p);
        assert!(
            outside_mask.none(),
            "expected distant point to be outside for all lanes"
        );
    }
}