March 05, 2021, 07:47:34 AM

Author Topic: [bmx] Generalized A* Pathfinding by Pineapple [ 1+ years ago ]  (Read 520 times)

Offline BlitzBot

Title : Generalized A* Pathfinding
Author : Pineapple
Posted : 1+ years ago

Description : Works with variable tile costs, has a flexible method of getting tile cost, and is just generally very generalized. It should play nice with any setup you have that's a rectangular 2D grid.

Requires the pine.heap module, available from the code archives here:  <a href="codearcs544e.html?code=2970" target="_blank">http://blitzbasic.com/codearcs/codearcs.php?code=2970[/url] (Mirror: <a href="https://dl.dropboxusercontent.com/u/10116881/blitz/code/heap.bmx)" target="_blank">https://dl.dropboxusercontent.com/u/10116881/blitz/code/heap.bmx)[/url]


Code :
Code: BlitzMax
  1. '       --+-----------------------------------------------------------------------------------------+--
  2. '         |   This code was originally written by Sophie Kirschner (sophiek@pineapplemachine.com)   |  
  3. '         | It is released as public domain. Please don't interpret that as liberty to claim credit |  
  4. '         |   that isn't yours, or to sell this code when it could otherwise be obtained for free   |  
  5. '         |                because that would be a really shitty thing of you to do.                |
  6. '       --+-----------------------------------------------------------------------------------------+--
  7.  
  8.  
  9.  
  10. SuperStrict
  11.  
  12. Import pine.Heap ' http://blitzbasic.com/codearcs/codearcs.php?code=2970 MIRROR: https://dl.dropboxusercontent.com/u/10116881/blitz/code/heap.bmx
  13. Import brl.linkedlist
  14. Import brl.math
  15.  
  16. ' GetAstarPath is a generalized A* pathfinder that plays nice with rectangular grids.
  17. ' It returns a list of pathnode objects that are sequential from the start coords to the goal coords. If no path could be found, returns null.
  18.  
  19. ' Mandatory arguments:
  20. ' startx                -       Starting X coord
  21. ' starty                -       Starting Y coord
  22. ' goalx         -       Goal X coord
  23. ' goaly         -       Goal Y coord
  24. ' width         -       Width of the grid space. (e.g. width of the array containing tile data)
  25. ' height                -       Height of the grid space. (e.g. height of the array containing tile data)
  26. ' getcost(x,y)  -       Function to use for calculating cost to traverse a tile. Impassable tiles should return a cost of -1.
  27.  
  28. ' Optional arguments:
  29. ' limitsteps                            -       If the function makes this many iterations without reaching the goal, disregard and return null. (Defaults to -1, which means don't stop.)
  30. ' heuristic(node,goalx,goaly)   -       Function to use for calculating the heuristic from a pathnode object to the goal. Default adds actual and manhattan distances.
  31. ' neighborhood[][]                      -       An array of tuples that tells the pathfinder where something can move from one tile, and the cost multiplier for doing so. Defaults to a Von Neumann neighborhood with all costs *1. (In other words, defaults to checking only directly adjacent spaces.)
  32.  
  33. Global _astar_VonNeumannNeighborhood%[][]=[[-1,0,1],[1,0,1],[0,-1,1],[0,1,1]]
  34.  
  35. Function GetAstarPath:TList(startx%,starty%,goalx%,goaly%,width%,height%,getcost%(x%,y%),limitsteps%=-1,heuristic%(node:pathnode,goalx%,goaly%)=_astar_defheuristic,neighborhood%[][]=Null)
  36.         Return pathtolist(GetAstarPathNode(startx,starty,goalx,goaly,width,height,getcost,limitsteps,heuristic,neighborhood))
  37. End Function
  38.  
  39. Function GetAstarPathNode:pathnode(startx%,starty%,goalx%,goaly%,width%,height%,getcost%(x%,y%),limitsteps%=-1,heuristic%(node:pathnode,goalx%,goaly%)=_astar_defheuristic,neighborhood%[][]=Null)
  40.         If Not neighborhood Then neighborhood=_astar_VonNeumannNeighborhood
  41.         Local start:pathnode=pathnode.Create(startx,starty,Null,goalx,goaly,0,heuristic)
  42.         Local nodeat:pathnode[width,height]
  43.         Local open:THeap=CreateHeap(0,1,6)
  44.         HeapInsert(open,start)
  45.         Local iterations%=0
  46.         Repeat
  47.                 Local curr:pathnode=pathnode(HeapRemove(open))
  48.                 If Not curr Exit
  49.                 If Not curr.open Then Continue
  50.                 If curr.x=goalx And curr.y=goaly Then Return curr
  51.                 curr.open=0
  52.                 nodeat[curr.x,curr.y]=curr
  53.                 For Local i%=0 Until neighborhood.length
  54.                         Local nx%=curr.x+neighborhood[i][0]
  55.                         Local ny%=curr.y+neighborhood[i][1]
  56.                         If nx<0 Or ny<0 Or nx>=width Or ny>=height Then Continue
  57.                         Local costhere%=getcost(nx,ny)
  58.                         If costhere=-1 Then Continue
  59.                         costhere:*neighborhood[i][2]
  60.                         If nodeat[nx,ny] And nodeat[nx,ny].open=0 And curr.gscore+costhere>nodeat[nx,ny].gscore Continue
  61.                         If (nodeat[nx,ny]=Null)
  62.                                 Local n:pathnode=pathnode.Create(nx,ny,curr,goalx,goaly,costhere,heuristic)
  63.                                 HeapInsert open,n
  64.                         ElseIf (nodeat[nx,ny].gscore>curr.gscore+costhere) Then
  65.                                 nodeat[nx,ny].from=curr
  66.                                 nodeat[nx,ny].gscore=curr.gscore+costhere
  67.                                 nodeat[nx,ny].fscore=nodeat[nx,ny].gscore+nodeat[nx,ny].heuristic(nodeat[nx,ny],goalx,goaly)
  68.                                 HeapInsert open,nodeat[nx,ny];nodeat[nx,ny].open=0
  69.                         EndIf
  70.                 Next
  71.                 iterations:+1
  72.                 If limitsteps>=0 And iterations>limitsteps Then Return Null
  73.         Forever
  74.         Return Null
  75. End Function
  76.  
  77. Function pathtolist:TList(lastnode:pathnode)
  78.         Local list:TList=CreateList()
  79.         Local on:pathnode=lastnode
  80.         While on
  81.                 list.addfirst on
  82.                 on=on.from
  83.         Wend
  84.         Return list
  85. End Function
  86.  
  87. Function _astar_defheuristic%(node:pathnode,goalx%,goaly%)
  88.         Local dx%=Abs(node.x-goalx)
  89.         Local dy%=Abs(node.y-goaly)
  90.         Local dist%=Sqr((dx*dx)+(dy*dy))
  91.         Local h%=(dx+dy)
  92.         Return h+dist
  93. End Function
  94.  
  95. Type pathnode
  96.         Field x%,y%
  97.         Field gscore%=0
  98.         Field fscore%=0
  99.         Field from:pathnode=Null
  100.         Field open%=1
  101.         Field heuristic%(node:pathnode,goalx%,goaly%)
  102.         Function Create:pathnode(x%,y%,from:pathnode,goalx%,goaly%,cost%,heuristic%(node:pathnode,goalx%,goaly%))
  103.                 Local n:pathnode=New pathnode
  104.                 n.x=x;n.y=y
  105.                 n.from=from
  106.                 If from Then n.gscore=from.gscore+cost
  107.                 n.heuristic=heuristic
  108.                 n.fscore=n.gscore+n.heuristic(n,goalx,goaly)
  109.                 Return n
  110.         End Function
  111.         Method compare%(o2:Object)
  112.                 Local o:pathnode=pathnode(o2)
  113.                 If fscore>o.fscore
  114.                         Return 1
  115.                 ElseIf fscore<o.fscore
  116.                         Return -1
  117.                 Else
  118.                         Return 0
  119.                 EndIf
  120.         End Method
  121.         Method lastnode:pathnode()
  122.                 Local node:pathnode=Self
  123.                 While node.from
  124.                         node=node.from
  125.                 Wend
  126.                 Return node
  127.         End Method
  128. End Type
  129.  
  130.  
  131.  
  132.  
  133. ' Example program
  134.  
  135. Rem
  136.  
  137. ' stuff for handling the map and the graphics window
  138. Const gw%=256,gh%=256
  139. Const tw%=8,th%=8
  140. Const mw%=gw/tw,mh%=gh/th
  141. Global map%[mw,mh]
  142.  
  143. ' actor class. these guys demonstrate the pathfinder by getting commanded around.
  144. Type actor
  145.         Global list:TList=CreateList(),count%=0
  146.         Global cols%[][]=[[255,0,0],[0,255,0]]
  147.         Field x%,y%,col%
  148.         Field path:TList,pathx%=-1,pathy%=-1
  149.         Function handle()
  150.                 For Local a:actor=EachIn list
  151.                         a.drawpath
  152.                 Next
  153.                 For Local a:actor=EachIn list
  154.                         a.draw
  155.                         a.update
  156.                 Next
  157.         End Function
  158.         Function add:actor(x%,y%,col%)
  159.                 Local a:actor=New actor
  160.                 a.x=x;a.y=y;a.col=col
  161.                 list.addlast a;count:+1
  162.                 Return a
  163.         End Function
  164.         Method pathto(goalx%,goaly%)
  165.                 path=GetAstarPath(x,y,goalx,goaly,mw,mh,getmapcost)',mw*mh)
  166.                 If path Then pathx=goalx;pathy=goaly
  167.         End Method
  168.         Method draw()
  169.                 SetColor cols[col][0],cols[col][1],cols[col][2]
  170.                 DrawRect x*tw,y*th,tw,th
  171.         End Method
  172.         Method drawpath()
  173.                 If Not path Then Return
  174.                 SetColor cols[col][0]/4,cols[col][1]/4,cols[col][2]/4
  175.                 For Local node:pathnode=EachIn path
  176.                         DrawRect node.x*tw,node.y*th,tw,th
  177.                 Next
  178.         End Method
  179.         Method update()
  180.                 If path Then
  181.                         Local node:pathnode=pathnode(path.first())
  182.                         If node Then
  183.                                 If map[node.x,node.y] And (pathx>=0 And pathy>=0) Then
  184.                                         pathto pathx,pathy
  185.                                         If Not path Then pathx=-1;pathy=-1
  186.                                 Else
  187.                                         x=node.x;y=node.y
  188.                                         path.removefirst
  189.                                 EndIf
  190.                         Else
  191.                                 path=Null;pathx=-1;pathy=-1
  192.                         EndIf
  193.                 EndIf
  194.         End Method
  195. End Type
  196.  
  197. ' this is for the getcost(x,y) argument of GetAstarPath
  198. Function getmapcost%(x%,y%)
  199.         If map[x,y] Return -1
  200.         Return 1
  201. End Function
  202.  
  203. ' make the graphics window and set up some misc. globals
  204. Graphics gw,gh
  205. Global drawmode%=-1
  206. Global commandactor%=0
  207. Global clicked%=0
  208.  
  209. ' make the actors
  210. actor.add 2,2,0
  211. actor.add mw-3,mh-3,1
  212.  
  213. ' main loop
  214. Repeat
  215.         Cls
  216.        
  217.         ' get mouse position on the map grid
  218.         Local mx%=MouseX()/tw,my%=MouseY()/th
  219.        
  220.         ' handle drawing with left click
  221.         If MouseDown(1) And (mx>=0 And my>=0 And mx<mw And my<mh) Then
  222.                 If drawmode=-1 Then drawmode=Not map[mx,my]
  223.                 map[mx,my]=drawmode
  224.                 clicked=1
  225.         Else
  226.                 drawmode=-1
  227.         EndIf
  228.        
  229.         ' handle commanding actors with right click
  230.         If MouseHit(2) Then
  231.                 For Local a:actor=EachIn actor.list
  232.                         If a.col=(commandactor Mod actor.count) Then
  233.                                 a.pathto mx,my
  234.                                 Exit
  235.                         EndIf
  236.                 Next
  237.                 commandactor:+1
  238.                 clicked=1
  239.         EndIf
  240.        
  241.         ' draw the map grid
  242.         SetColor 255,255,255
  243.         For Local i%=0 Until mw
  244.                 For Local j%=0 Until mh
  245.                         If map[i,j] Then DrawRect i*tw,j*th,tw,th
  246.                 Next
  247.         Next
  248.        
  249.         ' update and draw the actors
  250.         actor.handle
  251.        
  252.         ' draw a little instructional message that disappears once you click somewhere
  253.         SetColor 255,255,255
  254.         If Not clicked Then
  255.                 DrawText "LMB to draw/erase walls.",2,230
  256.                 DrawText "RMB to pathfind.",2,242
  257.         EndIf
  258.        
  259.         Flip
  260.         Delay 20
  261. Forever
  262.  
  263. EndRem


Comments :


GW(Posted 1+ years ago)

 This has some serious bugs. most of the time it will hang and chew through all the available memory.


Pineapple(Posted 1+ years ago)

 Would you elaborate? I tested thoroughly and didn't run into any issues. Nor should memory usage be a problem as long as the grid isn't inordinately large or complex.


GW(Posted 1+ years ago)

 If i remember correctly, just doubling or tripling the map size should reveal the issue.  
Code: [Select]
Const gw%=256*2,gh%=256*2


Pineapple(Posted 1+ years ago)

 Ah, shit, I see what I've done. I neglected to include a separate list of unvisited nodes from the heap. It's doing a ton of array resizing behind the scenes there. I'll have to get around to fixing that. [/i]

 

SimplePortal 2.3.6 © 2008-2014, SimplePortal