255 lines
6.5 KiB
Lua
255 lines
6.5 KiB
Lua
--[[ Lua A* Pathfinder ]] --
|
|
-- By GreenXenith
|
|
-- Minetest's builtin minetest.path_find uses A* and is 10-15x faster, but
|
|
-- doesn't return useful data. Here we roll our own.
|
|
-- NOTE: They don't jump yet.
|
|
local pathfind = {}
|
|
local P = minetest.pos_to_string
|
|
local debug = false
|
|
|
|
local function is_good_node(name)
|
|
return minetest.registered_nodes[name].walkable == false
|
|
end
|
|
|
|
-- Convert 2D area to table of valid nodes
|
|
local function get_nodemap(pos1, pos2, radius)
|
|
local map = {}
|
|
pos1, pos2 = vector.sort(vector.round(pos1), vector.round(pos2))
|
|
pos1.x, pos1.z = pos1.x - radius, pos1.z - radius
|
|
pos2.x, pos2.z = pos2.x + radius, pos2.z + radius
|
|
|
|
local VoxelManip = minetest.get_voxel_manip()
|
|
local min, max = VoxelManip:read_from_map(pos1, pos2)
|
|
local area = VoxelArea:new{MinEdge = min, MaxEdge = max}
|
|
local data = VoxelManip:get_data()
|
|
|
|
for i in area:iterp(pos1, pos2) do
|
|
if is_good_node(minetest.get_name_from_content_id(data[i])) then
|
|
map[minetest.pos_to_string(area:position(i))] = true
|
|
end
|
|
end
|
|
|
|
return map
|
|
end
|
|
|
|
-- Get open space
|
|
function pathfind.find_open_near(pos, depth)
|
|
-- Max check depth
|
|
if not tonumber(depth) or not (depth > 0 and depth <= 100) then
|
|
depth = 20
|
|
end
|
|
local d = 0
|
|
local checked = {[P(pos)] = true}
|
|
local checking = {pos}
|
|
local to_check = {}
|
|
local off = {{x = 0, y = 0, z = 1}, {x = 1, y = 0, z = 0}, {x = 0, y = 0, z = -1}, {x = -1, y = 0, z = 0}}
|
|
-- Might already be fine
|
|
if is_good_node(minetest.get_node(pos).name) then
|
|
return pos
|
|
end
|
|
while d <= depth do
|
|
for _, cpos in pairs(checking) do
|
|
for _, offset in pairs(off) do
|
|
local newpos = vector.add(cpos, offset)
|
|
if not checked[P(newpos)] then
|
|
if is_good_node(minetest.get_node(newpos).name) then
|
|
return newpos
|
|
else
|
|
table.insert(to_check, newpos)
|
|
checked[P(newpos)] = true
|
|
end
|
|
end
|
|
end
|
|
d = d + 1
|
|
end
|
|
checking = table.copy(to_check)
|
|
to_check = {}
|
|
end
|
|
end
|
|
|
|
-- A* Pathfinder
|
|
-- Profiler indicates roughly 5000+/- microseconds of processing time (C++ is 400 microseconds)
|
|
function pathfind.tree(seed, goal, depth)
|
|
-- Neighbor maps
|
|
local neighbors = {
|
|
up = {x = 0, y = 0, z = 1},
|
|
right = {x = 1, y = 0, z = 0},
|
|
down = {x = 0, y = 0, z = -1},
|
|
left = {x = -1, y = 0, z = 0},
|
|
}
|
|
local block = {
|
|
up = {"up_left", "up_right"},
|
|
right = {"up_right", "down_right"},
|
|
down = {"down_right", "down_left"},
|
|
left = {"down_left", "up_left"},
|
|
}
|
|
-- Node map
|
|
local map = get_nodemap(seed, goal, 25)
|
|
seed, goal = vector.round(seed), vector.round(goal)
|
|
|
|
-- Trackers
|
|
local path = {}
|
|
local checked = {}
|
|
local start_nodes = {{pos = {x = math.floor(seed.x), y = seed.y, z = math.floor(seed.z)}, length = 0}}
|
|
local start_leaves = {}
|
|
local end_nodes = {{pos = {x = math.floor(goal.x), y = goal.y, z = math.floor(goal.z)}, length = 0}}
|
|
local end_leaves = {}
|
|
-- Main function to loop
|
|
local function branch(nodes, track)
|
|
local leaves = {}
|
|
for _, node in pairs(nodes) do
|
|
-- Diagonal map
|
|
local diagonals = {
|
|
up_right = {x = 1, y = 0, z = 1},
|
|
down_right = {x = 1, y = 0, z = -1},
|
|
down_left = {x = -1, y = 0, z = -1},
|
|
up_left = {x = -1, y = 0, z = 1},
|
|
}
|
|
-- Check different neighbors
|
|
local function check(directions, length, adjacent)
|
|
for dir, offset in pairs(directions) do
|
|
local pos = vector.add(node.pos, offset)
|
|
local str = P(pos)
|
|
-- Continue if empty space or if new path is shorter
|
|
if not checked[str] or checked[str].node.length > node.length + length then
|
|
-- If it is checked, unoptimal leaves are dried
|
|
if checked[str] then
|
|
local clone = {}
|
|
-- Only keep healthy leaves
|
|
for i, leaf in pairs(leaves) do
|
|
if leaf.parent ~= checked[str].node then
|
|
clone[#clone + 1] = leaf
|
|
end
|
|
end
|
|
leaves = clone
|
|
end
|
|
-- Add leaf if clear
|
|
if map[str] then
|
|
leaves[#leaves + 1] = {parent = node, pos = pos, length = node.length + length}
|
|
checked[str] = {track = 0, node = leaves[#leaves]}
|
|
elseif adjacent then -- Otherwise mark blockage for diagonals
|
|
for _, d in pairs(block[dir]) do
|
|
diagonals[d] = nil
|
|
end
|
|
end
|
|
-- Ends meet
|
|
elseif checked[str].track ~= track and checked[str].track ~= 0 then
|
|
-- Backtrack from each end
|
|
local start_meet
|
|
local end_meet
|
|
if track == 1 then
|
|
start_meet = node
|
|
end_meet = checked[str].node
|
|
else
|
|
start_meet = checked[str].node
|
|
end_meet = node
|
|
end
|
|
local n = start_meet
|
|
while n ~= nil do
|
|
table.insert(path, 1, n.pos)
|
|
n = n.parent
|
|
end
|
|
n = end_meet
|
|
while n ~= nil do
|
|
path[#path + 1] = n.pos
|
|
n = n.parent
|
|
end
|
|
return false
|
|
end
|
|
end
|
|
return true
|
|
end
|
|
-- Check adjacents first, then diagonals
|
|
if not check(neighbors, 1, true) or not check(diagonals, 1.4142, false) then
|
|
return
|
|
end
|
|
checked[P(node.pos)] = {track = track, node = node}
|
|
end
|
|
return leaves
|
|
end
|
|
-- Loop until path found
|
|
local d = 0
|
|
depth = depth or 500
|
|
while true do
|
|
start_leaves = branch(start_nodes, 1)
|
|
if not start_leaves then
|
|
break
|
|
end
|
|
end_leaves = branch(end_nodes, -1)
|
|
if not end_leaves then
|
|
break
|
|
end
|
|
-- Incomplete path
|
|
if not next(start_leaves) then
|
|
return false, "NO_START"
|
|
elseif not next(end_leaves) then
|
|
return false, "NO_END"
|
|
else
|
|
start_nodes = start_leaves
|
|
start_leaves = {}
|
|
end_nodes = end_leaves
|
|
end_leaves = {}
|
|
end
|
|
d = d + 1
|
|
if d > depth then
|
|
return false, "DEPTH_REACHED"
|
|
end
|
|
end
|
|
if debug then
|
|
for step, pos in pairs(path) do
|
|
minetest.add_entity(pos, "staff:path", step)
|
|
end
|
|
end
|
|
return true, path
|
|
end
|
|
|
|
-- Only keep pivot points
|
|
function pathfind.trim(path)
|
|
if #path <= 2 then
|
|
return path
|
|
end
|
|
local trimmed = {path[1]}
|
|
local tail = 1
|
|
for i = 2, #path do
|
|
if path[i + 1] then
|
|
local ta = path[tail]
|
|
local th = path[i]
|
|
if not th then
|
|
return path
|
|
end
|
|
local ne = path[i + 1]
|
|
local this = math.atan2(th.z - ta.z, th.x - ta.x)
|
|
local next = math.atan2(ne.z - th.z, ne.x - th.x)
|
|
if this == next then
|
|
path[i] = nil
|
|
else
|
|
tail = i
|
|
trimmed[#trimmed + 1] = path[i]
|
|
end
|
|
end
|
|
end
|
|
return trimmed
|
|
end
|
|
|
|
-- Debug entities
|
|
if debug then
|
|
minetest.register_entity("ikea_staff:path", {
|
|
visual = "sprite",
|
|
textures = {"blank.png^[invert:a^[colorize:red"},
|
|
visual_size = {x = 0.3, y = 0.3},
|
|
on_activate = function(self, data)
|
|
self.object:set_nametag_attributes({text = data})
|
|
minetest.after(5, function()
|
|
if self.object then
|
|
self.object:remove()
|
|
end
|
|
end)
|
|
end,
|
|
on_punch = function(self)
|
|
self.object:remove()
|
|
end,
|
|
})
|
|
end
|
|
|
|
return pathfind
|