use std::f64::consts::PI;
pub const EARTH_R: f64 = 6_371_008.8;
#[inline]
fn rad(d: f64) -> f64 {
d * PI / 180.0
}
#[inline]
fn deg(r: f64) -> f64 {
r * 180.0 / PI
}
#[derive(Debug, Clone, Copy, PartialEq)]
pub struct LonLat {
pub lon: f64,
pub lat: f64,
}
impl LonLat {
pub fn new(lon: f64, lat: f64) -> Self {
LonLat { lon, lat }
}
}
pub fn haversine(a: LonLat, b: LonLat) -> f64 {
let (la1, la2, dlo, dla) = (
rad(a.lat),
rad(b.lat),
rad(b.lon - a.lon),
rad(b.lat - a.lat),
);
let h = (dla / 2.0).sin().powi(2) + la1.cos() * la2.cos() * (dlo / 2.0).sin().powi(2);
2.0 * EARTH_R * h.sqrt().asin()
}
pub fn bearing(a: LonLat, b: LonLat) -> f64 {
let (la1, la2, dlo) = (rad(a.lat), rad(b.lat), rad(b.lon - a.lon));
let y = dlo.sin() * la2.cos();
let x = la1.cos() * la2.sin() - la1.sin() * la2.cos() * dlo.cos();
(deg(y.atan2(x)) + 360.0) % 360.0
}
pub fn destination(a: LonLat, dist_m: f64, bearing_deg: f64) -> LonLat {
let (la1, lo1, br, d) = (rad(a.lat), rad(a.lon), rad(bearing_deg), dist_m / EARTH_R);
let la2 = (la1.sin() * d.cos() + la1.cos() * d.sin() * br.cos()).asin();
let lo2 = lo1 + (br.sin() * d.sin() * la1.cos()).atan2(d.cos() - la1.sin() * la2.sin());
LonLat::new(deg(lo2), deg(la2))
}
pub fn polyline_length(points: &[LonLat]) -> f64 {
points.windows(2).map(|w| haversine(w[0], w[1])).sum()
}
#[cfg(test)]
mod tests {
use super::*;
#[test]
fn hanoi_to_haiphong_distance() {
let hanoi = LonLat::new(105.85, 21.02);
let haiphong = LonLat::new(106.68, 20.86);
let d = haversine(hanoi, haiphong);
assert!((d - 88_000.0).abs() < 5_000.0, "got {d}");
}
#[test]
fn bearing_due_north() {
let a = LonLat::new(105.0, 21.0);
let b = LonLat::new(105.0, 22.0);
assert!((bearing(a, b) - 0.0).abs() < 0.5);
}
#[test]
fn destination_roundtrip() {
let a = LonLat::new(105.85, 21.02);
let b = destination(a, 10_000.0, 90.0);
let d = haversine(a, b);
assert!((d - 10_000.0).abs() < 1.0, "got {d}");
assert!((bearing(a, b) - 90.0).abs() < 0.5);
}
#[test]
fn polyline_sums_segments() {
let pts = [
LonLat::new(0.0, 0.0),
LonLat::new(0.0, 1.0),
LonLat::new(0.0, 2.0),
];
let total = polyline_length(&pts);
let one = haversine(pts[0], pts[1]);
assert!((total - 2.0 * one).abs() < 1.0);
}
}