| 12345678910111213141516171819202122232425262728293031323334353637383940414243444546474849505152535455565758 |
- from __future__ import annotations
- import math
- from typing import Iterable, Optional, Tuple
- def haversine_m(lat1: float, lon1: float, lat2: float, lon2: float) -> float:
- """Great-circle distance in meters."""
- r = 6371000.0
- phi1 = math.radians(lat1)
- phi2 = math.radians(lat2)
- d_phi = math.radians(lat2 - lat1)
- d_lam = math.radians(lon2 - lon1)
- a = (
- math.sin(d_phi / 2.0) ** 2
- + math.cos(phi1) * math.cos(phi2) * math.sin(d_lam / 2.0) ** 2
- )
- c = 2.0 * math.atan2(math.sqrt(a), math.sqrt(1.0 - a))
- return r * c
- def clamp(n: int, lo: int, hi: int) -> int:
- return max(lo, min(hi, n))
- def nearest_point_index(
- points_latlon: Iterable[Tuple[float, float]],
- lat: float,
- lon: float,
- *,
- start_idx: Optional[int] = None,
- window: int = 80,
- ) -> Tuple[int, float]:
- """Return (idx, distance_m) of the nearest point.
- If start_idx is given, search a window around it for speed.
- """
- pts = list(points_latlon)
- if not pts:
- raise ValueError("empty points")
- if start_idx is None:
- search_lo = 0
- search_hi = len(pts) - 1
- else:
- search_lo = clamp(start_idx - window, 0, len(pts) - 1)
- search_hi = clamp(start_idx + window, 0, len(pts) - 1)
- best_i = search_lo
- best_d = float("inf")
- for i in range(search_lo, search_hi + 1):
- p_lat, p_lon = pts[i]
- d = haversine_m(lat, lon, p_lat, p_lon)
- if d < best_d:
- best_d = d
- best_i = i
- return best_i, best_d
|
