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

http://blog.csdn.net/shirro123/article/details/47134329

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