46 lines
1.4 KiB
Matlab
46 lines
1.4 KiB
Matlab
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 |