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 glam_det::{Dot, Isometry3, Point3, Vec3};

use crate::minkowski::epa::ContactInfo;
use crate::minkowski::simplex::{PointSize, Simplex};
use crate::traits::MinkowskiSupport;

#[inline]
fn gjk_init(init_dir: Vec3) -> (Simplex, Vec3, Vec3) {
    let simplex = Simplex::default();
    let direction = if init_dir == Vec3::ZERO {
        Vec3::X
    } else {
        init_dir
    };
    let support_point = Vec3::ZERO;
    (simplex, direction, support_point)
}

pub fn gjk_intersection(
    shape_a: &impl MinkowskiSupport,
    transform_a: Isometry3,
    shape_b: &impl MinkowskiSupport,
    transform_b: Isometry3,
    init_dir: Vec3,
    contact_tolerance: f32,
) -> (bool, Simplex) {
    let mut iter = GjkIter::new(
        shape_a,
        transform_a,
        shape_b,
        transform_b,
        init_dir,
        contact_tolerance,
    );
    loop {
        match iter.intersection_next() {
            GjkResult::Continue => {}
            GjkResult::Intersect => {
                return (true, iter.simplex());
            }
            GjkResult::NoIntersect | GjkResult::Close => {
                return (false, iter.simplex());
            }
        }
    }
}

pub(super) fn gjk_penetration<A, B>(
    shape_a: &A,
    transform_a: Isometry3,
    shape_b: &B,
    transform_b: Isometry3,
    init_dir: Vec3,
    contact_tolerance: f32,
) -> PenetrationResult
where
    A: MinkowskiSupport,
    B: MinkowskiSupport,
{
    let mut iter = GjkIter::new(
        shape_a,
        transform_a,
        shape_b,
        transform_b,
        init_dir,
        contact_tolerance,
    );
    loop {
        let result = iter.penetration_next();
        match result {
            PenetrationResult::Continue => {}
            _ => {
                return result;
            }
        }
    }
}

#[derive(Debug)]
pub(super) struct SimplexInfo {
    pub support_points_a: [Vec3; 4],
    pub support_points_b: [Vec3; 4],
    pub size: PointSize,
}

#[derive(Debug)]
#[repr(u8)]
pub(super) enum GjkResult {
    Continue,
    Intersect,
    NoIntersect,
    Close,
}

#[derive(Debug)]
#[repr(u8)]
pub(super) enum PenetrationResult {
    Continue,
    GjkIntersectWithContactInfo(ContactInfo),
    GjkDegenerate(ContactInfo),
    EpaIntersectWithSimplexInfo(SimplexInfo),
    NoIntersect,
}

impl GjkResult {
    #[inline]
    #[allow(dead_code)]
    pub fn discriminant(&self) -> u8 {
        unsafe { *<*const _>::from(self).cast::<u8>() }
    }
}

impl PenetrationResult {
    #[inline]
    #[allow(dead_code)]
    pub fn discriminant(&self) -> u8 {
        unsafe { *<*const _>::from(self).cast::<u8>() }
    }
}

pub struct GjkIter<'a, A, B> {
    shape_a: &'a A,
    transform_a: Isometry3,
    shape_b: &'a B,
    transform_b: Isometry3,
    support_point: Vec3,
    simplex: Simplex,
    direction: Vec3,
    last_closest: Vec3,
    last_distance: f32,
    contact_tolerance: f32,
}

impl<'a, A, B> GjkIter<'a, A, B>
where
    A: MinkowskiSupport,
    B: MinkowskiSupport,
{
    #[inline]
    pub fn simplex(self) -> Simplex {
        self.simplex
    }

    // gjk test use it
    #[inline]
    #[allow(dead_code)]
    pub fn simplex_ref(&self) -> &Simplex {
        &self.simplex
    }

    pub fn new(
        shape_a: &'a A,
        transform_a: Isometry3,
        shape_b: &'a B,
        transform_b: Isometry3,
        init_dir: Vec3,
        contact_tolerance: f32,
    ) -> Self {
        let (simplex, direction, support_point) = gjk_init(init_dir);
        let last_distance = f32::MAX;
        Self {
            shape_a,
            transform_a,
            shape_b,
            transform_b,
            support_point,
            simplex,
            direction,
            last_closest: Vec3::ZERO,
            last_distance,
            contact_tolerance,
        }
    }

    pub(super) fn intersection_next(&mut self) -> GjkResult {
        let support_a = self
            .shape_a
            .support_point(self.direction, &self.transform_a);
        let support_b = self
            .shape_b
            .support_point(-self.direction, &self.transform_b);
        self.support_point = support_a.point - support_b.point;
        let signed_distance = self.support_point.dot(self.direction.normalize());

        let eps_percentage = 0.000225_f32; // square of 1.5%
        let less_error = 1f32 - eps_percentage;
        if signed_distance < -self.contact_tolerance {
            return GjkResult::NoIntersect;
        }
        if signed_distance < 0.0f32 && -signed_distance > less_error * self.last_distance {
            return GjkResult::Close;
        }
        self.simplex.push(
            self.support_point,
            support_a.point.as_vec3(),
            support_b.point.as_vec3(),
            support_a.point_index,
            support_b.point_index,
        );
        let (closest, next_normal) = self.simplex.closest_simplex();
        let now_distance = closest.length();
        if closest == Vec3::ZERO {
            return GjkResult::Intersect;
        }
        if now_distance < self.contact_tolerance {
            self.last_distance = now_distance;
            return GjkResult::Intersect;
        }
        if self.last_distance < now_distance {
            return if self.last_distance < self.contact_tolerance {
                GjkResult::Intersect
            } else {
                GjkResult::Close
            };
        }

        self.direction = next_normal;

        self.last_distance = now_distance;
        GjkResult::Continue
    }

    pub(super) fn penetration_next(&mut self) -> PenetrationResult {
        let support_a = self
            .shape_a
            .support_point(self.direction, &self.transform_a);
        let support_b = self
            .shape_b
            .support_point(-self.direction, &self.transform_b);
        self.support_point = support_a.point - support_b.point;
        let normal = self.direction.normalize_to_unit();
        let signed_distance = (self.support_point).dot(normal);

        let eps_percentage = 0.000225_f32; // square of 1.5%
        let less_error = 1f32 - eps_percentage;
        if signed_distance < -self.contact_tolerance {
            return PenetrationResult::NoIntersect;
        }
        if -signed_distance > less_error * self.last_distance {
            // intersect in margin
            let last_closest = -self.direction;
            let (a, b) = self.simplex.get_closest_point(last_closest);
            return PenetrationResult::GjkIntersectWithContactInfo(ContactInfo {
                normal,
                depth: self.last_distance,
                point_a: Point3::from_vec3(a),
                point_b: Point3::from_vec3(b),
            });
        }
        self.simplex.push(
            self.support_point,
            support_a.point.as_vec3(),
            support_b.point.as_vec3(),
            support_a.point_index,
            support_b.point_index,
        );
        let (closest, next_normal) = self.simplex.closest_simplex();
        let now_distance = closest.length();

        if now_distance <= self.contact_tolerance {
            return PenetrationResult::EpaIntersectWithSimplexInfo(SimplexInfo {
                support_points_a: self.simplex.support_points_a(),
                support_points_b: self.simplex.support_points_b(),
                size: self.simplex.size(),
            });
        }
        if self.last_distance <= now_distance {
            // degenerate case
            let last_normal = (self.last_closest / self.last_distance).as_unit_vec3_unchecked();
            let (a, b) = self.simplex.get_closest_point(self.last_closest);
            return PenetrationResult::GjkDegenerate(ContactInfo {
                normal: last_normal,
                depth: now_distance,
                point_a: Point3::from_vec3(a),
                point_b: Point3::from_vec3(b),
            });
        }
        self.direction = next_normal;
        self.last_distance = now_distance;
        self.last_closest = closest;
        PenetrationResult::Continue
    }
}

#[cfg(test)]
mod tests {

    use glam_det::{UnitQuat, Vec3};
    use wasm_bindgen_test::{wasm_bindgen_test, wasm_bindgen_test_configure};

    use crate::minkowski::epa::ContactInfo;
    use crate::minkowski::gjk::{
        gjk_init, gjk_intersection, GjkIter, GjkResult, PenetrationResult,
    };
    use crate::Cuboid;
    wasm_bindgen_test_configure!(run_in_browser);

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

        let shape_a = Cuboid::new_xyz(0.5, 0.5f32, 0.5);
        let shape_b = Cuboid::new_xyz(0.5, 0.5f32, 0.5);
        let contact_tolerance = 0.0001f32;
        let transform_a = glam_det::Isometry3::default();
        let transform_b = glam_det::Isometry3 {
            translation: Vec3::new(0.6, 0.0, 0.0),
            rotation: UnitQuat::from_rotation_y(0.0f32.to_radians()),
        };
        let init_vertex_a = transform_a.translation;
        let init_vertex_b = transform_b.translation;
        let init_dir = init_vertex_b - init_vertex_a; // ab
        let (intersect, _closest_simplex) = gjk_intersection(
            &shape_a,
            transform_a,
            &shape_b,
            transform_b,
            init_dir,
            contact_tolerance,
        );
        assert!(!intersect);
    }

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

        let shape_a = Cuboid::new_xyz(0.5, 0.5f32, 0.5);
        let shape_b = Cuboid::new_xyz(0.5, 0.5f32, 0.5);
        let contact_tolerance = 0.0001f32;
        let transform_a = glam_det::Isometry3::default();
        let transform_b = glam_det::Isometry3 {
            translation: Vec3::new(0.5 + contact_tolerance * 0.99f32, 0.0, 0.0),
            rotation: UnitQuat::from_rotation_y(0.0f32.to_radians()),
        };
        let init_vertex_a = transform_a.translation;
        let init_vertex_b = transform_b.translation;
        let init_dir = init_vertex_b - init_vertex_a; // ab
        let (intersect, _closest_simplex) = gjk_intersection(
            &shape_a,
            transform_a,
            &shape_b,
            transform_b,
            init_dir,
            contact_tolerance,
        );
        assert!(intersect);
    }
    #[test]
    #[wasm_bindgen_test]
    fn test3() {
        let _ = env_logger::builder().is_test(true).try_init();

        let shape_a = Cuboid::new_xyz(0.5, 0.5f32, 0.5);
        let shape_b = Cuboid::new_xyz(0.5, 0.5f32, 0.5);
        let contact_tolerance = 0.0001f32;
        let transform_a = glam_det::Isometry3::default();
        let transform_b = glam_det::Isometry3 {
            translation: Vec3::new(0.5 + contact_tolerance, 0.0, 0.0),
            rotation: UnitQuat::from_rotation_y(0.0f32.to_radians()),
        };
        let init_vertex_a = transform_a.translation;
        let init_vertex_b = transform_b.translation;
        let init_dir = init_vertex_b - init_vertex_a; // ab
        let (intersect, _closest_simplex) = gjk_intersection(
            &shape_a,
            transform_a,
            &shape_b,
            transform_b,
            init_dir,
            contact_tolerance,
        );
        assert!(!intersect);
    }
    #[test]
    #[wasm_bindgen_test]
    fn test4() {
        let _ = env_logger::builder().is_test(true).try_init();

        let shape_a = Cuboid::new_xyz(0.5, 0.5f32, 0.5);
        let shape_b = Cuboid::new_xyz(0.5, 0.5f32, 0.5);
        let contact_tolerance = 0.0001f32;
        let transform_a = glam_det::Isometry3::default();
        let transform_b = glam_det::Isometry3 {
            translation: Vec3::new(0.5 + contact_tolerance, 0.0, 0.0),
            rotation: UnitQuat::from_rotation_y(45.0f32.to_radians()),
        };
        let init_vertex_a = transform_a.translation;
        let init_vertex_b = transform_b.translation;
        let init_dir = init_vertex_b - init_vertex_a; // ab
        let (intersect, _closest_simplex) = gjk_intersection(
            &shape_a,
            transform_a,
            &shape_b,
            transform_b,
            init_dir,
            contact_tolerance,
        );
        assert!(intersect);
    }
    #[test]
    #[wasm_bindgen_test]
    fn test5() {
        let _ = env_logger::builder().is_test(true).try_init();

        let shape_a = Cuboid::new_xyz(0.5, 0.5f32, 0.5);
        let shape_b = Cuboid::new_xyz(0.5, 0.5f32, 0.5);
        let contact_tolerance = 0.0001f32;
        let transform_a = glam_det::Isometry3::default();
        let transform_b = glam_det::Isometry3 {
            translation: Vec3::new(0.0, 0.0, 0.0),
            rotation: UnitQuat::from_rotation_y(0.0f32.to_radians()),
        };
        let init_vertex_a = transform_a.translation;
        let init_vertex_b = transform_b.translation;
        let init_dir = init_vertex_b - init_vertex_a; // ab
        let (intersect, _closest_simplex) = gjk_intersection(
            &shape_a,
            transform_a,
            &shape_b,
            transform_b,
            init_dir,
            contact_tolerance,
        );
        assert!(intersect);
    }

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

        let shape_a = Cuboid::new_xyz(1.0, 1.0, 1.0);
        let shape_b = Cuboid::new_xyz(1.0, 1.0, 1.0);
        let contact_tolerance = 0.0001f32;
        let transform_a = glam_det::Isometry3::from_translation(Vec3::new(0.0, 0.0, 0.0));
        let transform_b = glam_det::Isometry3 {
            translation: Vec3::new(-0.5, 0.75, 1.0),
            rotation: UnitQuat::from_rotation_x(45.0f32.to_radians()),
        };
        let init_vertex_a = transform_a.translation;
        let init_vertex_b = transform_b.translation;
        let init_dir = init_vertex_b - init_vertex_a; // ab
        let (intersect, _closest_simplex) = gjk_intersection(
            &shape_a,
            transform_a,
            &shape_b,
            transform_b,
            init_dir,
            contact_tolerance,
        );
        assert!(!intersect);
    }
    #[test]
    #[wasm_bindgen_test]
    fn test7() {
        let _ = env_logger::builder().is_test(true).try_init();

        let shape_a = Cuboid::new_xyz(1.0, 1.0, 1.0);
        let shape_b = Cuboid::new_xyz(1.0, 1.0, 1.0);
        let contact_tolerance = 0.0001f32;
        let transform_a = glam_det::Isometry3::from_translation(Vec3::new(0.0, 0.0, 0.0));
        let transform_b = glam_det::Isometry3 {
            translation: Vec3::new(-0.5, 0.75, 1.0),
            rotation: UnitQuat::from_rotation_x(45.0f32.to_radians()),
        };
        let init_vertex_a = transform_a.translation;
        let init_vertex_b = transform_b.translation;
        let init_dir = init_vertex_b - init_vertex_a; // ab
        let (simplex, direction, support_point) = gjk_init(init_dir);
        let mut last_distance = 0.1f32;
        let mut iter = GjkIter {
            shape_a: &shape_a,
            transform_a,
            shape_b: &shape_b,
            transform_b,
            support_point,
            simplex: simplex.clone(),
            direction,
            last_closest: Vec3::default(),
            last_distance,
            contact_tolerance,
        };
        let status = iter.intersection_next();
        assert_eq!(status.discriminant(), GjkResult::Close.discriminant());
        last_distance = contact_tolerance * 0.5f32;
        let mut iter = GjkIter {
            shape_a: &shape_a,
            transform_a,
            shape_b: &shape_b,
            transform_b,
            support_point,
            simplex,
            direction,
            last_closest: Vec3::default(),
            last_distance,
            contact_tolerance,
        };
        let status = iter.intersection_next();
        assert_eq!(status.discriminant(), GjkResult::Intersect.discriminant());
        last_distance = contact_tolerance * 0.9f32 * 0.5f32;
        let transform_a = glam_det::Isometry3::from_translation(Vec3::new(0.0, 0.0, 0.0));
        let transform_b = glam_det::Isometry3 {
            translation: Vec3::new(-0.0, 0.0, 1.0 + contact_tolerance * 0.9f32),
            rotation: UnitQuat::from_rotation_x(0.0f32.to_radians()),
        };
        let init_vertex_a = transform_a.translation;
        let init_vertex_b = transform_b.translation;
        let init_dir = init_vertex_b - init_vertex_a; // ab
        let (simplex, direction, support_point) = gjk_init(init_dir);
        let mut iter = GjkIter {
            shape_a: &shape_a,
            transform_a,
            shape_b: &shape_b,
            transform_b,
            support_point,
            simplex,
            direction,
            last_closest: Vec3::default(),
            last_distance,
            contact_tolerance,
        };
        let status = iter.intersection_next();
        assert_eq!(status.discriminant(), GjkResult::Close.discriminant());
    }

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

        let shape_a = Cuboid::new_xyz(1.0, 1.0, 1.0);
        let shape_b = Cuboid::new_xyz(1.0, 1.0, 1.0);
        let contact_tolerance = 0.0001f32;
        let transform_a = glam_det::Isometry3::from_translation(Vec3::new(0.0, 0.0, 0.0));
        let transform_b = glam_det::Isometry3 {
            translation: Vec3::new(0.0, 0.0, 1.0 + contact_tolerance * 0.9f32),
            rotation: UnitQuat::from_rotation_x(0.0f32.to_radians()),
        };
        let init_vertex_a = transform_a.translation;
        let init_vertex_b = transform_b.translation;
        let init_dir = init_vertex_b - init_vertex_a; // ab
        let (mut simplex, direction, support_point) = gjk_init(init_dir);

        simplex.push(Vec3::ZERO, Vec3::ZERO, Vec3::ZERO, 0, 0);
        let mut iter = GjkIter {
            shape_a: &shape_a,
            transform_a,
            shape_b: &shape_b,
            transform_b,
            support_point,
            simplex: simplex.clone(),
            direction,
            last_closest: Vec3::default(),
            last_distance: 0.0,
            contact_tolerance,
        };
        let status = iter.penetration_next();
        assert_eq!(
            status.discriminant(),
            PenetrationResult::GjkIntersectWithContactInfo(ContactInfo::default()).discriminant()
        );
    }
}