79 lines
2.2 KiB
Mathematica
79 lines
2.2 KiB
Mathematica
|
function path = Dijkstras(map, start, goal)
|
|||
|
|
[rows, cols] = size(map);
|
|||
|
|
dist = inf(rows, cols); % 距离矩阵
|
|||
|
|
visited = false(rows, cols); % 访问标记
|
|||
|
|
prev = cell(rows, cols); % 前驱节点
|
|||
|
|
|
|||
|
|
dist(start(1), start(2)) = 0; % 起点距离为0
|
|||
|
|
openList = start; % 初始化开放列表
|
|||
|
|
|
|||
|
|
% 四方向移动：[上，下，左，右]
|
|||
|
|
directions = [0 1; 1 0; 0 -1; -1 0];
|
|||
|
|
|
|||
|
|
while ~isempty(openList)
|
|||
|
|
% 在 openList 中找到最小距离的点
|
|||
|
|
minDist = inf;
|
|||
|
|
minIdx = 1;
|
|||
|
|
for i = 1:size(openList, 1)
|
|||
|
|
r = openList(i,1); c = openList(i,2);
|
|||
|
|
if dist(r, c) < minDist
|
|||
|
|
minDist = dist(r, c);
|
|||
|
|
minIdx = i;
|
|||
|
|
end
|
|||
|
|
end
|
|||
|
|
|
|||
|
|
current = openList(minIdx, :);
|
|||
|
|
openList(minIdx, :) = []; % 从开放列表移除
|
|||
|
|
|
|||
|
|
r = current(1); c = current(2);
|
|||
|
|
if visited(r, c), continue; end
|
|||
|
|
visited(r, c) = true;
|
|||
|
|
|
|||
|
|
% 如果到达终点，停止
|
|||
|
|
if isequal(current, goal)
|
|||
|
|
break;
|
|||
|
|
end
|
|||
|
|
|
|||
|
|
% 遍历邻居
|
|||
|
|
for d = 1:size(directions, 1)
|
|||
|
|
nr = r + directions(d, 1);
|
|||
|
|
nc = c + directions(d, 2);
|
|||
|
|
|
|||
|
|
% 判断是否在地图内，是否为可通行格
|
|||
|
|
if nr >= 1 && nr <= rows && nc >= 1 && nc <= cols
|
|||
|
|
if map(nr, nc) == 0 && ~visited(nr, nc)
|
|||
|
|
alt = dist(r, c) + 1; % 假设代价为1
|
|||
|
|
if alt < dist(nr, nc)
|
|||
|
|
dist(nr, nc) = alt;
|
|||
|
|
prev{nr, nc} = [r, c];
|
|||
|
|
openList = [openList; nr, nc]; % 加入开放列表
|
|||
|
|
end
|
|||
|
|
end
|
|||
|
|
end
|
|||
|
|
end
|
|||
|
|
end
|
|||
|
|
|
|||
|
|
% 回溯路径
|
|||
|
|
path = [];
|
|||
|
|
cur = goal;
|
|||
|
|
while ~isempty(prev{cur(1), cur(2)})
|
|||
|
|
path = [cur; path];
|
|||
|
|
cur = prev{cur(1), cur(2)};
|
|||
|
|
end
|
|||
|
|
if isequal(cur, start)
|
|||
|
|
path = [start; path];
|
|||
|
|
else
|
|||
|
|
path = [];
|
|||
|
|
end
|
|||
|
|
|
|||
|
|
% 打印路径
|
|||
|
|
if ~isempty(path)
|
|||
|
|
disp('路径为：');
|
|||
|
|
for i = 1:size(path, 1)
|
|||
|
|
fprintf('(%d, %d)\n', path(i, 2), path(i, 1));
|
|||
|
|
end
|
|||
|
|
else
|
|||
|
|
disp('未找到路径。');
|
|||
|
|
end
|
|||
|
|
end
|