function path = dijkstra(visibilityGraph, pointMap, S, G)
    numPoints = size(visibilityGraph, 1);
    
    % Find indices for Start and Goal in the point map
    startIndex = findPointIndex(pointMap, S);
    goalIndex = findPointIndex(pointMap, G);

    % Initialize distance and previous node arrays
    dist = inf(numPoints, 1);
    dist(startIndex) = 0;
    prev = -ones(numPoints, 1);

    % Create a priority queue and add the start index
    priorityQueue = startIndex;

    while ~isempty(priorityQueue)
        % Find the point with the smallest distance in the queue
        [~, uIdx] = min(dist(priorityQueue));
        u = priorityQueue(uIdx);
        priorityQueue(uIdx) = []; % Remove u from the queue

        % For each neighbor v of u
        for v = 1:numPoints
            if visibilityGraph(u, v) > 0
                alt = dist(u) + visibilityGraph(u, v);
                if alt < dist(v)
                    dist(v) = alt;
                    prev(v) = u;
                    if isempty(find(priorityQueue == v, 1))
                        priorityQueue(end + 1) = v; % Add v to the queue
                    end
                end
            end
        end
    end

    % Reconstruct the shortest path
    path = [];
    u = goalIndex;
    if prev(u) ~= -1 || u == startIndex
        while u ~= -1
            path = [pointMap(u) path];
            u = prev(u);
        end
    end
end