//本程序求解某制導炸彈的未擾動彈道(控制其攻角始終為0)
#include"math.h"
#include"stdio.h"
#include"enlgr.c"//一元不等距插值子函數
#define rad 57.3//將“度”轉化為“弧度”
#define g0 9.81 //重力加速度取常數
//定義參數如下：
//彈體參數
double m=230.0;
double Jx=2.9698;
double Jy=51.255;
double Jz=51.255;
double S=0.07022;
double L=2.11;
//氣動參數
double Cx,Cx0,Cxalpha,Cxdelta;
double yMa[6]={0.4,0.6,0.8,0.9,1.0,1.1};
double yCx0[6]={0.1631,0.1748,0.1796,0.2035,0.3214,0.4672};
double yCxalpha[6]={6.073,4.745,9.395,8.931,9.072,8.617};
double yCxdelta[6]={1.192,0.913,1.220,0.911,1.168,0.690};
double Cy;
double Cyalpha=0.0798;
double Cydelta=0.03291;
double mz;
double mzalpha=-0.0147;
double mzdelta=-0.01845;
double mzw_=-1.83;
double mzdalpha_=-0.0175;
double Ma,wz_,dalpha_,dalpha,deltaz,alpha;
//其他
double rho,sonic,Hy;
double k=1.40;
double R1=287.0;
double G=6.0e-3;
double taoon=289.4;
double rhoon=1.220;
double Vmax,Vmin,Vmax_Y[9],Vmin_Y[9];
double yy[50];//用于存放H(y)-y表中的y值(主要用來算隨高度變化的空氣密度)
double yHy[50];
int int_yHy[5][10];//用于存放H(y)-y表中的H(y)值
double Y[7];//存放未知量
double Vmin_Hy,Vmin_Cx0,Vmin_Cxalpha,Vmin_Cxdelta,Vmin_sonic;
double Vmax_Hy,Vmax_Cx0,Vmax_Cxalpha,Vmax_Cxdelta,Vmax_sonic;
//讀取H(y)-y的值(Hy.txt文件必須存在于指定路徑下)
void Read_Hy()
{
	FILE *filedata=NULL;
	int i,j;
	if((filedata=fopen("c:\\Hy.txt","r"))==NULL)
	{
		printf("文件不存在，無法讀取!");
		exit(0);
	}
	for(i=0;i<5;i++)
		for(j=0;j<10;j++)
			fscanf(filedata,"%d",&int_yHy[i][j]);
	for(i=0;i<5;i++)
		for(j=0;j<10;j++)
			yHy[i*10+j]=int_yHy[i][j]/10000.0;
	for(i=0;i<50;i++)
        yy[i]=i*100;
	return;
}

//右端函數
void dery(n,dy,Y)
int n;
double dy[],Y[];
{
	alpha=Y[6]-Y[2];
	deltaz=-1.0*mzalpha*alpha/mzdelta;
	if(Y[5]<0.0)
	{
		Y[5]=0.0;
	}
	Hy=enlgr(yy,yHy,50,Y[5]);
	rho=rhoon*Hy;
	sonic=sqrt(k*R1*(taoon-G*Y[5]));
	Ma=Y[1]/sonic;
	Cx0=enlgr(yMa,yCx0,6,Ma);
	Cxalpha=enlgr(yMa,yCxalpha,6,Ma);
	Cxdelta=enlgr(yMa,yCxdelta,6,Ma);
	Cx=Cx0+Cxalpha*alpha*alpha/rad/rad+Cxdelta*deltaz*deltaz/rad/rad;///////
	Cy=Cyalpha*alpha+Cydelta*deltaz;/////////
	wz_=Y[3]*L/Y[1];
    dalpha_=(Y[3]-(0.5*rho*Y[1]*Y[1]*Cy*S-m*g0*cos(Y[2]))/(m*Y[1]))*L/Y[1];
	mz=mzalpha*alpha+mzdelta*deltaz+mzw_*wz_+mzdalpha_*dalpha_;////////
	dy[0]=1;//表示dt/dt=1
	dy[1]=(-0.5*rho*Y[1]*Y[1]*Cx*S-m*g0*sin(Y[2]))/m;
	dy[2]=(0.5*rho*Y[1]*Y[1]*Cy*S-m*g0*cos(Y[2]))/(m*Y[1]);
	dy[3]=(0.5*rho*Y[1]*Y[1]*mz*S*L)/Jz;
	dy[4]=Y[1]*cos(Y[2]);
	dy[5]=Y[1]*sin(Y[2]);
	dy[6]=Y[3];
	return;
}

//四階龍格庫塔子程序
void rk(n,h)
int n;
double h;
{
	extern void dery();
	double a[4],old_y[8],Y1[8],*dy;
	int i,j;
	dy=calloc(n,sizeof(double));
	a[0]=a[1]=h/2.0;
	a[2]=a[3]=h;
  	dery(n,dy,Y);
	for(i=0;i<n;i++)
		old_y[i]=Y[i];
	for(j=0;j<3;j++)
	{
		for(i=0;i<n;i++)
		{
			Y1[i]=old_y[i]+a[j]*dy[i];
			Y[i]=Y[i]+a[j+1]*dy[i]/3.0;
		}
		dery(n,dy,Y1);	
	}
	for(i=0;i<n;i++)
		Y[i]=Y[i]+a[0]*dy[i]/3.0;
	free(dy);
	return;
}

//主函數
main()
{
	double step;
	int nn,ii;
	FILE *ftxtfile=NULL;
	Read_Hy();
	if((ftxtfile=fopen("d:\\某制導炸彈未擾動彈道求解數據(段笑菊).txt","w"))==NULL)
      {
	printf("Can't open the file\n");
	exit(0);
      } 
	Vmax=0.0;
	Vmin=100000.0;
	step=0.0005;
	nn=7;
	Y[0]=0.0;    //時間t
	Y[1]=208.3;  //速度V
	Y[2]=-1.1/rad;   //彈道傾角θ
	Y[3]=0.0;  //繞z軸的角速度ωz
	Y[4]=0.0;    //射程X
	Y[5]=4000.0; //高度Y
	Y[6]=-1.1/rad;   //俯仰角Θ
	//Y[7]=0.0;    //攻角α
	fprintf(ftxtfile,"%f  %f  %f  %f  %f  %f  %f  %f  %f\n",Y[0],Y[1],Y[2],Y[3],Y[4],Y[5],Y[6],alpha,deltaz);
	rk(nn,step); 
	alpha=0.0;//取方案攻角α*為0
	fprintf(ftxtfile,"%f  %f  %f  %f  %f  %f  %f  %f  %f\n",Y[0],Y[1],Y[2],Y[3],Y[4],Y[5],Y[6],alpha,deltaz);
    for(ii=0;;ii++)
	//do
	{
	    rk(nn,step);
		alpha=0.0;//取方案攻角α*為0
		fprintf(ftxtfile,"%f  %f  %f  %f  %f  %f  %f  %f  %f\n",Y[0],Y[1],Y[2],Y[3],Y[4],Y[5],Y[6],alpha,deltaz);
	    printf("%f  %f  %f  %f  %f  %f  %f  %f  %f\n",Y[0],Y[1],Y[2],Y[3],Y[4],Y[5],Y[6],alpha,deltaz);
	    //rk(nn,step);
		//alpha=0.0;//取方案攻角α*為0
		if(Y[1]>=Vmax)
		{
			Vmax=Y[1];
			Vmax_Y[0]=Y[0];
			Vmax_Y[2]=Y[2];
            Vmax_Y[3]=Y[3];
			Vmax_Y[4]=Y[4];
			Vmax_Y[5]=Y[5];
			Vmax_Y[6]=Y[6];
			Vmax_Y[7]=alpha;
			Vmax_Y[8]=deltaz;
			Vmax_Hy=Hy;
			Vmax_Cx0=Cx0;
			Vmax_Cxalpha=Cxalpha;
			Vmax_Cxdelta=Cxdelta;
			Vmax_sonic=sonic;
		}
		if(Y[1]<=Vmin)
		{
			Vmin=Y[1];
			Vmin_Y[0]=Y[0];
			Vmin_Y[2]=Y[2];
            Vmin_Y[3]=Y[3];
			Vmin_Y[4]=Y[4];
			Vmin_Y[5]=Y[5];
			Vmin_Y[6]=Y[6];
			Vmin_Y[7]=alpha;
			Vmin_Y[8]=deltaz;
			Vmin_Hy=Hy;
			Vmin_Cx0=Cx0;
			Vmin_Cxalpha=Cxalpha;
			Vmin_Cxdelta=Cxdelta;
			Vmin_sonic=sonic;
		}
		if(Y[5]<=0.0)
		{
            break;
		}
	}
    //rk(nn,step*(-0.12));
//	Vmax=Y[1];
//	Vmax_Y[0]=Y[0];
//	Vmax_Y[2]=Y[2];
//  Vmax_Y[3]=Y[3];
//  Vmax_Y[4]=Y[4];
//  Vmax_Y[5]=Y[5];
//  Vmax_Y[6]=Y[6];
//	Vmax_Y[7]=alpha;
//	Vmax_Y[8]=deltaz;
//	Vmax_Hy=Hy;
//	Vmax_Cx0=Cx0;
//	Vmax_Cxalpha=Cxalpha;
//	Vmax_Cxdelta=Cxdelta;
//	Vmax_sonic=sonic;
	//while(Y[5]>0.0);
	
	printf("計算數據保存在D盤根目錄下!\n");
	printf("\n");
    printf("最大速度點時刻的彈道諸元：\n");
	printf("時間=%f  速度=%f  彈道傾角=%f  角速度=%f\n射程=%f  高度=%f  俯仰角=%f  攻角=%f  舵偏角=%f\n",Vmax_Y[0],Vmax,Vmax_Y[2],Vmax_Y[3],Vmax_Y[4],Vmax_Y[5],Vmax_Y[6],Vmax_Y[7],Vmax_Y[8]);
    printf("\n");
	//fprintf(ftxtfile,"最大速度點時刻的彈道諸元：\n");
	//fprintf(ftxtfile,"%f  %f  %f  %f  %f  %f  %f  %f  %f\n",Vmax_Y[0],Vmax,Vmax_Y[2],Vmax_Y[3],Vmax_Y[4],Vmax_Y[5],Vmax_Y[6],Vmax_Y[7],Vmax_Y[8]);
	printf("最小速度點時刻的彈道諸元：\n");
	printf("時間=%f  速度=%f  彈道傾角=%f  角速度=%f\n射程=%f  高度=%f  俯仰角=%f  攻角=%f  舵偏角=%f\n",Vmin_Y[0],Vmin,Vmin_Y[2],Vmin_Y[3],Vmin_Y[4],Vmin_Y[5],Vmin_Y[6],Vmin_Y[7],Vmin_Y[8]);
	//fprintf(ftxtfile,"最小速度點時刻的彈道諸元：\n");
	//fprintf(ftxtfile,"%f  %f  %f  %f  %f  %f  %f  %f  %f\n",Vmin_Y[0],Vmin,Vmin_Y[2],Vmin_Y[3],Vmin_Y[4],Vmin_Y[5],Vmin_Y[6],Vmin_Y[7],Vmin_Y[8]);
	printf("\n");
	printf("最大速度點時刻的輔助彈道參量：\n");
	printf("rho=%f  Cx0=%f  Cxa=%f\nCxdelta=%f  Cs=%f  Ma=%f\n",rhoon*Vmax_Hy,Vmax_Cx0,Vmax_Cxalpha,Vmax_Cxdelta,Vmax_sonic,Vmax/Vmax_sonic);
	printf("\n");
	printf("最小速度點時刻的輔助彈道參量：\n");
	printf("rho=%f  Cx0=%f  Cxa=%f\nCxdelta=%f  Cs=%f  Ma=%f\n",rhoon*Vmin_Hy,Vmin_Cx0,Vmin_Cxalpha,Vmin_Cxdelta,Vmin_sonic,Vmin/Vmin_sonic);
	printf("\n");
	fclose(ftxtfile);

}