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

Game development specific discussions.
Gunslinger
Posts: 103
Joined: Mar 08, 2016 19:10
Location: The Netherlands

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

Post by Gunslinger »

A* Implantation in badidea's code
Added code at line 235 and a heuristic function, seem to work fine.

Code: Select all

#include once "fbgfx.bi"
#define heuristic2(ax, ay, bx, by) (abs(ax - bx ) + abs(ay - by)) '# Manhattan distance on a square grid

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 = 1920, SCRN_H = SCRN_W * (9 / 16)
const as integer BOX_SIZE = 5
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) + heuristic2(xNbr, yNbr, xTarget, yTarget))) ' A* implant
         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 20,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"
Edit: last line code loop added
Last edited by Gunslinger on Mar 04, 2020 11:55, edited 6 times in total.
badidea
Posts: 2586
Joined: May 24, 2007 22:10
Location: The Netherlands

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

Post by badidea »

+loop
Post Reply