function [visibilityGraph, pointMap] = createVisibilityGraph(q1q2_valid, S, G, L1, L2)
    % Combine the PRM points with the Start and Goal points
    allPoints = [S, G, q1q2_valid];
    numPoints = size(allPoints, 2);

    % Create a map for point indexing
    pointMap = containers.Map('KeyType', 'double', 'ValueType', 'any');
    for i = 1:numPoints
        pointMap(i) = allPoints(:, i);
    end

    % Initialize the visibility graph
    visibilityGraph = zeros(numPoints, numPoints);

    % Check visibility between each pair of points
    for i = 1:numPoints
        for j = i+1:numPoints
            if i ~= j
                A = allPoints(:, i);
                B = allPoints(:, j);
                if ~isLineIntersectingObstacle(A, B, L1, L2)
                    visibilityGraph(i, j) = norm(A - B); % Distance between A and B
                    visibilityGraph(j, i) = visibilityGraph(i, j); % The graph is undirected
                end
            end
        end
    end
end