// Degrees per radian
static void Main0(string[] args)
{
// Position and velocity
Geo.Algorithm.Vector r = new Geo.Algorithm.Vector(+10000.0e3, +40000.0e3, -5000.0e3); // [m]
Geo.Algorithm.Vector v = new Geo.Algorithm.Vector(-1.500e3, +1.000e3, -0.100e3); // [m/s]
// Variables
int i;
Geo.Algorithm.Vector y = new Geo.Algorithm.Vector(6), Kep = new Geo.Algorithm.Vector(6);
// Orbital elements
y = r.Stack(v);
Kep = Kepler.Elements(OrbitConsts.GM_Earth, y);
// Output
var info = "Exercise 2-3: Osculating elements";
Console.WriteLine(info);
info = "State vector:";
Console.WriteLine(info);
info = " Position ";
Console.Write(info);
for (i = 0; i < 3; i++)
{
Console.Write(r[i] / 1000.0 + ", ");
//Console.WriteLine(info);
}
;
info = " [km]";
Console.WriteLine(info);
info = " Velocity ";
Console.Write(info);
for (i = 0; i < 3; i++)
{
Console.Write(v[i] / 1000.0 + ", ");
}
Console.WriteLine(" [km/s]");
Console.WriteLine();
Console.WriteLine("Orbital elements:");
Console.WriteLine(" Semimajor axis " + String.Format("{0:f3}", (Kep[0] / 1000.0)) + " km");
Console.WriteLine(" Eccentricity " + String.Format("{0:f3}", Kep[1]));
Console.WriteLine(" Inclination " + String.Format("{0:f3}", Kep[2] * OrbitConsts.DegPerRad) + " deg");
Console.WriteLine(" RA ascend. node " + String.Format("{0:f3}", Kep[3] * OrbitConsts.DegPerRad) + " deg");
Console.WriteLine(" Arg. of perigee " + String.Format("{0:f3}", Kep[4] * OrbitConsts.DegPerRad) + " deg");
Console.WriteLine(" Mean anomaly " + String.Format("{0:f3}", Kep[5] * OrbitConsts.DegPerRad) + " deg");
Console.ReadKey();
}
/// <summary>
/// Computes the derivative of the state vector for the normalized (GM=1)
/// Kepler's problem in three dimensions
/// Note:
///
/// pAux is expected to point to an integer variable that will be incremented
// by one on each call of Deriv
/// </summary>
/// <param name="t"></param>
/// <param name="y"></param>
/// <param name="pAux"></param>
/// <returns></returns>
static Geo.Algorithm.Vector f_Kep6D(double t, Geo.Algorithm.Vector y, Object pAux)
{
// Pointer to auxiliary integer variable used as function call counter
// State vector derivative
Geo.Algorithm.Vector r = y.Slice(0, 2);
Geo.Algorithm.Vector v = y.Slice(3, 5);
Geo.Algorithm.Vector yp = v.Stack(-r / (Math.Pow(r.Norm(), 3)));
// Increment function call count
((Action)pAux)();
return(yp);
}
/// <summary>
/// 由开普勒卫星轨道根数计算卫星状态,前三位置，后三速度。Computes the satellite state vector from osculating Keplerian elements
/// for elliptic orbits
/// Notes:
/// The semimajor axis a=Kep(0), dt and GM must be given in consistent units,
/// e.g. [m], [s] and [m^3/s^2]. The resulting units of length and velocity
/// are implied by the units of GM, e.g. [m] and [m/s].
/// </summary>
/// <param name="GM"> GM Gravitational coefficient (gravitational constant * mass of central body)</param>
/// <param name="kepElements">Kep Keplerian elements (a,e,i,Omega,omega,M) with
/// a Semimajor axis
/// e Eccentricity
/// i Inclination [rad]
/// Omega Longitude of the ascending node [rad]
/// omega Argument of pericenter [rad]
/// M Mean anomaly at epoch [rad]
/// dt Time since epoch
///
/// </param>
/// <param name="dt"></param>
/// <returns>State vector (x,y,z,vx,vy,vz)</returns>
public static Geo.Algorithm.Vector State(double GM, Geo.Algorithm.Vector kepElements, double dt = 0)
{
// Variables
double a, e, i, Omega, omega, M, M0, n;
double E, cosE, sinE, fac, R, V;
Geo.Algorithm.Vector r = new Geo.Algorithm.Vector(3), v = new Geo.Algorithm.Vector(3);
Matrix PQW = new Matrix(3, 3);
// Keplerian elements at epoch
a = kepElements[0]; Omega = kepElements[3];
e = kepElements[1]; omega = kepElements[4];
i = kepElements[2]; M0 = kepElements[5];
// Mean anomaly
if (dt == 0.0)
{
M = M0;
}
else
{
n = Math.Sqrt(GM / (a * a * a));
M = M0 + n * dt;
};
// Eccentric anomaly
E = EccAnom(M, e);
cosE = Math.Cos(E);
sinE = Math.Sin(E);
// Perifocal coordinates
fac = Math.Sqrt((1.0 - e) * (1.0 + e));
R = a * (1.0 - e * cosE); // Distance
V = Math.Sqrt(GM * a) / R; // Velocity
r = new Geo.Algorithm.Vector(a * (cosE - e), a * fac * sinE, 0.0);
v = new Geo.Algorithm.Vector(-V * sinE, +V * fac * cosE, 0.0);
// Transformation to reference system (Gaussian vectors)
PQW = Matrix.RotateZ3D(-Omega) * Matrix.RotateX3D(-i) * Matrix.RotateZ3D(-omega);
r = PQW * r;
v = PQW * v;
// State vector
return(r.Stack(v));
}
Geo.Algorithm.Vector r_p, v_p; // Predictor
/// <summary>
/// 获取状态
/// </summary>
/// <param name="e"></param>
/// <param name="t"></param>
/// <param name="h"></param>
/// <param name="step"></param>
/// <returns></returns>
public Geo.Algorithm.Vector GetState(double e, double t, double h, int step)
{
var r = new Geo.Algorithm.Vector(1.0 - e, 0.0, 0.0);
var v = new Geo.Algorithm.Vector(0.0, Math.Sqrt((1 + e) / (1 - e)), 0.0);
// Integration from t=t to t=t_end
this.Init(t, r, v, h);
for (int i = 1; i <= step; i++)
{
Step(ref t, out r, out v);
}
var y = r.Stack(v);
return(y);
}
/// <summary>
/// 导数、微分
/// Computes the derivative of the state vector .
/// pAux is expected to point to a variable of type AuxDataRecord, which is
/// used to communicate with the other program sections and to hold satData
/// between subsequent calls of this function
/// </summary>
/// <param name="t">历元</param>
/// <param name="y">卫星状态，坐标和速度</param>
/// <param name="pAux">附加选项</param>
static Geo.Algorithm.Vector Deriv(double t, Geo.Algorithm.Vector y, Object pAux)
{
// Pointer to auxiliary satData record
ForceModelOption p = (ForceModelOption)pAux;// static_cast<ForceModelOption*>(pAux);
// Time
double Mjd_TT = p.Mjd0_TT + t / 86400.0;
// State vector components
Geo.Algorithm.Vector r = y.Slice(0, 2);
Geo.Algorithm.Vector v = y.Slice(3, 5);
// Acceleration
AccelerationCalculator calculator = new AccelerationCalculator(p, IERS);
var a = calculator.GetAcceleration(Mjd_TT, r, v);
// State vector derivative
Geo.Algorithm.Vector yp = v.Stack(a);
return(yp);
}
//------------------------------------------------------------------------------
//
// f_Kep6D_
//
// Purpose:
//
// Computes the derivative of the state vector for the normalized (GM=1)
// Kepler's problem in three dimensions
//
// Note:
//
// pAux is expected to point to a variable of type AuxDataRecord, which is
// used to communicate with the other program sections and to hold satData
// between subsequent calls of this function
//
//------------------------------------------------------------------------------
static Geo.Algorithm.Vector f_Kep6D_(double t, Geo.Algorithm.Vector y, Object pAux)
{
// State vector derivative
Geo.Algorithm.Vector r = y.Slice(0, 2);
Geo.Algorithm.Vector v = y.Slice(3, 5);
Geo.Algorithm.Vector yp = v.Stack(-r / (Math.Pow(r.Norm(), 3)));
// Pointer to auxiliary satData record
AuxDataRecord p = (AuxDataRecord)pAux;// static_cast<AuxDataRecord*>(pAux);
// Write current time, step aboutSize and radius; store time for next step
if (t - p.t > 1.0e-10)
{
p.n_step++;
var info = String.Format("{0, 5:D}{1,12:F6}{2,12:F6}{3,12:F3}", p.n_step, t, t - p.t, r.Norm());
Console.WriteLine(info);
p.t = t;
}
;
pAux = p;
return(yp);
}
