I wrote this awhile ago, it's a* path finding, in a grid, you set a start location, and end location, and where the obstacles are, and it finds its way through
SuperStrict
Graphics 1680,1050
Global GridSquareSize:Int = 20
Global CurrentPath:TPath
'''''TEMP''''''
TPathNode.SetGridSize(85,52)
For Local i:Int = 0 To 50
TPathNode.Grid[20,i] = True
Next
For Local o:Int = 1 To 20
TPathNode.Grid[o,28] = True
Next
For Local p:Int = 5 To 27
TPathNode.Grid[4,p] = True
Next
For Local r:Int = 4 To 18
TPathNode.Grid[r,5] = True
Next
Global Time:Int
'''''TEMP''''''
Type TPath
Field NodeList:TList = CreateList()
End Type
Type TPathNode
Global OpenList:TList = CreateList()
Global ClosedList:TList = CreateList()
Global Grid:Int[40,30]
Global ClosedArray:TPathNode[,]
Global _startx:Int
Global _starty:Int
Global _endx:Int
Global _endy:Int
Global MaxX:Int
Global MaxY:Int
Field GridX:Int
Field GridY:Int
Field Parent:TPathNode
Field FCost:Int ' F = G + H
Field GCost:Int ' Movement cost from parent
Field HCost:Int ' Distance between this square and destination
Function GetLocalNodes(ParentNode:TPathNode)
Local startx:Int = ParentNode.GridX
Local starty:Int = ParentNode.GridY
For Local agridx:Int = startx - 1 To startx + 1
For Local agridy:Int = starty - 1 To starty + 1
If Not (agridx = startx And agridy = starty)
If agridx >= 0
If agridx <= MaxX
If agridy >= 0
If agridy <= MaxY
If ClosedArray[agridx,agridy] = Null
If Grid[agridx,agridy] = False
Local newNode:TPathNode = New TPathNode
newNode.GridX = agridx
newNode.GridY = agridy
newNode.Parent = ParentNode
'Is it diagonal or orthagonal
If ((_startx + _starty) = (agridx + agridy)) Or (startx + starty) - (agridx + agridy) = 2 Or (startx + starty) - (agridx + agridy) = -2
newNode.GCost = 14
Else
newNode.GCost = 10
EndIf
newNode.HCost = (Abs(agridx - _endx) + Abs(agridy - _endy))* 10
newNode.FCost = newNode.GCost + newNode.HCost
OpenList.AddLast(newNode)
ClosedArray[agridx,agridy] = newNode
'Draw()
EndIf
EndIf
EndIf
EndIf
EndIf
EndIf
EndIf
Next
Next
End Function
Function GetPath:TPath(startx:Int,starty:Int,endx:Int,endy:Int)
TPathNode._startx = startx
TPathNode._starty = starty
TPathNode._endx = endx
TPathNode._endy = endy
ClosedArray = New TPathNode[MaxX + 1,MaxY + 1]
'1. Add start square to open list
Local StartNode:TPathNode = New TPathNode
StartNode.GridX = _startx
StartNode.GridY = _starty
Local FindingPath:Byte = True
Local PathFound:Byte = True
While FindingPath
'2. Add reachable squares to open list with previous square as parent
'ClosedArray[StartNode.GridX,StartNode.GridY] = StartNode
'3. Drop previous square from open list and add it to closedlist)
GetLocalNodes(StartNode)
'4. Find lowest F score
Local NodewithLowestFCost:TPathNode
For Local aNode:TPathNode = EachIn OpenList
If NodewithLowestFCost = Null
NodewithLowestFCost = aNode
EndIf
If aNode.FCost < NodewithLowestFCost.FCost
NodewithLowestFCost = aNode
EndIf
Next
'5. Drop it from openlist and add it to closedlist
If NodewithLowestFCost = Null'if no path was found
PathFound = False
Exit
EndIf
OpenList.Remove(NodewithLowestFCost)
ClosedArray[NodewithLowestFCost.GridX,NodewithLowestFCost.GridY] = NodewithLowestFCost
'6. Check if we've reached the end square
If NodewithLowestFCost.GridX = _endx And NodewithLowestFCost.GridY = _endy Then FindingPath = False
StartNode = NodewithLowestFCost
Wend
OpenList.Clear()
'7. Create Path
If PathFound
Local Path:TPath = New TPath
While StartNode.Parent <> Null
Path.NodeList.AddLast(StartNode)
StartNode = StartNode.Parent
Wend
Return Path
Else
Return Null
EndIf
'6. Check adjacent squares, ignoring those in closedlist and unwalkable, make selected square the parent
'GetLocalNodes(NodewithLowestFCost)
'7. If adjacent square is already on the openlist check to see if GCost is lower
End Function
Function SetGridSize(x:Int,y:Int)
Grid = New Int[x,y]
MaxX = x - 1
MaxY = y - 1
End Function
EndType
While Not KeyHit(KEY_ESCAPE)
If KeyHit(KEY_ENTER)
Local start:Int = MilliSecs()
' For Local z:Int = 0 To 1
CurrentPath = TPathNode.GetPath(5,15,80,1)
' Next
Time = MilliSecs() - start
EndIf
UserInput()
Draw()
Wend
Function Draw()
Cls
For Local x:Int = 0 To 83
For Local y:Int = 0 To 51
'Draw occupied squares
If TPathNode.Grid[x,y]
SetColor(0,0,255)
DrawRect((x * GridSquareSize), (y * GridSquareSize) ,GridSquareSize,GridSquareSize)
EndIf
'Draw line
SetColor(255,255,255)
DrawLine(x*GridSquareSize,y*GridSquareSize,x*GridSquareSize,(y+1)*GridSquareSize)
DrawLine(x*GridSquareSize,y*GridSquareSize,(x+1)*GridSquareSize,y*GridSquareSize)
Next
Next
'Draw Start
SetColor(0,255,0)
DrawRect((TPathNode._startx * GridSquareSize), (TPathNode._starty * GridSquareSize) ,GridSquareSize,GridSquareSize)
'Draw End
SetColor(255,0,0)
DrawRect((TPathNode._endx * GridSquareSize) , (TPathNode._endy * GridSquareSize),GridSquareSize,GridSquareSize)
'Draw Path
SetColor(150,150,150)
If CurrentPath <> Null
For Local aNode:TPathNode = EachIn CurrentPath.NodeList
DrawRect(aNode.GridX *GridSquareSize ,aNode.GridY * GridSquareSize,GridSquareSize,GridSquareSize)
Next
EndIf
If CurrentPath = Null
SetColor(0,0,150)
For Local bNode:TPathNode = EachIn TPathNode.OpenList
DrawRect(bNode.GridX * GridSquareSize,bNode.GridY * GridSquareSize,GridSquareSize,GridSquareSize)
Next
SetColor(150,0,0)
For Local cNode:TPathNode = EachIn TPathNode.ClosedList
DrawRect(cNode.GridX * GridSquareSize,cNode.GridY * GridSquareSize,GridSquareSize,GridSquareSize)
Next
EndIf
DrawText TPathNode.OpenList.Count(),0,0
DrawText Time,0,12
Flip 0
End Function
Function UserInput()
If MouseHit(MOUSE_LEFT)
If TPathNode.Grid[Floor(MouseX()/20),Floor(MouseY()/20)] = True
TPathNode.Grid[Floor(MouseX()/20),Floor(MouseY()/20)] = False
Else
TPathNode.Grid[Floor(MouseX()/20),Floor(MouseY()/20)] = True
EndIf
EndIf
End Function
|