添加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

homework_map.m

@@ -1,3 +1,4 @@
+%  ！！！ 运行代码需将Astar和Dijkstra文件夹添加到设置路径中 ！！！
 % 创建地图
 %      1 2 3 4 5 6 7 8 9 0 1 2 3 4 5 6 7 8 9 0
 map = [0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0;   % 1
@@ -26,11 +27,13 @@ start = [2, 2];
 goal = [19, 19];
 
 % 调用A*算法
-path = Astar(map, start, goal);
+path_Astar = Astar(map, start, goal);
+% 调用Dijkstra算法
+path_Dijkstra = Dijkstras(map, start, goal);
 
-visualize_path(map, start, goal, path);
+visualize_path(map, start, goal, path_Astar, path_Dijkstra);
 
-function visualize_path(map, start, goal, path)
+function visualize_path(map, start, goal, path_Astar, path_Dijkstra)
     % 可视化地图，0为空地，1为障碍
     imagesc(map);
     colormap(flipud(gray));          % 灰度图（0白 1黑）
@@ -43,19 +46,21 @@ function visualize_path(map, start, goal, path)
     % 红色终点
     plot(goal(2), goal(1), 'ro', 'MarkerSize', 10, 'LineWidth', 2);
 
-    if ~isempty(path)
+    if ~isempty(path_Astar)
     % 如果起点不等于路径首元素，插入
-        if ~isequal(path(1,:), start)
-            path = [start; path];
+        if ~isequal(path_Astar(1,:), start)
+            path_Astar = [start; path_Astar];
         end
     % 如果终点不等于路径末元素，插入
-        if ~isequal(path(end,:), goal)
-            path = [path; goal];
+        if ~isequal(path_Astar(end,:), goal)
+            path_Astar = [path_Astar; goal];
         end
 
     % 显示路径线
-        plot(path(:,2), path(:,1), 'g-', 'LineWidth', 2); % 绿色路径线
-        plot(path(:,2), path(:,1), 'go', 'MarkerSize', 4, 'MarkerFaceColor', 'g'); % 每个点画圈
+        plot(path_Astar(:,2), path_Astar(:,1), 'r-', 'LineWidth', 2); % 绿色路径线
+        plot(path_Astar(:,2), path_Astar(:,1), 'ro', 'MarkerSize', 4, 'MarkerFaceColor', 'r'); % 每个点画圈
+        plot(path_Dijkstra(:,2), path_Dijkstra(:,1), 'g-', 'LineWidth', 2); % 绿色路径线
+        plot(path_Dijkstra(:,2), path_Dijkstra(:,1), 'go', 'MarkerSize', 4, 'MarkerFaceColor', 'g'); % 每个点画圈
     end
 
     % 网格线