function Lazy_Theta_star
clc;
clear;
%% 初始化界面
%load maze.mat map
n = 20; % field size n x n tiles 20*20的界面
%wallpercent = 0.3; % this percent of field is walls 15%的界面作为阻碍物（墙）
cmap = [1 1 1; ...% 1 - white - 空地
0 0 0; ...% 2 - black - 障碍
1 0 0; ...% 3 - red - 已搜索过的地方
0 0 1; ...% 4 - blue - 下次搜索备选中心
0 1 0; ...% 5 - green - 起始点
1 1 0;...% 6 - yellow - 到目 标点的路径
1 0 1];% 7 - - 目标点
colormap(cmap);
global field
field= ones(n);
startposind =22; %sub2ind用来将行列坐标转换为线性坐标，这里是必要的，因为如果把startposind设置成[x,y]的形式，访问field([x,y])的时候
goalposind =86; %它并不是访问x行y列元素，而是访问线性坐标为x和y的两个元素
%field(ceil(n^2.*rand(floor(n*n*wallpercent),1) )) =2;
field(1:5, 7) = 2;
field(8,1:3) = 2;
field(3:5,1:5)=2;
field(5,4)=2;
% startposind = sub2ind([n,n],ceil(n.*rand),ceil(n.*rand)); %sub2ind用来将行列坐标转换为线性坐标，这里是必要的，因为如果把startposind设置成[x,y]的形式，访问field([x,y])的时候
%goalposind = sub2ind([n,n],ceil(n.*rand),ceil(n.*rand)); %它并不是访问x行y列元素，而是访问线性坐标为x和y的两个元素
field(startposind )=5;
field(goalposind )=7;
costchart = NaN*ones(n); %costchart用来存储各个点的实际代价，NaN代表不是数据（不明确的操作）
costchart(startposind) = 0; %起点的实际代价
fieldpointers = zeros(n); %fieldpointers用来存储节点的父节点
global setOpenCosts;
setOpen = (startposind); setOpenCosts = (0); setOpenHeuristics = (Inf);
setClosed = []; setClosedCosts = [];%初始化起点的open表和close表
[goalpos(1) ,goalpos(2)] = ind2sub([n n],goalposind);
% uicontrol('Style','pushbutton','String','RE-DO', 'FontSize',12, ...
% 'Position', [10 10 60 40], 'Callback','ASTAR');
tic
while true %ismember(A,B)返回与A同大小的矩阵，其中元素1表示A中相应位置的元素在B中也出现，0则是没有出现
field(startposind )=5;
field(goalposind )=7;
image(1.5,1.5,field);
set(gca,'gridline','-','gridcolor','y','linewidth',2,'GridAlpha',0.5);
set(gca,'xtick',1:1:21,'ytick',1:1:21);
grid on;
axis image;
drawnow;
if(max(ismember(setClosed,goalposind)))
break;
end;
[~, ii] = min(setOpenCosts + setOpenHeuristics); %从OPEN表中选择花费最低的点temp,ii是其下标(也就是标号索引)
field(setOpen(ii))=3;
[currentpos(1), currentpos(2)] = ind2sub([n n],setOpen(ii));
if fieldpointers(setOpen(ii))~=0
if fieldpointers(fieldpointers(setOpen(ii)))~=0
trued=lineofsight(n,fieldpointers(fieldpointers(setOpen(ii))),setOpen(ii));
if(trued)
id=find(setClosed==fieldpointers(fieldpointers(setOpen(ii))));
[parent_postion_x,parent_position_y]=ind2sub([n,n],setClosed(id));
if setOpenCosts(ii)>=setClosedCosts(id)+sqrt(power((parent_postion_x-currentpos(1)),2)+power((parent_position_y-currentpos(2)),2))
fieldpointers(setOpen(ii)) = fieldpointers(fieldpointers(setOpen(ii))) ; %将此点的方向存在对应的fieldpointers中
setOpenCosts(ii)=setClosedCosts(id)+sqrt(power((parent_postion_x-currentpos(1)),2)+power((parent_position_y-currentpos(2)),2));
end
end
end
end
%% 获得当前点的八邻域
[ posinds,cost,heuristic]=get_neighbors(n,ii,currentpos(1),currentpos(2),goalpos(1) ,goalpos(2));
setClosed = [setClosed; setOpen(ii)]; %将temp插入CLOSE表中
setClosedCosts = [setClosedCosts; setOpenCosts(ii)]; %将temp的花费计入ClosedCosts
%% 更新OPEN表 分为三种情况
if (ii > 1 && ii < length(setOpen)) %t在OPEN表的中间，删除temp
setOpen = [setOpen(1:ii-1); setOpen(ii+1:end)];
setOpenCosts = [setOpenCosts(1:ii-1); setOpenCosts(ii+1:end)];
setOpenHeuristics = [setOpenHeuristics(1:ii-1); setOpenHeuristics(ii+1:end)];
elseif (ii == 1)
setOpen = setOpen(2:end); %是OPEN表的第一个元素，删除temp
setOpenCosts = setOpenCosts(2:end);
setOpenHeuristics = setOpenHeuristics(2:end);
else %是OPEN表的最后一个元素，删除temp
setOpen = setOpen(1:end-1);
setOpenCosts = setOpenCosts(1:end-1);
setOpenHeuristics = setOpenHeuristics(1:end-1);
end
for jj=1:length(posinds) %对于扩展的四个方向的坐标
if(field(posinds(jj))~=3 && field(posinds(jj))~=2 && field(posinds(jj))~=5 && posinds(jj)~=1)
parent=sub2ind([n,n],currentpos(1),currentpos(2));
if ~max([setClosed; setOpen] == posinds(jj)) %如果此点不在OPEN表和CLOSE表中
trued=lineofsight(n,parent,posinds(jj));
if(trued)
field(posinds(jj))=4;
fieldpointers(posinds(jj)) = sub2ind([n,n],currentpos(1), currentpos(2)); %将此点的方向存在对应的fieldpointers中
costchart(posinds(jj)) = cost(jj); %将实际代价值存入对应的costchart中
setOpen = [setOpen; posinds(jj)]; %将此点加入OPEN表中
setOpenCosts = [setOpenCosts; cost(jj)]; %更新OPEN表实际代价
setOpenHeuristics = [setOpenHeuristics; heuristic(jj)]; %更新OPEN表启发代价
end
elseif max(setOpen == posinds(jj)) %如果此点在OPEN表中
I = find(setOpen == posinds(jj)); %找到此点在OPEN表中的位置
if setOpenCosts(I) >=cost(jj) %如果在OPEN表中的此点实际代价比现在所得的大
costchart(setOpen(I)) = cost(jj); %将当前的代价存入costchart中，注意此点在costchart中的坐标与其自身坐标是一致的（setOpen(I)其实就是posinds(jj)），下同fieldpointers
setOpenCosts(I) = cost(jj); %更新OPEN表中的此点代价，注意此点在setOpenCosts中的坐标与在setOpen中是一致的，下同setOpenHeuristics
setOpenHeuristics(I) = heuristic(jj); %更新OPEN表中的此点启发代价(窃以为这个是没有变的)
fieldpointers(posinds(jj)) = setOpen(ii); %更新此点的方向
end
end
end
end
if isempty(setOpen)%当OPEN表为空，代表可以经过的所有点已经查询完毕
break;
end
end
toc
if max(ismember(setClosed,goalposind)) %当找到目标点时
disp('已找到路径!'); %disp： Display array， disp(X)直接将矩阵显示出来，不显示其名字，如果X为string，就直接输出文字X
posind1 = goalposind;
%% 获得路径
while (fieldpointers(posind1(1)) ~= 0)
posind1 = [fieldpointers(posind1(1)), posind1];
end
[posind1_x,posind1_y]=ind2sub([n,n],posind1);
%% 显示路径
for k = 2:length(posind1) - 1
field(posind1(k)) =6;
image(gca,1.5, 1.5, field);
grid on;
set(gca,'gridline','-','gridcolor','y','linewidth',2,'GridAlpha',0.5);
set(gca,'xtick',1:1:21,'ytick',1:1:21);
axis image;
drawnow;
end
line(posind1_y,posind1_x,'linewidth',3,'color',[1,0,1]);
else if isempty(setOpen)
disp('路径不存在!');
end
end
xlabel('x(单位:cm)','FontWeight','bold');
ylabel('y(单位:cm)','FontWeight','bold');
title('基于{ \color{red}Theta*} 算法的路径规划 ','fontsize',16)
end
%% 获得当前节点的八邻域
function [posinds,cost,heuristic]=get_neighbors(n,ii,currentpos_x,currentpos_y,goalpos_x,goalpos_y)
global setOpenCosts;
cost = Inf*ones(8,1); heuristic = Inf*ones(8,1); pos = ones(8,2);
% 左侧查询
newx = currentpos_x ; newy = currentpos_y-1;
if newy > 0 %如果没有到边界
pos(1,:) = [newx newy]; %获得新的坐标
heuristic(1) = abs(goalpos_x-newx) + abs(goalpos_y-newy); %heuristic(1)为启发函数计算的距离代价
cost(1) = setOpenCosts(ii) + 1; %costsofar为之前花费的代价，field(newy,newx)为环境威胁代价，cost(1)为经过此方向点的真实代价
end
% 向右查询
newx = currentpos_x; newy = currentpos_y+1;
if newy <= n
pos(2,:) = [newx newy];
heuristic(2) = abs(goalpos_x-newx) +
