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);
}
