clc
clear
%%===========================================================================
pic_num = 1;% 输出为gif文件用到的参数,与本文涉及的算法无关
%% =========================================================================
%创建具有障碍物的栅格地图
%矩阵中0代表黑色栅格
map = ones(20);
map(3,3:7)=0;
map(3:10,7)=0;
map(10,3:7)=0;
map(17,13:17)=0;
map(10:17,13)=0;
map(10,13:17)=0;
map(14,15)=0;
map1 = map;
map1(end + 1,end + 1) = 0;
colormap([0,0,0;1,1,1]); % 创建颜色
%pcolor(map1');
pcolor(0.5:size(map,1) + 0.5, 0.5:size(map,2) + 0.5, map1'); % 赋予栅格颜色
set(gca,'XTick',1:size(map,1),'YTick',1:size(map,2)); % 设置坐标
axis image xy; % 沿每个坐标轴使用相同的数据单位，保持一致
hold on
grid
axis([0,20,0,20]);
%%
%================================================================
%A star 算法
start_point = [2,2];%起始点坐标
end_point = [15,15];%目标点坐标
openlen = 0;%open列表长度
closelen = 0;%close列表长度
open = struct([]);%虽然算法中称为列表，但实际用的是结构体，因为要包含每个点的坐标和函数值
close = struct([]);%道理同上
move = [-1,1;0,1;1,1;1,0;1,-1;0,-1;-1,-1;-1,0];%因为涉及到可以对角移动，用一个列表的形式是一种很巧妙的方法
isGoal = 0;%是否到达目标点的标志位
%起点初始化
open(1).axis = start_point;%open集的第一个元素是起始点
open(1).g = 0;
open(1).h = abs(end_point(1)-start_point(1))^2 + abs(end_point(2)-start_point(1))^2;%用到的是欧几里得距离
open(1).f = open(1).g + open(1).h;
openlen = openlen + 1;
father(1,:) = start_point;%父节点的轨迹，这是为了保存算法走过的每个点的坐标
get_son = 0;%得到一个新的子节点
current = open(1);%当前所在节点的初始化
%%
%%=========================================================================
%在地图上标出起点（绿*）和终点（红*）
plot(end_point(1),end_point(2),'r*'); hold on;
plot(start_point(1),start_point(2),'g*'); hold on;
text(start_point(1),start_point(2),'起点');
text(end_point(1),end_point(2),'终点');
%%
%%=========================================================================
%起点初始化结束，进入循环部分
for i = 1:25 %本例用25步循环足以，如果地图范围更大，可以用while循环
if isequal(,end_point) %判断是否到达终点
isGoal = 1; %如是，标志位置1；
disp('Find the Goal!');
break; %跳出循环
else
if openlen ~= 0 %openlen ~= 0表示还有可以继续走下去的机会，openlen == 0就见else部分
plot((1),(2),'bo'); hold on; %画出当前所在的节点
x = (1); %当前节点的x坐标
y = (2); %当前节点的y坐标
temp = struct([]); %temp是当前节点所有可通行的区域,临时的结构体变量
f1 = [];f2 = []; %定义f1和f2的目的是为了存储close集和open集所包含的节点的坐标，用结构体的形式也可以，但是不便于后续的比较和查找
if closelen~=0
for j=1:length(close)
tpp = [close(j).axis(1),close(j).axis(2)];
f1 = [f1;tpp]; %将close集中的每个节点的坐标以一个n*2的矩阵的形式保存下来，以f1命名
end
end
k1 = 0; %每个节点8个方向的运动可能不会都有效，这是区别于下文的j变量的临时变量
for j = 1:8
x1 = x + move(j,1);
y1 = y + move(j,2); %以查询列表的形式做8个方向的运动
if closelen~=0
[tmp,ind] = ismember([x1,y1],f1,'rows'); %看当前的[x1,y1]是否在close集内，'rows'表示按行比较
if tmp == 1 %如果新的点在close列表内，tmp == 1，则后续不执行，跳过该节点
continue;
end
end
if x1>0 && x1<=20 && y1>0 && y1<=20 && map(x1,y1)~=0 %防止周围的八个点越界和保证障碍点不能作为可行点
k1 = k1 + 1; %有一个可行点，则 k1增加1
temp(k1).axis = [x1,y1]; %对于当前的节点而言，其temp包含了其所有的下一步可行点
temp(k1).g = get_son + 1;
temp(k1).h = abs(end_point(1) - x1)^2 + abs(end_point(2) - y1)^2;
temp(k1).f = temp(k1).g + temp(k1).h;
else
disp('Cross the border or Meet Obstacle!');
end
end
tpp1 = []; %tpp的作用与f1和f2的作用类似，保存open集中的所有节点的坐标，n*2的矩阵的形式
for k = 1:length(open)
tpp1(end+1,:) = [open(k).axis(1),open(k).axis(2)];
end
for j = 1:length(temp)%将当前节点所有可通行的下一步节点，如果不再open集内，就添加到open内
tpp2 = [temp(j).axis(1),temp(j).axis(2)];
if ismember(tpp2,tpp1,'rows')
continue;
else
open(end+1).axis = temp(j).axis;
open(end).g = temp(j).g;
open(end).h = temp(j).h;
open(end).f = temp(j).f;
openlen = openlen + 1; % 每向open集内增加一个，openlen加1
end
end
for j = 1:length(temp)
f2(j) = temp(j).f; %f2保存当前节点的temp中所有节点的f函数值，以具最小f值的节点作为下一步将要走的节点
end
[f3,index] = sort(f2); %找到f值最小的那个节点，sort函数默认升序排列
index = index(1); %sort函数得到的index是一组索引，取第一个即可
get_son = get_son + 1; %得到一个新的节点，子节点数加1
father(get_son,:) = ; %保存当前的父节点
f4 = [];
for j = 1:length(open)
f4 = [f4;open(j).axis];
end
[~,current_index] = ismember(,f4,'rows') ;
close(end+1).axis = open(current_index).axis; %close集中添加当前节点
close(end).h = open(current_index).h;
close(end).g = open(current_index).g;
close(end).f = open(current_index).f;
closelen = closelen + 1; %closelen加1
open(current_index) = []; %将当前节点从open集中删掉
openlen = openlen - 1; %openlen减1
current = temp(index); %找到新的当前节点(子节点)
plot((1),(2),'bo'); hold on;
temp4 = ['Now:(',num2str((1)),',',num2str((2)),')'];
disp(temp4); %显示部分
else
disp('No way to the goal!'); %如果没有可以走的下一步，就显示没有通向终点的路
break;
end
end
pause(0.5); %用于控制GIF的变化速度
%======================================================================
%将结果显示为GIF的部分
F = getframe(gcf);
I = frame2im(F);
[I,map2] = rgb2ind(I,256);
if pic_num == 1
imwrite(I,map2,'','gif', 'Loopcount',inf,'DelayTime',0.2);
else
imwrite(I,map2,'','gif','WriteMode','append','DelayTime',0.2);
end
pic_num = pic_num + 1;
end
hold off;