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?-?已搜索過(guò)的地方
????????0?0?1;?...%?4?-?blue?-?下次搜索備選中心?
????????0?1?0;?...%?5?-?green?-?起始點(diǎn)
????????1?1?0;...%?6?-?yellow?-??到目?標(biāo)點(diǎn)的路徑?
???????1?0?1];%?7?-?-??目標(biāo)點(diǎn)?
colormap（cmap）;?
global?field
field=?ones（n）;
startposind?=22;???%sub2ind用來(lái)將行列坐標(biāo)轉(zhuǎn)換為線性坐標(biāo)，這里是必要的，因?yàn)槿绻裺tartposind設(shè)置成[xy]的形式，訪問(wèn)field（[xy]）的時(shí)候
goalposind?=86;????%它并不是訪問(wèn)x行y列元素，而是訪問(wèn)線性坐標(biāo)為x和y的兩個(gè)元素
?%field（ceil（n^2.*rand（floor（n*n*wallpercent）1）?））?=2;
field（1:5?7）?=?2;
field（81:3）?=?2;?
field（3:51:5）=2;
field（54）=2;
%?startposind?=?sub2ind（[nn]ceil（n.*rand）ceil（n.*rand））;???%sub2ind用來(lái)將行列坐標(biāo)轉(zhuǎn)換為線性坐標(biāo)，這里是必要的，因?yàn)槿绻裺tartposind設(shè)置成[xy]的形式，訪問(wèn)field（[xy]）的時(shí)候
%goalposind?=?sub2ind（[nn]ceil（n.*rand）ceil（n.*rand））;????%它并不是訪問(wèn)x行y列元素，而是訪問(wèn)線性坐標(biāo)為x和y的兩個(gè)元素
field（startposind?）=5;
field（goalposind?）=7;
costchart?=?NaN*ones（n）;??????%costchart用來(lái)存儲(chǔ)各個(gè)點(diǎn)的實(shí)際代價(jià)，NaN代表不是數(shù)據(jù)（不明確的操作）
costchart（startposind）?=?0;?????%起點(diǎn)的實(shí)際代價(jià)
fieldpointers?=?zeros（n）;?%fieldpointers用來(lái)存儲(chǔ)節(jié)點(diǎn)的父節(jié)點(diǎn)
global?setOpenCosts;
setOpen?=?（startposind）;?setOpenCosts?=?（0）;?setOpenHeuristics?=?（Inf）;
setClosed?=?[];?setClosedCosts?=?[];%初始化起點(diǎn)的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（AB）返回與A同大小的矩陣，其中元素1表示A中相應(yīng)位置的元素在B中也出現(xiàn)，0則是沒有出現(xiàn)
???????field（startposind?）=5;
???????field（goalposind?）=7;
???????image（1.51.5field）;?
???????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（setClosedgoalposind）））?
??????????break;
??????end;????
??????[~?ii]?=?min（setOpenCosts?+?setOpenHeuristics）;???%從OPEN表中選擇花費(fèi)最低的點(diǎn)tempii是其下標(biāo)（也就是標(biāo)號(hào)索引）

??????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（nfieldpointers（fieldpointers（setOpen（ii）））setOpen（ii））;
??????????if（trued）
??????????????id=find（setClosed==fieldpointers（fieldpointers（setOpen（ii））））;
??????????????[parent_postion_xparent_position_y]=ind2sub（[nn]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）））?;?%將此點(diǎn)的方向存在對(duì)應(yīng)的fieldpointers中

????????????????????setOpenCosts（ii）=setClosedCosts（id）+sqrt（power（（parent_postion_x-currentpos（1））2）+power（（parent_position_y-currentpos（2））2））;
??????????????end
????????