nx_geo_match.nx source

1// nx_geo_match.nx -- LIB: GEO-023 MAP-MATCHING (snap a noisy GPS fix to a road network). 2// 3// For each road EDGE (a segment A->B) we project the GPS point P onto the segment (closest point), 4// clamped to the endpoints, then pick the edge whose snapped point is nearest P. Projection parameter 5// t = dot(P-A, B-A) / dot(B-A, B-A) clamped to [0,1]; the snapped point is A + t*(B-A). 6// 7// THE EXCEED ANGLE: the projection + nearest-edge choice are INTEGER and deterministic -- the snapped 8// position and the chosen edge are reproducible to the bit (no float jitter that makes a GPS fix flip 9// between two near-equidistant roads). Overflow-safe at any scale: the t fraction (num,den) is 10// normalized (lockstep >>1, ratio preserved) before multiplying by the edge delta, exactly as the 11// GEO-015 clip intersection does. Sovereign + offline; the road graph is seeded data, not a service. 12// Convention: points/edges in microdegrees, [lat,lon]; edges = flat [alat,alon,blat,blon, ...]. 13// license_tier: ORIGINAL 14import "nx_syscalls.nx" 15const K_MAGIC_17179869184: i64 = 17179869184 16 17func geo_mm_abs(x: i64) -> i64 { if x < 0 { return 0 - x } return x } 18 19// snapped (closest) point of P on segment A->B, written to out2 = [lat,lon]. 20func geo_snap_to_segment(alat: i64, alon: i64, blat: i64, blon: i64, plat: i64, plon: i64, out2: *i64) -> i64 { 21 let dlat: i64 = blat - alat 22 let dlon: i64 = blon - alon 23 let den: i64 = dlat * dlat + dlon * dlon 24 if den == 0 { out2[0] = alat; out2[1] = alon; return 0 } // degenerate edge = a point 25 let num: i64 = (plat - alat) * dlat + (plon - alon) * dlon 26 if num <= 0 { out2[0] = alat; out2[1] = alon; return 0 } // projects before A -> clamp A 27 if num >= den { out2[0] = blat; out2[1] = blon; return 0 } // projects after B -> clamp B 28 // interior: A + (num/den)*(B-A); normalize the fraction so n2*delta can't overflow i64. 29 var n2: i64 = num 30 var d2: i64 = den 31 var go: i64 = 1 32 while go == 1 { 33 if geo_mm_abs(n2) > K_MAGIC_17179869184 { n2 = n2 / 2; d2 = d2 / 2 } else { 34 if geo_mm_abs(d2) > K_MAGIC_17179869184 { n2 = n2 / 2; d2 = d2 / 2 } else { go = 0 } 35 } 36 } 37 if d2 == 0 { out2[0] = alat; out2[1] = alon; return 0 } 38 out2[0] = alat + n2 * dlat / d2 39 out2[1] = alon + n2 * dlon / d2 40 return 0 41} 42 43// squared distance between two [lat,lon] points (microdeg^2). 44func geo_mm_dist2(lat1: i64, lon1: i64, lat2: i64, lon2: i64) -> i64 { 45 let dy: i64 = lat1 - lat2 46 let dx: i64 = lon1 - lon2 47 return dy * dy + dx * dx 48} 49 50// snap P to the nearest road edge; writes the snapped point to out2, returns the best edge index. 51func geo_map_match(plat: i64, plon: i64, edges: *i64, nedges: i64, out2: *i64) -> i64 { 52 let tmp: *i64 = sys_mmap(16) as *i64 53 var best: i64 = 0 - 1 54 var bestd: i64 = 0 55 var e: i64 = 0 56 while e < nedges { 57 geo_snap_to_segment(edges[e * 4], edges[e * 4 + 1], edges[e * 4 + 2], edges[e * 4 + 3], plat, plon, tmp) 58 let d: i64 = geo_mm_dist2(plat, plon, tmp[0], tmp[1]) 59 if best < 0 { best = e; bestd = d; out2[0] = tmp[0]; out2[1] = tmp[1] } else { 60 if d < bestd { best = e; bestd = d; out2[0] = tmp[0]; out2[1] = tmp[1] } 61 } 62 e = e + 1 63 } 64 return best 65}