bevy_a5 0.1.2

//! Coordinate conversion utilities between geographic (lon/lat) and 3D Cartesian coordinates.

use a5::LonLat;
use bevy_math::{DVec3, Vec3};

/// Convert a [`LonLat`] (degrees) to a 3D position on a sphere of the given `radius`.
///
/// The coordinate system uses:
/// - Y-axis pointing toward the north pole
/// - X-axis pointing toward (0°N, 0°E)
/// - Z-axis pointing toward (0°N, 90°E)
pub fn lonlat_to_dvec3(ll: &LonLat, radius: f64) -> DVec3 {
    let lat = ll.latitude().to_radians();
    let lng = ll.longitude().to_radians();
    DVec3::new(
        radius * lat.cos() * lng.cos(),
        radius * lat.sin(),
        radius * lat.cos() * lng.sin(),
    )
}

/// Convert a [`LonLat`] to a single-precision 3D position on a sphere.
pub fn lonlat_to_vec3(ll: &LonLat, radius: f64) -> Vec3 {
    lonlat_to_dvec3(ll, radius).as_vec3()
}

/// Convert a 3D position back to [`LonLat`] (degrees).
///
/// Returns `None` if the position is at the origin (degenerate).
pub fn dvec3_to_lonlat(pos: DVec3) -> Option<LonLat> {
    let r = pos.length();
    if r < 1e-12 {
        return None;
    }
    let lat = (pos.y / r).asin().to_degrees();
    let lng = pos.z.atan2(pos.x).to_degrees();
    Some(LonLat::new(lng, lat))
}

/// Compute a local tangent frame at the given [`LonLat`].
///
/// Returns `(east, up, north)` where:
/// - `up` points radially outward
/// - `north` points toward increasing latitude
/// - `east` points toward increasing longitude
pub fn tangent_frame(ll: &LonLat) -> (DVec3, DVec3, DVec3) {
    let lat = ll.latitude().to_radians();
    let lng = ll.longitude().to_radians();

    // Up = radial direction
    let up = DVec3::new(lat.cos() * lng.cos(), lat.sin(), lat.cos() * lng.sin());

    // North = d(position)/d(lat), normalized
    let north = DVec3::new(-lat.sin() * lng.cos(), lat.cos(), -lat.sin() * lng.sin()).normalize();

    // East = d(position)/d(lng), normalized
    let east = DVec3::new(-lng.sin(), 0.0, lng.cos()).normalize();

    (east, up, north)
}

/// Convert an A5 cell to its 3D center position on a sphere.
///
/// Returns `None` if the cell index is invalid.
pub fn cell_to_dvec3(cell: u64, radius: f64) -> Option<DVec3> {
    let ll = a5::cell_to_lonlat(cell).ok()?;
    Some(lonlat_to_dvec3(&ll, radius))
}

/// Convert an A5 cell to its single-precision 3D center position.
///
/// Returns `None` if the cell index is invalid.
pub fn cell_to_vec3(cell: u64, radius: f64) -> Option<Vec3> {
    cell_to_dvec3(cell, radius).map(|v| v.as_vec3())
}

/// Project a world-space position radially onto the planet sphere and return
/// its [`LonLat`].
///
/// The point's distance from the planet centre is irrelevant — only the
/// direction is used. Returns `None` if `pos` is the origin.
pub fn world_pos_to_lonlat(pos: DVec3) -> Option<LonLat> {
    dvec3_to_lonlat(pos)
}

/// Project a world-space position to the A5 cell index containing its
/// sub-point on the sphere, at the given resolution.
///
/// This is the inverse of [`cell_to_dvec3`] for sub-cell positions: given any
/// 3D point near (or far from) the planet, return the cell whose footprint
/// covers the point's projection onto the sphere.
///
/// Returns `None` if `pos` is the origin or `resolution` is invalid.
pub fn world_pos_to_cell(pos: DVec3, resolution: i32) -> Option<u64> {
    let ll = dvec3_to_lonlat(pos)?;
    a5::lonlat_to_cell(ll, resolution).ok()
}

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

    #[test]
    fn roundtrip_lonlat_to_dvec3() {
        let ll = LonLat::new(90.0, 45.0);
        let pos = lonlat_to_dvec3(&ll, 6371.0);
        let ll2 = dvec3_to_lonlat(pos).unwrap();
        assert!((ll.latitude() - ll2.latitude()).abs() < 1e-10);
        assert!((ll.longitude() - ll2.longitude()).abs() < 1e-10);
    }

    #[test]
    fn north_pole_up() {
        let ll = LonLat::new(0.0, 90.0);
        let (_, up, _) = tangent_frame(&ll);
        assert!((up.y - 1.0).abs() < 1e-10);
        assert!(up.x.abs() < 1e-10);
        assert!(up.z.abs() < 1e-10);
    }
}