#include <stdio.h>

#include <string.h>

#include <math.h>

#define v0 500.0? //分離時導(dǎo)彈初速度 單位:s

#define t0 3.0? ? //分離時刻,單位:s

//#define x0 674.0

//#define y0 329.0

#define alpha0 1.5*PI/180? ?//攻角α,單位rad

#define theta0 26.0*PI/180? //俯仰角θ,單位rad

#define is 2156.0? //總沖Is,單位:N*s/kg

#define g 9.801? ? //重力加速度g,單位:m/s^2

#define P 2.2? ? ? //推重比P

#define p0 5880.0? //翼載p0,單位:N/m^2

#define vt 420.0? ?//飛機(jī)速度vt,單位:m/s

#define yt0 15000.0? //飛機(jī)初始縱坐標(biāo)yt,單位:m

#define dt0 34200.0? //彈目斜距Dt0,單位:m

#define xt0 31569.39345 //飛機(jī)初始橫坐標(biāo)xt0,單位:m? ?xt0=sqrt(dt0^2-(yt0-y0)^2)+x0

#define cotq0 2.10462623//初始目標(biāo)對制導(dǎo)站的航向角的余切,cotq0=xt0/yt0;

#define PI 3.1415926535 //圓周率π

#define h 0.001? //步長

double inter_linear(double x0,double x1,double y0,double y1,double x)

{//線性插值函數(shù)

? ? double a0,a1,y;

? ? a0=(x-x1)/(x0-x1);

? ? a1=(x-x0)/(x1-x0);

? ? y=a0*y0+a1*y1;

? ? return y;

}

double interp1(double x[],double y[],int n,double a)

{//一維線性插值函數(shù)

? ? double z;

? ? int i=0;

? ? for(i=0;i<(n-1);i++)

? ? ? ? {

? ? ? ? if((a>=x[i])&&(a<=x[i+1])) break;//確定a的位置

? ? ? ? }

? ? z = inter_linear(x[i],x[i+1],y[i],y[i+1],a);

? ? return z;

}

double interp2(double x[],double y[],double z[6][6],int m,int n,double a,double b)

{//雙線性插值函數(shù)

? ? int i,j;

? ? double w,w1,w2;

? ? for(i=0; i<(m-1); i++)

? ? ? ? { //確定a在x軸的位置

? ? ? ? if( (a>=x[i])&&(a<=x[i+1]) ) break;

? ? ? ? }

? ? for(j=0; j<(n-1); j++)

? ? { //確定b在y軸的位置

? ? ? ? if( (b>=y[j])&&(b<=y[j+1]) ) break;

? ? }

? ? if (a<x[0]) i=0;

? ? else if (a>x[m-1]) i=m-1;

? ? if (b<y[0]) j=0;

? ? else if (b>y[n-1]) j=n-1;

? ? w1 = inter_linear(x[i],x[i+1],z[i][j],z[i+1][j],a); //在x軸方向進(jìn)行第一次插值

? ? w2 = inter_linear(x[i],x[i+1],z[i][j+1],z[i+1][j+1],a);//在x軸方向進(jìn)行第二次插值

? ? w = inter_linear(y[j],y[j+1],w1,w2,b);? ? ? ? ? ? ? ? //在y軸方向進(jìn)行插值

? ? return w;

}

int main()

{

? ? double cx, cya;//阻力系數(shù)Cx, 升力系數(shù)斜率Cyα

? ? double alpha1[5] = {2,4,6,8,10}, ma1[5] = {1.5,2.1,2.7,3.3,4.0};? ? ? //Cx插值表中的α向量,Ma向量

? ? double alpha2[6] = {1,2,4,6,8,10}, ma2[6] = {1.5,2.0,2.5,3.0,3.5,4.0};//Cyα插值表中的α向量,Ma向量

? ? double cx0[6][6] = {{0.0430,0.0511,0.0651,0.0847,0.1120},\

? ? ? ? ? ? ? ? ? ? ? ? {0.0360,0.0436,0.0558,0.0736,0.0973},\

? ? ? ? ? ? ? ? ? ? ? ? {0.0308,0.0372,0.0481,0.0641,0.0849},\

? ? ? ? ? ? ? ? ? ? ? ? {0.0265,0.0323,0.0419,0.0560,0.0746},\

? ? ? ? ? ? ? ? ? ? ? ? {0.0222,0.0272,0.0356,0.0478,0.0644}};? ? ? ? ? ? //Cx插值表

? ? double cy0[6][6] = {{0.0302,0.0304,0.0306,0.0309,0.0311,0.0313},\

? ? ? ? ? ? ? ? ? ? ? ? {0.0279,0.0280,0.0284,0.0286,0.0288,0.0290},\

? ? ? ? ? ? ? ? ? ? ? ? {0.0261,0.0264,0.0267,0.0269,0.0272,0.0274},\

? ? ? ? ? ? ? ? ? ? ? ? {0.0247,0.0248,0.0251,0.0254,0.0257,0.0259},\

? ? ? ? ? ? ? ? ? ? ? ? {0.0226,0.0227,0.0231,0.0233,0.0236,0.0238},\

? ? ? ? ? ? ? ? ? ? ? ? {0.0209,0.0210,0.0213,0.0216,0.0219,0.0221}};? ? ?//Cyα插值表

? ? double yh[23] = {0,1000,2000,3000,4000,5000,6000,7000,8000,\

? ? ? ? ? ? ? ? ? ? 9000,10000,11000,12000,13000,14000,15000,\

? ? ? ? ? ? ? ? ? ? 16000,17000,18000,19000,20000,21000,22000};? ? ? ? ? //大氣高度y(h)

? ? double ph[23] = {1.22505,1.11168,1.00646,0.90913,0.81913,0.73612,0.65969,\

? ? ? ? ? ? ? ? ? ? ?0.58950,0.52517,0.46635,0.41270,0.36391,0.31083,0.26549,\

? ? ? ? ? ? ? ? ? ? ?0.22675,0.19367,0.16542,0.14128,0.12068,0.10307,0.08803,\

? ? ? ? ? ? ? ? ? ? ?0.07487,0.06373};? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ?//大氣密度表ρ(h)

? ? double ah[23] = {340.29,336.43,332.53,328.58,324.58,320.53,316.43,312.27,\

? ? ? ? ? ? ? ? ? ? ?308.06,303.79,299.46,295.07,295.07,295.07,295.07,295.07,\

? ? ? ? ? ? ? ? ? ? ?295.07,295.07,295.07,295.07,295.07,295.75,296.43};? //大氣聲速表a(h)

? ? int i;

? ? double p, a, q, cotq;

? ? double x=674.0, y=329.0, v=v0, t=t0, u=0, alpha=alpha0, theta=theta0, ma, dr;

? ? double x_2, y_2, v_2, theta_2, alpha_2;

? ? double xt=xt0;

? ? double Kv1, Kv2, Kv3, Kv4;

? ? double Kth1, Kth2, Kth3, Kth4;

? ? char str[500];

? ? FILE *fp;

? ? fp=fopen("output.txt","w");

? ? printf("? ?μ? ? ? ? ? ?t? ? ? ? ? ?x? ? ? ? ? ?y? ? ? ? ? ? v? ? ? ? ? ? ?θ? ? ? ? ? ?α? ? ? ? ? ?q? ? ? ? ? xt? ? ? ? ? ? ?Dr? ? ? ? ? ? Ma\n");

? ? sprintf(str,"? ?μ? ? ? ? ? ?t? ? ? ? ? ?x? ? ? ? ? ?y? ? ? ? ? ? v? ? ? ? ? ? ?θ? ? ? ? ? ?α? ? ? ? ? ?q? ? ? ? ? xt? ? ? ? ? ? ?Dr? ? ? ? ? ? Ma\n");

? ? fputs(str,fp);

? ? for(i=1;y<=yt0;i++){

? ? ? ? p = interp1(yh,ph,23,y);

? ? ? ? a = interp1(yh,ah,23,y);

? ? ? ? ma = v/a;

? ? ? ? cx = interp2(ma1,alpha1,cx0,5,5,ma,alpha*180/PI); //當(dāng)前Ma,α下的Cx

? ? ? ? cya = interp2(ma2,alpha2,cy0,6,6,ma,alpha*180/PI);//當(dāng)前Ma,α下的Cyα

? ? ? ? Kv1 = is/(1-u)-p*v*v*cx*is/2/P/p0/(1-u)-is/P*sin(theta);

? ? ? ? Kv2 = is/(1-(u+h/2))-p*(v+h*Kv1/2)*(v+h*Kv1/2)*cx*is/2/P/p0/(1-(u+h/2))-is/P*sin(theta);

? ? ? ? Kv3 = is/(1-(u+h/2))-p*(v+h*Kv2/2)*(v+h*Kv2/2)*cx*is/2/P/p0/(1-(u+h/2))-is/P*sin(theta);

? ? ? ? Kv4 = is/(1-(u+h))-p*(v+h*Kv3)*(v+h*Kv3)*cx*is/2/P/p0/(1-(u+h))-is/P*sin(theta);

? ? ? ? v_2 = v + h*(Kv1+2*Kv2+2*Kv3+Kv4)/6;? ? ? ? ? ? ?//榮格庫塔法算方程1的v(n+1)

? ? ? ? y_2 = y + h*is*v*sin(theta)/P/g;? ? ? ? ? ? ? ? ?//用方程3算y(n+1)

? ? ? ? x_2 = x + h*is*v*cos(theta)/P/g;? ? ? ? ? ? ? ? ?//利用方程4算x(n+1)

? ? ? ? Kth1 = vt/yt0*(is/P/g+(is*v*v*sin(theta)/P/g-\

? ? ? ? ? ? ? ?y*Kv1)/v/v/sin(theta))/\

? ? ? ? ? ? ? ?(1+(cotq0-u*is*vt/P/g/yt0)/tan(theta));

? ? ? ? Kth2 = vt/yt0*(is/P/g+(is*v*v*sin(theta+h*Kth1/2)/P/g-\

? ? ? ? ? ? ? ?y*Kv1)/v/v/sin(theta+h*Kth1/2))/\

? ? ? ? ? ? ? ?(1+(cotq0-(u+h/2)*is*vt/P/g/yt0)/tan(theta+h*Kth1/2));

? ? ? ? Kth3 = vt/yt0*(is/P/g+(is*v*v*sin(theta+h*Kth2/2)/P/g-\

? ? ? ? ? ? ? ?y*Kv1)/v/v/sin(theta+h*Kth2/2))/\

? ? ? ? ? ? ? ?(1+(cotq0-(u+h/2)*is*vt/P/g/yt0)/tan(theta+h*Kth2/2));

? ? ? ? Kth4 = vt/yt0*(is/P/g+(is*v*v*sin(theta+h*Kth3)/P/g-\

? ? ? ? ? ? ? ?y*Kv1)/v/v/sin(theta+h*Kth3))/\

? ? ? ? ? ? ? ?(1+(cotq0-(u+h)*is*vt/P/g/yt0)/tan(theta+h*Kth3));

? ? ? ? theta_2 = theta + h*(Kth1+2*Kth2+2*Kth3+Kth4)/6;//容格庫塔法算方程2的θ(n+1)

? ? ? ? alpha_2 = (v*Kth1+is*cos(theta)/P)/(57.3*p*v*v*cya*is/2/P/p0/(1-u)+is/(1-u));//利用方程5算α

? ? ? ? cotq = cotq0-u*is*vt/P/g/yt0;? ? ? ? ? ? ? ? ? ?//利用方程6算cotq

? ? ? ? q = atan(1/cotq);? ? ? ? ? ? ? ? ? ? ? ? ? ? ? //算q,單位:rad

? ? ? ? xt = cotq*yt0;? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? //幾何關(guān)系cotq=xt/yt

? ? ? ? dr = sqrt((x-xt)*(x-xt)+(y-yt0)*(y-yt0));? ? ? ?//利用方程7算Dr

? ? ? ? t = t0+u*is/P/g;? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? //利用式(3-22)算時間

? ? ? ? y = y_2;

? ? ? ? x = x_2;

? ? ? ? v = v_2;

? ? ? ? theta = theta_2;

? ? ? ? alpha = alpha_2;

? ? ? ? u = u + h;

? ? ? ? printf("%lf? ?%2.6lf? ?%5.6lf? ?%5.6lf? ?%3.6lf? ?%2.6lf°? %2.6lf°? %lf°? %5.6lf? ?%5.6lf? ?%lf\n",

? ? ? ? ? ? ? ?u,t,x,y,v,theta*180/PI,alpha*180/PI,q*180/PI,xt,dr,ma);

? ? ? ? if(fp!=NULL){

? ? ? ? ? ? sprintf(str,"%.3lf? ?%2.3lf? ?%5.3lf? ?%5.3lf? ?%3.3lf? ?%2.3lf°? %2.3lf°? %.3lf°? %5.3lf? ?%5.3lf? ?%.3lf\n",

? ? ? ? ? ? ? ?u,t,x,y,v,theta*180/PI,alpha*180/PI,q*180/PI,xt,dr,ma);

? ? ? ? ? ? fputs(str,fp);

? ? ? ? }

? ? }

? ? fclose(fp);

? ? return 0;

}