alice-physics 3.0.1

//! Height Field Collision
//!
//! Grid-based terrain collider with bilinear interpolation.
//! Efficient for large flat terrain with vertical displacement.

use crate::collider::Contact;
use crate::math::{Fix128, Vec3Fix};

#[cfg(not(feature = "std"))]
use alloc::vec;
#[cfg(not(feature = "std"))]
use alloc::vec::Vec;

/// Height field terrain collider
///
/// Heights stored as a 2D grid in row-major order (Z-major):
/// `index = x + z * width`
#[derive(Clone, Debug, PartialEq, Eq)]
pub struct HeightField {
    /// Height values (Fix128)
    pub heights: Vec<Fix128>,
    /// Grid width (number of columns, X axis)
    pub width: u32,
    /// Grid depth (number of rows, Z axis)
    pub depth: u32,
    /// World-space spacing between grid points
    pub spacing: Fix128,
    /// World-space origin (min corner)
    pub origin: Vec3Fix,
}

impl HeightField {
    /// Create a height field from a grid of heights
    #[must_use]
    pub fn new(
        heights: Vec<Fix128>,
        width: u32,
        depth: u32,
        spacing: Fix128,
        origin: Vec3Fix,
    ) -> Self {
        debug_assert_eq!(heights.len(), (width * depth) as usize);
        Self {
            heights,
            width,
            depth,
            spacing,
            origin,
        }
    }

    /// Create a flat height field at a given Y level
    #[must_use]
    pub fn flat(width: u32, depth: u32, spacing: Fix128, origin: Vec3Fix, height: Fix128) -> Self {
        let heights = vec![height; (width * depth) as usize];
        Self::new(heights, width, depth, spacing, origin)
    }

    /// Get height at grid coordinates (clamped to bounds)
    #[inline]
    #[must_use]
    pub fn get_height(&self, gx: u32, gz: u32) -> Fix128 {
        if self.width == 0 || self.depth == 0 {
            // No grid points at all: nothing to clamp to.
            return Fix128::ZERO;
        }
        let gx = gx.min(self.width - 1);
        let gz = gz.min(self.depth - 1);
        self.heights[(gx + gz * self.width) as usize]
    }

    /// Set height at grid coordinates
    #[inline]
    pub fn set_height(&mut self, gx: u32, gz: u32, height: Fix128) {
        if gx < self.width && gz < self.depth {
            self.heights[(gx + gz * self.width) as usize] = height;
        }
    }

    /// World-space to grid coordinates (fractional)
    fn world_to_grid(&self, world_pos: Vec3Fix) -> (Fix128, Fix128) {
        let local_x = world_pos.x - self.origin.x;
        let local_z = world_pos.z - self.origin.z;

        if self.spacing.is_zero() {
            return (Fix128::ZERO, Fix128::ZERO);
        }

        (local_x / self.spacing, local_z / self.spacing)
    }

    /// Sample height at world-space XZ with bilinear interpolation
    #[must_use]
    pub fn sample_height(&self, world_x: Fix128, world_z: Fix128) -> Fix128 {
        let local_x = world_x - self.origin.x;
        let local_z = world_z - self.origin.z;

        if self.spacing.is_zero() {
            return Fix128::ZERO;
        }

        let gx_f = local_x / self.spacing;
        let gz_f = local_z / self.spacing;

        // Integer grid coords (clamped to valid range for u32 conversion)
        let max_gx = (self.width as i64).saturating_sub(2).max(0);
        let max_gz = (self.depth as i64).saturating_sub(2).max(0);
        let gx0 = if gx_f.is_negative() {
            0u32
        } else {
            (gx_f.hi.min(max_gx).max(0)) as u32
        };
        let gz0 = if gz_f.is_negative() {
            0u32
        } else {
            (gz_f.hi.min(max_gz).max(0)) as u32
        };

        let gx1 = gx0 + 1;
        let gz1 = gz0 + 1;

        // Fractional part for interpolation
        let fx = gx_f - Fix128::from_int(gx0 as i64);
        let fz = gz_f - Fix128::from_int(gz0 as i64);

        // Clamp fractions to [0, 1]
        let fx = clamp01(fx);
        let fz = clamp01(fz);

        // Sample 4 corners
        let h00 = self.get_height(gx0, gz0);
        let h10 = self.get_height(gx1, gz0);
        let h01 = self.get_height(gx0, gz1);
        let h11 = self.get_height(gx1, gz1);

        // Bilinear interpolation
        let one = Fix128::ONE;
        let h0 = h00 * (one - fx) + h10 * fx;
        let h1 = h01 * (one - fx) + h11 * fx;
        h0 * (one - fz) + h1 * fz
    }

    /// Compute surface normal at world-space XZ via central difference
    #[must_use]
    pub fn sample_normal(&self, world_x: Fix128, world_z: Fix128) -> Vec3Fix {
        let eps = self.spacing.half();
        // The two samples on each axis are kept on the grid: past an edge the
        // height is clamped flat, and a central difference reaching over it
        // halved the slope at the border points.
        let span = |lo: Fix128, cells: u32, at: Fix128| {
            let hi = lo + self.spacing * Fix128::from_int(i64::from(cells.saturating_sub(1)));
            let a = if at - eps < lo { lo } else { at - eps };
            let b = if at + eps > hi { hi } else { at + eps };
            (a, b)
        };
        let (x0, x1) = span(self.origin.x, self.width, world_x);
        let (z0, z1) = span(self.origin.z, self.depth, world_z);
        let slope = |h0: Fix128, h1: Fix128, a: Fix128, b: Fix128| {
            if b > a {
                (h1 - h0) / (b - a)
            } else {
                Fix128::ZERO
            }
        };
        let sx = slope(
            self.sample_height(x0, world_z),
            self.sample_height(x1, world_z),
            x0,
            x1,
        );
        let sz = slope(
            self.sample_height(world_x, z0),
            self.sample_height(world_x, z1),
            z0,
            z1,
        );

        // Normal = (-dh/dx, 1, -dh/dz) normalized
        Vec3Fix::new(-sx, Fix128::ONE, -sz).normalize()
    }

    /// Sphere vs `HeightField` collision
    ///
    /// The sphere is clear of the surface when its centre is at least `radius`
    /// away **along the surface normal** — the distance to the tangent plane, which
    /// is `Δy · n_y` for a centre `Δy` above the surface point below it. (Measuring
    /// `Δy` itself, as this used to, buries a sphere on a slope by `r·(1/cosθ − 1)`.)
    /// A field with no grid points, or a non-positive spacing, has no surface.
    #[must_use]
    pub fn collide_sphere(&self, center: Vec3Fix, radius: Fix128) -> Option<Contact> {
        if self.width == 0 || self.depth == 0 || self.spacing <= Fix128::ZERO {
            return None;
        }
        let (gx_f, gz_f) = self.world_to_grid(center);

        // Check if within bounds (with margin)
        // the same 2 cells past the first and the last grid point (index
        // `width - 1`, not `width`)
        let margin = Fix128::from_int(2);
        let last = |n: u32| Fix128::from_int(i64::from(n) - 1);
        if gx_f < -margin || gx_f > last(self.width) + margin {
            return None;
        }
        if gz_f < -margin || gz_f > last(self.depth) + margin {
            return None;
        }

        let ground_height = self.sample_height(center.x, center.z);
        let normal = self.sample_normal(center.x, center.z);
        // Distance from the centre to the tangent plane at the point below it.
        let dist = (center.y - ground_height) * normal.y;

        if dist < radius {
            let depth = radius - dist;
            let point_on_surface = Vec3Fix::new(center.x, ground_height, center.z);

            Some(Contact {
                depth,
                normal,
                point_a: center - normal * radius,
                point_b: point_on_surface,
            })
        } else {
            None
        }
    }

    /// Point vs `HeightField`: get signed distance (positive = above)
    #[inline]
    #[must_use]
    pub fn signed_distance(&self, point: Vec3Fix) -> Fix128 {
        let ground_height = self.sample_height(point.x, point.z);
        point.y - ground_height
    }

    /// Get world-space AABB of the height field
    #[must_use]
    pub fn aabb(&self) -> crate::collider::AABB {
        if self.heights.is_empty() || self.spacing <= Fix128::ZERO {
            // No grid points, or a spacing that is not positive (collide_sphere
            // treats both as having no surface): a degenerate point AABB at the
            // origin. A negative spacing would otherwise give max < min.
            return crate::collider::AABB::new(self.origin, self.origin);
        }
        let mut min_h = self.heights[0];
        let mut max_h = self.heights[0];
        for &h in &self.heights[1..] {
            if h < min_h {
                min_h = h;
            }
            if h > max_h {
                max_h = h;
            }
        }

        let max_x = self.origin.x + self.spacing * Fix128::from_int((self.width - 1) as i64);
        let max_z = self.origin.z + self.spacing * Fix128::from_int((self.depth - 1) as i64);

        crate::collider::AABB::new(
            Vec3Fix::new(self.origin.x, min_h, self.origin.z),
            Vec3Fix::new(max_x, max_h, max_z),
        )
    }
}

#[inline]
fn clamp01(v: Fix128) -> Fix128 {
    if v < Fix128::ZERO {
        Fix128::ZERO
    } else if v > Fix128::ONE {
        Fix128::ONE
    } else {
        v
    }
}

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

    #[test]
    fn test_flat_heightfield() {
        let hf = HeightField::flat(10, 10, Fix128::ONE, Vec3Fix::ZERO, Fix128::ZERO);
        let h = hf.sample_height(Fix128::from_int(5), Fix128::from_int(5));
        assert!(
            h.abs() < Fix128::from_ratio(1, 10),
            "Flat field should have height ~0"
        );
    }

    #[test]
    fn test_heightfield_interpolation() {
        // Create a simple slope: heights increase with X
        let mut heights = Vec::new();
        for z in 0..4u32 {
            for x in 0..4u32 {
                let _ = z;
                heights.push(Fix128::from_int(x as i64));
            }
        }
        let hf = HeightField::new(heights, 4, 4, Fix128::ONE, Vec3Fix::ZERO);

        // At x=0, height should be 0
        let h0 = hf.sample_height(Fix128::ZERO, Fix128::ONE);
        assert!(h0.abs() < Fix128::ONE, "Height at x=0 should be ~0");

        // At x=2, height should be ~2
        let h2 = hf.sample_height(Fix128::from_int(2), Fix128::ONE);
        let error = (h2 - Fix128::from_int(2)).abs();
        assert!(error < Fix128::ONE, "Height at x=2 should be ~2");
    }

    #[test]
    fn test_sphere_heightfield_collision() {
        let hf = HeightField::flat(10, 10, Fix128::ONE, Vec3Fix::ZERO, Fix128::ZERO);

        // Sphere penetrating ground
        let contact = hf.collide_sphere(
            Vec3Fix::new(
                Fix128::from_int(5),
                Fix128::from_ratio(1, 2),
                Fix128::from_int(5),
            ),
            Fix128::ONE,
        );

        assert!(
            contact.is_some(),
            "Sphere at y=0.5 with r=1 should penetrate ground at y=0"
        );
        let c = contact.unwrap();
        assert!(c.depth > Fix128::ZERO);
    }

    #[test]
    fn test_sphere_above_heightfield() {
        let hf = HeightField::flat(10, 10, Fix128::ONE, Vec3Fix::ZERO, Fix128::ZERO);

        let contact = hf.collide_sphere(
            Vec3Fix::new(
                Fix128::from_int(5),
                Fix128::from_int(5),
                Fix128::from_int(5),
            ),
            Fix128::ONE,
        );

        assert!(
            contact.is_none(),
            "Sphere at y=5 should not collide with ground"
        );
    }

    #[test]
    fn test_normal_on_flat() {
        let hf = HeightField::flat(10, 10, Fix128::ONE, Vec3Fix::ZERO, Fix128::ZERO);
        let normal = hf.sample_normal(Fix128::from_int(5), Fix128::from_int(5));

        // On flat terrain, normal should point up (0, 1, 0)
        assert!(
            normal.y > Fix128::from_ratio(9, 10),
            "Normal should point up on flat terrain"
        );
    }

    #[test]
    fn test_signed_distance() {
        let hf = HeightField::flat(10, 10, Fix128::ONE, Vec3Fix::ZERO, Fix128::ZERO);

        let above = hf.signed_distance(Vec3Fix::from_int(5, 3, 5));
        assert!(
            above > Fix128::ZERO,
            "Point above should have positive distance"
        );

        let below = hf.signed_distance(Vec3Fix::from_int(5, -2, 5));
        assert!(
            below < Fix128::ZERO,
            "Point below should have negative distance"
        );
    }

    #[test]
    fn test_aabb() {
        let hf = HeightField::flat(10, 10, Fix128::ONE, Vec3Fix::ZERO, Fix128::from_int(5));
        let aabb = hf.aabb();
        assert_eq!(aabb.min.y.hi, 5);
        assert_eq!(aabb.max.y.hi, 5);
    }

    #[test]
    fn set_height_updates_one_cell_and_feeds_bilinear_sampling() {
        // 4 (width, x) × 3 (depth, z) の平地、spacing 1、origin 原点
        let mut hf = HeightField::flat(4, 3, Fix128::ONE, Vec3Fix::ZERO, Fix128::ZERO);
        hf.set_height(2, 1, Fix128::from_int(8));
        // 該当 cell だけが変わる (index = gx + gz * width の取り違えを検出)
        for gz in 0..3u32 {
            for gx in 0..4u32 {
                let want = if (gx, gz) == (2, 1) {
                    Fix128::from_int(8)
                } else {
                    Fix128::ZERO
                };
                assert_eq!(hf.get_height(gx, gz), want, "({gx}, {gz})");
            }
        }
        // 頂点上のサンプルはその高さ、隣接頂点との中点は線形補間 = 4、cell 中央は 8/4 = 2
        assert_eq!(
            hf.sample_height(Fix128::from_int(2), Fix128::from_int(1)),
            Fix128::from_int(8)
        );
        assert_eq!(
            hf.sample_height(Fix128::from_ratio(5, 2), Fix128::from_int(1)),
            Fix128::from_int(4)
        );
        assert_eq!(
            hf.sample_height(Fix128::from_int(2), Fix128::from_ratio(1, 2)),
            Fix128::from_int(4)
        );
        assert_eq!(
            hf.sample_height(Fix128::from_ratio(3, 2), Fix128::from_ratio(1, 2)),
            Fix128::from_int(2)
        );
        // 2 cell 以上離れた点には影響しない
        assert_eq!(
            hf.sample_height(Fix128::from_ratio(1, 2), Fix128::from_ratio(3, 2)),
            Fix128::ZERO
        );
        // 上書きできる
        hf.set_height(2, 1, Fix128::from_int(-3));
        assert_eq!(hf.get_height(2, 1), Fix128::from_int(-3));
        // 範囲外 (gx == width / gz == depth) は無視され、他の cell も壊さない
        hf.set_height(4, 0, Fix128::from_int(99));
        hf.set_height(0, 3, Fix128::from_int(99));
        hf.set_height(u32::MAX, u32::MAX, Fix128::from_int(99));
        let mut sum = Fix128::ZERO;
        for gz in 0..3u32 {
            for gx in 0..4u32 {
                sum = sum + hf.get_height(gx, gz);
            }
        }
        assert_eq!(sum, Fix128::from_int(-3));
        // AABB の y 範囲は set_height に追従する
        let aabb = hf.aabb();
        assert_eq!(aabb.min.y, Fix128::from_int(-3));
        assert_eq!(aabb.max.y, Fix128::ZERO);
    }
}