www.gusucode.com > 全国大学生数学建模竞赛-机器人避障问题【论文+程序】 > 全国大学生数学建模竞赛-机器人避障问题【论文+程序】/文章+源码/程序/method1.m

    clear
clc
close all
theta=0:pi/100:2*pi;
zb{1}=[300 400;500 400;500 600;300 600];
zb{2}=[550 450;80 80];
zb{3}=[360 240;500 240;540 330;400 330;];
zb{4}=[280 100;410 100;345 210;];
zb{5}=[80 60;230 60;230 210;80 210];
zb{6}=[60 300;235 300;150 435;];
zb{7}=[0 470;220 470;220 530;0 530];
zb{8}=[150 600;240 600;270 680;180 680];
zb{9}=[370 680;430 680;430 800;370 800];
zb{10}=[540 600;670 600;670 730;540 730];
zb{11}=[640 520;720 520;720 600;640 600];
zb{12}=[500 140;800 140;800 200;500 200];
zb{13}=[0 0;800 0;800 800;0 800;];
zb{14}=[0 0 79];
zb{15}=[300 300 65];
zb{16}=[100 700 66];
zb{17}=[700 640 67];
%***********************计算区域内所有的直线段的方程*************************
lines=[];kxyh=[];
for i=1:length(zb)
    temp=zb{i};
    if size(zb{i},1)==2
        x=temp(2,1)*cos(theta)+temp(1,1);
        y=temp(2,2)*sin(theta)+temp(1,2);
        plot(x,y,'b-');hold on
    elseif size(temp,1)==3
        temp0=temp+[-10 -10;10 -10;0 10];
        for j=1:size(temp,1)
            if j==size(temp,1)   tt=1;  else   tt=j+1; end            
            lines=[lines [temp(j,1);temp(j,2);temp(tt,1);temp(tt,2)]];
        end
        plot([temp(:,1);temp(1,1)],[temp(:,2);temp(1,2)],'b-');hold on
    elseif size(temp,1)==4
        if i==13
            temp0=temp+[10 10;-10 10;-10 -10;10 -10];
        else
            temp0=temp+[-10 -10;10 -10;10 10;-10 10];
        end
        for j=1:size(temp,1)
            if j==size(temp,1) tt=1; else tt=j+1;end
            lines=[lines [temp(j,1);temp(j,2);temp(tt,1);temp(tt,2)]];
        end
        plot([temp(:,1);temp(1,1)],[temp(:,2);temp(1,2)],'b-');hold on
    elseif size(temp,1)==1
        plot(temp(1),temp(2),'r.','MarkerSize',12);text(temp(1)+10,temp(2)+20,char(temp(3)));hold on
    end
    if i<=12&i~=2
        for j=1:size(temp,1)
            temp1=temp(j,:);
            x=10*cos(theta)+temp1(1);
            y=10*sin(theta)+temp1(2);
            plot(x,y,'b-');hold on
        end
    end
end
axis([0 800 0 800]);grid on
%***********************计算区域内所有的圆弧段的方程*************************
kkk=1;
for i=1:length(zb)-4
    temp5=zb{i};if i==2 pp=1;else pp=size(temp5);end
    for j=1:pp
        if ~ismember(temp5(j,1),[0,800]) & ~ismember(temp5(j,2),[0,800])
            if j==1   ppp=size(temp5,1);  else   ppp=j-1; end
            if j==size(temp5,1) tt=1; else tt=j+1;end
            kxyh(:,kkk)=[temp5(ppp,1);temp5(ppp,2);temp5(j,1);temp5(j,2);temp5(tt,1);temp5(tt,2)];
            yxzb(1:2,kkk)=temp5(j,:);
            if i==2 yxzb(3,kkk)=80;else yxzb(3,kkk)=10;end
            kkk=kkk+1;
        end
    end
end
% ******************下面计算四个点O,A,B,C到各个圆弧的可行路径****************
yxbh=0;
for i=1:4
    %**********************计算四个点到各园的切点坐标************************
    x=[];y=[];z=[];kk=1;
    temp=zb{13+i};A=temp(1:2);
    for j=1:12
        temp2=zb{j};
        if j==2
            r=70;temp2=temp2(1,:);
        else
            r=10;
        end
        for k=1:size(temp2,1)
            B=temp2(k,:);
            if ~ismember(B(1),[0,800]) & ~ismember(B(2),[0,800])
                [x(:,kk) y(:,kk)]=qiedian(A,B,r);
                z=[z kk];
                kk=kk+1;
            end
        end
    end
    qdzb=[reshape(x,1,2*length(x));reshape(y,1,2*length(y));reshape([z;z],1,2*length(z))];
    %*****%********************判断是否为可行路径***************************
    for p=1:length(qdzb)
        %*********************判断与直线段不相交****************************
        for q=1:length(lines)
            temp3(p,q)=islineIntersect(temp,qdzb(1:2,p),lines(1:2,q),lines(3:4,q));
            if temp3(p,q)>0 break;end
        end
        %*********************判断与圆弧段不相交****************************
        for q=(length(lines)+1):(length(lines)+length(yxzb))
            temp3(p,q)=iscycleIntersect([A';qdzb(1:2,p)],yxzb(1:2,q-length(lines)),yxzb(3,q-length(lines)));
            if temp3(p,q)>0 break;end
        end
        %****************判断切点是否位于可行圆弧上*************************           
        if linedistance(qdzb(1:2,p),kxyh(:,qdzb(3,p)))==0
            temp3(p,(length(lines)+length(yxzb)+1):(length(lines)+length(yxzb)+length(kxyh)))=0;
        else
            temp3(p,(length(lines)+length(yxzb)+1):(length(lines)+length(yxzb)+length(kxyh)))=1;
        end
    end
    ind{i}=find(sum(temp3,2)==0);temp4=ind{i};
        for p=1:length(temp4)
            plot([A(1) qdzb(1,temp4(p))],[A(2) qdzb(2,temp4(p))],'r-');hold on
        end
    dykxlj{i,1}=[repmat(A',1,length(ind{i}));[qdzb(1,ind{i});qdzb(2,ind{i})];repmat([length(yxzb)+i],1,length(ind{i}));qdzb(3,ind{i})];
end
%%%******************下面计算各个圆与圆之间的可行路径***********************
yykxlj=[];
for i=1:length(yxzb)
    for j=i+1:length(yxzb)
        temp6=yuanyuanqiexian(yxzb(1:2,i),yxzb(3,i),yxzb(1:2,j),yxzb(3,j));
        temp7=[];
        for k=1:size(temp6,2)
            %********************判断与直线段不相交***************************
            for L=1:length(lines)
                temp7(k,L)=islineIntersect(temp6(1:2,k),temp6(3:4,k),lines(1:2,L),lines(3:4,L));
            end
            %********************判断与圆弧段不相交****************************
            for L=(length(lines)+1):(length(lines)+length(yxzb))
                temp7(k,L)=iscycleIntersect(temp6(:,k),yxzb(1:2,L-length(lines)),yxzb(3,L-length(lines)));
            end
            %****************判断切点是否位于可行圆弧上*************************
            if linedistance(temp6(1:2,k),kxyh(:,i))==0&&linedistance(temp6(3:4,k),kxyh(:,j))==0
                temp7(k,(length(lines)+length(yxzb)+1):(length(lines)+length(yxzb)+length(kxyh)))=0;
            else
                temp7(k,(length(lines)+length(yxzb)+1):(length(lines)+length(yxzb)+length(kxyh)))=1;
            end            
        end
        ind2=find(sum(temp7,2)==0);
        if ~isempty(ind2)
            yykxlj=[yykxlj [temp6(:,ind2);repmat([i;j],1,length(ind2))]];
        end
    end
end
for i=1:length(yykxlj)
    plot(yykxlj([1 3],i),yykxlj([2 4],i),'k-');hold on
end
save D11