29 lines
991 B
Matlab
29 lines
991 B
Matlab
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
|