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

// Edge utilities — crossing tests, interpolation, distance.

use crate::point::S2Point;

/// Test whether two edges (a0->a1) and (b0->b1) cross each other.
/// Returns true if the edges properly cross (interiors intersect).
/// Does not count shared endpoints as crossings.
pub fn crosses(a0: &S2Point, a1: &S2Point, b0: &S2Point, b1: &S2Point) -> bool {
    // Two great-circle arcs cross if and only if:
    // 1) a0 and a1 are on opposite sides of the plane through (origin, b0, b1), AND
    // 2) b0 and b1 are on opposite sides of the plane through (origin, a0, a1).
    //
    // The normal to the plane through b0 and b1 is b0 x b1.
    // A point p is on the positive side if dot(p, b0 x b1) > 0.

    let n_a = a0.cross(*a1); // normal to edge A's great circle
    let n_b = b0.cross(*b1); // normal to edge B's great circle

    // Check if b0 and b1 are on opposite sides of edge A's plane.
    let d_b0 = b0.dot(n_a);
    let d_b1 = b1.dot(n_a);

    if d_b0 * d_b1 >= 0.0 {
        return false; // b0, b1 on same side (or on the plane)
    }

    // Check if a0 and a1 are on opposite sides of edge B's plane.
    let d_a0 = a0.dot(n_b);
    let d_a1 = a1.dot(n_b);

    if d_a0 * d_a1 >= 0.0 {
        return false; // a0, a1 on same side
    }

    true
}

/// Spherical linear interpolation (slerp) between two points.
/// t=0 returns a, t=1 returns b.
pub fn interpolate(a: &S2Point, b: &S2Point, t: f64) -> S2Point {
    let angle = a.angle(*b);

    if angle.abs() < 1e-15 {
        return *a;
    }

    let sin_angle = angle.sin();
    let sa = ((1.0 - t) * angle).sin() / sin_angle;
    let sb = (t * angle).sin() / sin_angle;

    S2Point::new(
        sa * a.x + sb * b.x,
        sa * a.y + sb * b.y,
        sa * a.z + sb * b.z,
    )
}

/// Angular distance from a point to the nearest point on a great-circle edge (a->b).
/// Returns the distance in radians.
pub fn distance_to_edge(point: &S2Point, a: &S2Point, b: &S2Point) -> f64 {
    // Normal to the great circle containing the edge.
    let n = a.cross(*b);
    let n_norm = n.norm();

    if n_norm < 1e-15 {
        // Degenerate edge: a and b are nearly identical (or antipodal).
        return point.angle(*a);
    }

    // Signed distance from point to the great circle plane.
    let sin_dist = point.dot(n) / n_norm;

    // The closest point on the great circle is at distance asin(|sin_dist|).
    // But we need to check if the projection falls within the arc.

    // Project point onto the great circle.
    let proj = point.add(n.scale(-sin_dist / n_norm)).normalize();

    // Check if the projection falls within the arc a->b.
    // It does if the cross products a x proj and proj x b have the same sign
    // as a x b (all relative to the normal).
    let ab_sign = a.cross(*b).dot(n);
    let ap_sign = a.cross(proj).dot(n);
    let pb_sign = proj.cross(*b).dot(n);

    if ab_sign > 0.0 && ap_sign >= 0.0 && pb_sign >= 0.0 {
        // Projection is within the arc.
        sin_dist.abs().asin()
    } else if ab_sign < 0.0 && ap_sign <= 0.0 && pb_sign <= 0.0 {
        sin_dist.abs().asin()
    } else {
        // Projection is outside the arc. Distance is to the nearest endpoint.
        point.angle(*a).min(point.angle(*b))
    }
}

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

    #[test]
    fn crossing_edges() {
        // Two edges that cross on the equator.
        let a0 = S2Point::from_lat_lng(0.1, -0.1);
        let a1 = S2Point::from_lat_lng(-0.1, 0.1);
        let b0 = S2Point::from_lat_lng(-0.1, -0.1);
        let b1 = S2Point::from_lat_lng(0.1, 0.1);
        assert!(crosses(&a0, &a1, &b0, &b1));
    }

    #[test]
    fn non_crossing_edges() {
        // Two parallel-ish edges that don't cross.
        let a0 = S2Point::from_lat_lng(0.1, 0.0);
        let a1 = S2Point::from_lat_lng(0.2, 0.0);
        let b0 = S2Point::from_lat_lng(0.1, 0.5);
        let b1 = S2Point::from_lat_lng(0.2, 0.5);
        assert!(!crosses(&a0, &a1, &b0, &b1));
    }

    #[test]
    fn interpolate_endpoints() {
        let a = S2Point::new(1.0, 0.0, 0.0);
        let b = S2Point::new(0.0, 1.0, 0.0);

        let p0 = interpolate(&a, &b, 0.0);
        assert!((p0.x - a.x).abs() < 1e-14);
        assert!((p0.y - a.y).abs() < 1e-14);

        let p1 = interpolate(&a, &b, 1.0);
        assert!((p1.x - b.x).abs() < 1e-14);
        assert!((p1.y - b.y).abs() < 1e-14);
    }

    #[test]
    fn interpolate_midpoint() {
        let a = S2Point::new(1.0, 0.0, 0.0);
        let b = S2Point::new(0.0, 1.0, 0.0);
        let mid = interpolate(&a, &b, 0.5);
        // Midpoint should be equidistant from both.
        let da = mid.angle(a);
        let db = mid.angle(b);
        assert!((da - db).abs() < 1e-14);
    }

    #[test]
    fn distance_to_edge_on_edge() {
        let a = S2Point::new(1.0, 0.0, 0.0);
        let b = S2Point::new(0.0, 1.0, 0.0);
        let mid = interpolate(&a, &b, 0.5).normalize();
        let dist = distance_to_edge(&mid, &a, &b);
        assert!(dist < 1e-12, "point on edge should have distance ~0, got {dist}");
    }

    #[test]
    fn distance_to_edge_at_pole() {
        // Edge along equator, point at north pole.
        let a = S2Point::new(1.0, 0.0, 0.0);
        let b = S2Point::new(0.0, 1.0, 0.0);
        let pole = S2Point::new(0.0, 0.0, 1.0);
        let dist = distance_to_edge(&pole, &a, &b);
        assert!(
            (dist - PI / 2.0).abs() < 1e-12,
            "north pole to equatorial edge should be PI/2, got {dist}"
        );
    }

    #[test]
    fn distance_to_degenerate_edge() {
        let a = S2Point::new(1.0, 0.0, 0.0);
        let p = S2Point::new(0.0, 1.0, 0.0);
        let dist = distance_to_edge(&p, &a, &a);
        assert!((dist - PI / 2.0).abs() < 1e-12);
    }
}