%攻擊水平機動目標比例導引三維彈道仿真
clear;
dt=0.01;
vm=1000;
vt=600;
v=vt;
g=9.81;
sm=vm*dt;
st=vt*dt;
x(1)=0;y(1)=0;z(1)=0;
pmr(:,1)=[x(1);y(1);z(1)];
ae=pi/180;
%r0=100;h0=80;b0=10*ae;e0=asin(h0/r0);
%x0=r0*cos(e0)*sin(b0);y0=r0*cos(e0)*cos(b0);z0=h0;dt=0.001;ac=500;
ac=-100;
x0=1000;y0=1000;z0=1000;
h0=z0;
r0=sqrt(x0^2+z0^2+y0^2);
b0=atan(y0/x0);
e0=asin(h0/r0);
ptr(:,1)=[x0;y0;z0];
m=1;
q(1)=0;
o(1)=0;
a(1)=0;
time=0;
for(k=2:2000)
   %  if(k>=200)              %設置目標機動
     %   ac=200;
     % end
     time=time+dt;
    if(time>=2)
    ac=50*g*sign(sin(pi*(time-2))/3);      %目標作開關機動每3秒一次
    end
    zu=v^2/ac;
    db=ac*dt/v;
    bm=b0+db;
    xm=x0+2*zu*sin(db/2)*cos(db/2+b0);
    ym=y0+2*zu*sin(db/2)*sin(db/2+b0);
    zm=h0;
    b0=bm;
    x0=xm;
    y0=ym;
    ptr(:,k)=[xm;ym;zm];
    r(k-1)=sqrt((ptr(1,k-1)-pmr(1,k-1))^2+(ptr(2,k-1)-pmr(2,k-1))^2+(ptr(3,k-1)-pmr(3,k-1))^2);
    c=sqrt((ptr(1,k)-pmr(1,k-1))^2+(ptr(2,k)-pmr(2,k-1))^2+(ptr(3,k)-pmr(3,k-1))^2);
    b=acos((r(k-1)^2+st^2-c^2)/(2*r(k-1)*st));

    dq=acos((r(k-1)^2-st^2+c^2)/(2*r(k-1)*c));
    if abs(imag(b))>0
        b=0.0000001;
    end
    if abs(imag(dq))>0
        dq=0.0000001;
    end
    q(k)=q(k-1)+dq;
    o(k)=o(k-1)+m*(dq);
    a(k)=o(k)-q(k-1);

    c1=r(k-1)*sin(b)/sin(a(k)+b);
    c2=r(k-1)*sin(a(k))/sin(a(k)+b);
    c3=sqrt((c1-sm)^2+(c2-st)^2+2*(c1-sm)*(c2-st)*cos(a(k)+b));
    dq=a(k)-acos(((c1-sm)^2+c3^2-(c2-st)^2)/(2*(c1-sm)*c3));
    if abs(imag(dq))>0
        dq=0.0000001;
    end
    q(k)=q(k-1)+dq;
    o(k)=o(k-1)+m*(dq);
    a(k)=o(k)-q(k-1);
    c1=r(k-1)*sin(b)/sin(a(k)+b);
    c2=r(k-1)*sin(a(k))/sin(a(k)+b);
    c3=sqrt((c1-sm)^2+(c2-st)^2+2*(c1-sm)*(c2-st)*cos(a(k)+b));
    x1(k)=ptr(1,k-1)+c2/st*(ptr(1,k)-ptr(1,k-1));
    y1(k)=ptr(2,k-1)+c2/st*(ptr(2,k)-ptr(2,k-1));
    z1(k)=ptr(3,k-1)+c2/st*(ptr(3,k)-ptr(3,k-1));
    x(k)=pmr(1,k-1)+sm/c1*(x1(k)-pmr(1,k-1));
    y(k)=pmr(2,k-1)+sm/c1*(y1(k)-pmr(2,k-1));
    z(k)=pmr(3,k-1)+sm/c1*(z1(k)-pmr(3,k-1));
    pmr(:,k)=[x(k);y(k);z(k)];
    r(k)=sqrt((ptr(1,k)-pmr(1,k))^2+(ptr(2,k)-pmr(2,k))^2+(ptr(3,k)-pmr(3,k))^2);
    if r(k)<0.9;break;end;
end;
sprintf('遭遇時間:%3.1f',time),

figure(1);
plot3(pmr(1,:),pmr(2,:),pmr(3,:),'k',ptr(1,:),ptr(2,:),ptr(3,:));
%text(x(80),y(80),z(80),'\leftarrow 比例導引');
%axis([0 100 0 100 0 100]);
grid on