改编的三维空间A*star算法matlab

source=[5 5 5];goal=[45 45 45];
conn（::1）=[1 1 1; 1 1 1; 1 1 1;];
conn（::2）=[1 1 1; 1 2 1; 1 1 1;];
conn（::3）=[1 1 1; 1 1 1; 1 1 1;];
mapOriginal=im2bw（imread（‘map2.bmp‘））;
map0=imresize（mapOriginal[50 50]）;
map（::1）=map0;
for i=2:1:50
  map（::i）=map0;
end
display=true;


P=[];S=[];
for i=1:50
c=map（::i）;
[m n]=find（c==0）;
P=[mn];
P（:3）=i;
S=[S;P];
end

plot3（ S（:3）S（:2）S（:1）‘k.‘‘MarkerSize‘50）;hold on

mx=[];my=[];mz=[];mv=[];
rx=2;ry=2;rz=2;rv=1;
for i=1:1:3
for j=1:1:3
for k=1:1:3
if（~（i==2&&j==2&&k==2））
mx=[mx;k];my=[my;j];mz=[mz;k];mv=[mv;1];
end
end
end
end


Q=[source 0 heuristic（sourcegoal） 0+heuristic（sourcegoal） -1]; 
closed=ones（size（map））; 
closedList=[];
pathFound=false;tic;counter=0;
while size（Q1）>0
     [A I]=min（Q[]1）;
     n=Q（I（6）:）;
     Q=