Dijkstra Pathfinding Algorithm

local Nodes = workspace:WaitForChild("Nodes")
local Table = Nodes:GetChildren()

local A = workspace:WaitForChild("A")
local B = workspace:WaitForChild("B")

function Connect()
    for _,A in ipairs(Table)do
        -- Don't overfill the nodes with neighbors.
        if #A:GetChildren() <= 0 then
            for _,B in ipairs(Table)do
                -- Make sure the path can be completed.
                local P = workspace:FindPartOnRay(Ray.new(A.Position, B.Position - A.Position))
                if P == B then
                    local Neighbor = Instance.new("ObjectValue")
                    Neighbor.Value = B
                    Neighbor.Name = "Neighbor"
                    Neighbor.Parent = A
                end
            end
        end
    end
end

function Length(A, B)
    if not A or not B then return math.huge; end
    return (A.Position - B.Position).Magnitude
end

function ClosestNode(Coord)
    local Output = nil;
    local Cap = math.huge;
    for _,v in ipairs(Table)do
        local Distance = (v.Position - Coord).Magnitude
        if Distance < Cap then
            Output = v
            Cap = Distance
        end
    end

    return Output
end

function Dijkstra(A, B)
    local A = ClosestNode(A);
    local B = ClosestNode(B);

    local Q = {}; -- EmptySet
    local D = {}; -- Distance
    local P = {}; -- Parent
    local u;

    -- Set all nodes except A to infinity.
    for _,v in ipairs(Table)do
        D[v] = math.huge;
        P[v] = nil;
        table.insert(Q, v)
    end
    D[A] = 0;

    repeat wait()
        u = nil;
        local l = -1;
        local H = math.huge;
        -- Find node with lowest distance value.
        for i,v in ipairs(Q)do
            if D[v] < H then
                H = D[v]
                u = v
                l = i
            end
        end

        if u then
            if l > 0 then
                -- Remove u from table Q so it won't be used again.
                table.remove(Q, l)
            end

            for _,v in ipairs(u:GetChildren())do
                if v:IsA("ObjectValue") then
                    local Alt = D[u] + Length(u, v.Value)
                    if Alt < D[v.Value] then
                        D[v.Value] = Alt
                        P[v.Value] = u
                    end
                end
            end
        end
        -- Terminate when path found, or no path found.
    until u == B or not u

    -- Retrace path.
    local Path = {u};
    local Current = u;
    repeat
        if Current then
            table.insert(Path, Current)
            Current = P[Current]
        end
    until not Current

    local Output = {};
    -- Reverse path.
    for i = #Path, 1, -1 do
        table.insert(Output, Path[i])
    end

    if u then
        print("Path finished")
        return Output
    else
        print("No path found")
    end
end

Connect()
Dijkstra(A.Position, B.Position)