www.pudn.com > SHIPCONTROL.rar > marineravoidcollision.m, change:2014-02-16,size:22198b


function marineravoidcollision 
clear all;clc; 
 
delta=0.1; 
iniangel=0; 
n=1500; 
R=0; 
indd=0; 
 meet=0; 
 xm=0; 
 ym=0; 
 laichuanshu=3; 
 buchang=1; 
 deltae=0; 
 hxu1=8;  
 xielv=0; 
 jieju=0; 
 adds=0.2; 
 xmm=0; 
 ymm=0; 
%  am=zeros(laichuanshu); 
%  bm=zeros(laichuanshu); 
 Dis=zeros(n,laichuanshu); 
 %Dis=zeros(laichuanshu); 
 sida1=zeros(1,laichuanshu); 
 sida2=zeros(1,laichuanshu); 
 sida3=zeros(1,laichuanshu); 
 a5s=zeros(1,laichuanshu); 
 rvx=zeros(1,laichuanshu); 
 rvy=zeros(1,laichuanshu); 
 Kp=4;    %4.1; 
 Td=0;     %1.5; 
 Ti=400; 
 ind=zeros(1,n); 
 lacd=zeros(1,laichuanshu); 
 ind2=zeros(1,n); 
 sdd=iniangel; 
     % numbers of other ship  
% for i=1:laichuanshu  
x=input('please select ship meeting situation:1 or 2 or 3 or 4\n'); 
 switch (x)   
case 1 
    rad1=200+rand*2; 
    rad2=200+rand*2; 
    rad3=200+rand*2; 
     
    a11=rand*100+4000; 
    a21=(a11+15*rad1)*cos(asin(rad1/a11)*(0.3+rand*0.7)+asin(rad2/(a11+15*rad1))); 
    a31=a11+500; 
    b11=-rand*3+1; 
    b21=-rand*3-1; 
    b31=rand*3-1; 
     
     
     
    a12=0;    %-rand*100-1000; 
    a22=(a11+15*rad1)*sin(asin(rad1/a11)*(0.3+rand*0.7)+asin(rad2/(a11+15*rad1))); 
    a32=a12-rad1*3; 
     
     
    b12=0;%-rand*2-1; 
    b22=-rand*0.1; 
    b32=-rand*2; 
     
    case 2 
     rad1=200+rand*2; 
    rad2=200+rand*2; 
    rad3=200+rand*2; 
     
    a11=rand*100+3500; 
    a21=(a11+15*rad1)*cos(-asin(rad1/a11)*(0.3+rand*0.7)-asin(rad2/(a11+15*rad1))); 
    a31=a11+500; 
    b11=-rand*3+1; 
    b21=-rand*3-1; 
    b31=rand*3+1; 
     
     
     
    a12=0;    %-rand*100-1000; 
    a22=(a11+15*rad1)*sin(-asin(rad1/a11)*(0.3+rand*0.7)-asin(rad2/(a11+15*rad1))); 
    a32=a12+rad1*3; 
     
     
    b12=0.01;%-rand*2-1; 
    b22=-rand*0.1; 
    b32=rand*0.2;     
         
         
         
         
    case 3 
     rad1=200+rand*2; 
    rad2=200+rand*2; 
    rad3=200+rand*2; 
     
    a11=rand*100+3500; 
    a21=(a11+15*rad1)*cos(asin(rad1/a11)*(2.1+rand*0.7)+asin(rad2/(a11+15*rad1))); 
    a31=(a11+15*rad1)*cos(-asin(rad1/a11)*(2.1+rand*0.7)-asin(rad2/(a11+15*rad1))); 
    b11=-rand*3+1; 
    b21=-rand*3-1; 
    b31=rand*3-1; 
     
     
     
    a12=0;    %-rand*100-1000; 
    a22=(a11+15*rad1)*sin(asin(rad1/a11)*(2.1+rand*0.7)+asin(rad2/(a11+15*rad1))); 
    a32=(a11+15*rad1)*sin(-asin(rad1/a11)*(2.1+rand*0.7)-asin(rad2/(a11+15*rad1))); 
     
     
    b12=0.01;%-rand*2-1; 
    b22=-rand*0.1; 
    b32=rand*0.2; 
     
    case 4 
     rad1=200+rand*2; 
    rad2=200+rand*2; 
    rad3=200+rand*2; 
     
    a11=rand*100+3500; 
    a21=(a11+15*rad1)*cos(-asin(rad1/a11)*(0.3+rand*0.7)-asin(rad2/(a11+15*rad1))); 
    a31=(a11+15*rad1)*cos(asin(rad1/a11)*(0.3+rand*0.7)+asin(rad2/(a11+15*rad1))); 
    b11=-rand*3+1; 
    b21=-rand*3-1; 
    b31=rand*3+1; 
     
     
     
    a12=0;    %-rand*100-1000; 
    a22=(a11+15*rad1)*sin(-asin(rad1/a11)*(0.3+rand*0.7)-asin(rad2/(a11+15*rad1))); 
    a32=(a11+15*rad1)*sin(asin(rad1/a11)*(0.3+rand*0.7)+asin(rad2/(a11+15*rad1))); 
     
     
    b12=0.01;%-rand*2-1; 
    b22=-rand*0.1; 
    b32=rand*0.2;     
         
     
 end 
   
    alpha=0:pi/20:2*pi; 
     
    xer1=rad1*cos(alpha); 
    xer2=rad2*cos(alpha); 
    xer3=rad3*cos(alpha); 
     
    yer1=rad1*sin(alpha); 
    yer2=rad2*sin(alpha); 
    yer3=rad3*sin(alpha); 
    xui=1:1:6000; 
    xer4=xui; 
    yer4=xui; 
    xer5=xui; 
    yer5=xui; 
    xer6=xui; 
    yer6=xui;     
    x02=xm; 
    y02=ym; 
%     x06=0; 
%     y06=0; 
    G=plot(x02, y02, 'r');  
    hold on  
     Q1=plot(xer1, yer1); 
     %hold  on 
     Q2=plot(xer2, yer2); 
     %hold  on 
     Q3=plot(xer3, yer3); 
     Q4=plot(xer4,yer4); 
     Q5=plot(xer5,yer5); 
     Q6=plot(xer6,yer6,'g--'); 
     %hold on 
%      Q4=plot(x06,y06, 'b'); 
%    eval(['a',num2str(i),'1=rand*1000+4500',';']) 
%    eval(['b',num2str(i),'1=-rand*10+5',';'])  % 
%    eval(['a',num2str(i),'2=-rand*1000-3000',';']) 
%    eval(['b',num2str(i),'2=rand*10-2',';']) % 
%    eval(['rad',num2str(i),'=900+rand*100',';']) 
%      
%     eval(['xer',num2str(i),'=','rad',num2str(i),'*cos(alpha)',';']); 
%     eval(['yer',num2str(i),'=','rad',num2str(i),'*sin(alpha)',';']); 
%     eval(['Q',num2str(i),'=plot(xer',num2str(i),',yer',num2str(i),');']); 
%     hold  on % 画出小球 
% end 
 
 
 
 
ui=zeros(1,n); 
m7=7; 
YY=zeros(n+1,m7); 
YY01=zeros(1,n); 
YY(1,:)=[1 0 0 0 0 0 0]; 
for i=2:n 
    if i==80||i==400 
        pause 
    end 
  sl=0;   
 gt=mariner(YY(i,:),ui(i)); 
   for jv=1:m7 
       if i>1 
    YY(i+1,jv)=YY(i,jv)+gt(jv); 
       end 
   end 
x01=YY(i,5); 
y01=YY(i,6); 
x02=[x02,x01]; 
y02=[y02,y01]; 
% delta=YY(i,7); 
 
x00=gt(5); 
 
y00=gt(6); 
YY01(i)=YY(i+1,4);   
YY01(i)=atan2(y00,x00);    
pause(0.001);  
 
    x1=a11+b11*i; 
    x2=a21+b21*i; 
    x3=a31+b31*i; 
     
    y1=a12+b12*i; 
    y2=a22+b22*i; 
    y3=a32+b32*i; 
     
     xer1=x1+rad1*cos(alpha); 
     xer2=x2+rad2*cos(alpha);    
     xer3=x3+rad3*cos(alpha); 
      
    yer1=y1+rad1*sin(alpha); 
    yer2=y2+rad2*sin(alpha);      
    yer3=y3+rad3*sin(alpha); 
     
     
       
     axis([-4000+min(min(x02)) 4000+max(max(x02)) -4000+min(min(y02)) 4000+max(max(y02)) ]); 
     
     %set(gcf,'doublebuffer','on'); 
     hold on 
     %pause(0.1); 
    set(G,'xdata',x02,'ydata',y02); 
    hold on 
    axis equal 
    set(Q1,'xdata',xer1,'ydata',yer1) 
    axis equal 
    hold on 
    set(Q2,'xdata',xer2,'ydata',yer2) 
    axis equal 
     hold on 
    set(Q3,'xdata',xer3,'ydata',yer3) 
    axis equal 
%      hold on 
%       set(Q4,'xdata',x06,'ydata',y06) 
%     axis equal 
   
for j1=1:laichuanshu 
%     eval(['y',num2str(j),'=a',num2str(j),'2+b',num2str(j),'2*i',';']) 
%     eval(['x',num2str(j),'=a',num2str(j),'1+b',num2str(j),'1*i',';']) 
    
%     alpha=0:pi/20:2*pi; 
%    eval(['xer',num2str(j),'=','x',num2str(j),'+','rad',num2str(j),'*cos(alpha)',';']); 
%    eval(['yer',num2str(j),'=','y',num2str(j),'+','rad',num2str(j),'*sin(alpha)',';']); 
%    eval(['set(Q',num2str(j),',','xdata',',xer',num2str(j),',','ydata',',yer',num2str(j),');']); 
 
     
     
%     sida1(1)=atan2(y1-y01,x1-x01) 
%     sida1(2)=atan2(y2-y01,x2-x01) 
%     sida1(3)=atan2(y3-y01,x3-x01) 
    sida1(j1)= eval(['atan2(','y', num2str(j1),'-y01',',','x',num2str(j1),'-x01)',';']); 
     
%     if sida1(j1)<0 
%         sida1(j1)=2*pi+sida1(j1); 
%     end 
%      
    %sida1(j1) 
   %bijiaojuli=sqrt((y1-y01)^2+(x1-x01)^2) 
    Dis(i,j1)=eval(['sqrt(','(y',num2str(j1),'-y01)^2','+(x',num2str(j1),'-x01)^2)',';']); 
    sida2(j1)=eval(['sida1(',num2str(j1),')-','asin(rad',num2str(j1),'/Dis(',num2str(i),',',num2str(j1),')',')',';' ]); %sin不能小于1,否则出错   
    sida3(j1)=eval(['sida1(',num2str(j1),')+','asin(rad',num2str(j1),'/Dis(',num2str(i),',',num2str(j1),')',')',';' ]); 
%     eval(['a1s(',num2str(j),')=Dis',num2str(j),';']) 
%     eval(['a2s(',num2str(j),')=sida2',num2str(j),';']) 
%     eval(['a3s(',num2str(j),')=sida3',num2str(j),';']) 
%     eval(['a4s(',num2str(j),')=sida1',num2str(j),';']) 
    rvx(j1)=eval(['-b',num2str(j1),'1+x00',';']);   %此处有问题,已修改 
    rvy(j1)=eval(['-b',num2str(j1),'2+y00',';']);   %此处有问题,已修改 
    a5s(j1)=atan2(rvy(j1),rvx(j1));                %此处有问题,已修改  
%     if a5s(j1)<0 
%         a5s(j1)=2*pi+a5s(j1); 
%     end 
end                                                   
%sida1 
 
 
 
[hxu2,hindex2]=sort(sida1); 
%size(hindex2) 
 
% for j5=1:laichuanshu 
%     Dis(i,j5)=Dis(i,hindex2(j5)); 
%     sida1(j5)=sida1(hindex2(j5)); 
%     sida2(j5)=sida2(hindex2(j5)); 
%     a5s(j5)=a5s(hindex2(j5)); 
%     sida3(j5)=sida3(hindex2(j5)); 
%     rvx(j5)=rvx(hindex2(j5)); 
%     rvy(j5)=rvy(hindex2(j5)); 
%     dex=eval(['hindex2(',num2str(j5),');']); 
% %     eval(['x',num2str(j5),'=x',num2str(dex),';']); 
% %     eval(['y',num2str(j5),'=y',num2str(dex),';']); 
% %     eval(['rad',num2str(j5),'=rad',num2str(dex),';']); 
% %     eval(['b',num2str(j5),'1=b',num2str(dex),'1;']); 
% %     eval(['b',num2str(j5),'2=b',num2str(dex),'2;']); 
% %     eval(['a',num2str(j5),'1=a',num2str(dex),'1;']); 
% %     eval(['a',num2str(j5),'2=a',num2str(dex),'2;']); 
%      
% end    
     
for j=1:laichuanshu 
    %j=hindex2(j); 
    sx0=eval(['x',num2str(j),';']); 
    sy0=eval(['y',num2str(j),';']); 
    R=eval(['rad',num2str(j),';']); 
    xielv=rvy(j)/rvx(j); %y00/x00; 
   jieju=y01-x01*xielv; 
    a=xielv;     
    b=jieju; 
    
%    
  
 
if i>2&&Dis(i,j)<5000 
     
%     (R^2*a^2 + R^2 - a^2*sx0^2 - 2*a*b*sx0 + 2*a*sx0*sy0 - b^2 + 2*b*sy0 -sy0^2) 
%  Dis(i,j) 
% Dis(i-2,j) 
%      
     
     
if (R^2*a^2 + R^2 - a^2*sx0^2 - 2*a*b*sx0 + 2*a*sx0*sy0 - b^2 + 2*b*sy0 -sy0^2)>0&&(Dis(i,j)<Dis(i-2,j)); 
    sl=sl+1 
x10=(xielv*(sy0-jieju)+sx0)/(1+xielv^2); 
y10=jieju+xielv*x10; 
xmm=(x01+x10)/2; 
ymm=(y01+y10)/2; 
 inds=2; 
 n0=10000; 
 Diss=zeros(1,n0); 
 Diss(inds-1)=2; 
 Diss(inds)=3; 
 j3=1; 
 YYo=zeros(n0+1,7); 
 while sqrt((Diss(inds-1)-R)^2+(Diss(inds)-R)^2)>=(R/100)&&j3<50 
     j3=j3+1; 
     YYo(1,:)=[1 0 0 0.5 0 0 YY(i+1,7)]; 
     uio=0.6; 
      for io=2:2:n0 
          gt=mariner(YYo(io,:),uio); 
   for jv=1:m7 
       if io>1 
    YYo(io+1,jv)=YYo(io,jv)+gt(jv); 
       end 
   end 
            
            
           eval([ 'x',num2str(j),'10=x',num2str(j),'+b',num2str(j),'1*',num2str(io),';']); 
           eval([ 'y',num2str(j),'10=y',num2str(j),'+b',num2str(j),'2*',num2str(io),';']); 
           xRo=eval([ 'x',num2str(j),'10',';']); 
           yRo=eval([ 'y',num2str(j),'10',';']); 
           Diss(io)=sqrt((YYo(io,5)-xRo)^2+(YYo(io,6)-yRo)^2); 
           we=(Diss(io)-Diss(io-1)); 
           if we>0||Diss(io)<1.02*R 
               inds=io;    
               break; 
           end 
      end 
if (Diss(inds)>R&&Diss(inds-1)>R&&((YYo(inds-1,5)-xRo)*(YYo(inds,6)-yRo)-(YYo(inds-1,6)-yRo)*(YYo(inds,5)-xRo))>0) 
    
    x01=xmm; 
    y01=ymm; 
else 
   x10=xmm; 
    y10=ymm;  
end           
  xmm=(x01+x10)/2; 
  ymm=(y01+y10)/2; 
  lacd(j)=sqrt((xmm-xRo)^2+(ymm-yRo)^2); 
   
    %size(lacd)  
 end 
  
  
end 
end 
%lacd 
end 
% 
 
if sl~=0 
    [hxu1,hindex1]=max(lacd); 
    %size(hindex1) 
    for  j8=1:laichuanshu 
%         hindex1 
%         size(hindex1) 
      if  hindex1==hindex2(j8) 
    ind2(i)=hindex1; 
    ind(i)=j8; 
    indd=j8; 
      end 
    end 
end 
 
 
% if sl~=0 
%     [hxu1,hindex1]=max(lacd); 
%  
%  ind2(i)=hindex1; 
%     ind(i)=hindex1; 
%     indd=hindex1; 
% end 
 
 
 
    indd 
     
   
     
    x01=YY(i,5); 
    y01=YY(i,6); 
     
    if ind(i)~=0  
    px2=YY(i,5)+rvx(hindex2(ind(i))); 
    py2=YY(i,6)+rvy(hindex2(ind(i))); 
    px3=eval(['x',num2str(hindex2(ind(i))),';']); 
    py3=eval(['y',num2str(hindex2(ind(i))),';']); 
    xielv=tan(sida3(hindex2(ind(i)))); 
   jieju=y01-x01*xielv; 
    a=xielv;     
    b=jieju; 
    end 
    xer4=x01+xui-3000; 
    xer5=x01+xui-3000; 
    xer6=x01+xui-3000; 
     
     
   if ind(i)~=0  
        ssxielv=tan(sida3(hindex2(ind(i)))); 
        ssjieju=y01-x01*ssxielv; 
        yer5=xer5*ssxielv+ssjieju; 
        set(Q5,'xdata',xer5,'ydata',yer5) 
         
        sxielv=tan(sida2(hindex2(ind(i)))); 
        sjieju=y01-x01*sxielv; 
        yer4=xer4*sxielv+sjieju; 
        set(Q4,'xdata',xer4,'ydata',yer4)  
   end 
    
   if ind(i)~=0 
         
       yer6=xer6*(rvy(hindex2(ind(i)))/rvx(hindex2(ind(i))))+y01-x01*(rvy(hindex2(ind(i)))/rvx(hindex2(ind(i)))); 
       %yer6=xer6*a+b; 
       set(Q6,'xdata',xer6,'ydata',yer6) 
        
   end 
  if hxu1==0 
      pause 
      hxu1=hxu1 
  end 
       
    
    
if ind(i)~=laichuanshu&&ind(i)~=0 
     
    x0=eval(['x',num2str(hindex2(ind(i)+1)),';']); 
    y0=eval(['y',num2str(hindex2(ind(i)+1)),';']); 
    R=eval(['rad',num2str(hindex2(ind(i)+1)),';']); 
    panduan=(py3+(x00/y00)*px3-(y01+x01*(x00/y00)))*(y0+(x00/y00)*x0-(y01+x01*(x00/y00))); 
end 
 
 
   
if ind(i)~=1&&ind(i)~=0 
     
     dx0=eval(['x',num2str(hindex2(ind(i)-1)),';']); 
    dy0=eval(['y',num2str(hindex2(ind(i)-1)),';']); 
    panduan2=(py3+(x00/y00)*px3-(y01+x01*(x00/y00)))*(dy0+(x00/y00)*dx0-(y01+x01*(x00/y00))); 
     
    dR=eval(['rad',num2str(hindex2(ind(i)-1)),';']); 
end 
        
if ind(i)~=0 
    dxielv=tan(sida2(hindex2(ind(i)))); 
   djieju=y01-x01*dxielv; 
    da=dxielv;     
    db=djieju; 
     
     
%     plot(xui,yui,'g--') 
     
      
%     axis equal 
   v=eval(['b',num2str(hindex2(ind(i))),'2']); 
   u=eval(['b',num2str(hindex2(ind(i))),'1']); 
   V=sqrt(x00^2+y00^2); 
    
%    (R^2*a^2 + R^2 - a^2*x0^2 - 2*a*b*x0 + 2*a*x0*y0 - b^2 + 2*b*y0 -y0^2)>0 
% (panduan>0) 
% (dR^2*da^2 + dR^2 - da^2*dx0^2 - 2*da*db*dx0 + 2*da*dx0*dy0 - db^2 + 2*db*dy0 -dy0^2) 
 
    
end 
 
%ceshiuy=((dR^2*da^2 + dR^2 - da^2*dx0^2 - 2*da*db*dx0 + 2*da*dx0*dy0 - db^2 + 2*db*dy0 -dy0^2)<0) 
 
 
 
if ind(i)~=1&&ind(i)~=laichuanshu 
 
%case 1 
if (ind(i)~=0)&&(R^2*a^2 + R^2 - a^2*x0^2 - 2*a*b*x0 + 2*a*x0*y0 - b^2 + 2*b*y0 -y0^2)>0&&(panduan>0)&&(dR^2*da^2 + dR^2 - da^2*dx0^2 - 2*da*db*dx0 + 2*da*dx0*dy0 - db^2 + 2*db*dy0 -dy0^2)<0&&(i>2); 
  
     
     
     
    meet=1 
    s=tan(rem(2*pi,sida2(hindex2(ind(i))))+adds); 
  sdd1=asin((v + s*(V^2*s^2 + V^2 - s^2*u^2 + 2*s*u*v - v^2)^(1/2) - s*u)/(V*(s^2 + 1))); 
  sdd2=asin(-(s*(V^2*s^2 + V^2 - s^2*u^2 + 2*s*u*v - v^2)^(1/2) - v + s*u)/(V*(s^2 + 1))); 
  a4s=atan2(y00,x00); 
% if a4s<0 
%     a4s=2*pi+a4s; 
% end 
 
if abs(a4s-sdd1)<abs(a4s-sdd2) 
         sdd=sdd1; 
else 
    sdd=sdd2; 
end 
 
 
% if abs(linkxielv*laichuanxielv+1)<0.1;  %(ind(i)==ind(i-2))&&i>2&& 
%     sdd=iniangel; 
% end 
 
end 
 
%case 2 
if(ind(i)~=0)&&(i>2)&&(R^2*a^2 + R^2 - a^2*x0^2 - 2*a*b*x0 + 2*a*x0*y0 - b^2 + 2*b*y0 -y0^2)<0&&(panduan2>0)&&(dR^2*da^2 + dR^2 - da^2*dx0^2 - 2*da*db*dx0 + 2*da*dx0*dy0 - db^2 + 2*db*dy0 -dy0^2)>0; 
  meet=2 
    s=tan(rem(2*pi,sida3(hindex2(ind(i))))+adds); 
  sdd1=asin((v + s*(V^2*s^2 + V^2 - s^2*u^2 + 2*s*u*v - v^2)^(1/2) - s*u)/(V*(s^2 + 1))); 
  sdd2=asin(-(s*(V^2*s^2 + V^2 - s^2*u^2 + 2*s*u*v - v^2)^(1/2) - v + s*u)/(V*(s^2 + 1))); 
  a4s=atan2(y00,x00); 
% if a4s<0 
%     a4s=2*pi+a4s; 
% end 
 
if abs(a4s-sdd1)<abs(a4s-sdd2) 
         sdd=sdd1; 
else 
    sdd=sdd2; 
end 
 
% if abs(linkxielv*laichuanxielv+1)<0.1;  %(ind(i)==ind(i-2))&&i>2&& 
%     sdd=iniangel; 
% end 
 
 
end 
 
 
%case 3 
 
if (ind(i)~=0)&&(i>2)&&(panduan2>0)&&(panduan>0)&&(R^2*a^2 + R^2 - a^2*x0^2 - 2*a*b*x0 + 2*a*x0*y0 - b^2 + 2*b*y0 -y0^2)<0&&(dR^2*da^2 + dR^2 - da^2*dx0^2 - 2*da*db*dx0 + 2*da*dx0*dy0 - db^2 + 2*db*dy0 -dy0^2)<0; 
      meet=3 
   
   if (px1-px3)*(py2-py3)-(py1-py3)*(px2-px3)>0 
        s=tan(rem(2*pi,sida2(hindex2(ind(i))))+adds); 
   end 
    
   if (px1-px3)*(py2-py3)-(py1-py3)*(px2-px3)<0 
    s=tan(rem(2*pi,sida3(hindex2(ind(i))))+adds); 
   end 
    
  sdd1=asin((v + s*(V^2*s^2 + V^2 - s^2*u^2 + 2*s*u*v - v^2)^(1/2) - s*u)/(V*(s^2 + 1))); 
  sdd2=asin(-(s*(V^2*s^2 + V^2 - s^2*u^2 + 2*s*u*v - v^2)^(1/2) - v + s*u)/(V*(s^2 + 1))); 
  a4s=atan2(y00,x00); 
% if a4s<0 
%     a4s=2*pi+a4s; 
% end 
 
if abs(a4s-sdd1)<abs(a4s-sdd2) 
         sdd=sdd1; 
else 
    sdd=sdd2; 
end 
 
% if abs(linkxielv*laichuanxielv+1)<0.1;  %(ind(i)==ind(i-2))&&i>2&& 
%     sdd=iniangel; 
% end 
 
end 
 
%case 4 
 
if (ind(i)~=0)&&(i>2)&&(panduan2>0)&&(panduan>0)&&(R^2*a^2 + R^2 - a^2*x0^2 - 2*a*b*x0 + 2*a*x0*y0 - b^2 + 2*b*y0 -y0^2)>0&&(dR^2*da^2 + dR^2 - da^2*dx0^2 - 2*da*db*dx0 + 2*da*dx0*dy0 - db^2 + 2*db*dy0 -dy0^2)>0; 
      meet=4 
   
   if (px1-px3)*(py2-py3)-(py1-py3)*(px2-px3)>0 
        s=tan(rem(2*pi,sida2(hindex2(ind(i)-1)))+adds); 
   end 
    
   if (px1-px3)*(py2-py3)-(py1-py3)*(px2-px3)<0 
    s=tan(rem(2*pi,sida3(hindex2(ind(i)+1)))+adds); 
   end 
    
  sdd1=asin((v + s*(V^2*s^2 + V^2 - s^2*u^2 + 2*s*u*v - v^2)^(1/2) - s*u)/(V*(s^2 + 1))); 
  sdd2=asin(-(s*(V^2*s^2 + V^2 - s^2*u^2 + 2*s*u*v - v^2)^(1/2) - v + s*u)/(V*(s^2 + 1))); 
  a4s=atan2(y00,x00); 
% if a4s<0 
%     a4s=2*pi+a4s; 
% end 
 
if abs(a4s-sdd1)<abs(a4s-sdd2) 
         sdd=sdd1; 
else 
    sdd=sdd2; 
end 
 
% if abs(linkxielv*laichuanxielv+1)<0.1;  %(ind(i)==ind(i-2))&&i>2&& 
%     sdd=iniangel; 
% end 
 
end 
 
 
end 
 
px1=YY(i,5); 
py1=YY(i,6); 
 
ind(i); 
if (ind(i)==1) 
    meet=5 
    if ((px1-px3)*(py2-py3)-(py1-py3)*(px2-px3))<0&&(R^2*a^2 + R^2 - a^2*x0^2 - 2*a*b*x0 + 2*a*x0*y0 - b^2 + 2*b*y0 -y0^2)<0&&panduan>0 
        Dis(i,hindex2(ind(i))) 
        sida1(hindex2(ind(i))) 
        sida2(hindex2(ind(i))) 
        sida3(hindex2(ind(i))) 
        s=tan(rem(2*pi,sida3(hindex2(ind(i))))+adds); 
    else 
         s=tan(rem(2*pi,sida2(hindex2(ind(i))))+adds); 
    end 
     
    sdd1=asin((v + s*(V^2*s^2 + V^2 - s^2*u^2 + 2*s*u*v - v^2)^(1/2) - s*u)/(V*(s^2 + 1))); 
  sdd2=asin(-(s*(V^2*s^2 + V^2 - s^2*u^2 + 2*s*u*v - v^2)^(1/2) - v + s*u)/(V*(s^2 + 1))); 
  a4s=atan2(y00,x00); 
% if a4s<0 
%     a4s=2*pi+a4s; 
% end 
 
if abs(a4s-sdd1)<abs(a4s-sdd2) 
         sdd=sdd1; 
else 
    sdd=sdd2; 
end 
 
% if abs(linkxielv*laichuanxielv+1)<0.1;  %(ind(i)==ind(i-2))&&i>2&& 
%     sdd=iniangel; 
% end     
     
end 
 
 
 
if ind(i)==laichuanshu 
    meet=6 
     if ((px1-px3)*(py2-py3)-(py1-py3)*(px2-px3))>0&&(dR^2*da^2 + dR^2 - da^2*dx0^2 - 2*da*db*dx0 + 2*da*dx0*dy0 - db^2 + 2*db*dy0 -dy0^2)<0&&panduan2>0 
        s=tan(rem(2*pi,sida3(hindex2(ind(i))))+adds); 
    else 
         s=tan(rem(2*pi,sida2(hindex2(ind(i))))+adds); 
    end 
     
    sdd1=asin((v + s*(V^2*s^2 + V^2 - s^2*u^2 + 2*s*u*v - v^2)^(1/2) - s*u)/(V*(s^2 + 1))); 
  sdd2=asin(-(s*(V^2*s^2 + V^2 - s^2*u^2 + 2*s*u*v - v^2)^(1/2) - v + s*u)/(V*(s^2 + 1))); 
  a4s=atan2(y00,x00); 
% if a4s<0 
%     a4s=2*pi+a4s; 
% end 
 
if abs(a4s-sdd1)<abs(a4s-sdd2) 
         sdd=sdd1; 
else 
    sdd=sdd2; 
end 
 
% if abs(linkxielv*laichuanxielv+1)<0.1;  %(ind(i)==ind(i-2))&&i>2&& 
%     sdd=iniangel; 
% end        
     
     
end 
%(a5s(j)>sida2(j))&&(a5s(j)<sida3(j))   
%       R=eval(['rad',num2str(j),';']); 
%       
%        
%       am(j)=eval(['x',num2str(j),';']); 
%       bm(j)=eval(['y',num2str(j),';']); 
   
%        
%        
%       R=eval(['rad',num2str(j),';']); 
%       
 
%  
% % x010=YYo(i,1); 
% % y010=YYo(i,2); 
%   
   
%     
%    %Diss(io-1)=sqrt((YYo((io-1),1)-xRo)^2+(YYo((io-1),2)-yRo)^2); 
 
%        
%    lacd; 
%   
% [hxu1,hindex1]=sort(lacd) 
% [hxu2,hindex2]=sort(sida1); 
%  
% for j=laichuanshu:-1:1 
%      
%      
% if  hindex2(j)==hindex1(1) 
%     ind=j; 
% end 
%  
% if  (a5s(ind)>sida2(ind))&&(a5s(ind)<sida3(ind)) 
%    cjdsd=sqrt(YY(i,5)^2+YY(i,6)^2); 
%      
%    for ki=1:314 
%        zxjd=YY(i,3)+ki*0.01; 
%    sx=eval(['((cjdsd*sin(zxjd)-b',num2str(ind),'2)/(cjdsd*cos(zxjd)-b',num2str(ind),'1)-tan(sida2(ind)','+0.4))',';' ]); 
%    if abs(sx)<0.01 
%       sdd=zxjd; 
%       break 
%    end 
%         
%      
%    end 
%  
%     
%     
%     
%  
% end 
% sdd     
%  
%  
% end        
%      
%      
%      
%   end  
              %ind(i) 
              %indd 
               
 if (indd~=0)&&(i>25) 
       if ((Dis(i-20,hindex2(indd))>Dis(i-10,hindex2(indd)))&&(Dis(i,hindex2(indd))>Dis(i-10,hindex2(indd))))||(Dis(i,hindex2(indd))>4000) 
         sdd=iniangel; 
     end 
%         linkxielv=eval(['(YY(i,6)','-y',num2str(indd),')/(YY(i,5)','-x',num2str(indd),');']) 
%         laichuanxielv=v/u 
%               abs(linkxielv*laichuanxielv+1) 
%                
%    if abs(linkxielv*laichuanxielv+1)<0.1;  %(ind(i)==ind(i-2))&&i>2&& 
%     sdd=iniangel; 
%    end 
    
   end 
               
   sdd  
  
%deltae=deltae+(Kp+Ti/2+Td)*(sdd-YY(i,3))+(Ti/2-Kp-2*Td)*(sdd-YY(i-1,3))+Td*(sdd-YY(i-2,3)); 
 
 
if i>2 
ui(i+1)=ui(i)+Kp*(1+buchang/Ti+Td/buchang)*(YY01(i)-sdd)-Kp*(1+2*Td/buchang)*(YY01(i-1)-sdd)+Kp*(Td/buchang)*(YY01(i-2)-sdd); 
end  
  
 
hold on 
end 
ind 
ind2 
% hindex2(ind) 
  figure  
plot(ui);   
figure  
plot(YY01); 
 
%hold on 
 
 
   
 
 
 
 
 
 
function xdot=mariner(x,ui) 
%M.S.Chislett and J, Stroem-Tejsen(1965) 
 
if ~(length(x)==7),error('x-vector must have dimension 7 !'); end 
if ~(length(ui)==1),error('u-vector must have dimension 1!'); end 
 
 
%Normalization variables 
U0=7.72; 
U=sqrt((U0+x(1)^2)+x(2)^2); 
L=160.93; 
%cruise speed UO =- 7.72m/s 15 knots 
%total speed Uin m/s 
%length of Ship in ID 
 
%Non-dimensional states and inputs 
delta_c=ui(1); 
u=x(1)/U; v=x(2)/U; delta=x(7); 
r=x(3)*L/U; psi=x(4); 
 
  
  
 
%Parameters,hydrodynamic derivatives and maindimensions 
 
delta_max=10; 
Ddelta_max=5; 
 
m=798e-5; Iz=39.2e-5; xG=-0.023; 
 
 
Xudot= -840e-5; Yvdot= -1546e-5; Nvdot=23e-5; 
Xu= -184e-5; Yrdot=9e-5; Nrdot=-83e-5; 
Xuu= -110e-5; Yv= -1160e-5; Nv= -264e-5; 
Xuuu= -215e-5; Yr= -499e-5; Nr=-166e-5; 
Xvv=-899e-5; Yvvv=-8078e-5;Nvvv=1636e-5; 
Xrr=18e-5; Yvvr=15356e-5; Nvvr=-5483e-5; 
Xdd=-95e-5; Yvu =-1160e-5; Nvu=-264e-5; 
Xudd=-190e-5; Yru=-499e-5; Nru=-166e-5; 
Xrv=798e-5; Yd =278e-5; Nd=-139e-5; 
Xvd=93e-5; Yddd = -90e-5; Nddd = 45e-5; 
Xuvd=93e-5; Yud=556e-5; Nud=-278e-5; 
Yuud=278e-5; Nuud=-139e-5; 
Yvdd=-4e-5;  Nvdd=13e-5; 
Yvvd=1190e-5; Nvvd=-489e-5; 
Y0=-4e-5; N0=3e-5; 
Y0u=-8e-5; N0u=6e-5; 
Y0uu=-4e-5; N0uu=3e-5; 
 
 
 
%Masses and moments of inertia 
m11=m-Xudot; m23= m*xG-Yrdot; m33=Iz-Nrdot; 
m22=m-Yvdot; m32=m*xG-Nvdot; 
 
% Rudder saturation and dynamics 
 
% delta_c=(delta_max*pi/180)*tanh(dj/(delta_max*pi/180)); 
 
 
 
 
if abs(delta_c) >= delta_max*pi/180 
delta_c=sign(delta_c)*delta_max*pi/180;  
end 
 delta_dot=(delta_c- delta); 
% delta=delta_c-(delta_c-delta)*exp(-1/4); 
if abs(delta_dot)>= Ddelta_max*pi/180 
delta_dot=sign(delta_dot)*Ddelta_max*pi/180;  
end 
 
%Forces and moments 
X=Xu*u+Xuu*u^2 + Xuuu*u^3+ Xvv*v^2+ Xrr*r^2+ Xrv*r*v+ Xdd*delta^2+... 
  Xudd*u*delta^2+ Xvd*v*delta+ Xuvd*u*v*delta; 
 
Y=Yv*v+ Yr*r+Yvvv*v^3+ Yvvr*v^2*r+ Yvu*v*u+ Yru*r*u+ Yd*delta+... 
  Yddd*delta^3+Yud*u*delta+ Yuud*u^2*delta+Yvdd*v*delta^2+... 
  Yvvd*v^2*delta+ (Y0+Y0u*u+Y0uu*u^2); 
N=Nv*v+ Nr*r+Nvvv*v^3+ Nvvr*v^2*r+ Nvu*v*u+ Nru*r*u+ Nd*delta+... 
Nddd*delta^3+ Nud*u*delta+ Nuud*u^2*delta+ Nvdd*v*delta^2+ ... 
Nvvd*v^2*delta+ (N0 +N0u*u+N0uu*u^2); 
%Dimensional state derivative 
xdot= [ X*(U^2/L)/m11 
-(-m33*Y+m23*N)/(m22*m33-m23*m32)*U^2/L 
 (-m32*Y+m22*N)/(m22*m33-m23*m32)*U^2/L^2 
  r*U/L 
(cos(psi)*(U0/U+u)-sin(psi)*v)*U 
(sin(psi)*(U0/U+u)+cos(psi)*v)*U 
     delta_dot     ];