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 std::ops::BitOr;

use glam_det::nums::{bool32x4, f32x4, i32x4, Bool, Float, Num, NumConstEx, PartialOrdEx};
use glam_det::{Cross, Dot, UnitVec3x4, Vec3x4};

use super::common::{NormalizeExt, EPS_6};
use crate::collision_tasks::traits::TransformativeWide;
use crate::traits::MinkowskiSupportWide;
use crate::ShapeContainer;

pub fn find_minimum_depth(
    a: &impl MinkowskiSupportWide,
    b: &impl MinkowskiSupportWide,
    transform_b_2_a: &impl TransformativeWide,
    initial_normal: Vec3x4,
    context: &IterContext,
) -> TootBirdWideResult {
    debug_assert!((initial_normal.length_squared() - f32x4::ONE)
        .lt(f32x4::splat(1e-6))
        .bitor(context.inactive_lanes)
        .all());
    let support_result = support_wide(
        a,
        b,
        transform_b_2_a,
        initial_normal,
        context.shape_container,
    );
    let init_depth = initial_normal.dot(support_result.support);
    //init simplex is a vertex which is the first support point
    let mut init_simplex = Simplex::new(support_result);
    tootbird_iteration(
        a,
        b,
        transform_b_2_a,
        &mut init_simplex,
        initial_normal,
        init_depth,
        context,
    )
}

fn tootbird_iteration(
    a: &impl MinkowskiSupportWide,
    b: &impl MinkowskiSupportWide,
    transform_b_2_a: &impl TransformativeWide,
    simplex: &mut Simplex,
    init_normal: Vec3x4,
    init_depth: f32x4,
    context: &IterContext,
) -> TootBirdWideResult {
    // so the focus thing here is to find the closest simplex to the origin, and the closest point
    // on the simplex surface to the origin
    // 1. we need to update the normal depend on the distance from tootbird to the support plane,
    // 2. in intersection case,the termination condition is the distance from tootbird to the
    // support plane is less than the minimum_depth_threshold
    // 3. in separation case, the termination condition is the distance from tootbird to the support
    // plane is larger than the -minimum_depth_threshold or the change of ||distance|| is very small
    // the tootbird point is  the closest point to the origin on the so-far-best bounding plane
    let mut terminate = init_depth
        .lt(context.depth_threshold)
        .bitor(context.inactive_lanes);
    if terminate.all() {
        return TootBirdWideResult::new(init_depth, UnitVec3x4::Y, Vec3x4::ZERO);
    }
    let mut normal = UnitVec3x4::Y;
    let mut tootbird = TootBird::new(init_normal.normalize_to_unit(), init_depth);
    next_normal_and_simplex(
        None,
        context.search_threshold,
        &tootbird,
        &mut terminate,
        &mut normal,
        simplex,
        context.push_normal_policy,
    );
    for _ in 0..context.maximum_iterations {
        let support_result = support_wide(
            a,
            b,
            transform_b_2_a,
            normal.as_vec3x4(),
            context.shape_container,
        );
        let depth = normal.dot(support_result.support);
        tootbird.update(normal, depth, terminate);
        terminate = tootbird.depth.le(context.depth_threshold).bitor(terminate);
        if terminate.all() {
            break;
        }
        next_normal_and_simplex(
            Some(support_result),
            context.search_threshold,
            &tootbird,
            &mut terminate,
            &mut normal,
            simplex,
            context.push_normal_policy,
        );
    }
    let inverse_denominator = simplex.weight_denominator.recip();
    let a_weight = simplex.a.weight * inverse_denominator;
    let b_weight = simplex.b.weight * inverse_denominator;
    let c_weight = simplex.c.weight * inverse_denominator;

    let closest_point_on_a = simplex.a.point.support_in_a_local_space * a_weight
        + simplex.b.point.support_in_a_local_space * b_weight
        + simplex.c.point.support_in_a_local_space * c_weight;
    TootBirdWideResult::new(tootbird.depth, tootbird.normal, closest_point_on_a)
}

fn next_normal_and_simplex(
    support_wide_result: Option<SupportWideResult>,
    search_threshold: f32x4,
    toot_bird: &TootBird,
    terminate: &mut bool32x4,
    normal: &mut UnitVec3x4,
    simplex: &mut Simplex,
    push_normal_policy: bool,
) {
    let toot_bird_point = toot_bird.point();
    // in separation case,the tootbird is origin,the distance from tootbird to the support plane
    // should be -tootbird.depth,so the terminate condition is new_distance - -tootbird.depth <
    // search_threshold,so new_distance < -tootbird.depth + search_threshold
    // in intersection case, the terminate condition is new_distance < search_threshold
    let search_threshold = (search_threshold - toot_bird.depth)
        .select(toot_bird.depth.le(f32x4::ZERO), search_threshold);
    let terminate_threshold_squared = search_threshold * search_threshold;
    if let Some(support_wide_result) = support_wide_result {
        // if any lane's simplex is triangle abc,the new support point is d,we need to choose the
        // best triangle from abc,abd,acd,bcd
        let is_triangle = simplex.is_triangle() & (!*terminate);
        simplex.update_if_not_exists(support_wide_result, *terminate);
        if is_triangle.any() {
            // calculate the distance between point and triangle is expensive
            let ab = simplex.b.point.support - simplex.a.point.support;
            let ca = simplex.a.point.support - simplex.c.point.support;
            let ad = support_wide_result.support - simplex.a.point.support;
            let bd = support_wide_result.support - simplex.b.point.support;
            let cd = support_wide_result.support - simplex.c.point.support;
            let abc_normal = Vec3x4::cross(ab, ca);
            let toot_bird_point_to_d = support_wide_result.support - toot_bird_point;
            //the nx_offset is magic
            let nx_offset = abc_normal.cross(toot_bird_point_to_d);
            let ad_plane_test = Vec3x4::dot(nx_offset, ad);
            let bd_plane_test = Vec3x4::dot(nx_offset, bd);
            let cd_plane_test = Vec3x4::dot(nx_offset, cd);

            let use_abd = ad_plane_test.ge(f32x4::ZERO) & bd_plane_test.lt(f32x4::ZERO);
            let use_bcd = bd_plane_test.ge(f32x4::ZERO) & cd_plane_test.lt(f32x4::ZERO);
            let use_cad = cd_plane_test.ge(f32x4::ZERO) & ad_plane_test.lt(f32x4::ZERO);

            // if tootbird point is not projected into the triangle,then choose abd
            let use_abd = use_abd.select(use_abd | use_bcd | use_cad, bool32x4::TRUE);
            simplex
                .a
                .write_by_condition(use_bcd & is_triangle, support_wide_result);
            simplex
                .b
                .write_by_condition(use_cad & is_triangle, support_wide_result);
            simplex
                .c
                .write_by_condition(use_abd & is_triangle, support_wide_result);
        }
    } else {
        let a = simplex.a.point;
        simplex.update_if_not_exists(a, *terminate);
    }
    // so we have the new simplex,then we need to calculate the new normal
    // and degenerate the simplex in some case
    // in the triangle case we need to calculate the barycentric coordinate of the tootbird point
    // projected into the triangle in the line case we need to calculate the barycentric
    // coordinate of the tootbird point projected into the line in the point case, we need check
    // the distance from tootbird to the point,if the distance is less than the
    // search_threshold,then terminate
    // in the wide case,we will do all the above

    // calculate the barycentric coordinate ,but not the value bewteen 0 and 1
    let ab = simplex.b.point.support - simplex.a.point.support;
    let ca = simplex.a.point.support - simplex.c.point.support;
    let bc = simplex.c.point.support - simplex.b.point.support;
    let mut triangle_normal = Vec3x4::cross(ab, ca);
    let triangle_normal_length_squared = triangle_normal.length_squared();
    let tootbird_to_a = simplex.a.point.support - toot_bird_point;
    let tootbird_to_b = simplex.b.point.support - toot_bird_point;
    let ab_tootbird_area_value = Vec3x4::cross(ab, tootbird_to_a).dot(triangle_normal);
    let bc_tootbird_area_value = Vec3x4::cross(bc, tootbird_to_b).dot(triangle_normal);
    let ca_tootbird_area_value =
        triangle_normal_length_squared - ab_tootbird_area_value - bc_tootbird_area_value;

    // so the barycentric coordinate is prepared above,then we need to check it is a segment or a
    // point ;if it is a point ,the area of the triangle and the length of each segment will be
    // nearly zero ;if it is a segment, the area of the triangle will be nearly zero
    let ab_test_not_valid = ab_tootbird_area_value.lt(f32x4::ZERO);
    let bc_test_not_valid = bc_tootbird_area_value.lt(f32x4::ZERO);
    let ca_test_not_valid = ca_tootbird_area_value.lt(f32x4::ZERO);
    let ab_length_squared = ab.length_squared();
    let bc_length_squared = bc.length_squared();
    let ca_length_squared = ca.length_squared();
    let longest_edge_length_squared = ab_length_squared
        .max(bc_length_squared)
        .max(ca_length_squared);
    let is_a_point = longest_edge_length_squared.lt(f32x4::splat(1e-14));
    let is_a_segment = triangle_normal_length_squared.lt(f32x4::EPSILON) & !is_a_point;
    let need_negative = triangle_normal.dot(toot_bird.normal).lt(f32x4::ZERO);
    triangle_normal = Vec3x4::lane_select(need_negative, -triangle_normal, triangle_normal);
    //we will calculate 6 cases in the wide
    // 1. triangle case,and it's terminate case
    // 2. segment case,and it's terminate case
    // 3. point case,and it's terminate case
    // the point case weight will be deal out of the function by depth check

    // if terminate happens out of the function,we deal with point case weight
    simplex.a.weight = simplex.a.weight.select(*terminate, f32x4::ONE);
    simplex.b.weight = simplex.b.weight.select(*terminate, f32x4::ZERO);
    simplex.c.weight = simplex.c.weight.select(*terminate, f32x4::ZERO);
    simplex.weight_denominator = simplex.weight_denominator.select(*terminate, f32x4::ONE);

    //if point a is valid,then feature will bitor 1
    //if point b is valid,then feature will bitor 2
    //if point c is valid,then feature will bitor 4

    //in point case the normal is the vector from a to tootbird,and feature is 1
    let mut next_un_normalized_normal = -tootbird_to_a;
    let mut features = i32x4::ONE;

    // the point terminate case
    let target_to_a_length_squared = tootbird_to_a.length_squared();
    *terminate =
        *terminate | (is_a_point & (target_to_a_length_squared.lt(terminate_threshold_squared)));
    let target_outside_triangle_edges = ab_test_not_valid | bc_test_not_valid | ca_test_not_valid;
    // the segment  case
    let is_segment_case = (is_a_segment | target_outside_triangle_edges) & !*terminate;
    if is_segment_case.any() {
        //choose the segment that shortest distance from tootbird to the segment
        //for example, use the ab segment
        //we need to calculate the project point of tootbird on the ab segment p
        //and the distance from tootbird to p
        //so p = a + t * ab
        //t = (tootbird - a).dot(ab) / ab.length_squared() and t is in [0,1]
        //and the distance is |tootbird - p|

        // let o= tootbird;
        // ab
        let ao = tootbird_to_a;
        let bo = tootbird_to_b;
        let oc = toot_bird_point - simplex.c.point.support;
        let ao_dot_ab = ao.dot(ab);
        let t_ab = (-ao_dot_ab) / ab_length_squared;
        let t_ab = t_ab.max(f32x4::ZERO).min(f32x4::ONE);
        let p_ab = simplex.a.point.support + ab * t_ab;

        let bo_dot_bc = bo.dot(bc);
        let t_bc = (-bo_dot_bc) / bc_length_squared;
        let t_bc = t_bc.max(f32x4::ZERO).min(f32x4::ONE);
        let p_bc = simplex.b.point.support + bc * t_bc;

        let oc_dot_ca = oc.dot(ca);
        let t_ca = (oc_dot_ca) / ca_length_squared;
        let t_ca = t_ca.max(f32x4::ZERO).min(f32x4::ONE);
        let p_ca = simplex.c.point.support + ca * t_ca;
        let o_ab_distance_squared = (toot_bird_point - p_ab).length_squared();
        let o_bc_distance_squared = (toot_bird_point - p_bc).length_squared();
        let o_ca_distance_squared = (toot_bird_point - p_ca).length_squared();

        let bc_degenerate = bc_length_squared.eq(f32x4::ZERO);
        let ca_degenerate = ca_length_squared.eq(f32x4::ZERO);

        let ab_closer_than_bc = bc_degenerate | o_ab_distance_squared.lt(o_bc_distance_squared);
        let ab_closer_than_ca = ca_degenerate | o_ab_distance_squared.lt(o_ca_distance_squared);
        let bc_closer_than_ca = ca_degenerate | o_bc_distance_squared.lt(o_ca_distance_squared);
        let use_ab = ab_closer_than_bc & ab_closer_than_ca;
        let use_bc = bc_closer_than_ca & (!use_ab);
        let best_distance_squared = o_ab_distance_squared.select(
            use_ab,
            o_bc_distance_squared.select(use_bc, o_ca_distance_squared),
        );
        *terminate =
            *terminate | (is_segment_case & best_distance_squared.le(terminate_threshold_squared));
        let t = t_ab.select(use_ab, t_bc.select(use_bc, t_ca));
        let p = Vec3x4::lane_select(use_ab, p_ab, p_ca);
        let p = Vec3x4::lane_select(use_bc, p_bc, p);
        let segment_normal = toot_bird_point - p;
        let origin_nearest_start = t.eq(f32x4::ZERO);
        let origin_nearest_end = t.eq(f32x4::ONE);
        let feature_for_ab = i32x4::ONE.select(
            origin_nearest_start,
            i32x4::splat(2).select(origin_nearest_end, i32x4::splat(1 + 2)),
        );
        let feature_for_bc = i32x4::splat(2).select(
            origin_nearest_start,
            i32x4::splat(4).select(origin_nearest_end, i32x4::splat(2 + 4)),
        );
        let feature_for_ca = i32x4::splat(4).select(
            origin_nearest_start,
            i32x4::splat(1).select(origin_nearest_end, i32x4::splat(4 + 1)),
        );
        features = (feature_for_ab.select(use_ab, feature_for_bc.select(use_bc, feature_for_ca)))
            .select(is_segment_case, features);
        next_un_normalized_normal =
            Vec3x4::lane_select(is_segment_case, segment_normal, next_un_normalized_normal);
        let weight_edge_start = f32x4::ONE - t;
        simplex.a.weight = (weight_edge_start.select(use_ab, f32x4::ZERO.select(use_bc, t)))
            .select(is_segment_case, simplex.a.weight);
        simplex.b.weight = (t.select(use_ab, weight_edge_start.select(use_bc, f32x4::ZERO)))
            .select(is_segment_case, simplex.b.weight);
        simplex.c.weight = (f32x4::ZERO.select(use_ab, t.select(use_bc, weight_edge_start)))
            .select(is_segment_case, simplex.c.weight);
    }
    //the triangle case
    let tootbird_projected_on_triangle =
        (!is_a_segment) & (!is_a_point) & (!target_outside_triangle_edges) & (!*terminate);
    if tootbird_projected_on_triangle.any() {
        next_un_normalized_normal = Vec3x4::lane_select(
            tootbird_projected_on_triangle,
            triangle_normal,
            next_un_normalized_normal,
        );
        //check the distance from tootbird to the triangle is less than the search_threshold
        let toot_bird_to_a_dot = tootbird_to_a.dot(triangle_normal);
        let can_terminate = (toot_bird_to_a_dot * toot_bird_to_a_dot)
            .le(terminate_threshold_squared * triangle_normal_length_squared);
        *terminate = *terminate | (can_terminate & tootbird_projected_on_triangle);
        // a b c are all valid
        features = i32x4::splat(1 + 2 + 4).select(tootbird_projected_on_triangle, features);
        simplex.a.weight =
            bc_tootbird_area_value.select(tootbird_projected_on_triangle, simplex.a.weight);
        simplex.b.weight =
            ca_tootbird_area_value.select(tootbird_projected_on_triangle, simplex.b.weight);
        simplex.c.weight =
            ab_tootbird_area_value.select(tootbird_projected_on_triangle, simplex.c.weight);
        simplex.weight_denominator = triangle_normal_length_squared
            .select(tootbird_projected_on_triangle, simplex.weight_denominator);
    }
    simplex.a.exists = (features & i32x4::ONE).ne(i32x4::ZERO);
    simplex.b.exists = (features & i32x4::splat(2)).ne(i32x4::ZERO);
    simplex.c.exists = (features & i32x4::splat(4)).ne(i32x4::ZERO);

    if !terminate.all() {
        // the feature is not correct in small mesh case
        if push_normal_policy {
            let triangle_to_tootbird = next_un_normalized_normal;
            let push_offset = triangle_to_tootbird * f32x4::splat(4.0);
            let push_normal_candidate = toot_bird_point + push_offset;
            next_un_normalized_normal = Vec3x4::lane_select(
                toot_bird.depth.le(f32x4::ZERO) | tootbird_projected_on_triangle,
                next_un_normalized_normal,
                push_normal_candidate,
            );
        }
        // No active lanes can have a zero length targetToTriangle, so we can normalize safely.
        *normal = next_un_normalized_normal
            .normalize_or(Vec3x4::Y, EPS_6)
            .as_unit_vec3x4_unchecked();
    }
}

fn support_wide(
    a: &impl MinkowskiSupportWide,
    b: &impl MinkowskiSupportWide,
    transform_b_2_a: &impl TransformativeWide,
    direction: Vec3x4,
    shape_container: Option<&ShapeContainer>,
) -> SupportWideResult {
    // Support(N, A) - Support(-N, B)
    let support_a = a.support_point_local(direction, shape_container).point;
    let support_b = b
        .support_point(-direction, transform_b_2_a.orientation(), shape_container)
        .point
        + transform_b_2_a.offset();
    SupportWideResult::new(support_a - support_b, support_a.as_vec3x4())
}

#[derive(Debug)]
pub struct TootBirdWideResult {
    pub depth: f32x4,
    pub normal: UnitVec3x4,
    pub closest_point_on_a: Vec3x4,
}

impl TootBirdWideResult {
    #[inline]
    pub fn new(depth: f32x4, normal: UnitVec3x4, closest_point_on_a: Vec3x4) -> Self {
        Self {
            depth,
            normal,
            closest_point_on_a,
        }
    }
}

pub struct IterContext<'a> {
    inactive_lanes: bool32x4,
    depth_threshold: f32x4,  // use to stop in precheck state of tootbird
    search_threshold: f32x4, // use to stop in iteration state of tootbird
    maximum_iterations: usize,
    shape_container: Option<&'a ShapeContainer>,
    push_normal_policy: bool,
}

impl<'a> IterContext<'a> {
    #[inline]
    #[must_use]
    pub fn new(
        inactive_lanes: bool32x4,
        search_threshold: f32x4,
        depth_threshold: f32x4,
        maximum_iterations: usize,
        shape_container: Option<&'a ShapeContainer>,
        push_normal_policy: bool,
    ) -> Self {
        Self {
            inactive_lanes,
            depth_threshold,
            search_threshold,
            maximum_iterations,
            shape_container,
            push_normal_policy,
        }
    }
}

/// - a tootbird is just a point in the minkowski space
/// - and the struct here store the depth and normal that can be used to calculate the tootbird
///   point
/// - this can be caulated faster
struct TootBird {
    normal: UnitVec3x4,
    depth: f32x4,
}

impl TootBird {
    #[inline]
    pub fn new(normal: UnitVec3x4, depth: f32x4) -> Self {
        Self { normal, depth }
    }

    #[inline]
    pub fn update(&mut self, normal: UnitVec3x4, depth: f32x4, terminate: bool32x4) {
        let new_normal_is_better = depth.lt(self.depth) & !terminate;
        self.depth = depth.select(new_normal_is_better, self.depth);
        self.normal = UnitVec3x4::lane_select(new_normal_is_better, normal, self.normal);
    }

    #[inline]
    pub fn point(&self) -> Vec3x4 {
        //in separate case, let the tootbird point be the origin
        self.normal * self.depth.max(f32x4::ZERO)
    }
}

#[derive(Clone, Copy)]
struct SupportWideResult {
    // support = support_a - support_b
    support: Vec3x4,
    support_in_a_local_space: Vec3x4,
}
impl SupportWideResult {
    pub fn new(support: Vec3x4, support_in_a_local_space: Vec3x4) -> Self {
        Self {
            support,
            support_in_a_local_space,
        }
    }

    #[inline]
    pub fn select(self, condition: bool32x4, other: Self) -> Self {
        Self {
            support: Vec3x4::lane_select(condition, self.support, other.support),
            support_in_a_local_space: Vec3x4::lane_select(
                condition,
                self.support_in_a_local_space,
                other.support_in_a_local_space,
            ),
        }
    }
}
struct Vertex {
    point: SupportWideResult,
    weight: f32x4,
    exists: bool32x4,
}

impl Vertex {
    #[inline]
    pub fn write_if_not_exists(&mut self, point: SupportWideResult, terminated_lane: bool32x4) {
        let cant_write = self.exists | terminated_lane;
        self.point = self.point.select(cant_write, point);
        self.exists = self.exists.select(cant_write, bool32x4::TRUE);
    }

    #[inline]
    pub fn write_by_condition(&mut self, condition: bool32x4, point: SupportWideResult) {
        self.point = point.select(condition, self.point);
        self.exists = self.exists | condition;
    }
}

struct Simplex {
    pub a: Vertex,
    pub b: Vertex,
    pub c: Vertex,
    pub weight_denominator: f32x4,
}

impl Simplex {
    #[inline]
    fn new(support_point: SupportWideResult) -> Self {
        Self {
            a: Vertex {
                point: support_point,
                weight: f32x4::ONE,
                exists: bool32x4::TRUE,
            },
            b: Vertex {
                point: support_point,
                weight: f32x4::ONE,
                exists: bool32x4::FALSE,
            },
            c: Vertex {
                point: support_point,
                weight: f32x4::ONE,
                exists: bool32x4::FALSE,
            },
            weight_denominator: f32x4::ONE,
        }
    }

    #[inline]
    fn is_triangle(&self) -> bool32x4 {
        self.a.exists & self.b.exists & self.c.exists
    }

    /// - if the new point is not exists,then write it to the vertex
    /// - if the new point is existing,then do nothing
    /// - so if the b and c point are not exist ,then b and c will be the same point of input
    #[inline]
    fn update_if_not_exists(&mut self, new_point: SupportWideResult, terminated_lane: bool32x4) {
        self.a.write_if_not_exists(new_point, terminated_lane);
        self.b.write_if_not_exists(new_point, terminated_lane);
        self.c.write_if_not_exists(new_point, terminated_lane);
    }
}