[CC] 3D A* Pathfinding API

--CC Pathfinding API by Oli414 - V0.9

local generatedPath = {}
local checkPath = {}
local map = {}

local function _getDistance(vec1, vec2)
    return math.sqrt(math.pow(vec1.x - vec2.x, 2) + math.pow(vec1.y - vec2.y, 2) + math.pow(vec1.z - vec2.z, 2))
end

local function _setDynamicTable(table, pos, value)
    if table == nil then
        table = {}
    end
    if table[pos.x] == nil then
        table[pos.x] = {}
    end
    if table[pos.x][pos.y] == nil then
        table[pos.x][pos.y] = {}
    end
    table[pos.x][pos.y][pos.z] = value
end

local function _getDynamicTable(table, pos, value)
    local returnValue = false
    if value == nil then
        returnValue = true
    end
    if table == nil then
        return returnValue
    end
    if table[pos.x] == nil then
        return returnValue
    end
    if table[pos.x][pos.y] == nil then
        return returnValue
    end
    if table[pos.x][pos.y][pos.z] == nil then
        return returnValue
    end
    if table[pos.x][pos.y][pos.z] == value then
        return true
    end
    return false
end

local function _getDynamicTableValue(table, pos)
    if table == nil then
        return nil
    end
    if table[pos.x] == nil then
        return nil
    end
    if table[pos.x][pos.y] == nil then
        return nil
    end
    if table[pos.x][pos.y][pos.z] == nil then
        return nil
    else
        return table[pos.x][pos.y][pos.z]
    end
end

function setMapValue(position, value)
    _setDynamicTable(map, position, value)
end

function getMapValue(position)
    return _getDynamicTableValue(map, position)
end

function getMap()
    return map
end

function generatePath(from, to)
    local checked = 1
    local setCheck = 1
    local distance = 0
    local distances = {}
    local distanceIndex = {}
    generatedPath = {}
    checkPath = {}

    checkPath[setCheck] = to
    distances[setCheck] = _getDistance(to, from)
    distanceIndex[setCheck] = 0

    setCheck = setCheck + 1

    while checked < setCheck do
        os.queueEvent("randomEvent")
        os.pullEvent()
        local previousSetCheck = setCheck
        local closest = nil
        local minDistance = -1
        for i = checked, previousSetCheck do
            if distances[i] ~= nil then
                if distances[i] < minDistance or minDistance == -1 then
                    closest = i
                    minDistance = distances[i]
                end
            end
        end
        if closest == nil then
            return false
        end
        distance = distance + 1
        if _getDynamicTable(map, checkPath[closest], nil) or _getDynamicTable(map, checkPath[closest], false) then
            print(setCheck)
            _setDynamicTable(generatedPath, checkPath[closest], distanceIndex[closest])
            distances[closest] = nil
            if checkPath[closest].x == from.x and checkPath[closest].y == from.y and checkPath[closest].z == from.z then
                return true
            end
            if _getDynamicTable(generatedPath, {x = checkPath[closest].x - 1, y = checkPath[closest].y, z = checkPath[closest].z}, nil) and (_getDynamicTable(map, {x = checkPath[closest].x - 1, y = checkPath[closest].y, z = checkPath[closest].z}, false) or _getDynamicTable(map, {x = checkPath[closest].x - 1, y = checkPath[closest].y, z = checkPath[closest].z}, nil)) then
                checkPath[setCheck] = {x = checkPath[closest].x - 1, y = checkPath[closest].y, z = checkPath[closest].z}
                distances[setCheck] = _getDistance(checkPath[setCheck], from)
                distanceIndex[setCheck] = distanceIndex[closest] + 1
                setCheck = setCheck + 1
            end
            if _getDynamicTable(generatedPath, {x = checkPath[closest].x + 1, y = checkPath[closest].y, z = checkPath[closest].z}, nil) and (_getDynamicTable(map, {x = checkPath[closest].x + 1, y = checkPath[closest].y, z = checkPath[closest].z}, false) or _getDynamicTable(map, {x = checkPath[closest].x + 1, y = checkPath[closest].y, z = checkPath[closest].z}, nil)) then
                checkPath[setCheck] = {x = checkPath[closest].x + 1, y = checkPath[closest].y, z = checkPath[closest].z}
                distances[setCheck] = _getDistance(checkPath[setCheck], from)
                distanceIndex[setCheck] = distanceIndex[closest] + 1
                setCheck = setCheck + 1
            end
            if _getDynamicTable(generatedPath, {x = checkPath[closest].x, y = checkPath[closest].y - 1, z = checkPath[closest].z}, nil) and (_getDynamicTable(map, {x = checkPath[closest].x, y = checkPath[closest].y - 1, z = checkPath[closest].z}, false) or _getDynamicTable(map, {x = checkPath[closest].x, y = checkPath[closest].y - 1, z = checkPath[closest].z}, nil)) then
                checkPath[setCheck] = {x = checkPath[closest].x, y = checkPath[closest].y - 1, z = checkPath[closest].z}
                distances[setCheck] = _getDistance(checkPath[setCheck], from)
                distanceIndex[setCheck] = distanceIndex[closest] + 1
                setCheck = setCheck + 1
            end
            if _getDynamicTable(generatedPath, {x = checkPath[closest].x, y = checkPath[closest].y + 1, z = checkPath[closest].z}, nil) and (_getDynamicTable(map, {x = checkPath[closest].x, y = checkPath[closest].y + 1, z = checkPath[closest].z}, false) or _getDynamicTable(map, {x = checkPath[closest].x, y = checkPath[closest].y + 1, z = checkPath[closest].z}, nil)) then
                checkPath[setCheck] = {x = checkPath[closest].x, y = checkPath[closest].y + 1, z = checkPath[closest].z}
                distances[setCheck] = _getDistance(checkPath[setCheck], from)
                distanceIndex[setCheck] = distanceIndex[closest] + 1
                setCheck = setCheck + 1
            end
            if _getDynamicTable(generatedPath, {x = checkPath[closest].x, y = checkPath[closest].y, z = checkPath[closest].z - 1}, nil) and (_getDynamicTable(map, {x = checkPath[closest].x, y = checkPath[closest].y, z = checkPath[closest].z - 1}, false) or _getDynamicTable(map, {x = checkPath[closest].x, y = checkPath[closest].y, z = checkPath[closest].z - 1}, nil)) then
                checkPath[setCheck] = {x = checkPath[closest].x, y = checkPath[closest].y, z = checkPath[closest].z - 1}
                distances[setCheck] = _getDistance(checkPath[setCheck], from)
                distanceIndex[setCheck] = distanceIndex[closest] + 1
                setCheck = setCheck + 1
            end
            if _getDynamicTable(generatedPath, {x = checkPath[closest].x, y = checkPath[closest].y, z = checkPath[closest].z + 1}, nil) and (_getDynamicTable(map, {x = checkPath[closest].x, y = checkPath[closest].y, z = checkPath[closest].z + 1}, false) or _getDynamicTable(map, {x = checkPath[closest].x, y = checkPath[closest].y, z = checkPath[closest].z + 1}, nil)) then
                checkPath[setCheck] = {x = checkPath[closest].x, y = checkPath[closest].y, z = checkPath[closest].z + 1}
                distances[setCheck] = _getDistance(checkPath[setCheck], from)
                distanceIndex[setCheck] = distanceIndex[closest] + 1
                setCheck = setCheck + 1
            end
            checkPath[closest] = nil
        end
    end
end

function getPath(from)
    if _getDynamicTableValue(generatedPath, from) ~= nil then
        if _getDynamicTableValue(generatedPath, {x = from.x - 1, y = from.y, z = from.z}) ~= nil and _getDynamicTableValue(generatedPath, {x = from.x - 1, y = from.y, z = from.z}) < _getDynamicTableValue(generatedPath, from) then
            return {x = from.x - 1, y = from.y, z = from.z}
        elseif _getDynamicTableValue(generatedPath, {x = from.x + 1, y = from.y, z = from.z}) ~= nil and _getDynamicTableValue(generatedPath, {x = from.x + 1, y = from.y, z = from.z}) < _getDynamicTableValue(generatedPath, from) then
            return {x = from.x + 1, y = from.y, z = from.z}
        elseif _getDynamicTableValue(generatedPath, {x = from.x, y = from.y - 1, z = from.z}) ~= nil and _getDynamicTableValue(generatedPath, {x = from.x, y = from.y - 1, z = from.z}) < _getDynamicTableValue(generatedPath, from) then
            return {x = from.x, y = from.y - 1, z = from.z}
        elseif _getDynamicTableValue(generatedPath, {x = from.x, y = from.y + 1, z = from.z}) ~= nil and _getDynamicTableValue(generatedPath, {x = from.x, y = from.y + 1, z = from.z}) < _getDynamicTableValue(generatedPath, from) then
            return {x = from.x, y = from.y + 1, z = from.z}
        elseif _getDynamicTableValue(generatedPath, {x = from.x, y = from.y, z = from.z - 1}) ~= nil and _getDynamicTableValue(generatedPath, {x = from.x, y = from.y, z = from.z - 1}) < _getDynamicTableValue(generatedPath, from) then
            return {x = from.x, y = from.y, z = from.z - 1}
        elseif _getDynamicTableValue(generatedPath, {x = from.x, y = from.y, z = from.z + 1}) ~= nil and _getDynamicTableValue(generatedPath, {x = from.x, y = from.y, z = from.z + 1}) < _getDynamicTableValue(generatedPath, from) then
            return {x = from.x, y = from.y, z = from.z + 1}
        else
            return nil
        end
    end
    return nil
end