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.

#![allow(unused)] // todo remove this in issue #1190

use std::cmp::Ordering;
use std::collections::BinaryHeap;

use glam_det::nums::Signed;
use glam_det::{Cross, Dot, Isometry3, Point3, UnitVec3, Vec3};
use vslab::{new_type_id, Slab};

use crate::minkowski::epa_facet::{
    Buffer, Edge, EdgeBuffer, EdgeKey, Facet, FacetBuffer, FacetKeyDistance,
};
use crate::minkowski::gjk::{gjk_penetration, GjkResult, PenetrationResult};
use crate::minkowski::simplex::PointSize;
use crate::traits::MinkowskiSupport;

pub(super) const MAX_EDGES: usize = 32;
pub(super) const MAX_FACETS: usize = 64;
const MAX_SUPPORT_POINTS: usize = 64;

new_type_id!(FacetKey);

#[derive(Default, Debug, Clone, Copy)]
pub struct ContactInfo {
    pub normal: UnitVec3, //normal from b to a
    pub depth: f32,
    pub point_a: Point3,
    pub point_b: Point3,
}

pub(crate) struct Input<'a, A, B> {
    shape_a: &'a A,
    transform_a: Isometry3,
    shape_b: &'a B,
    transform_b: Isometry3,
    support_points_a: &'a [Vec3],
    support_points_b: &'a [Vec3],
    size: PointSize,
    tolerance: f32,
}

/// The Expanding Polytope Algorithm.
pub(super) struct Epa {
    a_buffer: [Vec3; MAX_SUPPORT_POINTS],
    b_buffer: [Vec3; MAX_SUPPORT_POINTS],
    size: PointSize,
    facets: FacetBuffer,
    edges: EdgeBuffer,
    heap: BinaryHeap<FacetKeyDistance>, // key, distance
}

/// the result is same as (index + 1) % 3
/// the benchmark mod3.rs shows that this is probably 30% faster than (index + 1) % 3
#[inline]
fn inc_mod3(index: usize) -> usize {
    const TABLE: [usize; 3] = [1, 2, 0];
    TABLE[index]
}

struct ExpandResult {
    success: bool,
    local_size: usize,
}

impl Epa {
    fn new() -> Self {
        Self {
            a_buffer: [Vec3::ZERO; MAX_SUPPORT_POINTS],
            b_buffer: [Vec3::ZERO; MAX_SUPPORT_POINTS],
            size: PointSize::Zero,
            facets: FacetBuffer::with_capacity(MAX_FACETS),
            edges: EdgeBuffer::default(),
            heap: BinaryHeap::<FacetKeyDistance>::with_capacity(MAX_FACETS),
        }
    }

    fn calculate_contact_info<A, B>(&mut self, input: &Input<A, B>) -> Option<ContactInfo>
    where
        A: MinkowskiSupport,
        B: MinkowskiSupport,
    {
        for i in 0..input.size.as_usize() {
            self.a_buffer[i] = input.support_points_a[i];
            self.b_buffer[i] = input.support_points_b[i];
        }
        self.size = input.size;
        let mut upper_bound = f32::MAX;
        let expand_result = match input.size {
            PointSize::One => {
                // a support point is Vec3::Zero
                self.expand_point(
                    input.shape_a,
                    input.transform_a,
                    input.shape_b,
                    input.transform_b,
                    upper_bound,
                )
            }
            PointSize::Two => {
                // a line contains Vec3::Zero, so we can construct two triangle that back to back
                self.expand_segment(
                    input.shape_a,
                    input.transform_a,
                    input.shape_b,
                    input.transform_b,
                    upper_bound,
                )
            }
            PointSize::Three => {
                // Vec3::Zero is in the triangle, so we can construct two triangle that back
                // to back
                self.expand_triangle(upper_bound)
            }
            PointSize::Four => {
                // make sure all face normal is outwards
                let a = self.a_buffer[0] - self.b_buffer[0];
                let b = self.a_buffer[1] - self.b_buffer[1];
                let c = self.a_buffer[2] - self.b_buffer[2];
                let d = self.a_buffer[3] - self.b_buffer[3];
                let v1 = b - a;
                let v2 = c - a;
                let normal = v1.cross(v2).normalize();
                let sign_distance = normal.dot(d - a);
                if sign_distance > 0.0 {
                    self.a_buffer.swap(1, 2);
                    self.b_buffer.swap(1, 2);
                }
                let facet_0 = self.add_facet(0, 1, 2, upper_bound);
                let facet_1 = self.add_facet(0, 3, 1, upper_bound);
                let facet_2 = self.add_facet(0, 2, 3, upper_bound);
                let facet_3 = self.add_facet(1, 3, 2, upper_bound);
                if self.heap.is_empty() {
                    ExpandResult {
                        success: false,
                        local_size: 4,
                    }
                } else {
                    self.link(facet_0, 0, facet_1, 2);
                    self.link(facet_0, 1, facet_3, 2);
                    self.link(facet_0, 2, facet_2, 0);
                    self.link(facet_1, 0, facet_2, 2);
                    self.link(facet_1, 1, facet_3, 0);
                    self.link(facet_2, 1, facet_3, 1);
                    ExpandResult {
                        success: true,
                        local_size: 4,
                    }
                }
            }
            PointSize::Zero => {
                return None;
            }
        };

        if !expand_result.success {
            return None;
        }
        let mut local_vertex_count = expand_result.local_size;
        let mut facet_id = FacetKey::default();
        let mut a;
        let mut b;
        let mut support;
        let eps = 1e-3;
        let mut loop_count = 0;
        while !self.heap.is_empty() && local_vertex_count < MAX_SUPPORT_POINTS {
            /// pop the facet with the smallest distance from origin
            let facet_key_distance = self.heap.pop().unwrap(); // self.heap.len() > 0 ,so safe unwrap
            facet_id = facet_key_distance.key;
            let facet = self.facets.get_mut(facet_id).unwrap(); // safe unwrap
            /// This facet has been removed from the heap, and a few new facets created from
            /// `add_facet` will be added to the heap
            facet.set_in_heap(false);
            if !facet.obsolete() {
                let normal = facet.normal();
                let plane_distance = facet.plane_distance();
                a = input
                    .shape_a
                    .support_point(normal.as_vec3(), &input.transform_a)
                    .point
                    .as_vec3();
                b = input
                    .shape_b
                    .support_point(-normal.as_vec3(), &input.transform_b)
                    .point
                    .as_vec3();
                support = a - b;
                let distance = support.dot(normal);
                let distance_from_plane = distance - plane_distance;
                if distance_from_plane.absf() < eps {
                    // close enough,so calculate the contact info
                    let (closest_a, closest_b) =
                        facet.calculate_closest_point(&self.a_buffer, &self.b_buffer);
                    let plane_distance_abs = plane_distance.absf();
                    let tolerance_eps = input.tolerance * 1e-3;
                    if (closest_a - closest_b).length() > plane_distance_abs + tolerance_eps {
                        //  test coverage fo this line. issue #1275
                        break;
                    }
                    let contact_info = ContactInfo {
                        normal: -normal,
                        depth: -plane_distance_abs,
                        point_a: Point3::from_vec3(closest_a),
                        point_b: Point3::from_vec3(closest_b),
                    };
                    return Some(contact_info);
                }
                upper_bound = upper_bound.min(distance);
                self.a_buffer[local_vertex_count] = a;
                self.b_buffer[local_vertex_count] = b;
                let index = local_vertex_count;
                local_vertex_count += 1;
                loop_count += 1;
                self.edges.clear();
                facet.set_obsolete(true);
                let ajacent_facets = facet.ajacent_facets;
                let ajacent_edges = facet.ajacent_edges;
                self.silhouette(&ajacent_facets[..], &ajacent_edges[..], support);
                if !self.edges.valid() {
                    log::debug!("EPA: DEGENERATE");
                    // test coverage fo this line. issue #1275
                    break;
                }
                let buffer_len = self.edges.len();
                if buffer_len > self.facets.remain_count() {
                    log::debug!("EPA: facets will overflow");
                    // test coverage fo this line. issue #1275
                    break;
                }
                let edge = self.edges.get(0);
                let mut first_facet = self.add_facet(
                    self.get_edge_target(edge.index, edge.facet),
                    self.get_edge_source(edge.index, edge.facet),
                    index,
                    upper_bound,
                );
                assert_ne!(first_facet, FacetKey::default());
                self.link(first_facet, 0, edge.facet, edge.index);

                let mut last_facet = first_facet;
                (1..buffer_len).for_each(|i| {
                    let edge = self.edges.get(i);
                    let new_facet = self.add_facet(
                        self.get_edge_target(edge.index, edge.facet),
                        self.get_edge_source(edge.index, edge.facet),
                        index,
                        upper_bound,
                    );
                    self.link(new_facet, 0, edge.facet, edge.index);
                    self.link(new_facet, 2, last_facet, 1);
                    last_facet = new_facet;
                });
                self.link(first_facet, 2, last_facet, 1);
            }
            self.facets.remove(facet_id);
        }
        // test coverage fo this line. issue #1275
        None
    }

    #[inline]
    fn get_edge_target(&self, edge_index: usize, facet: FacetKey) -> usize {
        let facet = self.facets.get(facet).unwrap(); // safe unwrap
        facet.get_index(inc_mod3(edge_index))
    }

    #[inline]
    fn get_edge_source(&self, edge_index: usize, facet: FacetKey) -> usize {
        let facet = self.facets.get(facet).unwrap(); // safe unwrap
        facet.get_index(edge_index)
    }

    #[allow(clippy::as_conversions, clippy::cast_sign_loss)]
    pub fn silhouette(&mut self, ajacent_facets: &[FacetKey], ajacent_edges: &[i8], support: Vec3) {
        ajacent_facets
            .iter()
            .zip(ajacent_edges.iter())
            .for_each(|(facet_id, edge_index)| {
                assert!(*edge_index >= 0);
                self.silhouette_with_edge_index(*facet_id, *edge_index as usize, support);
            });
    }

    #[allow(clippy::as_conversions, clippy::cast_sign_loss)]
    pub fn silhouette_with_edge_index(
        &mut self,
        facet_id: FacetKey,
        edge_index: usize,
        support: Vec3,
    ) {
        let mut stack = [Edge::default(); MAX_FACETS];
        stack[0] = Edge {
            facet: facet_id,
            index: edge_index,
        };
        let mut stack_size = 1;
        while stack_size > 0 {
            stack_size -= 1;
            let facet_id = stack[stack_size].facet;
            if let Some(facet) = self.facets.get_mut(facet_id) {
                let edge_index = stack[stack_size].index;
                assert!(facet.valid());

                if !facet.obsolete() {
                    let distance =
                        facet.get_sign_distance_from_point(support, &self.a_buffer, &self.b_buffer);
                    if distance < 0.0 {
                        if !self.edges.insert(Edge {
                            facet: facet_id,
                            index: edge_index,
                        }) {
                            log::debug!("EPA: edge count overflow!");
                            // test coverage fo this line. issue #1275
                            return;
                        }
                    } else {
                        facet.set_obsolete(true);
                        let index1 = inc_mod3(edge_index);
                        let index2 = inc_mod3(index1);
                        stack[stack_size] = Edge {
                            facet: facet.ajacent_facets[index2],
                            index: facet.ajacent_edges[index2] as usize,
                        };
                        stack_size += 1;
                        stack[stack_size] = Edge {
                            facet: facet.ajacent_facets[index1],
                            index: facet.ajacent_edges[index1] as usize,
                        };
                        stack_size += 1;
                        assert!(stack_size < MAX_FACETS);
                        if !facet.in_heap() {
                            self.facets.remove(facet_id);
                        }
                    }
                }
            }
        }
    }

    fn expand_point(
        &mut self,
        shape_a: &impl MinkowskiSupport,
        transform_a: Isometry3,
        shape_b: &impl MinkowskiSupport,
        transform_b: Isometry3,
        upper_bound: f32,
    ) -> ExpandResult {
        let normal = Vec3::Y;
        let a = self.a_buffer[0];
        let b = self.b_buffer[0];
        let c = a - b;
        self.a_buffer[1] = shape_a.support_point(-normal, &transform_a).point.as_vec3();
        self.b_buffer[1] = shape_b.support_point(normal, &transform_b).point.as_vec3();
        let f = self.a_buffer[1] - self.b_buffer[1];
        if c == f {
            ExpandResult {
                success: false,
                local_size: 1,
            }
        } else {
            self.expand_segment(shape_a, transform_a, shape_b, transform_b, upper_bound)
        }
    }

    fn expand_segment(
        &mut self,
        shape_a: &impl MinkowskiSupport,
        transform_a: Isometry3,
        shape_b: &impl MinkowskiSupport,
        transform_b: Isometry3,
        upper_bound: f32,
    ) -> ExpandResult {
        let a = self.a_buffer[0] - self.b_buffer[0];
        let b = self.a_buffer[1] - self.b_buffer[1];
        let c = b - a;
        let c_abs = c.abs();
        let normal = if c_abs.x > c_abs.y && c_abs.z > c_abs.y {
            Vec3::Y
        } else if c_abs.x > c_abs.z {
            Vec3::Z
        } else {
            Vec3::X
        };
        // may be need not normalize
        let normal = normal.cross(c).normalize();
        self.a_buffer[2] = shape_a.support_point(-normal, &transform_a).point.as_vec3();
        self.b_buffer[2] = shape_b.support_point(normal, &transform_b).point.as_vec3();
        let f = self.a_buffer[2] - self.b_buffer[2];
        self.expand_triangle(upper_bound)
    }

    fn expand_triangle(&mut self, upper_bound: f32) -> ExpandResult {
        let fact0 = self.add_facet(0, 1, 2, upper_bound);
        let fact1 = self.add_facet(1, 0, 2, upper_bound);
        if self.heap.is_empty() {
            return ExpandResult {
                success: false,
                local_size: 3,
            };
        }
        self.link(fact0, 0, fact1, 0);
        self.link(fact0, 1, fact1, 2);
        self.link(fact0, 2, fact1, 1);
        ExpandResult {
            success: true,
            local_size: 3,
        }
    }

    #[allow(clippy::cast_possible_wrap)]
    pub fn link(
        &mut self,
        facet0_id: FacetKey,
        edge_index0: usize,
        facet1_id: FacetKey,
        edge_index1: usize,
    ) -> bool {
        let facet0 = self.facets.get_mut(facet0_id).unwrap(); // safe unwrap
        facet0.set_ajacent_facet(edge_index0, facet1_id);
        facet0.set_ajacent_edge(edge_index0, edge_index1 as i8);
        let facet0_index0 = facet0.get_index(edge_index0);
        let facet0_index1 = facet0.get_index(inc_mod3(edge_index0));

        let facet1 = self.facets.get_mut(facet1_id).unwrap(); // safe unwrap
        facet1.set_ajacent_facet(edge_index1, facet0_id);
        facet1.set_ajacent_edge(edge_index1, edge_index0 as i8);
        let facet1_index0 = facet1.get_index(inc_mod3(edge_index1));
        let facet1_index1 = facet1.get_index(edge_index1);
        facet0_index0 == facet1_index0 && facet0_index1 == facet1_index1
    }

    fn add_facet(&mut self, a: usize, b: usize, c: usize, upper_bound: f32) -> FacetKey {
        let facet_id = self.facets.insert(Facet::new([a, b, c]));
        let facet = self.facets.get_mut(facet_id).unwrap(); // safe unwrap
        facet.set_id(facet_id);
        let valid = facet.check_valid_and_set_normal_distance(
            &self.a_buffer[..],
            &self.b_buffer[..],
            a,
            b,
            c,
            upper_bound,
        );
        if valid {
            self.heap.push(FacetKeyDistance {
                key: facet_id,
                distance: facet.distance(),
            });
            facet.set_in_heap(true);
        } else {
            facet.set_in_heap(false);
        }
        facet_id
    }
}

pub fn calc_contact_info(
    shape_a: &impl MinkowskiSupport,
    shape_b: &impl MinkowskiSupport,
    contact_tolerance: f32,
    transform_a: Isometry3,
    transform_b: Isometry3,
) -> Option<ContactInfo> {
    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 gjk_result = gjk_penetration(
        shape_a,
        transform_a,
        shape_b,
        transform_b,
        init_dir,
        contact_tolerance,
    );
    match gjk_result {
        PenetrationResult::GjkIntersectWithContactInfo(info) => {
            // test coverage fo this line. issue #1275
            Some(info)
        }
        PenetrationResult::EpaIntersectWithSimplexInfo(info) => {
            let mut epa = Epa::new();
            epa.calculate_contact_info(&Input {
                shape_a,
                transform_a,
                shape_b,
                transform_b,
                support_points_a: &info.support_points_a,
                support_points_b: &info.support_points_b,
                size: info.size,
                tolerance: contact_tolerance,
            })
        }
        PenetrationResult::GjkDegenerate(info) => Some(info),
        PenetrationResult::Continue | PenetrationResult::NoIntersect => None,
    }
}

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

    use glam_det::{Isometry3, Point3, UnitQuat, UnitVec3, Vec3};
    use wasm_bindgen_test::*;

    use crate::minkowski::epa::{calc_contact_info, ContactInfo, Epa, Input};
    use crate::minkowski::gjk::{gjk_penetration, GjkResult};
    use crate::minkowski::simplex::PointSize;
    use crate::traits::MinkowskiSupport;
    use crate::{ConvexHull, Cuboid};

    wasm_bindgen_test_configure!(run_in_browser);

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

        let mut frame_index = 0;
        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, 1f32 + contact_tolerance * 1.5f32, 0.0),
            rotation: UnitQuat::from_rotation_x(0.0f32.to_radians()),
        };
        let contact_info = calc_contact_info(
            &shape_a,
            &shape_b,
            contact_tolerance,
            transform_a,
            transform_b,
        );
        assert!(contact_info.is_none());
    }

    #[test]
    #[wasm_bindgen_test]
    fn test_contact_info_contact_tolerance() {
        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.0f32 + contact_tolerance * 1.5f32),
            rotation: UnitQuat::from_rotation_x(2.0f32.to_radians()),
        };
        let contact_info = calc_contact_info(
            &shape_a,
            &shape_b,
            contact_tolerance,
            transform_a,
            transform_b,
        );
        assert!(contact_info.is_some());
    }

    #[test]
    #[wasm_bindgen_test]
    fn test_contact_info_contact_45_degree() {
        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 {
            translation: Vec3::new(0.0, 0.0, 1.0),
            rotation: UnitQuat::from_rotation_x(45.0f32.to_radians()),
        };
        let transform_b = glam_det::Isometry3 {
            translation: Vec3::new(0.0, 0.0, 0.0),
            rotation: UnitQuat::from_rotation_x(45.0f32.to_radians()),
        };
        let contact_info = calc_contact_info(
            &shape_a,
            &shape_b,
            contact_tolerance,
            transform_a,
            transform_b,
        );
        assert!(contact_info.is_some());
    }

    #[test]
    #[wasm_bindgen_test]
    fn test_contact_info_contact_one_point() {
        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 {
            translation: Vec3::new(0.0, 0.0, 2.0f32.sqrt()),
            rotation: UnitQuat::from_rotation_x(45.0f32.to_radians()),
        };
        let transform_b = glam_det::Isometry3 {
            translation: Vec3::new(0.0, 0.0, 0.0),
            rotation: UnitQuat::from_rotation_x(45.0f32.to_radians()),
        };
        let contact_info = calc_contact_info(
            &shape_a,
            &shape_b,
            contact_tolerance,
            transform_a,
            transform_b,
        );
        assert!(contact_info.is_some());
    }

    #[test]
    #[wasm_bindgen_test]
    fn test_contact_info_gjk_contact() {
        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 {
            translation: Vec3::new(0.0, 0.0, 2.0f32.sqrt() + 0.5f32 * contact_tolerance),
            rotation: UnitQuat::from_rotation_x(45.0f32.to_radians()),
        };
        let transform_b = glam_det::Isometry3 {
            translation: Vec3::new(0.0, 0.0, 0.0),
            rotation: UnitQuat::from_rotation_x(45.0f32.to_radians()),
        };
        let contact_info = calc_contact_info(
            &shape_a,
            &shape_b,
            contact_tolerance,
            transform_a,
            transform_b,
        );
        assert!(contact_info.is_some());
    }

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

        let mut epa = Epa::new();
        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 result = epa.calculate_contact_info(&Input {
            shape_a: &shape_a,
            transform_a: Isometry3::default(),
            shape_b: &shape_b,
            transform_b: Isometry3::default(),
            support_points_a: &[Vec3::ZERO; 4],
            support_points_b: &[
                Vec3::ZERO + Vec3::new(f32::EPSILON, 0.0, 0.0),
                Vec3::ZERO + Vec3::new(0.0, f32::EPSILON, 0.0),
                Vec3::ZERO,
                Vec3::ZERO,
            ],
            size: PointSize::Four,
            tolerance: 0.0,
        });
        assert!(result.is_none());
        let result = epa.calculate_contact_info(&Input {
            shape_a: &shape_a,
            transform_a: Isometry3::default(),
            shape_b: &shape_b,
            transform_b: Isometry3::default(),
            support_points_a: &[Vec3::ZERO; 4],
            support_points_b: &[
                Vec3::ZERO + Vec3::new(f32::EPSILON, 0.0, 0.0),
                Vec3::ZERO + Vec3::new(0.0, f32::EPSILON, 0.0),
                Vec3::ZERO,
                Vec3::ZERO,
            ],
            size: PointSize::Zero,
            tolerance: 0.0,
        });
        assert!(result.is_none());
    }

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

        let mut epa = Epa::new();
        let shape_a = Cuboid::new_xyz(1.0, 1.0, 1.0);
        let shape_b = Cuboid::new_xyz(1.0, 1.0, 1.0);
        epa.a_buffer[0] = Vec3::ZERO;
        epa.b_buffer[0] = Vec3::new(0.0, 1.0, 0.0);
        epa.expand_point(
            &shape_a,
            Isometry3::default(),
            &shape_b,
            Isometry3::default(),
            0.0,
        );
    }

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

        let mut epa = Epa::new();
        let shape_a = Cuboid::new_xyz(1.0, 1.0, 1.0);
        let shape_b = Cuboid::new_xyz(1.0, 1.0, 1.0);
        epa.a_buffer[0] = Vec3::ZERO;
        epa.b_buffer[0] = Vec3::new(
            3.0f32 * f32::EPSILON,
            2.0 * f32::EPSILON,
            1.0 * f32::EPSILON,
        );
        let result = epa.expand_segment(
            &shape_a,
            Isometry3::default(),
            &shape_b,
            Isometry3::default(),
            0.0,
        );
        assert!(result.success);
    }

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

        let mut epa = Epa::new();
        epa.a_buffer[0] = Vec3::ZERO;
        epa.b_buffer[0] = Vec3::new(
            3.0f32 * f32::EPSILON,
            2.0 * f32::EPSILON,
            1.0 * f32::EPSILON,
        );
        let result = epa.expand_triangle(0.0);
        assert!(!result.success);
    }
}