This content is provided AS IS for archive purposes only. Content may be out of date with no guarantee of correctness.

A-star Pathfinder - Example Cobra Code - Manuals

Main Article

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.

Program
   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:

Screenshot of A* Pathfinding result

Stay Subscribed

Follow me on twitter @golightlyb, subscribe to the RSS feed or get updates by e-mail.

You can also contact me directly - I make an effort to reply to every e-mail.


Login
v0.34.archive