A* Pathfinding Support Using Heaps

Guruu_G · November 1, 2019, 8:19am

I’m a little new to Lua, and I’m not fairly sure how to write to the standard of Lua. But I wrote a pathfinding service in C# once, and I’ve nearly replicated the entire script. From creating the grid to creating my heap.

I’m hoping someone here today, can help me figure out why my main loop is repeating forever and never really breaking to find the end path. Here is the main find path function

function PathFinding:FindPath(StartPos, EndPos)
	local waypoints = {}
	local pathSuccess = false
	local start_node = Grid:NodeFromWorldPosition(StartPos)
	local target_node = Grid:NodeFromWorldPosition(EndPos)
	
	start_node.parent = start_node 
	print("TargetNode walkable?: ", target_node.walkable, " StartNode walkable: ", start_node.walkable)
	if(start_node.walkable and target_node.walkable) then
		local openset = Heap.new()
		local closedset = Heap:new()
		openset:Add(start_node)
		start_node = openset.Items[openset.currentcount - 1]
		local count = 1
		while (openset.currentcount > 1) do
			print("before pop: ", openset.currentcount)
			local currentNode = openset:RemoveFirst()
			print("after pop: ", openset.currentcount)
			closedset:Add(currentNode)
			currentNode = openset.Items[closedset.currentcount -1]

			if(currentNode:Equals(target_node)== true) then
				pathSuccess = true
				print("broke path. cause same node.")
				break
			end
			for i,v in pairs(Grid:GetNeighbours(currentNode)) do
			
				if(v.walkable ==  false or closedset:Contains(v) == true) then
					
				else
					local MovementCostToNeighbour = currentNode.gcost + GetDistance(currentNode, v) + v.movementPenalty
					if(MovementCostToNeighbour < v.gcost or openset:Contains(v) == false) then
						v.gcost = MovementCostToNeighbour
						v.hcost = GetDistance(v, target_node)
						v.fcost = v.gcost + v.hcost
						v.parent = currentNode
						if(openset:Contains(v) == false) then
							openset:Add(v)
						else
							openset:SortUp(v)
						end
						count = count + 1;
					end
				end
			end
		end
	end
	if(pathSuccess == true) then
		print("Retracing path.")
		waypoints = RetracPath(start_node,target_node)
		pathSuccess = (waypoints ~= nil and #waypoints > 1)
	end
	if(pathSuccess) then
		print("Waypoints to create is :", #waypoints)
	else
		print("No path available.")
	end
end

And here is my fairly useful heap class replication (module)

local Heap = {}
Heap.__index = Heap

function Heap.new()
	local heap = {}
	setmetatable(heap, Heap)
	heap.Items = {}
	heap.currentcount = 1

	return heap
end

function Heap:RemoveFirst()
	local first = self.Items[1] 
	self.currentcount = self.currentcount - 1
	self.Items[1] = self.Items[self.currentcount]
	self.Items[1].heapIndex = 1
	self:SortDown(self.Items[1])
	print(first)
	return first
end

function Heap:Add(Node)
	Node.heapIndex = self.currentcount
	self.Items[Node.heapIndex] = Node
	self.currentcount = self.currentcount + 1
	self:SortUp(Node)
end

function Heap:SortUp(Node)
	if(Node.heapIndex ~= 1) then
		local parent = (Node.heapIndex) / 2
		while (true) do 
			local parentItem = self.Items[parent]
			if(parentItem == nil or Node:CompareTo(parentItem) > 0) then
				self:Swap(Node, parentItem)
			else
				break;
			end
			parent = (Node.heapIndex) / 2
		end
	end
end

function Heap:SortDown(Node)
	while (true) do
		local childLeftIndex = Node.heapIndex * 2
		local childRightIndex = Node.heapIndex * 2 + 1
		local swapId = 0
		if(childLeftIndex < self.currentcount) then
			swapId = childLeftIndex
			if(childRightIndex < self.currentcount) then
				if(self.Items[childLeftIndex]:CompareTo(self.Items[childRightIndex]) < 0) then
					swapId = childRightIndex
				end
			end
			if (Node:CompareTo(self.Items[swapId]) < 0) then
				self:Swap(Node, self.Items[swapId])
			else
				return
			end
		else
			return
		end
	end
	
end

function Heap:Contains(Node)
	for _,v in pairs (self.Items) do
		if(v:Equals(Node)) then
			return true
		end
	end
	return false
end

function Heap:Swap(NodeA, NodeB)
	if(NodeA == nil or NodeB == nil) then
		return
	end
	self.Items[NodeA.heapIndex] = NodeB
	self.Items[NodeB.heapIndex] = NodeA
	local AIndex = NodeA.heapIndex
	NodeA.heapIndex = NodeB.heapIndex
	NodeB.heapIndex = AIndex 
end


return Heap

Here is my Node:Equals()

function Node:Equals(Node)
	if(self.gridX == Node.gridX and self.gridY == Node.gridY and self.fcost == Node.fcost and self.gcost == Node.gcost and self.hcost == Node.hcost) then
		return true
	end
	return false
end

Romeo_Duncan · November 1, 2019, 9:42am

i think you need to add a wait() at the end of the body within your while loop

Guruu_G · November 1, 2019, 7:09pm

I tried this actually, but that did not end up working. I have no idea really…

Emskipo · November 1, 2019, 7:36pm

When I was dealing with the built in roblox pathfinding service. I had issues with glitches where the movement stalled out or couldn’t detect the player reaching a waypoint. I realize this is not the quite the same as the pathfinding service. But I was able to resolve the issue by eliminating the checks on the Y coordinate. I noticed that the player position’s Y coordinate wasn’t always matching the target position’s Y coordinates and it was causing the code hang. I wonder if you might be having the same sort of issue. I believe it was the Y coordinate anyways it has been awhile since I was messing with it. Whatever coordinate dealt with Up/Down. The drawback of course is if you need the player to climb something like a ladder it can potentially cause an issue. But I’m sure a workaround can made to deal with that situation. Hope this helps.

nooneisback · November 1, 2019, 8:00pm

The issue might be with the way you handle the tables. If a table contains 5 values, the last value’s index will be 5. The first value in an array has the index of 1 as it doesn’t represent a space in memory. The reason behind this is that rrays in Lua aren’t real arrays, but hash tables.

Just remove the -1 in the table indexing and it should work.

Also, you don’t need a heap module as the table library already does most of the job for you in case your table is an array. table.remove(arr) will remove the first element and sort the entire table. table.insert(arr,val) will add the value to the heap. You can also just use arr[#arr+1]=val to speed it up. Table look-ups and function calls have a considerable overhead, especially in cases like yours where they have to be done multiple times for a potentially massive amount of nodes.

Guruu_G · November 2, 2019, 1:30am

Still doesnt seem to work. I keep getting into an infinite loop

BanTech · November 2, 2019, 2:07am

What’s telling you that it goes into an infinite loop? Are you getting loads of the same few print statements before the whole client hangs? The fact there’s no yielding means the client will freeze entirely (or the server if you’re running it there).

It’d be interesting to know if it got stuck in a certain part of the loop, or if it’s the whole loop, and whether the printed values are changing as expected, what’s happening to the currentcount, etc. May help to narrow down the cause.

Guruu_G · November 2, 2019, 3:35am

Well, i have an algorithm here that works. The problem is I am trying to optimize it now.
I am trying to sort my closed/open lists as needed to have the minimum fcost node always be the first one. I took some ideas from other A* PathFinding algorithms posted before. I optimized it as much as i could to get it to do some pretty decent distance pathfinding with no time at all less then 1 second. But the real problems come from really far nodes and the main loop for finding the minimum fcost value each iteration of the while loop.

Currently my algorithm now takes 5 seconds to go from one end of the map to the other. Given the size of the grid is 2048x2048, i believe theres a possible 4 million + nodes?

5 seconds is good. But no one wants to wait 5 seconds to really auto walk to lets say a quest thats located far out…

Heres what i got so far…

local function GetPath(StartPos, EndPos) 
	local OpenSet = {};
	local ClosedSet = {};
	local Path = {}
	StartPos.heapIndex = #OpenSet + 1
	table.insert(OpenSet,StartPos);
	local Start = tick();
	local Time = 0;
	while #OpenSet > 0 do
		if #OpenSet > 0 then
			--Determine Smallest FValue
			local Min = 1;
			for i = 1,#OpenSet do
				if OpenSet[i].fcost < OpenSet[Min].fcost then
					Min = i;
				end
				
			end
			print("min is :" , Min)
			local Current = OpenSet[Min];
			--Close for Winner
			if Current == EndPos then
				print("end because current == endpos")
				break
				---break;
			end
			RemoveFromTable(OpenSet,Current);
			Current.heapIndex = #ClosedSet + 1
			table.insert(ClosedSet,Current);	
			local Neighbors = Grid:GetNeighbours(Current);
			for i = 1,#Neighbors do
				local Neighbor = Neighbors[i];
				local NewPath = false
				
				if (not CheckInclusion(ClosedSet,Neighbor)) then
					local _TempG = Current.gcost + 1;
	
					if CheckInclusion(OpenSet, Neighbor) then
						if _TempG < Neighbor.gcost then
							Neighbor.gcost = _TempG;
							NewPath = true;
						end
					else
						Neighbor.gcost = _TempG;
						NewPath = true
						Neighbor.heapIndex = #OpenSet + 1
						table.insert(OpenSet,Neighbor);
					end
				end
				if NewPath then
					Neighbor.hcost = GetDistance(Neighbor, EndPos)
					Neighbor.fcost = Neighbor.gcost + Neighbor.hcost
					Neighbor.previous = Current;
				end
			end
			
			
			
			Path = {};
			local Temp = Current;
			table.insert(Path,Temp);
			local Value = 1;
			while Temp.previous do 
				Value = Value + 1;
				table.insert(Path,Temp.previous)
				Temp = Temp.previous;
			end
			--Current:ShowNeighbors();
			--Current:ShowNeighbors()
		end
		--wait()
	end
	print("Time it took to find path in ms:", (tick()-Start)-Time)
	
	return Path
end

I have these other functions kinda here, to help guide me on the process. But every time I try to implement any sorta sorting, I seem to get stuck and frozen in a loop. Probably somewhere within the sorting algorithms.

local function RemoveFromTable(Table,Value)
	table.remove(Table, Value.heapIndex)
end

local function Exists(Table, Value)
	if(Table[Value.heapIndex] == Value) then
		return true
	end
	return false;
end

local function CheckInclusion(Table,Value)
	if Table[Value.heapIndex] == Value then
		return true
	end
	for i = 1,#Table do 
		if Table[i] == Value then
			return true;
		end
	end
	return false;
end

local function Swap(_Items, itemA, itemB)
	_Items[itemA.heapIndex] = itemB;
    _Items[itemB.heapIndex] = itemA;
    local AIndex = itemA.heapIndex;
    itemA.heapIndex = itemB.heapIndex;
    itemB.heapIndex = AIndex;

 	return _Items
end

local function Compare(a, b)
	if a.fcost < b.fcost then
		return true
	else
		return false
	end
end
local function SortDown(Items, index)
	local leftChildIndex, rightChildIndex, minIndex
	leftChildIndex = index * 2
	rightChildIndex = index * 2 + 1
	if rightChildIndex > #Items then
		if leftChildIndex > #Items then
			return Items
		else
			minIndex = leftChildIndex
		end
	else
		if not Compare(Items[leftChildIndex], Items[rightChildIndex]) then
			minIndex = leftChildIndex
		else
			minIndex = rightChildIndex
		end
	end
	
	if Compare(Items[index], Items[minIndex]) then
		Items[minIndex], Items[index] = Items[index], Items[minIndex]
		return SortDown(Items, minIndex)
	end
	return Items
end
local function SortUp(Items, index)
	local parentIndex
	if index ~= 1 then
		parentIndex = math.floor(index/2)
		if Compare(Items[parentIndex], Items[index]) then
			Items[parentIndex], Items[index] = Items[index], Items[parentIndex]
			return SortUp(parentIndex)
		else
		end
	end

	return Items
end

please note before critizing the return Items and return Sorts, i was just trying random things cause i wasn’t sure if recursion worked the same way it did for lua compared to other languages…for example i didnt know if calling Sortup within a function that returns Items at the end would return the final sorted list… but nothing i tried worked so i kinda just left it there as is.

Guruu_G · November 2, 2019, 3:52am

The original Heap sort sortup is where it got stuck. I’m guessing it was the CompareTo function…

For example…

    function PathFinding:FindPath(StartPos, EndPos)
    	local nodeA = NodeClass.new(true,Vector3.new(0,0,0),1,1,1)
    	local nodeB = NodeClass.new(true,Vector3.new(1,1,1),2,3,4)
    	local nodeC = NodeClass.new(true,Vector3.new(3,3,3),4,6,4)
    	nodeA.fcost = 5
    	nodeB.fcost = 1
    	nodeC.fcost = 2

    	local list = Heap.new()
    	list = list:Add(nodeA)
    	list = list:Add(nodeB)
    	list = list:Add(nodeC)
    	
    	for _,v in pairs(list) do
    		print("GridX: ", v.gridX, " GridY: ", v.gridY, " fcost: ", v.fcost)
    	end
    	
    end

seems to start giving me an infinite loop here.

 function Heap:SortUp(Node)
	if(Node.heapIndex ~= 1) then
		local parent = (Node.heapIndex) / 2
		while (true) do 
			local parentItem = self.Items[parent]
			if(Compare(Node, parentItem) == true) then
				print("Performing swap...")
				self:Swap(Node, parentItem)
			else
				break;
			end
			parent = (Node.heapIndex) / 2
		end
	end
end

Guruu_G · November 2, 2019, 4:02am

I’ve got it sorting down once now… not really more then that tho… one swap and thats it…Doesn’t do the rest… seems to really break and hit an infinite loop when i have an even number of elements in the list…

heres what im trying to sort rn…

function Heap:Add(Node)
	Node.heapIndex = #self.Items + 1
	self.Items[Node.heapIndex] = Node
	self.currentcount = #self.Items
	self:SortUp(Node)
end


function Heap:Compare(a, b)
	if a < b then
		return true
	else
		return false
	end
end

function Heap:SortUp(Node)
	if(Node.heapIndex ~= 1) then
		local parent = math.floor(Node.heapIndex / 2)
		while (true) do 
			local parentItem = self.Items[parent]
			if(parentItem ~= nil and self:Compare(Node.fcost, parentItem.fcost) == true) then
				self:Swap(Node, parentItem)
				--Node = self.Items[Node.heapIndex]
			else
				break;
			end
			parent = math.floor(Node.heapIndex / 2)
		end
	end
end

If anyone got any bright ideas…

function PathFinding:FindPath(StartPos, EndPos)
	local nodeA = NodeClass.new(true,Vector3.new(0,0,0),1,1,1)
	local nodeB = NodeClass.new(true,Vector3.new(1,1,1),2,3,4)
	local nodeC = NodeClass.new(true,Vector3.new(3,3,3),4,6,4)
	local nodeD = NodeClass.new(true,Vector3.new(3,3,3),8,6,4)
	nodeA.fcost = 5
	nodeB.fcost = 1
	nodeC.fcost = 2
	nodeD.fcost = 3

	local list = Heap.new()
	list:Add(nodeA)
	list:Add(nodeB)
	list:Add(nodeC)
	list:Add(nodeD)
	
	
	for _,v in pairs(list.Items) do
		print("GridX: ", v.gridX, " GridY: ", v.gridY, " fcost: ", v.fcost)
	end
	
end

Guruu_G · November 2, 2019, 5:29am

Minor changes to the code above…

function Heap:SortUp(Node)
	if(Node.heapIndex ~= 1) then
		local parent = math.floor(Node.heapIndex / 2)
		while (true) do 
			local parentItem = self.Items[parent]
			if(parentItem ~= nil and self:Compare(Node.fcost, parentItem.fcost) == true) then
				local val = self:Swap(Node, parentItem)
				Node = val[2]
			else
				break;
			end
			parent = math.floor(Node.heapIndex / 2)
		end
	end
end
function Heap:Swap(NodeA, NodeB)
	if(NodeA == nil or NodeB == nil) then
		return
	end
	self.Items[NodeA.heapIndex] = NodeB
	self.Items[NodeB.heapIndex] = NodeA
	local AIndex = NodeA._Index;
   	NodeA.heapindex = NodeB.heapindex;
    NodeB.heapindex = AIndex;
	return {NodeA, NodeB}
end

My Output… for the final list…

GridX: 8 GridY: 6 fcost: 3
GridX: 1 GridY: 1 fcost: 5
GridX: 4 GridY: 6 fcost: 2
GridX: 1 GridY: 1 fcost: 5

As you can see from my local list in my Pathfinding:FindPath() i have 4 nodes
focsts are 5, 1, 2, 3
It seems like after adding a 4th element not only did my fcost nodeB and the data it self such as gridX is completly gone. or i should say replaced with the NodeA…

BanTech · November 2, 2019, 11:20am

Have you tried using table.sort with your own comparison function instead of doing it manually?

You could sort the table using the built in function and then do a for loop over the sorted table to set the heapindex values.

You could then just have a single sort function that isn’t node-specific, instead of having Compare, Swap, SortUp and SortDown:

function Heap:Sort()
    table.sort( self.Items, function ( a, b )
        return a.fcost < b.fcost
    end )
    for index, item in pairs( self.Items ) do
        item.heapindex = index
    end
end

Guruu_G · November 2, 2019, 6:56pm

That would defeat the purpose of being able to sort using nlogn time… This way would probably be worse on the run time when finding path. I’d have to sort thousands of nodes each itteration going through all nodes

Using the above functions I’m trying to sort what I need based on what already exists. And when I add elements I will always sort up because ill always gets added to the end. And when I remove the first element, i’d be sorting down to shift stuff around.

Guruu_G · November 3, 2019, 1:22am

  minimum fcost is at index: 1
  minimum fcost is at index: 5
  minimum fcost is at index: 2
  minimum fcost is at index: 3
  minimum fcost is at index: 5
  minimum fcost is at index: 2
  minimum fcost is at index: 7
  minimum fcost is at index: 3
  minimum fcost is at index: 4
  minimum fcost is at index: 5 (x2)
  minimum fcost is at index: 2
  minimum fcost is at index: 6
  minimum fcost is at index: 7 (x2)
  minimum fcost is at index: 3
  minimum fcost is at index: 4 (x3)
  minimum fcost is at index: 5 (x4)
  minimum fcost is at index: 2
  minimum fcost is at index: 6 (x3)
  minimum fcost is at index: 7 (x4)
  minimum fcost is at index: 3
  minimum fcost is at index: 4 (x7)
  minimum fcost is at index: 5 (x8)
  minimum fcost is at index: 2
  minimum fcost is at index: 6 (x7)
  minimum fcost is at index: 7 (x4)
  minimum fcost is at index: 15
  minimum fcost is at index: 3 (x7)
  minimum fcost is at index: 2 (x64)
  minimum fcost is at index: 3 (x64)
  minimum fcost is at index: 2 (x47)
  Time it took to find path in ms: 0.32014012336731

Seems to kinda sort… but not entirely…

Guruu_G · November 3, 2019, 1:46am

I got it to work and is now sorted!

Still trying to optimize it more but for now Heres the time

  Time it took to find path in ms: 2.9761648178101  number of waypoints created in this time is : 817

The biggest part i’m trying to optimize is:

for i = 1,#Neighbors do
				local Neighbor = Neighbors[i];
				local NewPath = false
				
				if (not CheckInclusion(ClosedSet.Items,Neighbor)) then
					local _TempG = Current.gcost + 1;
	
					if CheckInclusion(OpenSet.Items, Neighbor) then

local function CheckInclusion(Table,Value)
	if Table[Value.heapIndex] == Value then
		return true
	end
	for i = 1,#Table do 
		if Table[i] == Value then
			return true;
		end
	end
	return false;
end

If you have any idea how i can get rid of that loop. IT would get even faster.

Guruu_G · November 3, 2019, 2:12am

My FINAL version

Time it took to find path in ms: 0.89433455467224  number of waypoints created in this time is : 817

To give you an idea of how big this is, there are 4.5+ million possible nodes to go through.
The map size is 2048 by 2048

nooneisback · November 3, 2019, 8:04am

The way I entirely solved this in my floodfill pathfinding is by dividing the map into multiple sectors. The sectors have a “Connections” table which contains arrays of intermediate sectors to load when trying to reach the target. This way, you can significantly decrease the amount of nodes your algorithm has to search through.

Also, you should check out jump point search. It basically avoids iterating over long vertical and horizontal lines.

Guruu_G · November 3, 2019, 8:42am

Basically you’re saying preload my neighbor nodes for each node that way I do have to create it each time? I’m not sure I fully understand. I could see how that could improve my overall speed though. Thank you for the input!

nooneisback · November 3, 2019, 9:22am

This is a simple example of what I mean, though it doesn’t exactly show the advantage. Imagine there is a small cave-like dead-end between the character and the target. With normal A*, you would have to check if the cave has a passage, but by dividing the map into pre-processed sectors, you can completelly avoid it.

Here is the cave example:

If you want an example of jump-point A*, then PathFinding.js has it under Jump Point Search. It is essentially A*, but it eliminates the need to evaluate straight lines.

Guruu_G · November 3, 2019, 11:08pm

Feels way slower the mine to be honest especially on the grid size its got. Way smaller then what i have still takes awhile.