aStar

if not pQueue then
    if not os.loadAPI("pQueue") then
        error("could not load pQueue API")
    end
end

-- a very basic map API used to store node information
local mapMethods = {
    get = function(self, tVector)
        if self.map[tVector.x] and self.map[tVector.x][tVector.y] then
            return self.map[tVector.x][tVector.y][tVector.z]
        end
    end,
    set = function(self, tVector, value)
        if not self.map[tVector.x] then
            self.map[tVector.x] = {}
        end
        if not self.map[tVector.x][tVector.y] then
            self.map[tVector.x][tVector.y] = {}
        end
        self.map[tVector.x][tVector.y][tVector.z] = value
        return self.map[tVector.x][tVector.y][tVector.z]
    end,
    getOrSet = function(self, tVector, value)
        if self.map[tVector.x] and self.map[tVector.x][tVector.y] and self.map[tVector.x][tVector.y][tVector.z] ~= nil then
            return self.map[tVector.x][tVector.y][tVector.z], false
        else
            return self:set(tVector, value), true
        end
    end,
}
local mapMetatable = {__index = mapMethods}

function newMap()
    return setmetatable({map = {}}, mapMetatable)
end

local function makePath(nodes, start, startEnd, goalStart, goal)
    local current, path = startEnd, {}
    while not vectorEquals(current, start) do
        table.insert(path, current)
        current = nodes:get(current)[1]
    end
    current = goalStart
    while not vectorEquals(current, goal) do
        table.insert(path, 1, current)
        current = nodes:get(current)[1]
    end
    table.insert(path, 1, goal)
    return path
end

function vectorEquals(a, b) -- the comparison function used in pQueue
    return a.x == b.x and a.y == b.y and a.z == b.z
end

local posZ = vector.new(0, 0, 1)
local negX = vector.new(-1, 0, 0)
local negZ = vector.new(0, 0, -1)
local posX = vector.new(1, 0, 0)
local posY = vector.new(0, 1, 0)
local negY = vector.new(0, -1, 0)
function adjacent(u)
    return {
        u + posZ,
        u + negX,
        u + negZ,
        u + posX,
        u + posY,
        u + negY,
    }
end

function distance(a, b) -- 1-norm/manhattan metric
    return math.abs(a.x - b.x) + math.abs(a.y - b.y) + math.abs(a.z - b.z)
end

function compute(distanceFunction, start, goal)

    if type(distanceFunction) ~= "function" then
        error("aStar new: distanceFunction must be of type function", 2)
    end

    local distanceFunc = distanceFunction -- is this necessary?

    -- node data structure is {parent node, true cost from startNode/goalNode, whether in closed list, search direction this node was found in, whether in open list}
    local nodes = newMap()
    nodes:set(start, {start + vector.new(0, 0, -1), 0, false, true, true})
    nodes:set(goal, {goal + vector.new(0, 0, -1), 0, false, false, true})

    local openStartSet = pQueue.new()
    openStartSet:insert(start, distance(start, goal))
    local openGoalSet = pQueue.new()
    openGoalSet:insert(goal, distance(start, goal))

    local yieldCount = 0
    local activeOpenSet, pendingOpenSet = openStartSet, openGoalSet
    local forwardSearch, lastNode, switch = true, false, false

    local current, currNode, parent
    local baseCost
    local newCost
    local nbrNode, newNode
    local preHeuristic

    while not openStartSet:isEmpty() and not openGoalSet:isEmpty() do

        --yield every so often to avoid getting timed out
        yieldCount = yieldCount + 1
        if yieldCount > 200 then
            os.pullEvent(os.queueEvent("yield"))
            yieldCount = 0
        end

        if switch then --switch the search direction
            activeOpenSet, pendingOpenSet = pendingOpenSet, activeOpenSet
            forwardSearch = not forwardSearch
            lastNode = false
        end

        current = activeOpenSet:pop()
        currNode = nodes:get(current)
        parent = current - currNode[1]

        currNode[3], currNode[5], switch = true, false, true

        for _, neighbour in ipairs(adjacent(current)) do

            baseCost = distanceFunc(current, neighbour)
            if baseCost < math.huge then -- if not graph:get(neighbour) then

                newCost = currNode[2] + baseCost

                nbrNode, newNode = nodes:getOrSet(neighbour, {current, newCost, false, forwardSearch, false})
                if switch and ((not lastNode) or vectorEquals(lastNode, neighbour)) then
                    switch = false
                end

                if not newNode then
                    if forwardSearch ~= nbrNode[4] then -- nbrNode has been discovered in the opposite search direction
                        if nbrNode[3] then -- and is in the closed list so has been expanded already
                            return makePath(nodes, start, (forwardSearch and current) or neighbour, (forwardSearch and neighbour) or current, goal)
                        end
                    elseif newCost < nbrNode[2] then
                        if nbrNode[5] then
                            activeOpenSet:remove(neighbour, vectorEquals)
                            nbrNode[5] = false
                        end
                        nbrNode[3] = false
                    end
                end

                if (newNode or (forwardSearch ~= nbrNode[4] and not nbrNode[5] and not nbrNode[3])) and newCost < math.huge then
                    nbrNode[1] = current
                    nbrNode[2] = newCost
                    nbrNode[4] = currNode[4]
                    nbrNode[5] = true
                    preHeuristic = distance(neighbour, (forwardSearch and goal) or start)
                    activeOpenSet:insert(neighbour, newCost + preHeuristic + 0.0001*(preHeuristic + parent.length(parent:cross(neighbour - current))))
                end
            end
        end
        lastNode = current
    end
    return false
end