2012-04-30, 12:10 AM
you can run the code to get a little demo:
Code:
const found as ubyte=1
const pathsuccess as ubyte=1
const CantCreatePathError as ubyte=2
Dim path as integer
Dim SquareState(32,24) as UByte
Dim SquareParentX(32,24) as Ubyte
Dim SquareParentY(32,24) as Ubyte
Dim HCost(32,24) as UByte
Dim PathToWalkX(255) as UByte
Dim PathToWalkY(255) as UByte
Dim StepsOnPath as Integer
Dim PathBlockedByPerson as Ubyte
Dim PathBlocker as Integer
Dim numberofopenlistitems as Integer
Dim LowestFCostSquareX as Integer
Dim LowestFCostSquareY as Integer
Function FindPath(startx as Integer,starty as Integer,targetx as Integer,targety as Integer) as Integer
ClearPath()
pathfindmessage("FindPath")
If startx =targetx And starty = targety then
pathfindmessage("target square = start square")
Return 3
End if
HCost(startx,starty)=GetHCost(startx,starty,targetx,targety)
AddToOpenList(startx,starty,startx,starty)
LowestFCostSquareX=startx
LowestFCostSquareY=starty
path=-1
Dim DebugIter as Integer
while path=-1
DebugIter=DebugIter+1
If DebugIter>200 then
pathfindmessage("path too long")
Exit While
End if
If numberofopenlistitems=0 then
pathfindmessage("no more open list items")
Exit While
End if
'pathfindmessage("Lowest f cost square "+STR(LowestFCostSquareX)+" "+STR(LowestFCostSquareY))
Dim lowx as ubyte
lowx=LowestFCostSquareX
Dim lowy as ubyte
lowy=LowestFCostSquareY
CheckSquare(lowx+1,lowy,targetx,targety,lowx,lowy)
CheckSquare(lowx-1,lowy,targetx,targety,lowx,lowy)
CheckSquare(lowx,lowy+1,targetx,targety,lowx,lowy)
CheckSquare(lowx,lowy-1,targetx,targety,lowx,lowy)
if path<>found then
AddToClosedList(lowx,lowy)
end if
end while
If path = found then
'Print "path was found"
If CreatePath(startx , starty , targetx ,targety)<>1 then
Return CantCreatePathError
End if
Return 1
End if
return 0
End Function
Function CheckSquare(squarex as integer,squarey as integer,targetx as integer,targety as integer,originalx as integer,originaly as integer) as UByte
if squarex>-1 then
if squarex<32 then
if squarey>-1 then
if squarey<24 then
'pathfindmessage("Check square:"+STR(squarex)+" "+STR(squarey))
If OnClosedList(squarex,squarey)=0 then
'pathfindmessage("not on closed list")
If OnOpenList(squarex,squarey)=0 then
'pathfindmessage("not on open list")
If squarex=targetx And squarey=targety then
path = found
End if
HCost(squarex,squarey)=GetHCost(squarex,squarey,targetx,targety)
AddToOpenList(squarex,squarey,originalx,originaly)
If path=found then
SquareParentX(squarex,squarey)=originalx
SquareParentY(squarex,squarey)=originaly
'Print "path is found"
AddToClosedList(squarex,squarey)
End if
End if
End if
End if
End if
End if
end if
End Function
Function GetHCost(squarex as integer,squarey as integer,targetx as integer,targety as integer) as integer
Return (Abs(squarex-targetx)+Abs(squarey-targety))
End Function
Function OnOpenList(squarex as Integer, squarey as Integer) as integer
if SquareState(squarex,squarey)=1 then
return 1
end if
return 0
end function
Function OnClosedList(squarex as Integer, squarey as Integer) as integer
if SquareState(squarex,squarey)=2 then
return 1
end if
return 0
end function
Function AddToOpenList(squarex as Integer, squarey as Integer,parx as integer, pary as integer)
SquareParentX(squarex,squarey)=parx
SquareParentY(squarex,squarey)=pary
numberofopenlistitems=numberofopenlistitems+1
If HCost(squarex,squarey)<HCost(LowestFCostSquareX,LowestFCostSquareY) then
LowestFCostSquareX=squarex
LowestFCostSquareY=squarey
End if
SquareState(squarex,squarey)=1
Print at squarey,squarex;"O"
end function
Function AddToClosedList(squarex as Integer, squarey as Integer)
if LowestFCostSquareX=squarex then
if LowestFCostSquareY=squarey then
Dim dist as Integer
dim chosenX as integer
Dim chosenY as integer
chosenX=-1
chosenY=-1
dist=9999
for x=0 to 32
for y=0 to 24
if OnOpenList(x,y) then
If HCost(x,y)<dist then
dist=HCost(x,y)
chosenX=x
chosenY=y
end if
End if
Next
Next
If chosenX>-1 then
LowestFCostSquareX=chosenX
LowestFCostSquareY=chosenY
End if
end if
end if
if SquareState(squarex,squarey)=1 then
numberofopenlistitems=numberofopenlistitems-1
end if
SquareState(squarex,squarey)=2
Print at squarey,squarex;"C"
end function
Function AddToPath(squarex as Integer, squarey as Integer)
Print at squarey,squarex;"P"
end function
Function CreatePath(StartX as integer,StartY as integer,TargetX as integer,TargetY as integer) as integer
PathBlockedByPerson=0
PathBlocker=0
dim PathCreated as Ubyte
dim ParX as Integer
dim ParY as Integer
StepsOnPath=StepsOnPath+1
PathToWalkX(StepsOnPath)=TargetX
PathToWalkY(StepsOnPath)=TargetY
ParX=PathToWalkX(StepsOnPath)
ParY=PathToWalkY(StepsOnPath)
While PathCreated=0
Dim NewParX as integer
Dim NewParY as integer
NewParX=SquareParentX(ParX,ParY)
NewParY=SquareParentY(ParX,ParY)
ParX=NewParX
ParY=NewParY
AddToPath(ParX,ParY)
StepsOnPath=StepsOnPath+1
If StepsOnPath=StepsOnPath>254 then
Return 2
End if
PathToWalkX(StepsOnPath)=ParX
PathToWalkY(StepsOnPath)=ParY
If PathToWalkX(StepsOnPath)=StartX And PathToWalkY(StepsOnPath)=StartY then
exit while
End if
Wend
Return 1
End Function
function ClearPath()
for x=0 to 32
for y=0 to 24
SquareState(x,y)=0
SquareParentX(x,y)=0
SquareParentY(x,y)=0
HCost(x,y)=0
next
next
'for x=0 to 768
'PathToWalkX(x)=0
'PathToWalkY(x)=0
'next
StepsOnPath=0
PathBlockedByPerson=0
PathBlocker=0
numberofopenlistitems=0
End Function
Dim pathfindprint as ubyte
Function pathfindmessage(msg as string)
Print at pathfindprint,0 ; msg
pathfindprint=pathfindprint+1
if pathfindprint>24 then
pathfindprint=0
end if
end function
Border 5
FindPath(0,10,10,0)
FindPath(0,10,31,23)
FindPath(31,23,0,0)