rustial_engine/math/

frustum.rs

//! View frustum extraction and intersection testing (AABB, sphere, point).
//!
//! # Algorithm
//!
//! Frustum planes are extracted from a combined view-projection matrix using
//! the Gribb/Hartmann method (["Fast Extraction of Viewing Frustum Planes
//! from the World-View-Projection Matrix"][gh]).  The six resulting planes
//! have **inward-pointing** normals: a positive signed distance from a plane
//! means the query point is *inside* the frustum on that side.
//!
//! # Coordinate convention
//!
//! This module operates in **world space** (meters, EPSG:3857, right-handed
//! Z-up).  The input `DMat4` must be `projection * view`, both in f64.
//! The engine computes these matrices camera-relative to avoid f32 jitter,
//! then passes the result here before the f64-to-f32 GPU cast.
//!
//! # Culling guarantee
//!
//! All three tests (`contains_point`, `intersects_sphere`,
//! `intersects_aabb`) are **conservative**: they will never report a visible
//! object as outside (no false negatives), but may occasionally report an
//! object outside the frustum as inside (false positives) for AABB/sphere
//! near frustum corners.  This is the standard trade-off -- exact corner
//! tests cost 8x more and are not needed for tile/model culling.
//!
//! # Degenerate input
//!
//! A zero or near-zero view-projection matrix produces all-zero planes.
//! These pass every intersection test, so a degenerate camera never
//! incorrectly culls geometry (safe conservative fallback).
//!
//! [gh]: http://www.cs.otago.ac.nz/postgrads/alexis/planeExtraction.pdf

use crate::bounds::WorldBounds;
use crate::coord::WorldCoord;
use glam::DVec4;

// ---------------------------------------------------------------------------
// Plane
// ---------------------------------------------------------------------------

/// A plane in Hessian normal form: `ax + by + cz + d = 0`.
///
/// The normal `[a, b, c]` is always unit-length after extraction.  The sign
/// convention is: positive [`distance_to_point`](Self::distance_to_point)
/// means the query point is on the **inside** (visible) half-space.
#[derive(Debug, Clone, Copy, PartialEq)]
pub struct Plane {
    normal: [f64; 3],
    d: f64,
}

impl Plane {
    /// Construct a plane from a unit normal and signed distance.
    ///
    /// The caller is responsible for ensuring the normal is unit-length;
    /// no normalisation is performed here.
    #[inline]
    pub fn new(normal: [f64; 3], d: f64) -> Self {
        Self { normal, d }
    }

    /// The inward-facing normal of this plane.
    ///
    /// For frustum planes extracted via the Gribb/Hartmann method, this
    /// normal points **into** the frustum volume.  A positive
    /// [`distance_to_point`](Self::distance_to_point) value means the
    /// query point is on the inside (visible) side of the plane.
    #[inline]
    pub fn normal(&self) -> [f64; 3] {
        self.normal
    }

    /// The signed distance constant `d` in `n . p + d = 0`.
    #[inline]
    pub fn d(&self) -> f64 {
        self.d
    }

    /// Signed distance from a point to this plane.
    ///
    /// - **Positive**: the point is inside the frustum on this plane's side.
    /// - **Zero**: the point lies exactly on the plane.
    /// - **Negative**: the point is outside.
    ///
    /// Because the normal is unit-length, the returned value is a true
    /// metric distance in the same units as the input coordinates (meters).
    #[inline]
    pub fn distance_to_point(&self, x: f64, y: f64, z: f64) -> f64 {
        self.normal[0] * x + self.normal[1] * y + self.normal[2] * z + self.d
    }
}

// ---------------------------------------------------------------------------
// Frustum
// ---------------------------------------------------------------------------

/// Index constant for the left frustum plane.
///
/// These match the extraction order used by
/// [`Frustum::from_view_projection`] and correspond to the array returned
/// by [`Frustum::planes`].
pub const PLANE_LEFT: usize = 0;
/// Right frustum plane index.
pub const PLANE_RIGHT: usize = 1;
/// Bottom frustum plane index.
pub const PLANE_BOTTOM: usize = 2;
/// Top frustum plane index.
pub const PLANE_TOP: usize = 3;
/// Near frustum plane index.
pub const PLANE_NEAR: usize = 4;
/// Far frustum plane index.
pub const PLANE_FAR: usize = 5;

/// Six-plane view frustum extracted from a view-projection matrix.
///
/// Constructed via [`from_view_projection`](Self::from_view_projection),
/// then queried with one of the intersection methods.  The plane order is
/// `[left, right, bottom, top, near, far]` and is accessible through
/// [`planes()`](Self::planes) or the `PLANE_*` index constants.
///
/// # When to rebuild
///
/// Rebuild every frame (or whenever the camera moves).  Construction is
/// six normalisations -- negligible compared to the tile-selection loop
/// it gates.
#[derive(Debug, Clone)]
pub struct Frustum {
    planes: [Plane; 6],
}

impl Frustum {
    /// Extract the 6 frustum planes from a combined view-projection matrix.
    ///
    /// Uses the Gribb/Hartmann row-combination method:
    ///
    /// ```text
    /// left   = row3 + row0      right = row3 - row0
    /// bottom = row3 + row1      top   = row3 - row1
    /// near   = row3 + row2      far   = row3 - row2
    /// ```
    ///
    /// Each raw plane `(a, b, c, d)` is then normalised by dividing by
    /// `sqrt(a^2 + b^2 + c^2)` so that `distance_to_point` returns a
    /// true metric distance.  Degenerate planes (norm < 1e-15) are left
    /// as all-zeros, which makes them pass all intersection tests (safe
    /// conservative default).
    pub fn from_view_projection(vp: &glam::DMat4) -> Self {
        // Transpose the column-major matrix into row vectors.
        let row0 = DVec4::new(vp.col(0).x, vp.col(1).x, vp.col(2).x, vp.col(3).x);
        let row1 = DVec4::new(vp.col(0).y, vp.col(1).y, vp.col(2).y, vp.col(3).y);
        let row2 = DVec4::new(vp.col(0).z, vp.col(1).z, vp.col(2).z, vp.col(3).z);
        let row3 = DVec4::new(vp.col(0).w, vp.col(1).w, vp.col(2).w, vp.col(3).w);

        let raw_planes = [
            row3 + row0, // left
            row3 - row0, // right
            row3 + row1, // bottom
            row3 - row1, // top
            row3 + row2, // near
            row3 - row2, // far
        ];

        let mut planes = [Plane {
            normal: [0.0; 3],
            d: 0.0,
        }; 6];
        for (i, p) in raw_planes.iter().enumerate() {
            let len = (p.x * p.x + p.y * p.y + p.z * p.z).sqrt();
            // Guard against degenerate matrices (e.g. zero-area viewport).
            // A zero-normal plane has distance 0 to every point, so all
            // intersection tests conservatively return "inside".
            if len > 1e-15 {
                planes[i] = Plane {
                    normal: [p.x / len, p.y / len, p.z / len],
                    d: p.w / len,
                };
            }
        }

        Self { planes }
    }

    /// Access the six frustum planes: `[left, right, bottom, top, near, far]`.
    #[inline]
    pub fn planes(&self) -> &[Plane; 6] {
        &self.planes
    }

    /// Test whether a single point is inside the frustum.
    ///
    /// Returns `true` if the point is on the inside (or boundary) of
    /// every frustum plane.  Useful for quick single-coordinate checks
    /// such as cursor hit-testing or model anchor visibility.
    pub fn contains_point(&self, point: &WorldCoord) -> bool {
        let (x, y, z) = (point.position.x, point.position.y, point.position.z);
        for plane in &self.planes {
            if plane.distance_to_point(x, y, z) < 0.0 {
                return false;
            }
        }
        true
    }

    /// Test whether a bounding sphere intersects the frustum.
    ///
    /// Returns `true` if the sphere (defined by a centre point and
    /// radius in meters) is at least partially inside.  Preferred over
    /// AABB for rotated 3D models where the axis-aligned box would be
    /// excessively loose.
    ///
    /// When `radius == 0.0` this degrades to a point-containment test.
    ///
    /// Conservative: may return `true` for spheres that only overlap a
    /// frustum corner region without actually intersecting the volume.
    pub fn intersects_sphere(&self, center: &WorldCoord, radius: f64) -> bool {
        let (x, y, z) = (center.position.x, center.position.y, center.position.z);
        for plane in &self.planes {
            if plane.distance_to_point(x, y, z) < -radius {
                return false;
            }
        }
        true
    }

    /// Test whether an axis-aligned bounding box intersects the frustum.
    ///
    /// Uses the "p-vertex" (positive vertex) optimisation: for each plane,
    /// only the single AABB corner most aligned with the plane normal is
    /// tested.  If that corner is outside, the entire box is outside.
    ///
    /// Returns `true` if the AABB is at least partially inside.
    ///
    /// Conservative: may return `true` for boxes near frustum corners that
    /// are geometrically outside the exact frustum volume.
    pub fn intersects_aabb(&self, bounds: &WorldBounds) -> bool {
        let min = bounds.min.position;
        let max = bounds.max.position;

        for plane in &self.planes {
            // P-vertex: the AABB corner furthest along the plane normal.
            // If even this corner is on the outside, every other corner
            // is further outside, so the whole box is culled.
            let px = if plane.normal[0] >= 0.0 { max.x } else { min.x };
            let py = if plane.normal[1] >= 0.0 { max.y } else { min.y };
            let pz = if plane.normal[2] >= 0.0 { max.z } else { min.z };

            if plane.distance_to_point(px, py, pz) < 0.0 {
                return false;
            }
        }
        true
    }
}

// ---------------------------------------------------------------------------
// Tests
// ---------------------------------------------------------------------------

#[cfg(test)]
mod tests {
    use super::*;
    use glam::DMat4;

    // -- Plane constructor ------------------------------------------------

    #[test]
    fn plane_new_and_accessors() {
        let p = Plane::new([0.0, 1.0, 0.0], -5.0);
        assert_eq!(p.normal(), [0.0, 1.0, 0.0]);
        assert_eq!(p.d(), -5.0);
    }

    // -- Identity / orthographic ------------------------------------------

    #[test]
    fn identity_frustum_contains_origin() {
        let vp = DMat4::IDENTITY;
        let frustum = Frustum::from_view_projection(&vp);
        let bounds = WorldBounds::new(
            WorldCoord::new(-0.5, -0.5, -0.5),
            WorldCoord::new(0.5, 0.5, 0.5),
        );
        assert!(frustum.intersects_aabb(&bounds));
    }

    #[test]
    fn ortho_frustum_culls_outside() {
        let vp = DMat4::orthographic_rh(-100.0, 100.0, -100.0, 100.0, -100.0, 100.0);
        let frustum = Frustum::from_view_projection(&vp);

        let inside = WorldBounds::new(
            WorldCoord::new(-50.0, -50.0, -50.0),
            WorldCoord::new(50.0, 50.0, 50.0),
        );
        let outside = WorldBounds::new(
            WorldCoord::new(200.0, 200.0, 200.0),
            WorldCoord::new(300.0, 300.0, 300.0),
        );
        assert!(frustum.intersects_aabb(&inside));
        assert!(!frustum.intersects_aabb(&outside));
    }

    // -- Extracted normals are unit-length ---------------------------------

    #[test]
    fn extracted_normals_are_unit_length() {
        let proj = DMat4::perspective_rh(std::f64::consts::FRAC_PI_4, 1.0, 0.1, 1000.0);
        let view = DMat4::look_at_rh(
            glam::DVec3::new(0.0, 0.0, 10.0),
            glam::DVec3::ZERO,
            glam::DVec3::Y,
        );
        let frustum = Frustum::from_view_projection(&(proj * view));
        for plane in frustum.planes() {
            let n = plane.normal();
            let len = (n[0] * n[0] + n[1] * n[1] + n[2] * n[2]).sqrt();
            assert!(
                (len - 1.0).abs() < 1e-12,
                "plane normal not unit-length: {len}"
            );
        }
    }

    // -- Degenerate (zero) matrix -----------------------------------------

    #[test]
    fn degenerate_matrix_passes_all_tests() {
        let frustum = Frustum::from_view_projection(&DMat4::ZERO);
        // All-zero planes should conservatively pass everything.
        assert!(frustum.contains_point(&WorldCoord::new(999.0, 999.0, 999.0)));
        assert!(frustum.intersects_sphere(&WorldCoord::new(0.0, 0.0, 0.0), 1.0));
        let bounds = WorldBounds::new(
            WorldCoord::new(-1.0, -1.0, -1.0),
            WorldCoord::new(1.0, 1.0, 1.0),
        );
        assert!(frustum.intersects_aabb(&bounds));
    }

    // -- Plane index constants --------------------------------------------

    #[test]
    fn plane_index_constants() {
        let vp = DMat4::orthographic_rh(-100.0, 100.0, -100.0, 100.0, -100.0, 100.0);
        let frustum = Frustum::from_view_projection(&vp);
        let planes = frustum.planes();
        // Left plane normal should have positive X component (points right/inward).
        // Right plane normal should have negative X component (points left/inward).
        assert!(planes[PLANE_LEFT].normal()[0] > 0.0);
        assert!(planes[PLANE_RIGHT].normal()[0] < 0.0);
    }

    // -- Perspective (axis-aligned camera) --------------------------------

    #[test]
    fn perspective_frustum() {
        let proj = DMat4::perspective_rh(std::f64::consts::FRAC_PI_4, 1.0, 0.1, 1000.0);
        let view = DMat4::look_at_rh(
            glam::DVec3::new(0.0, 0.0, 10.0),
            glam::DVec3::ZERO,
            glam::DVec3::Y,
        );
        let vp = proj * view;
        let frustum = Frustum::from_view_projection(&vp);

        let visible = WorldBounds::new(
            WorldCoord::new(-1.0, -1.0, -1.0),
            WorldCoord::new(1.0, 1.0, 1.0),
        );
        let behind = WorldBounds::new(
            WorldCoord::new(-1.0, -1.0, 20.0),
            WorldCoord::new(1.0, 1.0, 30.0),
        );
        assert!(frustum.intersects_aabb(&visible));
        assert!(!frustum.intersects_aabb(&behind));
    }

    // -- Pitched camera (real 2.5D map scenario) --------------------------

    #[test]
    fn pitched_camera_frustum() {
        let proj = DMat4::perspective_rh(std::f64::consts::FRAC_PI_4, 1.5, 1.0, 5000.0);
        let eye = glam::DVec3::new(0.0, -500.0, 500.0);
        let target = glam::DVec3::ZERO;
        let view = DMat4::look_at_rh(eye, target, glam::DVec3::Z);
        let vp = proj * view;
        let frustum = Frustum::from_view_projection(&vp);

        let ahead = WorldBounds::new(
            WorldCoord::new(-100.0, 100.0, 0.0),
            WorldCoord::new(100.0, 300.0, 0.0),
        );
        assert!(frustum.intersects_aabb(&ahead));

        let behind = WorldBounds::new(
            WorldCoord::new(-100.0, -2000.0, 0.0),
            WorldCoord::new(100.0, -1500.0, 0.0),
        );
        assert!(!frustum.intersects_aabb(&behind));

        let far_left = WorldBounds::new(
            WorldCoord::new(-5000.0, 0.0, 0.0),
            WorldCoord::new(-4000.0, 100.0, 0.0),
        );
        assert!(!frustum.intersects_aabb(&far_left));
    }

    // -- Point containment ------------------------------------------------

    #[test]
    fn contains_point_inside() {
        let proj = DMat4::perspective_rh(std::f64::consts::FRAC_PI_4, 1.0, 0.1, 1000.0);
        let view = DMat4::look_at_rh(
            glam::DVec3::new(0.0, 0.0, 10.0),
            glam::DVec3::ZERO,
            glam::DVec3::Y,
        );
        let frustum = Frustum::from_view_projection(&(proj * view));

        assert!(frustum.contains_point(&WorldCoord::new(0.0, 0.0, 0.0)));
    }

    #[test]
    fn contains_point_outside() {
        let proj = DMat4::perspective_rh(std::f64::consts::FRAC_PI_4, 1.0, 0.1, 1000.0);
        let view = DMat4::look_at_rh(
            glam::DVec3::new(0.0, 0.0, 10.0),
            glam::DVec3::ZERO,
            glam::DVec3::Y,
        );
        let frustum = Frustum::from_view_projection(&(proj * view));

        assert!(!frustum.contains_point(&WorldCoord::new(0.0, 0.0, 50.0)));
        assert!(!frustum.contains_point(&WorldCoord::new(1000.0, 0.0, 0.0)));
    }

    // -- Sphere intersection ----------------------------------------------

    #[test]
    fn intersects_sphere_inside() {
        let proj = DMat4::perspective_rh(std::f64::consts::FRAC_PI_4, 1.0, 0.1, 1000.0);
        let view = DMat4::look_at_rh(
            glam::DVec3::new(0.0, 0.0, 10.0),
            glam::DVec3::ZERO,
            glam::DVec3::Y,
        );
        let frustum = Frustum::from_view_projection(&(proj * view));

        assert!(frustum.intersects_sphere(&WorldCoord::new(0.0, 0.0, 0.0), 1.0));
    }

    #[test]
    fn intersects_sphere_partially_outside() {
        let vp = DMat4::orthographic_rh(-100.0, 100.0, -100.0, 100.0, -100.0, 100.0);
        let frustum = Frustum::from_view_projection(&vp);

        // Centre 5m outside the right plane, but 10m radius reaches in.
        assert!(frustum.intersects_sphere(&WorldCoord::new(105.0, 0.0, 0.0), 10.0));
        // 100m outside -- radius cannot reach.
        assert!(!frustum.intersects_sphere(&WorldCoord::new(200.0, 0.0, 0.0), 10.0));
    }

    #[test]
    fn intersects_sphere_zero_radius_degrades_to_point() {
        let proj = DMat4::perspective_rh(std::f64::consts::FRAC_PI_4, 1.0, 0.1, 1000.0);
        let view = DMat4::look_at_rh(
            glam::DVec3::new(0.0, 0.0, 10.0),
            glam::DVec3::ZERO,
            glam::DVec3::Y,
        );
        let frustum = Frustum::from_view_projection(&(proj * view));
        let inside = WorldCoord::new(0.0, 0.0, 0.0);
        let outside = WorldCoord::new(0.0, 0.0, 50.0);
        assert_eq!(
            frustum.intersects_sphere(&inside, 0.0),
            frustum.contains_point(&inside),
        );
        assert_eq!(
            frustum.intersects_sphere(&outside, 0.0),
            frustum.contains_point(&outside),
        );
    }

    // -- Plane helpers ----------------------------------------------------

    #[test]
    fn plane_partial_eq() {
        let a = Plane::new([1.0, 0.0, 0.0], 5.0);
        let b = Plane::new([1.0, 0.0, 0.0], 5.0);
        assert_eq!(a, b);
    }
}