A*算法 A star 算法matlab

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;%欧几里德距离启发函数
                H_diagonal=min（abs（Nextpoint（1）-2+i-Epoint（1））abs（Nextpoint（2）-2+j-Epoint（2）））;%比较复杂的对角线启发函数
                H_strai