新增RTT算法

homework_map.m

@@ -13,7 +13,7 @@ map = [0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0;   % 1
        1 0 0 0 0 0 0 0 0 0 0 1 1 0 0 0 0 0 0 0;   %10
        0 0 0 0 0 0 0 0 0 0 1 1 0 0 0 0 0 0 0 0;   %11
        0 0 0 0 0 0 0 0 0 0 0 0 0 1 1 1 1 1 0 0;   %12
-       0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0;   %13
+       0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 1 0;   %13
        0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0;   %14
        0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0;   %15
        0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0;   %16
@@ -30,10 +30,12 @@ goal = [19, 19];
 path_Astar = Astar(map, start, goal);
 % 调用Dijkstra算法
 path_Dijkstra = Dijkstras(map, start, goal);
+%调用RTT算法
+path_RTT = RTT(map, start, goal);
 
-visualize_path(map, start, goal, path_Astar, path_Dijkstra);
+visualize_path(map, start, goal, path_Astar, path_Dijkstra, path_RTT);
 
-function visualize_path(map, start, goal, path_Astar, path_Dijkstra)
+function visualize_path(map, start, goal, path_Astar, path_Dijkstra, path_RTT)
     % 可视化地图，0为空地，1为障碍
     imagesc(map);
     colormap(flipud(gray));          % 灰度图（0白 1黑）
@@ -42,25 +44,29 @@ function visualize_path(map, start, goal, path_Astar, path_Dijkstra)
 
     % 蓝色起点
     plot(start(2), start(1), 'bo', 'MarkerSize', 10, 'LineWidth', 2);
-
     % 红色终点
     plot(goal(2), goal(1), 'ro', 'MarkerSize', 10, 'LineWidth', 2);
 
+    % 绘制 A* 路径
     if ~isempty(path_Astar)
-    % 如果起点不等于路径首元素，插入
         if ~isequal(path_Astar(1,:), start)
             path_Astar = [start; path_Astar];
         end
-    % 如果终点不等于路径末元素，插入
         if ~isequal(path_Astar(end,:), goal)
             path_Astar = [path_Astar; goal];
         end
+        plot(path_Astar(:,2), path_Astar(:,1), 'r-', 'LineWidth', 2);
+        plot(path_Astar(:,2), path_Astar(:,1), 'ro', 'MarkerSize', 4, 'MarkerFaceColor', 'r');
+    end
 
-    % 显示路径线
+    % 绘制 Dijkstra 路径
-        plot(path_Astar(:,2), path_Astar(:,1), 'r-', 'LineWidth', 2); % 绿色路径线
+    if ~isempty(path_Dijkstra)
-        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), 'g-', 'LineWidth', 2); % 绿色路径线
+        plot(path_Dijkstra(:,2), path_Dijkstra(:,1), 'go', 'MarkerSize', 4, 'MarkerFaceColor', 'g');
-        plot(path_Dijkstra(:,2), path_Dijkstra(:,1), 'go', 'MarkerSize', 4, 'MarkerFaceColor', 'g'); % 每个点画圈
+    end
+    % 绘制 RTT 路径
+    if ~isempty(path_RTT)
+        plot(path_RTT(:,2), path_RTT(:,1), 'b-', 'LineWidth', 2);
     end
 
     % 网格线