hello world! Today, we’re gonna make a simple A* Pathfinder. I have tried to optimize this but I just can’t seem to wrap my head around multithreading this algorithm. I’m not gonna explain the algorithm in this topic in depth. Anyways, lets get to work
DISCLAIMER
This tutorial is meant to give you a base understanding of how the algorithm works, and how you should code it. You should NOT use this in a production environment as it is not optimized.
However, you are free to use the tutorial to get you started and modify it to fit your needs
Next Part
Programming the Algorithm
I have made this pseudocode for the A* algorithm below:
OPEN SET = {} -- SET OF NODES TO BE EVALUATED
CLOSED SET = {} -- SET OF NODES THAT HAS BEEN ALREADY EVALUATED
ADD (START NODE) TO (OPEN SET)
while LOOP
CURRENT = GET (NODE) IN (OPEN SET) WITH THE LOWEST F COST do
REMOVE (CURRENT) FROM (OPEN SET)
ADD (CURRENT) TO (CLOSED SET)
IF CURRENT IS TARGET NODE
RETURN
FOR EACH (NEIGHBOUR) OF (CURRENT)
IF ISTRAVERSABLE(NEIGHBOUR) OR (NEIGHBOUR) IN (CLOSED SET)
SKIP
IF NEW PATH TO NEIGHBOUR IS SHORTER OR NEIGHBOUR IS NOT IN (OPEN SET)
SET (F_COST) OF (NEIGHBOUR)
SET (PARENT) OF (NEIGHBOUR) TO (CURRENT)
IF NEIGHBOUR NOT IN (OPEN SET)
ADD (NEIGHBOUR) IN (OPEN SET)
First, we need to define the function and the parameters needed
function FindPath(start: Vector3, target: Vector3)
local OpenSet = {}
local ClosedSet = {}
end
Now we’re going to need a constructor for the nodes, which is just a table:
local function ConstructNode(a: Vector3, b: Vector3, gc)
return {
Position = a;
Costs = {
G = gc;
H = 0; -- placeholder heuristic
F = gc + hc;
};
}
end
Now we add the starting point node to the open set:
function FindPath(start: Vector3, target: Vector3)
local OpenSet = { ConstructNode(start, target, 0) }
local ClosedSet = {}
end
We need to round the starting point and the end point to be in a x^2 grid
local SIZE = 5
local function RoundToNearest(v)
return Vector3.new(
math.round(v.X / SIZE) * SIZE,
math.round(v.Y / SIZE) * SIZE,
math.round(v.Z / SIZE) * SIZE
)
end
function FindPath(start: Vector3, target: Vector3)
start = RoundToNearest(start)
target = RoundToNearest(target)
After that we need to make the while loop and get the node with the lowest cost:
-- ...
while OpenSet[1] do -- checks if theres any nodes left to be evaluated
table.sort(OpenSet, function(a, b)
return a.Costs.F <= b.Costs.F and a.Costs.H < b.Costs.H
end)
local Node = table.remove(OpenSet, 1)
if Node.Position == target then -- checks if node is the target
return
end
ClosedSet[Node.Position] = true
end
Now for the FOR EACH (NEIGHBOUR) OF (CURRENT) in the pseudocode:
-- this should be a constant thus put it in the global scope (directions are in 2d)
-- the more directions, the more time to compute the path
local DIRECTIONS = {
Vector3.xAxis;
Vector3.zAxis;
Vector3.xAxis * -1;
Vector3.zAxis * -1;
Vector3.zAxis + Vector3.xAxis;
Vector3.xAxis * -1 + Vector3.zAxis;
Vector3.xAxis + Vector3.zAxis * -1;
}
for i, dir in DIRECTIONS do
DIRECTIONS[i] = dir * SIZE -- scale the directions with the grid size
end
local function FindNodeInOpenSet(point: Vector3) -- a function to find a node in the open set since we use arrays
for _, node in OpenSet do
if node.Position == point then
return node
end
end
end
-- ...
for _, direction in DIRECTIONS do
local Neighbor = Node.Position + direction
local IsTraversable = true -- traversable function coming later on
if not IsTraversable or ClosedSet[Neighbor] then
return
end
local Distance = (Node.Position - Neighbor).Magnitude -- basically the G cost
local CostToNeighbor = Node.Costs.G + Distance
local NeighborNode = FindNodeInOpenSet(Neighbor)
local NeighborGCost = NeighborNode and NeighborNode.Costs.G or 0 -- if neighbour node exists, then get the g cost, else its 0
if CostToNeighbor < NeighborGCost or not NeighborNode then
table.insert(OpenSet, ConstructNode(Neighbor, target, CostToNeighbor))
end
end
And most of the logic is done! we just need to fill in the placeholder values I left on. First we need to define the heuristic functions we want, currently I use the manhattan distance heuristic:
function HeuristicFunction(a, b)
return math.abs(a.X - b.X) + math.abs(a.Y - b.Y) + math.abs(a.Z - b.Z)
end
Now we need to implement in the construct node func:
local function ConstructNode(a: Vector3, b: Vector3, gc)
local hc = HeuristicFunction(a, b)
return {
Position = a;
Costs = {
G = gc;
H = hc;
F = gc + hc;
};
}
end
After that we need to make a IsTraversable function for the nodes:
local function NodeIsTraversable(point: Vector3)
local collisions = workspace:GetPartBoundsInBox(CFrame.new(point), Vector3.new(SIZE, 0, SIZE))
if collisions[1] then
return false
end
end
local IsTraversable = NodeIsTraversable(Neighbor)
Now for the last part! Retracing the path! We need to define another array called Parents:
local OpenSet = { ConstructNode(start, target, 0) }
local ClosedSet = {}
local Parents = {}
In the direction loop, after we add the neighbour node to the open set:
table.insert(OpenSet, ConstructNode(Neighbor, target, CostToNeighbor))
Parents[Neighbor] = Node.Position
Now we just need this function to return the path:
local Parents = {}
local function RetracePath()
local Path = {}
local CurrentNode = target
while CurrentNode ~= start do
table.insert(Path, 1, CurrentNode)
CurrentNode = Parents[CurrentNode]
end
return Path
end
And return the path once its done:
if Node.Position == target then
return RetracePath()
end
And thats it! You should have a working A* Pathfinder. Note that this is unoptimized, I have optimized my own to some extent and I’m pretty happy with it. The pathfinder with debug parts in action:
Optimized Heap Sort Pathfinder:
This was pretty rushed so let me know if there’s any thing I missed!