code wiki / _hdl_build / nx_geo_query.nx
nx_geo_query.nx source
↩ module page · 87 lines · 3343 B
1// nx_geo_query.nx -- LIB: GEO-009 spatial QUERY primitives, composing the rungs below.
2// geo_nearest -- nearest candidate point to a query (the "find nearest store/place" query)
3// geo_within -- all candidates within R meters (the ad engine's "offer near the visitor")
4// geo_bearing -- compass bearing A->B in whole degrees (navigation / routing direction)
5// nearest/within REUSE GEO-003 geo_distance_m; bearing adds a vectoring-mode CORDIC atan2 that
6// REUSES fx.nx's atan table (fx_cordic_atan) -- no reinvention, fx.nx untouched. All integer/
7// fixed-point -> deterministic. license_tier: ORIGINAL
8import "nx_geo_distance.nx"
9import "fx.nx"
10import "nx_syscalls.nx"
11const K_MAGIC_360000000: i64 = 360000000
12
13// index of the nearest of npts candidate points (flat [lat,lon,...]) to (qlat,qlon); -1 if none.
14func geo_nearest(qlat: i64, qlon: i64, pts: *i64, npts: i64) -> i64 {
15 var best: i64 = 0 - 1
16 var bestd: i64 = 0
17 var i: i64 = 0
18 while i < npts {
19 let d: i64 = geo_distance_m(qlat, qlon, pts[i * 2], pts[i * 2 + 1])
20 if best < 0 { best = i; bestd = d } else { if d < bestd { best = i; bestd = d } }
21 i = i + 1
22 }
23 return best
24}
25
26// indices of all candidates within radius_m meters; writes them to out_idx, returns the count.
27func geo_within(qlat: i64, qlon: i64, radius_m: i64, pts: *i64, npts: i64, out_idx: *i64) -> i64 {
28 var c: i64 = 0
29 var i: i64 = 0
30 while i < npts {
31 let d: i64 = geo_distance_m(qlat, qlon, pts[i * 2], pts[i * 2 + 1])
32 if d <= radius_m { out_idx[c] = i; c = c + 1 }
33 i = i + 1
34 }
35 return c
36}
37
38// atan2(y, x) in Q16.16 radians via vectoring-mode CORDIC (drives y->0, accumulates the angle).
39// Reuses the fx.nx atan table. Handles all quadrants by point-reflecting when x<0.
40func geo_atan2(y: i64, x: i64) -> i64 {
41 if x == 0 {
42 if y > 0 { return FX_HALF_PI }
43 if y < 0 { return 0 - FX_HALF_PI }
44 return 0
45 }
46 var xx: i64 = x
47 var yy: i64 = y
48 var addz: i64 = 0
49 if x < 0 {
50 xx = 0 - x
51 yy = 0 - y
52 if y >= 0 { addz = FX_PI } else { addz = 0 - FX_PI }
53 }
54 var z: i64 = 0
55 var i: i64 = 0
56 while i < 16 {
57 let atan_i: i64 = fx_cordic_atan(i)
58 if yy >= 0 {
59 let xn: i64 = xx + (yy >> i)
60 yy = yy - (xx >> i)
61 xx = xn
62 z = z + atan_i
63 } else {
64 let xn: i64 = xx - (yy >> i)
65 yy = yy + (xx >> i)
66 xx = xn
67 z = z - atan_i
68 }
69 i = i + 1
70 }
71 return z + addz
72}
73
74// compass bearing from (lat1,lon1) to (lat2,lon2) in whole degrees [0,360): 0=N, 90=E, 180=S, 270=W.
75// East delta is cos-compressed by the mean latitude (reuses fx_cos), matching ground geometry.
76func geo_bearing(lat1: i64, lon1: i64, lat2: i64, lon2: i64) -> i64 {
77 let phim: i64 = (lat1 + lat2) / 2
78 let phim_q16: i64 = phim * FX_TWO_PI / K_MAGIC_360000000
79 let cosphi: i64 = fx_cos(phim_q16)
80 let dnorth: i64 = lat2 - lat1
81 let deast: i64 = (lon2 - lon1) * cosphi / FX_ONE
82 let z: i64 = geo_atan2(deast, dnorth) // clockwise angle from north, Q16.16 radians
83 // radians(Q16.16) -> degrees: deg = z * 180 / (pi*2^16) = z * 180 / FX_PI
84 var deg: i64 = z * 180 / FX_PI
85 if deg < 0 { deg = deg + 360 }
86 return deg
87}