code wiki / _hdl_build / nx_pathfind.nx
nx_pathfind.nx source
↩ module page · 76 lines · 3483 B
1// nx_pathfind.nx -- A* grid pathfinding, pure-Nishi NO-FLOAT (integer g/h/f, Manhattan heuristic). Closes the
2// RESEARCHED->PRESENT gap the sovereign research census (GR-S-R5) identified for P5/char-AI navigation: the
3// corpus attests pathfinding/A* (gr_e_astar/gr_e_pathfinding/gr_e_gameai) but Nishi hadn't built it. 4-direction
4// grid, walls, optimal shortest path + parent reconstruction. Open set = linear min-f scan (fine for game grids;
5// no float, no heap-float). Deterministic. Reusable by RTS/tower-defense/roguelike/char-AI. license_tier: ORIGINAL
6import "nx_syscalls.nx"
7
8// caller provides a flat grid (w*h, 0=open 1=wall) + scratch arrays sized w*h each: g, f, came, open_flag, closed.
9// returns the path length (steps) from (sx,sy) to (tx,ty), or -1 if no path. fills `path` (caller, len*1 cells of
10// packed y*w+x) when reachable and path!=0. Manhattan heuristic (admissible on a 4-connected grid => optimal).
11const PF_INF: i64 = 1000000000
12
13func pf_abs(v: i64) -> i64 { if v < 0 { return 0 - v } return v }
14func pf_h(ax: i64, ay: i64, bx: i64, by: i64) -> i64 { return pf_abs(ax-bx) + pf_abs(ay-by) }
15
16func pf_astar(grid: *i64, w: i64, h: i64, sx: i64, sy: i64, tx: i64, ty: i64,
17 g: *i64, f: *i64, came: *i64, openf: *i64, closed: *i64, path: *i64) -> i64 {
18 let n: i64 = w*h
19 var i: i64 = 0
20 while i < n { g[i]=PF_INF; f[i]=PF_INF; came[i]=0-1; openf[i]=0; closed[i]=0; i=i+1 }
21 let s: i64 = sy*w + sx
22 let t: i64 = ty*w + tx
23 if grid[s]!=0 { return 0-1 }
24 if grid[t]!=0 { return 0-1 }
25 g[s]=0; f[s]=pf_h(sx,sy,tx,ty); openf[s]=1
26 var found: i64 = 0
27 var guard: i64 = 0
28 while guard < n*4 {
29 guard = guard + 1
30 // pick the open node with the lowest f (linear scan; integer compare, no float)
31 var cur: i64 = 0-1
32 var bestf: i64 = PF_INF
33 var j: i64 = 0
34 while j < n { if openf[j]==1 { if f[j] < bestf { bestf=f[j]; cur=j } } j=j+1 }
35 if cur < 0 { guard = n*4 } // open set empty -> no path
36 else {
37 if cur == t { found=1; guard = n*4 }
38 else {
39 openf[cur]=0; closed[cur]=1
40 let cx: i64 = cur % w
41 let cy: i64 = cur / w
42 var d: i64 = 0
43 while d < 4 {
44 var nx: i64 = cx
45 var ny: i64 = cy
46 if d==0 { nx=cx+1 }
47 if d==1 { nx=cx-1 }
48 if d==2 { ny=cy+1 }
49 if d==3 { ny=cy-1 }
50 if nx>=0 { if nx<w { if ny>=0 { if ny<h {
51 let nb: i64 = ny*w + nx
52 if grid[nb]==0 { if closed[nb]==0 {
53 let tg: i64 = g[cur] + 1
54 if tg < g[nb] {
55 came[nb]=cur; g[nb]=tg; f[nb]=tg + pf_h(nx,ny,tx,ty); openf[nb]=1
56 }
57 } }
58 } } } }
59 d = d + 1
60 }
61 }
62 }
63 }
64 if found == 0 { return 0-1 }
65 // reconstruct length (and path if requested), walking parents from t back to s
66 var len: i64 = 0
67 var node: i64 = t
68 while node != s { len = len + 1; node = came[node]; if node < 0 { return 0-1 } }
69 if (path as i64) != 0 {
70 var node2: i64 = t
71 var k: i64 = len
72 while node2 != s { path[k]=node2; k=k-1; node2=came[node2] }
73 path[0]=s
74 }
75 return len
76}