/*****************************************************************
功能：軌跡發生器，采用北天東坐標系
******************************************************************/
#include?“math.h“
#include?“stdlib.h“
#include?“stdio.h“

#define?DYNUM		18
#define?PI			3.1415926535897932384626433832795
#define?wie			7.292115E-5
#define?Re			20925000.????
#define?esqu????????0.00669437999013
#define?g0			32.174????????????
#define?TIME????????90000
#define?step?????????0.01
void?dery（double?*ydouble?*Wnbbdouble?*accdouble?*dy）;
void?init_cnb（double?*angledouble?*cnb）;
void?indata（double?*y）;
void?outdata（double?*Wnbbdouble?*adouble?*y）;
void??chacheng（double?*p1double?*p2double?*p3）;
void?matrixmultiply（double?*p1double?*p2double?*p3int?iiint?kkint?jj）;
double?Wnbb[3]acc[3]cbn[9]cnb[9]Wibb[3]Aibb[3]aaa;
int?flag_motion=1flag_pitch=0time;


FILE?*tr*Velo;
main（）
{

	int?ij;
	static?int?IsInit=0flag_rk=1;
	static?double?y0[DYNUM]dy[DYNUM];
	double?y[DYNUM]yy[DYNUM]angle[3];
	double?iniv[3]v[3];
????double?stepl;//=0.01;
	time=0;
		???
	if（（tr=fopen（“trace.dat““w“））==NULL）
	{	puts（“CAN‘T?OPEN?trace.dat?to?save?trace.data“）;
		puts（“PROGRAM?IS?TERMINATED?!“）;
		exit（1）;
	}???
	if（（Velo=fopen（“velocity.dat““w“））==NULL）
	{	puts（“CAN‘T?OPEN?velocity.dat?to?save?velocity.data“）;
		puts（“PROGRAM?IS?TERMINATED?!“）;
		exit（1）;
	}???
????angle[0]=0./180.*PI;?????????????//pesi
????angle[1]=0./180.*PI;???????????????//sita
????angle[2]=0./180.*PI;???????????????//gama

????init_cnb（anglecnb）;
????for（i=0;i<3;i++）??for（j=0;j<3;j++）??cbn[i*3+j]=cnb[j*3+i];

????iniv[0]=0.;
????iniv[1]=0.0;
????iniv[2]=0.0;

????v[0]=cbn[0*3+0]*iniv[0]+cbn[0*3+1]*iniv[1]+cbn[0*3+2]*iniv[2];
????v[1]=cbn[1*3+0]*iniv[0]+cbn[1*3+1]*iniv[1]+cbn[1*3+2]*iniv[2];
????v[2]=cbn[2*3+0]*iniv[0]+cbn[2*3+1]*iniv[1]+cbn[2*3+2]*iniv[2];

????y[0]=39./180.*PI;???y[1]=116./180.*PI;???y[2]=0.0;?????????????
????y[3]=cnb[0*3+0];????y[4]=cnb[0*3+1];?????y[5]=cnb[0*3+2];
????y[6]=cnb[1*3+0];????y[7]=cnb[1*3+1];?????y[8]=cnb[1*3+2];
????y[9]=cnb[2*3+0];????y[10]=cnb[2*3+1];????y[11]=cnb[2*3+2];
????y[12]=v[0];?????????y[13]=v[1];??????????y[14]=v[2];????
????y[15]=angle[0];?????y[16]=angle[1];??????y[17]=angle[2];	?
?
??while（time??{
	if（!IsInit）
	{??
		stepl=step;	
		for（i=0;i		{??yy[i]=y[i];	
		???y0[i]=y[i];
???????????dy[i]=0.0;
		}
		indata（y）;
????????outdata（Wnbbaccy）;
????????fprintf（tr“%20.12lf“time*stepl）;
	????fprintf（tr“%20.12lf“Aibb[0]）;
		fprintf（tr“%20.12lf“Aibb[1]）;
		fprintf（tr“%20.12lf“Aibb[2]）;
	????fprintf（tr“%20.12lf“Wibb[0]）;
		fprintf（tr“%20.12lf“Wibb[1]）;
		fprintf（tr“%20.12lf“Wibb[2]）;
????????dery（yyWnbbaccdy）;
	????fprintf（tr“\n“）;
		for（i=0;i		flag_rk=2;
		IsInit=1;	
	}
????else?if（flag_rk==2）
	{
	??for（j=0;j<=1;j++）
		{??	for（i=0;i