主要由类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