www.gusucode.com > 目标搜索源码程序 > 目标搜索源码程序/AStar_GUI/A_Star_Search.m

    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%Author:Michael Jacob Mathew

% The following code illustrates the A star search algorithm.
% The code is self explanatory
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% clc; clear all; clear classes;
function [PathTake, Found]=A_Star_Search(grid,init,goal)
tic;
cost=1;
Found=false;
Resign=false;

% grid=NewGrid();
% 
% init = [1,1]; %Start
% goal = [size(grid,1), size(grid,2)]; % Goal.

Heuristic=CalculateHeuristic(grid,goal); %Calculate the Heuristic   
 
ExpansionGrid(1:size(grid,1),1:size(grid,2)) = -1; % to show the path of expansion

ActionTaken=zeros(size(grid)); %Matrix to store the action taken to reach that particular cell

OptimalPath(1:size(grid,1),1:size(grid,2))={' '}; %Optimal Path derived from A Star

%how to move in the grid

delta = [-1,  0; % go up
          0, -1; % go left
          1,  0; %go down
          0,  1]; % go right
%           1,  1; %diagonal down
%          -1, -1]; %diagonal up
 

 
 for i=1:size(grid,1)
     for j=1:size(grid,2)
         gridCell=search();
         if(grid(i,j)>0)
            gridCell=gridCell.Set(i,j,1,Heuristic(i,j));
         else
             gridCell=gridCell.Set(i,j,0,Heuristic(i,j));
         end
         GRID(i,j)=gridCell;
         clear gridCell;
     end
 end

% drawEnvironment(grid,init,goal);

Start=search();
Start=Start.Set(init(1),init(2),grid(init(1),init(2)),Heuristic(init(1),init(2)));
Start.isChecked=1;
GRID(Start.currX,Start.currY).isChecked=1;
Goal=search();
Goal=Goal.Set(goal(1),goal(2),grid(goal(1),goal(2)),0);
 
OpenList=[Start];
ExpansionGrid(Start.currX,Start.currY)=0;

small=Start.gValue+Start.hValue;

count=0;
 while(Found==false || Resign==false) 
    
 small=OpenList(1).gValue+OpenList(1).hValue+cost;

for i=1:size(OpenList,2)
        fValue=OpenList(i).gValue+OpenList(i).hValue;
        if(fValue<=small)
            small=fValue;
            ExpandNode=OpenList(i);
            OpenListIndex=i;
        end
    end
    
   
    OpenList(OpenListIndex)=[];

    
    ExpansionGrid(ExpandNode.currX,ExpandNode.currY)=count;
    count=count+1;
    
    for i=1:size(delta,1)
        direction=delta(i,:);
        if(ExpandNode.currX+ direction(1)<1 || ExpandNode.currX+direction(1)>size(grid,1)|| ExpandNode.currY+ direction(2)<1 || ExpandNode.currY+direction(2)>size(grid,2))
            continue;
        else
            NewCell=GRID(ExpandNode.currX+direction(1),ExpandNode.currY+direction(2));
            
             if(NewCell.isChecked~=1 && NewCell.isEmpty~=1)
                GRID(NewCell.currX,NewCell.currY).gValue=GRID(ExpandNode.currX,ExpandNode.currY).gValue+cost;
                GRID(NewCell.currX,NewCell.currY).isChecked=1; %modified line from the v1
                OpenList=[OpenList,GRID(NewCell.currX,NewCell.currY)]; 
                ActionTaken(NewCell.currX,NewCell.currY)=i;
             end
            
             if(NewCell.currX==Goal.currX && NewCell.currY==Goal.currY && NewCell.isEmpty~=1)
                Found=true;
                Resign=true;
                disp('Search Successful');
                GRID(NewCell.currX,NewCell.currY).isChecked=1;
                ExpansionGrid(NewCell.currX,NewCell.currY)=count;
                GRID(NewCell.currX,NewCell.currY);
                break;
            end
            
        end
    end

     if(isempty(OpenList) && Found==false)
         Resign=true;
         disp('Search Failed');
         break;
     end
 end
 PathTake=[]; %For stroring the values taken for the path.
 if(Found==true) %further process only if there is a path
     Policy={'Up','Left','Down','Right','Diag Down','Diag Up'};
     X=goal(1);Y=goal(2);
     OptimalPath(X,Y)={'GOAL'};
     while(X~=init(1)|| Y~=init(2))
         x2=X-delta(ActionTaken(X,Y),1);
         y2=Y-delta(ActionTaken(X,Y),2);
         OptimalPath(x2,y2)=Policy(ActionTaken(X,Y));
         PathTake=[PathTake;[X,Y]];
         X=x2;
         Y=y2;
     end
     PathTake=[PathTake;[init(1),init(2)]]; % add the start state to the end
     Total_Elapsed_Time=toc

%     figure;
    plot(fliplr((PathTake(:,2))'),fliplr((PathTake(:,1))'));
    set(gca,'XLim',[-1,size(grid,2)+2],'YLim',[-1,size(grid,1)+2]);
    set(gca,'YDir','reverse');

   % SmoothPath(PathTake,size(grid));

%  ExpansionGrid; %to see how the expansion took place
%     OptimalPath %to see the optimal path taken by the Search Algo
 else

     disp('No Path to Display');
     Total_Elapsed_Time=toc
 end
end