} // findc2c3
/// <summary>
/// Determine the time of flight in physics time units (GE internal time) that it takes for the body
/// in orbit to go from position r0 to position r1 in an orbit with parameter p.
///
/// The angle between two 3D vectors cannot be greater than 180 degrees unless the orientation of the
/// plane they define is also specified. The normal parameter is used for this. If an angle less than
/// 180 is desired then the cross product of r0 and v0 can be used as the normal.
/// Calling TOF via the OrbitUniversal wrapper will handle all that automatically.
///
/// </summary>
/// <param name="r0">from point (with respect to center)</param>
/// <param name="r1">to point (with respect to center)</param>
/// <param name="p">orbit semi-parameter</param>
/// <param name="mu">centerbody mass</param>
/// <param name="normal">normal to orital plane</param>
/// <returns>time to travel from r0 to r1 in GE time</returns>
public static double TimeOfFlight(Vector3d r0, Vector3d r1, double p, double mu, Vector3d normal)
{
// Vallado, Algorithm 11, p126
double tof = 0;
double r0r1 = r0.magnitude * r1.magnitude;
double cos_dnu = Vector3d.Dot(r0, r1) / r0r1;
double sin_dnu = Vector3d.Cross(r0, r1).magnitude / r0r1;
// use the normal to determine if angle is > 180
if (Vector3d.Dot(Vector3d.Cross(r0, r1), normal) < 0.0)
{
sin_dnu *= -1.0;
}
// GE - precision issue at 180 degrees. Simply return 1/2 the orbit period.
if (Mathd.Abs(1.0 + cos_dnu) < 1E-5)
{
double a180 = 0.5f * (r0.magnitude + r1.magnitude);
return(Mathd.Sqrt(a180 * a180 * a180 / mu) * Mathd.PI);
}
// sin_nu: Need to use direction of flight to pick sign per Algorithm 53
double k = r0r1 * (1.0 - cos_dnu);
double l = r0.magnitude + r1.magnitude;
double m = r0r1 * (1 + cos_dnu);
double a = (m * k * p) / ((2.0 * m - l * l) * p * p + 2.0 * k * l * p - k * k);
double f = 1.0 - (r1.magnitude / p) * (1.0 - cos_dnu);
double g = r0r1 * sin_dnu / (Mathd.Sqrt(mu * p));
double alpha = 1 / a;
if (alpha > 1E-7)
{
// ellipse
double delta_nu = Mathd.Atan2(sin_dnu, cos_dnu);
double fdot = Mathd.Sqrt(mu / p) * Mathd.Tan(0.5 * delta_nu) *
((1 - cos_dnu) / p - (1 / r0.magnitude) - (1.0 / r1.magnitude));
double cos_deltaE = 1 - r0.magnitude / a * (1.0 - f);
double sin_deltaE = -r0r1 * fdot / (Mathd.Sqrt(mu * a));
double deltaE = Mathd.Atan2(sin_deltaE, cos_deltaE);
tof = g + Mathd.Sqrt(a * a * a / mu) * (deltaE - sin_deltaE);
}
else if (alpha < -1E-7)
{
// hyperbola
double cosh_deltaH = 1.0 + (f - 1.0) * r0.magnitude / a;
double deltaH = GEMath.Acosh(cosh_deltaH);
tof = g + Mathd.Sqrt(-a * a * a / mu) * (GEMath.Sinh(deltaH) - deltaH);
}
else
{
// parabola
double c = Mathd.Sqrt(r0.magnitude * r0.magnitude + r1.magnitude * r1.magnitude - 2.0 * r0r1 * cos_dnu);
double s = 0.5 * (r0.magnitude + r1.magnitude + c);
tof = 2 / 3 * Mathd.Sqrt(s * s * s / (2.0 * mu)) * (1 - Mathd.Pow(((s - c) / s), 1.5));
}
return(tof);
}
/* -----------------------------------------------------------------------------
*
* function newtonnu
*
* this function solves keplers equation when the true anomaly is known.
* the mean and eccentric, parabolic, or hyperbolic anomaly is also found.
* the parabolic limit at 168ΓΈ is arbitrary. the hyperbolic anomaly is also
* limited. the hyperbolic sine is used because it's not double valued.
*
* author : david vallado 719-573-2600 27 may 2002
*
* revisions
* vallado - fix small 24 sep 2002
*
* inputs description range / units
* ecc - eccentricity 0.0 to
* nu - true anomaly -2pi to 2pi rad
*
* outputs :
* e0 - eccentric anomaly 0.0 to 2pi rad 153.02 deg
* m - mean anomaly 0.0 to 2pi rad 151.7425 deg
*
* locals :
* e1 - eccentric anomaly, next value rad
* sine - sine of e
* cose - cosine of e
* ktr - index
*
* coupling :
* arcsinh - arc hyperbolic sine
* sinh - hyperbolic sine
*
* references :
* vallado 2013, 77, alg 5
* --------------------------------------------------------------------------- */
private static void NewtonNu(OrbitElements oe)
{
double small, sine, cose, cosnu, temp;
double ecc = oe.ecc;
double nu = oe.nu;
double e0, m;
// --------------------- implementation ---------------------
e0 = 999999.9;
m = 999999.9;
small = 0.00000001;
// --------------------------- circular ------------------------
if (Mathd.Abs(ecc) < small)
{
m = nu;
e0 = nu;
}
else
// ---------------------- elliptical -----------------------
if (ecc < 1.0 - small)
{
cosnu = Mathd.Cos(nu);
temp = 1.0 / (1.0 + ecc * cosnu);
sine = (Mathd.Sqrt(1.0 - ecc * ecc) * Mathd.Sin(nu)) * temp;
cose = (ecc + cosnu) * temp;
e0 = Mathd.Atan2(sine, cose);
m = e0 - ecc * Mathd.Sin(e0);
}
else
// -------------------- hyperbolic --------------------
if (ecc > 1.0 + small)
{
if ((ecc > 1.0) && (Mathd.Abs(nu) + 0.00001 < Mathd.PI - Mathd.Acos(Mathd.Clamp(1.0 / ecc, -1.0, 1.0))))
{
sine = (Mathd.Sqrt(ecc * ecc - 1.0) * Mathd.Sin(nu)) / (1.0 + ecc * Mathd.Cos(nu));
e0 = GEMath.Asinh(sine);
m = ecc * GEMath.Sinh(e0) - e0;
}
}
else
// ----------------- parabolic ---------------------
if (Mathd.Acos(Mathd.Clamp(nu, -1.0, 1.0)) < 168.0 * Mathd.PI / 180.0)
{
e0 = Mathd.Tan(nu * 0.5);
m = e0 + (e0 * e0 * e0) / 3.0;
}
if (ecc < 1.0)
{
m = System.Math.Truncate(m / (2.0 * Mathd.PI));
if (m < 0.0)
{
m = m + 2.0 * Mathd.PI;
}
e0 = System.Math.Truncate(e0 / (2.0 * Mathd.PI));
}
oe.m = m;
oe.eccanom = e0;
} // newtonnu
} // coe2rv
public static void COEtoRVMirror(OrbitElements oe,
NBody centerBody,
ref Vector3d r,
ref Vector3d v,
bool relativePos)
{
double temp, sinnu, cosnu;
Vector3d rpqw, vpqw;
double mu = GravityEngine.Instance().GetMass(centerBody);
// -------------------- implementation ----------------------
// determine what type of orbit is involved and set up the
// set up angles for the special cases.
// -------------------------------------------------------------
if (oe.ecc < small)
{
// ---------------- circular equatorial ------------------
if ((oe.incl < small) | (Mathd.Abs(oe.incl - Mathd.PI) < small))
{
oe.argp = 0.0;
oe.raan = 0.0;
oe.nu = oe.truelon;
}
else
{
// -------------- circular inclined ------------------
oe.argp = 0.0;
oe.nu = oe.arglat;
}
}
else
{
// --------------- elliptical equatorial -----------------
if ((oe.incl < small) | (Mathd.Abs(oe.incl - Mathd.PI) < small))
{
oe.argp = oe.lonper;
oe.raan = 0.0;
}
}
// ---------- form pqw position and velocity vectors ----------
cosnu = Mathd.Cos(oe.nu);
sinnu = Mathd.Sin(oe.nu);
temp = oe.p / (1.0 + oe.ecc * cosnu);
// flip Y
rpqw = new Vector3d(temp * cosnu, -temp * sinnu, 0.0);
if (Mathd.Abs(oe.p) < 0.00000001)
{
oe.p = 0.00000001;
}
// flip X (not Y)
vpqw = new Vector3d(sinnu * Mathd.Sqrt(mu / oe.p),
(oe.ecc + cosnu) * Mathd.Sqrt(mu / oe.p),
0.0);
// ---------------- perform transformation to ijk ------------
r = GEMath.Rot3(rpqw, -oe.argp);
r = GEMath.Rot1(r, -oe.incl);
r = GEMath.Rot3(r, -oe.raan);
v = GEMath.Rot3(vpqw, -oe.argp);
v = GEMath.Rot1(v, -oe.incl);
v = GEMath.Rot3(v, -oe.raan);
if (!relativePos)
{
r += GravityEngine.Instance().GetPositionDoubleV3(centerBody);
v += GravityEngine.Instance().GetVelocityDoubleV3(centerBody);
}
} // coe2rvMirror
