A* (A star) pathfinding
Posted: Thu Mar 21, 2019 3:20 pm
Hi,
I've ported this code to PureBasic. Maybe someone will find that useful.
Cheers.
EDIT (08/05/2019): Fixed a memory leak (ClearList(nodes()) added in AStar()).
I've ported this code to PureBasic. Maybe someone will find that useful.
Code: Select all
; A* pathfinding implementation, ported from a Python code:
; https://medium.com/@nicholas.w.swift/easy-a-star-pathfinding-7e6689c7f7b2
; @structure _position
; Basic x/y structure (integer).
; @variable allow_diagonals
; Allows the algorithm to look for diagonal nodes. #True by default.
; @procedure AStar(Array maze.i(2), List path._position(), start_x.i, start_y.i, end_x.i, end_y.i)
; The main function. The list must use the AStar::_position structure. Returns #True if a path has been found, #False otherwise.
; maze(x, y) equals 1 if the position should be considered as an obstacle (not walkable). 0 is walkable.
DeclareModule AStar
Structure _position
x.i
y.i
EndStructure
Global allow_diagonals.b = #True
Declare.b AStar(Array maze.i(2), List path._position(), start_x.i, start_y.i, end_x.i, end_y.i)
EndDeclareModule
Module AStar
EnableExplicit
Structure _node
*parent._node
position._position
g.i
h.i
f.i
EndStructure
Global NewList nodes._node()
; Creates a new node and returns its memory address from the global nodes list.
Procedure.i NodeCreate(*parent._node = 0, *position._position = 0)
LastElement(nodes()) : AddElement(nodes())
If *parent
nodes()\parent = *parent
EndIf
If *position
CopyStructure(*position, nodes()\position, _position)
EndIf
ProcedureReturn nodes()
EndProcedure
; Returns whether or not the two given nodes have the same position.
Procedure.b NodeEquals(*a._node, *b._node)
If *a\position\x = *b\position\x And *a\position\y = *b\position\y
ProcedureReturn #True
EndIf
EndProcedure
Procedure.b ChildrenCheckClose(List *children._node(), List *closed_list._node())
ForEach *closed_list()
If NodeEquals(*children(), *closed_list())
ProcedureReturn #True
EndIf
Next
EndProcedure
Procedure.b ChildrenCheckOpen(List *children._node(), List *open_list._node())
ForEach *open_list()
If NodeEquals(*children(), *open_list()) And *children()\g > *open_list()\g
ProcedureReturn #True
EndIf
Next
EndProcedure
Procedure.b AStar(Array maze.i(2), List path._position(), start_x.i, start_y.i, end_x.i, end_y.i)
Protected *start_node._node,
*end_node._node,
*current_node._node,
current_index.i,
*child_node._node,
start_position._position,
end_position._position,
child_position._position
NewList *open_list._node()
NewList *closed_list._node()
NewList *children._node()
NewList new_position._position()
ClearList(nodes())
AddElement(new_position())
new_position()\x = 0 : new_position()\y = -1
AddElement(new_position())
new_position()\x = 0 : new_position()\y = 1
AddElement(new_position())
new_position()\x = -1 : new_position()\y = 0
AddElement(new_position())
new_position()\x = 1 : new_position()\y = 0
If allow_diagonals
AddElement(new_position())
new_position()\x = -1 : new_position()\y = -1
AddElement(new_position())
new_position()\x = -1 : new_position()\y = 1
AddElement(new_position())
new_position()\x = 1 : new_position()\y = -1
AddElement(new_position())
new_position()\x = 1 : new_position()\y = 1
EndIf
; Creates start and end nodes.
start_position\x = start_x : start_position\y = start_y
end_position\x = end_x : end_position\y = end_y
*start_node = NodeCreate(0, @start_position)
*end_node = NodeCreate(0, @end_position)
; Adds the start node to the list.
AddElement(*open_list())
*open_list() = *start_node
;- Main loop.
While ListSize(*open_list())
FirstElement(*open_list())
*current_node = *open_list()
current_index = 0
; Gets the current node.
ForEach *open_list()
If *open_list()\f < *current_node\f
*current_node = *open_list()
current_index = ListIndex(*open_list())
EndIf
Next
; Move current node from open list to closed list.
SelectElement(*open_list(), current_index)
DeleteElement(*open_list())
AddElement(*closed_list())
*closed_list() = *current_node
; If we find the goal, we create and return path().
If NodeEquals(*current_node, *end_node)
ClearList(path())
While *current_node
InsertElement(path())
path()\x = *current_node\position\x : path()\y = *current_node\position\y
*current_node = *current_node\parent
Wend
ProcedureReturn #True
EndIf
; Generates children.
ClearList(*children())
ForEach new_position()
child_position\x = *current_node\position\x + new_position()\x
child_position\y = *current_node\position\y + new_position()\y
; Checks if we're still in range.
If child_position\x > ArraySize(maze(), 1) Or child_position\x < 0 Or child_position\y > ArraySize(maze(), 2) Or child_position\y < 0
Continue
EndIf
; Checks if the position is an obstacle.
If maze(child_position\x, child_position\y)
Continue
EndIf
; Creates a new node.
*child_node = NodeCreate(*current_node, @child_position)
AddElement(*children())
*children() = *child_node
Next
; Loops through children
ForEach *children()
; Ignore the following if the child has already been used.
If ChildrenCheckClose(*children(), *closed_list()) : Continue : EndIf
*children()\g = *current_node\g + 1
*children()\h = Pow(*children()\position\x - *end_node\position\x, 2) + Pow(*children()\position\y - *end_node\position\y, 2)
*children()\f = *children()\g + *children()\h
; Ignore the following if the child is already in the open list and has a greater g.
If ChildrenCheckOpen(*children(), *open_list()) : Continue : EndIf
LastElement(*open_list()) : AddElement(*open_list())
*open_list() = *children()
Next
Wend
EndProcedure
EndModule
;- Example.
If #PB_Compiler_IsMainFile
NewList path.AStar::_position()
Dim maze(0, 0)
;AStar::allow_diagonals = #False
ParseJSON(0, "[[0, 0, 1, 0, 0]," +
" [0, 0, 0, 0, 0]," +
" [0, 0, 1, 0, 0]," +
" [0, 0, 1, 0, 0]," +
" [0, 0, 1, 0, 0]]")
ExtractJSONArray(JSONValue(0), maze())
If Not AStar::AStar(maze(), path(), 0, 0, 4, 4)
Debug "No path found."
Else
ForEach path()
Debug Str(path()\y) + " - " + Str(path()\x)
Next
EndIf
EndIf
EDIT (08/05/2019): Fixed a memory leak (ClearList(nodes()) added in AStar()).