use crate::utils::haversine::haversine_km;
use std::f64::consts::PI;
use super::stvd_candidate_graph::CandidateGraphConfig;
use super::stvd_sparse_sinkhorn::sparse_stvd_emd_impl;
#[derive(Clone, Copy, Debug)]
pub struct SinkhornConfig {
pub reg: f64,
pub max_iter: usize,
pub tol: Option<f64>,
}
impl Default for SinkhornConfig {
fn default() -> Self {
Self { reg: 0.01, max_iter: 200, tol: None }
}
}
fn angular_diff(theta1: f64, theta2: f64) -> f64 {
let diff = (theta1 - theta2).abs() % (2.0 * PI);
diff.min(2.0 * PI - diff)
}
fn temporal_chord_m(t1: f64, t2: f64, cyclical_period: f64, r: f64) -> f64 {
let theta1 = 2.0 * PI * (t1.rem_euclid(cyclical_period) / cyclical_period);
let theta2 = 2.0 * PI * (t2.rem_euclid(cyclical_period) / cyclical_period);
2.0 * r * (angular_diff(theta1, theta2) / 2.0).sin()
}
#[allow(clippy::too_many_arguments)]
pub(crate) fn pair_cost_m(
lat_a: f64,
lng_a: f64,
t_a: f64,
lat_b: f64,
lng_b: f64,
t_b: f64,
cyclical_period: f64,
r: f64,
) -> f32 {
let spatial_m = haversine_km(lat_a, lng_a, lat_b, lng_b) * 1000.0;
let temporal_m = temporal_chord_m(t_a, t_b, cyclical_period, r);
(spatial_m.powi(2) + temporal_m.powi(2)).sqrt() as f32
}
#[allow(clippy::too_many_arguments)]
pub fn stvd_emd_impl(
lats_a: &[f64],
lngs_a: &[f64],
ts_a: &[f64],
ws_a: &[f64],
lats_b: &[f64],
lngs_b: &[f64],
ts_b: &[f64],
ws_b: &[f64],
alpha: f64,
cyclical_period: f64,
sinkhorn: SinkhornConfig,
) -> Result<f64, String> {
let n = lats_a.len();
let m = lats_b.len();
if n == 0 || m == 0 {
return Err("distributions must be non-empty".to_string());
}
if lngs_a.len() != n || ts_a.len() != n || ws_a.len() != n {
return Err("all arrays for distribution A must have the same length".to_string());
}
if lngs_b.len() != m || ts_b.len() != m || ws_b.len() != m {
return Err("all arrays for distribution B must have the same length".to_string());
}
if cyclical_period <= 0.0 {
return Err("cyclical_period must be positive".to_string());
}
if !alpha.is_finite() || !cyclical_period.is_finite() {
return Err("alpha and cyclical_period must be finite".to_string());
}
if lats_a
.iter()
.chain(lngs_a)
.chain(ts_a)
.chain(lats_b)
.chain(lngs_b)
.chain(ts_b)
.any(|value| !value.is_finite())
{
return Err("coordinates and times must be finite".to_string());
}
if ws_a
.iter()
.chain(ws_b)
.any(|weight| !weight.is_finite() || *weight < 0.0)
{
return Err("weights must be finite and non-negative".to_string());
}
if sinkhorn.reg <= 0.0 || !sinkhorn.reg.is_finite() {
return Err("sinkhorn reg must be positive and finite".to_string());
}
if sinkhorn.max_iter == 0 {
return Err("sinkhorn max_iter must be positive".to_string());
}
if let Some(tol) = sinkhorn.tol
&& (tol <= 0.0 || !tol.is_finite())
{
return Err("sinkhorn tol must be positive and finite".to_string());
}
let sum_a: f64 = ws_a.iter().sum();
let sum_b: f64 = ws_b.iter().sum();
if sum_a <= 0.0 || sum_b <= 0.0 {
return Err("weights must sum to a positive value".to_string());
}
sparse_stvd_emd_impl(
lats_a, lngs_a, ts_a, ws_a, lats_b, lngs_b, ts_b, ws_b, alpha, cyclical_period, sinkhorn,
CandidateGraphConfig::default(),
)
}
#[cfg(test)]
#[allow(clippy::too_many_arguments)]
pub(crate) fn dense_reference_impl(
lats_a: &[f64],
lngs_a: &[f64],
ts_a: &[f64],
ws_a: &[f64],
lats_b: &[f64],
lngs_b: &[f64],
ts_b: &[f64],
ws_b: &[f64],
alpha: f64,
cyclical_period: f64,
sinkhorn: SinkhornConfig,
) -> Result<f64, String> {
use ndarray::{Array1, Array2};
use rayon::prelude::*;
use wass::{sinkhorn_log, sinkhorn_log_with_convergence};
let n = lats_a.len();
let m = lats_b.len();
let r = (alpha * cyclical_period) / (2.0 * PI);
let cost_len = n * m;
let mut cost_values = vec![0.0_f32; cost_len];
cost_values
.par_chunks_mut(m)
.enumerate()
.for_each(|(i, row)| {
for j in 0..m {
row[j] = pair_cost_m(
lats_a[i], lngs_a[i], ts_a[i], lats_b[j], lngs_b[j], ts_b[j], cyclical_period, r,
);
}
});
let cost =
Array2::from_shape_vec((n, m), cost_values).expect("cost buffer has exactly n * m entries");
let a: Array1<f32> = ws_a.iter().map(|&w| w as f32).collect();
let b: Array1<f32> = ws_b.iter().map(|&w| w as f32).collect();
let reg = sinkhorn.reg as f32;
let distance = if let Some(tol) = sinkhorn.tol {
let (_, distance, _iterations) =
sinkhorn_log_with_convergence(&a, &b, &cost, reg, sinkhorn.max_iter, tol as f32)
.map_err(|err| err.to_string())?;
distance
} else {
let (_, distance) = sinkhorn_log(&a, &b, &cost, reg, sinkhorn.max_iter);
distance
};
Ok(distance as f64)
}
#[cfg(test)]
mod tests {
use super::*;
#[test]
fn identical_single_point_is_zero_distance() {
let value = stvd_emd_impl(
&[10.0],
&[20.0],
&[480.0],
&[1.0],
&[10.0],
&[20.0],
&[480.0],
&[1.0],
10.0,
1440.0,
SinkhornConfig::default(),
)
.unwrap();
assert!(value.abs() < 1e-6, "got {value}");
}
#[test]
fn spatial_displacement_matches_haversine_distance() {
let lat1 = 0.0;
let lng1 = 0.0;
let lat2 = 0.0;
let lng2 = 1.0; let expected_m = haversine_km(lat1, lng1, lat2, lng2) * 1000.0;
let value = stvd_emd_impl(
&[lat1],
&[lng1],
&[480.0],
&[1.0],
&[lat2],
&[lng2],
&[480.0],
&[1.0],
10.0,
1440.0,
SinkhornConfig::default(),
)
.unwrap();
assert!(
(value - expected_m).abs() / expected_m < 0.05,
"got {value}, expected close to {expected_m}"
);
}
#[test]
fn cyclical_time_wraps_around() {
let near = stvd_emd_impl(
&[0.0],
&[0.0],
&[1439.0],
&[1.0],
&[0.0],
&[0.0],
&[1.0],
&[1.0],
10.0,
1440.0,
SinkhornConfig::default(),
)
.unwrap();
let far = stvd_emd_impl(
&[0.0],
&[0.0],
&[1439.0],
&[1.0],
&[0.0],
&[0.0],
&[700.0],
&[1.0],
10.0,
1440.0,
SinkhornConfig::default(),
)
.unwrap();
assert!(near < far, "near={near} far={far}");
}
#[test]
fn empty_distribution_is_rejected() {
assert!(stvd_emd_impl(
&[],
&[],
&[],
&[],
&[0.0],
&[0.0],
&[0.0],
&[1.0],
10.0,
1440.0,
SinkhornConfig::default(),
)
.is_err());
}
#[test]
fn large_input_completes_via_sparse_path() {
let n = 24_000;
let m = 24_000;
struct XorShift(u64);
impl XorShift {
fn next(&mut self) -> u64 {
self.0 ^= self.0 << 13;
self.0 ^= self.0 >> 7;
self.0 ^= self.0 << 17;
self.0
}
fn range_f64(&mut self, lo: f64, hi: f64) -> f64 {
let frac = (self.next() % 1_000_000) as f64 / 1_000_000.0;
lo + frac * (hi - lo)
}
}
let mut rng = XorShift(7);
let mut gen_side = |count: usize| -> (Vec<f64>, Vec<f64>, Vec<f64>, Vec<f64>) {
let mut lats = Vec::with_capacity(count);
let mut lngs = Vec::with_capacity(count);
let mut ts = Vec::with_capacity(count);
let mut ws = Vec::with_capacity(count);
for _ in 0..count {
lats.push(rng.range_f64(48.5, 49.1));
lngs.push(rng.range_f64(2.0, 2.6));
ts.push(rng.range_f64(0.0, 1439.0));
ws.push(rng.range_f64(0.1, 1.0));
}
(lats, lngs, ts, ws)
};
let (lats_a, lngs_a, ts_a, ws_a) = gen_side(n);
let (lats_b, lngs_b, ts_b, ws_b) = gen_side(m);
let value = stvd_emd_impl(
&lats_a,
&lngs_a,
&ts_a,
&ws_a,
&lats_b,
&lngs_b,
&ts_b,
&ws_b,
10.0,
1440.0,
SinkhornConfig::default(),
)
.unwrap();
assert!(value.is_finite() && value >= 0.0, "got {value}");
}
#[test]
fn non_positive_cyclical_period_is_rejected() {
assert!(stvd_emd_impl(
&[0.0],
&[0.0],
&[0.0],
&[1.0],
&[0.0],
&[0.0],
&[0.0],
&[1.0],
10.0,
0.0,
SinkhornConfig::default(),
)
.is_err());
}
#[test]
fn zero_weight_sum_is_rejected() {
assert!(stvd_emd_impl(
&[0.0],
&[0.0],
&[0.0],
&[0.0],
&[0.0],
&[0.0],
&[0.0],
&[1.0],
10.0,
1440.0,
SinkhornConfig::default(),
)
.is_err());
}
#[test]
fn invalid_weights_and_coordinates_are_rejected() {
assert!(stvd_emd_impl(
&[f64::NAN],
&[0.0],
&[0.0],
&[1.0],
&[0.0],
&[0.0],
&[0.0],
&[1.0],
10.0,
1440.0,
SinkhornConfig::default(),
)
.is_err());
assert!(stvd_emd_impl(
&[0.0],
&[0.0],
&[0.0],
&[-1.0],
&[0.0],
&[0.0],
&[0.0],
&[1.0],
10.0,
1440.0,
SinkhornConfig::default(),
)
.is_err());
}
#[test]
fn invalid_sinkhorn_reg_is_rejected() {
assert!(stvd_emd_impl(
&[0.0],
&[0.0],
&[0.0],
&[1.0],
&[0.0],
&[0.0],
&[0.0],
&[1.0],
10.0,
1440.0,
SinkhornConfig { reg: 0.0, ..SinkhornConfig::default() },
)
.is_err());
assert!(stvd_emd_impl(
&[0.0],
&[0.0],
&[0.0],
&[1.0],
&[0.0],
&[0.0],
&[0.0],
&[1.0],
10.0,
1440.0,
SinkhornConfig { max_iter: 0, ..SinkhornConfig::default() },
)
.is_err());
assert!(stvd_emd_impl(
&[0.0],
&[0.0],
&[0.0],
&[1.0],
&[0.0],
&[0.0],
&[0.0],
&[1.0],
10.0,
1440.0,
SinkhornConfig { tol: Some(-1.0), ..SinkhornConfig::default() },
)
.is_err());
}
#[test]
fn custom_sinkhorn_config_with_tolerance_converges() {
let value = stvd_emd_impl(
&[10.0],
&[20.0],
&[480.0],
&[1.0],
&[10.0],
&[20.0],
&[480.0],
&[1.0],
10.0,
1440.0,
SinkhornConfig { reg: 0.1, max_iter: 500, tol: Some(1e-6) },
)
.unwrap();
assert!(value.abs() < 1e-3, "got {value}");
}
}