astar

astar.lua

local M = {}

local queue = require "minheap"
local open = queue:create()
local close = {}
local camefrom = {}
local F = {}
local G = {}
local H = {}
local function clear(tbl)
	for k, _ in pairs(tbl) do
		tbl[k] = nil
	end
end
local function distance(a, b)
	local x = a.x - b.x
	local y = a.y - b.y
	return math.sqrt(x*x + y*y)
end

local function openit(current, neighbor, goal)
	if not neighbor.canpass then
		return
	end
	local g = G[current] + distance(current, neighbor)
	local h = distance(neighbor, goal)
	local f = g + h
	local n = close[neighbor]
	if n then
		if  n <= f then --closed, and not the nearest
			return
		end
		close[neighbor] = nil
	else
		n = open:score(neighbor)
		if n and n <= f then
			return 
		end
	end
	print("score:", neighbor.id, n, f)
	--first appear or closed or nearest
	print("push:", current.id, "->", neighbor.id, "g:", g, "h:", h, "f:", f)
	open:push(neighbor, f)
	--print(neighbor.id, "came from", current.id)
	camefrom[neighbor] = current
	G[neighbor] = g
	H[neighbor] = h
	F[neighbor] = f
end

--[[
start == 'point'
goal = 'point'
point = {
	x = 1,
	y = 2,
	canpass = false,
	neighbors = {[1] = 'point', [2] = 'point', ..}
}
]]--

function M.search(start, goal,back)
	local  route
	local current
	local f, g, h
	g = 0
	h = distance(start, goal)
	f = g + h
	G[start] = g
	H[start] = h
	F[start] = f
	print("start", start.id, f)
	open:push(start, f)
	current, score = open:pop()
	while current do
		print("----test id", current.id)
		if current == goal then
			break
		end
		close[current] = score
		for _, neighbor in pairs(current.neighbors) do
			openit(current, neighbor, goal)
		end
		current, score = open:pop()
	end
	if current then
		route = {goal.id}
		local nxt = camefrom[goal]
		while nxt do
			route[#route + 1] = nxt.id
			nxt = camefrom[nxt]
		end
	end
	if route then
		if back ~= nil then
			back(route)
		end
		print("find it:", table.concat(route, "<-"))
	else
		print("find fail")
	end
	clear(close)
	clear(camefrom)
	clear(F)	
	clear(G)	
	clear(H)
	open:clear()
end

--test
do
	local xm, ym = 8, 6
	local map = {}
	for x = 1, xm do
		for y = 1, ym do
			local m = {
				id = x * 100 + y,
				x = x,
				y = y,
				canpass = true,
				neighbors = false,
			}
			map[m.id] = m
		end
	end
	local side = {
		{-1, 0,},
		{0, 1},
		{1,0},
		{0, -1},
		{-1, 1},
		{1, 1},
		{-1, -1},
		{1, -1},
	}
	around = function (g)
		local neighbors = {}
		local x, y = g.x, g.y
		for _, v in pairs(side) do
			local xx = x + v[1]
			local yy = y + v[2]
			if xx >= 1 and xx <= xm and yy >= 1 and yy <= ym then
				local id = xx * 100 + yy
				print("around:", g.id, "=>", id)
				local n = assert(map[id], id)
				neighbors[#neighbors+1] = n
			end
		end
		g.neighbors = neighbors
	end
	for k, v in pairs(map) do
		around(v)
	end
	map[501].canpass = false
	map[502].canpass = false
	map[503].canpass = false
	map[504].canpass = false
	map[505].canpass = false
	M.search(map[303], map[703])
end

return M

minheap.lua

local M = {}
local mt = {__index = M}

local function down(self, i)
	local count = self.__count // 2
	local __score = self.__score
	while i <= count do
		local li = i * 2
		local ri = li + 1
		local p = self[i]
		local l = self[li]
		local r = self[ri]
		local pscore = __score[p]
		local lscore = __score[l]
		if r then
			local rscore = __score[r]
			if lscore > rscore then
				l = r
				li = ri
				lscore = rscore
			end
		end
		if pscore <= lscore then
			break
		end
		self[l] = i
		self[p] = li
		self[i] = l
		self[li] = p
		i = li
	end
end

local function up(self, i)
	local __score = self.__score
	while i > 1 do
		local pi = i // 2
		local p = self[pi]
		local c = self[i]
		local pscore = __score[p]
		local cscore = __score[c]
		if pscore <= cscore then
			break
		end
		self[p] = i
		self[c] = pi
		self[i] = p
		self[pi] = c
		i = pi
	end
end

function M:create()
	return setmetatable({
		__count = 0,
		__score = {}
	}, mt)
end

function M:clear()
	local score = self.__score
	for k, _ in pairs(self) do
		self[k] = nil
	end
	for k, _ in pairs(score) do
		score[k] = nil
	end
	self.__count = 0
	self.__score = score
end

function M:push(obj, score)
	local n
	local __score = self.__score
	local s = __score[obj]
	if not s then --first
		n = self.__count + 1
		self.__count = n
		self[n] = obj
		self[obj] = n
		__score[obj] = score
		up(self, n)
	elseif score > s then --down
		local idx = self[obj]
		__score[obj] = score
		if idx > 1 then
			idx = idx // 2
		end
		down(self, idx)
	elseif score < s then --up
		__score[obj] = score
		up(self, self[obj])
	end
end

function M:score(obj)
	return self.__score[obj]
end

function M:pop()
	local obj = self[1]
	if not obj then --empty
		return
	end
	local __score = self.__score
	local score = __score[obj]
	local n = self.__count
	self[1] = self[n]
	self[n] = nil
	__score[obj] = nil
	print("pop", obj.id)
	n = n - 1
	self.__count = n
	down(self, 1)
	return obj, score
end

function M:dump()
	local buf = {}
	local format = string.format
	local __score = self.__score
	for i = 1, self.__count do
		local obj = self[i]
		local score = __score[obj]
		buf[#buf + 1] = format("%s:%s", obj, score)
	end
	print(table.concat(buf, ","))
end

return M