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