code wiki / _hdl_build / nx_geo_distance.nx
nx_geo_distance.nx source
↩ module page · 32 lines · 1963 B
1// nx_geo_distance.nx -- LIB: GEO-003 geodesic DISTANCE in meters (equirectangular). Hardware-up:
2// REUSES the sovereign CORDIC cos (fx.nx, Q16.16 fixed-point -- no reinvention) for the mean-latitude
3// longitude-compression, REUSES nx_isqrt for the magnitude, and keeps the lat/lon deltas as INTEGER
4// microdegrees (a single microdegree in radians would underflow Q16.16, so we do NOT convert the
5// deltas -- only the mean latitude, a full angle, goes through cos). Pure integer/fixed-point ->
6// deterministic distance (the exceed angle vs float distance libs).
7//
8// Accuracy: equirectangular is exact-direction and ~exact for local/regional spans; error grows to
9// ~0.5% only at continental scale (haversine is the precision refinement, GEO-003b). Good for
10// nearest-queries, true-meters radius fences, and routing edge weights. license_tier: ORIGINAL
11import "fx.nx"
12import "nx_isqrt.nx"
13import "nx_syscalls.nx"
14const GEOD_MAGIC_360000000: i64 = 360000000
15
16// meters per microdegree of latitude = (2*pi*R/360/1e6); R=6371000 -> 0.111195 m. Stored as a ratio.
17const GEOD_M_NUM: i64 = 111195
18const GEOD_M_DEN: i64 = 1000000
19
20// distance in METERS between two microdegree points (lat/lon in microdegrees, deg*1e6).
21func geo_distance_m(lat1: i64, lon1: i64, lat2: i64, lon2: i64) -> i64 {
22 let phim_micro: i64 = (lat1 + lat2) / 2
23 // microdegrees -> Q16.16 radians: 360e6 microdeg == 2*pi == FX_TWO_PI. Multiply before divide.
24 let phim_q16: i64 = phim_micro * FX_TWO_PI / GEOD_MAGIC_360000000
25 let cosphi: i64 = fx_cos(phim_q16) // Q16.16 cosine of the mean latitude
26 let dlat: i64 = lat2 - lat1
27 let dlon: i64 = lon2 - lon1
28 let dlon_s: i64 = dlon * cosphi / FX_ONE // compress longitude by cos(mean lat)
29 let d2: i64 = dlat * dlat + dlon_s * dlon_s
30 let d_micro: i64 = nx_isqrt(d2) // distance in latitude-microdegree-equivalents
31 return d_micro * GEOD_M_NUM / GEOD_M_DEN // -> meters
32}