Path-finding for 2d-grid-based games, using Dijkstra's algorithm

[badidea]

Not in the form of a ready to use library, but a working demo.

Code: Select all

#include once "fbgfx.bi"

const as integer ALLOW_DIAGONAL_MOVEMENT = 1

'-------------------------------------------------------------------------------

type int2d
	as integer x, y
end type

type intxyd
	dim as integer x,y
	dim as single dist
end type

#define dll_data_type intxyd

type dll_node_type
	dim as dll_data_type value
	dim as dll_node_type ptr pNext
	dim as dll_node_type ptr pPrev
end type

sub dll_delete(byval pNode as dll_node_type ptr)
	if pNode then
		(pNode->pPrev)->pNext = pNode->pNext
		if pNode->pNext then
			(pNode->pNext)->pPrev = pNode->pPrev
		end if
		delete pNode
	end if
end sub

sub dll_deleteFirst(byval pRootNode as dll_node_type ptr)
	dim as dll_node_type ptr pNode = pRootNode->pNext
	if pNode then
		(pNode->pPrev)->pNext = pNode->pNext
		if pNode->pNext then
			(pNode->pNext)->pPrev = pNode->pPrev
		end if
		delete pNode
	end if
end sub

sub dll_deleteAll(byval pRootNode as dll_node_type ptr)
	dim as dll_node_type ptr pDelNode
	while (pRootNode->pNext)
		pDelNode = pRootNode->pNext
		pRootNode->pNext = pDelNode->pNext
		delete pDelNode
	wend
end sub

function dll_count(byval pNode as dll_node_type ptr) as integer
	dim as integer count = 0
	while (pNode->pNext)
		pNode = pNode->pNext
		count += 1
	wend
	return count
end function

'low values at root node
sub dll_insertSorted(byval pNode as dll_node_type ptr, byval value as dll_data_type)
	dim as dll_node_type ptr pNewNode = new dll_node_type
	while (pNode->pNext)
		pNode = pNode->pNext
		if pNode->value.dist >= value.dist then
			pNode = pNode->pPrev
			exit while
		end if
	wend
	if pNode->pNext then
		pNewNode->pNext = pNode->pNext
		(pNewNode->pNext)->pPrev = pNewNode
	end if
	pNewNode->pPrev = pNode
	pNode->pNext = pNewNode
	pNewNode->value = value
end sub

'-------------------------------------------------------------------------------

const as ubyte BOX_FREE = 0
const as ubyte BOX_BLOCKED = 1
const as ubyte BOX_START = 2
const as ubyte BOX_TARGET = 3
const as ubyte BOX_VISITED = 4
const as ubyte BOX_ROUTE = 5

const as integer SCRN_W = 1280, SCRN_H = SCRN_W * (9 / 16)
const as integer BOX_SIZE = 10
const as integer MAP_SIZE_X = SCRN_W \ BOX_SIZE
const as integer MAP_SIZE_Y = SCRN_H \ BOX_SIZE
dim shared as ubyte map(MAP_SIZE_X, MAP_SIZE_Y)

'predefine colors (argb), 32-bit unsigned
dim shared as ulong BOX_COLOR(0 to 5) = _
	{&h00444444, &h00707070, &h0000a000, &h00d00000, &h00c0c000, &h00f0f0f0}

function rndIntBetween(min as integer, max as integer) as integer
	return int(rnd*(max-min+1))+min
end function

sub drawBox(x as integer, y as integer)
	if map(x, y) = BOX_FREE then
		'show grid / empty box
		line(x*BOX_SIZE, y*BOX_SIZE)-_
		step(BOX_SIZE-1, BOX_SIZE-1), BOX_COLOR(BOX_FREE), b
	else
		'show boxes
		line(x*BOX_SIZE, y*BOX_SIZE)-_
		step(BOX_SIZE-1, BOX_SIZE-1), BOX_COLOR(map(x, y)), bf
	end if
end sub

sub drawMap()
	dim as integer x, y
	For y = 0 to MAP_SIZE_Y-1
		for x = 0 to MAP_SIZE_X-1
			drawBox(x, y)
		next
	next
end sub

'-------------------------------------------------------------------------------

dim as integer x, y, i
dim as integer xStart, yStart
dim as integer xTarget, yTarget
dim as string key

const as single D_INF = 1e6
dim shared as single dist(MAP_SIZE_X, MAP_SIZE_Y) 'distance form start position
dim shared as int2d par(MAP_SIZE_X, MAP_SIZE_Y) 'x,y position of parent
dim as dll_node_type ptr pRoot = new dll_node_type 'priority queue
dim as integer xCur, yCur, xNbr, yNbr
dim as single curDist
dim as integer iDelta, routeFound
dim as double tSTart, tEnd
dim as integer allowDir
dim as intxyd delta(0 to 7) = _
{_
	type(0, 1, 1.0), type(0, -1, 1.0), type(1, 0, 1.0), type(-1, 0, 1.0), _
	type(+1, +1, 1.41), type(+1, -1, 1.41), type(-1, +1, 1.41), type(-1, -1, 1.41) _
}

if (ALLOW_DIAGONAL_MOVEMENT = 1) then allowDir = 8 else allowDir = 4

screenres SCRN_W, SCRN_H, 32

randomize timer
'randomize 1242

do
	'create random map
	For y = 0 to MAP_SIZE_Y-1
		for x = 0 to MAP_SIZE_X-1
			map(x, y) = BOX_FREE
			'make box / borders
			if (x = 0) or (x = MAP_SIZE_X-1) then map(x, y) = BOX_BLOCKED
			if (y = 0) or (y = MAP_SIZE_Y-1) then map(x, y) = BOX_BLOCKED
			'fill with random blocks
			if (rnd >= .7) then map(x, y) = BOX_BLOCKED
		next
	next

	'random start and target position
	xStart = rndIntBetween(1, MAP_SIZE_X-2)
	yStart = rndIntBetween(1, MAP_SIZE_Y-2)
	xTarget = rndIntBetween(1, MAP_SIZE_X-2)
	yTarget = rndIntBetween(1, MAP_SIZE_Y-2)

	map(xStart, yStart) = BOX_START
	map(xTarget, yTarget) = BOX_TARGET

	cls()
	drawMap()

	'-------------------------------------------------------------------------------

	'init Dijkstra stuff
	tSTart = timer()
	routeFound = 0
	For y = 0 to MAP_SIZE_Y-1
		for x = 0 to MAP_SIZE_X-1
			if map(x, y) <> BOX_BLOCKED then
				dist(x, y) = D_INF
			else
				dist(x, y) = -D_INF
			end if
			par(x, y) = type(-1, -1)
		next
	next
	'set start point, add to list
	dist(xStart, yStart) = 0
	par(xStart, yStart) = type(-1, -1)
	dll_insertSorted(pRoot, type(xStart, yStart, 0))
	'zolang iets in queue:
	'x,y = pop from queue
	while(pRoot->pNext)
		xCur = pRoot->pNext->value.x
		yCur = pRoot->pNext->value.y
		curDist = dist(xCur, yCur)
		dll_deleteFirst(pRoot)
		if (xCur = xTarget) and (yCur = yTarget) then
			routeFound = 1
			exit while
		end if
		'debug
		map(xCur, yCur) = BOX_VISITED
		'drawBox(xCur, yCur)
		for iDelta = 0 to allowDir-1
			'check not outside map
			xNbr = xCur + delta(iDelta).x
			if (xNbr < 0) or (xNbr >= MAP_SIZE_X) then continue for
			yNbr = yCur + delta(iDelta).y
			if (yNbr < 0) or (yNbr >= MAP_SIZE_Y) then continue for
			'skip blocked
			if dist(xNbr, yNbr) < 0 then continue for
			'check diagonal move allowed
			if iDelta > 3 then
				if (dist(xCur, yNbr) < 0) and (dist(xNbr, yCur) < 0) then continue for
			end if
			'current dist to start + neighbour moveto cost < neighbour dist to start
			if curDist + delta(iDelta).dist <  dist(xNbr, yNbr) then
				'update neighbour
				dist(xNbr, yNbr) = curDist + delta(iDelta).dist
				par(xNbr, yNbr) = type(xCur, yCur)
				dll_insertSorted(pRoot, type(xNbr, yNbr, dist(xNbr, yNbr)))
			end if
		next
		if (inkey() = "q") then exit while
	wend
	dll_deleteAll(pRoot)
	tEnd = timer() - tStart

	map(xStart, yStart) = BOX_START
	drawMap()

	locate 1,1: print "Time: " & tEnd
	print

	if (routeFound = 1) then
		print "Route found :-)"
		'Route for end to start
		xCur = xTarget
		yCur = yTarget
		while (1)
			x = par(xCur, yCur).x
			y = par(xCur, yCur).y
			xCur = x
			yCur = y
			if ((xCur = xStart) and (yCur = yStart)) then exit while
			if (xCur < 0) then print "(xCur < 0)": sleep 10000, 0
			if (yCur < 0) then print "(xCur < 0)": sleep 10000, 0
			if (xCur >= MAP_SIZE_X-1) then print "(xCur >= MAP_SIZE_X-1)": sleep 10000, 0
			if (yCur >= MAP_SIZE_Y-1) then print "(yCur >= MAP_SIZE_Y-1)": sleep 10000, 0
			map(xCur, yCur) = BOX_ROUTE
			drawBox(xCur, yCur)
			Sleep 50,1
		wend
	else
		print "NO route found!"
	end if

	print
	print "Press <any> for next"
	print "Press <q> to quit"
	
	key = inkey
	while (key ="")
		key = inkey
		sleep 1, 0
	wend

loop until key = "q"

[MrSwiss]

@badidea,

some input on possible (speed) improvements:

Code: Select all

'const as integer SCRN_W = 1280, SCRN_H = SCRN_W * (9 / 16) ' slowest = FB float div
'const as integer SCRN_W = 1280, SCRN_H = (SCRN_W \ 16) * 9	' medium  = FB int div
const as integer SCRN_W = 1280, SCRN_H = (SCRN_W Shr 4) * 9 ' fastest = FB ASM-like int div

avoiding (later, possible) Problems:

Code: Select all

'predefine colors (argb), 32-bit unsigned, asking for later troubles: Alpha = fully transparent !
'dim shared as ulong BOX_COLOR(0 to 5) = _
'   {&h00444444, &h00707070, &h0000a000, &h00d00000, &h00c0c000, &h00f0f0f0}
'predefine colors (argb), 32-bit unsigned, OK because Alpha = fully opaque !
dim shared as ulong BOX_COLOR(0 to 5) = _
   {&hff444444, &hff707070, &hff00a000, &hffd00000, &hffc0c000, &hfff0f0f0}

see Comments in Code.

[counting_pine]

There's no point trying to optimise constant expressions at the extent of readability. The constants will be evaluated at compile-time, and you won't even notice any difference in the compile speed.

[MrSwiss]

@counting_pine,

I'm aware of that, doing it to Constants, is not very useful, however the Principle shown, is
applicable to any sort of Variable assignment (mainly run-time Stuff benefits from Speed).

[dodicat]

Very interesting Badidea.
And nice graphical demo.

I Googled Dijkstra, and had a bash at a brute force method of traversing between two given points and visiting once each other point in a given set.
It is much simplified, with only a few points each set.
Press the space bar to refresh, esc to quit.
I have loaded (number of points multiplied by 2000 ) for an iteration number, just for the sake of getting running code, without much thought to what would be a better number.

Code: Select all

screen 19
screencontrol 100,250,50
#include "crt.bi"
#define Intrange(f,l) int(Rnd*(((l)+1)-(f))+(f))

type pt
    as integer x,y
end type

type pts
    as pt p(any)
    as integer index
end type

'omit sqr for speed
#define length(a,b) ((a.x-b.x)*(a.x-b.x)+(a.y-b.y)*(a.y-b.y))

function distance(points as pts) as integer
    dim as single total,L
    for n as integer=lbound(points.p) to ubound(points.p)-1
    L= length(points.p(n),points.p(n+1))
    total+=sqr(L)
next n
return total
end function

randomize 1
start:
dim as integer dots=IntRange(4,15)       'number of points
dim as integer runs=dots*2000             'iterations

dim as pts points
dim shared as pts cpy(runs)

redim points.p(1 to dots)

#macro setup()
for n as integer=lbound(points.p) to ubound(points.p)
    points.p(n)=type<pt>(IntRange(10,790),IntRange(50,590))
next n
#endmacro

#macro show(c,flag)
 circle(c.p(1).x,c.p(1).y),5,4,,,,f 
for n as integer=lbound(c.p)+1 to ubound(c.p)
    if flag then line -(c.p(n).x,c.p(n).y)
  if n< ubound(c.p) then circle(c. p(n).x,c.p(n).y),5 else circle(c. p(n).x,c.p(n).y),5,2,,,,f
next n
if flag then
draw string(c.p(1).x,c.p(1).y-20),str(1)
for n as integer=lbound(c.p)+1 to ubound(c.p)
   draw string(c.p(n).x,c.p(n).y-20),str(n)
next n
end if
#endmacro

dim as integer total,L,max=-1e7,min=1e7,ctr,lastmin

setup()
show(points,0)
print "Press a key"
var key=input(1)

dim as string i,st
do
    ctr+=1
    dim as integer x,y
    do
    x=IntRange(lbound(points.p)+1,ubound(points.p)-1):y=IntRange(lbound(points.p)+1,ubound(points.p)-1)
    loop until x<>y
    swap points.p(x),points.p(y)'change the configuration here.
    Total=0
 'Get the total sum through the points   
for n as integer=lbound(points.p) to ubound(points.p)-1
    L= length(points.p(n),points.p(n+1))
    total+=L
next n
'keep track history
cpy(ctr)=points      'copy the whole array
cpy(ctr).index=total 'set the index number.

'get max and min sum 
if max<total then max=total
if min>total then min=total
' consol print
if lastmin<>min then
puts str(min)
end if

lastmin=min
loop until  ctr=runs


dim as integer Vmin,Vmax
'get the array elements for max and min values.
for n as integer=lbound(cpy) to ubound(cpy)
    if cpy(n).index=min then Vmin=n
    if cpy(n).index=max then Vmax=n  
    next n
show(cpy(Vmin),1)
draw string(10,10), "minimum distance = "& distance(cpy(Vmin)) & "   Iterations = " &runs
var tmp=input(1)
cls
show(cpy(Vmax),1)
draw string(10,10), "maximum distance = "& distance(cpy(Vmax))
i=input(1)
if i<>chr(27) then cls:puts " ":puts "iterate down": goto start else end
sleep

 

[badidea]

Is that the "Travelling salesman problem"?

https://en.wikipedia.org/wiki/Travellin ... an_problem

[dodicat]

I see that the salesperson goes back home after visiting his/her locations.
I've adjusted for the return home, and kept his/her visits outside a radius of 200 from the screen centre.
You would expect a kind of orbital path to be the shortest??
maybe!
small edit made

Code: Select all

 
 
screen 19
screencontrol 100,250,50
#include "crt.bi"
#define Intrange(f,l) int(Rnd*(((l)+1)-(f))+(f))

type pt
    as integer x,y
end type

type pts
    as pt p(any)
    as integer index
end type

'omit sqr for speed
#define length(a,b) ((a.x-b.x)*(a.x-b.x)+(a.y-b.y)*(a.y-b.y))

function distance(points as pts) as integer
    dim as single total,L
    for n as integer=lbound(points.p) to ubound(points.p)-1
    L= length(points.p(n),points.p(n+1))
    total+=sqr(L)
next n
L= sqr(length(points.p(ubound(points.p)),points.p(lbound(points.p))))
total+=L
return total
end function

randomize 1
start:
dim as integer dots=IntRange(5,15)       'number of points 40,150
dim as integer runs=dots*2000             'iterations

dim as pts points
dim shared as pts cpy(runs)

redim points.p(1 to dots)

#macro setup()
for n as integer=lbound(points.p) to ubound(points.p)
    do
    var x=IntRange(10,790),y=IntRange(50,590)
     dist=sqr(length(type<pt>(x,y),type<pt>(400,300)))
    points.p(n)=type<pt>(x,y)
    loop until dist>200
next n
#endmacro

#macro show(c,flag)
 circle(c.p(1).x,c.p(1).y),10,4,,,,f
for n as integer=lbound(c.p)+1 to ubound(c.p)
    if flag then line -(c.p(n).x,c.p(n).y)
   circle(c. p(n).x,c.p(n).y),5
next n

if flag then'draw to close
 lb=lbound(c.p)
line -(c.p(lb).x,c.p(lb).y)
end if
circle(400,300),200
if flag then
draw string(c.p(1).x,c.p(1).y-20),str(1)
for n as integer=lbound(c.p)+1 to ubound(c.p)
   draw string(c.p(n).x,c.p(n).y-20),str(n)
next n
end if
#endmacro

dim as integer total,L,max=-1e8,min=1e8,ctr,lastmin,lb,dist

setup()
show(points,0)
print "Press a key"
var key=input(1)

dim as string i,st
do
    ctr+=1
    dim as integer x,y
    do
    x=IntRange(lbound(points.p)+1,ubound(points.p)):y=IntRange(lbound(points.p)+1,ubound(points.p))
    loop until x<>y
    swap points.p(x),points.p(y)'change the configuration here.
    Total=0
 'Get the total sum through the points   
for n as integer=lbound(points.p) to ubound(points.p)-1
    L= length(points.p(n),points.p(n+1))
    total+=L
next n
'from last to first
L=length(points.p(lbound(points.p)),points.p(ubound(points.p)))
total+=L
'keep track history
cpy(ctr)=points      'copy the whole array
cpy(ctr).index=total 'set the index number.

'get max and min sum
if max<total then max=total
if min>total then min=total
' consol print
if lastmin<>min then
puts str(min)
end if

lastmin=min
loop until  ctr=runs


dim as integer Vmin,Vmax
'get the array elements for max and min values.
for n as integer=lbound(cpy) to ubound(cpy)
    if cpy(n).index=min then Vmin=n
    if cpy(n).index=max then Vmax=n
next n

'puts str(vmin)+","+str(lbound(cpy)) +"," + str(ubound(cpy))
show(cpy(Vmin),1)
draw string(10,10), "minimum distance = "& distance(cpy(Vmin)) & "   Iterations = " &runs & "  Number of locations " &(ubound(points.p)+1)
var tmp=input(1)
cls
show(cpy(Vmax),1)
draw string(10,10), "maximum distance = "& distance(cpy(Vmax))
i=input(1)
if i<>chr(27) then cls:puts " ":puts "iterate down": goto start else end
sleep

  
  

I'll leave it at that, it is distracting from your own path finder.
Cheers.

[Roland Chastain]

Hello!

@badidea

Whats does "dll" mean please?

[badidea]

Double linked list: https://en.wikipedia.org/wiki/Doubly_linked_list (nothing to do with dynamic linked library).
To be specific, a 'sorted double linked list', or 'priority queue' https://en.wikipedia.org/wiki/Priority_queue.
This list was essential to get the performance acceptable.
Actually, a single linked list works as well in this case, but the speed gain was zero.

[Roland Chastain]

I should have thought to that. Thank you.

[Dr_D]

This is one of my favorite algorithms of all time. Nice implementation. :)

[leopardpm]

This is one of my favorite algorithms of all time. Nice implementation. :)

Why do you like this better than A*?

[Dr_D]

I never specified. I admire both variations.. or actually ALL variations that actually work. If I did have a favorite for what I'm doing, it's a method using waypoints in 3d space, which works by avoiding making node connections for waypoints which have a very high vertical distance between them... In other words, a cliff or high ledge. ;)

[badidea]

This is one of my favorite algorithms of all time. Nice implementation. :)

Why do you like this better than A*?

A* is faster, but not guaranteed the shortest path, which the Dijkstra routine should give (or several shortest).

I actually used this information and code (http://www.cs.cmu.edu/~crpalmer/sp/) the make my freeBASIC version. So the comment "Nice implementation. :)" should go to mister Chris Palmer.

[leopardpm]

What about optimal paths? Breadth First Search and Dijkstra’s Algorithm are guaranteed to find the shortest path given the input graph. Greedy Best First Search is not. A* is guaranteed to find the shortest path if the heuristic is never larger than the true distance. As the heuristic becomes smaller, A* turns into Dijkstra’s Algorithm. As the heuristic becomes larger, A* turns into Greedy Best First Search.