添加Dijkstra算法

Dijkstra/Dijkstras.m

@@ -0,0 +1,78 @@
+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