A-Star Pathfinding

astar.lua

--[[Improved A-Star Pathfinding Algorithm
    Ported to by Roland Yonaba - aka SeanPaul223
    28/01/2010]]

--Looks for a node listed in a table of nodes.
--Returns true on success, plus the index where it was found.
--Otherwise returns false plus nil.
function isListed(list,node)
local bool,pos
bool = false
	for k,v in pairs(list) do
		if (v.x==node.x) and (v.y==node.y) then
		bool=true
		pos =k
		break
		end
	end
return bool,pos
end

--Computes the heuristic distance from a node A to a node B
--Uses integer values to speed up computing
function distance(A,B)
return 10*(1+math.abs(A.x-B.x))+10*(1+math.abs(A.y-B.y))
end

--Search for all nodes near a current one on a 2D map array
--Add them to the OpenList under some conditions
function addNode(map,currentNode,finalNode,CList,OList,OBST_VALUE)
--Gets the 2D map Limits
local limitX = #map[currentNode.y]
local limitY = #map
	--Indices near nodes
	for y = currentNode.y-1,currentNode.y+1,1 do
		--Only focus on Y-indices valid on the map
		if (y>=1) and (y<=limitY) then
			for x=currentNode.x-1,currentNode.x+1,1 do
			--Only focus on X-indices valid on the map
				if (x>=1) and (x<=limitX) then
					--Assuming 1 is not a walkable location
					if (map[y][x]~=OBST_VALUE) then
					--We do not consider adjacent moves
					local left = (y==currentNode.y-1) and ((x==currentNode.x-1) or (x==currentNode.x+1))
					local right = (y==currentNode.y+1) and ((x==currentNode.x-1) or (x==currentNode.x+1))
						if not left and not right then
						--Computes a node to study
						local tmpNode = {y=y,x=x}
						tmpNode.G=10+currentNode.G
						tmpNode.H=distance(tmpNode,finalNode)
						tmpNode.F = tmpNode.G+tmpNode.H
						tmpNode.Parent ={x=currentNode.x,y=currentNode.y}
						--We do not consider nodes already stored Closed List
						local enclosed = isListed(CList,tmpNode)
							if not enclosed then
							--We update if necessay nodes already stored in Open List
							local opened,pos = isListed(OList,tmpNode)
								if opened then
									if (tmpNode.F<OList[pos].F) then
									OList[pos] = tmpNode;
									end
								else
								--else we add the studied node in OpenList
								local Ins 
									for pos in pairs(OList) do
										if OList[pos].F>=tmpNode.F then Ins = pos break end
									end
								if not Ins then Ins = #OList end
								table.insert(OList,Ins,tmpNode)
								end
							end
						end
					end
				end
			end
		end
	end
end

--Finds a near walkable node searching from the ordered one
function getNewFreeNode(map,finalNode,OBST,MAX_DEEP)
	function inMap(y,x,map)
		if y>0 and y<#map and x>0 and x<#map[y] then return true
		else return false
		end	
	end	
	function getnext(map,finalNode,OBST,deep)
	local y,x
		y = finalNode.y-deep
		for x = finalNode.x-deep,finalNode.x+deep do
			if inMap(y,x,map) and map[y][x]==0 then return y,x end
		end
		y=finalNode.y+deep
		for x = finalNode.x-deep,finalNode.x+deep do
			if inMap(y,x,map) and map[y][x]==0 then return y,x end
		end
		x = finalNode.x-deep
		for y = finalNode.y-deep,finalNode.y+deep do
			if inMap(y,x,map) and map[y][x]==0 then return y,x end
		end
		x=finalNode.x+deep
		for y = finalNode.y-deep,finalNode.y+deep do
			if inMap(y,x,map) and map[y][x]==0 then return y,x end
		end
	end
	local deep=1
	local newx,newy
	repeat
	newy,newx = getnext(map,finalNode,OBST,deep)
	deep=deep+1
		if (newx~=nil) and (newy~=nil) then break end
	until (deep >= MAX_DEEP)
	return newy,newx
end

--Main function, does all the stuff
--Returns a table of walkable location if a valid path was found
--Otherwise returns nil
function pathfinding(map,startNode,finalNode,OBST_VALUE)
local path={} --will hold the path if found
--sets the starting node as the current one to study
local currentNode = {y= startNode.y, x= startNode.x,G=0,H=0,F=0,Parent = {y=startNode.y,x=startNode.x}}
local CList = {} --the Closed list
local OList = {} --The Open List
--we add the starting node in the Closed list
table.insert(CList,currentNode)
--Test if the dest node is walkable
--If not, tries to find a new Dest node near the previous one
local finalNode = finalNode
	if map[finalNode.y][finalNode.x]==OBST_VALUE then
	local y,x = getNewFreeNode(map,finalNode,OBST_VALUE,5)
		if (y~=nil) and (x~=nil) then
		finalNode.y,finalNode.x=y,x
		print("y="..y,"x="..x)
		end
	end
	repeat
	addNode(map,currentNode,finalNode,CList,OList,OBST_VALUE) --Looks for near nodes
	--Finds the lower cost node
	local bestPos = next(OList)
		if bestPos then
		--table.sort(OList,function(a,b) return a.F<b.F end) --sorts the OpenList basing on F-field of all nodes,in growing order **
		currentNode = OList[bestPos] --sets the lower cost node as the current one to study
		table.remove(OList,bestPos) --removes it from the OpenList
		table.insert(CList,currentNode) --adds it to the Closed List
		else CList = nil break  --stops computing if OpenList is empty.Meaning there is no valid path!
		end
	until (currentNode.x==finalNode.x) and (currentNode.y==finalNode.y)
path=CList
return path --returns path
end

--Returns the ordered path, otherwise nil
function track(p,finalNode,startNode)
	if p then
	local way={}
	local reverseWay={}
	local nxt={x=finalNode.x,y=finalNode.y}
	table.insert(way,1,nxt)
	local nxt=p[#p].Parent
	table.insert(way,1,nxt)
		repeat
		local bool,pos = isListed(p,nxt)
			if bool then
			nxt = p[pos].Parent
			table.insert(way,1,nxt)
			end
		until (nxt.y ==startNode.y and nxt.x == startNode.x)
		return way
	else return nil end
end