It is currently Sun Sep 21, 2014 11:07 pm

All times are UTC + 1 hour




Post new topic Reply to topic  [ 3 posts ] 
Author Message
 Post subject: A* Pathfinder using binary heap
PostPosted: Sun Jun 17, 2007 6:41 pm 
Offline
Enthusiast
Enthusiast
User avatar

Joined: Fri Jun 30, 2006 4:30 pm
Posts: 567
Location: NDIA Project
Quote:
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:
;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:
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

_________________

[Registered PB User since 2006]
[PureBasic 5.20 LTS] [Win 8.1 64bit]
[Intel i7 990x 3.47 Ghz] [18GB DDR3]


Last edited by JCV on Mon Jun 18, 2007 4:57 am, edited 1 time in total.

Top
 Profile  
 
 Post subject:
PostPosted: Mon Jun 18, 2007 3:37 am 
Offline
PureBasic Expert
PureBasic Expert
User avatar

Joined: Fri Apr 25, 2003 4:34 pm
Posts: 790
Location: Canada
You forgot to allow for this variable...
Code:
allowdiag = 0

_________________
Image Image


Top
 Profile  
 
 Post subject:
PostPosted: Mon Jun 18, 2007 5:19 am 
Offline
Enthusiast
Enthusiast
User avatar

Joined: Fri Jun 30, 2006 4:30 pm
Posts: 567
Location: NDIA Project
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

_________________

[Registered PB User since 2006]
[PureBasic 5.20 LTS] [Win 8.1 64bit]
[Intel i7 990x 3.47 Ghz] [18GB DDR3]


Top
 Profile  
 
Display posts from previous:  Sort by  
Post new topic Reply to topic  [ 3 posts ] 

All times are UTC + 1 hour


Who is online

Users browsing this forum: No registered users and 1 guest


You cannot post new topics in this forum
You cannot reply to topics in this forum
You cannot edit your posts in this forum
You cannot delete your posts in this forum

Search for:
Jump to:  
cron

 


Powered by phpBB © 2008 phpBB Group
subSilver+ theme by Canver Software, sponsor Sanal Modifiye