open-standards/map/map-rs/crates/map-index/src/point.rs

// S2Point — unit vector on the unit sphere.

use serde::{Deserialize, Serialize};

/// A point on the unit sphere represented as a unit vector (x, y, z).
#[derive(Clone, Copy, Debug, Serialize, Deserialize)]
pub struct S2Point {
    pub x: f64,
    pub y: f64,
    pub z: f64,
}

impl PartialEq for S2Point {
    fn eq(&self, other: &Self) -> bool {
        (self.x - other.x).abs() < 1e-15
            && (self.y - other.y).abs() < 1e-15
            && (self.z - other.z).abs() < 1e-15
    }
}

impl S2Point {
    /// Create a new S2Point. Does NOT normalize automatically.
    #[inline]
    pub const fn new(x: f64, y: f64, z: f64) -> Self {
        Self { x, y, z }
    }

    /// Construct from latitude and longitude in radians.
    #[inline]
    pub fn from_lat_lng(lat_rad: f64, lng_rad: f64) -> Self {
        let cos_lat = lat_rad.cos();
        Self {
            x: cos_lat * lng_rad.cos(),
            y: cos_lat * lng_rad.sin(),
            z: lat_rad.sin(),
        }
    }

    /// Convert to (latitude, longitude) in radians.
    #[inline]
    pub fn to_lat_lng(self) -> (f64, f64) {
        let lat = self.z.clamp(-1.0, 1.0).asin();
        let lng = self.y.atan2(self.x);
        (lat, lng)
    }

    /// Squared length of the vector.
    #[inline]
    pub fn norm2(self) -> f64 {
        self.x * self.x + self.y * self.y + self.z * self.z
    }

    /// Length of the vector.
    #[inline]
    pub fn norm(self) -> f64 {
        self.norm2().sqrt()
    }

    /// Return a normalized copy (unit vector).
    #[inline]
    pub fn normalize(self) -> Self {
        let n = self.norm();
        if n == 0.0 {
            return self;
        }
        Self {
            x: self.x / n,
            y: self.y / n,
            z: self.z / n,
        }
    }

    /// Dot product.
    #[inline]
    pub fn dot(self, other: Self) -> f64 {
        self.x * other.x + self.y * other.y + self.z * other.z
    }

    /// Cross product.
    #[inline]
    pub fn cross(self, other: Self) -> Self {
        Self {
            x: self.y * other.z - self.z * other.y,
            y: self.z * other.x - self.x * other.z,
            z: self.x * other.y - self.y * other.x,
        }
    }

    /// Angular distance to another point (in radians).
    #[inline]
    pub fn angle(self, other: Self) -> f64 {
        // Use atan2 for numerical stability at small and large angles.
        let cross_norm = self.cross(other).norm();
        let dot = self.dot(other);
        cross_norm.atan2(dot)
    }

    /// Negate the point.
    #[inline]
    #[allow(clippy::should_implement_trait)] // inherent S2Point vector helper
    pub fn neg(self) -> Self {
        Self {
            x: -self.x,
            y: -self.y,
            z: -self.z,
        }
    }

    /// Add two points (vector addition, result is NOT normalized).
    #[inline]
    #[allow(clippy::should_implement_trait)] // inherent S2Point vector helper
    pub fn add(self, other: Self) -> Self {
        Self {
            x: self.x + other.x,
            y: self.y + other.y,
            z: self.z + other.z,
        }
    }

    /// Scale by a scalar.
    #[inline]
    pub fn scale(self, s: f64) -> Self {
        Self {
            x: self.x * s,
            y: self.y * s,
            z: self.z * s,
        }
    }
}

#[cfg(test)]
mod tests {
    use super::*;
    use std::f64::consts::PI;

    #[test]
    fn from_lat_lng_origin() {
        // lat=0, lon=0 should be (1, 0, 0)
        let p = S2Point::from_lat_lng(0.0, 0.0);
        assert!((p.x - 1.0).abs() < 1e-14);
        assert!(p.y.abs() < 1e-14);
        assert!(p.z.abs() < 1e-14);
    }

    #[test]
    fn from_lat_lng_north_pole() {
        let p = S2Point::from_lat_lng(PI / 2.0, 0.0);
        assert!(p.x.abs() < 1e-14);
        assert!(p.y.abs() < 1e-14);
        assert!((p.z - 1.0).abs() < 1e-14);
    }

    #[test]
    fn round_trip_lat_lng() {
        let lat = 0.7;
        let lng = -1.3;
        let p = S2Point::from_lat_lng(lat, lng);
        let (lat2, lng2) = p.to_lat_lng();
        assert!((lat - lat2).abs() < 1e-14);
        assert!((lng - lng2).abs() < 1e-14);
    }

    #[test]
    fn normalize_works() {
        let p = S2Point::new(3.0, 4.0, 0.0).normalize();
        assert!((p.norm() - 1.0).abs() < 1e-14);
    }

    #[test]
    fn cross_product() {
        let x = S2Point::new(1.0, 0.0, 0.0);
        let y = S2Point::new(0.0, 1.0, 0.0);
        let z = x.cross(y);
        assert!((z.x).abs() < 1e-14);
        assert!((z.y).abs() < 1e-14);
        assert!((z.z - 1.0).abs() < 1e-14);
    }

    #[test]
    fn dot_product() {
        let a = S2Point::new(1.0, 0.0, 0.0);
        let b = S2Point::new(0.0, 1.0, 0.0);
        assert!(a.dot(b).abs() < 1e-14);
        assert!((a.dot(a) - 1.0).abs() < 1e-14);
    }

    #[test]
    fn angle_90_degrees() {
        let a = S2Point::new(1.0, 0.0, 0.0);
        let b = S2Point::new(0.0, 1.0, 0.0);
        assert!((a.angle(b) - PI / 2.0).abs() < 1e-14);
    }

    #[test]
    fn angle_opposite() {
        let a = S2Point::new(1.0, 0.0, 0.0);
        let b = S2Point::new(-1.0, 0.0, 0.0);
        assert!((a.angle(b) - PI).abs() < 1e-14);
    }

    #[test]
    fn serialize_deserialize() {
        let p = S2Point::new(1.0, 0.0, 0.0);
        let json = serde_json::to_string(&p).unwrap();
        let p2: S2Point = serde_json::from_str(&json).unwrap();
        assert_eq!(p, p2);
    }
}