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)]
use std::cmp::Ordering;

use glam_det::{Cross, Dot, UnitVec3, Vec3};
use vslab::{new_type_id, Slab, SlabId};

use crate::minkowski::epa::{FacetKey, MAX_EDGES, MAX_FACETS};

pub(super) type FacetBuffer = Buffer<FacetKey, Facet>;
new_type_id!(EdgeKey);

pub(super) struct FacetKeyDistance {
    pub key: FacetKey,
    pub distance: f32,
}

impl Eq for FacetKeyDistance {}

impl PartialEq<Self> for FacetKeyDistance {
    #[inline]
    fn eq(&self, other: &Self) -> bool {
        self.key == other.key // same facet's distance are always the same
    }
}

#[allow(clippy::non_canonical_partial_ord_impl)]
impl PartialOrd<Self> for FacetKeyDistance {
    #[inline]
    fn partial_cmp(&self, other: &Self) -> Option<Ordering> {
        other.distance.partial_cmp(&self.distance)
    }
}

impl Ord for FacetKeyDistance {
    #[inline]
    fn cmp(&self, other: &Self) -> std::cmp::Ordering {
        other
            .distance
            .partial_cmp(&self.distance)
            .unwrap_or(std::cmp::Ordering::Equal)
    }
}

#[derive(Default)]
pub(super) struct Facet {
    normal: UnitVec3,
    pub ajacent_facets: [FacetKey; 3],
    indices: [usize; 3],
    pub ajacent_edges: [i8; 3],
    distance: f32,
    in_heap: bool,
    id: FacetKey,
    obsolete: bool,
}

impl Facet {
    pub fn new(indices: [usize; 3]) -> Self {
        Self {
            indices,
            ajacent_edges: [-1; 3],
            ..Default::default()
        }
    }

    #[inline]
    pub fn get_sign_distance_from_point(
        &self,
        point: Vec3,
        buffer_a: &[Vec3],
        buffer_b: &[Vec3],
    ) -> f32 {
        let a = buffer_a[self.indices[0]];
        let b = buffer_b[self.indices[0]];
        let d = a - b;
        self.normal.dot(point - d)
    }

    #[inline]
    pub fn normal(&self) -> UnitVec3 {
        self.normal
    }

    #[inline]
    pub fn plane_distance(&self) -> f32 {
        self.distance
    }

    #[inline]
    pub fn obsolete(&self) -> bool {
        self.obsolete
    }

    #[inline]
    pub fn set_obsolete(&mut self, obsolete: bool) {
        self.obsolete = obsolete;
    }

    #[inline]
    pub fn get_index(&self, index: usize) -> usize {
        self.indices[index]
    }

    #[inline]
    pub fn set_ajacent_facet(&mut self, index: usize, key: FacetKey) {
        self.ajacent_facets[index] = key;
    }

    #[inline]
    pub fn valid(&self) -> bool {
        !self.ajacent_facets[0].is_null()
            && !self.ajacent_facets[1].is_null()
            && !self.ajacent_facets[2].is_null()
    }

    #[inline]
    pub fn set_ajacent_edge(&mut self, index: usize, edge: i8) {
        self.ajacent_edges[index] = edge;
    }

    #[inline]
    pub fn set_id(&mut self, id: FacetKey) {
        self.id = id;
    }

    #[inline]
    pub fn set_in_heap(&mut self, in_heap: bool) {
        self.in_heap = in_heap;
    }

    #[inline]
    pub fn in_heap(&self) -> bool {
        self.in_heap
    }

    #[inline]
    pub fn distance(&self) -> f32 {
        self.distance
    }

    pub fn check_valid_and_set_normal_distance(
        &mut self,
        buffer_a: &[Vec3],
        buffer_b: &[Vec3],
        index_a: usize,
        index_b: usize,
        index_c: usize,
        upper_bound: f32,
    ) -> bool {
        let a = buffer_a[index_a];
        let b = buffer_a[index_b];
        let c = buffer_a[index_c];
        let a1 = buffer_b[index_a];
        let b1 = buffer_b[index_b];
        let c1 = buffer_b[index_c];
        let support_a = a - a1;
        let support_b = b - b1;
        let support_c = c - c1;
        let edge0 = support_b - support_a;
        let edge1 = support_c - support_a;
        let normal = edge0.cross(edge1).try_normalize_to_unit(f32::EPSILON);
        if normal.is_none() {
            self.distance = 0.0f32;
            return false;
        }
        self.normal = normal.unwrap();
        self.distance = self.normal.dot(support_a);
        self.distance <= upper_bound
    }

    pub fn calculate_closest_point(&self, buffer_a: &[Vec3], buffer_b: &[Vec3]) -> (Vec3, Vec3) {
        let a = buffer_a[self.indices[0]];
        let b = buffer_a[self.indices[1]];
        let c = buffer_a[self.indices[2]];
        let a1 = buffer_b[self.indices[0]];
        let b1 = buffer_b[self.indices[1]];
        let c1 = buffer_b[self.indices[2]];
        let support_a = a - a1;
        let support_b = b - b1;
        let support_c = c - c1;
        let ab = support_b - support_a;
        let ac = support_c - support_a;
        // closet_point on minkowski difference space
        let closet_point_d = self.normal() * self.plane_distance();
        let ad = closet_point_d - support_a;
        let ab_ab = ab.dot(ab);
        let ab_ac = ab.dot(ac);
        let ac_ac = ac.dot(ac);

        let ab_ad = ab.dot(ad);
        let ac_ad = ac.dot(ad);
        let ad_ad = ad.dot(ad);

        let abc_area = ab_ab * ac_ac - ab_ac * ab_ac; // 2 * area of triangle abc
        let abc_area_recip = if abc_area > f32::EPSILON {
            abc_area.recip()
        } else {
            0.0
        };
        let lambda1 = (ac_ac * ab_ad - ab_ac * ac_ad) * abc_area_recip;
        let lambda2 = (ab_ab * ac_ad - ab_ac * ab_ad) * abc_area_recip;
        let lambda3 = 1.0 - lambda1 - lambda2;
        let closest_point_a = a * lambda3 + b * lambda1 + c * lambda2;
        let closest_point_b = a1 * lambda3 + b1 * lambda1 + c1 * lambda2;
        (closest_point_a, closest_point_b)
    }
}

#[derive(Default, Clone, Copy)]
pub(super) struct Edge {
    pub facet: FacetKey,
    pub index: usize,
}

pub(super) struct Buffer<Key: SlabId, Value> {
    buffer: Slab<Key, Value>,
    overflow: bool,
}

pub(super) struct EdgeBuffer {
    buffer: Vec<Edge>,
    overflow: bool,
}

impl Default for EdgeBuffer {
    fn default() -> Self {
        Self {
            buffer: Vec::with_capacity(MAX_EDGES),
            overflow: false,
        }
    }
}

impl EdgeBuffer {
    pub fn clear(&mut self) {
        self.buffer.clear();
        self.overflow = false;
    }

    pub fn valid(&self) -> bool {
        !self.overflow && !self.buffer.is_empty()
    }

    pub fn get(&self, index: usize) -> Edge {
        self.buffer[index]
    }

    pub fn len(&self) -> usize {
        self.buffer.len()
    }

    pub fn insert(&mut self, edge: Edge) -> bool {
        if self.buffer.len() < MAX_EDGES {
            self.buffer.push(edge);
            true
        } else {
            self.overflow = true;
            false
        }
    }
}

impl<Key, Value> Buffer<Key, Value>
where
    Key: Default + SlabId + Copy + Clone,
    Value: Default,
{
    #[inline]
    pub fn with_capacity(capacity: usize) -> Self {
        Self {
            buffer: Slab::<Key, Value>::with_capacity(capacity),
            overflow: false,
        }
    }

    #[inline]
    pub fn insert(&mut self, value: Value) -> Key {
        if self.buffer.len() >= self.buffer.capacity() {
            self.overflow = true;
            return Default::default();
        }
        self.buffer.insert(value)
    }

    #[inline]
    pub fn get(&self, key: Key) -> Option<&Value> {
        self.buffer.get(key)
    }

    #[inline]
    pub fn get_mut(&mut self, key: Key) -> Option<&mut Value> {
        self.buffer.get_mut(key)
    }

    #[inline]
    pub fn remove(&mut self, key: Key) -> Option<Value> {
        self.buffer.remove(key)
    }

    #[inline]
    pub fn remain_count(&self) -> usize {
        self.buffer.capacity() - self.buffer.len()
    }
}

#[cfg(test)]
mod tests {
    use glam_det::Vec3;
    use wasm_bindgen_test::{wasm_bindgen_test, wasm_bindgen_test_configure};

    use crate::minkowski::epa::FacetKey;
    use crate::minkowski::epa_facet::Facet;
    wasm_bindgen_test_configure!(run_in_browser);
    #[test]
    #[wasm_bindgen_test]
    fn test1() {
        let _ = env_logger::builder().is_test(true).try_init();

        let mut facet = super::Facet::new([0; 3]);
        let mut a_buffer = [Vec3::ZERO; 3];
        let mut b_buffer = [Vec3::ZERO; 3];
        let result = facet.check_valid_and_set_normal_distance(&a_buffer, &b_buffer, 0, 1, 2, 0.0);

        assert!(!result);
    }

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

        let mut edge_buffer = super::EdgeBuffer::default();
        let edge = super::Edge {
            facet: FacetKey::default(),
            index: 0,
        };
        for i in 0..super::MAX_EDGES {
            let result = edge_buffer.insert(edge);
            assert!(result);
        }
        let result = edge_buffer.insert(edge);
        assert!(!result);
    }

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

        let mut buffer = super::FacetBuffer::with_capacity(super::MAX_FACETS);
        for i in 0..super::MAX_FACETS {
            let facet = super::Facet::new([0; 3]);
            let result = buffer.insert(facet);
            assert_ne!(result, FacetKey::default());
        }
        let facet = super::Facet::new([0; 3]);
        let result = buffer.insert(facet);
        assert_eq!(result, FacetKey::default());
    }

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

        let facet = Facet::new([0, 1, 2]);
        let (a, b) = facet.calculate_closest_point(
            &[Vec3::ZERO, Vec3::ZERO, Vec3::ZERO],
            &[Vec3::ZERO, Vec3::ZERO, Vec3::ZERO],
        );
        assert_eq!(a, Vec3::ZERO);
        assert_eq!(b, Vec3::ZERO);
    }
}