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, DotClamp, Point3, UnitVec3, Vec3};

pub struct ClosestPointToTriangleResult {
    #[allow(dead_code)]
    pub region: Region,
    pub closest_point: Vec3,
}

pub enum Region {
    Region0,
    Region1,
    Region2,
    Region3,
    Region4,
    Region5,
    Region6,
}

/// [`NAS//share/Books/Geometric Tools For Computer Graphics.pdf` chapter 10](https://github.com/davideberly/GeometricTools/blob/51dfbc05846e6459fd548c78a49a7f5fff5b6536/GTL/Mathematics/Distance/ND/DistPointTriangle.h#L58)
/// this function use math method to compute closet point  from origin to triangle
pub fn closest_point_to_triangle(
    edge0: Vec3,
    edge1: Vec3,
    diff: Vec3,
    a: Vec3,
) -> ClosestPointToTriangleResult {
    let a00 = edge0.dot(edge0);
    let a01 = edge0.dot(edge1);
    let a11 = edge1.dot(edge1);
    let b0 = diff.dot(edge0);
    let b1 = diff.dot(edge1);
    let det = (a00 * a11 - a01 * a01).max(0f32);
    let mut s = a01 * b1 - a11 * b0;
    let mut t = a01 * b0 - a00 * b1;
    let zero = 0f32;
    let one = 1f32;
    let two = 2f32;
    let region: Region;

    if s + t <= det {
        if s < zero {
            if t < zero {
                region = Region::Region4;
                if b0 < zero {
                    t = zero;
                    if -b0 >= a00 {
                        s = one;
                    } else {
                        s = -b0 / a00;
                    }
                } else {
                    s = zero;
                    if b1 >= zero {
                        t = zero;
                    } else if -b1 >= a11 {
                        t = one;
                    } else {
                        t = -b1 / a11;
                    }
                }
            } else {
                region = Region::Region3;
                s = zero;
                if b1 >= zero {
                    t = zero;
                } else if -b1 >= a11 {
                    t = one;
                } else {
                    t = -b1 / a11;
                }
            }
        } else if t < zero {
            region = Region::Region5;
            t = zero;
            if b0 >= zero {
                s = zero;
            } else if -b0 >= a00 {
                s = one;
            } else {
                s = -b0 / a00;
            }
        } else {
            region = Region::Region0;
            // minimum at interior point
            s /= det;
            t /= det;
        }
    } else {
        let tmp0;
        let tmp1;
        let numer;
        let denom;

        if s < zero {
            region = Region::Region2;
            tmp0 = a01 + b0;
            tmp1 = a11 + b1;
            if tmp1 > tmp0 {
                numer = tmp1 - tmp0;
                denom = a00 - two * a01 + a11;
                if numer >= denom {
                    s = one;
                    t = zero;
                } else {
                    s = numer / denom;
                    t = one - s;
                }
            } else {
                s = zero;
                if tmp1 <= zero {
                    t = one;
                } else if b1 >= zero {
                    t = zero;
                } else {
                    t = -b1 / a11;
                }
            }
        } else if t < zero {
            region = Region::Region6;
            tmp0 = a01 + b1;
            tmp1 = a00 + b0;
            if tmp1 > tmp0 {
                numer = tmp1 - tmp0;
                denom = a00 - two * a01 + a11;
                if numer >= denom {
                    t = one;
                    s = zero;
                } else {
                    t = numer / denom;
                    s = one - t;
                }
            } else {
                t = zero;
                if tmp1 <= zero {
                    s = one;
                } else if b0 >= zero {
                    s = zero;
                } else {
                    s = -b0 / a00;
                }
            }
        } else {
            region = Region::Region1;
            numer = a11 + b1 - a01 - b0;
            if numer <= zero {
                s = zero;
                t = one;
            } else {
                denom = a00 - two * a01 + a11;
                if numer >= denom {
                    s = one;
                    t = zero;
                } else {
                    s = numer / denom;
                    t = one - s;
                }
            }
        }
    }
    ClosestPointToTriangleResult {
        region,
        closest_point: a + s * edge0 + t * edge1,
    }
}

pub fn distance_squared_between_two_segments(
    offset_a_to_b: Vec3,
    direction_a: UnitVec3,
    length_a: f32,
    direction_b: UnitVec3,
    length_b: f32,
) -> f32 {
    let dir_a_dot_offset = direction_a.dot(offset_a_to_b);
    let dir_b_dot_offset = direction_b.dot(offset_a_to_b);
    let dir_a_dot_b = direction_a.dot_clamp(direction_b);

    let mut term0 = (dir_a_dot_offset - dir_b_dot_offset * dir_a_dot_b)
        / (f32::max(f32::EPSILON, 1.0 - dir_a_dot_b * dir_a_dot_b));
    let mut term1 = term0 * dir_a_dot_b - dir_b_dot_offset;

    term0 = {
        let term_bound_0 = f32::clamp(dir_a_dot_offset, 0.0, length_a);
        let term_bound_1 = f32::clamp(dir_a_dot_offset + length_b * dir_a_dot_b, 0.0, length_a);

        let term_min = term_bound_0.min(term_bound_1);
        let term_max = term_bound_1.max(term_bound_0);

        f32::clamp(term0, term_min, term_max)
    };

    term1 = {
        let term_bound_0 = f32::clamp(-dir_b_dot_offset, 0.0, length_b);
        let term_bound_1 = f32::clamp(-dir_b_dot_offset + length_a * dir_a_dot_b, 0.0, length_b);

        let term_min = term_bound_0.min(term_bound_1);
        let term_max = term_bound_1.max(term_bound_0);

        f32::clamp(term1, term_min, term_max)
    };

    (direction_a * term0).distance_squared(offset_a_to_b + direction_b * term1)
}

pub fn distance_squared_point_to_segment(point: Point3, a: Point3, b: Point3, len: f32) -> f32 {
    let ap = point - a;
    let ab = b - a;
    let ac_len = ap.dot(ab) / len;
    let r = ac_len / len;

    if r <= 0.0 {
        ap.length_squared()
    } else if r >= 1.0 {
        (point - b).length_squared()
    } else {
        ap.length_squared() - ac_len * ac_len
    }
}

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

    use super::*;

    const EPS: f32 = 1e-9f32;

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

        assert_relative_eq!(
            1.0,
            distance_squared_between_two_segments(
                Vec3::new(1.0, 0.0, 0.0),
                UnitVec3::new_normalized(0.0, 1.0, 0.0),
                1.0,
                UnitVec3::new_normalized(0.0, 1.0, 0.0),
                1.0
            ),
            epsilon = EPS
        );

        assert_relative_eq!(
            1.0,
            distance_squared_between_two_segments(
                Vec3::new(1.0, 0.0, 0.0),
                UnitVec3::new_normalized(-1.0, 1.0, 0.0),
                1.0,
                UnitVec3::new_normalized(1.0, 1.0, 0.0),
                1.0
            ),
            epsilon = EPS
        );
    }

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

        assert_relative_eq!(
            1.0,
            distance_squared_point_to_segment(
                Point3::new(0.0, 0.0, 0.0),
                Point3::new(1.0, 0.0, 0.0),
                Point3::new(2.0, 0.0, 0.0),
                1.0,
            )
        );

        assert_relative_eq!(
            1.0,
            distance_squared_point_to_segment(
                Point3::new(3.0, 0.0, 0.0),
                Point3::new(1.0, 0.0, 0.0),
                Point3::new(2.0, 0.0, 0.0),
                1.0,
            )
        );

        assert_relative_eq!(
            0.0,
            distance_squared_point_to_segment(
                Point3::new(1.5, 0.0, 0.0),
                Point3::new(1.0, 0.0, 0.0),
                Point3::new(2.0, 0.0, 0.0),
                1.0,
            )
        );

        assert_relative_eq!(
            2.0,
            distance_squared_point_to_segment(
                Point3::new(0.0, 1.0, 0.0),
                Point3::new(1.0, 0.0, 0.0),
                Point3::new(2.0, 0.0, 0.0),
                1.0,
            )
        );

        assert_relative_eq!(
            2.0,
            distance_squared_point_to_segment(
                Point3::new(3.0, 1.0, 0.0),
                Point3::new(1.0, 0.0, 0.0),
                Point3::new(2.0, 0.0, 0.0),
                1.0,
            )
        );

        assert_relative_eq!(
            1.0,
            distance_squared_point_to_segment(
                Point3::new(1.5, 1.0, 0.0),
                Point3::new(1.0, 0.0, 0.0),
                Point3::new(2.0, 0.0, 0.0),
                1.0,
            )
        );
    }
}