komegaa.m

航迹规划中的AStar算法

%
%
%           K - O M E G A   By Emad Hasan
%           
%           Masters Project - RPI Fall 2007
%
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%


function [komega_path,ts, e_time,dist] = komegaA(Astar_coord, Astar_connect, s, d, k , b, n)
tic;
dist=inf;
komega_path=[];
%%
global exec or co crn;
openlist = [];
closedlist = [];
c=[]; ct=[];
stuck=0;
ts=0; nk=0;
%1. Add the source node to the open list
% G=0; H
if n==0
    H_s=abs(Astar_coord(2,s)-Astar_coord(2,d))+abs(Astar_coord(1,s)...
        -Astar_coord(1,d));
    openlist = [s s 0 H_s 0+H_s]';
    closedlist=[s s 0 H_s 0+H_s]';
    komega_path=[];
else
    c_crn=abs(Astar_coord(2,crn)-Astar_coord(2,crn))+abs(Astar_coord(1,s)...
        -Astar_coord(1,d));
    openlist = [crn crn 0 c_crn c_crn]';
    closedlist=[crn crn 0 c_crn c_crn]';
    komega_path=crn;
end
%% *************************************************************************


    % Do loop until,
    %     * Add the target square to the closed list, in which case the path
    %     has been found (see note below), or * Fail to find the target square,
    %     and the open list is empty. In this case, there is no path.


while (isempty(find(closedlist(1,:)==d, 1)) && ~isempty(openlist))

%%
    %%
    ts=ts+1;
    nk=nk+1;
    

    ct=[];
    x=find(openlist(5,:)==min(openlist(5,:)),1);
    curnode=openlist(1,x); % the node that has the lowest cost

    olindex=find(openlist(1,:)==curnode);
    
    ct=openlist(:,olindex);
    
    % Find out parents
    potcl=[];
    [potcl]=findparents(ct,c);
    [pw,pl]=size(potcl);
    
    olchk=1;
    if ~isempty(closedlist)
        while (isempty(find(closedlist(1,:)==ct(2,1), 1)) && isempty(potcl) && olchk<length(openlist(1,:)))
            % a) Look for the lowest F cost square on the open list. We refer to
            % this as the current square.
            x=find(openlist(5,:)==min(openlist(5,:)),1);
            curnode=openlist(1,x); % the node that has the lowest cost

            olindex=find(openlist(1,:)==curnode);
            ct=openlist(:,olindex);

            % Find out parents
            potcl=[];
            [potcl]=findparents(ct,c);
            [pw,pl]=size(potcl);

            % If parent of node not found in closed list or current tree, drop it
            % from the openlist and find next cost option
            if (isempty(find(closedlist(1,:)==ct(2,1))) && pl==0)
                openlist(:,olindex)=[];
            end
        end
    end
  
   
    
    % b 1) Switch it to the closed list.%
    [clw,lencl]=size(closedlist);
    % add node and its parent to closed list
    closedlist(:,lencl+1)=ct;
    if isempty(potcl)~=1
    closedlist(:,(lencl+2):((lencl+2)+pl-1))=potcl;
    end
    % Remove it from the open list
    openlist(:,olindex)=[];

    % If nk=n, then execute if capable
    
    % If at nth iteration excecute NXT if availabe
    if nk==n
        npath=[]; steps=[];
        nk=0;
        steps(1)=ct(1);
        nd=ct(1);
        npl=2;
        [r,l]=size(closedlist);
        while (nd~=crn && (npl-1)<l);
            x=find(closedlist(1,:)==nd, 1,'last');
            nd=closedlist(2,x);
            steps(npl)=nd;
            if size(steps)>k*n
                stuck=1;
            end
            npl=npl+1;
        end
        if ~isempty(find(steps==crn, 1))
            npath=fliplr(steps);
            [done]=execnxt(npath);
            npath(1)=[];
            komega_path=cat(2,komega_path,npath);
        else
            npath=[];
        end
       openlist=[];
       c_crn=abs(Astar_coord(2,crn)-Astar_coord(2,crn))+abs(Astar_coord(1,s)...
                -Astar_coord(1,d));
            if stuck
       closedlist=[crn crn 0 c_crn c_crn]';
       stuck=0;
       Astar_connect(:,crn)=0;
            end
    end

   
    %if ~isempty(openlist)
        clear c;
        c{1}=ct;
        % c) For b of the squares adjacent to this current square