Maze pathfinding

local start = Vector3.new(342, 72, 342)
local goal  = Vector3.new(309, 72, 372)

local step = 3
local size = Vector3.new(3, 3, 3)

local function free(p)
    return BlockService.getBlockAt(p) == nil
end

local function place(p)
    local part = PartService.createPart(ItemType.CLAY_WHITE, p)
    part:setSize(size)
    part:setAnchored(true)
    part:setCollidable(false)
end

local function equal(a, b)
    return a.X == b.X and a.Y == b.Y and a.Z == b.Z
end

local function getNeighbors(p)
    local n = {}

    local p1 = Vector3.new(p.X + step, p.Y, p.Z)
    local p2 = Vector3.new(p.X - step, p.Y, p.Z)
    local p3 = Vector3.new(p.X, p.Y, p.Z + step)
    local p4 = Vector3.new(p.X, p.Y, p.Z - step)

    if free(p1) then table.insert(n, p1) end
    if free(p2) then table.insert(n, p2) end
    if free(p3) then table.insert(n, p3) end
    if free(p4) then table.insert(n, p4) end

    return n
end

local function buildPath(s, g)
    if free(g) == false then
        error("unable reach goal")
        return
    end

    local queue = {s}
    local came = {}
    came[s.X .. "," .. s.Z] = s

    local found = false

    while #queue > 0 do
        local p = table.remove(queue, 1)

        if equal(p, g) then
            found = true
            break
        end

        local neighbors = getNeighbors(p)
        for _, np in ipairs(neighbors) do
            local key = np.X .. "," .. np.Z
            if came[key] == nil then
                came[key] = p
                table.insert(queue, np)
            end
        end
    end

    if found == false then
        error("unable reach goal")
        return
    end

    local path = {}
    local cur = g

    while true do
        table.insert(path, cur)
        if equal(cur, s) then
            break
        end
        local key = cur.X .. "," .. cur.Z
        cur = came[key]
    end

    for i = #path, 1, -1 do
        place(path[i])
    end
end

buildPath(start, goal)