A-Star Pathfinding

-=+=- -=+=- -=+=- -=+=- -=+=- -=+=- -=+=- -=+=- -=+=- -=+=- -=+=- -=+=- -=+=- -=+=- -=+=- -=+=- -=+=- -=+=- -=+=- -=+=- -=+=- -=+=- -=+=- -=+=- -=+=- -=+=- -=+=- -=+=- -=+=- -=+=- (c) WidthPadding Industries 1987 0\|718\|0 -=+=- -=+=- -=+=- -=+=- -=+=- -=+=- -=+=- -=+=- -=+=- -=+=- -=+=- -=+=- -=+=- -=+=- -=+=- -=+=- -=+=- -=+=- -=+=- -=+=- -=+=- -=+=- -=+=- -=+=- -=+=- -=+=- -=+=- -=+=- -=+=- -=+=-
SoCoder -> Snippet Home -> AI and Movement

mike_g	Created : 06 August 2007 Edited : 11 June 2008 Language : Blitz A-Star Pathfinding
Heres some AI code for pathfinding. If you want to know a bit more about it, heres the tutorial i worked off to create this. Its a pretty good explanation. Graphics 640, 480, 32, 2 Global font = LoadFont(arial, 10) SetFont font ;------------Globals----------------; Const x_cells=20 Const y_cells=16 Const all_cells = x_cells * y_cells Const PATH_MAX=100 Dim grid(all_cells) Type open Field grid_pos Field parent Field score Field move_cost End Type Type closed Field grid_pos Field parent Field score Field move_cost End Type Dim path(PATH_MAX) ;------------Init Grid--------------; For i = 0 To all_cells-1 Read grid(i) Next DrawGrid() PathFind(141, 186) DrawPath() WaitKey ;-----------------------------------------------------------------------------------------------; ;*************************** PATH FINDING FUNCTIONS ***************************************; ;-----------------------------------------------------------------------------------------------; Function PathFind(orig, dest) ;CREATE START POINT this.open = New open this\grid_pos = orig this\parent=-1 this\score=0 this\move_cost=0 Repeat ;FIND LOWEST SCORE lowest.open = First open For this.open = Each open If this\score < lowest\score Then lowest = this Next ;IF TARGET REACHED If lowest\grid_pos = dest GetPath(lowest.open) Return True EndIf ;CHECK EACH ADJACENT SQUARE TO TILE For y=-x_cells To x_cells Step x_cells For x=-1 To 1 new_pos = lowest\grid_pos+x+y If(PathIsValid(lowest\grid_pos, x, y, new_pos)=True) ;GET MOVE COST TO ADJACENT TILE If(y <> 0 And x <> 0) move_cost = grid(new_pos)+(grid(new_pos)0.4) Else move_cost = grid(new_pos) EndIf ;GUESS MOVE COST FROM TILE TO TARGET ;need a better method dx#=Abs((dest Mod x_cells)-(new_pos Mod x_cells)-1) dy#=Abs((dest / x_cells)-(new_pos / x_cells)) distance = Sqr((dxdx)+(dydy))10 goodness = distance + move_cost +lowest\move_cost ;CHECK IF DESTINATION IS ON OPEN LIST check=0 For test.open = Each open If test\grid_pos = new_pos If test\score < goodness check =1 Else Delete test.open EndIf Exit EndIf Next ;IF NOT ADD TO OPEN LIST If check = 0 create.open = New open create\parent = lowest\grid_pos create\move_cost = lowest\move_cost + move_cost create\grid_pos = new_pos create\score = goodness EndIf ;DEBUG TEXT - DEMO STUFF-------------------------------------------------, Color 200, 0, 0 ; Text(((new_pos Mod x_cells)32)+22, ((new_pos / x_cells)32)+2, move_cost) Color 0, 200, 0 Text(((new_pos Mod x_cells)32)+18, ((new_pos / x_cells)32)+23, distance) Color 0, 0, 200 If check = 0 Text(((new_pos Mod x_cells)32)+2, ((new_pos / x_cells)32)+23, create\score) EndIf Delay(50) ; ;------------------------------------------------------------------------' EndIf Next Next ;MOVE CURRENT TILE FROM OPEN LIST TO CLOSED LIST cl.closed = New closed cl\grid_pos = lowest\grid_pos cl\parent = lowest\parent cl\score = lowest\score Delete lowest.open Forever End Function ;-----------------------------------------------------; Function GetPath(lowest.open) waypoint = lowest\parent count = 0 While waypoint <> -1 For c.closed = Each closed If c\grid_pos = waypoint Then Exit Next path(count)=c\grid_pos waypoint=c\parent count=count+1 Wend End Function ;-----------------------------------------------------; Function PathIsValid(this_pos, x, y, pos) If x=0 And y=0 Then Return False If pos < 0 Or pos >= all_cells Then Return False If this_pos Mod x_cells = 0 And pos Mod x_cells = x_cells-1 Then Return False If this_pos Mod x_cells = x_cells-1 And pos Mod x_cells = 0 Then Return False If(PathIsBlocked(pos)=True) Then Return False For cl.closed = Each closed If cl\grid_pos = pos Return False Next Return True End Function ;--------OBSTRUCTING TILE REFERENCES GO IN HERE-------; Function PathIsBlocked(pos) If grid(pos) = -1 Return True Return False End Function ;-----------------------------------------------------------------------------------------------; ;************************** DEMO FUNCTIONS ***********************************************; ;-----------------------------------------------------------------------------------------------; Function DrawGrid() For x=0 To 640 Step 32 For y=0 To 480 Step 32 Color 255, 255, 255 Line x, 0, x, 480 Line 0, y, 640, y tile=grid(((y/32)x_cells)+(x/32)) If tile = -1 Color 80, 80, 80 Rect x, y, 32, 32, 1 Else If tile = -2 Color 120, 30, 30 Rect x, y, 32, 32, 1 Else If tile = -3 Color 30, 120, 30 Rect x, y, 32, 32, 1 Else Text x+3, y+2, tile EndIf Next Next End Function Function DrawPath() While path(i) <> 0 Color 200,200, 50 Oval (path(i) Mod x_cells)32+12, (path(i) / x_cells)32+12, 8, 8, 1 Color 20,20, 100 Text (path(i) Mod x_cells)32+14, (path(i) / x_cells)32+11, i i=i+1 Wend End Function ;-----------------------------------------------------------------------------------------------; ;***********************************************************************************************; ;-----------------------------------------------------------------------------------------------; Data 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10 Data 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10 Data 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10 Data 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10 Data 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10 Data 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10 Data 10, 10, 10, 10, -1, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10 Data 10, -2, 10, 10, -1, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10 Data 10, -1, -1, -1, -1, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10 Data 10, 10, 10, 10, 10, -3, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10 Data 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10 Data 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10 Data 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10 Data 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10 Data 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10 Data 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10 --v

Comments
Wednesday, 11 June 2008, 17:16
mike_g	The original code I posted for this was borked. I just edited it so it always finds the shortest path now.
Thursday, 12 June 2008, 08:46
HoboBen	Oh cool, I was just going to have a go at path-finding today! Thanks Mike, this will be a great starting point for a Cobra version
Thursday, 12 June 2008, 11:13
mike_g	Cheers ben. If you want to learn for yourself how it works heres the tutorial I made my version off: https://www.policyalmanac.org/games/aStarTutorial.htm I also have a dijkstras version, which I'll post if I get around to untangling the code from a game I made. As with that pathfinding algo you don't need to know the target before you begin the search.
Thursday, 12 June 2008, 12:29
JL235	I recently learnt Dijkstras shortest path for a module on graphs. It's quite a simple algorithm and I can see it being more useful then for just games.
Thursday, 12 June 2008, 13:10
mike_g	Yeah, Dijksra's algo is also used by routing protocols and probably loads of other things. Its very similar to A*, the only difference is that it does not use a heuristic to optimise the pathfinding.
Thursday, 12 June 2008, 14:53
Jayenkai	I think I've used Pathfinding, probably once! Most of the time I just try to avoid anything that needs it But this could come in handy, if I ever find a need for it.
Thursday, 12 June 2008, 18:38
Orion Pax	You are so wonderful Mike! This will come in great for my game. I planned on putting AI in my game much later. And I still do plan on doing it much later. I might put 1 bot in now or something....maybe...dunno. But thanks either way! I just realized that this is in 2d. I wonder how hard it would be to convert it to 3d?
Friday, 13 June 2008, 16:41
mike_g	Thanks guys. Orion, It shouldent be too hard to convert to 3d, but if its for your space game you might be able to get away with something much simpler/faster. As theres generally not a lot of obstacles in space. Pathfinding can be quite intensive. In realtime games for objects off-screen you can generally get away with the mindless walk to point approach. The only problem with that is when you have enclaves that things can get stuck in.
Friday, 13 June 2008, 20:05
JL235	Mike_G Pathfinding can be quite intensive. In realtime games for objects off-screen you can generally get away with the mindless walk to point approach. The only problem with that is when you have enclaves that things can get stuck in. Even if it's expensive I'd have imagined that in say an RTS it would only be needed to be calculated when a unit picks a new destination. Over time I could see this occurrence easily averaging to be far less then once a frame. For those moments when the player sends 10000 units to go somewhere you could have them all walk mindlessly and queue their path finding requests. The calculations for these could be run over the next few frames so say only 10 units paths are calculated per frame. This would help keep the frame rate consistent.
Saturday, 14 June 2008, 06:14
HoboBen	Another alternative, which I think is commonly used, is if you want to go for example from one end of the map to the other, you do it roughly with really large squares, and then you use the more accurate method to find a path to the next point along the rough path. As you reach each point on the rough path, you calculate the accurate path to the next point again.
Saturday, 14 June 2008, 13:56
mike_g	For those moments when the player sends 10000 units to go somewhere you could have them all walk mindlessly and queue their path finding requests. The problem with that scenario is that the units are all moving and will get in one-anothers way. I heard that there are algos better suited for this type of thing than A*. I just dont know their names. Another alternative, which I think is commonly used, is if you want to go for example from one end of the map to the other, you do it roughly with really large squares, and then you use the more accurate method to find a path to the next point along the rough path. As you reach each point on the rough path, you calculate the accurate path to the next point again. Yeah, thats another option. It just requires a bit more coding. I look forward to seeing your cobra version
Tuesday, 17 June 2008, 14:10
Paul	"For those moments when the player sends 10000 units to go somewhere you could have them all walk mindlessly and queue their path finding requests." One way to that could be th handle the units as a group. And by the way, here is some pathfinding code i wrote a while ago. leeks memory as shit though If anyone sees anything that might cause it, please tell me Press 1 to generate a path from a random position to another Press 2 to keep generating. The memory usage goes up with every path that is created, and the speed goes down Type PathNode Field x,y Field NextNode.PathNode End Type Type Square Field x,y Field F Field G Field H Field n Field n2 Field Previous.Square End Type Dim Open.Square(mapx,mapy);where all th open squares are saved Dim Closed(mapx,mapy) ;Keeping track of empty,open and closed squares ;[Block];twoX=Two^X-1 Const two0=0 Const two1=1 Const two2=3 Const two3=7 Const two4=15 Const two5=31 Const two6=63 Const two7=127 Const two8=255 Const two9=511 Const two10=1023 Const two11=(two10+1)2-1 Const two12=(two11+1)2-1 Const two13=(two12+1)2-1 Const two14=(two13+1)2-1 Const two15=(two14+1)2-1 Const two16=(two15+1)2-1 Const two17=(two16+1)2-1 Const two18=(two17+1)2-1 Const two19=(two18+1)2-1 Const two20=(two19+1)2-1 ;[End Block] ;[Block];tX=Two^X Const t0 =2^0 Const t1 =2^1 Const t2 =2^2 Const t3 =2^3 Const t4 =2^4 Const t5 =2^5 Const t6 =2^6 Const t7 =2^7 Const t8 =2^8 Const t9 =2^9 Const t10=2^10 Const t11=2^11 Const t12=2^12 Const t13=2^13 Const t14=2^14 Const t15=2^15 Const t16=2^16 Const t17=2^17 Const t18=2^18 Const t19=2^19 Const t20=2^20 ;[End Block] Dim ps(20);Calculate 2^x and save it for for loops For i=0 To 20 ps(i)=2^i Next Dim Fs (two20);some array to store the sorted open squares Dim afs2(two19);stor how many open squares under a fcost(F) value Const maxu1=2 ;How many F valuse can be the same Dim afs.Square(two19,maxu1);The actual array where the opens are stored Global TargetX,TargetY Global Penalty=100;the avrage cost to go anywhere(sqrt method) Global clean;Time it took to clean all the stuff after a search Const mapx=800-1 Const mapy=600-1 Dim map(mapx,mapy) Test() Function Test() SeedRnd(MilliSecs()) Graphics 800,600,32,2 SetBuffer BackBuffer() For x=0 To mapx;Generate a map For y=0 To mapy map(x,y)=Rand(0,200) If map(x,y)<5 Then map(x,y)=0 Next Next For i=0 To 100 Sx=Rand(0,mapx) Sy=Rand(0,mapy) Ex=sx+Rand(-100,100) Ey=sy+Rand(-100,100) If ex<0 Then Ex=0 If ex>mapx Then Ex=mapx If ey<0 Then Ey=0 If ey>mapy Then Ey=mapy If sx>ex Then tmp=ex:Ee=sx:sx=tmp If sy>ey Then tmp=ey:Ee=sy:sy=tmp For x=sx To ex For y=sy To ey map(x,y)=0 Next Next Next Local a.PathNode,b.PathNode While Not KeyHit(1) LockBuffer ;Draw the map For x=0 To mapx For y=0 To mapy Select True Case map(x,y)=0 c=$ff0000 Default c=$ffffff End Select WritePixelFast x,y,c Next Next UnlockBuffer Color 0,0,0 For a.PathNode=Each PathNode Rect a\x-1,a\y-1,3,3 Next Color 0,255,0 Rect startx-5,starty-5,11,11 Color 0,0,255 Rect Endx-5,endy-5,11,11 Color 0,0,0 Text startx,starty,"S",1,1 Color 255,255,255 Text endx,endy,"E",1,1 mx=MouseX() my=MouseY() If MouseDown(1);add walls For x=-2 To 2 For y=-2 To 2 If MX+x=>0 And MX+x=<mapx And MY+y=>0 And MY+y=<mapy map(MX+x,MY+y)=0 EndIf Next Next EndIf If MouseDown(2);Delete walls For x=-2 To 2 For y=-2 To 2 If MX+x=>0 And MX+x=<mapx And MY+y=>0 And MY+y=<mapy map(MX+x,MY+y)=Rand(0,200) EndIf Next Next EndIf If KeyHit(3) Then r=Not r If KeyHit(2) Or r Delete Each PathNode t=MilliSecs() startx=Rand(0,mapx) starty=Rand(0,mapy) endx=Rand(0,mapx) endy=Rand(0,mapy) b=PathFind(startx,starty,endx,endy);454;214;19 AppTitle "Total: "+(MilliSecs()-t)+" Cleaning: "+clean EndIf Flip Cls Wend End Function Function PathFind.PathNode(Startx,StartY,TX,TY) If map(Startx,StartY)=0 Or map(TX,TY)=0 Then Return Null;make sure the goal is actually reachable TargetX=TX TargetY=TY Local CurrentX=Startx Local CurrentY=StartY Local a.Square Local minf Open(CurrentX,CurrentY)=New Square;Create the first square and add ti to the open list, it will be closed the first thing that happens, its just to make sure everyting is set as it should Open(CurrentX,CurrentY)\x=Startx Open(CurrentX,CurrentY)\y=StartY Closed(CurrentX,CurrentY)=2 Open(CurrentX,CurrentY)\F=10 f=10 afs(f,0)=a afs2(f)=1 ;[Block] update + ;Update the binary array thing p=t19+((f And t18) Shr 17-1) Shl 18 Fs(p)=Fs(p)+1 p=p +((F And t17) Shr 16-1) Shl 17 Fs(p)=Fs(p)+1 p=p +((F And t16) Shr 15-1) Shl 16 Fs(p)=Fs(p)+1 p=p +((F And t15) Shr 14-1) Shl 15 Fs(p)=Fs(p)+1 p=p +((F And t14) Shr 13-1) Shl 14 Fs(p)=Fs(p)+1 p=p +((F And t13) Shr 12-1) Shl 13 Fs(p)=Fs(p)+1 p=p +((F And t12) Shr 11-1) Shl 12 Fs(p)=Fs(p)+1 p=p +((F And t11) Shr 10-1) Shl 11 Fs(p)=Fs(p)+1 p=p +((F And t10) Shr 9 -1) Shl 10 Fs(p)=Fs(p)+1 p=p +((F And t9 ) Shr 8 -1) Shl 9 Fs(p)=Fs(p)+1 p=p +((F And t8 ) Shr 7 -1) Shl 8 Fs(p)=Fs(p)+1 p=p +((F And t7 ) Shr 6 -1) Shl 7 Fs(p)=Fs(p)+1 p=p +((F And t6 ) Shr 5 -1) Shl 6 Fs(p)=Fs(p)+1 p=p +((F And t5 ) Shr 4 -1) Shl 5 Fs(p)=Fs(p)+1 p=p +((F And t4 ) Shr 3 -1) Shl 4 Fs(p)=Fs(p)+1 p=p +((F And t3 ) Shr 2 -1) Shl 3 Fs(p)=Fs(p)+1 p=p +((F And t2 ) Shr 1 -1) Shl 2 Fs(p)=Fs(p)+1 p=p +((F And t1 ) -1) Shl 1 Fs(p)=Fs(p)+1 p=p +((F And t0 ) Shl 1 -1) Fs(p)=Fs(p)+1 ;[End Block] Local o.Square Local b.PathNode,c.PathNode Repeat o=Open(CurrentX,CurrentY);Do the closing of a square afs2(o\F)=afs2(o\F)-1 afs(o\F,o\n)=Null ;[Block];Update - p=t19+((o\F And t18) Shr 17-1) Shl 18 Fs(p)=Fs(p)-1 p=p +((o\F And t17) Shr 16-1) Shl 17 Fs(p)=Fs(p)-1 p=p +((o\F And t16) Shr 15-1) Shl 16 Fs(p)=Fs(p)-1 p=p +((o\F And t15) Shr 14-1) Shl 15 Fs(p)=Fs(p)-1 p=p +((o\F And t14) Shr 13-1) Shl 14 Fs(p)=Fs(p)-1 p=p +((o\F And t13) Shr 12-1) Shl 13 Fs(p)=Fs(p)-1 p=p +((o\F And t12) Shr 11-1) Shl 12 Fs(p)=Fs(p)-1 p=p +((o\F And t11) Shr 10-1) Shl 11 Fs(p)=Fs(p)-1 p=p +((o\F And t10) Shr 9 -1) Shl 10 Fs(p)=Fs(p)-1 p=p +((o\F And t9 ) Shr 8 -1) Shl 9 Fs(p)=Fs(p)-1 p=p +((o\F And t8 ) Shr 7 -1) Shl 8 Fs(p)=Fs(p)-1 p=p +((o\F And t7 ) Shr 6 -1) Shl 7 Fs(p)=Fs(p)-1 p=p +((o\F And t6 ) Shr 5 -1) Shl 6 Fs(p)=Fs(p)-1 p=p +((o\F And t5 ) Shr 4 -1) Shl 5 Fs(p)=Fs(p)-1 p=p +((o\F And t4 ) Shr 3 -1) Shl 4 Fs(p)=Fs(p)-1 p=p +((o\F And t3 ) Shr 2 -1) Shl 3 Fs(p)=Fs(p)-1 p=p +((o\F And t2 ) Shr 1 -1) Shl 2 Fs(p)=Fs(p)-1 p=p +((o\F And t1 ) -1) Shl 1 Fs(p)=Fs(p)-1 p=p +((o\F And t0 ) Shl 1 -1) Fs(p)=Fs(p)-1 ;[End Block] Closed(CurrentX,CurrentY)=2 If Closed(TargetX,TargetY)=2;!if the target is found we have reached the goal and have to do some cleaning ;[Block];create the path o=Open(TargetX,TargetY) While o<>Null b.PathNode=New PathNode b\x=o\x b\y=o\y b\NextNode=c c=b o=o\Previous Wend ;[End Block] ;[Block]Clean tmp=MilliSecs() For o.Square=Each Square Closed(o\x,o\y)=0 Closed(o\y,o\y)=0 afs2(o\F)=0 p=t19+((o\F And t18) Shr 17-1) Shl 18 Fs(p)=0 p=p +((o\F And t17) Shr 16-1) Shl 17 Fs(p)=0 p=p +((o\F And t16) Shr 15-1) Shl 16 Fs(p)=0 p=p +((o\F And t15) Shr 14-1) Shl 15 Fs(p)=0 p=p +((o\F And t14) Shr 13-1) Shl 14 Fs(p)=0 p=p +((o\F And t13) Shr 12-1) Shl 13 Fs(p)=0 p=p +((o\F And t12) Shr 11-1) Shl 12 Fs(p)=0 p=p +((o\F And t11) Shr 10-1) Shl 11 Fs(p)=0 p=p +((o\F And t10) Shr 9 -1) Shl 10 Fs(p)=0 p=p +((o\F And t9 ) Shr 8 -1) Shl 9 Fs(p)=0 p=p +((o\F And t8 ) Shr 7 -1) Shl 8 Fs(p)=0 p=p +((o\F And t7 ) Shr 6 -1) Shl 7 Fs(p)=0 p=p +((o\F And t6 ) Shr 5 -1) Shl 6 Fs(p)=0 p=p +((o\F And t5 ) Shr 4 -1) Shl 5 Fs(p)=0 p=p +((o\F And t4 ) Shr 3 -1) Shl 4 Fs(p)=0 p=p +((o\F And t3 ) Shr 2 -1) Shl 3 Fs(p)=0 p=p +((o\F And t2 ) Shr 1 -1) Shl 2 Fs(p)=0 p=p +((o\F And t1 ) -1) Shl 1 Fs(p)=0 p=p +((o\F And t0 ) Shl 1 -1) Fs(p)=0 s=s+1 Delete o Next ;[End Block] clean=(MilliSecs()-tmp) DebugLog s/Float(clean) Return b EndIf GetOpens1(CurrentX,CurrentY,CurrentX-1,CurrentY-1);Check the squares, caluculate f and g and update already checked squares if necessary GetOpens1(CurrentX,CurrentY,CurrentX+1,CurrentY-1) GetOpens1(CurrentX,CurrentY,CurrentX-1,CurrentY+1) GetOpens1(CurrentX,CurrentY,CurrentX+1,CurrentY+1) GetOpens2(CurrentX,CurrentY,CurrentX,CurrentY+1) GetOpens2(CurrentX,CurrentY,CurrentX,CurrentY-1) GetOpens2(CurrentX,CurrentY,CurrentX+1,CurrentY) GetOpens2(CurrentX,CurrentY,CurrentX-1,CurrentY) If Fs(t19+t18)+Fs(t19-t18)=0 ;Woot? we didnt find the goal?? tmp=MilliSecs();anyway we need to do some cleaning Delete Each Square Dim Closed(mapx,mapy) Dim Fs(two20) Dim afs2(two19) clean=(MilliSecs()-tmp) Return Null;are tere any open squares? EndIf p=t19;Searh for the lowest fcost open square For i=18 To 0 Step -1 If Fs(p-ps(i))>0 p=p-ps(i) Else p=p+ps(i) EndIf Next CurrentX=afs(p Shr 1,afs2(p Shr 1)-1)\x CurrentY=afs(p Shr 1,afs2(p Shr 1)-1)\y Forever End Function Function GetOpens1(CurrentX,CurrentY,x,y) If x<0 Or x>mapx Or y<0 Or y>mapy Then Return 0 If map(x,y)=0 Then Return 0 Local a.Square If Closed(x,y)=0;empty a=New Square Open(x,y)=a a\G=Open(CurrentX,CurrentY)\G+map(x,y)1.414 ;a\H=100(Abs(x-TargetX)+Abs(y-TargetY)) a\H=PenaltySqr((x-TargetX)^2+(y-TargetY)^2) a\F=a\G+a\H a\Previous=Open(CurrentX,CurrentY) Closed(x,y)=1 a\x=x a\y=y ;[Block];Find n for f. Update f and n a\n=afs2(a\F) While a\n>maxu1 a\F=a\F+1 a\n=afs2(a\F) Wend afs(a\F,a\n)=a afs2(a\F)=a\n+1 ;[End Block] ;[Block];Update + p=t19+((a\F And t18) Shr 17-1) Shl 18 Fs(p)=Fs(p)+1 p=p +((a\F And t17) Shr 16-1) Shl 17 Fs(p)=Fs(p)+1 p=p +((a\F And t16) Shr 15-1) Shl 16 Fs(p)=Fs(p)+1 p=p +((a\F And t15) Shr 14-1) Shl 15 Fs(p)=Fs(p)+1 p=p +((a\F And t14) Shr 13-1) Shl 14 Fs(p)=Fs(p)+1 p=p +((a\F And t13) Shr 12-1) Shl 13 Fs(p)=Fs(p)+1 p=p +((a\F And t12) Shr 11-1) Shl 12 Fs(p)=Fs(p)+1 p=p +((a\F And t11) Shr 10-1) Shl 11 Fs(p)=Fs(p)+1 p=p +((a\F And t10) Shr 9 -1) Shl 10 Fs(p)=Fs(p)+1 p=p +((a\F And t9 ) Shr 8 -1) Shl 9 Fs(p)=Fs(p)+1 p=p +((a\F And t8 ) Shr 7 -1) Shl 8 Fs(p)=Fs(p)+1 p=p +((a\F And t7 ) Shr 6 -1) Shl 7 Fs(p)=Fs(p)+1 p=p +((a\F And t6 ) Shr 5 -1) Shl 6 Fs(p)=Fs(p)+1 p=p +((a\F And t5 ) Shr 4 -1) Shl 5 Fs(p)=Fs(p)+1 p=p +((a\F And t4 ) Shr 3 -1) Shl 4 Fs(p)=Fs(p)+1 p=p +((a\F And t3 ) Shr 2 -1) Shl 3 Fs(p)=Fs(p)+1 p=p +((a\F And t2 ) Shr 1 -1) Shl 2 Fs(p)=Fs(p)+1 p=p +((a\F And t1 ) -1) Shl 1 Fs(p)=Fs(p)+1 p=p +((a\F And t0 ) Shl 1 -1) Fs(p)=Fs(p)+1 ;[End Block] ElseIf Closed(x,y)=1;open a=Open(x,y) If a\G>Open(CurrentX,CurrentY)\G+map(x,y)1.414 a\G=Open(CurrentX,CurrentY)\G+map(x,y)1.414 a\Previous=Open(CurrentX,CurrentY) afs2(a\F)=afs2(a\F)-1 afs(a\F,a\n)=afs(a\F,afs2(a\F)) If afs(a\F,a\n)<>Null Then afs(a\F,a\n)\n=a\n ;[Block];Update - p=t19+((a\F And t18) Shr 17-1) Shl 18 Fs(p)=Fs(p)-1 p=p +((a\F And t17) Shr 16-1) Shl 17 Fs(p)=Fs(p)-1 p=p +((a\F And t16) Shr 15-1) Shl 16 Fs(p)=Fs(p)-1 p=p +((a\F And t15) Shr 14-1) Shl 15 Fs(p)=Fs(p)-1 p=p +((a\F And t14) Shr 13-1) Shl 14 Fs(p)=Fs(p)-1 p=p +((a\F And t13) Shr 12-1) Shl 13 Fs(p)=Fs(p)-1 p=p +((a\F And t12) Shr 11-1) Shl 12 Fs(p)=Fs(p)-1 p=p +((a\F And t11) Shr 10-1) Shl 11 Fs(p)=Fs(p)-1 p=p +((a\F And t10) Shr 9 -1) Shl 10 Fs(p)=Fs(p)-1 p=p +((a\F And t9 ) Shr 8 -1) Shl 9 Fs(p)=Fs(p)-1 p=p +((a\F And t8 ) Shr 7 -1) Shl 8 Fs(p)=Fs(p)-1 p=p +((a\F And t7 ) Shr 6 -1) Shl 7 Fs(p)=Fs(p)-1 p=p +((a\F And t6 ) Shr 5 -1) Shl 6 Fs(p)=Fs(p)-1 p=p +((a\F And t5 ) Shr 4 -1) Shl 5 Fs(p)=Fs(p)-1 p=p +((a\F And t4 ) Shr 3 -1) Shl 4 Fs(p)=Fs(p)-1 p=p +((a\F And t3 ) Shr 2 -1) Shl 3 Fs(p)=Fs(p)-1 p=p +((a\F And t2 ) Shr 1 -1) Shl 2 Fs(p)=Fs(p)-1 p=p +((a\F And t1 ) -1) Shl 1 Fs(p)=Fs(p)-1 p=p +((a\F And t0 ) Shl 1 -1) Fs(p)=Fs(p)-1 ;[End Block] a\F=a\G+a\H ;[Block];Find n for new f. Update f and n a\n=afs2(a\F) While a\n>maxu1 a\F=a\F+1 a\n=afs2(a\F) Wend afs(a\F,a\n)=a afs2(a\F)=a\n+1 ;[End Block] ;[Block];Update + p=t19+((a\F And t18) Shr 17-1) Shl 18 Fs(p)=Fs(p)+1 p=p +((a\F And t17) Shr 16-1) Shl 17 Fs(p)=Fs(p)+1 p=p +((a\F And t16) Shr 15-1) Shl 16 Fs(p)=Fs(p)+1 p=p +((a\F And t15) Shr 14-1) Shl 15 Fs(p)=Fs(p)+1 p=p +((a\F And t14) Shr 13-1) Shl 14 Fs(p)=Fs(p)+1 p=p +((a\F And t13) Shr 12-1) Shl 13 Fs(p)=Fs(p)+1 p=p +((a\F And t12) Shr 11-1) Shl 12 Fs(p)=Fs(p)+1 p=p +((a\F And t11) Shr 10-1) Shl 11 Fs(p)=Fs(p)+1 p=p +((a\F And t10) Shr 9 -1) Shl 10 Fs(p)=Fs(p)+1 p=p +((a\F And t9 ) Shr 8 -1) Shl 9 Fs(p)=Fs(p)+1 p=p +((a\F And t8 ) Shr 7 -1) Shl 8 Fs(p)=Fs(p)+1 p=p +((a\F And t7 ) Shr 6 -1) Shl 7 Fs(p)=Fs(p)+1 p=p +((a\F And t6 ) Shr 5 -1) Shl 6 Fs(p)=Fs(p)+1 p=p +((a\F And t5 ) Shr 4 -1) Shl 5 Fs(p)=Fs(p)+1 p=p +((a\F And t4 ) Shr 3 -1) Shl 4 Fs(p)=Fs(p)+1 p=p +((a\F And t3 ) Shr 2 -1) Shl 3 Fs(p)=Fs(p)+1 p=p +((a\F And t2 ) Shr 1 -1) Shl 2 Fs(p)=Fs(p)+1 p=p +((a\F And t1 ) -1) Shl 1 Fs(p)=Fs(p)+1 p=p +((a\F And t0 ) Shl 1 -1) Fs(p)=Fs(p)+1 ;[End Block] EndIf Else;Closed a=Open(x,y) If a\G>Open(CurrentX,CurrentY)\G+map(x,y)1.414 a\G=Open(CurrentX,CurrentY)\G+map(x,y)1.414 a\Previous=Open(CurrentX,CurrentY) EndIf EndIf ;Return 0 End Function Function GetOpens2(CurrentX,CurrentY,x,y) If x<0 Or x>mapx Or y<0 Or y>mapy Then Return 0 If map(x,y)=0 Then Return 0 Local a.Square If Closed(x,y)=0;empty a=New Square Open(x,y)=a a\G=Open(CurrentX,CurrentY)\G+map(x,y) ;a\H=100(Abs(x-TargetX)+Abs(y-TargetY)) a\H=PenaltySqr((x-TargetX)^2+(y-TargetY)^2) a\F=a\G+a\H a\Previous=Open(CurrentX,CurrentY) Closed(x,y)=1 a\x=x a\y=y ;[Block];Find n for f. Update f and n a\n=afs2(a\F) While a\n>maxu1 a\F=a\F+1 a\n=afs2(a\F) Wend afs(a\F,a\n)=a afs2(a\F)=afs2(a\F)+1 ;[End Block] ;[Block];Update + p=t19+((a\F And t18) Shr 17-1) Shl 18 Fs(p)=Fs(p)+1 p=p +((a\F And t17) Shr 16-1) Shl 17 Fs(p)=Fs(p)+1 p=p +((a\F And t16) Shr 15-1) Shl 16 Fs(p)=Fs(p)+1 p=p +((a\F And t15) Shr 14-1) Shl 15 Fs(p)=Fs(p)+1 p=p +((a\F And t14) Shr 13-1) Shl 14 Fs(p)=Fs(p)+1 p=p +((a\F And t13) Shr 12-1) Shl 13 Fs(p)=Fs(p)+1 p=p +((a\F And t12) Shr 11-1) Shl 12 Fs(p)=Fs(p)+1 p=p +((a\F And t11) Shr 10-1) Shl 11 Fs(p)=Fs(p)+1 p=p +((a\F And t10) Shr 9 -1) Shl 10 Fs(p)=Fs(p)+1 p=p +((a\F And t9 ) Shr 8 -1) Shl 9 Fs(p)=Fs(p)+1 p=p +((a\F And t8 ) Shr 7 -1) Shl 8 Fs(p)=Fs(p)+1 p=p +((a\F And t7 ) Shr 6 -1) Shl 7 Fs(p)=Fs(p)+1 p=p +((a\F And t6 ) Shr 5 -1) Shl 6 Fs(p)=Fs(p)+1 p=p +((a\F And t5 ) Shr 4 -1) Shl 5 Fs(p)=Fs(p)+1 p=p +((a\F And t4 ) Shr 3 -1) Shl 4 Fs(p)=Fs(p)+1 p=p +((a\F And t3 ) Shr 2 -1) Shl 3 Fs(p)=Fs(p)+1 p=p +((a\F And t2 ) Shr 1 -1) Shl 2 Fs(p)=Fs(p)+1 p=p +((a\F And t1 ) -1) Shl 1 Fs(p)=Fs(p)+1 p=p +((a\F And t0 ) Shl 1 -1) Fs(p)=Fs(p)+1 ;[End Block] ElseIf Closed(x,y)=1;open a=Open(x,y) If a\G>Open(CurrentX,CurrentY)\G+map(x,y) a\G=Open(CurrentX,CurrentY)\G+map(x,y) a\Previous=Open(CurrentX,CurrentY) afs2(a\F)=afs2(a\F)-1 afs(a\F,a\n)=afs(a\F,afs2(a\F)) If afs(a\F,a\n)<>Null Then afs(a\F,a\n)\n=a\n ;[Block];Update - p=t19+((a\F And t18) Shr 17-1) Shl 18 Fs(p)=Fs(p)-1 p=p +((a\F And t17) Shr 16-1) Shl 17 Fs(p)=Fs(p)-1 p=p +((a\F And t16) Shr 15-1) Shl 16 Fs(p)=Fs(p)-1 p=p +((a\F And t15) Shr 14-1) Shl 15 Fs(p)=Fs(p)-1 p=p +((a\F And t14) Shr 13-1) Shl 14 Fs(p)=Fs(p)-1 p=p +((a\F And t13) Shr 12-1) Shl 13 Fs(p)=Fs(p)-1 p=p +((a\F And t12) Shr 11-1) Shl 12 Fs(p)=Fs(p)-1 p=p +((a\F And t11) Shr 10-1) Shl 11 Fs(p)=Fs(p)-1 p=p +((a\F And t10) Shr 9 -1) Shl 10 Fs(p)=Fs(p)-1 p=p +((a\F And t9 ) Shr 8 -1) Shl 9 Fs(p)=Fs(p)-1 p=p +((a\F And t8 ) Shr 7 -1) Shl 8 Fs(p)=Fs(p)-1 p=p +((a\F And t7 ) Shr 6 -1) Shl 7 Fs(p)=Fs(p)-1 p=p +((a\F And t6 ) Shr 5 -1) Shl 6 Fs(p)=Fs(p)-1 p=p +((a\F And t5 ) Shr 4 -1) Shl 5 Fs(p)=Fs(p)-1 p=p +((a\F And t4 ) Shr 3 -1) Shl 4 Fs(p)=Fs(p)-1 p=p +((a\F And t3 ) Shr 2 -1) Shl 3 Fs(p)=Fs(p)-1 p=p +((a\F And t2 ) Shr 1 -1) Shl 2 Fs(p)=Fs(p)-1 p=p +((a\F And t1 ) -1) Shl 1 Fs(p)=Fs(p)-1 p=p +((a\F And t0 ) Shl 1 -1) Fs(p)=Fs(p)-1 ;[End Block] a\F=a\G+a\H ;[Block];Find n for new f. Update f and n a\n=afs2(a\F) While a\n>maxu1 a\F=a\F+1 a\n=afs2(a\F) Wend afs(a\F,a\n)=a afs2(a\F)=a\n+1 ;[End Block] ;[Block];Update + p=t19+((a\F And t18) Shr 17-1) Shl 18 Fs(p)=Fs(p)+1 p=p +((a\F And t17) Shr 16-1) Shl 17 Fs(p)=Fs(p)+1 p=p +((a\F And t16) Shr 15-1) Shl 16 Fs(p)=Fs(p)+1 p=p +((a\F And t15) Shr 14-1) Shl 15 Fs(p)=Fs(p)+1 p=p +((a\F And t14) Shr 13-1) Shl 14 Fs(p)=Fs(p)+1 p=p +((a\F And t13) Shr 12-1) Shl 13 Fs(p)=Fs(p)+1 p=p +((a\F And t12) Shr 11-1) Shl 12 Fs(p)=Fs(p)+1 p=p +((a\F And t11) Shr 10-1) Shl 11 Fs(p)=Fs(p)+1 p=p +((a\F And t10) Shr 9 -1) Shl 10 Fs(p)=Fs(p)+1 p=p +((a\F And t9 ) Shr 8 -1) Shl 9 Fs(p)=Fs(p)+1 p=p +((a\F And t8 ) Shr 7 -1) Shl 8 Fs(p)=Fs(p)+1 p=p +((a\F And t7 ) Shr 6 -1) Shl 7 Fs(p)=Fs(p)+1 p=p +((a\F And t6 ) Shr 5 -1) Shl 6 Fs(p)=Fs(p)+1 p=p +((a\F And t5 ) Shr 4 -1) Shl 5 Fs(p)=Fs(p)+1 p=p +((a\F And t4 ) Shr 3 -1) Shl 4 Fs(p)=Fs(p)+1 p=p +((a\F And t3 ) Shr 2 -1) Shl 3 Fs(p)=Fs(p)+1 p=p +((a\F And t2 ) Shr 1 -1) Shl 2 Fs(p)=Fs(p)+1 p=p +((a\F And t1 ) -1) Shl 1 Fs(p)=Fs(p)+1 p=p +((a\F And t0 ) Shl 1 -1) Fs(p)=Fs(p)+1 ;[End Block] EndIf Else;closed a=Open(x,y) If a\G>Open(CurrentX,CurrentY)\G+map(x,y);1.414 a\G=Open(CurrentX,CurrentY)\G+map(x,y);*1.414 a\Previous=Open(CurrentX,CurrentY) EndIf EndIf ;Return 0 End Function --v

A-Star Pathfinding

Comments