focusddstar.m

通过研讨Stentz的Focussed D*文章

    if (flag ==1)
        h=plot(X.x, X.y, 'ro');drawnow;
        legend('Start','Goal','OPEN','CLOSED');
    end
%    
    Ylist = GetNeighbour8(X);
    for j = 1 : 8
        Y = Ylist(j); 
        if Y.x<1 | Y.y<1 | Y.x>Xmax | Y.y>Ymax
            continue;
        end
        Ynext = Var.B(Y.y, Y.x);
        Cxy = C(X,Y,map);

        
        %************comment of IF*****************************************
        %if Y is a NEW node, then insert Y into OPEN
        %or when Y points to X but H(y) dosen't equal to H(x) plus
        %Cost(X,y), this means X state or map(X) is changed, then re-insert
        %Y into OPEN. Or Y dosen't point to X, but X may be is on the
        %optimal next point of Y.
        if Var.Tag(Y.y, Y.x)==0 ... % a NEW state
                | (isequal(Ynext, X) & Var.H(Y.y, Y.x)~=Var.H(X.y, X.x)+Cxy) ...%Y指向了X但代价不一致
            Var.B(Y.y, Y.x) = X;
            INSERT(Y, Var.H(X.y,X.x)+Cxy);
        else
            if (~isequal(Ynext, X) & Var.H(Y.y, Y.x)>Var.H(X.y, X.x)+Cxy)
                if Var.H(X.y, X.x) > K_old
                    if Var.Tag(X.y, X.x)==2 INSERT(X, Var.H(X.y, X.x)); end
                else
                    Var.B(Y.y, Y.x) = X;
                    INSERT(Y, Var.H(X.y,X.x)+Cxy);                   
                end
            else
                if ~isequal(Ynext, X) & Var.H(X.y, X.x)>Var.H(Y.y, Y.x)+Cxy%Cxy=Cyx
                    if Var.H(Y.y, Y.x) > K_old
                        if Var.Tag(Y.y, Y.x)==2 INSERT(Y, Var.H(Y.y, Y.x)); end
                    else
                        Var.B(X.y, X.x) = Y;
                        INSERT(X, Var.H(Y.y, Y.x)+Cxy);
                    end
                end
            end
        end
    end
  
    val = GET_MIN_VAL();
    return; 

    
 %GET_MIN_STATE: Return the state on the OPEN list with minimum k(*) value,
 %NULL if the list is empty. Since OPEN is ordered degressive, so the 
 %last element is the minimal state
function state = GET_STATE()
    global Var;
    if isempty(Var.OPEN)
        state = [];
        disp(1)
    else
        state = Var.OPEN(end);
    end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
       
 

 
 %DELETE(X): Deletes state X from the OPEN list and set Tag(X)=CLOSED
function DELETE(X)
    global Var;
    Var.Tag(X.y, X.x) = 2;

    num = length(Var.OPEN);
    while num~=0 & ~(Var.OPEN(num).x==X.x & Var.OPEN(num).y==X.y)
        num = num - 1;
    end
    % 不考虑状态增大或者减小吗？
    Var.OPEN = [Var.OPEN(1:num-1), Var.OPEN(num+1:end)];

        
 %INSERT(X): set Tag(X) = 1(OPEN), compute k(X) from H(X) and Pre(X), and places
 % or re-positions state X on the OPEN list sorted by K(*)
function pos = INSERT(X, hnew)
    global Var;


    Var.Rcur = Var.R;

    if Var.Tag(X.y, X.x)==0 %X is in NEW state
       Var.K(X.y, X.x) = hnew;
    else
       if Var.Tag(X.y,X.x)==1 % X is in OPEN state
            Var.K(X.y,X.x) = min(Var.K(X.y,X.x), hnew);
            DELETE(X);
       else % X in in CLOSED state
            Var.K(X.y,X.x) = min(Var.H(X.y,X.x), hnew);
       end
    end

    Var.H(X.y, X.x) = hnew;
    Var.r(X.y, X.x) = Var.Rcur;
    Var.f(X.y, X.x) = Var.K(X.y, X.x)+Get_gVal(X, Var.Rcur);;
    Var.fB(X.y, X.x) = Var.f(X.y, X.x)+Var.Dcur;
    
    %Uncomment this line to watch which nodes are put into OPEN
%     persistent h;
%     if ishandle(h) delete(h);end
%     h = plot(X.x, X.y, 'bo'); drawnow;
    
    pos = PUT_STATE(X);
%            
function  pos = PUT_STATE(X)
   global Var;
   % OPEN is null
    if isempty(Var.OPEN)
       Var.OPEN = X;
       pos = 1;

    % X is in NEW or CLOSED state    
    elseif Var.Tag(X.y, X.x)~=1 % ~= OPEN
        num = length(Var.OPEN);
        while num>0 & Var.fB(Var.OPEN(num).y, Var.OPEN(num).x)<Var.fB(X.y, X.x)
              num = num - 1;
        end
        while num>0 & Var.fB(Var.OPEN(num).y, Var.OPEN(num).x)==Var.fB(X.y, X.x) ...
                    & Var.f(Var.OPEN(num).y, Var.OPEN(num).x)<Var.f(X.y, X.x)
              num = num - 1;
        end
        while num>0 & Var.fB(Var.OPEN(num).y, Var.OPEN(num).x)==Var.fB(X.y, X.x) ...
                    & Var.f(Var.OPEN(num).y, Var.OPEN(num).x)==Var.f(X.y, X.x) ...
                    & Var.K(Var.OPEN(num).y, Var.OPEN(num).x)<Var.K(X.y, X.x)
              num = num - 1;
        end    
        Var.OPEN = [Var.OPEN(1:num), X, Var.OPEN(num+1:end)];
        pos = num+1;
        
    % X is in OPEN     
    else  %Tag(X.y, X.x)==1 %OPEN  %delete X from OPEN
        DELETE(X);
        pos = PUT_STATE(X);
   end

   Var.Tag(X.y, X.x) = 1;                
 
   
   %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    %function s = S(X,Y)
      %  s=abs(Y.y-X.y)+abs(Y.x-X.x)+abs(Y.z-X.z);
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function val = MODIFY_COST(X,Y,cval)
    global Var;   
    %  cxy=cval;

    if Var.Tag(X.y, X.x)==2 
        pos = INSERT(X, Var.H(X.y,X.x));
    end
    val = GET_MIN_VAL();
    
    
    
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%    
function val = LESS(x,y)
    val = 0;
    if x(1)<y(1)
        val=1;
    elseif x(1)==y(1) & x(2)<y(2)
        val=1;
    else
        val=0;
    end
    
                
 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function f_K = GET_MIN_VAL()
    global Var;
    X=GET_MIN_STATE();
    if isempty(X)
        f_K = [];
    else
        f_K = [Var.f(X.y,X.x), Var.K(X.y,X.x)];
    end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function state=GET_MIN_STATE()
    global Var;
    state = [];
    X = GET_STATE();
    while ~isempty(X)
        r = Var.r(X.y, X.x);
        R = Var.Rcur;
        if ~isequal(r, R)
           DELETE(X); INSERT(X, Var.H(X.y,X.x));
           X = GET_STATE();
        else
            state=X;
            return;
        end
    end
      
      
function Ylist = GetNeighbour8(X)
        Ylist = [];
        Y = X;
        for j = 1 : 8
            switch (j)
                case 1
                    Y.x = Y.x + 1;
                case 2
                    Y.y = Y.y + 1;
                case 3
                    Y.x = Y.x - 1;
                case 4
                    Y.x = Y.x - 1;
                case 5
                    Y.y = Y.y - 1;
                case 6
                    Y.y = Y.y - 1;
                case 7
                    Y.x = Y.x + 1;
                case 8
                    Y.x = Y.x + 1;
            end
            Ylist = [Ylist Y];
        end


function y = IsNodeOnPath(list, p)
%确认节点是否存在于列表中
    y = 0;
    num = length(list);
    while num~=0
        if list(num).x==p.x & list(num).y==p.y
            y=1;
            return
        end
        num = num-1;
    end
	return;


function InconsistentX = CheckBySensor(Center, r, map)
    global ori_map;
    [Ymax Xmax] = size(map);
    min_x = max(Center.x - r, 1);
    max_x = min(Center.x + r, Xmax);
    min_y = max(Center.y - r, 1);
    max_y = min(Center.y + r, Ymax);
    Dmap = map(min_y : max_y, min_x : max_x) ...
           - ori_map(min_y : max_y, min_x : max_x);
    [ty tx] = find(Dmap~=0);
    InconsistentX = [];
    for i=1:length(tx)
        InconsistentX(i).x = tx(i) + min_x - 1;
        InconsistentX(i).y = ty(i) + min_y - 1;
        ori_map(InconsistentX(i).y, InconsistentX(i).x) = map(InconsistentX(i).y, InconsistentX(i).x);
    end
    

function bInconsistency = Sensor(X, map)
    global Var;
    Ylist = GetNeighbour8(X);
    [Ymax Xmax] = size(map);
    bInconsistency = 0;
    for i = 1 : 8
        Y = Ylist(i);
        if Y.x<1 | Y.y<1 | Y.x>Xmax | Y.y>Ymax
            continue;
        end
        if Var.Tag(Y.y, Y.x)~=0
            Sxy = abs(Var.H(Y.y, Y.x)-Var.H(X.y, X.x));
            Cxy = C(X, Y, map);
            if Sxy~=Cxy
                bInconsistency = 1;
                return;
            end
        end
    end
                
function f_H = COST(X)
    global Var;
    f = Var.H(X.y, X.x) + Get_gVal(X, Var.Rcur);
    H = Var.H(X.y, X.x);
    f_H = [f, H];
    return;