Page 1 of 1

A* Pathfinder using binary heap

Posted: Sun Jun 17, 2007 6:41 pm
by JCV
Serious A* programmers who want real speed use something called a binary heap. Using binary heap will be at least 2-3 times as fast and geometrically faster (10+ times as fast) on longer paths.

aStarLibrary.pbi

Code: Select all

;A* Pathfinder (Version 1.82) by Patrick Lester. Used by permission.
;==================================================================

;An article describing A* and this code in particular can be found at:
;http://www.policyalmanac.org/games/aStarTutorial.htm

;Setup
;-----
;1. Include "aStarLibrary.pbi" at the top of your program.

;2. Create an array called walkability(x,y) that contains information
;  about the walkability of each square/tile on your map, with
;  0 = walkable (the default value) and 1 = unwalkable. The array
;  should range from (0,0) in the upper left hand corner to 
;  (mapWidth-1,mapHeight-1) in the bottom right hand corner.

;3. Adjust the following variables at the top of the .declareVariables
;  subroutine below. All three should be made global.
;  - tileSize = the width and height of your square tiles in pixels
;  - mapWidth = the width of your map in tiles = x value in
;    walkability array.
;  - mapHeight = the height of your map in tiles = y value in
;    walkability array.


;Calling the Procedures
;---------------------
;There are three main Procedures

;1.  FindPath(unit.unit,targetX,targetY)
;  - unit.unit = unit that is doing the pathfinding
;  - targetX,targetY = location of the target destination (pixel based coordinates)

;  The FindPath() Procedure returns whether a path could be found (1) or
;  if it's nonexistent (2). If there is a path, it stores it in a bank
;  called unit\pathBank.

;2.   CheckPathStepAdvance(unit.unit)
;  This Procedure updates the current path.

;3.  ReadPath(unit.unit)
;   This Procedure reads the path data generated by FindPath() and returns
;  the x and y coordinates of the next step on the path. They are stored
;  as xPath and yPath. These coordinates are pixel coordinates 
;  on the screen. See the Procedure for more info.

;==========================================================
;DECLARE VARIABLES
;Adjust these variables to match your map dimensions (see "setup" above)
Global tileSize = 1, mapWidth = 20, mapHeight = 20

;Create needed arrays
Global Dim walkability(mapWidth+1,mapHeight+1) ;array that holds wall/obstacle information  
Global Dim openlist(mapWidth*mapHeight+2) ;1 dimensional array holding ID# of open list items
Global Dim whichList(mapWidth+1,mapHeight+1)  ;2 dimensional array used to record 
;whether a cell is on the open list or on the closed list.
Global Dim openX(mapWidth*mapHeight+2) ;1d array stores the x location of an item on the open list
Global Dim openY(mapWidth*mapHeight+2) ;1d array stores the y location of an item on the open list
Global Dim parentX(mapWidth+1,mapHeight+1) ;2d array to store parent of each cell (x)
Global Dim parentY(mapWidth+1,mapHeight+1) ;2d array to store parent of each cell (y)
Global Dim Fcost(mapWidth*mapHeight+2)  ;1d array to store F cost of a cell on the open list
Global Dim Gcost(mapWidth+1,mapHeight+1)   ;2d array to store G cost for each cell.
Global Dim Hcost(mapWidth*mapHeight+2)  ;1d array to store H cost of a cell on the open list    

;Declare constants
Global onClosedList = 10 ;openList variable  
#notfinished = 0
#notStarted = 0
#found = 1
#nonexistent = 2; pathStatus constants 
#walkable = 0
#unwalkable = 1; walkability array constants

Structure unit
  xLoc.l
  yLoc.l
  targetX.l
  targetY.l
  pathLocation.l
  pathLength.l
  pathBank.l
  xPath.l
  yPath.l
  pathStatus.l
EndStructure

;{ Declarations
Declare CheckPathStepAdvance(*unit.unit)
Declare ReadPathY(*unit.unit,pathLocation)
Declare ReadPathX(*unit.unit,pathLocation)
Declare ReadPath(*unit.unit)      
Declare FindPath(*unit.unit,targetX,targetY)
;}

;==========================================================
;FIND PATH: This Procedure finds the path and saves it. Non-Blitz users please note,
;the first parameter is a pointer to a user-defined object called a unit, which contains all
;relevant info about the unit in question (its current location, speed, etc.). As an
;object-oriented data structure, types are similar to structs in C.
;  Please note that targetX and targetY are pixel-based coordinates relative to the
;upper left corner of the map, which is 0,0.
Procedure FindPath(*unit.unit,targetX,targetY)
  
;1.  Convert location data (in pixels) to coordinates in the walkability array.
  startx = Round(*unit\xLoc/tileSize,0) : starty = Round(*unit\yLoc/tileSize,0)  
  targetX = Round(targetX/tileSize,0) : targetY = Round(targetY/tileSize,0)
;2.  Quick Path Checks: Under the some circumstances no path needs to
  ;be generated ...
  
  ;If starting location and target are in the same location...
  If startx = targetX And starty = targetY And *unit\pathLocation > 0 
    ProcedureReturn #found
  EndIf
  If startx = targetX And starty = targetY And *unit\pathLocation = 0 
    ProcedureReturn #nonexistent
  EndIf
  ;If target square is unwalkable, return that it's a nonexistent path.
  If walkability(targetX,targetY) = #unwalkable 
    Goto noPath
  EndIf
  
;3.  Reset some variables that need to be cleared
  If onClosedList > 1000000 ;occasionally redim whichList
    Redim whichList(mapWidth,mapHeight) 
    onClosedList = 10
  EndIf
  onClosedList = onClosedList+2 ;changing the values of onOpenList and onClosed list is faster than redimming whichList() array
  onOpenList = onClosedList-1
  *unit\pathLength = #notStarted ;i.e, = 0
  *unit\pathLocation = #notStarted ;i.e, = 0
  Gcost(startx,starty) = 0 ;reset starting square's G value to 0
  
;4.  Add the starting location to the open list of squares to be checked.
  numberOfOpenListItems = 1
  openlist(1) = 1 ;assign it as the top (and currently only) item in the open list, which is maintained as a binary heap (explained below)
  openX(1) = startx : openY(1) = starty
  
  
  ;5.  Do the following until a path is found or deemed nonexistent.
  Repeat
  
  
;6.  If the open list is not empty, take the first cell off of the list.
  ;This is the lowest F cost cell on the open list.
  If numberOfOpenListItems <> 0
  
  ;Pop the first item off the open list.
  parentXval = openX(openlist(1)) : parentYVal = openY(openlist(1)) ;record cell coordinates of the item
  whichList(parentXval,parentYVal) = onClosedList ;add the item to the closed list
  
  ;Open List = Binary Heap: Delete this item from the open list, which
  ;is maintained as a binary heap. For more information on binary heaps, see:
  ;http://www.policyalmanac.org/games/binaryHeaps.htm
  numberOfOpenListItems = numberOfOpenListItems - 1 ;reduce number of open list items by 1  
  openlist(1) = openlist(numberOfOpenListItems+1) ;move the last item in the heap up to slot #1
  v = 1  
  Repeat ;Repeat the following until the new item in slot #1 sinks to its proper spot in the heap.
  u = v  
  If 2*u+1 <= numberOfOpenListItems ;if both children exist
    ;Check if the F cost of the parent is greater than each child.
    ;Select the lowest of the two children.  
    If Fcost(openlist(u)) >= Fcost(openlist(2*u)) 
      v = 2*u
      EndIf
      If Fcost(openlist(v)) >= Fcost(openlist(2*u+1)) 
      v = 2*u+1    
      EndIf
    Else
      If 2*u <= numberOfOpenListItems ;if only child #1 exists
      ;Check if the F cost of the parent is greater than child #1  
      If Fcost(openlist(u)) >= Fcost(openlist(2*u)) 
        v = 2*u
        EndIf
      EndIf  
    EndIf
    If u<>v ;if parent's F is > one of its children, swap them
    temp = openlist(u)
    openlist(u) = openlist(v)
    openlist(v) = temp        
    Else
    Break ;otherwise, exit loop
  EndIf  
  ForEver
  
  
;7.  Check the adjacent squares. (Its "children" -- these path children
  ;are similar, conceptually, to the binary heap children mentioned
  ;above, but don't confuse them. They are different. Path children
  ;are portrayed in Demo 1 with grey pointers pointing toward
  ;their parents.) Add these adjacent child squares to the open list
  ;for later consideration if appropriate (see various if statements
  ;below).
  For b = parentYVal-1 To parentYVal+1
    For a = parentXval-1 To parentXval+1

      ;If not off the map (do this first to avoid array out-of-bounds errors)
      If a <> -1 And b <> -1 And a <> mapWidth And b <> mapHeight
    
        ;If not already on the closed list (items on the closed list have
        ;already been considered and can now be ignored).      
        If whichList(a,b) <> onClosedList 
      
          ;If not a wall/obstacle square.
          If walkability(a,b) <> #unwalkable 
        
            ;Don't cut across corners (this is optional)
            corner = #walkable  
            If a = parentXval-1 
              If b = parentYVal-1 
                If walkability(parentXval-1,parentYVal) = #unwalkable Or walkability(parentXval,parentYVal-1) = #unwalkable 
                  corner = #unwalkable
                EndIf
              ElseIf b = parentYVal+1 
                If walkability(parentXval,parentYVal+1) = #unwalkable Or walkability(parentXval-1,parentYVal) = #unwalkable 
                corner = #unwalkable 
                EndIf
              EndIf
            ElseIf a = parentXval+1 
              If b = parentYVal-1 
                If walkability(parentXval,parentYVal-1) = #unwalkable Or walkability(parentXval+1,parentYVal) = #unwalkable 
                  corner = #unwalkable 
                EndIf
              ElseIf b = parentYVal+1 
                If walkability(parentXval+1,parentYVal) = #unwalkable Or walkability(parentXval,parentYVal+1) = #unwalkable 
                corner = #unwalkable 
                EndIf
              EndIf
            EndIf
        
            If corner = #walkable
              ;If not already on the open list, add it to the open list.      
              If whichList(a,b) <> onOpenList  
  
                ;Create a new open list item in the binary heap.
                newOpenListItemID = newOpenListItemID + 1; each new item has a unique ID #
                m = numberOfOpenListItems+1
                openlist(m) = newOpenListItemID   ;place the new open list item (actually, its ID#) at the bottom of the heap
                openX(newOpenListItemID) = a : openY(newOpenListItemID) = b ;record the x and y coordinates of the new item
            
                ;Figure out its G cost
                If Abs(a-parentXval) = 1 And Abs(b-parentYVal) = 1
                  addedGCost = 14 ;cost of going to diagonal squares  
                Else  
                  addedGCost = 10 ;cost of going to non-diagonal squares        
                EndIf
                Gcost(a,b) = Gcost(parentXval,parentYVal)+addedGCost
              
                ;Figure out its H and F costs and parent
                Hcost(openlist(m)) = 10*(Abs(a - targetX) + Abs(b - targetY)) ; record the H cost of the new square
                Fcost(openlist(m)) = Gcost(a,b) + Hcost(openlist(m)) ;record the F cost of the new square
                parentX(a,b) = parentXval : parentY(a,b) = parentYVal  ;record the parent of the new square  
              
                ;Move the new open list item to the proper place in the binary heap.
                ;Starting at the bottom, successively compare to parent items,
                ;swapping as needed until the item finds its place in the heap
                ;or bubbles all the way to the top (if it has the lowest F cost).
                While m <> 1 ;While item hasn't bubbled to the top (m=1)  
                  ;Check if child's F cost is < parent's F cost. If so, swap them.  
                  If Fcost(openlist(m)) <= Fcost(openlist(m/2))
                    temp = openlist(m/2)
                    openlist(m/2) = openlist(m)
                    openlist(m) = temp
                    m = m/2
                  Else
                    Break
                  EndIf
                Wend 
                numberOfOpenListItems = numberOfOpenListItems+1 ;add one to the number of items in the heap
              
                ;Change whichList to show that the new item is on the open list.
                whichList(a,b) = onOpenList
              
  ;8.  If adjacent cell is already on the open list, check to see if this 
      ;path to that cell from the starting location is a better one. 
      ;If so, change the parent of the cell and its G and F costs.  
              Else; If whichList(a,b) = onOpenList
              
                ;Figure out the G cost of this possible new path
                If Abs(a-parentXval) = 1 And Abs(b-parentYVal) = 1
                  addedGCost = 14;cost of going to diagonal tiles  
                Else  
                  addedGCost = 10 ;cost of going to non-diagonal tiles        
                EndIf
                tempGcost = Gcost(parentXval,parentYVal)+addedGCost
              
                ;If this path is shorter (G cost is lower) then change
                ;the parent cell, G cost and F cost.     
                If tempGcost < Gcost(a,b) ;if G cost is less,
                  parentX(a,b) = parentXval   ;change the square's parent
                  parentY(a,b) = parentYVal
                  Gcost(a,b) = tempGcost   ;change the G cost      
              
                  ;Because changing the G cost also changes the F cost, if
                  ;the item is on the open list we need to change the item's
                  ;recorded F cost and its position on the open list to make
                  ;sure that we maintain a properly ordered open list.
                  For x = 1 To numberOfOpenListItems ;look for the item in the heap
                    If openX(openlist(x)) = a And openY(openlist(x)) = b ;item found
                      Fcost(openlist(x)) = Gcost(a,b) + Hcost(openlist(x)) ;change the F cost
                    
                      ;See if changing the F score bubbles the item up from it's current location in the heap
                      m = x
                      While m <> 1 ;While item hasn't bubbled to the top (m=1)  
                        ;Check if child is < parent. If so, swap them.  
                        If Fcost(openlist(m)) < Fcost(openlist(m/2)) 
                          temp = openlist(m/2)
                          openlist(m/2) = openlist(m)
                          openlist(m) = temp
                          m = m/2
                        Else
                          Break ;while/wend
                        EndIf
                      Wend 
                      Break ;for x = loop
                    EndIf ;If openX(openList(x)) = a
                  Next ;For x = 1 To numberOfOpenListItems
                EndIf ;If tempGcost < Gcost(a,b) Then      
              EndIf ;If not already on the open list        
            EndIf ;If corner = walkable
          EndIf ;If not a wall/obstacle cell.  
        EndIf ;If not already on the closed list  
      EndIf ;If not off the map.  
    Next
  Next
  
;9.  If open list is empty then there is no path.  
  Else
    Path = #nonexistent 
    Break
  EndIf
  
  ;If target is added to open list then path has been found.
  If whichList(targetX,targetY) = onOpenList 
    Path = found
    Break
  EndIf
  
  ForEver ;repeat until path is found or deemed nonexistent
  
  
;10.  Save the path if it exists. Copy it to a bank. 
  If Path = found
    
    ;a. Working backwards from the target to the starting location by checking
    ;each cell's parent, figure out the length of the path.
    pathX = targetX : pathY = targetY  
    Repeat
      tempx = parentX(pathX,pathY)    
      pathY = parentY(pathX,pathY)
      pathX = tempx
      *unit\pathLength = *unit\pathLength + 1  
      Until pathX = startx And pathY = starty
      
      ;b. Resize the data bank to the right size (leave room to store step 0,
      ;which requires storing one more step than the length)
      ReAllocateMemory(*unit\pathBank, (*unit\pathLength+1)*4)
      
      ;c. Now copy the path information over to the databank. Since we are
      ;working backwards from the target to the start location, we copy
      ;the information to the data bank in reverse order. The result is
      ;a properly ordered set of path data, from the first step to the
      ;last.  
      pathX = targetX : pathY = targetY        
      cellPosition = *unit\pathLength*4 ;start at the end  
      While Not (pathX = startx And pathY = starty)      
      PokeW(*unit\pathBank+cellPosition,pathX) ;store x value  
      PokeW(*unit\pathBank+cellPosition+2,pathY) ;store y value  
      cellPosition = cellPosition - 4 ;work backwards    
      tempx = parentX(pathX,pathY)    
      pathY = parentY(pathX,pathY)
      pathX = tempx
    Wend  
    PokeW(*unit\pathBank,startx) ;store starting x value  
    PokeW(*unit\pathBank+2,starty) ;store starting y value
      
  EndIf ;If path = found Then 
  
  
;11. Return info on whether a path has been found.
  ProcedureReturn Path; Returns 1 if a path has been found, 2 if no path exists. 
  
;12.If there is no path to the selected target, set the pathfinder's
  ;xPath and yPath equal to its current location and return that the
  ;path is nonexistent.
noPath:
  *unit\xPath = startingX
  *unit\yPath = startingY
  ProcedureReturn #nonexistent
  
EndProcedure


;==========================================================
;READ PATH DATA: These Procedures read the path data and convert
;it to screen pixel coordinates.
Procedure ReadPath(*unit.unit)      
  *unit\xPath = ReadPathX(*unit.unit,*unit\pathLocation)
  *unit\yPath = ReadPathY(*unit.unit,*unit\pathLocation)
EndProcedure

Procedure ReadPathX(*unit.unit,pathLocation)
  If pathLocation <= *unit\pathLength
    x = PeekW(*unit\pathBank+(pathLocation*4))
    ProcedureReturn tileSize * x + (1/2) * tileSize;align w/center of square  
  EndIf
EndProcedure  

Procedure ReadPathY(*unit.unit,pathLocation)
  If pathLocation <= *unit\pathLength
  y = PeekW(*unit\pathBank+(pathLocation*4+2))
  ProcedureReturn tileSize*y + (1/2) * tileSize ;align w/center of square
  EndIf
EndProcedure


;This Procedure checks whether the unit is close enough to the next
;path node to advance to the next one or, if it is the last path step,
;to stop.
Procedure CheckPathStepAdvance(*unit.unit)
  If (*unit\xLoc = *unit\xPath And *unit\yLoc = *unit\yPath) Or *unit\pathLocation = 0
  If *unit\pathLocation = *unit\pathLength 
    *unit\pathStatus = #notStarted  
  Else     
    *unit\pathLocation = *unit\pathLocation + 1
    ReadPath(*unit) ;update xPath and yPath
  EndIf  
  EndIf  
EndProcedure
example.pb - modified example of Heathen

Code: Select all

InitSprite()
InitMouse()
InitKeyboard()

#width = 20
#height = 20

startx = 0
starty = 0
tox = 19
toy = 19

XIncludeFile "aStarLibrary.pbi"

Global unit.unit
unit\pathBank = AllocateMemory(1) ;data bank that unit's path data is stored	in	
unit\xLoc = 5 
unit\yLoc = 5 

walkability(7,4) = 1
walkability(7,5) = 1
walkability(7,6) = 1
walkability(7,7) = 1

Macro draw()
  StartDrawing(ImageOutput(0))
  Box(0,0,640,640)
  For x = 1 To 20
    LineXY(x*32,0,x*32,640,#White)
  Next x
  For y = 1 To 20
    LineXY(0,y*32,640,y*32,#White)
  Next y
  Box(starty*32,startx*32,32,32,#Yellow)
  If unit > 0
    For a = 0 To 100
      xx = ReadPathX(unit,a)
      yy = ReadPathY(unit,a)
      If xx > -1 And yy > -1
        Box(xx*32,yy*32,32,32,#Green)
      EndIf
    Next a
  EndIf
  Box(tox*32,toy*32,32,32,#Blue)
  For x = 0 To 20
    For y = 0 To 20
      If walkability(x,y)=1
        Box(x*32,y*32,32,32,#Red)
      EndIf
    Next y
  Next x
  StopDrawing()
EndMacro

unit\xLoc = startx
unit\yLoc = starty 
FindPath(unit,tox,toy)
OpenWindow(0,0,0,640,640,"",#PB_Window_ScreenCentered|#PB_Window_SystemMenu)

OpenWindowedScreen(WindowID(0),0,0,640,640,0,0,0)
CreateImage(0,640,640,#PB_Image_DisplayFormat)
draw()
Repeat
  event = WaitWindowEvent()
  FlipBuffers(2)
  StartDrawing(ScreenOutput())
  DrawImage(ImageID(0),0,0)
  Circle(MouseX(),MouseY(),10,#White)
  StopDrawing()
  ExamineMouse()
  ExamineKeyboard()
  If MouseButton(#PB_MouseButton_Left) And walkability(MouseX()/32,MouseY()/32) = 0
    walkability(MouseX()/32,MouseY()/32) = 1
    t = GetTickCount_()
    If *path > 0
      FreeMemory(*path)
    EndIf
    unit\xLoc = startx
    unit\yLoc = starty 
    FindPath(unit,tox,toy)
    SetWindowTitle(0,Str(GetTickCount_()-t))
    draw()
  ElseIf MouseButton(#PB_MouseButton_Right) And walkability(MouseX()/32,MouseY()/32) = 1
    walkability(MouseX()/32,MouseY()/32) = 0
    t = GetTickCount_()
    If *path > 0
      FreeMemory(*path)
    EndIf
    ;*path = get_path(startx,starty,tox,toy,#width,#height,allowdiag,blockdiag)
    unit\xLoc = startx
    unit\yLoc = starty 
    FindPath(unit,tox,toy)
    SetWindowTitle(0,Str(GetTickCount_()-t))
    draw()
  EndIf
  If KeyboardPushed(#PB_Key_Escape)
    End
  EndIf
  Delay(1)
Until event = #PB_Event_CloseWindow 

Posted: Mon Jun 18, 2007 3:37 am
by Paul
You forgot to allow for this variable...

Code: Select all

allowdiag = 0

Posted: Mon Jun 18, 2007 5:19 am
by JCV
Variable is not used. Removed.
I'm using this method on my game and its fast on really huge maps.

I did some benchmark

Both 20x20 map

Using binary heap
0.062 seconds - 1k loops
Hearthen's example
0.781 seconds - 1k loops

Both 1000x1000 map

Using binary heap
0.078 seconds - 1k loops
Hearthen's example
1.312 seconds - 1k loops