Bouncer's A*
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.
;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
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 FunctionBecky Rose A*
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 