## Wormhole Pathfinding (Starflight GPS)

This is a special case of node based (not cell based) pathfinding with weighted edges and without obstacles.

I was playing around with a setting like Starflight (Binary Systems, 1986). We have a 2D starmap with wormhole tunnels.

starflightmap.jpg

In terms of a graph each wormhole represents a node and each node is connected with all other nodes. These connections are the edges. Travelling through a wormhole tunnel is very cheap and costs almost no fuel. Normal travelling costs fuel proportionally to distance. These costs are the edges' weights.

You're treasure hunting across the galaxy, but planning your route on the map can be tedious. You're scrolling around in order to figure out which combination of wormhole tunnels would bring you to your destination at a minimal expense.

So I built a navigation system for wormhole tunnels. It is based on code from the book "Algorithms" from Robert Sedgewick (1991) which is an implementation of Dijkstra's algorithm (shortest path problem).

Code:
```program wormpath;

uses
crt;

var
maxx, maxy: longint; // regarding crt

type
pttyp = packed record
x, y: longint;
end;

type
wormholetyp = packed record
position: pttyp;
target: longint;
end;

const
wormholemaxn = 50;

var
wormholes: array[0..wormholemaxn-1] of wormholetyp;

const
{ max of entries in cost matrix: all wormholes + start + destination }
nodemaxn = wormholemaxn+2;

var
{ cost matrix }
costmat: array[0..nodemaxn-1, 0..nodemaxn-1] of longint;
{ priority queue; all nodes +1 }
cost, pred: array[0..nodemaxn] of longint;

var
{ route }
start, dest: pttyp;

const
{ last 2 entries in cost matrix belong to start/destination }
startindex = wormholemaxn+1;
destindex = wormholemaxn;

{ routines for drawing a line in crt with clipping }

procedure draw_hline(x1, x2, y: longint; letter: char);
var
i, temp: longint;
begin
if y<1 then exit;
if y>maxy then exit;

if x1>x2 then begin
temp := x1;
x1 := x2;
x2 := temp;
end;

if x1<1 then x1 := 1;
if x2>maxx then x2 := maxy;
if x1>x2 then exit;

gotoxy(x1*2-1, y);
for i := x1 to x2 do write(letter, ' ');
end;

procedure draw_vline(x, y1, y2: longint; letter: char);
var
i, temp: longint;
begin
if x<1 then exit;
if x>maxx then exit;

if y1>y2 then begin
temp := y1;
y1 := y2;
y2 := temp;
end;

if y1<1 then y1 := 1;
if y2>maxy then y2 := maxy;
if y1>y2 then exit;

for i := y1 to y2 do begin
gotoxy(x*2-1, i);
write(letter);
end;
end;

procedure draw_line(x1, y1, x2, y2: longint; letter: char; color: byte);
var
dx, dy, rx, ry: longint;
error, c: longint;
step, rest: longint;
begin

textcolor(color);

if y1=y2 then begin
draw_hline(x1, x2, y1, letter);
exit;
end;
if x1=x2 then begin
draw_vline(x1, y1, y2, letter);
exit;
end;

dy := y2-y1;
if dy>=0 then ry := 1
else begin
dy := -dy;
ry := -1
end;
inc(dy);

dx := x2-x1;
if dx>=0 then rx := 1
else begin
dx := -dx;
rx := -1
end;
inc(dx);

{ dy/dx <= 1 }
if dx>=dy then begin

step := dx div dy;
rest := dx-(dy*step);
step := step*rx;

x2 := x1;

error := dy shr 1;
for c := 1 to dy do begin

dec(error, rest);
if error<0 then begin
inc(error, dy);
inc(x2, rx);
end;

inc(x2, step);
draw_hline(x1, x2-rx, y1, letter);
x1 := x2;

inc(y1, ry);
end;

end
{ dy/dx > 1 }
else begin

step := dy div dx;
rest := dy-(dx*step);
step := step*ry;

y2 := y1;

error := dx shr 1;
for c := 1 to dx do begin

dec(error, rest);
if error<0 then begin
inc(error, dx);
inc(y2, ry);
end;

inc(y2, step);
draw_vline(x1, y1, y2-ry, letter);
y1 := y2;

inc(x1, rx);
end;

end;
end;

{ pathfinding }

procedure connect_wormholes;
{ every wormhole calculates distances to all other wormholes
and puts results in cost matrix }
var
i, j: longint;
xa, ya, xb, yb: longint;
dx, dy, dist: longint;
targeta: longint;
begin
for i := 0 to wormholemaxn-1 do begin
{ wormhole A }
xa := wormholes[i].position.x;
ya := wormholes[i].position.y;
targeta := wormholes[i].target;

for j := 0 to wormholemaxn-1 do begin
{ the very same point: distance = 0 }
if j=i then dist := 0
else begin
{ wormhole's mate: distance = 1 }
if j=targeta then dist := 1
else begin
{ wormhole B }
xb := wormholes[j].position.x;
yb := wormholes[j].position.y;
{ distance wormhole A -> wormhole B }
dx := xb-xa;
dy := yb-ya;
dist := dx*dx+dy*dy;
dist := round(sqrt(dist));
end;
end;

costmat[i, j] := dist;
end;

end;
end;

procedure enter_route(startx, starty, destx, desty: longint);
{ enter start and destination into cost matrix }
var
j: longint;
xb, yb: longint;
dx, dy, dist: longint;
begin

{ save route }
start.x := startx;
start.y := starty;
dest.x := destx;
dest.y := desty;

dx := destx-startx;
dy := desty-starty;
dist := dx*dx+dy*dy;
dist := round(sqrt(dist));

costmat[startindex, destindex] := dist;
costmat[destindex, startindex] := dist;

{ distances for start/destination to all wormholes }
for j := 0 to wormholemaxn-1 do begin
xb := wormholes[j].position.x;
yb := wormholes[j].position.y;

{ start -> wormhole }
dx := xb-startx;
dy := yb-starty;
dist := dx*dx+dy*dy;
dist := round(sqrt(dist));
costmat[startindex, j] := dist;
costmat[j, startindex] := dist;

{ dest -> wormhole }
dx := xb-destx;
dy := yb-desty;
dist := dx*dx+dy*dy;
dist := round(sqrt(dist));
costmat[destindex, j] := dist;
costmat[j, destindex] := dist;
end;
end;

procedure find_path;
{ the core algorithm, adapted from R. Sedgewick: Algorithms (Dijkstra variant)

cost[]
negative entry : node yet in queue
positive entry : node in spanning tree already

pred[]
predecessor on path to start

unseen
used as mark

shortest path: from destindex repeatedly backwards via pred[] to startindex
costs for shortest path: abs(cost[destindex])
costs for direct route: costmat[startindex, destindex] }
const
unseen = maxlongint-10;
var
i, current, nextbest, next: longint;
newcost: longint;
begin

for i := 0 to nodemaxn-1 do begin
cost[i] := -unseen;
pred[i] := -1;
end;
cost[destindex] := -(unseen+1);
nextbest := startindex;

repeat
current := nextbest;
cost[current] := -cost[current];
nextbest := destindex;
if cost[current]=unseen then cost[current] := 0;

{ all nodes except start (last entry) }
for next := 0 to nodemaxn-1-1 do begin

{ next is still in queue? }
if cost[next]<0 then begin

{ current and next aren't identical }
if costmat[current, next]<>0 then begin

newcost := cost[current]+costmat[current, next];

if cost[next]<-newcost then begin
cost[next] := -newcost;
pred[next] := current;
end;
end;

if cost[next]>cost[nextbest] then nextbest := next;
end;

end;

until nextbest=destindex;
end;

procedure draw_path;
var
i: longint;
p1, p2: pttyp;
begin
i := destindex;
repeat
{ wormholes and destination (but not start) }
if i<=wormholemaxn then begin

if i<wormholemaxn
{ wormhole }
then p1 := wormholes[i].position
{ destination }
else p1 := dest;

{ predecessor is wormhole }
if pred[i]<wormholemaxn then begin
p2 := wormholes[pred[i]].position;

if wormholes[pred[i]].target=i
then draw_line(p1.x, p1.y, p2.x, p2.y, 'o', lightgreen)
else draw_line(p1.x, p1.y, p2.x, p2.y, 'o', lightred);
end
{ predecessor is start }
else begin
p2 := start;
draw_line(p1.x, p1.y, p2.x, p2.y, 'o', lightred);
end;

end;

i := pred[i];
until i=-1;
end;

procedure generate_wormholes;
var
i: longint;
begin
for i := 0 to wormholemaxn-1 do begin
with wormholes[i] do begin
position.x := 3+random(maxx-4);
position.y := 1+random(maxy-1);

if odd(i) then target := i-1 else target := i+1;
end;
end;
end;

procedure draw_wormholes;
var
i: longint;
p1, p2: pttyp;
begin
for i := 0 to wormholemaxn-1 do begin
if odd(i) then continue;

p1 := wormholes[i].position;
p2 := wormholes[i+1].position;

draw_line(p1.x, p1.y, p2.x, p2.y, '.', lightgray);
end;
end;

begin
{ try to create a square window }
window(1, 1, windmaxx, windmaxx shr 1);
maxx := windmaxx shr 1;
maxy := windmaxy;

{ start: top left corner; destination: bottom right corner }
start.x := 1;
start.y := 1;
dest.x := maxx;
dest.y := maxy-1;

repeat
{ update }
generate_wormholes;
connect_wormholes;

enter_route(start.x, start.y, dest.x, dest.y);
find_path;

{ render }
clrscr;
draw_wormholes;
draw_path;

textcolor(white);
gotoxy(1, maxy-3);
writeln(maxx, ' x ', maxy);
writeln('cost path   : ', abs(cost[destindex]));
write('cost direct : ', costmat[startindex, destindex]);