www.pudn.com > Improved-AStar-on-3D-Terrain.rar > expand_array.m, change:2013-02-22,size:1895b


%Function to return and expanded array 
function exp_array=expand_array(node_x,node_y,xTarget,yTarget,path_cost,zx,CLOSED,dimao,MAX_X,MAX_Y) 
exp_array=[]; 
exp_count=1; 
c2=size(CLOSED,1);%Number of elements in CLOSED including the zeros 
flag=0;%Determines if the node is to be put as a successor. 
for k= 1:-1:-1 
    for j= 1:-1:-1    
     if (k~=j || k~=0)  %The node itself is not its successor,保证(0,0)点取不到 
         s_x = node_x+k;%横向移动1个单位 
         s_y = node_y+j;%纵向移动1个单位 
         if( (s_x >0 && s_x <=MAX_X) && (s_y >0 && s_y <=MAX_Y))%node within array bound 
             flag=1;    
             for c1=1:c2   
               if(s_x == CLOSED(c1,1) && s_y == CLOSED(c1,2)) 
                   flag=0; 
               end; 
             end;%End of for loop to check if a successor is on closed list. 
              
             if (flag == 1) 
                 exp_array(exp_count,1) = s_x;  %下一个节点的坐标 
                 exp_array(exp_count,2) = s_y; 
                 %cost of travelling to node 
                 exp_array(exp_count,3) = path_cost+sqrt(25*(node_x-s_x)^2 + 25*(node_y-s_y)^2+... 
                     (zx(node_y,node_x)-zx(s_y,s_x))^2)/(5*dimao(s_y,s_x));%g(n-1)+D(n-1,n)  
                 %distance between node and goal 
                 %以终点的粗糙度为判断准则 
                 exp_array(exp_count,4) =sqrt(25*(xTarget-s_x)^2 + 25*(yTarget-s_y)^2+... 
                     (zx(yTarget,xTarget)-zx(s_y,s_x))^2)/(5*dimao(yTarget,xTarget));%h(n) 
                 %f(n)=g(n-1)+D(n-1,n)+h(n) 
                 exp_array(exp_count,5) = exp_array(exp_count,3)+exp_array(exp_count,4); 
                 exp_count=exp_count+1; 
             end%Populate the exp_array list           
         end% End of node within array bound 
     end%End of if node is not its own successor loop 
   end%End of j for loop 
end%End of k for loop