3d a* pathfinding

Blitz3D Forums/Blitz3D Programming/3d a* pathfinding

skyfire1
 #1
how do you tackle pathfinding in 3d? should you use types or arrays?

Dreamora
 #2
if it is really 3D you wouldn't use a* but given pathing graphs.

if it is a 3D representation of a 2D map then a 2D array. types become extremely slow if you need many of them

Techlord
 #3
skyfire1

You can use both. You can use banks as well. It really does depend on how your implementing it. I used a types to manage priority queues for open/closed waypoint nodes. I've seen others use a Binary Heap Array.

You may be interested in the Bot System I propose here.

skyfire1
 #4
dang it! pathfinding is hard! is there any a* pathfinding library that's easy to implement?

Dreamora
 #5
Nope but a waypoint based pathfinding library :-)
I think its even in the toolbox.

Banshee
 #6
3D games usually use 2D pathfinding because at the end of the day, tanks dont fly, if you see what i'm getting at. The point is doing a game in 3D need not complicate your path finding, you can treat it just like 2D.

You're better off writting the pathfinding yourself so that you understand it enough to make it work well for your game, but there's no harm in doing so by learning from existing code examples/libraries.

Path finding is one of the harder things to implement in a game which is why I try to solve it relatively early in the programs development cycle. Better to grin and bear it early than to put it off and never face it.

I usually make my path finding functions in a seperate program using the games actual data but shown onscreen as a 2D map - it helps me tweak the algorythm.

This is a modified A* routine which is quite good for searching routes out of a large heightmap based terrain. You can make extra "blocked" areas for trees & houses simply by painting the colour red(255).

You'll need to change the heightmap file loaded before this sample will run - I last used this for my Paradise Island project so it's trying to load the heightmap from that...

Graphics 512,512,0,2
AppTitle("A*")
Dim mapData(512,512)

Global treeMap=LoadImage("C:\BeckysGames\Paradise Island\Gfx\Terrain\map2.png")
SetBuffer ImageBuffer(treeMap)
For x=0 To 511
	For z=0 To 511
		GetColor(x,z)
		mapData(x,511-z)=ColorRed()
		If mapData(x,511-z)=255 Then mapData(x,511-z)=999
	Next
Next
SetBuffer BackBuffer()

Type openList
	Field x,z
	Field parentX,parentZ
	Field fSum
	Field gCost
	Field hGuess
	Field mapHeight
End Type

Type closedList
	Field x,z
	Field parentX,parentZ
End Type

Type AIpath
	Field x,z
End Type

sx=256
sz=256
tx=256
tz=256

SeedRnd MilliSecs()
Repeat
	strt=MilliSecs()
	unit=Rnd(1,5)
	route(sx,sz,tx,tz,unit)
	time=MilliSecs()-strt

	Cls
	DrawImage treeMap,0,0

	Color 255,255,0
	Oval sx,511-sz,3,3,0
	Color 0,255,0
	Oval tx,511-tz,5,5,0

	Color 0,0,255
	For n=1 To 5
		id=0
		For p.AIpath=Each AIpath
			id=id+1
			If id>1
				o.AIpath=Before p
				Line p\x,512-p\z, o\x,512-o\z
			EndIf
		Next
	Next
	Color 255,255,0

	Text 10,10,time+"ms for "+id+" steps"
	Flip 0
	
	Repeat
		If MouseDown(1)
			sX=MouseX()
			sZ=512-MouseY()
		EndIf
		If MouseDown(2)
			tX=MouseX()
			tZ=512-MouseY()
		EndIf
	Until KeyDown(17)
	;Stop

	For clearPath.AIpath=Each AIpath
		Delete clearPath
	Next
Forever

Function route(sx,sz,tx,tz,id)

	node.openList=New openList
	node\x=sx : node\z=sz
	node\fSum=0 : node\gCost=0
	node\hGuess=0
	node\mapHeight=mapData(node\x,node\z)

	maxNodeRange=(Abs(sx-tx)+Abs(sz-tz))*1.1
	searchTime=MilliSecs()+333
	
	Repeat
		lowestScore=99999999
		nodes=0
		For checkNode.openList=Each openList
			nodes=nodes+1
			If checkNode\fSum<lowestScore
				lowestScore=checkNode\fSum
				node.openList=checkNode.openList
			EndIf
		Next

		;DebugLog(node\x+"x"+node\z+" = "+node\gCost+" ("+tX+"x"+tZ+") ["+nodes+"]")
	
		;we now have the node to search
		If lowestScore=99999999; Or MilliSecs()>searchTime
			buildPath=2
		Else
			If Abs(node\x-tx)<=3 And Abs(node\z-tz)<=3
				buildPath=1
			Else
				For bearing=0 To 359 Step 40
					rX#=node\x : rZ#=node\z
					range=0
					ht=mapData(node\x,node\z)
					While range<=5
						rX=rX+Cos(bearing)
						rZ=rZ+Sin(bearing)
						If rX=>0 And rX<=511 And rZ=>0 And rZ<=511
							If Int(rX)=tX And Int(rZ)=tZ
								range=10
							Else
								If mapData(rX,rZ)-ht<=4
									ht=mapdata(rX,rZ)
									range=range+1
								Else
									range=10
								EndIf
							EndIf
						Else
							range=10
						EndIf
					Wend
					
					sX=rX : sZ=rZ
					;*** CORE SYSTEM	
						If sX=>0 And sX<=511 And sZ=>0 And sZ<=511
							If sX<>node\X Or sZ<>node\Z
								onClosedList=0
								For checkCNode.closedList=Each closedList
									If Abs(checkCNode\x-sX)<=1
										If Abs(checkCNode\z-sZ)<=1
											onClosedList=1
											checkCNode.closedList=Last closedList
										EndIf
									EndIf
								Next

								If Not onClosedList
									onOpenList=0
									For checkONode.openList=Each openList
										If Abs(checkONode\x-sX)<=1
											If Abs(checkONode\z-sZ)<=1
												onOpenList=1
												If node\gCost+1<checkONode\gCost
													If checkONode\mapheight-node\mapHeight<=4
														If checkOnode\mapHeight<8 Then cost=10 Else cost=1
														checkONode\parentX=node\X
														checkONode\parentZ=node\Z
														checkONode\gCost=node\gCost+cost
														checkONode\fSum=checkONode\gCost+checkONode\hGuess
													EndIf
												EndIf
												checkONode.openList=Last openList
											EndIf
										EndIf
									Next

									If Not onOpenList
										If mapData(sX,sZ)-node\mapHeight<=4
											If node\gCost<maxNodeRange
												If mapData(sX,sZ)<8 Then cost=10 Else cost=1
												newNode.openList=New openList
												newNode\x=sX : newNode\z=sZ
												newNode\parentX=node\x : newNode\parentZ=node\z
												newNode\gCost=node\gCost+cost
												newNode\hGuess=moveHeuristic(sX,sZ,tX,tZ)
												newNode\fSum=newNode\gCost+newNode\hGuess
												newNode\mapHeight=mapData(sX,sZ)
											EndIf
										EndIf
									EndIf
								EndIf
							EndIf
						EndIf
					;*** END CORE SYSTEM
				Next
		
				finished.closedList=New closedList
				finished\x=node\x : finished\z=node\z
				finished\parentX=node\parentX : finished\parentZ=node\parentZ
				Delete node.openList
			EndIf
		EndIf
		
		up=up+1
		ms=MilliSecs()
		If up=>lastTime-ms
			lastTime=ms
			up=0
			Color 255,255,0
			DrawImage treeMap,0,0
			For showO.openList=Each openList
				Plot showO\x,512-showO\z
			Next
			Color 0,255,255
			For showC.closedList=Each closedList
				Plot showC\x,512-showC\z
			Next
			Color 0,255,0
			Oval tx,512-tz,3,3,0
			Flip 0
			Color 0,0,255
			Oval ox,512-oz,5,5,0
		EndIf
	Until buildPath>0
	
	If buildPath=2
		For checkOnode.openList=Each openList
			finished.closedList=New closedList
			finished\x=checkOnode\x
			finished\z=checkOnode\z
			finished\parentX=checkOnode\parentX
			finished\parentZ=checkOnode\parentZ
			Delete checkOnode
		Next
	
		nearest=99999999
		newTx=tx : newTz=tz
		For CheckCnode.closedList=Each closedList
			dx#=checkCnode\x-tx
			dz#=checkCnode\z-tz
			rng=Sqr((dx*dx)+(dz*dz))
			If rng<nearest
				newTx=checkCnode\x
				newTz=checkCnode\z
				thisNode.closedList=checkCnode.closedList
				nearest=rng
			EndIf
		Next
		path.AIpath=New AIpath
		path\x=thisNode\x
		path\z=thisNode\z
		x=thisNode\parentX
		z=thisNode\parentZ
	EndIf

	If buildPath=1	
		path.AIpath=New AIpath
		path\x=node\x : path\z=node\z
		x=node\parentX : z=node\parentZ
		Delete node
	EndIf
	
	Repeat
		For pathnode.closedList=Each closedList
			If x=pathnode\x And z=pathnode\z
				path.AIPath=New AIPath
				path\x=x
				path\z=z
				x=pathnode\parentX
				z=pathnode\parentZ
				Delete pathNode
			EndIf
		Next
	Until x=0 And z=0

	For clearOList.openList=Each openList
		Delete clearOList
	Next
	For clearCList.closedList=Each closedList
		Delete clearCList
	Next
End Function
;Function moveHeuristic(sX,sZ,tX,tZ)
;	difX=Abs(sX-tX)
;	difZ=Abs(sZ-tZ)
;	If difX>difZ
;		Return difZ+(difX*3)
;	Else
;		Return difX+(difZ*3)
;	EndIf
;End Function
;Function moveHeuristic(sX,sZ,tX,tZ)
;	dx#=sX-tX
;	dz#=sZ-tZ
;	Return Sqr((dx*dx)+(dz*dz))
;End Function
Function moveHeuristic(sX,sZ,tX,tZ)
	Return (Abs(sX-tX)+Abs(sZ-tZ))*2
End Function


Techlord
 #7
Becky,

Very nice looking A* code you got there.

Bouncer
 #8
And use heaps... you can really optimize a* with it.

Bouncer
 #9
Here's my a* function ... might give you ideas how to construct yours. It's a node graph based and it uses heap to really speed it up.


Function P_ASTAR(node1%,GOAL%,ply%=0)
Local CLOSED%[P_MAXNODES]
Local PARENT%[P_MAXNODES]
Local G#[P_MAXNODES]					;GENERATED COST
Local H#[P_MAXNODES]					;ESTIMATED COST (to target)
goal_found=0

If ply=0 Then P_GOAL_NODE(ply) = 0


If NODE1 <> GOAL
CLEAR_HEAP()
HEAP_INSERT(node1,0)


;P_ELAPSEDTIME=MilliSecs()
Repeat
;GET THE MOST LOWEST COST NODE FROM THE HEAP
current = HEAP_REMOVELOWEST()

;PUT IT TO THE CLOSED LIST
CLOSED[current] = 1

	;CHECK ALL THE NODES CONNECTED TO CURRENT NODE
	For i=1 To P_NODE_CONNECTIONS(current)
	nn = P_NODE_CON(current,i)

	;IF NOT ON THE CLOSED LIST PROCEED
	If CLOSED[nn]=0
	
	;SEARCH THE OPEN LIST -> PUT THE HEAP POS IN found ELSE found = 0
	found=0
	For ii=1 To HEAPSIZE
	If HEAPITEM(ii) = nn Then found=ii:Exit
	Next

	;NOT ON OPENLIST ---> CALCULATE COST AND PUT IT IN!
	If found=0
	PARENT[nn] = current
	H[nn] = P_MANHATTAN_DISTANCE(nn,GOAL)					;ESTIMATE THE DISTANCE TO GOAL
	G[nn] = P_NODE_ARCLENGTH(current,i) + G[current]		;G = distance to this node + G of the parent node
	F# = G[nn] + H[nn]																	;CALCULATE FINAL COST

	;PUT TO OPENLIST
	HEAP_INSERT(nn,F)
	End If

	;IS ON THE OPEN LIST --- > CHECK IF THIS IS BETTER PATH TO  IT
	If found>0
	NG# = P_NODE_ARCLENGTH(current,i) + G[current]

		;IF THIS PATH*S G SCORE = LOWER THAN OLD G --> UPDATE
		If NG<G[nn]
		PARENT[nn] = current
		G[nn] = NG																				;REPLACE THE OLD G WITH THE CURRENT G
		F# = G[nn] + H[nn]																	;CALCULATE FINAL COST
		UPDATE_HEAPITEM(nn,F,found)											;UPDATE THIS NODE
		End If
		
	End If

	End If
	Next

If current=GOAL Then goal_found=1
Until  HEAP_EMPTY()=1 Or goal_found=1 
End If

	P_PATHNODES(ply)=0
	P_PATHCOST(ply)=0
	
	If P_GOAL_NODE(ply)>0
	P_PATHNODES(ply) = P_PATHNODES(ply) + 1
	P_PATHNODE(ply,P_PATHNODES(ply)) = P_GOAL_NODE(ply)
	End If

;TRACE THE PATH!
If goal_found=1
current=GOAL
P_PATHCOST(ply) = G[current]

	Repeat
	P_PATHNODES(ply) = P_PATHNODES(ply)+1
	P_PATHNODE(ply,P_PATHNODES(ply)) = current
	current = PARENT[current]
	Until current=node1
	P_PATHNODES(ply) = P_PATHNODES(ply)+1
	P_PATHNODE(ply,P_PATHNODES(ply)) = current

End If



;P_ELAPSEDTIME=MilliSecs()-P_ELAPSEDTIME

Return P_PATHNODES(ply)
End Function


Techlord
 #10
Bouncer,

Very nice compact code. Thanks for sharing.

Bouncer
 #11
And while I'm at it... Here's some ready to use heap function.s


;HEAP FUNCTIONS
Global HEAPSIZE=0
Global HMAXSIZE=P_MAXNODES
Dim HEAPITEM%(HMAXSIZE)
Dim HEAPKEY#(HMAXSIZE)



Function HEAP_INSERT(n,key#)
HEAPSIZE=HEAPSIZE+1
HEAPITEM(HEAPSIZE) = n

m = HEAPSIZE
HEAPKEY(HEAPITEM(m))=key

  While m <> 1 ;While item hasn't bubbled to the top (m=1)

     ;Check if child is <= parent. If so, swap them.
     If HEAPKEY(HEAPITEM(m)) <= HEAPKEY(HEAPITEM(m/2)) Then
        temp = HEAPITEM(m/2)
        HEAPITEM(m/2) = HEAPITEM(m)
        HEAPITEM(m) = temp
        m = m/2 
     Else

        Exit ;exit the while/wend loop
     End If
  Wend
End Function




Function HEAP_REMOVELOWEST()
;If HEAP_EMPTY()=0
L = HEAPITEM(1)

;GET THE LAST ITEM AND PUT IT IN THE FIRST POSITION
;(FIRST GET*S DELETED)
HEAPITEM(1) = HEAPITEM(HEAPSIZE)
HEAPSIZE = HEAPSIZE - 1

v = 1
;Repeat the following until the item sinks to its proper spot in the binary heap.
Repeat
  u = v
  If 2*u+1 <= HEAPSIZE;if both children exist
    ;Select the lowest of the two children.
    If HEAPKEY(HEAPITEM(u)) >= HEAPKEY(HEAPITEM(2*u)) Then v = 2*u 
    If HEAPKEY(HEAPITEM(v)) >= HEAPKEY(HEAPITEM(2*u+1)) Then v = 2*u+1

  Else If 2*u <= HEAPSIZE ;if only child #1 exists
    ;Check if the F cost is greater than the child
    If HEAPKEY(HEAPITEM(u)) >= HEAPKEY(HEAPITEM(2*u))Then v = 2*u
  End If

  If u <> v Then ; If parent's F > one or both of its children, swap them
    temp = HEAPITEM(u)
    HEAPITEM(u) = HEAPITEM(v)
    HEAPITEM(v) = temp
  Else
    Exit ;if item <= both children, exit repeat/forever loop
  End If
Forever

Return L
;End If
End Function



Function UPDATE_HEAPITEM(item,newkey#,HEAPPOS=0)
If HEAPPOS=0
For i=1 To HEAPSIZE
If HEAPITEM(i) = item Then m=i:Exit
Next
Else 
m=HEAPPOS
End If

HEAPKEY(HEAPITEM(m)) = newkey

  While m <> 1 ;While item hasn't bubbled to the top (m=1)
     ;Check if child is <= parent. If so, swap them.
     If HEAPKEY(HEAPITEM(m)) <= HEAPKEY(HEAPITEM(m/2)) Then
        temp = HEAPITEM(m/2)
        HEAPITEM(m/2) = HEAPITEM(m)
        HEAPITEM(m) = temp
        m = m/2 
     Else
        Exit ;exit the while/wend loop
     End If
  Wend
End Function



Function HEAP_EMPTY()
If HEAPSIZE>0 Return 0 Else Return 1
End Function
Function CLEAR_HEAP()
Dim HEAPITEM%(HMAXSIZE)
Dim HEAPKEY#(HMAXSIZE)
HEAPSIZE=0
End Function