use geo::{Distance, Haversine, Point};
#[cfg(feature = "numkong")]
use numkong::Haversine as NumKongHaversine;
use rayon::prelude::*;
#[cfg(feature = "numkong")]
const GEO_HAVERSINE_RADIUS_M: f64 = 6_371_008.8;
#[cfg(feature = "numkong")]
const NUMKONG_HAVERSINE_RADIUS_M: f64 = 6_335_439.0;
#[cfg(feature = "numkong")]
const NUMKONG_TO_GEO_KM: f64 = GEO_HAVERSINE_RADIUS_M / NUMKONG_HAVERSINE_RADIUS_M / 1000.0;
pub fn haversine_km(lat1: f64, lon1: f64, lat2: f64, lon2: f64) -> f64 {
let p1 = Point::new(lon1, lat1);
let p2 = Point::new(lon2, lat2);
Haversine.distance(p1, p2) / 1000.0
}
pub fn haversine_m_batch(
lat1: &[f64],
lng1: &[f64],
lat2: &[f64],
lng2: &[f64],
) -> Result<Vec<f64>, String> {
if lat1.len() != lng1.len() || lat1.len() != lat2.len() || lat1.len() != lng2.len() {
return Err("lat1, lng1, lat2, and lng2 must have the same length".to_string());
}
Ok((0..lat1.len())
.into_par_iter()
.map(|i| haversine_km(lat1[i], lng1[i], lat2[i], lng2[i]) * 1000.0)
.collect())
}
pub fn adjacent_haversine_distances_km(
latitudes: &[f64],
longitudes: &[f64],
start: usize,
end: usize,
) -> Vec<f64> {
if end - start < 2 {
return Vec::new();
}
let mut distances = vec![0.0; end - start - 1];
adjacent_haversine_distances_into_km(latitudes, longitudes, start, end, &mut distances);
distances
}
pub fn adjacent_haversine_distances_into_km(
latitudes: &[f64],
longitudes: &[f64],
start: usize,
end: usize,
distances: &mut [f64],
) {
if end - start < 2 {
return;
}
#[cfg(feature = "numkong")]
{
let latitudes_rad: Vec<f64> = latitudes[start..end]
.iter()
.map(|lat| lat.to_radians())
.collect();
let longitudes_rad: Vec<f64> = longitudes[start..end]
.iter()
.map(|lon| lon.to_radians())
.collect();
f64::haversine(
&latitudes_rad[..latitudes_rad.len() - 1],
&longitudes_rad[..longitudes_rad.len() - 1],
&latitudes_rad[1..],
&longitudes_rad[1..],
distances,
)
.expect("adjacent coordinate slices have matching lengths");
distances
.iter_mut()
.for_each(|distance| *distance *= NUMKONG_TO_GEO_KM);
}
#[cfg(not(feature = "numkong"))]
for (idx, distance) in distances.iter_mut().enumerate() {
*distance = haversine_km(
latitudes[start + idx],
longitudes[start + idx],
latitudes[start + idx + 1],
longitudes[start + idx + 1],
);
}
}
pub fn adjacent_haversine_sum_km(
latitudes: &[f64],
longitudes: &[f64],
start: usize,
end: usize,
) -> f64 {
adjacent_haversine_distances_km(latitudes, longitudes, start, end)
.into_iter()
.sum()
}
pub fn adjacent_haversine_max_km(
latitudes: &[f64],
longitudes: &[f64],
start: usize,
end: usize,
) -> f64 {
adjacent_haversine_distances_km(latitudes, longitudes, start, end)
.into_iter()
.fold(0.0f64, f64::max)
}
const EARTH_RADIUS_KM: f64 = 6371.0088;
pub fn project_local_planar_km(latitudes: &[f64], longitudes: &[f64]) -> Vec<(f64, f64)> {
let n = latitudes.len();
if n == 0 {
return Vec::new();
}
let (mean_lat_rad, cos_mean_lat) = local_planar_km_params(latitudes);
project_local_planar_km_with_params(latitudes, longitudes, mean_lat_rad, cos_mean_lat)
}
pub fn local_planar_km_params(latitudes: &[f64]) -> (f64, f64) {
let n = latitudes.len();
if n == 0 {
return (0.0, 1.0);
}
let mean_lat_rad = (latitudes.iter().sum::<f64>() / n as f64).to_radians();
(mean_lat_rad, mean_lat_rad.cos())
}
pub fn project_local_planar_km_with_params(
latitudes: &[f64],
longitudes: &[f64],
mean_lat_rad: f64,
cos_mean_lat: f64,
) -> Vec<(f64, f64)> {
debug_assert_eq!(latitudes.len(), longitudes.len());
let _ = mean_lat_rad;
latitudes
.iter()
.zip(longitudes.iter())
.map(|(&lat, &lng)| {
let x = EARTH_RADIUS_KM * lng.to_radians() * cos_mean_lat;
let y = EARTH_RADIUS_KM * lat.to_radians();
(x, y)
})
.collect()
}
pub fn unproject_local_planar_km(
points: &[(f64, f64)],
mean_lat_rad: f64,
cos_mean_lat: f64,
) -> (Vec<f64>, Vec<f64>) {
let _ = mean_lat_rad;
let mut lats = Vec::with_capacity(points.len());
let mut lngs = Vec::with_capacity(points.len());
for &(x, y) in points {
lats.push((y / EARTH_RADIUS_KM).to_degrees());
lngs.push((x / (EARTH_RADIUS_KM * cos_mean_lat)).to_degrees());
}
(lats, lngs)
}
pub fn point_to_segment_distance_km(point: (f64, f64), a: (f64, f64), b: (f64, f64)) -> f64 {
let (px, py) = point;
let (ax, ay) = a;
let (bx, by) = b;
let dx = bx - ax;
let dy = by - ay;
let len_sq = dx * dx + dy * dy;
let (proj_x, proj_y) = if len_sq == 0.0 {
(ax, ay)
} else {
let t = ((px - ax) * dx + (py - ay) * dy) / len_sq;
let t_clamped = t.clamp(0.0, 1.0);
(ax + t_clamped * dx, ay + t_clamped * dy)
};
let ddx = px - proj_x;
let ddy = py - proj_y;
(ddx * ddx + ddy * ddy).sqrt()
}
pub fn bearing_deg(lat1: f64, lng1: f64, lat2: f64, lng2: f64) -> f64 {
let lat1_rad = lat1.to_radians();
let lat2_rad = lat2.to_radians();
let delta_lng_rad = (lng2 - lng1).to_radians();
let x = delta_lng_rad.sin() * lat2_rad.cos();
let y = lat1_rad.cos() * lat2_rad.sin() - lat1_rad.sin() * lat2_rad.cos() * delta_lng_rad.cos();
let initial_bearing_deg = x.atan2(y).to_degrees();
(initial_bearing_deg + 360.0) % 360.0
}
pub fn angular_difference(degrees1: f64, degrees2: f64) -> f64 {
let diff = (degrees1 - degrees2).abs();
if diff > 180.0 {
(diff - 360.0).abs()
} else {
diff
}
}
#[cfg(test)]
mod bearing_tests {
use super::*;
#[test]
fn bearing_due_north_is_zero() {
let bearing = bearing_deg(48.0, 2.0, 49.0, 2.0);
assert!(bearing.abs() < 1e-6, "expected ~0 deg, got {bearing}");
}
#[test]
fn bearing_due_east_is_ninety() {
let bearing = bearing_deg(0.0, 0.0, 0.0, 1.0);
assert!(
(bearing - 90.0).abs() < 1e-6,
"expected ~90 deg, got {bearing}"
);
}
#[test]
fn angular_difference_handles_wraparound() {
assert!((angular_difference(350.0, 10.0) - 20.0).abs() < 1e-9);
assert!((angular_difference(10.0, 350.0) - 20.0).abs() < 1e-9);
assert!((angular_difference(0.0, 180.0) - 180.0).abs() < 1e-9);
}
}