/// <summary>
/// Gets the True anomaly value from current distance from the focus (attractor).
/// </summary>
/// <param name="distance">The distance from attractor.</param>
/// <param name="eccentricity">The eccentricity.</param>
/// <param name="semiMajorAxis">The semi major axis.</param>
/// <returns>True anomaly in radians.</returns>
public static double CalcTrueAnomalyForDistance(double distance, double eccentricity, double semiMajorAxis)
{
if (eccentricity < 1)
{
var arg = (semiMajorAxis * (1d - eccentricity * eccentricity) - distance) / (distance * eccentricity);
if (arg < -1 || arg > 1)
{
return(Mathd.PI);
}
else
{
return(Mathd.Acos(arg));
}
}
else
{
var arg = (semiMajorAxis * (eccentricity * eccentricity - 1d) - distance) / (distance * eccentricity);
if (arg < -1 || arg > 1)
{
return(Mathd.PI);
}
else
{
return(Mathd.Acos(arg));
}
}
}
/// <summary>
/// Gets the descending node of orbit.
/// </summary>
/// <param name="desc">The desc.</param>
/// <returns><c>true</c> if descending node exists, otherwise <c>false</c></returns>
public bool GetDescendingNode(out Vector3d desc)
{
var norm = CelestialBodyUtils.CrossProduct(OrbitNormal, EclipticNormal);
var s = CelestialBodyUtils.DotProduct(CelestialBodyUtils.CrossProduct(norm, SemiMajorAxisBasis), OrbitNormal) < 0;
var ecc = 0d;
var trueAnom = Vector3d.Angle(norm, -CenterPoint) * Mathd.Deg2Rad;
if (Eccentricity < 1)
{
var cosT = Math.Cos(trueAnom);
ecc = Math.Acos((Eccentricity + cosT) / (1d + Eccentricity * cosT));
if (s)
{
ecc = Mathd.PI_2 - ecc;
}
}
else
{
trueAnom = Vector3d.Angle(norm, CenterPoint) * Mathd.Deg2Rad;
if (trueAnom >= Mathd.Acos(-1d / Eccentricity))
{
desc = new Vector3d();
return(false);
}
var cosT = Math.Cos(trueAnom);
ecc = CelestialBodyUtils.Acosh((Eccentricity + cosT) / (1 + Eccentricity * cosT)) * (s ? -1 : 1);
}
desc = GetFocalPositionAtEccentricAnomaly(ecc);
return(true);
}
public bool GetAscendingNode(out Vector3d asc)
{
var norm = CelestialBodyUtils.CrossProduct(orbitNormal, eclipticNormal);
var s = CelestialBodyUtils.DotProduct(CelestialBodyUtils.CrossProduct(norm, semiMajorAxisBasis), orbitNormal) < 0;
var ecc = 0d;
var trueAnom = Vector3d.Angle(norm, centerPoint) * Mathd.Deg2Rad;
if (eccentricity < 1)
{
var cosT = System.Math.Cos(trueAnom);
ecc = System.Math.Acos((eccentricity + cosT) / (1d + eccentricity * cosT));
if (!s)
{
ecc = Mathd.PI_2 - ecc;
}
}
else
{
trueAnom = Vector3d.Angle(-norm, centerPoint) * Mathd.Deg2Rad;
if (trueAnom >= Mathd.Acos(-1d / eccentricity))
{
asc = new Vector3d();
return(false);
}
var cosT = System.Math.Cos(trueAnom);
ecc = CelestialBodyUtils.Acosh((eccentricity + cosT) / (1 + eccentricity * cosT)) * (!s ? -1 : 1);
}
asc = GetFocalPositionAtEccentricAnomaly(ecc);
return(true);
}
/// <summary>
/// <para>Returns the signed angle in degrees between from and to.</para>
/// </summary>
/// <param name="from">The vector from which the angular difference is measured.</param>
/// <param name="to">The vector to which the angular difference is measured.</param>
/// <param name="axis">A vector around which the other vectors are rotated.</param>
public static double SignedAngle(Vector3d from, Vector3d to, Vector3d axis)
{
Vector3d normalized = from.normalized;
Vector3d normalized2 = to.normalized;
double num = Mathd.Acos(Mathd.Clamp(Vector3d.Dot(normalized, normalized2), -1f, 1f)) * 57.29578f;
double num2 = Mathd.Sign(Vector3d.Dot(axis, Vector3d.Cross(normalized, normalized2)));
return(num * num2);
}
public static double SignedAngle(Vector2_ from, Vector2_ to)
{
var nf = from.normalized;
var nt = to.normalized;
var num = Mathd.Acos(Mathd.Clamp(Dot(nf, nt), -1, 1)) * 57.29578;
var num2 = Mathd.Sign(nf.x * nt.y - nf.y * nt.x);
return(num * num2);
}
public static double CalcTrueAnomalyForDistance(double distance, double eccentricity, double semiMajorAxis)
{
if (eccentricity < 1)
{
return(Mathd.Acos((semiMajorAxis * (1d - eccentricity * eccentricity) - distance) / (distance * eccentricity)));
}
else
{
return(Mathd.Acos((semiMajorAxis * (eccentricity * eccentricity - 1d) - distance) / (distance * eccentricity)));
}
}
public bool IsAboveHorizon(Vector3d camera, double dist)
{
return(true);
Vector3d planetToCam = (camera - planet.position) / dist;
double horizonAngle = Mathd.Acos((planet.radius * .5) / dist);
double meshAngle;
foreach (Vector3d v in vertexSamples)
{
meshAngle = Mathd.Acos(Vector3d.Dot(planetToCam, v.normalized));
if (horizonAngle > meshAngle)
{
return(true);
}
}
return(false);
}
public static OrbitElements RVtoCOE(Vector3d r_in,
Vector3d v_in,
NBody centerBody,
bool relativePos)
{
double magr, magv, magn, sme, rdotv, temp, c1, hk, magh;
Vector3d r = r_in;
Vector3d v = v_in;
double mu = GravityEngine.Instance().GetMass(centerBody);
if (!relativePos)
{
r = r - GravityEngine.Instance().GetPositionDoubleV3(centerBody);
v = v - GravityEngine.Instance().GetVelocityDoubleV3(centerBody);
}
OrbitElements oe = new OrbitElements();
oe.eccanom = 0.0;
// ------------------------- implementation -----------------
magr = r.magnitude;
magv = v.magnitude;
// ------------------ find h n and e vectors ----------------
Vector3d hbar = Vector3d.Cross(r, v);
magh = hbar.magnitude;
if (magh > small)
{
Vector3d nbar = new Vector3d(-hbar.y, hbar.x, 0.0);
magn = nbar.magnitude;
c1 = magv * magv - mu / magr;
rdotv = Vector3d.Dot(r, v);
temp = 1.0 / mu;
Vector3d ebar = new Vector3d((c1 * r.x - rdotv * v.x) * temp,
(c1 * r.y - rdotv * v.y) * temp,
(c1 * r.z - rdotv * v.z) * temp);
oe.ecc_vec = ebar;
oe.ecc = ebar.magnitude;
// ------------ find a e and semi-latus rectum ----------
sme = (magv * magv * 0.5) - (mu / magr);
if (Mathd.Abs(sme) > small)
{
oe.a = -mu / (2.0 * sme);
}
else
{
oe.a = double.NaN;
}
oe.p = magh * magh * temp;
// ----------------- find inclination -------------------
hk = hbar.z / magh;
oe.incl = Mathd.Acos(Mathd.Clamp(hk, -1.0, 1.0));
oe.typeOrbit = OrbitElements.TypeOrbit.ELLIPTICAL_INCLINED;
if (oe.ecc < small)
{
// ---------------- circular equatorial ---------------
if ((oe.incl < small) || (Mathd.Abs(oe.incl - Mathd.PI) < small))
{
oe.typeOrbit = OrbitElements.TypeOrbit.CIRCULAR_EQUATORIAL;
}
else
{
oe.typeOrbit = OrbitElements.TypeOrbit.CIRCULAR_INCLINED;
}
}
else
{
// - elliptical, parabolic, hyperbolic equatorial --
if ((oe.incl < small) || (Mathd.Abs(oe.incl - Mathd.PI) < small))
{
oe.typeOrbit = OrbitElements.TypeOrbit.ELLIPTICAL_EQUATORIAL;
}
}
// ---------- find right ascension of the ascending node ------------
if (magn > small)
{
temp = nbar.x / magn;
if (Mathd.Abs(temp) > 1.0)
{
temp = Mathd.Sign(temp);
}
oe.raan = Mathd.Acos(Mathd.Clamp(temp, -1.0, 1.0));
if (nbar.y < 0.0)
{
oe.raan = 2.0 * Mathd.PI - oe.raan;
}
}
else
{
oe.raan = double.NaN;
}
// ---------------- find argument of perigee ---------------
if (oe.typeOrbit == OrbitElements.TypeOrbit.ELLIPTICAL_INCLINED)
{
oe.argp = Vector3d.Angle(nbar, ebar) * Mathd.Deg2Rad;;
if (ebar.z < 0.0)
{
oe.argp = 2.0 * Mathd.PI - oe.argp;
}
}
else
{
oe.argp = double.NaN;
}
// ------------ find true anomaly at epoch -------------
if (!oe.IsCircular())
{
oe.nu = Vector3d.Angle(ebar, r) * Mathd.Deg2Rad;
if (rdotv < 0.0)
{
oe.nu = 2.0 * Mathd.PI - oe.nu;
}
}
else
{
oe.nu = double.NaN;
}
// ---- find argument of latitude - circular inclined -----
if (oe.typeOrbit == OrbitElements.TypeOrbit.CIRCULAR_INCLINED)
{
oe.arglat = Vector3d.Angle(nbar, r) * Mathd.Deg2Rad;;
if (r.z < 0.0)
{
oe.arglat = 2.0 * Mathd.PI - oe.arglat;
}
oe.m = oe.arglat;
}
else
{
oe.arglat = double.NaN;
}
// -- find longitude of perigee - elliptical equatorial ----
if ((oe.ecc > small) && (oe.typeOrbit == OrbitElements.TypeOrbit.ELLIPTICAL_EQUATORIAL))
{
temp = ebar.x / oe.ecc;
if (Mathd.Abs(temp) > 1.0)
{
temp = Mathd.Sign(temp);
}
oe.lonper = Mathd.Acos(Mathd.Clamp(temp, -1.0, 1.0));
if (ebar.y < 0.0)
{
oe.lonper = 2.0 * Mathd.PI - oe.lonper;
}
if (oe.incl > 0.5 * Mathd.PI)
{
oe.lonper = 2.0 * Mathd.PI - oe.lonper;
}
}
else
{
oe.lonper = double.NaN;
}
// -------- find true longitude - circular equatorial ------
if ((magr > small) && (oe.typeOrbit == OrbitElements.TypeOrbit.CIRCULAR_EQUATORIAL))
{
temp = r.x / magr;
if (Mathd.Abs(temp) > 1.0)
{
temp = Mathd.Sign(temp);
}
oe.truelon = Mathd.Acos(Mathd.Clamp(temp, -1.0, 1.0));
if (r.y < 0.0)
{
oe.truelon = 2.0 * Mathd.PI - oe.truelon;
}
if (oe.incl > 0.5 * Mathd.PI)
{
oe.truelon = 2.0 * Mathd.PI - oe.truelon;
}
oe.m = oe.truelon;
}
else
{
oe.truelon = double.NaN;
}
// ------------ find mean anomaly for all orbits -----------
if (!oe.IsCircular())
{
NewtonNu(oe);
}
}
else
{
oe.p = double.NaN;
oe.a = double.NaN;
oe.ecc = double.NaN;
oe.incl = double.NaN;
oe.raan = double.NaN;
oe.argp = double.NaN;
oe.nu = double.NaN;
oe.m = double.NaN;
oe.arglat = double.NaN;
oe.truelon = double.NaN;
oe.lonper = double.NaN;
}
return(oe);
} // rv2coe
/// <summary>
/// <para>Returns the angle in degrees between two rotations a and b.</para>
/// </summary>
/// <param name="a"></param>
/// <param name="b"></param>
public static double Angle(Quaterniond a, Quaterniond b)
{
double f = Quaterniond.Dot(a, b);
return(Mathd.Acos(Mathd.Min(Mathd.Abs(f), 1f)) * 2f * 57.29578f);
}
public static double Angle(Quaterniond _a, Quaterniond _b)
{
double f = Dot(_a, _b);
return(Mathd.Acos(Mathd.Min(Mathd.Abs(f), 1.0f)) * 2.0f * 57.295788f);
}
public int ComputeXfer(
// r1, r2, v1 set by constructor
bool reverse, // was dm 'l' or 's'
bool df, // 'r' for retro case "alt approach for high energy(long way, retro multi - rev) case"
int nrev,
double dtsec
)
{
const double small = 0.000001;
Vector3d rcrossr;
int loops;
double u, b, x, xn, y, L, m, cosdeltanu, sindeltanu, dnu, a,
ror, h1, h2, tempx, eps, denom, chord, k2, s,
p, ecc, f, A;
double magr1, magr2, magrcrossr, lam, temp, temp1, temp2;
y = 0;
int error = 0; // PM - added an error code for loop not converging. Seems Vallado did not implement any?
magr1 = r1.magnitude;
magr2 = r2.magnitude;
cosdeltanu = Vector3d.Dot(r1, r2) / (magr1 * magr2);
// make sure it's not more than 1.0
if (Mathd.Abs(cosdeltanu) > 1.0)
{
cosdeltanu = 1.0 * Mathd.Sign(cosdeltanu);
}
rcrossr = Vector3d.Cross(r1, r2);
magrcrossr = rcrossr.magnitude;
if (!reverse)
{
sindeltanu = magrcrossr / (magr1 * magr2);
}
else
{
sindeltanu = -magrcrossr / (magr1 * magr2);
}
dnu = Mathd.Atan2(sindeltanu, cosdeltanu);
// the angle needs to be positive to work for the long way
if (dnu < 0.0)
{
dnu = 2.0 * pi + dnu;
}
// these are the same
//chord = Mathd.Sqrt(magr1 * magr1 + magr2 * magr2 - 2.0 * magr1 * magr2 * cosdeltanu);
chord = (r2 - r1).magnitude;
s = (magr1 + magr2 + chord) * 0.5;
ror = magr2 / magr1;
eps = ror - 1.0;
lam = 1.0 / s * Mathd.Sqrt(magr1 * magr2) * Mathd.Cos(dnu * 0.5);
L = Mathd.Pow((1.0 - lam) / (1.0 + lam), 2);
m = 8.0 * mu * dtsec * dtsec / (s * s * s * Mathd.Pow(1.0 + lam, 6));
// initial guess
if (nrev > 0)
{
xn = 1.0 + 4.0 * L;
}
else
{
xn = L; //l // 0.0 for par and hyp, l for ell
}
// alt approach for high energy(long way, retro multi - rev) case
if (df && (nrev > 0))
{
xn = 1e-20; // be sure to reset this here!!
x = 10.0; // starting value
loops = 1;
while ((Mathd.Abs(xn - x) >= small) && (loops <= 20))
{
x = xn;
temp = 1.0 / (2.0 * (L - x * x));
temp1 = Mathd.Sqrt(x);
temp2 = (nrev * pi * 0.5 + Mathd.Atan(temp1)) / temp1;
h1 = temp * (L + x) * (1.0 + 2.0 * x + L);
h2 = temp * m * temp1 * ((L - x * x) * temp2 - (L + x));
b = 0.25 * 27.0 * h2 / (Mathd.Pow(temp1 * (1.0 + h1), 3));
if (b < -1.0) // reset the initial condition
{
f = 2.0 * Mathd.Cos(1.0 / 3.0 * Mathd.Acos(Mathd.Sqrt(b + 1.0)));
}
else
{
A = Mathd.Pow(Mathd.Sqrt(b) + Mathd.Sqrt(b + 1.0), (1.0 / 3.0));
f = A + 1.0 / A;
}
y = 2.0 / 3.0 * temp1 * (1.0 + h1) * (Mathd.Sqrt(b + 1.0) / f + 1.0);
xn = 0.5 * ((m / (y * y) - (1.0 + L)) - Mathd.Sqrt(Mathd.Pow(m / (y * y) - (1.0 + L), 2) - 4.0 * L));
// fprintf(outfile, " %3i yh %11.6f x %11.6f h1 %11.6f h2 %11.6f b %11.6f f %11.7f \n", loops, y, x, h1, h2, b, f);
loops = loops + 1;
} // while
x = xn;
a = s * Mathd.Pow(1.0 + lam, 2) * (1.0 + x) * (L + x) / (8.0 * x);
p = (2.0 * magr1 * magr2 * (1.0 + x) * Mathd.Pow(Mathd.Sin(dnu * 0.5), 2)) / (s * Mathd.Pow(1.0 + lam, 2) * (L + x)); // thompson
ecc = Mathd.Sqrt(1.0 - p / a);
LambHodograph(p, ecc, dnu, dtsec);
// fprintf(outfile, "high v1t %16.8f %16.8f %16.8f %16.8f\n", v1t, astMath::mag(v1t));
}
else
{
// standard processing
// note that the dr nrev = 0 case is not represented
loops = 1;
x = 10.0; // starting value
while ((Mathd.Abs(xn - x) >= small) && (loops <= 30))
{
if (nrev > 0)
{
x = xn;
temp = 1.0 / ((1.0 + 2.0 * x + L) * (4.0 * x));
temp1 = (nrev * pi * 0.5 + Mathd.Atan(Mathd.Sqrt(x))) / Mathd.Sqrt(x);
h1 = temp * Mathd.Pow(L + x, 2) * (3.0 * Mathd.Pow(1.0 + x, 2) * temp1 - (3.0 + 5.0 * x));
h2 = temp * m * ((x * x - x * (1.0 + L) - 3.0 * L) * temp1 + (3.0 * L + x));
}
else
{
x = xn;
tempx = SeeBattin(x);
denom = 1.0 / ((1.0 + 2.0 * x + L) * (4.0 * x + tempx * (3.0 + x)));
h1 = Mathd.Pow(L + x, 2) * (1.0 + 3.0 * x + tempx) * denom;
h2 = m * (x - L + tempx) * denom;
}
// ---------------------- - evaluate cubic------------------
b = 0.25 * 27.0 * h2 / (Mathd.Pow(1.0 + h1, 3));
u = 0.5 * b / (1.0 + Mathd.Sqrt(1.0 + b));
k2 = KBattin(u);
y = ((1.0 + h1) / 3.0) * (2.0 + Mathd.Sqrt(1.0 + b) / (1.0 + 2.0 * u * k2 * k2));
xn = Mathd.Sqrt(Mathd.Pow((1.0 - L) * 0.5, 2) + m / (y * y)) - (1.0 + L) * 0.5;
loops = loops + 1;
} // while
}
if (loops < 30)
{
// blair approach use y from solution
// lam = 1.0 / s * sqrt(magr1*magr2) * cos(dnu*0.5);
// m = 8.0*mu*dtsec*dtsec / (s ^ 3 * (1.0 + lam) ^ 6);
// L = ((1.0 - lam) / (1.0 + lam)) ^ 2;
//a = s*(1.0 + lam) ^ 2 * (1.0 + x)*(lam + x) / (8.0*x);
// p = (2.0*magr1*magr2*(1.0 + x)*sin(dnu*0.5) ^ 2) ^ 2 / (s*(1 + lam) ^ 2 * (lam + x)); % loechler, not right ?
p = (2.0 * magr1 * magr2 * y * y * Mathd.Pow(1.0 + x, 2) * Mathd.Pow(Mathd.Sin(dnu * 0.5), 2)) /
(m * s * Mathd.Pow(1.0 + lam, 2)); // thompson
ecc = Mathd.Sqrt((eps * eps + 4.0 * magr2 / magr1 * Mathd.Pow(Mathd.Sin(dnu * 0.5), 2) *
Mathd.Pow((L - x) / (L + x), 2)) / (eps * eps + 4.0 * magr2 / magr1 * Mathd.Pow(Mathd.Sin(dnu * 0.5), 2)));
LambHodograph(p, ecc, dnu, dtsec);
// Battin solution to orbital parameters(and velocities)
// thompson 2011, loechler 1988
if (dnu > pi)
{
lam = -Mathd.Sqrt((s - chord) / s);
}
else
{
lam = Mathd.Sqrt((s - chord) / s);
}
// loechler pg 21 seems correct!
Vector3d v1dvl = 1.0 / (lam * (1.0 + lam)) * Mathd.Sqrt(mu * (1.0 + x) / (2.0 * s * s * s * (L + x))) * ((r2 - r1)
+ s * Mathd.Pow(1.0 + lam, 2) * (L + x) / (magr1 * (1.0 + x)) * r1);
// added v2
Vector3d v2dvl = 1.0 / (lam * (1.0 + lam)) * Mathd.Sqrt(mu * (1.0 + x) / (2.0 * s * s * s * (L + x))) * ((r2 - r1)
- s * Mathd.Pow(1.0 + lam, 2) * (L + x) / (magr2 * (1.0 + x)) * r2);
// Seems these are the answer. Not at all sure what the point of the Hodograph calls was...
// but for FRGeneric the Hodograph answer seems ok. In that case lam = 0 and v1vdl is NaN. Wha??
// Add code to use hodograph if lam=0
//Debug.LogFormat("lam={0}", lam);
if (Mathd.Abs(lam) > 1E-8)
{
v1t = v1dvl;
v2t = v2dvl;
}
//fprintf(1, 'loe v1t %16.8f %16.8f %16.8f %16.8f\n', v1dvl, mag(v1dvl));
//fprintf(1, 'loe v2t %16.8f %16.8f %16.8f %16.8f\n', v2dvl, mag(v2dvl));
// Debug.LogFormat("v1dvl={0} v2dvl={1}", v1dvl, v2dvl);
}
else
{
error = 2;
}
// Determine maneuvers needed for ship (fromOrbit)
if (error == 0)
{
maneuvers.Clear();
// Departure
Maneuver departure = new Maneuver();
departure.mtype = Maneuver.Mtype.vector;
departure.nbody = fromNBody;
departure.physPosition = r1 + center3d;
if (innerToOuter)
{
departure.velChange = v1t.ToVector3() - GravityEngine.Instance().GetVelocity(fromNBody);
}
else
{
// Need to establish arrival velocity
departure.velChange = v2t.ToVector3() - GravityEngine.Instance().GetVelocity(fromNBody);
}
departure.worldTime = (float)GravityEngine.Instance().GetGETime();
maneuvers.Add(departure);
deltaV = departure.velChange.magnitude;
// Arrival (will not be required if intercept)
if (toOrbit != null)
{
Maneuver arrival = new Maneuver();
arrival.nbody = fromNBody;
arrival.physPosition = r2 + center3d;
arrival.worldTime = departure.worldTime + (float)dtsec;
arrival.mtype = Maneuver.Mtype.vector;
arrival.velChange = toOrbit.GetPhysicsVelocityForEllipse(toOrbit.phase) - v2t.ToVector3();
maneuvers.Add(arrival);
deltaV += arrival.velChange.magnitude;
}
}
return(error);
} // lambertbattin
public static double Angle(Vector2d from, Vector2d to)
{
return(Mathd.Acos(Mathd.Clamp(Vector2d.Dot(from.normalized, to.normalized), -1d, 1d)) * 57.29578d);
}
public CircNonPlanarRendezvous(OrbitData fromOrbit, OrbitData toOrbit) : base(fromOrbit, toOrbit)
{
name = "Circular Non-Planar Rendezvous";
double w_target = toOrbit.GetOmegaAngular();
double w_int = fromOrbit.GetOmegaAngular();
bool innerToOuter = (fromOrbit.a < toOrbit.a);
if (!innerToOuter)
{
Debug.LogError("Fix me: algorithm does not support outer to inner yet!");
}
float a_transfer = 0.5f * (fromOrbit.a + toOrbit.a);
float t_transfer = Mathf.PI * Mathf.Sqrt(a_transfer * a_transfer * a_transfer / fromOrbit.mu);
Debug.LogFormat("int: a={0} w={1} target: a={2} w={3}", fromOrbit.a, w_int, toOrbit.a, w_target);
// lead angle required by target
double alpha_L = w_target * t_transfer;
// find the phase of the nearest node in the interceptor orbit
// (C&P from CircularInclinationAndAN, messy to extract)
double dOmega = (toOrbit.omega_uc - fromOrbit.omega_uc) * Mathd.Deg2Rad;
double i_initial = fromOrbit.inclination * Mathd.Deg2Rad;
double i_final = toOrbit.inclination * Mathd.Deg2Rad;
// u_initial = omega_lc + nu (i.e. phase of circular orbit)
// eqn (6-25)
double cos_theta = Mathd.Cos(i_initial) * Mathd.Cos(i_final) +
Mathd.Sin(i_initial) * Mathd.Sin(i_final) * Mathd.Cos(dOmega);
// Quadrant check
double theta = Mathd.Acos(Mathd.Clamp(cos_theta, -1.0, 1.0));
if (dOmega < 0)
{
theta = -theta;
}
// u_initial: phase of intersection in the initial orbit
double numer = Mathd.Sin(i_final) * Mathd.Cos(dOmega) - cos_theta * Mathd.Sin(i_initial);
double denom = Mathd.Sin(theta) * Mathd.Cos(i_initial);
if (Mathd.Abs(denom) < 1E-6)
{
Debug.LogError("u_initial: about to divide by zero (small theta)");
return;
}
double u_initial = Mathd.Acos(Mathd.Clamp(numer / denom, -1.0, 1.0));
// u_final: phase of intersection in the final orbit
numer = Mathd.Cos(i_initial) * Mathd.Sin(i_final) - Mathd.Sin(i_initial) * Mathd.Cos(i_final) * Mathd.Cos(dOmega);
if (Mathd.Abs(Mathd.Sin(theta)) < 1E-6)
{
Debug.LogError("u_final: about to divide by zero (small theta)");
return;
}
double u_final = Mathd.Acos(Mathd.Clamp(numer / Mathd.Sin(theta), -1.0, 1.0));
// how far is interceptor from a node?
double delta_theta_int = u_initial - fromOrbit.phase * Mathd.Deg2Rad;
if (delta_theta_int < -Mathd.PI)
{
delta_theta_int += 2.0 * Mathd.PI;
u_initial += 2.0 * Mathd.PI;
u_final += 2.00 * Mathd.PI;
}
else if (delta_theta_int < 0)
{
delta_theta_int += Mathd.PI;
u_initial += Mathd.PI;
u_final += Mathd.PI;
}
double deltat_node = delta_theta_int / w_int;
Debug.LogFormat("Node at: {0} (deg), distance to node (deg)={1}", u_initial * Mathd.Rad2Deg, delta_theta_int * Mathd.Rad2Deg);
// Algorithm uses lambda_true. See the definition in Vallada (2-92). defined for circular equitorial
// orbits as angle from I-axis (x-axis) to the satellite.
// This is not the same as u = argument of latitude, which is measured from the ascending node.
// Target moves to lambda_tgt1 as interceptor moves to node
double lambda_tgt1 = toOrbit.phase * Mathd.Deg2Rad + w_target * deltat_node;
// phase lag from interceptor to target (target is 1/2 revolution from u_initial)
// Vallado uses the fact that node is at omega, which assumes destination orbit is equitorial?
double lambda_int = u_final + Mathd.PI; // Mathd.PI + fromOrbit.omega_uc;
double theta_new = lambda_int - lambda_tgt1;
if (theta_new < 0)
{
theta_new += 2.0 * Mathd.PI;
}
// This is not working. Why??
// double alpha_new = Mathd.PI + theta_new;
// Keep in 0..2 Pi (my addition)
double alpha_new = theta_new % (2.0 * Mathd.PI);
Debug.LogFormat("lambda_tgt1={0} theta_new={1} toOrbit.phase={2} t_node={3} w_target={4}",
lambda_tgt1 * Mathd.Rad2Deg, theta_new * Mathd.Rad2Deg, toOrbit.phase, deltat_node, w_target);
// k_target: number of revolutions in transfer orbit. Provided as input
// k_int: number of revs in phasing orbit. Want to ensure a_phase < a_target to not
// waste deltaV.
double mu = fromOrbit.mu;
double k_target = 0.0;
double two_pi_k_target = k_target * 2.0 * Mathd.PI;
double P_phase = (alpha_new - alpha_L + two_pi_k_target) / w_target;
while (P_phase < 0)
{
Debug.Log("Pphase < 0. Bumping k_target");
k_target += 1.0;
two_pi_k_target = k_target * 2.0 * Mathd.PI;
P_phase = (alpha_new - alpha_L + two_pi_k_target) / w_target;
}
double k_int = 1.0;
double two_pi_k_int = k_int * 2.0 * Mathd.PI;
double a_phase = Mathd.Pow(mu * (P_phase * P_phase / (two_pi_k_int * two_pi_k_int)), 1.0 / 3.0);
Debug.LogFormat("alpha_new={0} alpha_L={1} Pphase={2}", alpha_new * Mathd.Rad2Deg, alpha_L * Mathd.Rad2Deg, P_phase);
// if need a long time to phase do multiple phasing orbits
int loopCnt = 0;
while (innerToOuter && (a_phase > toOrbit.a))
{
Debug.Log("a_phase > toOrbit - add a lap");
k_int += 1.0;
two_pi_k_int = k_int * 2.0 * Mathd.PI;
a_phase = Mathd.Pow(mu * (P_phase * P_phase / (two_pi_k_int * two_pi_k_int)), 1.0 / 3.0);
if (loopCnt++ > 10)
{
break;
}
}
double t_phase = 2.0 * Mathd.PI * Mathd.Sqrt(a_phase * a_phase * a_phase / fromOrbit.mu);
// Book has Abs(). Why? Need to remove this otherwise some phase differences do not work
// Only take Cos of delta_incl (no sign issues)
double deltaV_phase, deltaV_trans1, deltaV_trans2;
double delta_incl = (toOrbit.inclination - fromOrbit.inclination) * Mathd.Deg2Rad;
if (innerToOuter)
{
deltaV_phase = Mathd.Sqrt(2.0 * mu / fromOrbit.a - mu / a_phase)
- Mathd.Sqrt(mu / fromOrbit.a);
deltaV_trans1 = Mathd.Sqrt(2.0 * mu / fromOrbit.a - mu / a_transfer)
- Mathd.Sqrt(2.0 * mu / fromOrbit.a - mu / a_phase);
deltaV_trans2 = Mathd.Sqrt(2.0 * mu / toOrbit.a - mu / a_transfer
+ mu / toOrbit.a
- 2.0 * Mathd.Sqrt(2.0 * mu / toOrbit.a - mu / a_transfer)
* Mathd.Sqrt(mu / toOrbit.a) * Mathd.Cos(delta_incl));
}
else
{
// FIX ME!! Get NaN, so need to review from first principles.
deltaV_phase = Mathd.Sqrt(mu / a_phase - 2.0 * mu / fromOrbit.a)
- Mathd.Sqrt(mu / fromOrbit.a);
deltaV_trans1 = Mathd.Sqrt(mu / a_transfer - 2.0 * mu / fromOrbit.a)
- Mathd.Sqrt(2.0 * mu / fromOrbit.a - mu / a_phase);
deltaV_trans2 = Mathd.Sqrt(mu / a_transfer - 2.0 * mu / toOrbit.a
+ mu / toOrbit.a
- 2.0 * Mathd.Sqrt(mu / a_transfer - 2.0 * mu / toOrbit.a)
* Mathd.Sqrt(mu / toOrbit.a) * Mathd.Cos(delta_incl));
}
Debug.LogFormat("T1: a_int={0} a_phase={1} a_tgt={2} dt={3} dV_phase={4} dv1={5} dv2={6}",
fromOrbit.a, a_phase, toOrbit.a,
deltat_node, deltaV_phase, deltaV_trans1, deltaV_trans2);
// phasing burn: in same plane as the orbit at the node, use scalar maneuver
double time_start = GravityEngine.Instance().GetPhysicalTimeDouble();
Maneuver m_phase = new Maneuver();
m_phase.mtype = Maneuver.Mtype.scalar;
m_phase.dV = (float)deltaV_phase;
m_phase.worldTime = (float)(time_start + deltat_node);
m_phase.nbody = fromOrbit.nbody;
m_phase.physPosition = new Vector3d(fromOrbit.GetPhysicsPositionforEllipse((float)(u_initial * Mathd.Rad2Deg)));
maneuvers.Add(m_phase);
// transfer burn - stay in initial orbit plane
Maneuver m_trans1 = new Maneuver();
m_trans1.mtype = Maneuver.Mtype.scalar;
m_trans1.dV = (float)deltaV_trans1;
m_trans1.worldTime = (float)(time_start + deltat_node + t_phase);
m_trans1.nbody = fromOrbit.nbody;
m_trans1.physPosition = m_phase.physPosition;
maneuvers.Add(m_trans1);
// Arrival burn - do plane change here (just assign the correct velocity)
// TODO: Need to reverse this when from outer to inner...do plane change at start
float finalPhase = (float)(u_final * Mathd.Rad2Deg + 180f);
Vector3 finalV = toOrbit.GetPhysicsVelocityForEllipse(finalPhase);
Vector3 finalPos = toOrbit.GetPhysicsPositionforEllipse(finalPhase);
Maneuver m_trans2 = new Maneuver();
m_trans2.mtype = Maneuver.Mtype.setv;
m_trans2.dV = (float)deltaV_trans2;
m_trans2.velChange = finalV;
m_trans2.worldTime = (float)(time_start + deltat_node + t_phase + t_transfer);
m_trans2.nbody = fromOrbit.nbody;
m_trans2.physPosition = new Vector3d(finalPos);
maneuvers.Add(m_trans2);
}
/// <summary>
/// Returns the angle in degrees between from and to.
/// </summary>
/// <param name="from">The vector from which the angular difference is measured.</param>
/// <param name="to">The vector to which the angular difference is measured.</param>
public static double Angle(Vector3_ from, Vector3_ to)
{
return(Mathd.Acos(Mathd.Clamp(Dot(from.normalized, to.normalized), -1, 1)) * 57.29578);
}
public static double AngleBetween(Vector3D from, Vector3D to)
{
return(Mathd.Acos(Mathd.Clamp(Vector3D.Dot(from.normalized, to.normalized), -1d, 1d)));
}
/* -----------------------------------------------------------------------------
*
* 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
public CircularInclinationAndAN(OrbitData fromOrbit, OrbitData toOrbit) : base(fromOrbit, toOrbit)
{
name = "Circular Change Inclination and Ascending Node";
// check the orbits are circular and have the same radius
if (fromOrbit.ecc > GEConst.small)
{
Debug.LogWarning("fromOrbit is not circular. ecc=" + fromOrbit.ecc);
return;
}
if (toOrbit.ecc > GEConst.small)
{
Debug.LogWarning("toOrbit is not circular. ecc=" + toOrbit.ecc);
return;
}
if (Mathf.Abs(fromOrbit.a - toOrbit.a) > GEConst.small)
{
Debug.LogWarning("Orbits do not have the same radius delta=" + Mathf.Abs(fromOrbit.a - toOrbit.a));
return;
}
double dOmega = (toOrbit.omega_uc - fromOrbit.omega_uc) * Mathd.Deg2Rad;
double i_initial = fromOrbit.inclination * Mathd.Deg2Rad;
double i_final = toOrbit.inclination * Mathd.Deg2Rad;
// u_initial = omega_lc + nu (i.e. phase of circular orbit)
// eqn (6-25)
double cos_theta = Mathd.Cos(i_initial) * Mathd.Cos(i_final) +
Mathd.Sin(i_initial) * Mathd.Sin(i_final) * Mathd.Cos(dOmega);
// Quadrant check
double theta = Mathd.Acos(Mathd.Clamp(cos_theta, -1.0, 1.0));
if (dOmega < 0)
{
theta = -theta;
}
// u_initial: phase of intersection in the initial orbit
double numer = Mathd.Sin(i_final) * Mathd.Cos(dOmega) - cos_theta * Mathd.Sin(i_initial);
double denom = Mathd.Sin(theta) * Mathd.Cos(i_initial);
if (Mathd.Abs(denom) < 1E-6)
{
Debug.LogError("u_initial: about to divide by zero (small theta)");
// return;
}
double u_initial = Mathd.Acos(Mathd.Clamp(numer / denom, -1.0, 1.0));
// u_final: phase of intersection in the final orbit
numer = Mathd.Cos(i_initial) * Mathd.Sin(i_final) - Mathd.Sin(i_initial) * Mathd.Cos(i_final) * Mathd.Cos(dOmega);
if (Mathd.Abs(Mathd.Sin(theta)) < 1E-6)
{
Debug.LogError("u_final: about to divide by zero (small theta)");
return;
}
double u_final = Mathd.Acos(Mathd.Clamp(numer / Mathd.Sin(theta), -1.0, 1.0));
double u_initialDeg = u_initial * Mathd.Rad2Deg;
double u_finalDeg = u_final * Mathd.Rad2Deg;
// Orbits cross at two places, pick the location closest to the current position of the fromOrbit
double time_to_crossing = fromOrbit.period * (u_initialDeg - fromOrbit.phase) / 360f;
if (time_to_crossing < 0)
{
u_initialDeg += 180f;
u_finalDeg += 180f;
time_to_crossing += 0.5f * fromOrbit.period;
}
// Determine velocity change required
Vector3 dV = toOrbit.GetPhysicsVelocityForEllipse((float)u_finalDeg) -
fromOrbit.GetPhysicsVelocityForEllipse((float)u_initialDeg);
// Create a maneuver object
Maneuver m = new Maneuver();
m.physPosition = new Vector3d(fromOrbit.GetPhysicsPositionforEllipse((float)(u_initialDeg)));
m.mtype = Maneuver.Mtype.vector;
m.dV = dV.magnitude;
m.velChange = dV;
m.worldTime = GravityEngine.instance.GetPhysicalTime() + (float)time_to_crossing;
m.nbody = fromOrbit.nbody;
maneuvers.Add(m);
//Debug.LogFormat("u_initial = {0} u_final={1} (deg) dOmega={2} (deg) timeToCrossing={3} fromPhase={4} cos_theta={5} theta={6}",
// u_initialDeg,
// u_finalDeg,
// dOmega * Mathd.Rad2Deg,
// time_to_crossing,
// fromOrbit.phase,
// cos_theta,
// theta);
}
/// <summary>
/// <para>Returns the angle in degrees between from and to.</para>
/// </summary>
/// <param name="from">The vector from which the angular difference is measured.</param>
/// <param name="to">The vector to which the angular difference is measured.</param>
public static double Angle(Vector3d from, Vector3d to)
{
return(Mathd.Acos(Mathd.Clamp(Vector3d.Dot(from.normalized, to.normalized), -1f, 1f)) * 57.29578f);
}
