A-star Pathfinder - Example Cobra Code
I wrote this a long time ago; it has room for improvement. Particularly, try to avoid allocating and freeing list items. In fact, I don't recommend using it!
A* Pathfinding
An example implementation of the A* pathfinding algorithm (also known as the A* search algorithm) for the Cobra programming language.
This is just a simple example - solutions near to best-case are very fast but for a worst-case solution it may prove too slow for real-time so you should combine it with specific optimisations that suit your modelled environment. For example, an in-door map will benefit if you link up all doors or exits to a node map, build a list of doors to pass through, and use A* path-finding to find the path from door-to-door.
Required file: save as map.png
Please feel free to use this code with or without attribution.
Uses
cobra2d,
keyset
Const
INFINITY = 1000000
MAX_PATH_LENGTH = 1024
Var
WATCH: Boolean = TRUE // TRUE to visualise the search
Type nodes = Record
x, y: Integer
f_score: Integer // estimate using f = g + h
g_score: Integer // distance from start along optimum path
h_score: Integer // heuristic estimate
parent: ^nodes
hasParent: Boolean
EndType
Var
openList: List of nodes // set of tentative nodes to be evaluated
closedList: List of nodes //set of nodes already evaluated
openItems: Integer
// Results
ResultPath: Array [MAX_PATH_LENGTH, 2] of Integer // x, y
ResultSteps: Integer
Function PlotPath(x1, y1, x2, y2) : Boolean
Var
lowF, i, c: Integer = 0
openNode, closedNode, node: ^nodes
x, y: Integer
isInClosedList, isInOpenList: Boolean
tentative_g_score: Integer
tentative_is_better: Boolean
Begin
result = FALSE
// Add starting node to list
openNode = NewItem(openList)
openNode.x = x1
openNode.y = y1
openNode.g_score = 0
openNode.h_score = Abs(x1-x2) + Abs(y1-y2)
openNode.f_score = openNode.h_score
Inc(openItems)
While (openItems > 0)
If c = (MAX_PATH_LENGTH-1) then
// exit after unsuccesfully searching MAX_PATH_LENGTH squares
Loop openNode through openList
Free(openNode)
EndLoop
Loop closedNode through closedList
Free(closedNode)
EndLoop
exit
Endif
Inc(c)
// Get node from Open List with lowest f score
lowF = INFINITY
Loop node through openList
If node.f_score < lowF then
lowF = node.f_score
openNode = node
Endif
EndLoop
// If this node is the goal
If (openNode.x = x2) and (openNode.y = y2) then
// MessageBox("Found!")
result = TRUE
ReconstructPath(openNode)
Loop openNode through openList
Free(openNode)
EndLoop
Loop closedNode through closedList
Free(closedNode)
EndLoop
exit
Endif
// Take off open list, put on closed list
closedNode = NewItem(closedList)
closedNode.x = openNode.x
closedNode.y = openNode.y
closedNode.f_score = openNode.f_score
closedNode.g_score = openNode.g_score
closedNode.h_score = openNode.h_score
closedNode.parent = openNode.parent
closedNode.hasParent = openNode.hasParent
Free(openNode)
Dec(openItems)
// To manually watch the search...
If WATCH then
Rect(closedNode.x*8,closedNode.y*8,8,8,ToRGBA(200,200,255,150), TRUE, canvas2)
Flip
Endif
// Neighbouring nodes
For x = closedNode.x-1 to closedNode.x+1
For y = closedNode.y-1 to closedNode.y+1
If (((x = closedNode.x) and (y = closedNode.y)) = FALSE) then // ignore self
If (tile[x,y] = FALSE) then // EDIT THIS BIT - If node is walkable!
isInClosedList = FALSE
Loop node through closedList
If (node.x = x) and (node.y = y) then
isInClosedList = TRUE
Endif
EndLoop
If isInClosedList = FALSE then
tentative_g_score = closedNode.g_score + Sqrt( (x-closedNode.x)^2 + (y-closedNode.y)^2)
isInOpenList = FALSE
Loop node through openList
If (node.x = x) and (node.y = y) then
isInOpenList = TRUE
openNode = node
Endif
EndLoop
If isInOpenList = FALSE then
tentative_is_better = TRUE
openNode = NewItem(openList)
openNode.x = x
openNode.y = y
Inc(openItems)
Else
If tentative_g_score < openNode.g_score then
tentative_is_better = TRUE
Else
tentative_is_better = FALSE
Endif
Endif
If tentative_is_better then
openNode.parent = closedNode
openNode.hasParent = TRUE
openNode.g_score = tentative_g_score
openNode.h_score = Abs(x-x2) + Abs(y-y2)
openNode.f_score = openNode.g_score + openNode.h_score
Endif
Endif
Endif
Endif
Next
Next
Wend
End
Procedure ReconstructPath(node: ^nodes, depth:Integer = 0)
Begin
ResultSteps = depth + 1
If node.hasParent then ReconstructPath(node.parent, depth+1)
ResultPath[depth, 0] = node.x
ResultPath[depth, 1] = node.y
If WATCH then
Rect(node.x*8,node.y*8,8,8,ToRGBA(100,100,255,255), TRUE, canvas2)
Flip
Endif
End
Var
tile: Array[64, 64] of Boolean
x, y, i, m: Integer
canvas, canvas2: Element
tmpImg: Element
targetx: Array[2] of Integer
targety: Array[2] of Integer
Begin
SetAppName("Pathfinding Prototype")
OpenScreen(1024, 768, 32, FALSE, COB_SHOWBORDER)
canvas = CreateSprite(1024,768)
canvas2 = CreateSprite(1024,768)
tmpImg = LoadImage("map.png")
For x = 0 to 63
For y = 0 to 63
If (Green(Pixel(x, y, tmpImg)) = 192) then
tile[x,y] = TRUE
Else
tile[x,y] = FALSE
Endif
Next
Next
While Not KeyDown(VK_ESCAPE)
SetAppName("W to toggle visualisation of search ("+ToString(WATCH)+"), Space to plot, Mouse Left&Right to adjust targets")
Rect(0,0,1024,768,ToRGBA(0,0,0),TRUE,canvas)
If ResultSteps > 0 then
For i = 0 to (ResultSteps-1)
Rect(ResultPath[i,0]*8,ResultPath[i,1]*8,8,8,ToRGBA(128,255,128,192), TRUE, canvas)
Next
Endif
For x = 0 to 63
For y = 0 to 63
If tile[x,y] = TRUE then
Rect(x*8,y*8,8,8,ToRGBA(192,192,192), TRUE, canvas)
Rect(x*8,y*8,8,8,ToRGBA(0,0,0), FALSE, canvas)
Endif
If (targetx[0] = x) and (targety[0] = y) then Rect(x*8,y*8,8,8,ToRGBA(255,0,0), TRUE, canvas)
If (targetx[1] = x) and (targety[1] = y) then Rect(x*8,y*8,8,8,ToRGBA(0,0,255), TRUE, canvas)
Next
Next
For i = 0 to 1
If MouseHits(i) > 0 then
targetx[i] = MouseX/8
targety[i] = MouseY/8
Endif
Next
If KeyHits(VK_SPACE) > 0 then
CLS(ToRGBA(0,0,0,0), canvas2)
m = Millisecs
If PlotPath(targetx[0], targety[0], targetx[1], targety[1]) then
m = Millisecs - m
MessageBox("Sucessful search took "+m+" Millisecs (but visualisation is "+ToString(watch)+")")
Else
m = Millisecs - m
MessageBox("FAILED - Gave up after "+m+" Millisecs (but visualisation is "+ToString(watch)+")")
Endif
Cls(ToRGBA(0,0,0,0), canvas2)
Endif
If KeyHits(VK_W) > 0 then WATCH = Not WATCH
Flip
Pause(1)
Wend
End
Screenshot:
Stay Subscribed
RSS feed or get updates by e-mail.
, subscribe to theYou can also contact me directly - I make an effort to reply to every e-mail.