2023-12-17 19:45:57 -06:00
|
|
|
v@dir(u, dir) {
|
|
|
|
v.dir := dir
|
2023-12-17 21:23:54 -06:00
|
|
|
tmp := 32
|
|
|
|
bt dir, 4
|
2023-12-17 19:45:57 -06:00
|
|
|
if CF
|
2023-12-17 21:23:54 -06:00
|
|
|
tmp := LINE_LEN*32
|
|
|
|
bt dir, 3
|
2023-12-17 19:45:57 -06:00
|
|
|
if CF
|
|
|
|
tmp := -tmp
|
2023-12-17 21:23:54 -06:00
|
|
|
|
|
|
|
if dir & 0b0_111 = 0
|
|
|
|
v := u + tmp
|
|
|
|
CheckBounds(v)
|
|
|
|
Graph.Edges(u, v) := cost(v)
|
|
|
|
v := v + tmp
|
|
|
|
CheckBounds(v)
|
|
|
|
Graph.Edges(u, v) += cost(v)
|
|
|
|
v := v + tmp
|
|
|
|
CheckBounds(v)
|
|
|
|
Graph.Edges(u, v) += cost(v)
|
|
|
|
v := v + tmp
|
|
|
|
CheckBounds(v)
|
|
|
|
Graph.Edges(u, v) += cost(v)
|
|
|
|
else
|
|
|
|
v := u + tmp
|
|
|
|
CheckBounds(v)
|
|
|
|
Graph.Edges(u, v) := cost(v)
|
2023-12-17 19:45:57 -06:00
|
|
|
}
|
|
|
|
|
|
|
|
neighbors(u@dir) {
|
|
|
|
; can always turn
|
2023-12-17 21:23:54 -06:00
|
|
|
neighbors += v@dir(u, ~u.dir & 0b10_000)
|
|
|
|
neighbors += v@dir(u, (~u.dir & 0b10_000) | 0b01_000)
|
|
|
|
; can go forward if consec < 6
|
|
|
|
if u.dir & 0b00_111 < 6
|
2023-12-17 19:45:57 -06:00
|
|
|
neighbors += v@dir(u, u.dir + 1)
|
|
|
|
}
|
|
|
|
|
2023-12-17 21:23:54 -06:00
|
|
|
; 28 possibilities, use 32 so we can do bit hacks
|
|
|
|
; 00000 00001 00010 00011 00100 00101 00110 00111
|
|
|
|
; 0 1 2 3 4 5 6 7
|
|
|
|
dir = { R4, R5, R6, R7, R8, R9, RA, XX,
|
|
|
|
; 01000 01001 01010 01011 01100 01101 01110 01111
|
|
|
|
; 8 9 10 11 12 13 14 15
|
|
|
|
L4, L5, L6, L7, L8, L9, LA, XX,
|
|
|
|
; 10000 10001 10010 10011 10100 10101 10110 10111
|
|
|
|
; 16 17 18 19 20 21 22 23
|
|
|
|
D4, D5, D6, D7, D8, D9, DA, XX,
|
|
|
|
; 11000 11001 11010 11011 11100 11101 11110 11111
|
|
|
|
; 24 25 26 27 28 29 30 31
|
|
|
|
U4, U5, U6, U7, U8, U9, UA, XX}
|
2023-12-17 19:45:57 -06:00
|
|
|
|
|
|
|
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
|