A* Pathfinding Review

Help and Feedback Code Review
1

I made an A* pathfinding algorithm and am curious as to how I can optimize it. This algorithm is based on Sebastian Lague’s video on A* (https://www.youtube.com/watch?v=-L-WgKMFuhE). If anyone sees any optimizations that can be made, please inform me.

Video of Algorithm:
(While running: green=start, red=end, blue=closed, yellow=open.
When finished: red=closed, green=open, blue=path
Recorded with the following wait variables: local INITIAL_DELAY = 3, local WAIT = 0.05, local WAIT_INTERVAL = 10 (every ten loops it waits 0.05 seconds))

Code:

local grid = script.Parent

local START_COLOR =   Color3.fromRGB(0, 197, 0)
local END_COLOR =     Color3.fromRGB(255,0,0)
local WALL_COLOR =    Color3.fromRGB(17, 17, 17)
local DEFAULT_COLOR = Color3.fromRGB(163, 162, 165)

local LABELS = true

local startPiece = nil
local endPiece =   nil
local walls =      {}

for _, piece in grid:GetChildren() do
	if not piece:IsA("Part") then continue end
	--piece.Name = "Grid Piece"
	if piece.Name ~= "WALL" then
		if LABELS then
			local gui = Instance.new("SurfaceGui")
			gui.Name = "Cost Display"
			gui.Face = Enum.NormalId.Top
			gui.PixelsPerStud = 40
			gui.Parent = piece

			local gCost = Instance.new("TextLabel") 
			gCost.Size = UDim2.fromScale(0.2,0.2)
			gCost.Name = "GCost"
			gCost.TextScaled = true
			gCost.BackgroundColor3 = Color3.fromRGB(255,255,255)
			gCost.Parent = gui
			local hCost = Instance.new("TextLabel")
			hCost.AnchorPoint = Vector2.new(1, 0)
			hCost.Size = UDim2.fromScale(0.2,0.2)
			hCost.Position = UDim2.fromScale(1, 0)
			hCost.Name = "HCost"
			hCost.TextScaled = true
			hCost.BackgroundColor3 = Color3.fromRGB(255,255,255)
			hCost.Parent = gui
			local fCost = Instance.new("TextLabel")
			fCost.AnchorPoint = Vector2.new(0.5,0.5)
			fCost.Position = UDim2.fromScale(0.5, 0.5)
			fCost.Size = UDim2.fromScale(0.4, 0.4)
			fCost.Name = "FCost"
			fCost.TextScaled = true
			fCost.BackgroundColor3 = Color3.fromRGB(255,255,255)
			fCost.Parent = gui
		end
		piece:SetAttribute("GCost", 0)
		piece:SetAttribute("HCost", 0)
	end
	
	if piece.Name == "START" then
		if startPiece ~= nil then error("Only one grid piece named 'START' is allowed") end
		startPiece = piece
		piece.Color = START_COLOR
	elseif piece.Name == "END" then
		if endPiece ~= nil then error("Only one grid piece named 'END' is allowed") end
		endPiece = piece
		piece.Color = END_COLOR
	elseif piece.Name == "WALL" then
		table.insert(walls, piece)
		piece.Color = WALL_COLOR
	else
		piece.Color = DEFAULT_COLOR
	end
	
	piece.Anchored = true
end

-----------------------------------
---------===PATHFINDING===---------
-----------------------------------

local function distanceFormula(a: Vector2, b: Vector2): number
	return math.sqrt((a.X - b.X)^2 + (a.Y - b.Y)^2)
end
local function rawDistance(a: Vector2, b: Vector2): number
	local dx = math.abs(a.X - b.X)
	local dy = math.abs(a.Y - b.Y)
	local D = 10  -- cost of straight movement
	local D2 = 14 -- cost of diagonal movement
	return D * (dx + dy) + (D2 - 2 * D) * math.min(dx, dy)
end
local function labelPiece(piece: Part)
	local gCost = piece:GetAttribute("GCost")
	local hCost = piece:GetAttribute("HCost")

	local gui = piece:FindFirstChildWhichIsA("SurfaceGui")

	gui:FindFirstChild("GCost").Text = gCost
	gui:FindFirstChild("HCost").Text = hCost
	gui:FindFirstChild("FCost").Text = gCost + hCost
end
if LABELS then labelPiece(startPiece) end
local function getPieceFromIndex(index: Vector2)
	for _, v in grid:GetChildren() do
		if not v:IsA("Part") then continue end
		if v:GetAttribute("Index") == index then
			return v
		end
	end
	return nil
end
local function getNeighbors(piece)
	local neighbors = {}
	local directions = {
		{ 0, -1 }, { 0, 1 }, { -1, 0 }, { 1, 0 },      -- straight
		{ -1, -1 }, { -1, 1 }, { 1, -1 }, { 1, 1 }     -- diagonals
	}
	local index = piece:GetAttribute("Index")
	for _, dir in directions do
		local nx = index.X + dir[1]
		local ny = index.Y + dir[2]
		local neighbor = getPieceFromIndex(Vector2.new(nx, ny))
		
		if neighbor and neighbor.Name ~= "WALL" then
			table.insert(neighbors, neighbor)
		end
	end
	
	return neighbors
end

local open = {}
local closed = {}
local cameFrom = {}

table.insert(open, startPiece)

local current = startPiece

local i = 0

local INITIAL_DELAY = 3
local WAIT = 0.05
local WAIT_INTERVAL = 10 --how many loops before it waits, 1 for every loop
WAIT_INTERVAL = math.floor(WAIT_INTERVAL)

task.wait(INITIAL_DELAY)

while WAIT_INTERVAL <= 1 and task.wait(WAIT) or true do
	i += 1
	
	if WAIT_INTERVAL >= 1 and i%WAIT_INTERVAL == 0 then
		task.wait(WAIT)
	end
	
	table.remove(open, table.find(open, current))
	table.insert(closed, current)
	
	current.Color = Color3.fromRGB(0,0,255)
	
	if current == endPiece then
		print("Path found :)")
		break
	end
	
	for _, neighbor: Part in getNeighbors(current) do
		if table.find(closed, neighbor) then continue end
		
		local gCost = math.floor(distanceFormula(current:GetAttribute("Index"), neighbor:GetAttribute("Index"))*10) + 
			current:GetAttribute("GCost")
		if gCost < neighbor:GetAttribute("GCost") or not table.find(open, neighbor) then
			neighbor:SetAttribute("GCost", gCost)
			neighbor:SetAttribute("HCost", rawDistance(neighbor:GetAttribute("Index"), endPiece:GetAttribute("Index")))
			
			if LABELS then labelPiece(neighbor) end
			
			neighbor.Color = Color3.fromRGB(255,255,0)
			
			cameFrom[neighbor] = current
		end
		
		if not table.find(open, neighbor) then table.insert(open, neighbor) end
	end
	
	local lowest = math.huge
	for _, piece in open do
		local gCost = piece:GetAttribute("GCost")
		local fCost = gCost + piece:GetAttribute("HCost")
		if fCost < lowest then
			lowest = fCost
		end
	end
	
	local bestPieces = {}
	for _, piece in open do
		local fCost = piece:GetAttribute("GCost") + piece:GetAttribute("HCost")
		if fCost <= lowest then
			table.insert(bestPieces, piece)
		end
	end
	local lastCurrent = current
	if bestPieces[2] then
		local bestPiece = nil
		lowest = math.huge
		for _, piece in bestPieces do
			local hCost = piece:GetAttribute("HCost")
			if hCost < lowest then
				lowest = hCost
				bestPiece = piece
			end
		end 
		current = bestPiece
	else
		current = bestPieces[1]
	end
end

local path = {}
current = endPiece

while current ~= nil do
	table.insert(path, current)
	current = cameFrom[current]
end

for _, v in closed do
	v.Color = Color3.fromRGB(255,0,0)
end
for _, v in open do
	v.Color = Color3.fromRGB(0,255,0)
end

for _, v in path do
	v.Color = Color3.fromRGB(0,0,255)
end

Thanks.

1 Like
2

Your current open list uses table.insert and table.remove(open, table.find(open, current)). table.find is an O(n) operation (it has to scan the list), and table.remove can also be O(n) as it may need to shift elements. Finding the node with the lowest F cost also involves iterating through the entire open list (for _, piece in open do …) These operations become very slow as the open list grows.

You can implement a the open as a min-heap this should reduce it to a O(log n) operation for remove insert update and Get the element with the minimum value (in this case, the lowest F cost) in O(1) time (after heapify).

I don’t believe Luau has a built-in heap. You’ll need to implement one yourself or find a reliable Luau heap implementation. The main functions are heapifyUp, heapifyDown, insert, and extractMin.

I would also suggest using a hash set (Dictionary) for the closed set and optimising getPieceFromIndex by pre-processing the grid as the function iterates through all children every time it’s called. This is highly inefficient, especially for larger grids.

1 Like
3

Its likely more performant to perform A* directly on tables rather than instances, and then draw the A* path onto the instance after if thats all your doing with it.

1 Like
4

Thank you for the advice. I will add the stuff u guys recommended within the next few days after I finish up a side project I am working on.

I don’t really know much of anything about what @TheYusufGamer said (it is almost fs way out of my realm of programming). I will research that.
I will for sure implement doing A* with tables. For debugging, it was a little easier to see it done in real time on the parts and I kinda was getting frustrated with this code so I didn’t wanna change the code to use tables and transcribe it onto the parts once I got it to work.

Thanks.

Edit: I ended up doing this today because the main optimizations were quite easy to implement. The new code will be posted below. I will edit the reply later with the finalized code with all of the enhancements and optimizations you guys suggested.

1 Like
5

First “wave” of optimizations:

  • Removed all traces of table.find and table.remove
    • Done by using hash sets/dictionaries instead of normal index lists
    • Instead of adding the piece to closed or open, it now sets the id of the piece to true within the list
  • Changed it to instead use a node system based off of the world grid (createNode())
    • Uses ids instead of index look-ups
    • Never loops through the entire grid
    • New variable grid replaces what is now called worldGrid
    • All nodes store their original in-game piece (worldPiece) for the sake of adding labels and colors
  • Also removed a few lines that did nothing (idk why those were there in the first place)

Code:

local worldGrid = script.Parent
local grid =      {}

local START_COLOR =   Color3.fromRGB(0, 197, 0)
local END_COLOR =     Color3.fromRGB(255,0,0)
local WALL_COLOR =    Color3.fromRGB(17, 17, 17)
local DEFAULT_COLOR = Color3.fromRGB(163, 162, 165)

local LABELS = true

local startPiece = nil
local endPiece =   nil
local walls =      {}


local function createNode(gridPiece: BasePart): {}
	return {
		x =  gridPiece:GetAttribute("Index").X,
		y =  gridPiece:GetAttribute("Index").Y,
		id = gridPiece:GetAttribute("Index").X..", "..gridPiece:GetAttribute("Index").Y,
		gCost = 0,
		hCost = 0,
		traversable = true,
		worldPiece = gridPiece
	}
end
for i, piece in worldGrid:GetChildren() do
	if not piece:IsA("BasePart") then continue end
	if piece.Name ~= "WALL" then
		if LABELS then
			local gui = Instance.new("SurfaceGui")
			gui.Name = "Cost Display"
			gui.Face = Enum.NormalId.Top
			gui.PixelsPerStud = 40
			gui.Parent = piece

			local gCost = Instance.new("TextLabel") 
			gCost.Size = UDim2.fromScale(0.2,0.2)
			gCost.Name = "GCost"
			gCost.TextScaled = true
			gCost.BackgroundColor3 = Color3.fromRGB(255,255,255)
			gCost.Parent = gui
			local hCost = Instance.new("TextLabel")
			hCost.AnchorPoint = Vector2.new(1, 0)
			hCost.Size = UDim2.fromScale(0.2,0.2)
			hCost.Position = UDim2.fromScale(1, 0)
			hCost.Name = "HCost"
			hCost.TextScaled = true
			hCost.BackgroundColor3 = Color3.fromRGB(255,255,255)
			hCost.Parent = gui
			local fCost = Instance.new("TextLabel")
			fCost.AnchorPoint = Vector2.new(0.5,0.5)
			fCost.Position = UDim2.fromScale(0.5, 0.5)
			fCost.Size = UDim2.fromScale(0.4, 0.4)
			fCost.Name = "FCost"
			fCost.TextScaled = true
			fCost.BackgroundColor3 = Color3.fromRGB(255,255,255)
			fCost.Parent = gui
		end
	end
	
	grid[i] = createNode(piece)
	if piece.Name == "START" then
		if startPiece ~= nil then error("Only one grid piece named 'START' is allowed") end
		startPiece = grid[i]
		piece.Color = START_COLOR
	elseif piece.Name == "END" then
		if endPiece ~= nil then error("Only one grid piece named 'END' is allowed") end
		endPiece = grid[i]
		piece.Color = END_COLOR
	elseif piece.Name == "WALL" then
		grid[i].traversable = false
		table.insert(walls, grid[i])
		piece.Color = WALL_COLOR
	else
		piece.Color = DEFAULT_COLOR
	end
	
	piece.Anchored = true
end

-----------------------------------
---------===PATHFINDING===---------
-----------------------------------

local function distanceFormula(a: {number}, b: {number}): number
	return math.sqrt((a.X - b.X)^2 + (a.Y - b.Y)^2)
end
local function rawDistance(a: {number}, b: {number}): number
	local dx = math.abs(a.X - b.X)
	local dy = math.abs(a.Y - b.Y)
	local D = 10  -- cost of straight movement
	local D2 = 14 -- cost of diagonal movement
	return D * (dx + dy) + (D2 - 2 * D) * math.min(dx, dy)
end
local function labelPiece(piece: {})
	local gCost = piece.gCost
	local hCost = piece.hCost

	local gui = piece.worldPiece:FindFirstChildWhichIsA("SurfaceGui")

	gui:FindFirstChild("GCost").Text = gCost
	gui:FindFirstChild("HCost").Text = hCost
	gui:FindFirstChild("FCost").Text = gCost + hCost
end
if LABELS then labelPiece(startPiece) end
local function getPieceFromIndex(index: string)
	for _, node in grid do
		if node.id == index then
			return node
		end
	end
	return nil
end
local function getNeighbors(piece)
	local neighbors = {}
	local directions = {
		{ 0, -1 }, { 0, 1 }, { -1, 0 }, { 1, 0 },      -- straight
		{ -1, -1 }, { -1, 1 }, { 1, -1 }, { 1, 1 }     -- diagonals
	}
	local index = {x = piece.x, y = piece.y}
	for _, dir in directions do
		local nx = index.x + dir[1]
		local ny = index.y + dir[2]
		local neighbor = getPieceFromIndex(nx..", "..ny)
		if neighbor and neighbor.traversable == true then
			table.insert(neighbors, neighbor)
		end
	end
	
	return neighbors
end

local open =     {}
local closed =   {}
local cameFrom = {}

open[startPiece.id] = true

local current = startPiece

local i = 0

local INITIAL_DELAY = 2
local WAIT =          0
local WAIT_INTERVAL = 0 --how many loops before it waits, 1 for every loop
WAIT_INTERVAL = math.floor(WAIT_INTERVAL)

task.wait(INITIAL_DELAY)

while WAIT_INTERVAL <= 1 and (WAIT > 0 and task.wait(WAIT)) or true do
	i += 1
	
	if WAIT_INTERVAL > 1 and i%WAIT_INTERVAL == 0 then
		task.wait(WAIT)
	end
	open[current.id] = nil
	closed[current.id] = true
	
	current.worldPiece.Color = Color3.fromRGB(0,0,255)
	
	if current == endPiece then
		print("Path found :)")
		break
	end
	
	for _, neighbor: Part in getNeighbors(current) do
		if closed[neighbor.id] ~= nil then continue end
		
		local gCost = math.floor(distanceFormula({X = current.x, Y = current.y}, {X = neighbor.x, Y = neighbor.y})*10) + 
			current.gCost
		if gCost < neighbor.gCost or open[neighbor.id] == nil then
			neighbor.gCost = gCost
			neighbor.hCost = rawDistance({X = neighbor.x, Y = neighbor.y}, {X = endPiece.x, Y = endPiece.y})
			
			if LABELS then labelPiece(neighbor) end
			
			neighbor.worldPiece.Color = Color3.fromRGB(255,255,0)
			
			cameFrom[neighbor] = current
		end
		
		if open[neighbor.id] == nil then open[neighbor.id] = true end
	end
	
	local lowest = math.huge
	for id, isOpen in open do
		local piece = getPieceFromIndex(id)
		local gCost = piece.gCost
		local fCost = gCost + piece.hCost
		if fCost < lowest then
			lowest = fCost
		end
	end
	
	local bestPieces = {}
	for id, isOpen in open do
		local piece = getPieceFromIndex(id)
		local fCost = piece.gCost + piece.hCost
		if fCost <= lowest then
			table.insert(bestPieces, piece)
		end
	end
	
	if bestPieces[2] then
		local bestPiece = nil
		lowest = math.huge
		for _, piece in bestPieces do
			local hCost = piece.hCost
			if hCost < lowest then
				lowest = hCost
				bestPiece = piece
			end
		end 
		current = bestPiece
	else
		current = bestPieces[1]
	end
end

local path = {}
current = endPiece

while current ~= nil do
	table.insert(path, current)
	current = cameFrom[current]
end

local function getWorldNodeFromId(id: string)
	for _, v in worldGrid:GetChildren() do
		local node = getPieceFromIndex(id)
		if v:GetAttribute("Index") == Vector2.new(node.x, node.y) then
			return v
		end
	end
	return nil
end

for id, _ in closed do
	getPieceFromIndex(id).worldPiece.Color = Color3.fromRGB(255,0,0)
end
for id, _ in open do
	getPieceFromIndex(id).worldPiece.Color = Color3.fromRGB(0,255,0)
end

for _, v in path do
	getPieceFromIndex(v.id).worldPiece.Color = Color3.fromRGB(0,0,255)
end

Cool thing about this is now I don’t need to yield when running this code. I can just do while true do and there is no noticeable stutter. The only time it stutters is when it is coloring all of the parts at the end (only when WAIT = 0), but I don’t really care because I would never do that in an actual usage of this code.

1 Like