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::nums::Signed;
use glam_det::{Cross, Dot, Vec3};
use strum_macros::FromRepr;

use crate::geometric_tools::closest_point_to_triangle;

#[derive(Clone, Debug)]
pub struct Simplex {
    points: [Vec3; 4],
    support_points_a: [Vec3; 4],
    support_points_b: [Vec3; 4],
    indices_a: [usize; 4],
    indices_b: [usize; 4],
    size: PointSize,
}

#[derive(Clone, Copy, Debug, PartialEq, FromRepr)]
pub enum PointSize {
    Zero,
    One,
    Two,
    Three,
    Four,
}

impl PointSize {
    #[inline]
    pub fn as_usize(self) -> usize {
        self as usize
    }

    #[inline]
    pub fn increase(&mut self) {
        let value = *self as usize + 1;
        *self =
            Self::from_repr(value).expect("Cannot increase the size of a simplex beyond 4 points.");
    }
}

impl Simplex {
    #[inline]
    pub fn get_point(&self, index: usize) -> Vec3 {
        self.points[index]
    }

    #[inline]
    pub fn size(&self) -> PointSize {
        self.size
    }

    #[inline]
    pub fn support_points_a(&self) -> [Vec3; 4] {
        self.support_points_a
    }

    #[inline]
    pub fn support_points_b(&self) -> [Vec3; 4] {
        self.support_points_b
    }
}

impl Default for Simplex {
    fn default() -> Self {
        Simplex {
            points: [Vec3::ZERO; 4],
            support_points_a: [Vec3::ZERO; 4],
            support_points_b: [Vec3::ZERO; 4],
            indices_a: [0; 4],
            indices_b: [0; 4],
            size: PointSize::Zero,
        }
    }
}

pub(super) struct BarycentricCoordinates {}

impl BarycentricCoordinates {
    pub fn compute_line(p: Vec3, a: Vec3, b: Vec3) -> f32 {
        let ap = p - a;
        let ab = b - a;
        let ab_len_sqr = ab.length_squared();
        let v = ap.dot(ab);
        if ab_len_sqr > 0.0 {
            v / ab_len_sqr
        } else {
            0.0
        }
    }

    /// Compute the barycentric coordinates of a point in a triangle.
    /// The coordinates are returned as a tuple (u, v, w) where u + v + w = 1.
    /// The coordinates are computed using the formula:
    /// u = ((b - c) x (p - c)) . n / (2 * area)
    /// v = ((c - a) x (p - a)) . n / (2 * area)
    /// w = ((a - b) x (p - b)) . n / (2 * area)
    /// where n is the normal of the triangle and area is the area of the triangle.
    /// #Return (v,w)
    pub fn compute_triangle(p: Vec3, a: Vec3, b: Vec3, c: Vec3) -> (f32, f32) {
        let ab = b - a;
        let ac = c - a;
        let normal = ab.cross(ac);
        let pa = a - p;
        let pb = b - p;
        let pc = c - p;
        let pb_x_pc = pb.cross(pc);
        let pc_x_pa = pc.cross(pa);
        let pa_x_pb = pa.cross(pb);
        let va = normal.dot(pb_x_pc);
        let vb = normal.dot(pc_x_pa);
        let vc = normal.dot(pa_x_pb);
        let total = va + vb + vc;
        if total > 0.0 {
            (vb / total, vc / total)
        } else {
            (0.0, 0.0)
        }
    }
}

#[derive(Clone)]
struct SimplexNoRearrangement {
    pub points: [Vec3; 3],
    pub index: [usize; 3],
    pub size: PointSize,
}

trait GetClosestTriangle {
    #[allow(dead_code)]
    fn replace_from_to(&mut self, from: usize, to: usize);
    fn get_point(&self, index: usize) -> Vec3;
    fn set_point(&mut self, index: usize, value: Vec3);
    fn set_size(&mut self, size: PointSize);
    #[inline]
    fn closest_segment(&mut self) -> Vec3 {
        let a = self.get_point(0);
        let b = self.get_point(1);
        let delta = b - a;
        let delta_len_recip = delta.length_squared().recip();

        if delta_len_recip >= f32::EPSILON.recip() {
            self.set_size(PointSize::One);
            return self.get_point(0);
        }
        let origin_p0 = -self.get_point(0);
        let t = delta.dot(origin_p0) * delta_len_recip;
        let t = t.clamp(0f32, 1f32);
        delta * t + a
    }
    /// return (`closest_point,next_normal`)
    fn closest_triangle(&mut self) -> (Vec3, Vec3) {
        let a = self.get_point(0);
        let b = self.get_point(1);
        let c = self.get_point(2);
        let ab = b - a;
        let ac = c - a;
        let mut area_abc = ab.cross(ac);

        // area is nealy zero,it's same as line
        if area_abc.length_squared() <= f32::EPSILON {
            self.set_size(PointSize::Two);
            // closest_segment has clamp,so choose the longer segment here
            if ab.length_squared() < ac.length_squared() {
                self.set_point(1, c);
            }
            let closet = self.closest_segment();
            return (closet, -closet);
        }
        let closest_info = closest_point_to_triangle(ab, ac, a, a);
        if closest_info.closest_point.dot(area_abc) > 0.0f32 {
            area_abc = -area_abc;
        }
        (closest_info.closest_point, area_abc)
    }
}

impl GetClosestTriangle for SimplexNoRearrangement {
    #[inline]
    fn replace_from_to(&mut self, from: usize, to: usize) {
        self.index[to] = self.index[from];
        self.points[to] = self.points[from];
    }

    #[inline]
    fn get_point(&self, index: usize) -> Vec3 {
        self.points[index]
    }

    #[inline]
    fn set_point(&mut self, index: usize, value: Vec3) {
        self.points[index] = value;
    }

    #[inline]
    fn set_size(&mut self, size: PointSize) {
        self.size = size;
    }
}

impl GetClosestTriangle for Simplex {
    #[inline]
    fn replace_from_to(&mut self, from: usize, to: usize) {
        self.points[to] = self.points[from];
        self.support_points_a[to] = self.support_points_a[from];
        self.support_points_b[to] = self.support_points_b[from];
        self.indices_a[to] = self.indices_a[from];
        self.indices_b[to] = self.indices_b[from];
    }

    #[inline]
    fn get_point(&self, index: usize) -> Vec3 {
        self.points[index]
    }

    #[inline]
    fn set_point(&mut self, index: usize, value: Vec3) {
        self.points[index] = value;
    }

    #[inline]
    fn set_size(&mut self, size: PointSize) {
        self.size = size;
    }
}

impl Simplex {
    #[allow(unused)]
    #[cfg(test)]
    pub fn print(&self) {
        println!("Simplex");
        let size = self.size.as_usize();
        for (i, point) in self.points.iter().take(size).enumerate() {
            println!("point{i}: {point:?}");
        }
    }

    #[inline]
    pub fn push(
        &mut self,
        support_point: Vec3,
        support_points_a: Vec3,
        support_points_b: Vec3,
        index_a: usize,
        index_b: usize,
    ) {
        self.points[self.size.as_usize()] = support_point;
        self.support_points_a[self.size.as_usize()] = support_points_a;
        self.support_points_b[self.size.as_usize()] = support_points_b;
        self.indices_a[self.size.as_usize()] = index_a;
        self.indices_b[self.size.as_usize()] = index_b;
        self.size.increase();
    }

    /// returns the closest point on the simplex to the origin
    ///
    /// # Panics
    ///
    /// May panic if the simplex size is not 1,2,3,4
    /// return (`closest_point,next_normal`)
    pub fn closest_simplex(&mut self) -> (Vec3, Vec3) {
        match self.size {
            PointSize::One => (self.points[0], -self.points[0]),
            PointSize::Two => {
                let closet = self.closest_segment();
                (closet, -closet)
            }
            PointSize::Three => self.closest_triangle(),
            PointSize::Four => self.closest_tetrahedron(),
            PointSize::Zero => {
                panic!("zero point size is not allowed")
            }
        }
    }

    ///
    /// returns: [ `is_out_side_abc` , `is_out_side_abd` , `is_out_side_acd` , `is_out_side_bcd` ]
    #[inline]
    fn get_side_info_4plane(a: Vec3, b: Vec3, c: Vec3, d: Vec3) -> [bool; 4] {
        let ab = b - a;
        let ac = c - a;
        let ad = d - a;
        let bd = d - b;
        let bc = c - b;
        let area_abc = ab.cross(ac);
        /*
        the 4 points can't on the same plane
        */
        debug_assert!(area_abc.dot(ad).absf() > f32::EPSILON);

        let area_abd = ab.cross(ad);
        let area_bcd = bc.cross(bd);
        let area_acd = ac.cross(ad);

        let signed_v_abcd = area_abc.dot(d);
        let signed_v_abdc = area_abd.dot(c);
        let signed_v_acdb = area_acd.dot(b);
        let signed_v_bcda = area_bcd.dot(a);

        let signed_v_abcd_2 = area_abc.dot(a);
        let signed_v_abdc_2 = area_abd.dot(a);
        let signed_v_acdb_2 = area_acd.dot(a);
        let signed_v_bcda_2 = area_bcd.dot(b);
        [
            signed_v_abcd * signed_v_abcd_2 > 0f32,
            signed_v_abdc * signed_v_abdc_2 > 0f32,
            signed_v_acdb * signed_v_acdb_2 > 0f32,
            signed_v_bcda * signed_v_bcda_2 > 0f32,
        ]
    }

    #[inline]
    fn modify_simplex_no_arrangement(
        &self,
        data: &mut SimplexNoRearrangement,
        index0: usize,
        index1: usize,
        index2: usize,
    ) {
        data.points = [
            self.points[index0],
            self.points[index1],
            self.points[index2],
        ];
        data.index = [index0, index1, index2];
        data.size = PointSize::Three;
    }

    fn closest_tetrahedron(&mut self) -> (Vec3, Vec3) {
        let a = self.points[0];
        let b = self.points[1];
        let c = self.points[2];
        let d = self.points[3];
        let ab = b - a;
        let ac = c - a;
        let ad = d - a;
        let area_abc = ab.cross(ac);
        let volume_abcd = area_abc.dot(ad);
        if volume_abcd.absf() <= f32::EPSILON {
            self.size = PointSize::Three;
            // Using larger area triangles may have better results, but the performance is not good,
            // so here we only use the closest triangle.
            return self.closest_triangle();
        }
        let outside_mask = Self::get_side_info_4plane(a, b, c, d);
        if outside_mask.iter().all(|&x| !x) {
            return (Vec3::ZERO, Vec3::ZERO);
        }

        let mut min_distance_squared = f32::MAX;
        let mut closest_point = Vec3::ZERO;
        let mut simplex_closet = SimplexNoRearrangement {
            points: [Vec3::ZERO; 3],
            index: [0; 3],
            size: PointSize::Zero,
        };
        let mut simplex_temp = simplex_closet.clone();
        if outside_mask[0] {
            // abc
            self.modify_simplex_no_arrangement(&mut simplex_temp, 0, 1, 2);
            let (now_closet, _) = simplex_temp.closest_triangle();
            let len_squared = now_closet.length_squared();
            min_distance_squared = len_squared;
            closest_point = now_closet;
            simplex_closet = simplex_temp.clone();
        }

        if outside_mask[1] {
            // abd
            self.modify_simplex_no_arrangement(&mut simplex_temp, 0, 1, 3);
            let (now_closet, _) = simplex_temp.closest_triangle();
            let len_squared = now_closet.length_squared();
            if len_squared < min_distance_squared {
                min_distance_squared = len_squared;
                closest_point = now_closet;
                simplex_closet = simplex_temp.clone();
            }
        }

        if outside_mask[2] {
            // acd
            self.modify_simplex_no_arrangement(&mut simplex_temp, 0, 2, 3);
            let (now_closet, _) = simplex_temp.closest_triangle();
            let len_squared = now_closet.length_squared();
            if len_squared < min_distance_squared {
                min_distance_squared = len_squared;
                closest_point = now_closet;
                simplex_closet = simplex_temp.clone();
            }
        }
        if outside_mask[3] {
            // bcd
            self.modify_simplex_no_arrangement(&mut simplex_temp, 1, 2, 3);
            let (now_closet, _) = simplex_temp.closest_triangle();
            let len_squared = now_closet.length_squared();
            if len_squared < min_distance_squared {
                closest_point = now_closet;
                simplex_closet = simplex_temp.clone();
            }
        }
        self.size = simplex_closet.size;
        self.points[0] = simplex_closet.points[0];
        self.points[1] = simplex_closet.points[1];
        self.points[2] = simplex_closet.points[2];

        let index_a0 = self.indices_a[simplex_closet.index[0]];
        let index_a1 = self.indices_a[simplex_closet.index[1]];
        let index_a2 = self.indices_a[simplex_closet.index[2]];
        let index_b0 = self.indices_b[simplex_closet.index[0]];
        let index_b1 = self.indices_b[simplex_closet.index[1]];
        let index_b2 = self.indices_b[simplex_closet.index[2]];
        let support_point_a0 = self.support_points_a[simplex_closet.index[0]];
        let support_point_a1 = self.support_points_a[simplex_closet.index[1]];
        let support_point_a2 = self.support_points_a[simplex_closet.index[2]];
        let support_point_b0 = self.support_points_b[simplex_closet.index[0]];
        let support_point_b1 = self.support_points_b[simplex_closet.index[1]];
        let support_point_b2 = self.support_points_b[simplex_closet.index[2]];

        self.indices_a[0] = index_a0;
        self.indices_a[1] = index_a1;
        self.indices_a[2] = index_a2;

        self.indices_b[0] = index_b0;
        self.indices_b[1] = index_b1;
        self.indices_b[2] = index_b2;

        self.support_points_a[0] = support_point_a0;
        self.support_points_a[1] = support_point_a1;
        self.support_points_a[2] = support_point_a2;

        self.support_points_b[0] = support_point_b0;
        self.support_points_b[1] = support_point_b1;
        self.support_points_b[2] = support_point_b2;
        (closest_point, -closest_point)
    }

    pub(super) fn get_closest_point(&self, closest: Vec3) -> (Vec3, Vec3) {
        match self.size {
            PointSize::One => (self.support_points_a[0], self.support_points_b[0]),
            PointSize::Two => {
                let v = BarycentricCoordinates::compute_line(
                    closest,
                    self.get_point(0),
                    self.get_point(1),
                );
                let av = self.support_points_a[1] - self.support_points_a[0];
                let bv = self.support_points_b[1] - self.support_points_b[0];
                (
                    self.support_points_a[0] + av * v,
                    self.support_points_b[0] + bv * v,
                )
            }
            PointSize::Three => {
                let (v, w) = BarycentricCoordinates::compute_triangle(
                    closest,
                    self.get_point(0),
                    self.get_point(1),
                    self.get_point(2),
                );
                let av0 = self.support_points_a[1] - self.support_points_a[0];
                let av1 = self.support_points_a[2] - self.support_points_a[0];
                let bv0 = self.support_points_b[1] - self.support_points_b[0];
                let bv1 = self.support_points_b[2] - self.support_points_b[0];
                (
                    self.support_points_a[0] + av0 * v + av1 * w,
                    self.support_points_b[0] + bv0 * v + bv1 * w,
                )
            }
            _ => panic!("get_closest_point: size must be 1, 2 or 3"),
        }
    }
}

#[cfg(test)]
mod tests {
    use approx_det::assert_relative_eq;
    use glam_det::Vec3;
    use wasm_bindgen_test::*;

    use crate::minkowski::simplex::{
        BarycentricCoordinates, GetClosestTriangle, PointSize, Simplex, SimplexNoRearrangement,
    };
    wasm_bindgen_test_configure!(run_in_browser);

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

        let mut simplex = Simplex::default();
        let sa = Vec3::ZERO;
        let sb = Vec3::ONE;
        simplex.push(sa - sb, sa, sb, 0, 0);
        assert_eq!(simplex.size, PointSize::One);
        let (closest_point, _) = simplex.closest_simplex();
        assert_eq!(closest_point, Vec3::NEG_ONE);
        assert_eq!(simplex.size, PointSize::One);
    }

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

        let mut simplex = Simplex::default();
        let a = Vec3::ZERO;
        let b = Vec3::new(0f32, 1f32, 0f32);
        let c = Vec3::new(1f32, 0f32, 0f32);

        simplex.push(a, Vec3::ZERO, Vec3::ZERO, 0, 0);
        simplex.push(b, Vec3::ZERO, Vec3::ZERO, 0, 0);
        simplex.push(c, Vec3::ZERO, Vec3::ZERO, 0, 0);
        assert_eq!(simplex.size, PointSize::Three);
        let (closest_point, _) = simplex.closest_simplex();
        assert_eq!(closest_point, a);
        assert_eq!(simplex.size, PointSize::Three);
    }

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

        let mut simplex = Simplex::default();
        let a = Vec3::new(0.1f32, 0.1f32, 0f32);
        let b = Vec3::new(0f32, 1f32, 0f32);
        let c = Vec3::new(1f32, 0f32, 0f32);

        simplex.push(a, Vec3::ZERO, Vec3::ZERO, 0, 0);
        simplex.push(b, Vec3::ZERO, Vec3::ZERO, 0, 0);
        simplex.push(c, Vec3::ZERO, Vec3::ZERO, 0, 0);
        assert_eq!(simplex.size, PointSize::Three);
        let (closest_point, _) = simplex.closest_simplex();
        assert_eq!(closest_point, a);
    }

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

        let mut simplex = Simplex::default();
        let a = Vec3::new(0.5f32, 1.0f32, 0f32);
        let b = Vec3::new(0.5f32, -1f32, 0f32);
        let c = Vec3::new(1.5f32, -1f32, 0f32);

        simplex.push(a, Vec3::ZERO, Vec3::ZERO, 0, 0);
        simplex.push(b, Vec3::ZERO, Vec3::ZERO, 0, 0);
        simplex.push(c, Vec3::ZERO, Vec3::ZERO, 0, 0);
        assert_eq!(simplex.size, PointSize::Three);
        let (closest_point, _) = simplex.closest_simplex();
        assert_eq!(closest_point, Vec3::new(0.5, 0.0, 0.0));
    }

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

        let mut simplex = Simplex::default();
        let a = Vec3::new(0.5f32, 1.0f32, 0f32);
        let b = Vec3::new(0.5f32, -1f32, 0f32);
        let c = Vec3::new(1.5f32, -1f32, 1f32);

        simplex.push(a, Vec3::ZERO, Vec3::ZERO, 0, 0);
        simplex.push(b, Vec3::ZERO, Vec3::ZERO, 0, 0);
        simplex.push(c, Vec3::ZERO, Vec3::ZERO, 0, 0);
        assert_eq!(simplex.size, PointSize::Three);
        let (closest_point, _) = simplex.closest_simplex();
        assert_eq!(closest_point, Vec3::new(0.5, 0.0, 0.0));
    }

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

        let mut simplex = Simplex::default();
        let a = Vec3::new(-0.5f32, 1.0f32, 1f32);
        let b = Vec3::new(-0.5f32, -1f32, 1f32);
        let c = Vec3::new(1.5f32, -1f32, 1f32);

        simplex.push(a, Vec3::ZERO, Vec3::ZERO, 0, 0);
        simplex.push(b, Vec3::ZERO, Vec3::ZERO, 0, 0);
        simplex.push(c, Vec3::ZERO, Vec3::ZERO, 0, 0);
        assert_eq!(simplex.size, PointSize::Three);
        let (closest_point, _) = simplex.closest_simplex();
        assert_eq!(closest_point, Vec3::new(0.0, 0.0, 1.0));
        assert_eq!(simplex.size, PointSize::Three);
    }

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

        let mut simplex = Simplex::default();
        let a = Vec3::new(0.0f32, 0.0f32, 0.0f32);
        let b = Vec3::new(0f32, f32::EPSILON, 0f32);
        let c = Vec3::new(0f32, 1.0f32, 0f32);
        simplex.push(a, Vec3::ZERO, Vec3::ZERO, 0, 0);
        simplex.push(b, Vec3::ZERO, Vec3::ZERO, 0, 0);

        assert_eq!(simplex.size, PointSize::Two);
        let (closest_point, _) = simplex.closest_simplex();
        assert_eq!(closest_point, Vec3::new(0.0, 0.0, 0.0));
        assert_eq!(simplex.size, PointSize::One);
        simplex.push(c, Vec3::ZERO, Vec3::ZERO, 0, 0);
        let (closest_point, _) = simplex.closest_simplex();
        assert_eq!(closest_point, Vec3::new(0.0, 0.0, 0.0));
        assert_eq!(simplex.size, PointSize::Two);
    }

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

        let mut simplex = Simplex::default();
        let a = Vec3::new(0.1f32, 0.1f32, 1.0f32);
        let b = Vec3::new(1f32, 0.0f32, 1f32);
        let c = Vec3::new(0f32, 1.0f32, 1f32);
        simplex.push(a, Vec3::ZERO, Vec3::ZERO, 0, 0);
        simplex.push(b, Vec3::ZERO, Vec3::ZERO, 0, 0);
        simplex.push(c, Vec3::ZERO, Vec3::ZERO, 0, 0);

        assert_eq!(simplex.size, PointSize::Three);
        let (closest_point, _) = simplex.closest_simplex();
        assert_eq!(closest_point, a);
    }

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

        let mut simplex = Simplex::default();
        let a = Vec3::new(1.0f32, 0.0f32, 1.0f32);
        let b = Vec3::new(0.1f32, 0.1f32, 1.0f32);
        let c = Vec3::new(0.0f32, 1.0f32, 1.0f32);
        simplex.push(a, Vec3::ZERO, Vec3::ZERO, 0, 0);
        simplex.push(b, Vec3::ZERO, Vec3::ZERO, 0, 0);
        simplex.push(c, Vec3::ZERO, Vec3::ZERO, 0, 0);

        assert_eq!(simplex.size, PointSize::Three);
        let (closest_point, _) = simplex.closest_simplex();
        assert_relative_eq!(closest_point, b, epsilon = 0.000001_f32);
    }

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

        let mut simplex = Simplex::default();
        let a = Vec3::new(1.0f32, 0.0f32, 1.0f32);
        let b = Vec3::new(0.0f32, 1.0f32, 1.0f32);
        let c = Vec3::new(0.1f32, 0.1f32, 1.0f32);
        simplex.push(a, Vec3::ZERO, Vec3::ZERO, 0, 0);
        simplex.push(b, Vec3::ZERO, Vec3::ZERO, 0, 0);
        simplex.push(c, Vec3::ZERO, Vec3::ZERO, 0, 0);

        assert_eq!(simplex.size, PointSize::Three);
        let (closest_point, _) = simplex.closest_simplex();
        assert_relative_eq!(closest_point, c, epsilon = 0.000001_f32);
    }

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

        let mut simplex = Simplex::default();
        let offset = Vec3::new(0.1f32, -0.5f32, 0.0f32);
        let a = offset + Vec3::new(0.0f32, 0.0f32, 0.0f32);
        let b = offset + Vec3::new(1.0f32, 0.0f32, 0.0f32);
        let c = offset + Vec3::new(0.0f32, 1.0f32, 0.0f32);

        simplex.push(a, Vec3::ZERO, Vec3::ZERO, 0, 0);
        simplex.push(b, Vec3::ZERO, Vec3::ZERO, 0, 0);
        simplex.push(c, Vec3::ZERO, Vec3::ZERO, 0, 0);

        assert_eq!(simplex.size, PointSize::Three);
        let (closest_point, _) = simplex.closest_simplex();
        assert_relative_eq!(
            closest_point,
            Vec3::new(0.1f32, 0.0f32, 0.0f32),
            epsilon = 0.000001_f32
        );
    }

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

        let mut simplex = Simplex::default();
        let offset = Vec3::new(-1.0f32, -1.0f32, 0.0f32);
        let a = offset + Vec3::new(0.0f32, 0.0f32, 0.0f32);
        let b = offset + Vec3::new(1.0f32, 0.0f32, 0.0f32);
        let c = offset + Vec3::new(0.0f32, 1.0f32, 0.0f32);

        simplex.push(a, Vec3::ZERO, Vec3::ZERO, 0, 0);
        simplex.push(b, Vec3::ZERO, Vec3::ZERO, 0, 0);
        simplex.push(c, Vec3::ZERO, Vec3::ZERO, 0, 0);

        assert_eq!(simplex.size, PointSize::Three);
        let (closest_point, _) = simplex.closest_simplex();
        assert_relative_eq!(
            closest_point,
            Vec3::new(-0.5f32, -0.5f32, 0.0f32),
            epsilon = 0.000001_f32
        );
    }

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

        let mut simplex = Simplex::default();
        let offset = Vec3::new(-1.0f32, -1.0f32, 0.0f32);
        let a = offset + Vec3::new(0.0f32, 0.0f32, 0.0f32);
        let b = offset + Vec3::new(1.0f32, 0.0f32, 0.0f32);
        let c = offset + Vec3::new(2.0f32, 0.00001f32, 0.0f32);

        simplex.push(a, Vec3::ZERO, Vec3::ZERO, 0, 0);
        simplex.push(b, Vec3::ZERO, Vec3::ZERO, 0, 0);
        simplex.push(c, Vec3::ZERO, Vec3::ZERO, 0, 0);

        assert_eq!(simplex.size, PointSize::Three);
        let _closest_point = simplex.closest_simplex();
        assert_eq!(simplex.size, PointSize::Two);
        assert_eq!(c, simplex.get_point(1));
    }

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

        let mut simplex = Simplex::default();
        let offset = Vec3::new(-0.2f32, -0.2f32, -0.2f32);
        let a = offset + Vec3::new(0.0f32, 0.0f32, 0.0f32);
        let b = offset + Vec3::new(1.0f32, 0.0f32, 0.0f32);
        let c = offset + Vec3::new(0.0f32, 1.0f32, 0.0f32);
        let d = offset + Vec3::new(0.0f32, 0.0f32, 1.0f32);
        simplex.push(a, Vec3::ZERO, Vec3::ZERO, 0, 0);
        simplex.push(b, Vec3::ZERO, Vec3::ZERO, 0, 0);
        simplex.push(c, Vec3::ZERO, Vec3::ZERO, 0, 0);
        simplex.push(d, Vec3::ZERO, Vec3::ZERO, 0, 0);
        assert_eq!(simplex.size(), PointSize::Four);
        let (closest_point, _) = simplex.closest_simplex();
        assert_relative_eq!(closest_point, Vec3::ZERO, epsilon = 0.000001_f32);
        assert_eq!(simplex.size(), PointSize::Four);
    }

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

        let mut simplex = Simplex::default();
        let offset = Vec3::new(-0.2f32, -0.2f32, 0.0f32);
        let a = offset + Vec3::new(0.0f32, 0.0f32, 0.0f32);
        let b = offset + Vec3::new(1.0f32, 0.0f32, 0.0f32);
        let c = offset + Vec3::new(0.0f32, 1.0f32, 0.0f32);
        let d = offset + Vec3::new(0.0f32, 0.0f32, 0.000000_f32);
        simplex.push(a, Vec3::ZERO, Vec3::ZERO, 0, 0);
        simplex.push(b, Vec3::ZERO, Vec3::ZERO, 0, 0);
        simplex.push(c, Vec3::ZERO, Vec3::ZERO, 0, 0);
        simplex.push(d, Vec3::ZERO, Vec3::ZERO, 0, 0);
        assert_eq!(simplex.size(), PointSize::Four);
        let (closest_point, _) = simplex.closest_simplex();
        assert_relative_eq!(closest_point, Vec3::ZERO, epsilon = 0.000001_f32);
        assert_eq!(simplex.size, PointSize::Three);
    }

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

        let mut simplex = Simplex::default();
        let offset = Vec3::new(-0.2f32, -0.2f32, 0.2f32);
        let a = offset + Vec3::new(0.0f32, 0.0f32, 0.0f32);
        let b = offset + Vec3::new(1.0f32, 0.0f32, 0.0f32);
        let c = offset + Vec3::new(0.0f32, 1.0f32, 0.0f32);
        let d = offset + Vec3::new(0.0f32, 0.0f32, 1.0f32);
        simplex.push(a, Vec3::ZERO, Vec3::ZERO, 0, 0);
        simplex.push(b, Vec3::ZERO, Vec3::ZERO, 0, 0);
        simplex.push(c, Vec3::ZERO, Vec3::ZERO, 0, 0);
        simplex.push(d, Vec3::ZERO, Vec3::ZERO, 0, 0);
        assert_eq!(simplex.size(), PointSize::Four);
        let (closest_point, _) = simplex.closest_simplex();
        assert_relative_eq!(
            closest_point,
            Vec3::new(0.0f32, 0.0f32, 0.2f32),
            epsilon = 0.000001_f32
        );
        assert_eq!(simplex.size, PointSize::Three);
    }

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

        let mut simplex = Simplex::default();
        let offset = Vec3::new(0.2f32, -0.2f32, -0.2f32);
        let a = offset + Vec3::new(0.0f32, 0.0f32, 0.0f32);
        let b = offset + Vec3::new(1.0f32, 0.0f32, 0.0f32);
        let c = offset + Vec3::new(0.0f32, 1.0f32, 0.0f32);
        let d = offset + Vec3::new(0.0f32, 0.0f32, 1.0f32);
        simplex.push(a, Vec3::ZERO, Vec3::ZERO, 0, 0);
        simplex.push(b, Vec3::ZERO, Vec3::ZERO, 0, 0);
        simplex.push(c, Vec3::ZERO, Vec3::ZERO, 0, 0);
        simplex.push(d, Vec3::ZERO, Vec3::ZERO, 0, 0);
        assert_eq!(simplex.size(), PointSize::Four);
        let (closest_point, _) = simplex.closest_simplex();
        assert_relative_eq!(
            closest_point,
            Vec3::new(0.2f32, 0.0f32, 0.0f32),
            epsilon = 0.000001_f32
        );
        assert_eq!(simplex.size, PointSize::Three);
    }

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

        let mut simplex = Simplex::default();
        let offset = Vec3::new(-0.2f32, 0.2f32, -0.2f32);
        let a = offset + Vec3::new(0.0f32, 0.0f32, 0.0f32);
        let b = offset + Vec3::new(1.0f32, 0.0f32, 0.0f32);
        let c = offset + Vec3::new(0.0f32, 1.0f32, 0.0f32);
        let d = offset + Vec3::new(0.0f32, 0.0f32, 1.0f32);
        simplex.push(a, Vec3::ZERO, Vec3::ZERO, 0, 0);
        simplex.push(b, Vec3::ZERO, Vec3::ZERO, 0, 0);
        simplex.push(c, Vec3::ZERO, Vec3::ZERO, 0, 0);
        simplex.push(d, Vec3::ZERO, Vec3::ZERO, 0, 0);
        assert_eq!(simplex.size(), PointSize::Four);
        let (closest_point, _) = simplex.closest_simplex();
        assert_relative_eq!(
            closest_point,
            Vec3::new(0.0f32, 0.2f32, 0.0f32),
            epsilon = 0.000001_f32
        );
        assert_eq!(simplex.size, PointSize::Three);
    }

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

        let mut simplex = Simplex::default();
        let offset = Vec3::new(-0.2f32, -0.2f32, 0.2f32);
        let a = offset + Vec3::new(0.0f32, 0.0f32, 1.0f32);
        let b = offset + Vec3::new(1.0f32, 0.0f32, 0.0f32);
        let c = offset + Vec3::new(0.0f32, 1.0f32, 0.0f32);
        let d = offset + Vec3::new(0.0f32, 0.0f32, 0.0f32);
        simplex.push(a, Vec3::ZERO, Vec3::ZERO, 0, 0);
        simplex.push(b, Vec3::ZERO, Vec3::ZERO, 0, 0);
        simplex.push(c, Vec3::ZERO, Vec3::ZERO, 0, 0);
        simplex.push(d, Vec3::ZERO, Vec3::ZERO, 0, 0);
        assert_eq!(simplex.size(), PointSize::Four);
        let (closest_point, _) = simplex.closest_simplex();
        assert_relative_eq!(
            closest_point,
            Vec3::new(0.0f32, 0.0f32, 0.2f32),
            epsilon = 0.000001_f32
        );
        assert_eq!(simplex.size, PointSize::Three);
    }

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

        let point_a = Vec3::new(0.0f32, 0.0f32, 0.0f32);
        let point_b = Vec3::new(1.0f32, 7.0f32, 8.0f32);
        let t = 0.5f32;
        let point_c = point_a + t * (point_b - point_a);
        let t1 = BarycentricCoordinates::compute_line(point_c, point_a, point_b);
        assert_relative_eq!(t, t1, epsilon = 0.000001_f32);
        let t = 0.0f32;
        let point_c = point_a + t * (point_b - point_a);
        let t1 = BarycentricCoordinates::compute_line(point_c, point_a, point_b);
        assert_relative_eq!(t, t1, epsilon = 0.000001_f32);
        let point_b = point_a;
        let point_c = point_a + t * (point_b - point_a);
        let t1 = BarycentricCoordinates::compute_line(point_c, point_a, point_b);
        assert_relative_eq!(t, t1, epsilon = 0.000001_f32);
    }

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

        let point_a = Vec3::new(0.0f32, 0.0f32, 0.0f32);
        let point_b = Vec3::new(1.0f32, 0.0f32, 0.0f32);
        let point_c = Vec3::new(0.0f32, 1.0f32, 0.0f32);
        let t = Vec3::new(0.9f32, 0.9f32, 0.7f32).normalize();
        let point_d = point_a * t.x + point_b * t.y + point_c * t.z;
        let (v, w) = BarycentricCoordinates::compute_triangle(point_d, point_a, point_b, point_c);
        assert_relative_eq!(t.y, v, epsilon = 0.000001_f32);
        assert_relative_eq!(t.z, w, epsilon = 0.000001_f32);

        let point_a = Vec3::new(0.0f32, 0.0f32, 0.0f32);
        let point_b = point_a;
        let point_c = point_a;
        let t = Vec3::new(0.9f32, 0.9f32, 0.7f32).normalize();
        let point_d = point_a * t.x + point_b * t.y + point_c * t.z;
        let (v, w) = BarycentricCoordinates::compute_triangle(point_d, point_a, point_b, point_c);
        assert_relative_eq!(0.0f32, v, epsilon = 0.000001_f32);
        assert_relative_eq!(0.0f32, w, epsilon = 0.000001_f32);
    }

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

        let mut simplex = Simplex::default();
        let a = Vec3::new(0.5f32, 0.0f32, 0.0f32);
        simplex.push(a, a, Vec3::ZERO, 0, 0);
        let (closest_point_a, closest_point_b) = simplex.get_closest_point(a);
        assert_relative_eq!(closest_point_a, a, epsilon = 0.000001_f32);
        assert_relative_eq!(closest_point_b, Vec3::ZERO, epsilon = 0.000001_f32);
        simplex.set_size(PointSize::Zero);
        let a = Vec3::new(0.5f32, 0.0f32, 0.0f32);
        let b = Vec3::new(1.0f32, 0.0f32, 0.0f32);
        simplex.push(a, a, Vec3::ZERO, 0, 0);
        simplex.push(b, b, Vec3::ZERO, 0, 0);
        let (closest_point_a, closest_point_b) = simplex.get_closest_point(a);
        assert_relative_eq!(closest_point_a, a, epsilon = 0.000001_f32);
        assert_relative_eq!(closest_point_b, Vec3::ZERO, epsilon = 0.000001_f32);
    }

    #[test]
    // TODO: Fix for wasm (#1236)
    fn closest_point_with_4_point_and_closest_simplex_with_0_size() {
        let _ = env_logger::builder().is_test(true).try_init();

        let result = std::panic::catch_unwind(|| {
            let mut simplex = Simplex::default();
            simplex.closest_simplex();
        });
        assert!(result.is_err());
        let mut simplex = Simplex::default();
        simplex.set_size(PointSize::Four);
        let result = std::panic::catch_unwind(|| {
            simplex.get_closest_point(Vec3::ZERO);
        });
        assert!(result.is_err());
    }

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

        let mut simplex_closet = SimplexNoRearrangement {
            points: [Vec3::ZERO; 3],
            index: [0; 3],
            size: PointSize::Zero,
        };
        simplex_closet.set_size(PointSize::Two);
        simplex_closet.set_point(0, Vec3::new(1.0f32, 0.0f32, 0.0f32));
        simplex_closet.set_point(1, Vec3::new(0.0f32, 1.0f32, 0.0f32));
        simplex_closet.replace_from_to(0, 1);
        assert_relative_eq!(
            simplex_closet.get_point(1),
            Vec3::new(1.0f32, 0.0f32, 0.0f32)
        );
    }
    #[test]
    // TODO: Fix for wasm (#1236)
    fn point_size_increase() {
        let _ = env_logger::builder().is_test(true).try_init();

        let mut size = PointSize::Zero;
        size.increase();
        size.increase();
        size.increase();
        size.increase();
        assert_eq!(size, PointSize::Four);
        let result = std::panic::catch_unwind(move || {
            size.increase();
        });
        assert!(result.is_err());
    }
}