www.pudn.com > GPSGJ.rar > Gfunction1.h

#include 
#include 
#define pi 3.1415926 
double *Gdcs(double xy[],int n)                     //¼ÆËã¹ìµÀ²ÎÊý 
{ 
	double A,B,C,h,r,v,i,domiga,a,e,E,xomiga,f,miu,i1,E1; 
    double *px=xy; 
	double x=*px,y=*(px+n-5),z=*(px+n-4),vx=*(px+n-3),vy=*(px+n-2),vz=*(px+n-1); 
    double canshu[6]; 
	miu=3.986005e14; 
    x=-3613946.664; 
    y=-8938344.367; 
    z=-773304.431; 
 
    vx=5230.262219; 
    vy=-1886.855649; 
    vz=-3159.828074; 
 //printf("please input x,y,z:\n"); 
 //scanf("%lf%lf%lf",&x,&y,&z); 
 //printf("please input vx,vy,vz:\n"); 
 //scanf("%lf%lf%lf",&vx,&vy,&vz); 
    r=sqrt(x*x+y*y+z*z); 
    v=sqrt(vx*vx+vy*vy+vz*vz); 
    A=y*vz-z*vy; 
    B=z*vx-x*vz; 
    C=x*vy-y*vx; 
    h=sqrt(A*A+B*B+C*C); 
    i1=atan(sqrt(A*A+B*B)/C); 
    i=atan(sqrt(A*A+B*B)/C)*180/pi; 
    domiga=atan(-A/B)*180/pi;  
    a=miu*r/(2*miu-v*v*r); 
    e=sqrt(1-h*h/miu/a); 
    E1=acos((a-r)/a/e); 
    E=E1*180/pi; 
    f=2*atan(sqrt((1+e)/(1-e))*tan(E1/2))*180/pi; 
    xomiga=atan(z/sin(i1)/(x*cos(domiga*pi/180)+y*sin(domiga*pi/180)))*180/pi-f; 
    xomiga=180+xomiga; 
	canshu[0]=i; 
	canshu[1]=a; 
	canshu[2]=e; 
	canshu[3]=domiga; 
	canshu[4]=xomiga; 
	canshu[5]=E; 
    return canshu; 
 //printf("i=%lf\ndomiga=%lf\na=%lf\ne=%lf\nE=%lf\nxomiga=%lf\n",i,domiga,a,e,E,xomiga); 
} 
double *Wzsd(double xy[],int n) 
{ 
	double miu,p1,p2,p3,q1,q2,q3,x,y,z,vx,vy,vz,i,O,o,E; 
    double *px=xy; 
    double ii=*px,a=*(px+n-5),e=*(px+n-4),domiga=*(px+n-3),xomiga=*(px+n-2),EE=*(px+n-1); 
	double weizhi[6]; 
	miu=3.986005e14; 
	printf("please input a,e,i,domiga,xomiga,E:\n"); 
	scanf("%lf%lf%lf%lf%lf%lf",&a,&e,&ii,&domiga,&xomiga,&EE); 
	i=ii/180.0*pi; 
	O=domiga/180.0*pi; 
	o=xomiga/180.0*pi; 
	E=EE/180.0*pi; 
   	p1=cos(o)*cos(O)-sin(o)*sin(O)*cos(i); 
	p2=cos(o)*sin(O)+sin(o)*cos(O)*cos(i); 
	p3=sin(o)*sin(i); 
	q1=-sin(o)*cos(O)-cos(o)*sin(O)*cos(i); 
	q2=-sin(o)*sin(O)+cos(o)*cos(O)*cos(i); 
	q3=cos(o)*sin(i); 
    x=a*(cos(E)-e)*p1+a*sqrt(1-e*e)*sin(E)*q1; 
	y=a*(cos(E)-e)*p2+a*sqrt(1-e*e)*sin(E)*q2; 
    z=a*(cos(E)-e)*p3+a*sqrt(1-e*e)*sin(E)*q3; 
	vx=-sin(E)*sqrt(miu/a)/(1-e*cos(E))*p1+sqrt(1-e*e)*cos(E)*sqrt(miu/a)/(1-e*cos(E))*q1; 
	vy=-sin(E)*sqrt(miu/a)/(1-e*cos(E))*p2+sqrt(1-e*e)*cos(E)*sqrt(miu/a)/(1-e*cos(E))*q2; 
	vz=-sin(E)*sqrt(miu/a)/(1-e*cos(E))*p3+sqrt(1-e*e)*cos(E)*sqrt(miu/a)/(1-e*cos(E))*q3; 
	weizhi[0]=x; 
	weizhi[1]=y; 
	weizhi[2]=z; 
	weizhi[3]=vx; 
	weizhi[4]=vy; 
	weizhi[5]=vz; 
	return weizhi; 
	//printf("r1=%lf\nr2=%lf\nr3=%lf\nv1=%lf\nv2=%lf\nv3=%lf\n",x,y,z,vx,vy,vz); 
}