57 lines
1.3 KiB
Plaintext
57 lines
1.3 KiB
Plaintext
|
Graph.Edges(u@dir, v@dir) {
|
||
|
|
; if cost(v) = 0xff, we're off graph,
|
||
|
|
; in the padded area
|
||
|
|
Graph.Edges := cost(v)
|
||
|
|
}
|
||
|
|
|
||
|
|
v@dir(u, dir) {
|
||
|
|
v.dir := dir
|
||
|
|
tmp := 1
|
||
|
|
bt dir, 3
|
||
|
|
if CF
|
||
|
|
tmp := LINE_LEN
|
||
|
|
bt dir, 2
|
||
|
|
if CF
|
||
|
|
tmp := -tmp
|
||
|
|
v := u + tmp
|
||
|
|
}
|
||
|
|
|
||
|
|
neighbors(u@dir) {
|
||
|
|
; can always turn
|
||
|
|
neighbors += v@dir(u, ~u.dir & 0b1000)
|
||
|
|
neighbors += v@dir(u, (~u.dir & 0b1000) | 0b0100)
|
||
|
|
; can go forward if consec < 3
|
||
|
|
if u.dir & 0b0011 < 3
|
||
|
|
neighbors += v@dir(u, u.dir + 1)
|
||
|
|
}
|
||
|
|
|
||
|
|
; 12 possibilities, use 16 so we can do bit hacks
|
||
|
|
; 0000 0001 0010 0011 0100 0101 0110 0111
|
||
|
|
; 0 1 2 3 4 5 6 7
|
||
|
|
dir = { R1, R2, R3, XX, L1, L2, L3, XX,
|
||
|
|
; 1000 1001 1010 1011 1100 1101 1110 1111
|
||
|
|
; 8 9 10 11 12 13 14 15
|
||
|
|
D1, D2, D3, XX, U1, U2, U3, XX}
|
||
|
|
|
||
|
|
dist[(x,y),dir]
|
||
|
|
prev[(x,y),dir]
|
||
|
|
q[(x,y),dir]
|
||
|
|
|
||
|
|
for each vertex v in Graph.Vertices:
|
||
|
|
dist[v] ← INFINITY
|
||
|
|
prev[v] ← UNDEFINED
|
||
|
|
add v to Q
|
||
|
|
|
||
|
|
for all real dir:
|
||
|
|
dist[source@dir] ← 0
|
||
|
|
|
||
|
|
while Q is not empty and any dist[u] in Q < inf:
|
||
|
|
u ← vertex in Q with min dist[u]
|
||
|
|
remove u from Q
|
||
|
|
for each neighbor v of u still in Q:
|
||
|
|
alt ← dist[u] + Graph.Edges(u, v)
|
||
|
|
if alt < dist[v]:
|
||
|
|
dist[v] ← alt
|
||
|
|
prev[v] ← u
|
||
|
|
done
|