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=