alice-physics 2.2.0

//! Cone Collider
//!
//! Cone collider with arbitrary orientation, supporting GJK/EPA collision detection.
//!
//! # Features
//!
//! - **Cone**: Y-axis aligned cone with apex at +Y and base circle at -Y
//! - **GJK Support**: Implements `Support` trait for GJK/EPA pipeline
//! - **Fast AABB**: Compute world-space AABB from oriented cone
//!
//! # Geometry
//!
//! The cone is defined by a center (midpoint between apex and base center),
//! half-height (distance from center to apex or base), and base radius.
//! In local space the apex is at `(0, +half_height, 0)` and the base
//! circle center is at `(0, -half_height, 0)`.
//!
//! Author: Moroya Sakamoto

use crate::collider::{hypot3, Support, AABB};
use crate::math::{Fix128, QuatFix, Vec3Fix};

/// Cone collider
///
/// A cone defined by center position, half-height (from center to apex along
/// local Y axis), base radius, and an orientation quaternion. The apex sits
/// at local `+Y` and the base circle at local `-Y`.
#[derive(Clone, Copy, Debug, PartialEq, Eq)]
pub struct Cone {
    /// Center position in world space (midpoint between apex and base center)
    pub center: Vec3Fix,
    /// Base radius of the circular cross-section
    pub radius: Fix128,
    /// Half-height (from center to apex along local Y axis)
    pub half_height: Fix128,
    /// Orientation quaternion
    pub rotation: QuatFix,
}

impl Cone {
    /// Create a new axis-aligned cone (Y-up, apex at +Y)
    #[inline]
    #[must_use]
    pub const fn new(center: Vec3Fix, radius: Fix128, half_height: Fix128) -> Self {
        Self {
            center,
            radius,
            half_height,
            rotation: QuatFix::IDENTITY,
        }
    }

    /// Create a new oriented cone
    #[inline]
    #[must_use]
    pub const fn with_rotation(
        center: Vec3Fix,
        radius: Fix128,
        half_height: Fix128,
        rotation: QuatFix,
    ) -> Self {
        Self {
            center,
            radius,
            half_height,
            rotation,
        }
    }

    /// World-space position of the apex (+Y in local space)
    #[must_use]
    pub fn apex(&self) -> Vec3Fix {
        let local_apex = Vec3Fix::new(Fix128::ZERO, self.half_height, Fix128::ZERO);
        self.center + self.rotation.unit_rotation().rotate_vec(local_apex)
    }

    /// World-space position of the base circle center (-Y in local space)
    #[must_use]
    pub fn base_center(&self) -> Vec3Fix {
        let local_base = Vec3Fix::new(Fix128::ZERO, -self.half_height, Fix128::ZERO);
        self.center + self.rotation.unit_rotation().rotate_vec(local_base)
    }

    /// The smallest world-axis box enclosing this cone.
    ///
    /// The cone is the hull of its apex `c + a` and its base disc about `c − a`,
    /// with `a = R·(0, h, 0)`; the disc reaches `s_i = √(u_i² + w_i²)` along world
    /// axis `i` (`u = R·(r, 0, 0)`, `w = R·(0, 0, r)`; for a unit rotation
    /// `r·√(1 − â_i²)`). Per axis the box is
    /// `[min(a_i, −a_i − s_i), max(a_i, −a_i + s_i)]` about `c`, which is not
    /// symmetric about `c` unless the axis lies in the world plane normal to `i`.
    #[must_use]
    pub fn aabb(&self) -> AABB {
        let r = self.rotation.unit_rotation();
        let u = r.rotate_vec(Vec3Fix::new(self.radius, Fix128::ZERO, Fix128::ZERO));
        let a = r.rotate_vec(Vec3Fix::new(Fix128::ZERO, self.half_height, Fix128::ZERO));
        let w = r.rotate_vec(Vec3Fix::new(Fix128::ZERO, Fix128::ZERO, self.radius));
        let axis = |ai: Fix128, ui: Fix128, wi: Fix128| -> (Fix128, Fix128) {
            let s = hypot3(ui, wi, Fix128::ZERO);
            let (base_lo, base_hi) = (-ai - s, -ai + s);
            let lo = if ai < base_lo { ai } else { base_lo };
            let hi = if ai > base_hi { ai } else { base_hi };
            (lo, hi)
        };
        let (lx, hx) = axis(a.x, u.x, w.x);
        let (ly, hy) = axis(a.y, u.y, w.y);
        let (lz, hz) = axis(a.z, u.z, w.z);
        AABB::new(
            self.center + Vec3Fix::new(lx, ly, lz),
            self.center + Vec3Fix::new(hx, hy, hz),
        )
    }

    /// Volume of the cone: (1/3) * pi * r^2 * h where h = 2 * `half_height`
    #[inline]
    #[must_use]
    pub fn volume(&self) -> Fix128 {
        let pi = Fix128::PI;
        let two = Fix128::from_int(2);
        let three = Fix128::from_int(3);
        pi * self.radius * self.radius * two * self.half_height / three
    }

    /// Surface area: pi * r * (r + sqrt(r^2 + h^2))
    #[must_use]
    pub fn surface_area(&self) -> Fix128 {
        let pi = Fix128::PI;
        let two = Fix128::from_int(2);
        let h = two * self.half_height;
        let slant = (self.radius * self.radius + h * h).sqrt();
        pi * self.radius * (self.radius + slant)
    }

    /// Offset of the centre of mass from the geometric centre, in the cone's
    /// local frame: the centroid of a solid cone sits a quarter of the height
    /// above the base, i.e. at `y = −half_height / 2`.
    #[inline]
    #[must_use]
    pub fn center_of_mass_offset(&self) -> Vec3Fix {
        Vec3Fix::new(Fix128::ZERO, -self.half_height.half(), Fix128::ZERO)
    }

    /// Diagonal of the inertia tensor **about the centre of mass** for a given
    /// mass.
    ///
    /// For a solid cone aligned along Y with height `h = 2·half_height`:
    /// - `Iyy = 3/10 · m · r²`
    /// - `Ixx = Izz = 3/20 · m · r² + 3/80 · m · h²`
    ///
    /// The centre of mass is [`Self::center_of_mass_offset`] away from the
    /// geometric centre; a body built from this shape is placed at that point.
    #[must_use]
    pub fn inertia_diagonal(&self, mass: Fix128) -> Vec3Fix {
        let r2 = self.radius * self.radius;
        let two = Fix128::from_int(2);
        let h = two * self.half_height;
        let h2 = h * h;

        let ixx = mass * (Fix128::from_ratio(3, 20) * r2 + Fix128::from_ratio(3, 80) * h2);
        let iyy = Fix128::from_ratio(3, 10) * mass * r2;

        Vec3Fix::new(ixx, iyy, ixx)
    }
}

impl Support for Cone {
    fn support(&self, direction: Vec3Fix) -> Vec3Fix {
        // Only the direction matters: bring its scale into range first so the
        // squares below neither underflow nor overflow
        let direction = direction.rescaled_direction();
        // Transform direction to local space
        let local_dir = self
            .rotation
            .unit_rotation()
            .conjugate()
            .rotate_vec(direction);

        // The cone has:
        //   apex at (0, half_height, 0)
        //   base circle at (0, -half_height, 0) with given radius
        //
        // Support is the max of:
        //   1) The apex point
        //   2) The furthest point on the base circle edge

        // Dot product of apex with local direction
        let apex_dot = local_dir.y * self.half_height;

        // Furthest point on base circle: project direction onto XZ plane,
        // normalize, scale by radius, set Y = -half_height
        let xz_len_sq = local_dir.x * local_dir.x + local_dir.z * local_dir.z;
        let base_support = if xz_len_sq.is_zero() {
            // Direction is purely along Y — any point on circle edge works
            Vec3Fix::new(self.radius, -self.half_height, Fix128::ZERO)
        } else {
            let xz_len = xz_len_sq.sqrt();
            Vec3Fix::new(
                self.radius * local_dir.x / xz_len,
                -self.half_height,
                self.radius * local_dir.z / xz_len,
            )
        };

        let base_dot = base_support.dot(local_dir);

        let local_support = if apex_dot >= base_dot {
            Vec3Fix::new(Fix128::ZERO, self.half_height, Fix128::ZERO)
        } else {
            base_support
        };

        // Transform back to world space
        self.center + self.rotation.unit_rotation().rotate_vec(local_support)
    }
}

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

    #[test]
    fn test_cone_new() {
        let cone = Cone::new(Vec3Fix::ZERO, Fix128::from_int(2), Fix128::from_int(3));
        assert_eq!(cone.center.x.hi, 0);
        assert_eq!(cone.radius.hi, 2);
        assert_eq!(cone.half_height.hi, 3);
    }

    #[test]
    fn test_cone_apex() {
        let cone = Cone::new(Vec3Fix::ZERO, Fix128::ONE, Fix128::from_int(5));
        let apex = cone.apex();
        assert_eq!(apex.x.hi, 0);
        assert_eq!(apex.y.hi, 5);
        assert_eq!(apex.z.hi, 0);
    }

    #[test]
    fn test_cone_base_center() {
        let cone = Cone::new(Vec3Fix::ZERO, Fix128::ONE, Fix128::from_int(5));
        let base = cone.base_center();
        assert_eq!(base.x.hi, 0);
        assert_eq!(base.y.hi, -5);
        assert_eq!(base.z.hi, 0);
    }

    #[test]
    fn test_cone_support_up() {
        let cone = Cone::new(Vec3Fix::ZERO, Fix128::ONE, Fix128::from_int(3));
        let s = cone.support(Vec3Fix::UNIT_Y);
        // Support in +Y should be the apex
        assert_eq!(s.y.hi, 3);
        assert_eq!(s.x.hi, 0);
    }

    #[test]
    fn test_cone_support_down() {
        let cone = Cone::new(Vec3Fix::ZERO, Fix128::from_int(2), Fix128::from_int(3));
        let s = cone.support(-Vec3Fix::UNIT_Y);
        // Support in -Y should be on the base circle edge
        assert_eq!(s.y.hi, -3);
    }

    #[test]
    fn test_cone_support_side() {
        let cone = Cone::new(Vec3Fix::ZERO, Fix128::from_int(4), Fix128::from_int(1));
        let s = cone.support(Vec3Fix::UNIT_X);
        // Support in +X: base circle point at (radius, -half_height, 0)
        // vs apex at (0, half_height, 0). Dot with UNIT_X: radius vs 0.
        // Since radius(4) > 0, base circle wins.
        assert_eq!(s.x.hi, 4);
        assert_eq!(s.y.hi, -1);
    }

    #[test]
    fn test_cone_aabb() {
        let cone = Cone::new(
            Vec3Fix::from_int(5, 0, 0),
            Fix128::from_int(2),
            Fix128::from_int(3),
        );
        let aabb = cone.aabb();
        // Y-up: apex at y = +3, base disc of radius 2 at y = -3
        assert_eq!(aabb.min.x.hi, 3); // 5 - 2
        assert_eq!(aabb.max.x.hi, 7); // 5 + 2
        assert_eq!(aabb.min.y.hi, -3); // base
        assert_eq!(aabb.max.y.hi, 3); // apex
    }

    #[test]
    fn test_cone_volume() {
        let cone = Cone::new(Vec3Fix::ZERO, Fix128::ONE, Fix128::ONE);
        // V = (1/3) * pi * 1^2 * 2*1 = 2*pi/3 ≈ 2.094
        let v = cone.volume();
        assert!(
            v.hi >= 2 && v.hi <= 3,
            "Volume should be ~2.094, got {}",
            v.hi
        );
    }

    #[test]
    fn test_cone_gjk_collision() {
        use crate::collider::{gjk, Sphere};
        let cone = Cone::new(Vec3Fix::ZERO, Fix128::from_int(2), Fix128::from_int(2));
        let sphere = Sphere::new(Vec3Fix::ZERO, Fix128::ONE);
        let result = gjk(&cone, &sphere);
        assert!(
            result.colliding,
            "Overlapping cone and sphere should collide"
        );
    }

    #[test]
    fn test_cone_gjk_no_collision() {
        use crate::collider::{gjk, Sphere};
        let cone = Cone::new(Vec3Fix::ZERO, Fix128::ONE, Fix128::ONE);
        let sphere = Sphere::new(Vec3Fix::from_int(10, 0, 0), Fix128::ONE);
        let result = gjk(&cone, &sphere);
        assert!(
            !result.colliding,
            "Separated cone and sphere should not collide"
        );
    }

    #[test]
    fn test_cone_offset_center() {
        let cone = Cone::new(
            Vec3Fix::from_int(10, 20, 30),
            Fix128::ONE,
            Fix128::from_int(5),
        );
        let apex = cone.apex();
        assert_eq!(apex.x.hi, 10);
        assert_eq!(apex.y.hi, 25);
        assert_eq!(apex.z.hi, 30);

        let base = cone.base_center();
        assert_eq!(base.x.hi, 10);
        assert_eq!(base.y.hi, 15);
        assert_eq!(base.z.hi, 30);
    }
}