A*星算法matlab实现

a星算法matlab实现 还有c++实现的算法

%function astar（mapdataNode）
tic
%=============数据=====================
 load （‘map.mat‘）;
 map =map（[1:10][1:10]）;
%  block = [  
%     0110000110;  
%     0110001000;  
%     0000001110; 
%     0000000000;  
%     0000000000;  
%     0000000000;  
%     0000000000;  
%     0000000000;  
%     0000000000;  
%     0000000000;  
% ];
% map = block;
map=fliplr（map‘）

startNode=[5 5];
endNode =[7 7];
%endNode =[9 10];
dataNode=[startNode;endNode]
%================================
m = 10;n = 10;%表示不含四周墙的地图 行数  列数 
 Spoint = [2 2];% 开始点
%Spoint = [3 3];
Epoint = [m+1 n+1];%接受点
 %%//障碍表   
 block = [  
    0100000000;  
    0110111000;  
    0000000000; 
    1110100000;  
    0100101110;  
    0100111110;  
    0001100010;  
    0100001010;  
    0111011011;  
    0000001000;  
]; 
%========map 判空是否有效========
  [m n] = size（map）;
 if （[m n] ~= [0 0]）
  block =map;
  m = m;
  n = n;%表示不含四周墙的地图 行数  列数 
 else
     if（[dataNode（21） dataNode（22）] > [m n]）
        dataNode（2:）= [m n]
     end
 end
 Spoint = dataNode（1:）+1;% 开始点
 Epoint =dataNode（2:）+1;%接受点
%%地图
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
%%向地图添加障障碍
%Matrix = zeros（size（block））;
 for i = 1:m
     for j=1:n
      if（block（ij）==1）
         Matrix（i+1j+1）=-inf;%  这是四周加墙的缘故
      elseif（block（ij）==0）
          Matrix（i+1j+1）=inf;% 
      else
          Matrix（i+1j+1）=inf;% 
      end
     end
 end

    subplot（131）;
    plot（Spoint（1）Spoint（2）‘r+‘）;
%%寻路
Matrix（Spoint（1）Spoint（2））=0;
Matrix（Epoint（1）Epoint（2））=inf;
G=Matrix;%计算值G
F=Matrix;%F
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（131）;
            plot（ij‘r.‘）;
        elseif（k == inf）
            subplot（131）;
            plot（ij‘gh‘）;
        else
            subplot（131）;
            plot（ij‘gh‘）;
        end
        hold on
    end
end
title（‘A*‘）;
axis（[0 m+3 0 n+3]）;
subplot（131）;
plot（Epoint（1）Epoint（2）‘b+‘）;
subplot（131）;
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];%第一次 起始

 属性            大小     日期    时间   名称
----------- ---------  ---------- -----  ----

     文件       7723  2017-04-27 11:19  astar.m

     文件        719  2017-04-23 19:23  map.mat

     文件     186720  2017-04-21 11:32  AStar.zip

----------- ---------  ---------- -----  ----

               195162                    3