/// <summary>
/// 本地空间直角坐标到极坐标的转换. Computes azimuth and elevation from local tangent coordinates
/// </summary>
/// <param name="localEnz"> s Topocentric local tangent coordinates (East-North-Zenith frame)</param>
/// <param name="azimuthRad"> A Azimuth [rad]</param>
/// <param name="elevationRad"> E Elevation [rad]</param>
/// <param name="Range"> Range [m]</param>
public static void LocalEnzToPolar(Geo.Algorithm.Vector localEnz, out double azimuthRad, out double elevationRad, out double range)
{
range = localEnz.Norm();
azimuthRad = Math.Atan2(localEnz[0], localEnz[1]);
azimuthRad = ((azimuthRad < 0.0) ? azimuthRad + OrbitConsts.TwoPI : azimuthRad);
elevationRad = Math.Atan(localEnz[2] / Math.Sqrt(localEnz[0] * localEnz[0] + localEnz[1] * localEnz[1]));
}
//------------------------------------------------------------------------------
//
// Elements
//
// Purpose:
//
// Computes the osculating Keplerian elements from the satellite state vector
// for elliptic orbits
//
// Input/Output:
//
// GM Gravitational coefficient
// (gravitational constant * mass of central body)
// y State vector (x,y,z,vx,vy,vz)
// <return> 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 [rad]
//
// Notes:
//
// The state vector and GM must be given in consistent units,
// e.g. [m], [m/s] and [m^3/s^2]. The resulting unit of the semimajor
// axis is implied by the unity of y, e.g. [m].
//
// The function cannot be used with state vectors describing a circular
// or non-inclined orbit.
//
//------------------------------------------------------------------------------
/// <summary>
/// 由某点的位置和速度，求解开普勒轨道根数。
/// </summary>
/// <param name="GM"> Gravitational coefficien<t/param>
/// <param name="y"> State vector (x,y,z,vx,vy,vz) </param>
/// <returns></returns>
public static Geo.Algorithm.Vector Elements(double GM, Geo.Algorithm.Vector stateVector)
{
// Variables
Geo.Algorithm.Vector r = new Geo.Algorithm.Vector(3), v = new Geo.Algorithm.Vector(3), h = new Geo.Algorithm.Vector(3);
double H, u, R;
double eCosE, eSinE, e2, E, nu;
double a, e, i, Omega, omega, M;
r = stateVector.Slice(0, 2); // Position
v = stateVector.Slice(3, 5); // Velocity
h = r.Cross3D(v); // Areal velocity
H = h.Norm();
Omega = Math.Atan2(h[0], -h[1]); // Long. ascend. node
Omega = MathUtil.Modulo(Omega, OrbitConsts.TwoPI);
i = Math.Atan2(Math.Sqrt(h[0] * h[0] + h[1] * h[1]), h[2]); // Inclination
u = Math.Atan2(r[2] * H, -r[0] * h[1] + r[1] * h[0]); // Arg. of latitude
R = r.Norm(); // Distance
a = 1.0 / (2.0 / R - v.Dot(v) / GM); // Semi-major axis
eCosE = 1.0 - R / a; // e*cos(E)
eSinE = r.Dot(v) / Math.Sqrt(GM * a); // e*Math.Sin(E)
e2 = eCosE * eCosE + eSinE * eSinE;
e = Math.Sqrt(e2); // Eccentricity
E = Math.Atan2(eSinE, eCosE); // Eccentric anomaly
M = MathUtil.Modulo(E - eSinE, OrbitConsts.TwoPI); // Mean anomaly
nu = Math.Atan2(Math.Sqrt(1.0 - e2) * eSinE, eCosE - e2); // True anomaly
omega = MathUtil.Modulo(u - nu, OrbitConsts.TwoPI); // Arg. of perihelion
// Keplerian elements vector
return(new Geo.Algorithm.Vector(a, e, i, Omega, omega, M));
}
/// <summary>
/// 计算大气延迟。 Computes the acceleration due to the atmospheric drag.
/// </summary>
/// <param name="Mjd_TT">Mjd_TT Terrestrial Time (Modified Julian Date)</param>
/// <param name="satXyz">r Satellite position vector in the inertial system [m]</param>
/// <param name="satVelocity"> v Satellite velocity vector in the inertial system [m/s]</param>
/// <param name="transMatrix"> T Transformation matrix to true-of-date inertial system</param>
/// <param name="Area">Area Cross-section [m^2]</param>
/// <param name="mass"> mass Spacecraft mass [kg]</param>
/// <param name="atmosDragCoeff"> coefOfDrag Drag coefficient</param>
/// <returns>Acceleration (a=d^2r/dt^2) [m/s^2]</returns>
public static Geo.Algorithm.Vector AccelerDragOfAtmos(double Mjd_TT, Geo.Algorithm.Vector satXyz, Geo.Algorithm.Vector satVelocity,
Matrix transMatrix, double Area, double mass, double atmosDragCoeff)
{
// Earth angular velocity vector [rad/s]
double[] Data_omega = { 0.0, 0.0, 7.29212e-5 };
Geo.Algorithm.Vector omega = new Geo.Algorithm.Vector(Data_omega, 3);
// Variables
double v_abs, dens;
Geo.Algorithm.Vector r_tod = new Geo.Algorithm.Vector(3), v_tod = new Geo.Algorithm.Vector(3);
Geo.Algorithm.Vector v_rel = new Geo.Algorithm.Vector(3), a_tod = new Geo.Algorithm.Vector(3);
Matrix T_trp = new Matrix(3, 3);
// Transformation matrix to ICRF/EME2000 system
T_trp = transMatrix.Transpose();
// Position and velocity in true-of-date system
r_tod = transMatrix * satXyz;
v_tod = transMatrix * satVelocity;
// Velocity relative to the Earth's atmosphere
v_rel = v_tod - omega.Cross3D(r_tod);
v_abs = v_rel.Norm();
// Atmospheric density due to modified Harris-Priester model
dens = TerrestrialUtil.AtmosDensity_HP(Mjd_TT, r_tod);
// Acceleration
a_tod = -0.5 * atmosDragCoeff * (Area / mass) * dens * v_abs * v_rel;
return(T_trp * a_tod);
}
/// <summary>
/// 计算由质点万有引力引起的扰动加速度。 Computes the perturbational acceleration due to a point mass
/// </summary>
/// <param name="satXyz"> r Satellite position vector </param>
/// <param name="massXyz"> s Point mass position vector</param>
/// <param name="GM"> GM Gravitational coefficient of point mass</param>
/// <returns> Acceleration (a=d^2r/dt^2)</returns>
public static Geo.Algorithm.Vector AccelerOfPointMass(Geo.Algorithm.Vector satXyz, Geo.Algorithm.Vector massXyz, double GM)
{
//Relative position vector of satellite w.r.t. point mass
Geo.Algorithm.Vector d = satXyz - massXyz;
// Acceleration
return((-GM) * (d / Math.Pow(d.Norm(), 3) + massXyz / Math.Pow(massXyz.Norm(), 3)));
}
/// <summary>
/// Computes the second time derivative of the position vector for the
/// normalized (GM=1) Kepler's problem in three dimensions.
/// pAux is expected to point to an integer variable that will be incremented
/// by one on each call of f_Kep3D
///
/// </summary>
/// <param name="t"></param>
/// <param name="r"></param>
/// <param name="v"></param>
/// <param name="a"></param>
/// <param name="pAux"></param>
static void f_Kep3D(double t, Geo.Algorithm.Vector r, Geo.Algorithm.Vector v, ref Geo.Algorithm.Vector a, Action pAux)
{
// Pointer to auxiliary integer variable used as function call counter
//int* pCalls = static_cast<int*>(pAux);
// 2nd order derivative d^2(r)/dt^2 of the position vector
a = new Geo.Algorithm.Vector(-r / (Math.Pow(r.Norm(), 3)));
// Increment function call count
//(*pCalls)++;
pAux();
}
/// <summary>
/// 从两个位置向量和中间时间计算扇区三角形比率
/// Computes the sector-triangle ratio from two position vectors and
/// the intermediate time
/// </summary>
/// <param name="r_a">r_a Position at time t_a</param>
/// <param name="r_b">Position at time t_b</param>
/// <param name="tau"> Normalized time (Math.Sqrt(GM)*(t_a-t_b))</param>
/// <returns> Sector-triangle ratio</returns>
private static double FindEta(Geo.Algorithm.Vector r_a, Geo.Algorithm.Vector r_b, double tau)
{
// Constants
const int maxit = 30;
const double delta = 100.0 * eps_mach;
// Variables
int i;
double kappa, m, l, s_a, s_b, eta_min, eta1, eta2, F1, F2, d_eta;
// Auxiliary quantities
s_a = r_a.Norm();
s_b = r_b.Norm();
kappa = Math.Sqrt(2.0 * (s_a * s_b + r_a.Dot(r_b)));
m = tau * tau / Math.Pow(kappa, 3);
l = (s_a + s_b) / (2.0 * kappa) - 0.5;
eta_min = Math.Sqrt(m / (l + 1.0));
// Start with Hansen's approximation
eta2 = (12.0 + 10.0 * Math.Sqrt(1.0 + (44.0 / 9.0) * m / (l + 5.0 / 6.0))) / 22.0;
eta1 = eta2 + 0.1;
// Secant method
F1 = F(eta1, m, l);
F2 = F(eta2, m, l);
i = 0;
while (Math.Abs(F2 - F1) > delta)
{
d_eta = -F2 * (eta2 - eta1) / (F2 - F1);
eta1 = eta2; F1 = F2;
while (eta2 + d_eta <= eta_min)
{
d_eta *= 0.5;
}
eta2 += d_eta;
F2 = F(eta2, m, l); ++i;
if (i == maxit)
{
throw new ArgumentException("WARNING: Convergence problems in FindEta");
break;
}
}
return(eta2);
}
/// <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);
}
//------------------------------------------------------------------------------
//
// 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);
}
//------------------------------------------------------------------------------
//
// Elements
//
// Purpose:
//
// Computes orbital elements from two given position vectors and
// associated times
//
// Input/Output:
//
// GM Gravitational coefficient
// (gravitational constant * mass of central body)
// Mjd_a Time t_a (Modified Julian Date)
// Mjd_b Time t_b (Modified Julian Date)
// r_a Position vector at time t_a
// r_b Position vector at time t_b
// <return> Keplerian elements (a,e,i,Omega,omega,M)
// a Semimajor axis
// e Eccentricity
// i Inclination [rad]
// Omega Longitude of the ascending node [rad]
// omega Argument of pericenter [rad]
// M Mean anomaly [rad]
// at time t_a
//
// Notes:
//
// The function cannot be used with state vectors describing a circular
// or non-inclined orbit.
//
//------------------------------------------------------------------------------
/// <summary>
/// 计算轨道根数，由两个已知位置。
/// </summary>
/// <param name="GM"></param>
/// <param name="Mjd_a"></param>
/// <param name="Mjd_b"></param>
/// <param name="r_a"></param>
/// <param name="r_b"></param>
/// <returns></returns>
public static Geo.Algorithm.Vector Elements(double GM, double Mjd_a, double Mjd_b,
Geo.Algorithm.Vector r_a, Geo.Algorithm.Vector r_b)
{
// Variables
double tau, eta, p;
double n, nu, E, u;
double s_a, s_b, s_0, fac, sinhH;
double cos_dnu, sin_dnu, ecos_nu, esin_nu;
double a, e, i, Omega, omega, M;
Geo.Algorithm.Vector e_a = new Geo.Algorithm.Vector(3), r_0 = new Geo.Algorithm.Vector(3), e_0 = new Geo.Algorithm.Vector(3), W = new Geo.Algorithm.Vector(3);
// Calculate vector r_0 (fraction of r_b perpendicular to r_a)
// and the magnitudes of r_a,r_b and r_0
s_a = r_a.Norm(); e_a = r_a / s_a;
s_b = r_b.Norm();
fac = r_b.Dot(e_a); r_0 = r_b - fac * e_a;
s_0 = r_0.Norm(); e_0 = r_0 / s_0;
// Inclination and ascending node
W = e_a.Cross3D(e_0);
Omega = Math.Atan2(W[0], -W[1]); // Long. ascend. node
Omega = MathUtil.Modulo(Omega, OrbitConsts.TwoPI);
i = Math.Atan2(Math.Sqrt(W[0] * W[0] + W[1] * W[1]), W[2]); // Inclination
if (i == 0.0)
{
u = Math.Atan2(r_a[1], r_a[0]);
}
else
{
u = Math.Atan2(+e_a[2], -e_a[0] * W[1] + e_a[1] * W[0]);
}
// Semilatus rectum
tau = Math.Sqrt(GM) * 86400.0 * Math.Abs(Mjd_b - Mjd_a);
eta = FindEta(r_a, r_b, tau);
p = Math.Pow(s_a * s_0 * eta / tau, 2);
// Eccentricity, true anomaly and argument of perihelion
cos_dnu = fac / s_b;
sin_dnu = s_0 / s_b;
ecos_nu = p / s_a - 1.0;
esin_nu = (ecos_nu * cos_dnu - (p / s_b - 1.0)) / sin_dnu;
e = Math.Sqrt(ecos_nu * ecos_nu + esin_nu * esin_nu);
nu = Math.Atan2(esin_nu, ecos_nu);
omega = MathUtil.Modulo(u - nu, OrbitConsts.TwoPI);
// Perihelion distance, semimajor axis and mean motion
a = p / (1.0 - e * e);
n = Math.Sqrt(GM / Math.Abs(a * a * a));
// Mean anomaly and time of perihelion passage
if (e < 1.0)
{
E = Math.Atan2(Math.Sqrt((1.0 - e) * (1.0 + e)) * esin_nu, ecos_nu + e * e);
M = MathUtil.Modulo(E - e * Math.Sin(E), OrbitConsts.TwoPI);
}
else
{
sinhH = Math.Sqrt((e - 1.0) * (e + 1.0)) * esin_nu / (e + e * ecos_nu);
M = e * sinhH - Math.Log(sinhH + Math.Sqrt(1.0 + sinhH * sinhH));
}
// Keplerian elements vector
return(new Geo.Algorithm.Vector(a, e, i, Omega, omega, M));
}
//------------------------------------------------------------------------------
//
// StatePartials
//
// Purpose:
//
// Computes the partial derivatives of the satellite state vector with respect
// to the orbital elements for elliptic, Keplerian orbits
//
// Input/Output:
//
// GM Gravitational coefficient
// (gravitational constant * mass of central body)
// Kep Keplerian elements (a,e,i,Omega,omega,M) at epoch 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
// <return> Partials derivatives of the state vector (x,y,z,vx,vy,vz) at time
// dt with respect to the epoch orbital elements
//
// 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].
//
// The function cannot be used with circular or non-inclined orbit.
//
//------------------------------------------------------------------------------
/// <summary>
/// 计算卫星状态向量相对于椭圆轨道元素的偏导数
/// </summary>
/// <param name="GM"></param>
/// <param name="kepElements"></param>
/// <param name="dt"></param>
/// <returns></returns>
private static Matrix StatePartials(double GM, Geo.Algorithm.Vector kepElements, double dt)
{
// Variables
int k;
double a, e, i, Omega, omega, M, M0, n, dMda;
double E, cosE, sinE, fac, r, v, x, y, vx, vy;
Matrix PQW = new Matrix(3, 3);
Geo.Algorithm.Vector P = new Geo.Algorithm.Vector(3), Q = new Geo.Algorithm.Vector(3), W = new Geo.Algorithm.Vector(3), e_z = new Geo.Algorithm.Vector(3), N = new Geo.Algorithm.Vector(3);
Geo.Algorithm.Vector dPdi = new Geo.Algorithm.Vector(3), dPdO = new Geo.Algorithm.Vector(3), dPdo = new Geo.Algorithm.Vector(3), dQdi = new Geo.Algorithm.Vector(3), dQdO = new Geo.Algorithm.Vector(3), dQdo = new Geo.Algorithm.Vector(3);
Geo.Algorithm.Vector dYda = new Geo.Algorithm.Vector(6), dYde = new Geo.Algorithm.Vector(6), dYdi = new Geo.Algorithm.Vector(6), dYdO = new Geo.Algorithm.Vector(6), dYdo = new Geo.Algorithm.Vector(6), dYdM = new Geo.Algorithm.Vector(6);
Matrix dYdA = new Matrix(6, 6);
// Keplerian elements at epoch
a = kepElements[0]; Omega = kepElements[3];
e = kepElements[1]; omega = kepElements[4];
i = kepElements[2]; M0 = kepElements[5];
// Mean and eccentric anomaly
n = Math.Sqrt(GM / (a * a * a));
M = M0 + n * dt;
E = EccAnom(M, e);
// Perifocal coordinates
cosE = Math.Cos(E);
sinE = Math.Sin(E);
fac = Math.Sqrt((1.0 - e) * (1.0 + e));
r = a * (1.0 - e * cosE); // Distance
v = Math.Sqrt(GM * a) / r; // Velocity
x = +a * (cosE - e); y = +a * fac * sinE;
vx = -v * sinE; vy = +v * fac * cosE;
// Transformation to reference system (Gaussian vectors) and partials
PQW = Matrix.RotateZ3D(-Omega) * Matrix.RotateX3D(-i) * Matrix.RotateZ3D(-omega);
P = PQW.Col(0); Q = PQW.Col(1); W = PQW.Col(2);
e_z = new Geo.Algorithm.Vector(0, 0, 1); N = e_z.Cross3D(W); N = N / N.Norm();
dPdi = N.Cross3D(P); dPdO = e_z.Cross3D(P); dPdo = Q;
dQdi = N.Cross3D(Q); dQdO = e_z.Cross3D(Q); dQdo = -P;
// Partials w.r.t. semimajor axis, eccentricity and mean anomaly at time dt
dYda = ((x / a) * P + (y / a) * Q).Stack((-vx / (2 * a)) * P + (-vy / (2 * a)) * Q);
dYde = (((-a - Math.Pow(y / fac, 2) / r) * P + (x * y / (r * fac * fac)) * Q).Stack(
(vx * (2 * a * x + e * Math.Pow(y / fac, 2)) / (r * r)) * P
+ ((n / fac) * Math.Pow(a / r, 2) * (x * x / r - Math.Pow(y / fac, 2) / a)) * Q));
dYdM = ((vx * P + vy * Q) / n).Stack((-n * Math.Pow(a / r, 3)) * (x * P + y * Q));
// Partials w.r.t. inlcination, node and argument of pericenter
dYdi = (x * dPdi + y * dQdi).Stack(vx * dPdi + vy * dQdi);
dYdO = (x * dPdO + y * dQdO).Stack(vx * dPdO + vy * dQdO);
dYdo = (x * dPdo + y * dQdo).Stack(vx * dPdo + vy * dQdo);
// Derivative of mean anomaly at time dt w.r.t. the semimajor axis at epoch
dMda = -1.5 * (n / a) * dt;
// Combined partial derivative matrix of state with respect to epoch elements
for (k = 0; k < 6; k++)
{
dYdA[k, 0] = dYda[k] + dYdM[k] * dMda;
dYdA[k, 1] = dYde[k];
dYdA[k, 2] = dYdi[k];
dYdA[k, 3] = dYdO[k];
dYdA[k, 4] = dYdo[k];
dYdA[k, 5] = dYdM[k];
}
return(dYdA);
}
/// <summary>
/// 计算由太阳光压引起的加速度。 Computes the acceleration due to solar radiation pressure assuming
/// the spacecraft surface normal to the Sun direction
/// Notes: r, r_sun, Area, mass, P0 and AU must be given in consistent units,
/// e.g. m, m^2, kg and N/m^2.
/// </summary>
/// <param name="satXyz">r Spacecraft position vector </param>
/// <param name="sunXyz"> r_Sun Sun position vector </param>
/// <param name="Area">Area Cross-section </param>
/// <param name="mass"> mass Spacecraft mass</param>
/// <param name="solarRadiPressCoef"> coefOfRadiation Solar radiation pressure coefficient</param>
/// <param name="solarRadiPressurePerAU">P0 Solar radiation pressure at 1 AU </param>
/// <param name="AstroUnit">AU Length of one Astronomical Unit </param>
/// <returns>Acceleration (a=d^2r/dt^2)</returns>
public static Geo.Algorithm.Vector AccelerOfSolarRadiPressure(Geo.Algorithm.Vector satXyz, Geo.Algorithm.Vector sunXyz,
double Area, double mass, double solarRadiPressCoef,
double solarRadiPressurePerAU, double AstroUnit)
{
//Relative position vector of spacecraft w.r.t. Sun
Geo.Algorithm.Vector d = satXyz - sunXyz;
//Acceleration
return(solarRadiPressCoef * (Area / mass) * solarRadiPressurePerAU * (AstroUnit * AstroUnit) * d / Math.Pow(d.Norm(), 3));
}
static double Norm(Geo.Algorithm.Vector a)
{
return(a.Norm());
}
/// <summary>
/// 计算大气密度，基于Harris-Priesterm模型。
/// Computes the atmospheric density for the modified Harris-Priester model.
/// </summary>
/// <param name="Mjd_TT">Mjd_TT Terrestrial Time (Modified Julian Date)</param>
/// <param name="xyz">r_tod Satellite position vector in the inertial system [m]</param>
/// <returns> Density [kg/m^3]</returns>
public static double AtmosDensity_HP(double Mjd_TT, Geo.Algorithm.Vector xyz)
{
// Constants
const double upper_limit = 1000.0; // Upper height limit [km]
const double lower_limit = 100.0; // Lower height limit [km]
const double ra_lag = 0.523599; // Right ascension lag [rad]
const int n_prm = 3; // Harris-Priester parameter
// 2(6) low(high) inclination
// Harris-Priester atmospheric density model parameters
// Height [km], minimum density, maximum density [gm/km^3]
const int N_Coef = 50;
Geo.Algorithm.Vector h = new Geo.Algorithm.Vector(Data_h, N_Coef);
Geo.Algorithm.Vector c_min = new Geo.Algorithm.Vector(Data_c_min, N_Coef);
Geo.Algorithm.Vector c_max = new Geo.Algorithm.Vector(Data_c_max, N_Coef);
// Variables
int i, ih; // Height section variables
double height; // Earth flattening
double dec_Sun, ra_Sun, c_dec; // Sun declination, right asc.
double c_psi2; // Harris-Priester modification
double density, h_min, h_max, d_min, d_max; // Height, density parameters
Geo.Algorithm.Vector r_Sun = new Geo.Algorithm.Vector(3); // Sun position
Geo.Algorithm.Vector u = new Geo.Algorithm.Vector(3); // Apex of diurnal bulge
// Satellite height
var geoCoord = Geo.Coordinates.CoordTransformer.XyzToGeoCoord(new Geo.Coordinates.XYZ(xyz[0], xyz[1], xyz[2]));
height = geoCoord.Height / 1000.0; // [km]
// Exit with zero density outside height model limits
if (height >= upper_limit || height <= lower_limit)
{
return(0.0);
}
// Sun right ascension, declination
r_Sun = CelestialUtil.Sun(Mjd_TT);
ra_Sun = Math.Atan2(r_Sun[1], r_Sun[0]);
dec_Sun = Math.Atan2(r_Sun[2], Math.Sqrt(Math.Pow(r_Sun[0], 2) + Math.Pow(r_Sun[1], 2)));
// Unit vector u towards the apex of the diurnal bulge in inertial geocentric coordinates
c_dec = cos(dec_Sun);
u[0] = c_dec * cos(ra_Sun + ra_lag);
u[1] = c_dec * sin(ra_Sun + ra_lag);
u[2] = sin(dec_Sun);
// Cosine of half angle between satellite position vector and apex of diurnal bulge
c_psi2 = 0.5 + 0.5 * xyz.Dot(u) / xyz.Norm();
// Height index search and exponential density interpolation
ih = 0; // section index reset
for (i = 0; i <= N_Coef - 1; i++) // loop over N_Coef height regimes
{
if (height >= h[i] && height < h[i + 1])
{
ih = i; break;
} // ih identifies height section
}
h_min = (h[ih] - h[ih + 1]) / Math.Log(c_min[ih + 1] / c_min[ih]);
h_max = (h[ih] - h[ih + 1]) / Math.Log(c_max[ih + 1] / c_max[ih]);
d_min = c_min[ih] * Math.Exp((h[ih] - height) / h_min);
d_max = c_max[ih] * Math.Exp((h[ih] - height) / h_max);
// Density computation
density = d_min + (d_max - d_min) * Math.Pow(c_psi2, n_prm);
return(density * 1.0e-12); // [kg/m^3]
}
