use h3o::{CellIndex, LatLng, Resolution};
use rayon::prelude::*;
use rustc_hash::FxHashMap;
use crate::utils::haversine::haversine_km;
use super::stvd_emd::pair_cost_m;
#[derive(Clone, Copy, Debug)]
pub struct CandidateGraphConfig {
pub resolution: Resolution,
pub k_ring: u32,
pub k_ring_max: u32,
pub k_min: usize,
pub max_candidates: usize,
}
impl Default for CandidateGraphConfig {
fn default() -> Self {
Self { resolution: Resolution::Eight, k_ring: 2, k_ring_max: 32, k_min: 8, max_candidates: 64 }
}
}
pub struct CandidateGraph {
pub offsets_a: Vec<u32>,
pub edges_a: Vec<(u32, f32)>,
pub offsets_b: Vec<u32>,
pub edges_b: Vec<(u32, f32)>,
}
impl CandidateGraph {
pub fn edge_count(&self) -> usize {
self.edges_a.len()
}
}
fn bucket_by_cell(lats: &[f64], lngs: &[f64], resolution: Resolution) -> FxHashMap<u64, Vec<u32>> {
let mut buckets: FxHashMap<u64, Vec<u32>> = FxHashMap::default();
for (idx, (&lat, &lng)) in lats.iter().zip(lngs).enumerate() {
if let Ok(ll) = LatLng::new(lat, lng) {
let cell = u64::from(ll.to_cell(resolution));
buckets.entry(cell).or_default().push(idx as u32);
}
}
buckets
}
fn cell_center_distance_km(lat: f64, lng: f64, cell: u64) -> f64 {
match CellIndex::try_from(cell) {
Ok(cell) => {
let center = LatLng::from(cell);
haversine_km(lat, lng, center.lat(), center.lng())
}
Err(_) => f64::INFINITY,
}
}
#[allow(clippy::too_many_arguments)]
fn nearby_candidates(
home_lat: f64,
home_lng: f64,
home_t: f64,
other_lats: &[f64],
other_lngs: &[f64],
other_ts: &[f64],
other_buckets: &FxHashMap<u64, Vec<u32>>,
other_distinct_cells: &[u64],
cyclical_period: f64,
r: f64,
config: CandidateGraphConfig,
) -> Vec<(u32, f32)> {
let mut found = Vec::new();
let cost_to = |other_idx: u32| {
let j = other_idx as usize;
pair_cost_m(
home_lat,
home_lng,
home_t,
other_lats[j],
other_lngs[j],
other_ts[j],
cyclical_period,
r,
)
};
if let Ok(home_cell) = LatLng::new(home_lat, home_lng).map(|ll| ll.to_cell(config.resolution)) {
let mut k = config.k_ring.max(1);
loop {
found.clear();
let ring: Vec<(CellIndex, u32)> = home_cell.grid_disk_distances(k);
for (neighbor_cell, _distance) in ring {
if let Some(indices) = other_buckets.get(&u64::from(neighbor_cell)) {
found.extend(indices.iter().map(|&idx| (idx, cost_to(idx))));
}
}
if found.len() >= config.k_min || k >= config.k_ring_max {
break;
}
k = (k * 2).min(config.k_ring_max);
}
if found.len() > config.max_candidates {
found.sort_unstable_by(|a, b| a.1.partial_cmp(&b.1).expect("costs are always finite"));
found.truncate(config.max_candidates);
}
}
if found.is_empty()
&& let Some(&nearest_cell) = other_distinct_cells.iter().min_by(|&&a, &&b| {
cell_center_distance_km(home_lat, home_lng, a)
.partial_cmp(&cell_center_distance_km(home_lat, home_lng, b))
.expect("distances are always finite or +inf, never NaN")
})
&& let Some(indices) = other_buckets.get(&nearest_cell)
{
found.extend(indices.iter().map(|&idx| (idx, cost_to(idx))));
if found.len() > config.max_candidates {
found.sort_unstable_by(|a, b| a.1.partial_cmp(&b.1).expect("costs are always finite"));
found.truncate(config.max_candidates);
}
}
found
}
fn build_csr(sorted_edges: &[(u32, u32, f32)], group_count: usize, group_of: impl Fn(&(u32, u32, f32)) -> u32, other_of: impl Fn(&(u32, u32, f32)) -> u32) -> (Vec<u32>, Vec<(u32, f32)>) {
let mut offsets = vec![0u32; group_count + 1];
for edge in sorted_edges {
offsets[group_of(edge) as usize + 1] += 1;
}
for i in 0..group_count {
offsets[i + 1] += offsets[i];
}
let values: Vec<(u32, f32)> = sorted_edges.iter().map(|edge| (other_of(edge), edge.2)).collect();
(offsets, values)
}
#[allow(clippy::too_many_arguments)]
pub fn build_candidate_graph(
lats_a: &[f64],
lngs_a: &[f64],
ts_a: &[f64],
lats_b: &[f64],
lngs_b: &[f64],
ts_b: &[f64],
cyclical_period: f64,
r: f64,
config: CandidateGraphConfig,
) -> CandidateGraph {
let n = lats_a.len();
let m = lats_b.len();
let buckets_a = bucket_by_cell(lats_a, lngs_a, config.resolution);
let buckets_b = bucket_by_cell(lats_b, lngs_b, config.resolution);
let distinct_cells_a: Vec<u64> = buckets_a.keys().copied().collect();
let distinct_cells_b: Vec<u64> = buckets_b.keys().copied().collect();
let edges_from_a: Vec<(u32, u32, f32)> = (0..n)
.into_par_iter()
.flat_map_iter(|i| {
nearby_candidates(
lats_a[i], lngs_a[i], ts_a[i], lats_b, lngs_b, ts_b, &buckets_b, &distinct_cells_b,
cyclical_period, r, config,
)
.into_iter()
.map(move |(j, cost)| (i as u32, j, cost))
})
.collect();
let edges_from_b: Vec<(u32, u32, f32)> = (0..m)
.into_par_iter()
.flat_map_iter(|j| {
nearby_candidates(
lats_b[j], lngs_b[j], ts_b[j], lats_a, lngs_a, ts_a, &buckets_a, &distinct_cells_a,
cyclical_period, r, config,
)
.into_iter()
.map(move |(i, cost)| (i, j as u32, cost))
})
.collect();
let mut all_edges = edges_from_a;
all_edges.extend(edges_from_b);
all_edges.par_sort_unstable_by_key(|&(i, j, _)| (i, j));
all_edges.dedup_by_key(|&mut (i, j, _)| (i, j));
let (offsets_a, edges_a) = build_csr(&all_edges, n, |&(i, _, _)| i, |&(_, j, _)| j);
let mut by_b = all_edges.clone();
by_b.par_sort_unstable_by_key(|&(i, j, _)| (j, i));
let (offsets_b, edges_b) = build_csr(&by_b, m, |&(_, j, _)| j, |&(i, _, _)| i);
CandidateGraph { offsets_a, edges_a, offsets_b, edges_b }
}
#[cfg(test)]
mod tests {
use super::*;
fn default_config() -> CandidateGraphConfig {
CandidateGraphConfig::default()
}
#[test]
fn every_point_has_at_least_one_candidate() {
let lats_a = [37.7749, 37.7750, -33.8688];
let lngs_a = [-122.4194, -122.4195, 151.2093];
let ts_a = [480.0, 480.0, 480.0];
let lats_b = [37.7751, 51.5074];
let lngs_b = [-122.4196, -0.1278];
let ts_b = [480.0, 480.0];
let graph = build_candidate_graph(
&lats_a, &lngs_a, &ts_a, &lats_b, &lngs_b, &ts_b, 1440.0, 10.0, default_config(),
);
for i in 0..lats_a.len() {
let start = graph.offsets_a[i] as usize;
let end = graph.offsets_a[i + 1] as usize;
assert!(end > start, "point {i} of A has no candidates");
}
for j in 0..lats_b.len() {
let start = graph.offsets_b[j] as usize;
let end = graph.offsets_b[j + 1] as usize;
assert!(end > start, "point {j} of B has no candidates");
}
}
#[test]
fn no_duplicate_edges_after_bidirectional_union() {
let lats_a = [0.0, 0.0];
let lngs_a = [0.0, 0.001];
let ts_a = [480.0, 480.0];
let lats_b = [0.0, 0.0];
let lngs_b = [0.0005, 0.0015];
let ts_b = [480.0, 480.0];
let graph = build_candidate_graph(
&lats_a, &lngs_a, &ts_a, &lats_b, &lngs_b, &ts_b, 1440.0, 10.0, default_config(),
);
let mut seen = rustc_hash::FxHashSet::default();
for i in 0..lats_a.len() {
let start = graph.offsets_a[i] as usize;
let end = graph.offsets_a[i + 1] as usize;
for &(j, _) in &graph.edges_a[start..end] {
assert!(seen.insert((i as u32, j)), "duplicate edge ({i}, {j})");
}
}
}
#[test]
fn by_a_and_by_b_views_agree_on_the_same_edge_set() {
let lats_a = [0.0, 0.0, 10.0];
let lngs_a = [0.0, 0.001, 10.0];
let ts_a = [480.0, 500.0, 100.0];
let lats_b = [0.0, 0.0];
let lngs_b = [0.0005, 0.0015];
let ts_b = [480.0, 500.0];
let graph = build_candidate_graph(
&lats_a, &lngs_a, &ts_a, &lats_b, &lngs_b, &ts_b, 1440.0, 10.0, default_config(),
);
let mut from_a: Vec<(u32, u32)> = Vec::new();
for i in 0..lats_a.len() {
let start = graph.offsets_a[i] as usize;
let end = graph.offsets_a[i + 1] as usize;
for &(j, _) in &graph.edges_a[start..end] {
from_a.push((i as u32, j));
}
}
let mut from_b: Vec<(u32, u32)> = Vec::new();
for j in 0..lats_b.len() {
let start = graph.offsets_b[j] as usize;
let end = graph.offsets_b[j + 1] as usize;
for &(i, _) in &graph.edges_b[start..end] {
from_b.push((i, j as u32));
}
}
from_a.sort_unstable();
from_b.sort_unstable();
assert_eq!(from_a, from_b);
}
#[test]
fn candidate_costs_match_direct_pair_cost() {
let lats_a = [0.0];
let lngs_a = [0.0];
let ts_a = [480.0];
let lats_b = [0.0];
let lngs_b = [0.001];
let ts_b = [500.0];
let graph = build_candidate_graph(
&lats_a, &lngs_a, &ts_a, &lats_b, &lngs_b, &ts_b, 1440.0, 10.0, default_config(),
);
let expected = pair_cost_m(0.0, 0.0, 480.0, 0.0, 0.001, 500.0, 1440.0, 10.0);
assert_eq!(graph.edges_a[0].1, expected);
}
#[test]
fn empty_ring_search_falls_back_across_antipodal_points() {
let lats_a = [0.0];
let lngs_a = [0.0];
let ts_a = [0.0];
let lats_b = [0.0];
let lngs_b = [179.9];
let ts_b = [0.0];
let graph = build_candidate_graph(
&lats_a, &lngs_a, &ts_a, &lats_b, &lngs_b, &ts_b, 1440.0, 10.0, default_config(),
);
assert_eq!(graph.edge_count(), 1);
assert!(graph.edges_a[0].1.is_finite());
}
}