people/footpath.lua

local dbg
if moddebug then dbg=moddebug.dbg("people") else dbg={v1=function() end,v2=function() end,v3=function() end} end

local pathnodes = nil
local routecache = {}

--- Save the current footpath network status.
people.footpath_save = function()
    if not pathnodes then return end

    -- For the purposes of serialisation, we want a copy of the nodes with
    -- neighbours referenced by node name, not a reference to the actual
    -- node, otherwise it will serialise horribly!
    local lnodes = {}
    for n, nn in pairs(pathnodes) do
        local nei = {}
        for _, ne in pairs(nn.neighbours) do
            table.insert(nei, ne.name)
        end
        lnodes[n] = {name=n, pos=nn.pos, neighbours=nei}
    end

    local f = io.open(minetest.get_worldpath().."/people_footpaths.json", "w+")
    if f then
        f:write(minetest.write_json(lnodes))
        f:close()
    end
end

--- Load the footpath network status.
people.footpath_load = function()
    local f = io.open(minetest.get_worldpath().."/people_footpaths.json", "r")
    if f then
        pathnodes = minetest.parse_json(f:read("*all"))
        f:close()

        -- Now we need to undo the dereferencing we did when we saved it...
        for _, nn in pairs(pathnodes) do
            local nei = {}
            if nn.neighbours then
                for _, ne in pairs(nn.neighbours) do
                    table.insert(nei, pathnodes[ne])
                end
            end
            nn.neighbours = nei
        end

    end
end

--- Build the path nodes graph.
-- This is intended to be called via a chat command by a server administrator.
-- It builds a graph by interrogating the defined areas. This graph is then
-- used when finding paths.
-- @return A string describing an error in the setup (in which case no changes
--         are made), or nil on success (in which case, the new graph will
--         take effect).
people.footpath_make_pathnodes = function()

    newpathnodes = {}
    local allnodes = areas:getAreas({x=0,y=0,z=0}, "^path_.*", nil)
    for _, pn in pairs(allnodes) do
        if not vector.equals(pn.pos1, pn.pos2) then
            return "Path node "..pn.name.." is the wrong size"
        end
        newpathnodes[pn.name] = {name=pn.name, pos=pn.pos1, neighbours={}}
    end

    for _, pn in ipairs(allnodes) do
        local exits = areas:getAreas(pn.pos1, "^to_.*", 1.5)
        for _, tn in pairs(exits) do
            if tn.name ~= "to_*" then
                exname = tn.name:sub(4)
                if not newpathnodes["path_"..exname] then
                    return "Exit "..exname.." at "..minetest.pos_to_string(tn.pos1).." has no destination"
                end
                table.insert(newpathnodes[pn.name].neighbours, newpathnodes["path_"..exname])
            end
        end
    end

    for _, pn in pairs(newpathnodes) do
        for _, nn in pairs(pn.neighbours) do
            local ok = false
            for _, nnn in pairs(nn.neighbours) do
                if nnn == pn then
                    ok = true
                    break
                end
            end
            if not ok then
                return "Route from "..pn.name.." at "..minetest.pos_to_string(pn.pos)..
                        " to "..nn.name.." at "..minetest.pos_to_string(nn.pos)..
                        " does not return"
            end
        end
    end

    pathnodes = newpathnodes
    routecache = {}
    people.footpath_save()
    return nil

end

--- Fast removal of a value from a table
-- Avoids shuffling by swapping the last item with the one to be removed.
-- @param t The table
-- @param v The value to remove
local function table_fast_remove(t, v)
    for i, tv in ipairs(t) do
        if tv == v then
            t[i] = t[#t]
            t[#t] = nil
            return
        end
    end	
end

--- Check if a table contains the given value
-- @param t The table
-- @param v The value to check for
-- @return True if the table contains the value
local function table_contains(t, v)

    for _, tv in ipairs(t) do
        if tv == v then return true end
    end
    return false
end

--- Choose the best node from the list of open ones
-- @param open List of nodes
-- @param f Corresponding list of f scores for those node
-- @return The node with the lowest f score
local function choose_best(open, f)

    local lowest, best = 1/0, nil
    for _, node in ipairs(open) do
        local ts = f[node]
        if ts < lowest then
            lowest, best = ts, node
        end
    end
    return best
end

--- Reverse-iterate along the found route to construct the final path.
local function get_path(path, prev, current)

    if not prev[current] then return path end
    table.insert(path, 1, prev[current])
    return get_path(path, prev, prev[current])
end

--- Use A* to calculate the route from start to goal. Paths are cached.
local function astar(start, goal)

    if not routecache[start] then
        routecache[start] = {}
    elseif routecache[start][goal] then
        dbg.v3("astar used cached path")
        return routecache[start][goal]
    end

    local g = {[start]=0}
    local f = {[start]=vector.distance(start.pos, goal.pos)}
    local open = {start}
    local closed = {}
    local prev = {}

    while #open > 0 do

        local current = choose_best(open, f)
        if current == goal then
            local path = get_path({}, prev, goal)
            table.insert(path, goal)
            routecache[start][goal] = path
            return path
        end

        table_fast_remove(open, current)
        table.insert(closed, current)

        for _, neighbour in ipairs(current.neighbours) do
            if not table_contains(closed, neighbour) then

                local newg = g[current] + vector.distance(current.pos, neighbour.pos)

                if not table_contains(open, neighbour) or newg < g[neighbour] then
                    prev[neighbour] = current
                    g[neighbour] = newg
                    f[neighbour] = g[neighbour] + vector.distance(neighbour.pos, goal.pos)
                    if not table_contains(open, neighbour) then
                        table.insert(open, neighbour)
                    end
                end
            end
        end
    end
    return nil
end


--- Get a footpath route
-- @param start Starting node name, e.g. "path_My House"
-- @param goal Goal node name, e.g. "path_Your House"
-- @return A list of path node names defining the route, including the start
--         and the goal, or nil if no route was found.
people.get_footpath_route = function(start, goal)

    if not pathnodes then return nil end
    if not pathnodes[start] then
        dbg.v3("Route start "..start.." doesn't exist")
        return nil
    end
    if not pathnodes[goal] then
        dbg.v3("Route goal "..goal.." doesn't exist")
        return nil
    end

    path = astar(pathnodes[start], pathnodes[goal])
    if not path then
        dbg.v3("Route from "..start.." to "..goal.." not found")
        return nil
    end

    rpath = {}
    for _, n in ipairs(path) do
        table.insert(rpath, n.name)
    end
    dbg.v3("Route from "..start.." to "..goal.." is "..dump(rpath))
    return rpath

end

--- Find the next node along a footpath
-- All positions are exact (rounded) node positions.
-- @param curpos The position of a current footpath node
-- @param lastpos The position of the previous footpath node
-- @param samedironly True to only look in the current direction of travel
-- @return The position of the next footpath node, or nil if there isn't
--         one, or "unloaded" if there may or may not be one, but we can't
--         tell because of unloaded blocks.
people.footpath_findnext = function(curpos, lastpos, samedironly)
    --dbg.v3("footpath_findnext, cur="..minetest.pos_to_string(curpos)..", last="..minetest.pos_to_string(lastpos)..", same="..dump(samedironly))
    local xz
    if samedironly then
        xz = {}
    else
        xz = {{x=1,z=0}, {x=0,z=1}, {x=-1,z=0}, {x=0,z=-1}}
    end
    -- Favour the direction we're already going
    -- TODO - that means we could check it twice (but then again, only
    --        if we reach an invalid bit of footpath!
    table.insert(xz, 1, {x=curpos.x-lastpos.x, z=curpos.z-lastpos.z})
    for _, cxz in ipairs(xz) do
        for y = 1, -1, -1 do
            local x = cxz.x
            local z = cxz.z
            local npos = vector.add(curpos, vector.new(x, y, z))
            if not vector.equals(npos, lastpos) then
                local n = minetest.get_node(npos)
                if n.name == "ignore" then return "unloaded" end
                if n.name == 'default:cobble' or
                    n.name == 'default:mossycobble' or
                    n.name == 'stairs:stair_cobble' or
                    n.name == 'default:wood' or
                    n.name == 'stairs:stair_wood' then
                    local n = minetest.get_node({x=npos.x,y=npos.y+1,z=npos.z})
                    if n.name == "default:snow" then
                        -- Snow is walkable, but we will either walk over it
                        -- or dig it out of the way, so that's fine...
                        --dbg.v3("footpath_findnext found (snow-covered) "..minetest.pos_to_string(npos))
                        return npos
                    end
                    -- Otherwise, so long as there's nothing walkable above the
                    -- node it should be a valid footpath node...
                    if not minetest.registered_nodes[n.name].walkable then
                        --dbg.v3("footpath_findnext found "..minetest.pos_to_string(npos))
                        return npos
                    end
                end
            end
        end
    end
    return nil
end