geo3d 0.3.2

//! 3D shapes for "is it inside?" tests: [`Sphere`], [`Cone`], [`Cylinder`] and [`AltitudeBand`], all
//! behind the [`Volume`] trait. Each does its trigonometry once when built, so testing many points
//! against one shape is cheap.

use crate::ellipsoid::{Ellipsoid, WGS84};
use crate::frame::LocalFrame;
use crate::local::Aer;
#[cfg(all(feature = "libm", not(feature = "std"), not(test)))]
use crate::math::Float as _;
use crate::position::{Ecef, Geodetic};
use crate::vector::Vec3;

/// A 3D shape that can answer "is this position inside?".
pub trait Volume {
    /// Whether `point` lies inside (boundary inclusive).
    fn contains(&self, point: Geodetic) -> bool;
}

/// A range of heights with an optional floor and ceiling, metres — a slice of airspace. A `None`
/// bound is open on that side. Combine it with any 2D footprint test (from `geo`, PostGIS, …) for
/// an extruded shape:
///
/// ```
/// use geo3d::AltitudeBand;
/// let band = AltitudeBand::new(Some(5_000.0), Some(12_000.0));
/// let footprint_contains = true; // from your 2D geometry library
/// assert!(band.within_prism(footprint_contains, 8_000.0));
/// assert!(!band.within_prism(footprint_contains, 3_000.0));
/// ```
#[derive(Clone, Copy, Debug, Default, PartialEq)]
#[cfg_attr(feature = "serde", derive(serde::Serialize, serde::Deserialize))]
pub struct AltitudeBand {
    /// Lowest height inside, metres; `None` = unbounded below.
    pub floor_m: Option<f64>,
    /// Highest height inside, metres; `None` = unbounded above.
    pub ceiling_m: Option<f64>,
}

impl AltitudeBand {
    /// A band between two optional bounds.
    #[inline]
    pub const fn new(floor_m: Option<f64>, ceiling_m: Option<f64>) -> Self {
        AltitudeBand { floor_m, ceiling_m }
    }

    /// Everything at or above `floor_m`.
    #[inline]
    pub const fn above(floor_m: f64) -> Self {
        AltitudeBand::new(Some(floor_m), None)
    }

    /// Everything at or below `ceiling_m`.
    #[inline]
    pub const fn below(ceiling_m: f64) -> Self {
        AltitudeBand::new(None, Some(ceiling_m))
    }

    /// Whether a height (metres) is inside the band.
    #[inline]
    pub fn contains_height(&self, height_m: f64) -> bool {
        height_m >= self.floor_m.unwrap_or(f64::NEG_INFINITY) && height_m <= self.ceiling_m.unwrap_or(f64::INFINITY)
    }

    /// Inside the 2D footprint **and** inside the band: pass in your footprint test's answer.
    #[inline]
    pub fn within_prism(&self, footprint_contains: bool, height_m: f64) -> bool {
        footprint_contains && self.contains_height(height_m)
    }
}

impl Volume for AltitudeBand {
    #[inline]
    fn contains(&self, point: Geodetic) -> bool {
        self.contains_height(point.height_m)
    }
}

/// A [`Footprint`](crate::Footprint) extruded through an [`AltitudeBand`]: an airspace sector, a
/// restricted area, a building's floors. Inside means inside the band and inside the footprint or
/// on its boundary, as every [`Volume`] counts its boundary.
///
/// ```
/// use geo3d::{AltitudeBand, Footprint, Geodetic, Prism, Volume};
/// let at = |lon, lat| Geodetic::from_lon_lat(lon, lat, 0.0);
/// let sector = Footprint::polygon(&[at(151.0, -34.0), at(152.0, -34.0), at(152.0, -33.0), at(151.0, -33.0)], &[]);
/// let airspace = Prism::new(sector, AltitudeBand::new(Some(1_500.0), Some(3_000.0)));
/// assert!(airspace.contains(Geodetic::from_lon_lat(151.5, -33.5, 2_000.0)));
/// assert!(!airspace.contains(Geodetic::from_lon_lat(151.5, -33.5, 3_500.0))); // above it
/// ```
#[cfg(feature = "footprint")]
#[derive(Clone, Debug, PartialEq)]
#[cfg_attr(feature = "serde", derive(serde::Serialize, serde::Deserialize))]
pub struct Prism {
    /// The shape on the ground.
    pub footprint: crate::Footprint,
    /// The heights it spans.
    pub band: AltitudeBand,
}

#[cfg(feature = "footprint")]
impl Prism {
    /// The footprint extruded through the band.
    #[inline]
    pub fn new(footprint: crate::Footprint, band: AltitudeBand) -> Self {
        Prism { footprint, band }
    }
}

#[cfg(feature = "footprint")]
impl Volume for Prism {
    #[inline]
    fn contains(&self, point: Geodetic) -> bool {
        // the band first: one comparison against a polygon test
        self.band.contains_height(point.height_m) && self.footprint.containment(point) != crate::Containment::Outside
    }
}

/// A ball of `radius_m` around a centre — a range, coverage or proximity bubble. Distance is the
/// straight line through space.
#[derive(Clone, Copy, Debug, PartialEq)]
pub struct Sphere {
    ellipsoid: Ellipsoid,
    center: Geodetic,
    center_ecef: Ecef,
    radius_m: f64,
}

impl Sphere {
    /// A sphere on [`WGS84`].
    #[inline]
    pub fn new(center: Geodetic, radius_m: f64) -> Self {
        Self::on(WGS84, center, radius_m)
    }

    /// A sphere on the given ellipsoid.
    pub fn on(ellipsoid: Ellipsoid, center: Geodetic, radius_m: f64) -> Self {
        Sphere {
            ellipsoid,
            center,
            center_ecef: ellipsoid.to_ecef(center),
            radius_m,
        }
    }

    /// The centre.
    #[inline]
    pub const fn center(&self) -> Geodetic {
        self.center
    }

    /// The radius, metres.
    #[inline]
    pub const fn radius_m(&self) -> f64 {
        self.radius_m
    }

    /// Whether an ECEF point is inside.
    #[inline]
    pub fn contains_ecef(&self, p: Ecef) -> bool {
        self.center_ecef.distance_to(p) <= self.radius_m
    }
}

impl Volume for Sphere {
    #[inline]
    fn contains(&self, point: Geodetic) -> bool {
        self.contains_ecef(self.ellipsoid.to_ecef(point))
    }
}

/// A cone from an apex, pointing along an azimuth and elevation (degrees), with a half-angle spread
/// (degrees) and a reach (metres) — a camera's field of view, an antenna beam or a scan sector.
///
/// ```
/// use geo3d::{Cone, Geodetic, Volume};
/// // A camera on a 100 m mast looking due east, 20° half-angle, 5 km reach.
/// let cam = Cone::new(Geodetic::new(0.0, 0.0, 100.0), 90.0, 0.0, 20.0, 5_000.0);
/// assert!(cam.contains(Geodetic::new(0.0, 0.02, 100.0)));   // ~2.2 km east
/// assert!(!cam.contains(Geodetic::new(0.02, 0.0, 100.0)));  // ~2.2 km north: outside the spread
/// assert!(!cam.contains(Geodetic::new(0.0, 0.1, 100.0)));   // ~11 km east: beyond the range
/// ```
#[derive(Clone, Copy, Debug, PartialEq)]
pub struct Cone {
    frame: LocalFrame,
    axis_enu: Vec3,
    half_angle_rad: f64,
    azimuth_deg: f64,
    elevation_deg: f64,
    half_angle_deg: f64,
    range_m: f64,
}

impl Cone {
    /// A cone on [`WGS84`].
    #[inline]
    pub fn new(apex: Geodetic, azimuth_deg: f64, elevation_deg: f64, half_angle_deg: f64, range_m: f64) -> Self {
        Self::on(WGS84, apex, azimuth_deg, elevation_deg, half_angle_deg, range_m)
    }

    /// A cone on the given ellipsoid.
    pub fn on(
        ellipsoid: Ellipsoid,
        apex: Geodetic,
        azimuth_deg: f64,
        elevation_deg: f64,
        half_angle_deg: f64,
        range_m: f64,
    ) -> Self {
        Cone {
            frame: LocalFrame::on(ellipsoid, apex),
            axis_enu: Aer::new(azimuth_deg, elevation_deg, 1.0).direction_enu().vec(),
            half_angle_rad: half_angle_deg.to_radians(),
            azimuth_deg,
            elevation_deg,
            half_angle_deg,
            range_m,
        }
    }

    /// The apex.
    #[inline]
    pub const fn apex(&self) -> Geodetic {
        self.frame.origin()
    }

    /// The axis as a pointing (range = the cone's range).
    #[inline]
    pub const fn axis(&self) -> Aer {
        Aer::new(self.azimuth_deg, self.elevation_deg, self.range_m)
    }

    /// The half-angle, degrees.
    #[inline]
    pub const fn half_angle_deg(&self) -> f64 {
        self.half_angle_deg
    }

    /// Whether an ECEF point is inside: within range of the apex and within the half-angle of the axis.
    pub fn contains_ecef(&self, p: Ecef) -> bool {
        let v = self.frame.to_enu(p).vec();
        let r = v.norm();
        if r > self.range_m {
            return false;
        }
        if r == 0.0 {
            return true; // the apex itself
        }
        // The angle itself (by atan2), not its cosine, which can't tell tiny angles apart.
        v.angle_to(self.axis_enu) <= self.half_angle_rad
    }
}

impl Volume for Cone {
    #[inline]
    fn contains(&self, point: Geodetic) -> bool {
        self.contains_ecef(self.frame.ellipsoid().to_ecef(point))
    }
}

/// A circle on the ground extruded through an [`AltitudeBand`] — circular airspace, or a geofence
/// with a height range. The radius is a distance along the ground, as airspace is defined: a point
/// is inside if the geodesic from the centre to the ground below it is no longer than the radius,
/// whatever its height.
///
/// ```
/// use geo3d::{AltitudeBand, Cylinder, Geodetic, Volume};
/// let ctr = Cylinder::new(Geodetic::new(0.0, 0.0, 0.0), 10_000.0, AltitudeBand::new(Some(0.0), Some(3_000.0)));
/// assert!(ctr.contains(Geodetic::new(0.05, 0.0, 1_000.0)));   // ~5.5 km out, 1 km up
/// assert!(!ctr.contains(Geodetic::new(0.05, 0.0, 5_000.0)));  // too high
/// assert!(!ctr.contains(Geodetic::new(0.2, 0.0, 1_000.0)));   // ~22 km out
/// ```
#[derive(Clone, Copy, Debug, PartialEq)]
pub struct Cylinder {
    frame: LocalFrame,
    radius_m: f64,
    band: AltitudeBand,
}

impl Cylinder {
    /// A cylinder on [`WGS84`].
    #[inline]
    pub fn new(center: Geodetic, radius_m: f64, band: AltitudeBand) -> Self {
        Self::on(WGS84, center, radius_m, band)
    }

    /// A cylinder on the given ellipsoid.
    pub fn on(ellipsoid: Ellipsoid, center: Geodetic, radius_m: f64, band: AltitudeBand) -> Self {
        Cylinder {
            frame: LocalFrame::on(ellipsoid, center),
            radius_m,
            band,
        }
    }

    /// The centre.
    #[inline]
    pub const fn center(&self) -> Geodetic {
        self.frame.origin()
    }

    /// The radius, metres.
    #[inline]
    pub const fn radius_m(&self) -> f64 {
        self.radius_m
    }

    /// The height band.
    #[inline]
    pub const fn band(&self) -> AltitudeBand {
        self.band
    }
}

impl Volume for Cylinder {
    fn contains(&self, point: Geodetic) -> bool {
        if !self.band.contains_height(point.height_m) {
            return false;
        }
        // The ground below the point, seen flat from the centre: the straight line to it, and so
        // the distance along the ground, is at least this long.
        let ground = Geodetic::new(point.lat_deg, point.lon_deg, 0.0);
        let v = self.frame.enu_of(ground);
        let flat = v.east.hypot(v.north);
        if flat > self.radius_m {
            return false;
        }
        // And at most this long: the arc over the most sharply curved part of the ellipsoid
        // (radius b²/a) whose chord has that span. Only points near the edge need the geodesic.
        let ellipsoid = self.frame.ellipsoid();
        let tightest = ellipsoid.b() * ellipsoid.b() / ellipsoid.a();
        if flat < 0.5 * tightest && tightest * (flat / tightest).asin() * (1.0 + 1e-6) < self.radius_m {
            return true;
        }
        let centre = self.frame.origin();
        ellipsoid
            .geodesic_inverse(Geodetic::new(centre.lat_deg, centre.lon_deg, 0.0), ground)
            .is_some_and(|g| g.distance_m <= self.radius_m)
    }
}

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

    /// The radius is along the ground at any height, matching the geodesic exactly, including
    /// near the edge where the quick checks hand over.
    #[test]
    fn cylinder_radius_is_along_the_ground() {
        let mut seed: u64 = 0x2545_f491_4f6c_dd1d;
        let mut rnd = || {
            seed ^= seed << 13;
            seed ^= seed >> 7;
            seed ^= seed << 17;
            (seed >> 11) as f64 / (1u64 << 53) as f64
        };
        for _ in 0..20_000 {
            let centre = Geodetic::new(170.0 * rnd() - 85.0, 360.0 * rnd() - 180.0, 50.0);
            let radius = 10f64.powf(2.0 + 4.0 * rnd()); // 100 m to 1 000 km
            let zone = Cylinder::new(centre, radius, AltitudeBand::default());
            // Points from well inside to well outside, many right at the edge, up to 20 km high.
            let s = radius * (0.9 + 0.2 * rnd()) * if rnd() < 0.5 { 1.0 } else { 1.0 + 1e-7 * (rnd() - 0.5) };
            let (p, _) = WGS84.geodesic_direct(centre, 360.0 * rnd(), s);
            let p = p.with_height(20_000.0 * rnd());
            let exact = WGS84.geodesic_inverse(centre, p).unwrap().distance_m <= radius;
            assert_eq!(zone.contains(p), exact, "{centre:?} r {radius}: {p:?}");
        }
        // At airliner height, 50 km out is still inside a 50 km zone (the old flat measure lost 107 m).
        let zone = Cylinder::new(Geodetic::new(0.0, 0.0, 0.0), 50_000.0, AltitudeBand::default());
        let (p, _) = WGS84.geodesic_direct(Geodetic::new(0.0, 0.0, 0.0), 0.0, 49_990.0);
        assert!(zone.contains(p.with_height(13_700.0)));
        assert!(!zone.contains(Geodetic::new(f64::NAN, 0.0, 0.0)));
    }

    #[test]
    fn tiny_cones_keep_their_shape() {
        // A 1 microdegree beam: cosines can't tell its edge apart, angles can.
        let apex = Geodetic::new(0.0, 0.0, 0.0);
        let cone = Cone::new(apex, 90.0, 10.0, 1e-6, 1e9);
        let frame = LocalFrame::new(apex);
        assert!(cone.contains_ecef(frame.ecef_of_aer(Aer::new(90.0, 10.0 + 0.9e-6, 1e8))));
        assert!(!cone.contains_ecef(frame.ecef_of_aer(Aer::new(90.0, 10.0 + 1.1e-6, 1e8))));
    }

    #[test]
    fn altitude_band() {
        assert!(AltitudeBand::new(Some(3000.0), Some(10_000.0)).contains_height(5000.0));
        assert!(!AltitudeBand::new(Some(3000.0), Some(10_000.0)).contains_height(2000.0));
        assert!(AltitudeBand::default().contains_height(999_999.0));
        assert!(AltitudeBand::below(2000.0).contains_height(1000.0));
        assert!(!AltitudeBand::above(2000.0).contains_height(1000.0));
        assert!(AltitudeBand::new(Some(5000.0), Some(12_000.0)).contains(Geodetic::new(1.0, 2.0, 8000.0)));
    }

    #[test]
    fn sphere() {
        let site = Geodetic::new(34.0, -118.0, 0.0);
        let s = Sphere::new(site, 10_000.0);
        assert!(s.contains(Geodetic::new(34.0, -118.0, 1000.0)));
        assert!(!s.contains(Geodetic::new(34.2, -118.0, 0.0)));
        assert_eq!(s.center(), site);
        assert_eq!(s.radius_m(), 10_000.0);
    }

    #[test]
    fn cone_directional_volume() {
        let up = Cone::new(Geodetic::new(0.0, 0.0, 0.0), 0.0, 90.0, 30.0, 20_000.0);
        assert!(up.contains(Geodetic::new(0.0, 0.0, 5000.0)));
        assert!(!up.contains(Geodetic::new(0.0, 0.0, 25_000.0)));
        assert!(!up.contains(Geodetic::new(0.2, 0.0, 1000.0)));
        assert!(up.contains(up.apex()));
        let east = Cone::new(Geodetic::new(0.0, 0.0, 1000.0), 90.0, 0.0, 20.0, 100_000.0);
        assert!(east.contains(Geodetic::new(0.0, 0.3, 1000.0)));
        assert!(!east.contains(Geodetic::new(0.3, 0.0, 1000.0)));
        assert_eq!(east.axis().azimuth_deg, 90.0);
        assert_eq!(east.half_angle_deg(), 20.0);
    }

    #[test]
    fn cylinder() {
        let c = Cylinder::new(Geodetic::new(34.0, -118.0, 0.0), 5_000.0, AltitudeBand::above(100.0));
        assert!(c.contains(Geodetic::new(34.0, -118.0, 200.0)));
        assert!(!c.contains(Geodetic::new(34.0, -118.0, 50.0))); // under the floor
        assert!(!c.contains(Geodetic::new(34.1, -118.0, 200.0))); // ~11 km north
        assert_eq!(c.center(), Geodetic::new(34.0, -118.0, 0.0));
        assert_eq!(c.radius_m(), 5_000.0);
        assert_eq!(c.band(), AltitudeBand::above(100.0));
    }
}