79 lines
2.6 KiB
Matlab
79 lines
2.6 KiB
Matlab
% 创建地图
|
||
% 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
|
||
0 0 0 0 0 0 0 0 0 1 0 0 0 0 1 1 0 0 0 0; % 2
|
||
0 0 0 1 1 1 1 0 0 1 0 0 0 0 0 1 1 0 0 0; % 3
|
||
0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 1 1 0 0; % 4
|
||
0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 1 1 0; % 5
|
||
1 1 1 1 1 1 1 0 0 0 0 0 0 0 0 1 0 0 1 1; % 6
|
||
1 0 0 0 0 0 0 0 0 0 0 0 0 0 1 1 0 0 0 1; % 7
|
||
1 0 0 0 0 0 0 0 0 0 0 0 0 1 1 0 0 0 0 0; % 8
|
||
1 0 0 0 1 1 1 1 1 1 0 0 1 1 0 0 0 0 0 0; % 9
|
||
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 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
|
||
0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 1; %17
|
||
0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0; %18
|
||
0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0; %19
|
||
0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0;]; %20
|
||
|
||
% 设置起点和终点
|
||
start = [2, 2];
|
||
goal = [19, 19];
|
||
|
||
% 调用A*算法
|
||
path = Astar(map, start, goal);
|
||
|
||
visualize_path(map, start, goal, path);
|
||
|
||
function visualize_path(map, start, goal, path)
|
||
% 可视化地图，0为空地，1为障碍
|
||
imagesc(map);
|
||
colormap(flipud(gray)); % 灰度图（0白 1黑）
|
||
axis equal tight;
|
||
hold on;
|
||
|
||
% 蓝色起点
|
||
plot(start(2), start(1), 'bo', 'MarkerSize', 10, 'LineWidth', 2);
|
||
|
||
% 红色终点
|
||
plot(goal(2), goal(1), 'ro', 'MarkerSize', 10, 'LineWidth', 2);
|
||
|
||
if ~isempty(path)
|
||
% 如果起点不等于路径首元素，插入
|
||
if ~isequal(path(1,:), start)
|
||
path = [start; path];
|
||
end
|
||
% 如果终点不等于路径末元素，插入
|
||
if ~isequal(path(end,:), goal)
|
||
path = [path; goal];
|
||
end
|
||
|
||
% 显示路径线
|
||
plot(path(:,2), path(:,1), 'g-', 'LineWidth', 2); % 绿色路径线
|
||
plot(path(:,2), path(:,1), 'go', 'MarkerSize', 4, 'MarkerFaceColor', 'g'); % 每个点画圈
|
||
end
|
||
|
||
% 网格线
|
||
[rows, cols] = size(map);
|
||
for i = 1:rows+1
|
||
line([0.5, cols+0.5], [i-0.5, i-0.5], 'Color', 'k'); % 横线
|
||
end
|
||
for j = 1:cols+1
|
||
line([j-0.5, j-0.5], [0.5, rows+0.5], 'Color', 'k'); % 竖线
|
||
end
|
||
|
||
set(gca, ...
|
||
'XTick', 1:cols, ...
|
||
'YTick', 1:rows, ...
|
||
'XTickLabel', 1:cols, ...
|
||
'YTickLabel', 1:rows, ...
|
||
'XAxisLocation', 'top', ... % x轴
|
||
'YDir', 'reverse'); % y轴
|
||
title('地图');
|
||
hold off;
|
||
end |