轨道机动算法的C++实现

主要由类orbitAnimation实现：

头文件：orbitAnimation.h

#pragma once
#include "stdafx.h"
#include "polySolver.h"

class orbitAnimation
{
public:
orbitAnimation();
~orbitAnimation(void);
public:
double kepler_E(double e,double M);
double kepler_H(double e,double H);
double stumpS(double z);
double stumpC(double z);
double kepler_U(double dt,double r0,double vr0,double alpha);//全局kepler方程

void f_and_g(double x, double t,double ro, double a,double *f,double *g);//
void f_and_g_dot(double x, double r, double ro,double a,double *fdot,double *gdot);//

void rv_from_rovo(osg::Vec3d R0,osg::Vec3d V0,double t,osg::Vec3d *R,osg::Vec3d *V);//由初始的位置和速度，求出过时间t后的位置与状态
void coe_from_sv(osg::Vec3d R,osg::Vec3d V,double *_h,double*_e,double *_RA,double* _incl,double* _w,double* _TA,double* _a);//由当前状态得到轨道根数
void sv_from_coe(double h,double e,double RA,double incl,double w,double TA,osg::Vec3d *r,osg::Vec3d *v);//由轨道根数得到当前状态参数
bool gibbs(osg::Vec3d R1,osg::Vec3d R2,osg::Vec3d R3,osg::Vec3d *V2);//吉伯斯问题求解，三个矢量求出轨道参数

//以下三个函数为兰伯特问题求解应用
double func_y(double z,double r1,double r2,double A);
double func_F(double z,double t,double r1,double r2,double A);
double func_dFdz(double z,double r1,double r2,double A);//
bool lambert(osg::Vec3d R1,osg::Vec3d R2,double t,bool clockwise,osg::Vec3d *V1,osg::Vec3d *V2);//兰伯特问题求解：两个矢量和之间的时间，求出轨道参数

double theta_from_F(double,double);//由双曲线的平近点角转换为真近点角
double E_from_theta(double,double);//由椭圆真近点角求出偏近点角
double E_to_theta(double e,double E);//由椭圆的偏近点角转换为真近点角

osg::Vec3d Geocentic_to_near(osg::Vec3d rX,double omiga,double incl,double w);//由地心赤道坐标系转换为近焦点坐标系
osg::Vec3d Geocentic_from_near(osg::Vec3d rX,double omiga,double i,double w);//由近焦点坐标系转换为地心赤道坐标系
double omiga_dot(double,double,double);//地球扁率对轨道赤经的影响，返回赤经的变化率
double w_dot(double,double,double);//地球扁率对轨道近地点辐角的影响，返回近地点辐角的变化率
osg::Vec3d nearR_reprent(double e,double theta);//已知当前的真近地点角，求出R在近焦点坐标系中的表示
osg::Vec3d nearV_reprent(double e,double theta);//已知当前的真近地点角，求出V在近焦点坐标系中的表示

double getJulianDate( int year, int month, int date );//计算一个日期的世界0时儒略日另一种方法
double caculateJ0(double year,double month,double day);//计算一个日期的世界0时儒略日
double caculateJD(double year,double month,double day,double hour,double minute,double second);//计算一个日期的的儒略日
double thetaG0(double year,double month,double day);//计算格林威治世界0时的恒星时，单位为度
double thetaG_(double year,double month,double day,double hour,double minute,double second);
double theta_(double year,double month,double day,double hour,double minute,double second,double lon_degree,double lon_min,double lon_sec);//计算任意世界时（ＵＴ）的格林威治恒星时

//坐标系转换
//测站坐标系：参数纬度phi,恒星时theta,距地面高度是H,则求出位置矢量XYZ(IJK)
osg::Vec3d phiAndTheta_to_IJK(double phi,double theta,double height);
void IJK_to_phiAndTheta(osg::Vec3d R,double *phi,double *theta, double *height);
//测站赤道坐标系：参数地面上一观测点，赤经alpha,赤纬delta,距离rou
void r_thetaG_phi_WL_to_alpha_delta(osg::Vec3d r,double thetaG,double phi,double WL,double *alpha,double* delta);
osg::Vec3d alpha_delta_to_IJK(double alpha,double delta);//由赤经赤纬求出观测单位矢量
//测站地平坐标系: A方位角 a高低角
osg::Vec3d Geocentic_to_Aa(double phi,double theta,osg::Vec3d R);//地心坐标系转换为测站地平坐标系，A方位角，a 高低角
osg::Vec3d Geocentic_from_Aa(double phi,double theta,double A,double a);//测站地平坐标系转换为地心坐标系，A方位角，a 高低角
//由斜距，角位置及其变化率由轨道状态量
void rv_from_observer(double rho,double rhodot,double A,double Adot,double a,double adot,double theta,double phi,double H,osg::Vec3d * R,osg::Vec3d *V);
void gauss(osg::Vec3d Rho1,osg::Vec3d Rho2,osg::Vec3d Rho3,osg::Vec3d R1,osg::Vec3d R2,osg::Vec3d R3,double t1,double t2,double t3,osg::Vec3d *R,osg::Vec3d *V);
//获得太阳的位置
osg::Vec3d getSunPosition(int year, int month, int date, double hoursUTC );
//判断当前轨道是不是活动轨道
bool isOrbitActive(int i);
double theta_from_t(double t);//由时间求出真近点角
void position_caculate();
private:
//6个轨道根数
double e;bool b_e;//偏心率
double h;bool b_h;//角动量
double incl;//轨道倾角
double omiga;//
double w;
double TA;//真近点角

double a;bool b_a;//半长轴
double alpha;bool b_alpha;//半长轴倒数
double T;bool b_T;//周期
int style;bool b_style;//轨道类型，0圆，1椭圆，2抛物线，3双曲线

double Me;bool b_Me;//如果是椭圆，平近点角
double Mh;bool b_Mh;//如果是双曲线，平近点角
double kafi;bool b_kafi;//全局近点角
double E;bool b_E;//如果是椭圆，偏近点角
double F;bool b_F;//如果是双曲线圆，偏近点角
double z;bool b_z;//alpha*kafi^2
double rp;bool b_rp;//近地点距离
double ra;bool b_ra;//远地点距离
//瞬时变量
osg::Vec3d *R;
osg::Vec3d *V;
double omigaDot;
double wDot;
osg::DoubleArray *current_time;
//控制变量
bool if_drawCircle;
//求解多項式
polySolver *solver;

double elon;
double elat;//星下点坐标

bool set_ra(double);
bool set_rp(double);

bool set_e(double);
bool set_a(double);
bool set_h(double);
bool set_omiga(double);
bool set_w(double);
bool set_incl(double);

bool set_T(double);
bool set_E(double);
bool set_Me(double);
bool set_style(int);
bool set_F(double);
bool set_Mh(double);
bool set_alpha(double);
bool set_TA(double);
void set_CurrentTime(osg::DoubleArray *);
bool set_R(osg::Vec3d* temp);
bool set_V(osg::Vec3d* temp);

bool set_omiga_dot(double);
bool set_w_dot(double);
bool set_drawCircle(bool);//例子用于运算，不涉及轨道绘制
};

源文件：

#include "stdafx.h"
#include "orbitAnimation.h"

orbitAnimation::orbitAnimation()
{
if_drawCircle = true;
R = new osg::Vec3d();
V = new osg::Vec3d();
incl = e = a = T = elon = elat = rp = ra = omiga = w = h = TA = 0.0;
current_time = NULL;
}
orbitAnimation::~orbitAnimation(void)
{
delete[] R;
delete[] V;
}
bool orbitAnimation::set_ra(double p_ra)
{
if(p_ra==0) return false;
ra = p_ra;
b_ra = 1;
return 1;
}
bool orbitAnimation::set_rp(double p_rp)
{
if(p_rp==0) return false;
rp = p_rp;
b_rp = 1;
return 1;
}

bool orbitAnimation::set_e(double temp)
{
if(temp<0) return false;
e = temp;
b_e = 1;
return 1;
}
bool orbitAnimation::set_h(double temp)
{
if(temp==0) return false;
h = temp;
b_h = 1;
return 1;
}
bool orbitAnimation::set_T(double temp)
{
if(temp==0) return false;
T = temp;
b_T = 1;
return 1;
}
bool orbitAnimation::set_E(double temp)
{
if(temp==0) return false;
E = temp;
b_E = 1;
return 1;
}
bool orbitAnimation::set_Me(double temp)
{
if(temp==0) return false;
Me = temp;
b_Me = 1;
return 1;
}
bool orbitAnimation::set_style(int temp)
{
if(temp<0) return false;
style = temp;
b_style = 1;
return 1;
}
bool orbitAnimation::set_F(double temp)
{
if(temp==0) return false;
F = temp;
b_F = 1;
return 1;
}
bool orbitAnimation::set_Mh(double temp)
{
if(temp==0) return false;
Mh = temp;
b_Mh = 1;
return 1;
}
bool orbitAnimation::set_alpha(double temp)
{
if(temp==0) return false;
alpha = temp;
b_alpha = 1;
return 1;
}
bool orbitAnimation::set_omiga(double temp)//升交点赤经
{
omiga = temp;
return 1;
}
bool orbitAnimation::set_w(double temp)
{
w = temp;
return 1;
}
bool orbitAnimation::set_incl(double temp)
{
incl = temp;
return 1;
}
bool orbitAnimation::set_TA(double temp)
{
TA = temp;
return 1;
}
bool orbitAnimation::set_drawCircle(bool temp)
{
if_drawCircle = temp;
return 1;
}
//已知某一个位置，真近点角已知，由距离，速度的大小，求偏心率
double orbitAnimation::theta_from_F(double F,double e)
{
return 2* atan(sqrt((e+1)/(e-1))*tanh(0.5*F));
}
bool orbitAnimation::set_R(osg::Vec3d *temp)
{
R = temp;
return 1;
}
bool orbitAnimation::set_V(osg::Vec3d *temp)
{
V = temp;
return 1;
}
bool orbitAnimation::set_a(double temp)
{
a = temp;
return 1;
}
bool orbitAnimation::set_omiga_dot(double temp)
{
omigaDot = temp;
return 1;
}
bool orbitAnimation::set_w_dot(double temp)
{
wDot = temp;
return 1;
}
/**椭圆轨道的开普勒方程: Me=E-e*sinE
Me是平近点角；E是偏近点角
由位置求时刻的方法：先由真近近点角theta,求出偏近点角E，由E求出平近点角Me,由Me求出t.
由时刻求位置的方法：由t求出Me,由Me应用计算机求出E,由E求出theta.*/
double orbitAnimation::kepler_E(double e,double M)
{
double E;
if( M<osg::PI )
E = M + e/2;
else
E = M - e/2;
double ratio = 1.0;
while( abs(ratio) >error)
{
ratio = (E-e*sin(E)-M)/(1-e*cos(E));
E = E-ratio;
}
return E;
}
//双曲线开普勒方程
double orbitAnimation::kepler_H(double e,double H)
{
double F;
F = H;
double ratio = 1.0;
while( abs(ratio) >error)
{
ratio = (e*sinh(F)-F-H)/(e*cosh(F)-1);
F = F-ratio;
}
return F;
}
//斯达姆夫函数S(z)
double orbitAnimation::stumpS(double z)
{
double S = 0.0;
if(z>0)
S = (sqrt(z) - sin( sqrt(z))) / pow( (sqrt(z)), 3);
else if( z<0 )
S = (sinh( sqrt(-z)) - sqrt(-z)) / pow( sqrt(-z), 3);
else
S = 1/6;
return S;
}
//斯达姆夫函数C(z)
double orbitAnimation::stumpC(double z)
{
double C = 0.0;
if(z>0)
C = ( 1 - cos( sqrt(z))) / z;
else if( z<0 )
C = (cosh( sqrt(-z)) - 1 ) / (-z);
else
C = 1/2;
return C;
}
/**用牛顿法角全局开普勒方程
//参数：dt是时间的增量，r0是t=t0时刻的向径，vr0是t=t0时的径向速度，alpha是长半轴的倒数*/
double orbitAnimation::kepler_U(double dt,double r0,double vr0,double a)
{
int nMax = 1000;//选代最大次数
int n = 0;//当前的选代次数
double ratio = 1.0;//初始比率
double C;//斯达姆夫函数C(z)的当前值
double S;//斯达姆夫函数S(z)的当前值
double F;//构造的全局变量函数f(x)值
double dFdx;//f(x)导数的当前值
double x = sqrt(mu) * abs(a) * dt;//全局近点角
while( (abs(ratio)>error) && (n<=nMax) )
{
n++;
C = stumpC(a*x*x);
S = stumpS(a*x*x);
F = r0*vr0/sqrt(mu) * x*x*C + (1-a*r0)*x*x*x*S + r0*x-sqrt(mu)*dt;
dFdx = ( r0*vr0/sqrt(mu) )*x*(1-a*x*x*S) + (1-a*r0)*x*x*C + r0;
ratio = F/dFdx;
x = x-ratio;
}
if(n>nMax)
return 0;
else
return x;
}
//基于全局变量的拉格朗日系数f g的求解
void orbitAnimation::f_and_g(double x, double t,double ro, double a,double *f,double *g)
{
double z=a*x*x;
*f = 1-x*x*stumpC(z)/ro;
*g = t-x*x*x*stumpS(z)/sqrt(mu);
}
//基于全局变量的拉格朗日系数fdot,gdot的求解
void orbitAnimation::f_and_g_dot(double x, double r, double ro,double a,double *fdot,double *gdot)
{
double z=a*x*x;
*fdot = sqrt(mu)*x*(z*stumpS(z)-1)/(r*ro);
*gdot=1-x*x*stumpC(z)/r;
}
//R0,V0为初始时刻的卫星状态向量，R,V是t 时刻后的状态向量
void orbitAnimation::rv_from_rovo(osg::Vec3d R0,osg::Vec3d V0,double t,osg::Vec3d *R,osg::Vec3d *V)
{
double r0 = R0.length();//模
double v0 = V0.length();//模
h = r0*v0;
double vr0 = R0*V0 / r0;
alpha = 2/r0 - v0*v0/mu;
a = 1.0/alpha;
kafi = kepler_U(t,r0,vr0,alpha);

double *f,*g;
double *fdot,*gdot;
f= new double [1]; g = new double [1];
fdot = new double [1]; gdot = new double [1];
f_and_g(kafi,t,r0,alpha,f,g);
(*R) = R0*(*f) + V0*(*g);
double r = (*R).length();
f_and_g_dot(kafi,r,r0,alpha,fdot,gdot);
(*V) = R0*(*fdot) + V0*(*gdot);
}
//由状态向量计算轨道根数
void orbitAnimation::coe_from_sv(osg::Vec3d R,osg::Vec3d V,double *_h,double*_e,double *_RA,double* _incl,double* _w,double* _TA,double* _a)
{
//RA升交点赤经 incl轨道倾角; w近地点辐角
double r = R.length();
double v = V.length();
double vr = R*V/r;
osg::Vec3d H = R^V;
double h = H.length();
(*_h) = h;
double incl = acos(H.z()/h);//轨道倾角
(*_incl) = incl;
osg::Vec3d N = osg::Vec3d(0.0,0.0,1.0) ^ H;
double n = N.length();
//RA升角点赤经
double RA = 0;
if(abs(n)>eps) RA = acos(N.x()/n);
if(N.y()<0) RA = 2*osg::PI-RA;
(*_RA) = RA;
osg::Vec3d E = ( R* (v*v-mu/r) - V *(r*vr))*(1/mu) ;
double e = E.length();
(*_e) = e;
double w;
if(abs(n)>eps)
{
if(e>eps)
{
   w = acos( (N*E)/(n*e) );
   if( E.z()<0)
    w = 2*osg::PI-w;
}
else w = 0;
}
else
{
w = 0;
}
(*_w) = w;
double TA;
if( e>eps)
{
TA = acos( (E*R)/(e*r) );
if( vr<0 ) TA = 2*osg::PI -TA;
}
else
{
osg::Vec3d cp = N^R;
if(cp.z()>=0) TA = acos( (N*R)/(n*r) );
else
   TA = 2*osg::PI - acos( (N*R)/(n*r) );
}
(*_TA) = TA;
double a = (h*h)/( mu*(1-e*e) );
(*_a) = a;
}
//由轨道概数计算状态向量
void orbitAnimation::sv_from_coe(double h,double e,double RA,double incl,double w,double TA,osg::Vec3d *r,osg::Vec3d *v)
{
osg::Vec3d rp = ( (osg::Vec3d(1.0,0.0,0.0) * cos(TA) ) + osg::Vec3d(0.0,1.0,0.0) * sin(TA) ) * (h*h/mu) * (1/(1+e*cos(TA) ) );//4.37式
osg::Vec3d vp = ( ( osg::Vec3d(1.0,0.0,0.0) * -sin(TA) ) + ( osg::Vec3d(0.0,1.0,0.0) * ( e+cos(TA) ) )) * (mu/h) ;
//下面这种是分解方式
/*osg::Matrixd R3_W( cos(RA),sin(RA),0,0,
-sin(RA),cos(RA),0,0,
0 ,     0      ,1,0,
0,0,0,0);

osg::Matrixd R1_i( 1   ,0         ,0,0,
0    ,cos(incl),sin(incl),0,
0    ,-sin(incl),cos(incl),0,
0,0,0,0);

osg::Matrixd R3_w( cos(w),sin(w),0,0,
-sin(w),cos(w),0,0,
0 , 0 , 1,0,
0,0,0,0);
osg::Matrixd temp1,temp2,temp3,hellp;;
osg::Matrixd Q_Xx = R3_w*R1_i* R3_W;
osg::Matrixd Q_xX;
for(int i=0;i<4;i++)
{
for(int j=0;j<4;j++)
{
Q_xX(i,j)=Q_Xx(j,i);
}

}
(*r) = Q_xX.transform3x3(Q_xX,rp);
(*v) = Q_xX.transform3x3(Q_xX,vp);*/
//下面是合成矩阵方式
(*r) = Geocentic_from_near(rp,RA,incl,w);
(*v) = Geocentic_from_near(vp,RA,incl,w);
}
bool orbitAnimation::gibbs(osg::Vec3d R1,osg::Vec3d R2,osg::Vec3d R3,osg::Vec3d *V2)
{
double r1,r2,r3;
double n ;
double d ;

r1 = R1.length();
r2 = R2.length();
r3 = R3.length();

osg::Vec3d C12 = R1^R2;
osg::Vec3d C23 = R2^R3;
osg::Vec3d C31 = R3^R1;

osg::Vec3d C23Plus = C23;
C23Plus.normalize();
osg::Vec3d UR1Plus = R1;
UR1Plus.normalize();

if( (C23Plus*UR1Plus) > 1e-4 ) return false;

osg::Vec3d N = C23*r1 + C31*r2 + C12*r3; n=N.length();
osg::Vec3d D = C12+C23+C31; d = D.length();

osg::Vec3d S = R1*(r2-r3) + R2*(r3-r1) + R3*(r1-r2);
(*V2) = ((D^R2)/r2+S)*sqrt(mu/(n*d));

return true;
}

double orbitAnimation::func_y(double z,double r1,double r2,double A)
{
return r1 + r2 + A*(z*stumpS(z)-1)/sqrt(stumpC(z));
}
double orbitAnimation::func_F(double z,double t,double r1,double r2,double A)
{
double yz = func_y(z,r1,r2,A);
double cz = stumpC(z);
double first = yz/cz;
double sz = stumpS(z);
double second = pow(first,1.5);
second = second*sz;
return second + A*sqrt(yz) - sqrt(mu)*t;
}
double orbitAnimation::func_dFdz(double z,double r1,double r2,double A)
{
double yz = func_y(z,r1,r2,A);
if(z == 0)
{
return (sqrt(2.0) * pow(yz,1.5) / 40.0 )+ ( A/8.0*( sqrt(yz)+A*sqrt(0.5/yz)));
}
else
{
return pow(yz/stumpC(z),1.5)*((0.5/z)*(stumpC(z)-1.5*stumpS(z)/stumpC(z))+0.75*stumpS(z)*stumpS(z)/stumpC(z))+
0.125*A*((3*stumpS(z)*sqrt(yz)/stumpC(z)) + (A*sqrt(stumpC(z)/yz)));
}
}
bool orbitAnimation::lambert(osg::Vec3d R1,osg::Vec3d R2,double t,bool clockwise,osg::Vec3d *V1,osg::Vec3d *V2)
{
double r1,r2;
double delta_theta;
r1 = R1.length();
r2 = R2.length();

double judge = (R1^R2).z();
double theta = acos( (R1*R2) / (r1*r2) ); delta_theta = theta;
if(judge>=0)
{
if(!clockwise) delta_theta = 2*osg::PI - theta;
}
else
{
if(clockwise) delta_theta = 2*osg::PI - theta;
}

double A = sin(delta_theta)*sqrt( (r1*r2) / (1-cos(delta_theta) ) );
double z = -100;

/**解出初始的z*/
double Fz = func_F(z,t,r1,r2,A);

while( (Fz< 0) || (osg::isNaN(Fz)) )
{
Fz = func_F(z,t,r1,r2,A);
z+=0.1;
}
double ratio = 1.0;
int n = 0;
while( ( abs(ratio)>eps) && (n<=nmax) )
{
n = n+1;
ratio = func_F(z,t,r1,r2,A)/func_dFdz(z,r1,r2,A);
z = z-ratio;
}
if(n>=nmax) return false;
double yz = func_y(z,r1,r2,A);
double f = 1-yz/r1;
double g = A*sqrt(yz/mu);
double gdot = 1-yz/r2;
*V1 = (R2-R1*f)*(1/g);
*V2 = (R2*gdot-R1)*(1/g);

return 1;
}
osg::Vec3d orbitAnimation::Geocentic_to_near(osg::Vec3d rX,double omiga,double i,double w)
{
osg::Vec3d temp; osg::Matrixd mt;
double cos_omiga = cos(omiga); double cos_w = cos(w);
double sin_omiga = sin(omiga); double sin_w = sin(w);
double cos_i = cos(i); double sin_i = sin(i);
mt.set( cos_omiga*cos_w-sin_omiga*sin_w*cos_i,    sin_omiga*cos_w+cos_omiga*cos_i*sin_w,     sin_i*sin_w, 0,
-cos_omiga*sin_w-sin_omiga*cos_w*cos_i,   -sin_omiga*sin_w+cos_omiga*cos_i*cos_w,     sin_i*cos_w, 0,
sin_i*sin_omiga,                         -cos_omiga*sin_i,                           cos_i,        0,
0,0,0,1);

temp = mt*rX;
return temp;
}
osg::Vec3d orbitAnimation::Geocentic_from_near(osg::Vec3d rX,double omiga,double i,double w)
{
osg::Vec3d temp; osg::Matrixd mt;
double cos_omiga = cos(omiga); double cos_w = cos(w);
double sin_omiga = sin(omiga); double sin_w = sin(w);
double cos_i = cos(i); double sin_i = sin(i);
mt.set( cos_omiga*cos_w-sin_omiga*sin_w*cos_i,    sin_omiga*cos_w+cos_omiga*cos_i*sin_w,     sin_i*sin_w, 0,
-cos_omiga*sin_w-sin_omiga*cos_w*cos_i,   -sin_omiga*sin_w+cos_omiga*cos_i*cos_w,     sin_i*cos_w, 0,
sin_i*sin_omiga,                         -cos_omiga*sin_i,                           cos_i,        0,
0,0,0,1);

mt.invert(mt);
temp = mt*rX;
return temp;
}
double orbitAnimation::omiga_dot(double e,double a,double i)
{
return -(1.5*(sqrt(mu)*J2*re*re)/((pow(1-e*e,2.0))*pow(a,3.5)))*cos(i);
}
double orbitAnimation::w_dot(double e,double a,double i)
{
double k_para;
k_para = -(1.5*(sqrt(mu)*J2*re*re)/((pow(1-e*e,2))*pow(a,3.5)));
return k_para*(2.5*sin(i)*sin(i)-2);
}
double orbitAnimation::E_from_theta(double theta,double e)
{
return 2* atan(sqrt((1-e)/(e+1))*tan(0.5*theta));
}
double orbitAnimation::E_to_theta(double e,double E)
{
return 2* atan(sqrt((1+e)/(1-e))*tan(0.5*E));
}
osg::Vec3d orbitAnimation::nearR_reprent(double e,double TA)
{
return ( (osg::Vec3d(1.0,0.0,0.0) * cos(TA) ) + osg::Vec3d(0.0,1.0,0.0) * sin(TA) ) * (h*h/mu) * (1/(1+e*cos(TA) ) );
}
osg::Vec3d orbitAnimation::nearV_reprent(double e,double TA)
{
return ( ( osg::Vec3d(1.0,0.0,0.0) * -sin(TA) ) + ( osg::Vec3d(0.0,1.0,0.0) * ( e+cos(TA) ) )) * (mu/h) ;
}
double orbitAnimation::caculateJ0(double year,double month,double day)
{
double j0 = 367*year-int(7*(year+int((month+9)/12))/4) + int(275*month/9) + day +1721013.5;
return j0;
}
double orbitAnimation::caculateJD(double year,double month,double day,double hour,double minute,double second)
{
double J0 = caculateJ0(year,month,day);
double UT = hour + minute/60 + second/3600;
double JD = J0 + UT/24;
return JD;
}
double orbitAnimation::thetaG0(double year,double month,double day)
{
double J0 = caculateJ0(year,month,day);//计算世界0时刻的儒略日
double T0 = (J0-2451545)/36525;//计算J0与J2000之间的儒略世纪
double theta_g0 = 100.4606184+36000.77004*T0+0.000387933*T0*T0-2.583e-8*T0*T0*T0;
if(theta_g0>=360) theta_g0 = theta_g0-int(theta_g0/360) * 360;
else if(theta_g0<0) theta_g0 = theta_g0 - (int(theta_g0/360)-1) * 360;

return theta_g0;
}
double orbitAnimation::theta_(double year,double month,double day,double hour,double minute,
double second,double lon_degree,double lon_min,double lon_sec)
{
//首先对经度进行换算和限制
if(lon_degree<0) lon_degree-=360;
double EL = lon_degree + lon_min/60 + lon_sec/3600;

double G0 = thetaG0(year,month,day);
double UT = hour + minute/60 + second/3600;
double G = G0 + (360.98564724*UT)/24;

double theta = G + EL;

if(theta>=360) theta = theta-int(theta/360) * 360;
else if(theta<0) theta = theta - (int(theta/360)-1) * 360;

return theta;
}

osg::Vec3d orbitAnimation::phiAndTheta_to_IJK(double phi,double theta,double height)
{
//
osg::Vec3d R;
double X; double Y;double Z;
double sin_latitude = sin(phi);
double cos_latitude = cos(phi);
double _eccentricitySquared = 2*f-f*f;
double N = re / sqrt( 1.0 - _eccentricitySquared*sin_latitude*sin_latitude);
X = (N+height)*cos_latitude*cos(theta);
Y = (N+height)*cos_latitude*sin(theta);
Z = (N*(1-_eccentricitySquared)+height)*sin_latitude;
R.set(X,Y,Z);
return R;
}
void orbitAnimation::IJK_to_phiAndTheta(osg::Vec3d R,double *phi1,double *theta1, double *height1)
{
double X = R.x(); double Y = R.y(); double Z = R.z();
double p = sqrt(X*X + Y*Y);
double theta = atan2(Z*re , p*re_p);
double eDashSquared = (re*re - re_p*re_p)/(re_p*re_p);
double _eccentricitySquared = 2*f-f*f;
double sin_theta = sin(theta);
double cos_theta = cos(theta);

(*phi1) = atan( (Z + eDashSquared*re_p*sin_theta*sin_theta*sin_theta) /
(p - _eccentricitySquared*re*cos_theta*cos_theta*cos_theta) );
(*theta1) = atan2(Y,X);

double sin_latitude = sin(*phi1);
double N = re / sqrt( 1.0 - _eccentricitySquared*sin_latitude*sin_latitude);

(*height1) = p/cos(*phi1) - N;
}
void orbitAnimation::r_thetaG_phi_WL_to_alpha_delta(osg::Vec3d r,double thetaGs,double phi,double WL,double *alpha,double* delta)
{
double theta = thetaGs + WL;//当地恒星时
osg::Vec3d Ro = phiAndTheta_to_IJK(phi,theta,0);//地面观测站的矢量
osg::Vec3d rou = r - Ro;
rou.normalize();

(*delta) = asin(rou.z());
double alpha1 = asin(rou.y()/cos(*delta));
double alpha2 = acos(rou.x()/cos(*delta));
(*alpha) = alpha2;
}
osg::Vec3d orbitAnimation::Geocentic_to_Aa(double phi,double theta,osg::Vec3d R)
{
osg::Matrixd mat;
osg::Vec3d Rs;
double cosTheta;
double sinTheta;
double cosPhi;
double sinPhi;
cosTheta = cos(theta); sinTheta = sin(theta);
cosPhi = cos(phi); sinPhi = sin(phi);
mat.set(-sinTheta,        cosTheta,       0,       0,
-sinPhi*cosTheta,-sinPhi*sinTheta,cosPhi, 0,
cosPhi*cosTheta,cosPhi*sinTheta, sinPhi,0,
0,              0,               0,       1);
Rs = mat*R;
return Rs;
}
osg::Vec3d orbitAnimation::Geocentic_from_Aa(double phi,double theta,double A,double a)
{
osg::Matrixd mat;
double cosTheta;
double sinTheta;
double cosPhi;
double sinPhi;
cosTheta = cos(theta); sinTheta = sin(theta);
cosPhi = cos(phi); sinPhi = sin(phi);
mat.set(-sinTheta,        cosTheta,       0,       0,
-sinPhi*cosTheta,-sinPhi*sinTheta,cosPhi, 0,
cosPhi*cosTheta,cosPhi*sinTheta, sinPhi,0,
0,              0,               0,       1);
mat.invert(mat);//地平到地心变换矩阵
double rou_x = cos(a)*sin(A);
double rou_y = cos(a)*cos(A);
double rou_z = sin(a);
osg::Vec3d rou_unit;
rou_unit.set(rou_x,rou_y,rou_z);

osg::Vec3d Rs = mat*rou_unit;//地平在地心的表达式:xI + yJ +zK
return Rs;
}

void orbitAnimation::rv_from_observer(double rho,double rhodot,double A,double Adot,double a,
double adot,double theta,double phi,double H,osg::Vec3d * R,osg::Vec3d *V)
{
double height = 0.0;
osg::Vec3d omega(0,0,omegaEarth);
osg::Vec3d Ro = phiAndTheta_to_IJK(phi,theta,height);
osg::Vec3d Rdot = omega^Ro;
double dec = asin(cos(phi)*cos(A)*cos(a) + sin(phi)*sin(a));
osg::notify(osg::NOTICE)<<"delta为："<<osg::RadiansToDegrees(dec)<<std::endl;
double h = acos( (cos(phi)*sin(a) -sin(phi)*cos(A)*cos(a) )/cos(dec));
if ( (A>0) && (A<osg::PI) )
{
h = 2*osg::PI - h;
}
osg::notify(osg::NOTICE)<<"h为："<<osg::RadiansToDegrees(h)<<std::endl;
double RA = theta - h;
osg::notify(osg::NOTICE)<<"RA为："<<osg::RadiansToDegrees(RA)<<std::endl;
osg::Vec3d Rho(cos(RA)*cos(dec),sin(RA)*cos(dec),sin(dec));
osg::Vec3d r = Ro + Rho*rho;
//
(*R) = r;
double decdot = (-Adot*cos(phi)*sin(A)*cos(a)+adot*(sin(phi)*cos(a)-cos(phi)*cos(A)*sin(a))) / cos(dec);
double RAdot = earth_omiga + (Adot*cos(A)*cos(a)-adot*sin(A)*sin(a)+decdot*sin(A)*cos(a)*tan(dec))/(cos(phi)*sin(a)-sin(phi)*cos(A)*cos(a));
osg::Vec3d Rhodot (-RAdot*sin(RA)*cos(dec) - decdot*cos(RA)*sin(dec),RAdot*cos(RA)*cos(dec) - decdot*sin(RA)*sin(dec),decdot*cos(dec));
osg::Vec3d v = Rdot + Rho*rhodot + Rhodot*rho;

(*V) = v;
}
void orbitAnimation::gauss(osg::Vec3d Rho1,osg::Vec3d Rho2,osg::Vec3d Rho3,osg::Vec3d R1,osg::Vec3d R2,osg::Vec3d R3,double t1,double t2,double t3,osg::Vec3d *R,osg::Vec3d *V)
{
double tau1 = t1 -t2;
double tau3 = t3 -t2;
double tau = t3 -t1;

osg::Vec3d p1 = Rho2^Rho3;
osg::Vec3d p2 = Rho1^Rho3;
osg::Vec3d p3 = Rho1^Rho2;

double D0 = Rho1*p1;
osg::Matrixd D;
D.set(R1*p1,R1*p2,R1*p3,0,
R2*p1,R2*p2,R2*p3,0,
R3*p1,R3*p2,R3*p3,0,
0,0,0,1);
double E = R2*Rho2;
double A = 1.0/D0 *( -D(0,1)*(tau3/tau) + D(1,1) + D(2,1)*(tau1/tau));
double B = (1.0/(6*D0))*(D(0,1)*(tau3*tau3-tau*tau)*(tau3/tau) + D(2,1)*(tau*tau-tau1*tau1)*(tau1/tau));
double a = -(A*A + 2*A*E +R2.length2());
double b = -2*mu*B*(A+E);
double c = -(mu*B)*(mu*B);
//求一个8次多项式的根:x^8+aX^6+bx^3+c = 0
double coeff[]={1,0,a,0,0,b,0,0,c};
int n = sizeof(coeff)/sizeof(double)-1;
solver = new polySolver(coeff,n);
solver->Bairstow();

double *RootsReal, *RootsImag;
RootsReal=new double[n];
RootsImag=new double[n];
RootsReal=solver->getRootsr();
RootsImag=solver->getRootsi();
double x = 0;//根变量,|R2| =r2=x
for(int i=0; i<n; i++)
{
if(( RootsImag[i]== 0) && (RootsReal[i]>0) )
{
x = RootsReal[i];
break;
}
}
double f1 = 1-0.5*mu*tau1*tau1/(x*x*x);
double f3 = 1-0.5*mu*tau3*tau3/(x*x*x);
double g1 = tau1- 1.0*mu*tau1*tau1*tau1/(6.0*x*x*x);
double g3 = tau3- 1.0*mu*tau3*tau3*tau3/(6.0*x*x*x);

double rho2 = A + mu * B/(x*x*x);
double rho1 = (1/D0) *((6*(D(2,0)*tau1/tau3 + D(1,0)*tau/tau3)*x*x*x +
mu*D(2,0)*(tau*tau-tau1*tau1)*tau1/tau3)/(6*x*x*x + mu*(tau*tau-tau3*tau3))-D(0,0));
double rho3 = (1/D0) *((6*(D(0,2)*tau3/tau1 - D(1,2)*tau/tau1)*x*x*x +
mu*D(0,2)*(tau*tau-tau3*tau3)*tau3/tau1)/(6*x*x*x + mu*(tau*tau-tau1*tau1))-D(2,2));

osg::Vec3d r1 = R1 + Rho1*rho1;
osg::Vec3d r2 = R2 + Rho2*rho2;
osg::Vec3d r3 = R3 + Rho3*rho3;
osg::Vec3d v2 = (-r1*f3 + r3*f1)/(f1*g3 -f3*g1);
//下面开始近似求解
osg::Vec3d r_old = r2;
osg::Vec3d v_old = v2;

double rho1_old = rho1;
double rho2_old = rho2;
double rho3_old = rho3;

double diff1; double diff2; double diff3;
diff1 = diff2 = diff3 = 1.0;
n = 0;
double ro; //r2的模
double vo;//v2的模
double vro;//矢量2的径向速度大小
double alpha;//半长轴的倒数
double x1;//以矢量2为初始，经过t1到达1位置的全局近点角
double x3;//以矢量2为初始，经过t3到达3位置的全局近点角
double *ff1 = new double [1];
double *ff3 = new double [1];
double *gg1 = new double [1];
double *gg3 = new double [1];//基于全局近点角的拉格系数

double c1;
double c3;//组合系数
while( (diff1>error) && (diff2>error) && (diff3>error) && (n<nmax))
{
n = n+1;

ro = r2.length();
vo = v2.length();
vro = (v2*r2)/ro;
alpha = 2/ro - vo*vo/mu;
x1 = kepler_U(tau1,ro,vro,alpha);
x3 = kepler_U(tau3,ro,vro,alpha);
//计算1，3位置的拉格系数
f_and_g(x1,tau1,ro,alpha,ff1,gg1);
f_and_g(x3,tau3,ro,alpha,ff3,gg3);

f1 = (f1+(*ff1))/2; f3 = (f3+(*ff3))/2;
g1 = (g1+(*gg1))/2; g3 = (g3+(*gg3))/2;

c1 = g3/(f1*g3 - f3*g1);
c3 = -g1/(f1*g3 - f3*g1);

rho1 = (1/D0)*(-D(0,0) + (1/c1)*D(1,0) -c3*D(2,0)/c1 );
rho2 = (1/D0)*(-c1*D(0,1) + D(1,1) -c3*D(2,1));
rho3 = (1/D0)*(-c1*D(0,2)/c3 + (1/c3)*D(1,2) -D(2,2));

r1 = R1 + Rho1*rho1;
r2 = R2 + Rho2*rho2;
r3 = R3 + Rho3*rho3;

v2 = (r1*(-f3) + r3*f1)/(g3*f1 - g1*f3);
diff1 = abs(rho1-rho1_old);
diff2 = abs(rho2-rho2_old);
diff3 = abs(rho3-rho3_old);

rho1_old = rho1;
rho2_old = rho2;
rho3_old = rho3;
}
(*R) = r2;
(*V) = v2;
}

osg::Vec3d orbitAnimation::alpha_delta_to_IJK(double alpha,double delta)
{
osg::Vec3d rho;
double cosd = cos(delta);
double cosa = cos(alpha);
double sina = sin(alpha);
double sind = sin(delta);

rho.set(cosd*cosa,cosd*sina,sind);
return rho;
}

double orbitAnimation::theta_from_t(double t)
{
Me = t*2*osg::PI/(T);
E = kepler_E((e),(Me));
return 2*atan( sqrt( (1+(e))/(1-(e)) ) * tan((E)/2.0) );
}

void orbitAnimation::set_CurrentTime(osg::DoubleArray *t)
{
current_time = t;
}
————————————————
版权声明：本文为CSDN博主「shirro123」的原创文章，遵循CC 4.0 BY-SA版权协议，转载请附上原文出处链接及本声明。
原文链接：https://blog.csdn.net/shirro123/article/details/47134329

轨道机动算法的C++实现相关推荐

卫星轨道推演计算相关知识点总结（含欧拉角、旋转矩阵、及各坐标系转化等）
来源:轨道机动算法的C++实现_shirro123的专栏-CSDN博客卫星轨道推演计算相关基础知识点总结一.卫星的运动特性二.卫星的空间坐标系 ...
卫星过顶计算matlab,太阳同步侦察卫星轨道预报算法实现与仿真_问答库
太阳同步卫星是各国军事情报获取的重要手段,用来完成一定时间.分辨率约束下的情报.监视和侦察(ISR)任务.针对目前我方各类地面敏感目标易被其它国家卫星侦察,而地面测站提供的卫星过顶预报数据不详细等情况 ...
golang通过RSA算法生成token，go从配置文件中注入密钥文件，go从文件中读取密钥文件，go RSA算法下token生成与解析；go java token共用
RSA算法 token生成与解析本文演示两种方式,一种是把密钥文件放在配置文件中,一种是把密钥文件本身放入项目或者容器中. 下面两种的区别在于私钥公钥的初始化, init方法,需要哪种取哪种. 通过 ...
通用解题法——回溯算法（理解+练习）
积累算法经验,积累解题方法--回溯算法,你必须要掌握的解题方法! 什么是回溯算法呢? 回溯算法实际上一个类似枚举的搜索尝试过程,主要是在搜索尝试过程中寻找问题的解,当发现已不满足求解条件时,就&quo ...
伍六七带你学算法进阶篇-生命游戏
有趣的算法题–生命游戏难度-中等根据百度百科 ,生命游戏,简称为生命,是英国数学家约翰·何顿·康威在 1970 年发明的细胞自动机. 想要体验生命游戏的小伙伴可以到这里-->生命游戏进入 ...
伍六七带你学算法进阶篇-排序算法
给定一个整数数组 nums,将该数组升序排列. 示例 1: 输入:[5,2,3,1] 输出:[1,2,3,5] 示例 2: 输入:[5,1,1,2,0,0] 输出:[0,0,1,1,2,5] 各排序算 ...
伍六七带你学算法入门篇-卡牌分组
力扣-914. 卡牌分组难度-简单这是一道非常有趣的题,提交通过率令人深思 ,思考它是不是一道简单的题- 开始正题: 给定一副牌,每张牌上都写着一个整数. 此时,你需要选定一个数字 X,使我们可以 ...
伍六七带你学算法入门篇-最小的k个数
java面试题-最小的k个数难度-简单输入整数数组 arr ,找出其中最小的 k 个数.例如,输入4.5.1.6.2.7.3.8这8个数字,则最小的4个数字是1.2.3.4. 示例 1: 输入:a ...
十大算法，描述+代码+演示+分析+改进（赶紧收藏！）
十大算法 1.冒泡排序 (1)算法描述冒泡排序是一种简单的排序算法.它重复地走访过要排序的数列,一次比较两个元素,如果它们的顺序错误就把它们交换过来.走访数列的工作是重复地进行直到没有再需要 ...

轨道机动算法的C++实现

轨道机动算法的C++实现相关推荐

最新文章

热门文章