clc
clear?all
m?=?30;n?=?30;
Spoint?=?[3?3];
Epoint?=?[29?22];
%%地圖
for?i?=?1:m+2
????if?i?==?1
????????for?j?=?1:n+2
????????????Matrix（ij）?=?-inf;
????????end
????elseif?i?==?m+2
????????for?j?=?1:n+2
????????????Matrix（ij）?=?-inf;
????????end
????else
????????for?j?=?1:n+2
????????????if?（（j?==?1）|（j?==?n+2））
????????????????Matrix（ij）?=?-inf;
????????????else
????????????????Matrix（ij）?=?inf;
????????????end
????????end
????end
end
%%障礙
for?j=2:10
????Matrix（5j）=-inf;
for?j=2:15
?????Matrix（24j）=-inf;
for?j=9:24
%for?j=6:24
?????Matrix（10j）=-inf;
for?j=20:31
????Matrix（15j）=-inf;
for?j=5:20
????Matrix（20j）=-inf;
for?j=18:27
????Matrix（28j）=-inf;
for?i=2:6??
????Matrix（i18）=-inf;
for?i=17:20
??????Matrix（i5）=-inf;
for?i=23:25?
?????Matrix（i20）=-inf;
for?i=13:17
????Matrix（i13）=-inf;
end
end
end
end
end
end
end
end
end
end
%end

%subplot（221）;
h1?=?plot（Spoint（1）Spoint（2）‘gO‘）;
hold?on
h2?=?plot（Epoint（1）Epoint（2）‘rO‘）;

%%尋路
Matrix（Spoint（1）Spoint（2））=0;
Matrix（Epoint（1）Epoint（2））=inf;
G=Matrix;
F=Matrix;
openlist=Matrix;
closelist=Matrix;
parentx=Matrix;
parenty=Matrix;
openlist（Spoint（1）Spoint（2））?=0;
%closelist（Epoint（1）Epoint（2））=inf;

for?i?=?1:n+2
????for?j?=?1:m+2
????????k?=?Matrix（ij）;
????????if（k?==?-inf）
????????????%subplot（221）;
????????????h3?=?plot（ij‘k.‘）;
%?????????elseif（k?==?inf）??%?show?green?feasible?point
%?????????????%subplot（221）;
%?????????????plot（ij‘gh‘）;
%?????????else
%?????????????%subplot（221）;
%?????????????plot（ij‘gh‘）;
????????end
????????hold?on
????end
end
axis（[0?m+3?0?n+3]）;
%subplot（221）;
plot（Epoint（1）Epoint（2）‘b+‘）;
%subplot（221）;
plot（Spoint（1）Spoint（2）‘b+‘）;
while（1）
????num=inf;
????for?p=1:m+2
????????for?q=1:n+2
????????????if（openlist（pq）==0&&closelist（pq）~=1）
????????????????Outpoint=[pq];
????????????????if（F（pq）>=0&&num>F（pq））
????????????????????num=F（pq）;
????????????????????Nextpoint=[pq];
????????????????end
????????????end
????????end
????end
????closelist（Nextpoint（1）Nextpoint（2））=1;
????for?i?=?1:3
????????for?j?=?1:3
????????????k?=?G（Nextpoint（1）-2+iNextpoint（2）-2+j）;
????????????if（i==2&&j==2|closelist（Nextpoint（1）-2+iNextpoint（2）-2+j）==1）
????????????????continue;
????????????elseif?（k?==?-inf）
????????????????G（Nextpoint（1）-2+iNextpoint（2）-2+j）?=?G（Nextpoint（1）-2+iNextpoint（2）-2+j）;
????????????????closelist（Nextpoint（1）-2+iNextpoint（2）-2+j）=1;
????????????elseif?（k?==?inf）
????????????????distance=（（i-2）^2+（j-2）^2）^0.5;
????????????????G（Nextpoint（1）-2+iNextpoint（2）-2+j）=G（Nextpoint（1）Nextpoint（2））+distance;
????????????????openlist（Nextpoint（1）-2+iNextpoint（2）-2+j）=0;
???????????????%?H=（（Nextpoint（1）-2+i-Epoint（1））^2+（Nextpoint（2）-2+j-Epoint（2））^2）^0.5;%歐幾里德距離啟發(fā)函數(shù)
????????????????H_diagonal=min（abs（Nextpoint（1）-2+i-Epoint（1））abs（Nextpoint（2）-2+j-Epoint（2）））;%比較復(fù)雜的對角線啟發(fā)函數(shù)
????????????????H_strai