发射系导弹九参数计算.cpp

发射系下飞行器九参数计算

#include<iostream.h>
#include<stdio.h>
#include<math.h>
#define RE 6371110                                      /*地球半径*/
#define JD 67.6388-180                                  /*发射点经度*/
#define WD 34.49146123                                  /*发射点纬度*/   
#define FWJ 20.62648062                                 /*发射点方位角*/   

double x0[3]={-2319753.913,4472928.465,4909187.872},v0[3]={-1197.860,-2806.348,5566.481};             /*地心系中导弹位置，地心系中导弹速度*/   
double g[3][3],be[3][3],gen[3][3],bg[3][3];                           /*转换矩阵，地心系M1M2M3，转换矩阵的逆，发射系M1M2M3*/
double x[3],v[3],z[3];                                                /*发射系位置，发射系速度，发射系姿态*/
double a=-40.797,b=65.383,d=132.611;                                  /*地心系姿态*/                                  
int i=1,j=0;

void count_g()                                          /*计算转换矩阵*/
{g[0][0]=0.0-sin(FWJ)*sin(JD)-cos(FWJ)*sin(WD)*cos(JD);
 g[0][1]=sin(FWJ)*cos(JD)-cos(FWJ)*sin(WD)*sin(JD);
 g[0][2]=cos(FWJ)*cos(WD);
 g[1][0]=cos(WD)*cos(JD);
 g[1][1]=cos(WD)*sin(JD);
 g[1][2]=sin(WD);
 g[2][0]=0.0-cos(WD)*sin(JD)+sin(FWJ)*sin(WD)*cos(JD);
 g[2][1]=cos(FWJ)*cos(JD)+sin(FWJ)*sin(WD)*sin(JD);
 g[2][2]=0.0-sin(FWJ)*cos(WD);
 return;
}

void count_gen()                                        /*计算转换矩阵的逆*/
{double p,q,w;
 p=g[0][0]*g[1][1]*g[2][2]+g[1][0]*g[2][1]*g[0][2]+g[2][0]*g[0][1]*g[1][2];
 q=g[2][0]*g[1][1]*g[0][2]+g[1][0]*g[0][1]*g[2][2]+g[0][0]*g[2][1]*g[1][2];
 w=p-q;
 gen[0][0]=0+(g[1][1]*g[2][2]-g[2][1]*g[1][2])/w;
 gen[0][1]=0-(g[1][0]*g[2][2]-g[2][0]*g[1][2])/w;
 gen[0][2]=0+(g[1][0]*g[2][1]-g[2][0]*g[1][1])/w;
 gen[1][0]=0-(g[0][1]*g[2][2]-g[2][1]*g[0][2])/w;
 gen[1][1]=0+(g[0][0]*g[2][2]-g[2][0]*g[0][2])/w;
 gen[1][2]=0-(g[0][0]*g[2][1]-g[2][0]*g[0][1])/w;
 gen[2][0]=0+(g[0][1]*g[1][2]-g[1][1]*g[0][2])/w;
 gen[2][1]=0-(g[0][0]*g[1][2]-g[1][0]*g[0][2])/w;
 gen[2][2]=0+(g[0][0]*g[1][1]-g[1][0]*g[0][1])/w;
 return;
}

void count_wz()                                         /*计算发射系中的位置*/
{for(i=0;i<3;i++)
    {x[i]=g[i][0]*x0[0]+g[i][1]*x0[1]+g[i][2]*x0[2];
    }
 x[1]=x[1]-RE;
 return;
}

void count_sd()                                         /*计算发射系中的速度*/
{for(i=0;i<3;i++)
    {v[i]=g[i][0]*v0[0]+g[i][1]*v0[1]+g[i][2]*v0[2];
    }
 return;
}

void count_zt()                                         /*计算发射系中的姿态*/
{be[0][0]=cos(a)*cos(b);
 be[0][1]=sin(a)*cos(b);
 be[0][2]=0-sin(b);
 be[1][0]=cos(a)*sin(b)*sin(d)-sin(a)*cos(d);
 be[1][1]=sin(a)*sin(b)*sin(d)+cos(a)*cos(d);
 be[1][2]=cos(b)*sin(d);
 be[2][0]=cos(a)*sin(b)*cos(d)+sin(a)*sin(d);
 be[2][1]=sin(a)*sin(b)*cos(d)-cos(a)*sin(d);
 be[2][1]=cos(b)*cos(d);

 count_gen();                                           /*调用求逆子函数*/

 for(i=0;i<3;i++)                                       /*计算地心系M1M2M3*/
    {for(j=0;j<3;j++)
        {bg[i][j]=be[i][0]*gen[0][j]+be[i][1]*gen[1][j]+be[i][2]*gen[2][j];
        }
    }

 z[1]=asin(0-bg[0][2]);                                 /*计算姿态*/
 z[0]=acos((bg[0][0])/cos(z[1]));
 z[2]=atan((bg[1][2])/(bg[2][2]));

return;
}

main()                                                  /*主函数*/
{int c;
 count_g();
 while(i!=0)                                            /*循环，i=0则退出*/
 {cout<<"\n\n\nSelect please!"
      <<"\n1.   count WEIZHI"
      <<"\n2.   count SHUDU"
      <<"\n3.   count ZITAI"
      <<"\n4.   EXIT"
      <<"\nInput your choice with the number:\n";
   cin>>c;                                              /*输入选择*/
   if(c==1)                                             /*选择1，调用子函数计算位置*/
           {count_wz();
            cout<<"\nWEIZHI:\n   Xg="<<x[0]
                <<"\n   Yg="<<x[1]<<"\n   Zg="<<x[2];
           }
   if(c==2)                                             /*选择2，调用子函数计算速度*/
           {count_sd();
            cout<<"\nSHUDU:\n   Vgx="<<v[0]
                <<"\n   Vgy="<<v[1]<<"\n   Vgz="<<v[2];
           }
   if(c==3)                                             /*选择3，调用子函数计算姿态*/
           {count_zt();
            cout<<"\nZHITAI:\n   J1="<<z[0]
                <<"\n   J2="<<z[1]<<"\n   J3="<<z[2];
           }
   if(c==4)i=0;                                         /*选择4，退出*/
 }
}