You have a graph with 6 vertices on a grid with co-ordinates:
A ( 0,0)
B ( 4,7)
C ( 7,4)
D (10,4)
E (16,2)
F (16,0)
You can generate a complete graph on those vertices and assign a cost to each edge where the cost is MAX( ABS( x1 - x2 ), ABS( y1 - y2 ) ) for standard edges and a cost of 0 for wormholes.
This will give you the costs (as an adjacency matrix):
A B C D E F
- -- -- -- -- -- --
A - 7 7 10 16 16
B 7 - 0 6 12 12
C 7 0 - 3 9 9
D 10 6 3 - 0 6
E 16 12 9 0 - 2
F 16 12 9 6 2 -
If there are one-directional warps then only create edges in the graph (or adjacency matrix) that go in that direction but not in the opposite.
You can then use Dijkstra's algorithm with a priority queue.
Start from A and push each adjacent edge onto the priority queue:
Format: (path : cost)
queue = [ (A-B : 7), (A-C : 7), (A-D : 10), (A-E : 16), (A-F : 16) ]
As the items are pushed onto the queue - keep track of the minimum cost for each vertex and only push paths onto the queue if it is lower cost than the existing minimum cost.
min-costs = { A: 0, B: 7, C: 7, D: 10, E: 16, F: 16 }
Remove the first item from the queue and, if its cost still matches the minimum cost, push all the composite paths formed by that path and its adjacent edges back onto the priority queue (if the composite paths have lower cost than the existing minimum):
Remove: (A-B : 7)
- Try
(A-B-A : 14)- reject as higher cost - Try
(A-B-C : 7)- reject as same cost - Try
(A-B-D : 13)- reject as higher cost - Try
(A-B-E : 19)- reject as higher cost - Try
(A-B-F : 19)- reject as higher cost
Remove (A-C : 7)
- Try
(A-C-A : 14)- reject as higher cost - Try
(A-C-B : 7)- reject as same cost - Try
(A-C-D : 10)- reject as same cost - Try
(A-C-E : 16)- reject as same cost - Try
(A-C-F : 16)- reject as same cost
Remove (A-D : 7)
- Try
(A-D-A : 20)- reject as higher cost - Try
(A-D-B : 16)- reject as higher cost - Try
(A-D-C : 13)- reject as higher cost - Try
(A-D-E : 10)- insert into queue - Try
(A-D-F : 16)- reject as same cost
Now the queue will look like:
queue = [ (A-D-E : 10), (A-E : 16), (A-F : 16) ]
min-costs = { A: 0, B: 7, C: 7, D: 10, E: 10, F: 16 }
Remove (A-D-E : 10)
- Try
(A-D-E-A : 26)- reject as higher cost - Try
(A-D-E-B : 22)- reject as higher cost - Try
(A-D-E-C : 19)- reject as higher cost - Try
(A-D-E-D : 10)- reject as same cost - Try
(A-D-E-F : 12)- insert into queue
Then the queue is:
queue = [ (A-D-E-F : 12), (A-E : 16), (A-F : 16) ]
min-costs = { A: 0, B: 7, C: 7, D: 10, E: 10, F: 12 }
Remove (A-D-E-F : 12), find that you have got to the destination node in a cost of 12.
Note: the paths (A-B-C-D-E-F), (A-C-D-E-F) and (A-D-E-F) all have the same minimum cost of 12.