rs_math3d/

primitives.rs

// Copyright 2020-Present (c) Raja Lehtihet & Wael El Oraiby
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are met:
//
// 1. Redistributions of source code must retain the above copyright notice,
// this list of conditions and the following disclaimer.
//
// 2. Redistributions in binary form must reproduce the above copyright notice,
// this list of conditions and the following disclaimer in the documentation
// and/or other materials provided with the distribution.
//
// 3. Neither the name of the copyright holder nor the names of its contributors
// may be used to endorse or promote products derived from this software without
// specific prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
// POSSIBILITY OF SUCH DAMAGE.

//! Geometric primitives for 3D graphics and collision detection.
//!
//! This module provides common geometric shapes and primitives used in
//! 3D graphics, physics simulations, and collision detection systems.
//!
//! Discrete storage types such as rectangles and boxes work with integer scalars.
//! Metric and analytic primitives such as lines, rays, planes, spheres, and
//! triangles are intended for floating-point use.
//!
//! Constructors that normalize directions or normals provide checked `try_`
//! variants. The unchecked variants assume non-degenerate input and panic when
//! that precondition is violated.
//!
//! # Examples
//!
//! ```
//! use rs_math3d::primitives::{Ray, Plane, Tri3};
//! use rs_math3d::vector::Vector3;
//! use rs_math3d::EPS_F32;
//!
//! // Create a ray from origin pointing along +X axis
//! let ray = Ray::new(
//!     &Vector3::new(0.0f32, 0.0, 0.0),
//!     &Vector3::new(1.0, 0.0, 0.0),
//!     EPS_F32,
//! ).unwrap();
//!
//! // Create a plane at z=5 facing down
//! let plane = Plane::new(
//!     &Vector3::new(0.0f32, 0.0, -1.0),
//!     &Vector3::new(0.0, 0.0, 5.0)
//! );
//!
//! let checked = Plane::try_new(
//!     &Vector3::new(0.0f32, 0.0, -1.0),
//!     &Vector3::new(0.0, 0.0, 5.0),
//!     EPS_F32,
//! ).expect("plane normal should be valid");
//! assert_eq!(plane.constant(), checked.constant());
//! ```

use crate::scalar::*;
use crate::vector::*;
use num_traits::{One, Zero};

/// A 2D dimension with width and height.
#[repr(C)]
#[derive(Debug, Clone, Copy, Default)]
pub struct Dimension<T: Scalar> {
    /// Width component.
    pub width: T,
    /// Height component.
    pub height: T,
}

impl<T: Scalar> Dimension<T> {
    /// Creates a new dimension with the specified width and height.
    pub fn new(width: T, height: T) -> Self {
        Self { width, height }
    }
}

/// A 2D axis-aligned rectangle.
///
/// Defined by its top-left corner position (x, y) and dimensions (width, height).
#[repr(C)]
#[derive(Debug, Clone, Copy, Default)]
pub struct Rect<T: Scalar> {
    /// X coordinate of the top-left corner.
    pub x: T,
    /// Y coordinate of the top-left corner.
    pub y: T,
    /// Rectangle width.
    pub width: T,
    /// Rectangle height.
    pub height: T,
}

impl<T: Scalar> Rect<T> {
    /// Creates a new rectangle from position and dimensions.
    pub fn new(x: T, y: T, width: T, height: T) -> Self {
        Self {
            x,
            y,
            width,
            height,
        }
    }
    /// Creates a rectangle from two corner points.
    ///
    /// The points don't need to be min/max - they will be sorted.
    pub fn from(min_vec: &Vector2<T>, max_vec: &Vector2<T>) -> Self {
        let min_x = T::min(min_vec.x, max_vec.x);
        let min_y = T::min(min_vec.y, max_vec.y);
        let max_x = T::max(min_vec.x, max_vec.x);
        let max_y = T::max(min_vec.y, max_vec.y);
        Self {
            x: min_x,
            y: min_y,
            width: max_x - min_x,
            height: max_y - min_y,
        }
    }

    /// Returns the minimum corner (top-left) of the rectangle.
    pub fn min(&self) -> Vector2<T> {
        Vector2::new(self.x, self.y)
    }
    /// Returns the maximum corner (bottom-right) of the rectangle.
    pub fn max(&self) -> Vector2<T> {
        Vector2::new(self.x + self.width, self.y + self.height)
    }
    /// Computes the intersection of two rectangles.
    ///
    /// Returns `None` if the rectangles don't overlap.
    pub fn intersect(&self, other: &Self) -> Option<Self> {
        let smx = self.max();
        let smn = self.min();
        let omx = other.max();
        let omn = other.min();

        if smx.x < omn.x || smx.y < omn.y || smn.x > omx.x || smn.y > omx.y {
            return None;
        }

        let min_vec = Vector2::max(&self.min(), &other.min());
        let max_vec = Vector2::min(&self.max(), &other.max());
        Some(Self::from(&min_vec, &max_vec))
    }

    /// Checks if a point is inside the rectangle.
    pub fn contains(&self, p: &Vector2<T>) -> bool {
        p.x >= self.x && p.y >= self.y && p.x <= self.x + self.width && p.y <= self.y + self.height
    }
}

/// A 3D axis-aligned bounding box (AABB).
///
/// Defined by its minimum and maximum corners.
#[repr(C)]
#[derive(Debug, Clone, Copy, Default)]
pub struct Box3<T: Scalar> {
    /// Minimum corner.
    pub min: Vector3<T>,
    /// Maximum corner.
    pub max: Vector3<T>,
}

impl<T: Scalar> Box3<T> {
    /// Creates a new box from two corner points.
    ///
    /// The points are automatically sorted to find min/max.
    pub fn new(v0: &Vector3<T>, v1: &Vector3<T>) -> Self {
        Self {
            min: Vector3::min(v0, v1),
            max: Vector3::max(v0, v1),
        }
    }
    /// Checks if two boxes overlap.
    pub fn overlap(&self, other: &Self) -> bool {
        if self.max.x < other.min.x {
            return false;
        }
        if self.max.y < other.min.y {
            return false;
        }
        if self.max.z < other.min.z {
            return false;
        }

        if self.min.x > other.max.x {
            return false;
        }
        if self.min.y > other.max.y {
            return false;
        }
        if self.min.z > other.max.z {
            return false;
        }
        true
    }

    /// Expands the box to include a point.
    pub fn add(&self, p: &Vector3<T>) -> Self {
        Self {
            min: Vector3::min(p, &self.min),
            max: Vector3::max(p, &self.max),
        }
    }
}

impl<T: FloatScalar> Box3<T> {
    /// Returns the center point of the box.
    pub fn center(&self) -> Vector3<T> {
        (self.max + self.min) * T::half()
    }
    /// Returns the half-extents of the box from center to max.
    pub fn extent(&self) -> Vector3<T> {
        self.max - self.center()
    }

    /// Subdivides the box into 8 octants.
    ///
    /// Returns an array of 8 boxes representing the octree subdivision.
    pub fn subdivide(&self) -> [Self; 8] {
        let cube_table: [Vector3<i32>; 8] = [
            Vector3::new(0, 1, 0),
            Vector3::new(1, 1, 0),
            Vector3::new(1, 1, 1),
            Vector3::new(0, 1, 1),
            Vector3::new(0, 0, 0),
            Vector3::new(1, 0, 0),
            Vector3::new(1, 0, 1),
            Vector3::new(0, 0, 1),
        ];

        let ps: [Vector3<T>; 2] = [self.min, self.max];
        let mut vs = [Vector3::zero(); 8];
        for i in 0..8 {
            vs[i] = Vector3::new(
                ps[cube_table[i].x as usize].x,
                ps[cube_table[i].y as usize].y,
                ps[cube_table[i].z as usize].z,
            );
        }

        let c = self.center();
        let mut out = [Box3 {
            min: Vector3::zero(),
            max: Vector3::zero(),
        }; 8];
        for i in 0..8 {
            out[i] = Self::new(&Vector3::min(&c, &vs[i]), &Vector3::max(&c, &vs[i]));
        }
        out
    }
}

/// An infinite line in 2D or 3D space.
///
/// Defined by a point on the line and a direction vector.
#[repr(C)]
#[derive(Debug, Clone, Copy, Default)]
pub struct Line<T: Scalar, V: Vector<T>> {
    /// Point on the line.
    pub p: V,
    /// Direction of the line.
    pub d: V,
    t: core::marker::PhantomData<T>,
}

impl<T: FloatScalar, V: FloatVector<T>> Line<T, V> {
    /// Creates a new line from a point and direction.
    ///
    /// Returns `None` if the direction is too small.
    pub fn new(p: &V, d: &V, epsilon: T) -> Option<Self> {
        let d_ss = V::dot(d, d);
        if d_ss <= epsilon * epsilon {
            return None;
        }
        Some(Self {
            p: *p,
            d: *d,
            t: core::marker::PhantomData,
        })
    }
    /// Creates a line from two points.
    ///
    /// The line passes through both points, with direction from s to e.
    ///
    /// Returns `None` if the points are too close.
    pub fn from_start_end(s: &V, e: &V, epsilon: T) -> Option<Self> {
        let dir = *e - *s;
        Self::new(s, &dir, epsilon)
    }
    /// Finds the closest point on the line to a given point.
    ///
    /// Returns:
    /// - `t`: Parameter value where the closest point occurs
    /// - Point: The closest point on the line
    ///
    /// Returns `None` if the line direction is too small.
    pub fn closest_point_on_line(&self, p: &V, epsilon: T) -> Option<(T, V)> {
        let p_dir = *p - self.p;

        let d_sp = V::dot(&self.d, &p_dir);
        let d_ss = V::dot(&self.d, &self.d);

        if d_ss <= epsilon * epsilon {
            return None;
        }

        let t = d_sp / d_ss;

        Some((t, self.p + self.d * t))
    }

    /// Returns a line with normalized direction vector.
    ///
    /// Returns `None` if the direction is too small.
    pub fn normalize(&self, epsilon: T) -> Option<Self> {
        let d_ss = V::dot(&self.d, &self.d);
        if d_ss <= epsilon * epsilon {
            return None;
        }
        let inv_len = <T as One>::one() / d_ss.tsqrt();
        Some(Self {
            p: self.p,
            d: self.d * inv_len,
            t: core::marker::PhantomData,
        })
    }
}

/// Finds the shortest segment connecting two 3D lines.
///
/// Returns `None` if the lines are parallel (within epsilon tolerance).
pub fn shortest_segment3d_between_lines3d<T: FloatScalar>(
    line0: &Line<T, Vector3<T>>,
    line1: &Line<T, Vector3<T>>,
    epsilon: T,
) -> Option<Segment<T, Vector3<T>>> {
    let s0 = line0.p;
    let s1 = line1.p;

    let d1 = line1.d;
    let d0 = line0.d;

    let eps_sq = epsilon * epsilon;
    let d0_len_sq = Vector3::dot(&d0, &d0);
    let d1_len_sq = Vector3::dot(&d1, &d1);
    if d0_len_sq <= eps_sq || d1_len_sq <= eps_sq {
        return None;
    }

    let cross = Vector3::cross(&d1, &d0);
    let cross_len_sq = Vector3::dot(&cross, &cross);
    if cross_len_sq <= eps_sq {
        return None;
    }

    let normal = Vector3::normalize(&cross);
    let n0 = Vector3::normalize(&Vector3::cross(&normal, &d0));
    let n1 = Vector3::normalize(&Vector3::cross(&normal, &d1));

    let plane0 = Plane::try_new(&n0, &s0, epsilon)?;
    let plane1 = Plane::try_new(&n1, &s1, epsilon)?;

    let p1 = plane0.intersect_line(line1, epsilon);
    let p0 = plane1.intersect_line(line0, epsilon);

    match (p0, p1) {
        (Some((_, s)), Some((_, e))) => Some(Segment::new(&s, &e)),
        _ => None,
    }
}

/// A line segment with defined start and end points.
#[repr(C)]
#[derive(Clone, Copy, Default)]
pub struct Segment<T: Scalar, V: Vector<T>> {
    /// Start point.
    pub s: V,
    /// End point.
    pub e: V,
    t: core::marker::PhantomData<T>,
}

impl<T: Scalar, V: Vector<T>> Segment<T, V> {
    /// Creates a new segment from start to end point.
    pub fn new(s: &V, e: &V) -> Self {
        Self {
            s: *s,
            e: *e,
            t: core::marker::PhantomData,
        }
    }
}

impl<T: FloatScalar, V: FloatVector<T>> Segment<T, V> {
    /// Finds the closest point on the segment to a given point.
    ///
    /// Returns:
    /// - `t`: Parameter value \[0,1\] where 0=start, 1=end
    /// - Point: The closest point on the segment
    ///
    /// Returns `None` if the segment is too small.
    pub fn closest_point_on_segment(&self, p: &V, epsilon: T) -> Option<(T, V)> {
        let dir = self.e - self.s;
        let p_dir = *p - self.s;

        let d_sp = V::dot(&dir, &p_dir);
        let d_ss = V::dot(&dir, &dir);

        if d_ss <= epsilon * epsilon {
            return None;
        }

        if d_sp < <T as Zero>::zero() {
            return Some((<T as Zero>::zero(), self.s));
        } else if d_sp > d_ss {
            return Some((<T as One>::one(), self.e));
        }

        let t = d_sp / d_ss;

        Some((t, self.s + dir * t))
    }
    /// Computes the distance from a point to the segment.
    ///
    /// Returns `None` if the segment is too small.
    pub fn distance(&self, p: &V, epsilon: T) -> Option<T> {
        self.closest_point_on_segment(p, epsilon)
            .map(|(_, p_on_seg)| V::length(&(p_on_seg - *p)))
    }
}

/// A ray with an origin and direction.
///
/// Commonly used for ray casting and intersection tests.
#[repr(C)]
#[derive(Clone, Copy, Default)]
pub struct Ray<T: Scalar, V: Vector<T>> {
    /// Ray origin.
    pub start: V,
    /// Ray direction (typically normalized).
    pub direction: V,
    t: core::marker::PhantomData<T>,
}

impl<T: FloatScalar, V: FloatVector<T>> Ray<T, V> {
    /// Creates a new ray with normalized direction.
    ///
    /// Returns `None` if the direction is too small.
    pub fn new(start: &V, direction: &V, epsilon: T) -> Option<Self> {
        let d_ss = V::dot(direction, direction);
        if d_ss <= epsilon * epsilon {
            return None;
        }
        let inv_len = <T as One>::one() / d_ss.tsqrt();
        Some(Self {
            start: *start,
            direction: *direction * inv_len,
            t: core::marker::PhantomData,
        })
    }
}

impl<T: FloatScalar> Ray<T, Vector3<T>> {
    /// Computes ray-plane intersection.
    ///
    /// Returns the intersection point, or `None` if the ray doesn't hit the plane
    /// (parallel or pointing away).
    ///
    /// Returns `None` if the ray is parallel to the plane within `epsilon`.
    pub fn intersect_plane(&self, p: &Plane<T>, epsilon: T) -> Option<Vector3<T>> {
        let n = p.normal();
        let denom = Vector3::dot(&n, &self.direction);
        if denom.tabs() <= epsilon {
            return None;
        }
        let t: T = -(p.d + Vector3::dot(&n, &self.start)) / denom;
        if t < <T as Zero>::zero() {
            None
        } else {
            Some(self.direction * t + self.start)
        }
    }
}

////////////////////////////////////////////////////////////////////////////////
/// A sphere defined by center and radius.
////////////////////////////////////////////////////////////////////////////////
#[repr(C)]
#[derive(Clone, Copy, Default)]
pub struct Sphere3<T: FloatScalar> {
    /// Sphere center.
    pub center: Vector3<T>,
    /// Sphere radius.
    pub radius: T,
}

impl<T: FloatScalar> Sphere3<T> {
    /// Creates a sphere from center and radius.
    ///
    /// The radius is canonicalized with `abs(radius)`, so negative inputs are
    /// treated the same as their positive magnitude.
    pub fn new(center: Vector3<T>, radius: T) -> Self {
        Self {
            center,
            radius: radius.tabs(),
        }
    }
}

////////////////////////////////////////////////////////////////////////////////
/// A triangle defined by three vertices.
////////////////////////////////////////////////////////////////////////////////
#[repr(C)]
#[derive(Clone, Copy, Default)]
pub struct Tri3<T: FloatScalar> {
    vertices: [Vector3<T>; 3],
}

impl<T: FloatScalar> Tri3<T> {
    /// Creates a triangle from three vertices.
    pub fn new(vertices: [Vector3<T>; 3]) -> Self {
        Self { vertices }
    }
    /// Returns the triangle vertices.
    pub fn vertices(&self) -> &[Vector3<T>; 3] {
        &self.vertices
    }

    /// Computes barycentric coordinates of a point in the triangle plane.
    ///
    /// This method assumes the triangle is non-degenerate. When the triangle
    /// area is zero, the denominator vanishes and the returned coordinates are
    /// non-finite.
    pub fn barycentric_coordinates(&self, pt: &Vector3<T>) -> Vector3<T> {
        let v0 = self.vertices[0];
        let v1 = self.vertices[1];
        let v2 = self.vertices[2];
        let e0 = v1 - v0;
        let e1 = v2 - v0;
        let vp = *pt - v0;

        let d00 = Vector3::dot(&e0, &e0);
        let d01 = Vector3::dot(&e0, &e1);
        let d11 = Vector3::dot(&e1, &e1);
        let d20 = Vector3::dot(&vp, &e0);
        let d21 = Vector3::dot(&vp, &e1);
        let denom = d00 * d11 - d01 * d01;
        let v = (d11 * d20 - d01 * d21) / denom;
        let w = (d00 * d21 - d01 * d20) / denom;
        let u = <T as One>::one() - v - w;
        Vector3::new(u, v, w)
    }
}

////////////////////////////////////////////////////////////////////////////////
/// A plane defined by ax + by + cz + d = 0.
////////////////////////////////////////////////////////////////////////////////
#[repr(C)]
#[derive(Clone, Copy, Default)]
pub struct Plane<T: Scalar> {
    a: T,
    b: T,
    c: T,
    d: T,
}

impl<T: Scalar> Plane<T> {
    /// Returns the plane normal vector.
    pub fn normal(&self) -> Vector3<T> {
        Vector3::new(self.a, self.b, self.c)
    }
    /// Returns the plane constant term (d).
    pub fn constant(&self) -> T {
        self.d
    }
}

impl<T: FloatScalar> Plane<T> {
    /// Creates a plane from a normal and a point on the plane.
    ///
    /// # Panics
    /// Panics if `n` is too small to normalize. Use [`Plane::try_new`] when the
    /// input may be degenerate.
    pub fn new(n: &Vector3<T>, p: &Vector3<T>) -> Self {
        Self::try_new(n, p, T::epsilon()).expect("plane normal must be non-zero")
    }

    /// Creates a plane from a normal and a point on the plane.
    ///
    /// Returns `None` if `n` is too small to normalize.
    pub fn try_new(n: &Vector3<T>, p: &Vector3<T>, epsilon: T) -> Option<Self> {
        let norm = n.try_normalize(epsilon)?;
        let d = Vector3::dot(&norm, p);
        Some(Self {
            a: norm.x,
            b: norm.y,
            c: norm.z,
            d: -d,
        })
    }

    /// Creates a plane from triangle vertices.
    ///
    /// # Panics
    /// Panics if the triangle is degenerate. Use [`Plane::try_from_tri`] when
    /// the input may be degenerate.
    pub fn from_tri(v0: &Vector3<T>, v1: &Vector3<T>, v2: &Vector3<T>) -> Self {
        Self::try_from_tri(v0, v1, v2, T::epsilon()).expect("triangle must define a plane")
    }

    /// Creates a plane from triangle vertices.
    ///
    /// Returns `None` if the triangle is degenerate.
    pub fn try_from_tri(
        v0: &Vector3<T>,
        v1: &Vector3<T>,
        v2: &Vector3<T>,
        epsilon: T,
    ) -> Option<Self> {
        let n = try_tri_normal(v0, v1, v2, epsilon)?;
        Self::try_new(&n, v0, epsilon)
    }
    /// Creates a plane from quad vertices.
    ///
    /// # Panics
    /// Panics if the quad diagonals do not define a plane. Use
    /// [`Plane::try_from_quad`] when the input may be degenerate.
    ///
    /// This routine does not validate that the four vertices are coplanar.
    /// For non-coplanar quads it returns the diagonal-derived representative
    /// plane described by [`Plane::try_from_quad`].
    pub fn from_quad(v0: &Vector3<T>, v1: &Vector3<T>, v2: &Vector3<T>, v3: &Vector3<T>) -> Self {
        Self::try_from_quad(v0, v1, v2, v3, T::epsilon()).expect("quad must define a plane")
    }

    /// Creates a plane from quad vertices.
    ///
    /// Returns `None` if the quad diagonals do not define a plane.
    ///
    /// The plane normal is computed from the quad diagonals and anchored at the
    /// vertex centroid. This is a representative plane for the quad and does
    /// not validate coplanarity. For non-coplanar input, the returned plane is
    /// not guaranteed to pass through any of the vertices.
    pub fn try_from_quad(
        v0: &Vector3<T>,
        v1: &Vector3<T>,
        v2: &Vector3<T>,
        v3: &Vector3<T>,
        epsilon: T,
    ) -> Option<Self> {
        let n = try_quad_normal(v0, v1, v2, v3, epsilon)?;
        let c = (*v0 + *v1 + *v2 + *v3) * T::quarter();
        Self::try_new(&n, &c, epsilon)
    }

    /// Intersects the plane with a ray.
    pub fn intersect_ray(&self, r: &Ray<T, Vector3<T>>, epsilon: T) -> Option<Vector3<T>> {
        r.intersect_plane(self, epsilon)
    }

    /// Computes line-plane intersection.
    ///
    /// Returns the parameter t and intersection point, or `None` if parallel.
    pub fn intersect_line(
        &self,
        line: &Line<T, Vector3<T>>,
        epsilon: T,
    ) -> Option<(T, Vector3<T>)> {
        let s = line.p;
        let dir = line.d;
        let n = self.normal();

        let denom = Vector3::dot(&n, &dir);
        if denom.tabs() < epsilon {
            None
        } else {
            let t = -(self.constant() + Vector3::dot(&n, &s)) / denom;
            Some((t, dir * t + s))
        }
    }
}

////////////////////////////////////////////////////////////////////////////////
/// A parametric plane defined by center and axis vectors.
////////////////////////////////////////////////////////////////////////////////
#[repr(C)]
#[derive(Clone, Copy, Default)]
pub struct ParametricPlane<T: Scalar> {
    /// Plane center point.
    pub center: Vector3<T>,
    /// Plane X axis direction.
    pub x_axis: Vector3<T>,
    /// Plane Y axis direction.
    pub y_axis: Vector3<T>,
}

impl<T: FloatScalar> ParametricPlane<T> {
    /// Creates a parametric plane from center and axes.
    pub fn new(center: &Vector3<T>, x_axis: &Vector3<T>, y_axis: &Vector3<T>) -> Self {
        Self {
            center: *center,
            x_axis: *x_axis,
            y_axis: *y_axis,
        }
    }

    /// Converts the parametric plane to an infinite plane.
    pub fn plane(&self) -> Plane<T> {
        self.try_plane(T::epsilon())
            .expect("parametric plane axes must span a plane")
    }

    /// Converts the parametric plane to an infinite plane.
    ///
    /// Returns `None` if the axes are too small or parallel.
    pub fn try_plane(&self, epsilon: T) -> Option<Plane<T>> {
        let normal = self.try_normal(epsilon)?;
        Plane::try_new(&normal, &self.center, epsilon)
    }

    /// Computes the normal vector (cross product of axes).
    pub fn normal(&self) -> Vector3<T> {
        self.try_normal(T::epsilon())
            .expect("parametric plane axes must span a plane")
    }

    /// Computes the normal vector (cross product of axes).
    ///
    /// Returns `None` if the axes are too small or parallel.
    pub fn try_normal(&self, epsilon: T) -> Option<Vector3<T>> {
        Vector3::cross(&self.x_axis, &self.y_axis).try_normalize(epsilon)
    }

    /// Intersects the plane with a ray.
    pub fn intersect_ray(&self, r: &Ray<T, Vector3<T>>, epsilon: T) -> Option<Vector3<T>> {
        let plane = self.try_plane(epsilon)?;
        r.intersect_plane(&plane, epsilon)
    }

    /// Intersects the plane with a line.
    pub fn intersect_line(
        &self,
        line: &Line<T, Vector3<T>>,
        epsilon: T,
    ) -> Option<(T, Vector3<T>)> {
        let plane = self.try_plane(epsilon)?;
        plane.intersect_line(line, epsilon)
    }

    /// Projects a 3D point onto the plane's 2D coordinates.
    ///
    /// The coordinates are solved against the 2x2 Gram matrix of the plane
    /// axes, so non-orthogonal axes are handled correctly. The axes must still
    /// be linearly independent; otherwise the returned coordinates are
    /// non-finite.
    pub fn project(&self, v: &Vector3<T>) -> Vector2<T> {
        let p = *v - self.center;
        let g00 = Vector3::dot(&self.x_axis, &self.x_axis);
        let g01 = Vector3::dot(&self.x_axis, &self.y_axis);
        let g11 = Vector3::dot(&self.y_axis, &self.y_axis);
        let rhs0 = Vector3::dot(&p, &self.x_axis);
        let rhs1 = Vector3::dot(&p, &self.y_axis);
        let det = g00 * g11 - g01 * g01;

        let x_coord = (rhs0 * g11 - rhs1 * g01) / det;
        let y_coord = (rhs1 * g00 - rhs0 * g01) / det;
        Vector2::new(x_coord, y_coord)
    }
}

////////////////////////////////////////////////////////////////////////////////
/// Computes the normal vector of a triangle.
////////////////////////////////////////////////////////////////////////////////
pub fn tri_normal<T: FloatScalar>(v0: &Vector3<T>, v1: &Vector3<T>, v2: &Vector3<T>) -> Vector3<T> {
    try_tri_normal(v0, v1, v2, T::epsilon()).expect("triangle must be non-degenerate")
}

/// Computes the normal vector of a triangle.
///
/// Returns `None` if the triangle is degenerate.
pub fn try_tri_normal<T: FloatScalar>(
    v0: &Vector3<T>,
    v1: &Vector3<T>,
    v2: &Vector3<T>,
    epsilon: T,
) -> Option<Vector3<T>> {
    let v10 = *v1 - *v0;
    let v20 = *v2 - *v0;
    Vector3::cross(&v10, &v20).try_normalize(epsilon)
}

/// Computes the normal vector of a quadrilateral.
///
/// Uses the cross product of the two diagonals for a more stable result.
///
/// For non-coplanar quads this is a representative diagonal-based normal, not a
/// proof that the four vertices lie on a single plane.
pub fn quad_normal<T: FloatScalar>(
    v0: &Vector3<T>,
    v1: &Vector3<T>,
    v2: &Vector3<T>,
    v3: &Vector3<T>,
) -> Vector3<T> {
    try_quad_normal(v0, v1, v2, v3, T::epsilon()).expect("quad must be non-degenerate")
}

/// Computes the normal vector of a quadrilateral.
///
/// Returns `None` if the quad diagonals are too small or parallel.
pub fn try_quad_normal<T: FloatScalar>(
    v0: &Vector3<T>,
    v1: &Vector3<T>,
    v2: &Vector3<T>,
    v3: &Vector3<T>,
    epsilon: T,
) -> Option<Vector3<T>> {
    let v20 = *v2 - *v0;
    let v31 = *v3 - *v1;
    Vector3::cross(&v20, &v31).try_normalize(epsilon)
}

#[cfg(test)]
mod tests {
    use super::*;
    #[test]
    pub fn test_barycentric() {
        let v0 = Vector3::new(0.0, 0.0, 0.0);
        let v1 = Vector3::new(0.0, 1.0, 0.0);
        let v2 = Vector3::new(0.0, 0.0, 1.0);

        let tri = Tri3::new([v0, v1, v2]);
        let pp0 = tri.barycentric_coordinates(&v0);
        assert!(f32::abs(pp0.x - 1.0) < 0.01);
        assert!(f32::abs(pp0.y) < 0.01);
        assert!(f32::abs(pp0.z) < 0.01);

        let pp1 = tri.barycentric_coordinates(&v1);
        assert!(f32::abs(pp1.x) < 0.01);
        assert!(f32::abs(pp1.y - 1.0) < 0.01);
        assert!(f32::abs(pp1.z) < 0.01);

        let pp2 = tri.barycentric_coordinates(&v2);
        assert!(f32::abs(pp2.x) < 0.01);
        assert!(f32::abs(pp2.y) < 0.01);
        assert!(f32::abs(pp2.z - 1.0) < 0.01);
    }

    #[test]
    pub fn test_barycentric_translated() {
        // Test with a triangle not at the origin
        let v0 = Vector3::new(1.0, 2.0, 3.0);
        let v1 = Vector3::new(4.0, 2.0, 3.0);
        let v2 = Vector3::new(1.0, 5.0, 3.0);

        let tri = Tri3::new([v0, v1, v2]);

        // Test vertices
        let pp0 = tri.barycentric_coordinates(&v0);
        assert!(f32::abs(pp0.x - 1.0) < 0.001);
        assert!(f32::abs(pp0.y) < 0.001);
        assert!(f32::abs(pp0.z) < 0.001);

        let pp1 = tri.barycentric_coordinates(&v1);
        assert!(f32::abs(pp1.x) < 0.001);
        assert!(f32::abs(pp1.y - 1.0) < 0.001);
        assert!(f32::abs(pp1.z) < 0.001);

        let pp2 = tri.barycentric_coordinates(&v2);
        assert!(f32::abs(pp2.x) < 0.001);
        assert!(f32::abs(pp2.y) < 0.001);
        assert!(f32::abs(pp2.z - 1.0) < 0.001);

        // Test center point
        let center = (v0 + v1 + v2) / 3.0;
        let pp_center = tri.barycentric_coordinates(&center);
        assert!(f32::abs(pp_center.x - 1.0 / 3.0) < 0.001);
        assert!(f32::abs(pp_center.y - 1.0 / 3.0) < 0.001);
        assert!(f32::abs(pp_center.z - 1.0 / 3.0) < 0.001);

        // Verify barycentric coordinates sum to 1
        assert!(f32::abs((pp_center.x + pp_center.y + pp_center.z) - 1.0) < 0.001);
    }

    #[test]
    pub fn test_barycentric_edge_midpoints() {
        let v0 = Vector3::new(0.0, 0.0, 0.0);
        let v1 = Vector3::new(2.0, 0.0, 0.0);
        let v2 = Vector3::new(0.0, 2.0, 0.0);

        let tri = Tri3::new([v0, v1, v2]);

        // Midpoint of edge v0-v1
        let mid01 = (v0 + v1) / 2.0;
        let pp_mid01 = tri.barycentric_coordinates(&mid01);
        assert!(f32::abs(pp_mid01.x - 0.5) < 0.001);
        assert!(f32::abs(pp_mid01.y - 0.5) < 0.001);
        assert!(f32::abs(pp_mid01.z) < 0.001);

        // Midpoint of edge v0-v2
        let mid02 = (v0 + v2) / 2.0;
        let pp_mid02 = tri.barycentric_coordinates(&mid02);
        assert!(f32::abs(pp_mid02.x - 0.5) < 0.001);
        assert!(f32::abs(pp_mid02.y) < 0.001);
        assert!(f32::abs(pp_mid02.z - 0.5) < 0.001);

        // Midpoint of edge v1-v2
        let mid12 = (v1 + v2) / 2.0;
        let pp_mid12 = tri.barycentric_coordinates(&mid12);
        assert!(f32::abs(pp_mid12.x) < 0.001);
        assert!(f32::abs(pp_mid12.y - 0.5) < 0.001);
        assert!(f32::abs(pp_mid12.z - 0.5) < 0.001);
    }

    #[test]
    pub fn test_barycentric_degenerate_triangle() {
        let v0 = Vector3::new(0.0f32, 0.0, 0.0);
        let v1 = Vector3::new(1.0f32, 0.0, 0.0);
        let v2 = Vector3::new(2.0f32, 0.0, 0.0);

        let tri = Tri3::new([v0, v1, v2]);
        let test_point = Vector3::new(0.5f32, 0.0, 0.0);
        let bary = tri.barycentric_coordinates(&test_point);
        assert!(!bary.x.is_finite());
        assert!(!bary.y.is_finite());
        assert!(!bary.z.is_finite());
    }

    #[test]
    pub fn test_barycentric_interpolation() {
        // Test that we can reconstruct points using barycentric coordinates
        let v0 = Vector3::new(1.0, 0.0, 0.0);
        let v1 = Vector3::new(0.0, 1.0, 0.0);
        let v2 = Vector3::new(0.0, 0.0, 1.0);

        let tri = Tri3::new([v0, v1, v2]);

        // Test arbitrary point inside triangle
        let test_point = Vector3::new(0.2, 0.3, 0.5);
        let bary = tri.barycentric_coordinates(&test_point);

        // Reconstruct the point using barycentric coordinates
        let reconstructed = v0 * bary.x + v1 * bary.y + v2 * bary.z;

        assert!(f32::abs(reconstructed.x - test_point.x) < 0.001);
        assert!(f32::abs(reconstructed.y - test_point.y) < 0.001);
        assert!(f32::abs(reconstructed.z - test_point.z) < 0.001);
    }

    #[test]
    pub fn test_box3_overlap() {
        let a = Box3::new(&Vector3::new(0.0, 0.0, 0.0), &Vector3::new(1.0, 1.0, 1.0));
        let b = Box3::new(&Vector3::new(0.5, 0.5, 0.5), &Vector3::new(1.5, 1.5, 1.5));
        assert!(a.overlap(&b));

        let c = Box3::new(&Vector3::new(2.0, 0.0, 0.0), &Vector3::new(3.0, 1.0, 1.0));
        assert!(!a.overlap(&c));

        let d = Box3::new(&Vector3::new(0.0, 2.0, 0.0), &Vector3::new(1.0, 3.0, 1.0));
        assert!(!a.overlap(&d));

        let e = Box3::new(&Vector3::new(0.0, 0.0, 2.0), &Vector3::new(1.0, 1.0, 3.0));
        assert!(!a.overlap(&e));

        let f = Box3::new(&Vector3::new(1.0, 0.0, 0.0), &Vector3::new(2.0, 1.0, 1.0));
        assert!(a.overlap(&f));
    }

    #[test]
    fn test_line_new_zero_direction() {
        let p = Vector3::new(0.0f32, 0.0, 0.0);
        let d = Vector3::new(0.0f32, 0.0, 0.0);
        assert!(Line::new(&p, &d, EPS_F32).is_none());
    }

    #[test]
    fn test_line_from_start_end_zero_direction() {
        let p = Vector3::new(1.0f32, 2.0, 3.0);
        assert!(Line::from_start_end(&p, &p, EPS_F32).is_none());
    }

    #[test]
    fn test_line_closest_point_valid() {
        let p = Vector3::new(0.0f32, 0.0, 0.0);
        let d = Vector3::new(1.0f32, 0.0, 0.0);
        let line = Line::new(&p, &d, EPS_F32).expect("line should be valid");
        let target = Vector3::new(2.0f32, 1.0, 0.0);
        let (t, closest) = line
            .closest_point_on_line(&target, EPS_F32)
            .expect("closest point should exist");
        assert!((t - 2.0).abs() < 0.001);
        assert!((closest.x - 2.0).abs() < 0.001);
        assert!(closest.y.abs() < 0.001);
        assert!(closest.z.abs() < 0.001);
    }

    #[test]
    fn test_line_normalize_zero_direction() {
        let line = Line {
            p: Vector3::new(0.0f32, 0.0, 0.0),
            d: Vector3::new(0.0f32, 0.0, 0.0),
            t: core::marker::PhantomData,
        };
        assert!(line.normalize(EPS_F32).is_none());
    }

    #[test]
    fn test_line_normalize_valid_direction() {
        let p = Vector3::new(0.0f32, 0.0, 0.0);
        let d = Vector3::new(2.0f32, 0.0, 0.0);
        let line = Line::new(&p, &d, EPS_F32).expect("line should be valid");
        let norm = line.normalize(EPS_F32).expect("normalize should succeed");
        assert!((norm.d.length() - 1.0).abs() < 0.001);
    }

    #[test]
    fn test_segment_distance_zero_length() {
        let p = Vector3::new(0.0f32, 0.0, 0.0);
        let seg = Segment::new(&p, &p);
        let target = Vector3::new(1.0f32, 0.0, 0.0);
        assert!(seg.distance(&target, EPS_F32).is_none());
        assert!(seg.closest_point_on_segment(&target, EPS_F32).is_none());
    }

    #[test]
    fn test_ray_new_zero_direction() {
        let p = Vector3::new(0.0f32, 0.0, 0.0);
        let d = Vector3::new(0.0f32, 0.0, 0.0);
        assert!(Ray::new(&p, &d, EPS_F32).is_none());
    }

    #[test]
    fn test_sphere_new_abs_radius() {
        let sphere = Sphere3::new(Vector3::new(0.0f32, 0.0, 0.0), -2.5);
        assert!((sphere.radius - 2.5).abs() < 0.001);
    }

    #[test]
    fn test_shortest_segment_parallel_lines() {
        let p0 = Vector3::new(0.0f32, 0.0, 0.0);
        let p1 = Vector3::new(0.0f32, 1.0, 0.0);
        let d = Vector3::new(1.0f32, 0.0, 0.0);
        let l0 = Line::new(&p0, &d, EPS_F32).expect("line should be valid");
        let l1 = Line::new(&p1, &d, EPS_F32).expect("line should be valid");
        assert!(shortest_segment3d_between_lines3d(&l0, &l1, EPS_F32).is_none());
    }

    #[test]
    fn test_ray_intersect_plane_parallel() {
        let ray = Ray::new(
            &Vector3::new(0.0f32, 0.0, 0.0),
            &Vector3::new(1.0, 0.0, 0.0),
            EPS_F32,
        )
        .expect("ray should be valid");
        let plane = Plane::new(
            &Vector3::new(0.0f32, 1.0, 0.0),
            &Vector3::new(0.0, 0.0, 0.0),
        );
        assert!(ray.intersect_plane(&plane, EPS_F32).is_none());
        assert!(plane.intersect_ray(&ray, EPS_F32).is_none());
    }

    #[test]
    fn test_ray_intersect_plane_hit() {
        let ray = Ray::new(
            &Vector3::new(0.0f32, -1.0, 0.0),
            &Vector3::new(0.0, 1.0, 0.0),
            EPS_F32,
        )
        .expect("ray should be valid");
        let plane = Plane::new(
            &Vector3::new(0.0f32, 1.0, 0.0),
            &Vector3::new(0.0, 0.0, 0.0),
        );
        let hit = ray.intersect_plane(&plane, EPS_F32).expect("should hit");
        assert!(hit.y.abs() < 0.001);
    }

    #[test]
    fn test_plane_try_new_zero_normal_none() {
        let plane = Plane::try_new(
            &Vector3::new(0.0f32, 0.0, 0.0),
            &Vector3::new(0.0, 0.0, 0.0),
            EPS_F32,
        );
        assert!(plane.is_none());
    }

    #[test]
    fn test_box3_center_extent_subdivide() {
        let b = Box3::new(
            &Vector3::new(0.0f32, 0.0, 0.0),
            &Vector3::new(2.0, 2.0, 2.0),
        );
        let center = b.center();
        let extent = b.extent();
        assert!((center.x - 1.0).abs() < 0.001);
        assert!((center.y - 1.0).abs() < 0.001);
        assert!((center.z - 1.0).abs() < 0.001);
        assert!((extent.x - 1.0).abs() < 0.001);
        assert!((extent.y - 1.0).abs() < 0.001);
        assert!((extent.z - 1.0).abs() < 0.001);

        let subs = b.subdivide();
        assert_eq!(subs.len(), 8);
        for sub in subs.iter() {
            assert!(sub.min.x >= 0.0 && sub.min.y >= 0.0 && sub.min.z >= 0.0);
            assert!(sub.max.x <= 2.0 && sub.max.y <= 2.0 && sub.max.z <= 2.0);
        }
    }

    #[test]
    fn test_parametric_plane_project() {
        let plane = ParametricPlane::new(
            &Vector3::new(1.0f32, 2.0, 3.0),
            &Vector3::new(1.0, 0.0, 0.0),
            &Vector3::new(0.0, 1.0, 0.0),
        );
        let point = Vector3::new(3.0f32, 6.0, 3.0);
        let uv = plane.project(&point);
        assert!((uv.x - 2.0).abs() < 0.001);
        assert!((uv.y - 4.0).abs() < 0.001);
    }

    #[test]
    fn test_parametric_plane_project_non_orthogonal_axes() {
        let plane = ParametricPlane::new(
            &Vector3::new(0.0f32, 0.0, 0.0),
            &Vector3::new(1.0, 0.0, 0.0),
            &Vector3::new(1.0, 1.0, 0.0),
        );
        let point = Vector3::new(2.0f32, 1.0, 0.0);
        let uv = plane.project(&point);
        assert!((uv.x - 1.0).abs() < 0.001);
        assert!((uv.y - 1.0).abs() < 0.001);
    }

    #[test]
    fn test_parametric_plane_try_normal_parallel_axes_none() {
        let plane = ParametricPlane::new(
            &Vector3::new(0.0f32, 0.0, 0.0),
            &Vector3::new(1.0, 0.0, 0.0),
            &Vector3::new(2.0, 0.0, 0.0),
        );
        assert!(plane.try_normal(EPS_F32).is_none());
        assert!(plane.try_plane(EPS_F32).is_none());
    }
}