蚁群路径搜索算法的MATLAB仿真

clc;
clear;
close all;

n=20;

wallpercent=0.1;

[ field,startposind,goalposind,costchart,fieldpointers ] = initializefield( n,wallpercent );

axishandle = creatureFigure( field,startposind,goalposind );
setopen=[startposind];setopencosts=[0];setopenheuristics=[inf];
setclose=[];setclosecosts=[];
movementdirection=['R','L','D','U'];

while ~max(ismember(setopen,goalposind))&&~isempty(setopen)
    [temp,ii]=min(setopencosts+setopenheuristics);
    [costs,heuristics,posinds]=findfvalue(setopen(ii),setopencosts(ii),field,goalposind);
    setclose=[setclose;setopen(ii)];setclosecosts=[setclosecosts;setopencosts(ii)];
    if ii>1&&ii<length(setopen)
        setopen=[setopen(1:ii-1);setopen(ii+1:end)];
        setopencosts=[setopencosts(1:ii-1);setopencosts(ii+1:end)];
        setopenheuristics=[setopenheuristics(1:ii-1);setopenheuristics(ii+1:end)];
        
    elseif 1==ii
        setopen=[setopen(ii+1:end)];
        setopencosts=[setopencosts(ii+1:end)];
        setopenheuristics=[setopenheuristics(ii+1:end)];
    else
        setopen=[setopen(ii+1:end)];
        setopencosts=[setopencosts(ii+1:end)];
        setopenheuristics=[setopenheuristics(ii+1:end)];
    end
    for jj=1:length(posinds)
        if ~isinf(costs(jj))
            if ~max([setopen;setclose]==posinds(jj))
                fieldpointers{posinds(jj)}=movementdirection(jj);
                 setopen = [setopen; posinds(jj)];
                 setopencosts = [setopencosts; costs(jj)];
                 setopenheuristics = [setopenheuristics; heuristics(jj)];
            elseif max(setopen==posinds(jj))
                i=find(setopen==posinds(jj));
                if setopencosts(i)>costs(jj)
                    setopencosts(i)=costs(jj);
                    setopenheuristics(i)=heuristics(jj);
                    fieldpointers{setopen(i)}=movementdirection(jj);
                    
                    
                end
            else i=find(setclose==posinds(jj));
                if setclosecosts(i)>costs(jj)
                   setclosecosts(i)=costs(jj);
                   fieldpointers{setclose(i)}=movementdirection(jj);  
                end
            end
        end
    end
    if isempty(setopen)
        break;
    end
    
   
    
   
end
if max(ismember(setopen,goalposind))
    p=findwayback(goalposind,fieldpointers);
    p=[p;inf*ones(2)];
for kk=1:length(p)
    if p(kk+2,1)==inf||p(kk+1,1)==inf
        break;
    elseif abs(p(kk+2,1)-p(kk,1))==1&&abs(p(kk+2,2)-p(kk,2))==1
        if p(kk+1,1)-p(kk,1)==1&&p(kk+2,2)-p(kk+1,2)==1
            if field(p(kk,2)+1,p(kk,1))==inf
               p;
            else p=[p(1:kk,:);p(kk+2:end,:)];
            end
        elseif p(kk+1,2)-p(kk,2)==1&&p(kk+2,1)-p(kk+1,1)==1
            if field(p(kk,2),p(kk,1)+1)==inf
               p;
            else p=[p(1:kk,:);p(kk+2:end,:)];
            end
         elseif p(kk+1,1)-p(kk,1)==-1&&p(kk+2,2)-p(kk+1,2)==1
            if field(p(kk,2)+1,p(kk,1))==inf
               p;
            else p=[p(1:kk,:);p(kk+2:end,:)];
            end
          elseif p(kk+1,2)-p(kk,2)==1&&p(kk+2,1)-p(kk+1,1)==-1
            if field(p(kk,2),p(kk,1)-1)==inf
               p;
            else p=[p(1:kk,:);p(kk+2:end,:)];
            end
           elseif p(kk+1,1)-p(kk,1)==-1&&p(kk+2,2)-p(kk+1,2)==-1
            if field(p(kk,2)-1,p(kk,1))==inf
               p;
            else p=[p(1:kk,:);p(kk+2:end,:)];
            end 
            elseif p(kk+1,2)-p(kk,2)==-1&&p(kk+2,1)-p(kk+1,1)==-1
            if field(p(kk,2),p(kk,1)-1)==inf
               p;
            else p=[p(1:kk,:);p(kk+2:end,:)];
            end
            elseif p(kk+1,1)-p(kk,1)==1&&p(kk+2,2)-p(kk+1,2)==-1
            if field(p(kk,2)-1,p(kk,1))==inf
                p;
            else p=[p(1:kk,:);p(kk+2:end,:)];
            end
        else p(kk+1,2)-p(kk,2)==-1&&p(kk+2,1)-p(kk+1,1)==1
            if field(p(kk,2),p(kk,1)+1)==inf
               p;
            else p=[p(1:kk,:);p(kk+2:end,:)];
            end
        end
        
     else 
        p;
    end
end
p=p(1:end-2,:);
    plot(p(:,1)+0.5,p(:,2)+0.5,'color',[0.95 0.95 0],'LineWidth',5);drawnow;
elseif isempty(setopen)
    disp('no solution');
end

C81