//---------------------------------------------------------------------
// Get transfer orbit
//---------------------------------------------------------------------
public static double get_transfer_orbit(double t,
CelestialBody depBody,
CelestialBody arrivBody,
double phi,
ref Orbit orbit,
ref double destLambda)
{
// Is bodies has same reference body
if (depBody.get_ref_body() != arrivBody.get_ref_body())
return -1;
OrbitPos pos = new OrbitPos();
EclipticPos epos = new EclipticPos();
depBody.get_position(t, ref pos);
depBody.get_ecliptic_coords(pos.theta, ref epos);
destLambda = epos.lambda + Math.PI - phi;
destLambda = math.Trunc2PiN(destLambda);
double destTheta = get_dest_theta(arrivBody, destLambda);
Vector3D x1 = depBody.get_cartesian_pos(pos.theta);
Vector3D x2 = arrivBody.get_cartesian_pos(destTheta);
double u = 0;
get_transfer_orientation(x1, x2, ref orbit.i, ref orbit.Omega, ref u);
double r1 = x1.lenght();
double r2 = x2.lenght();
orbit.omega = u / math.RAD;
BodyData data = new BodyData();
arrivBody.get_data(ref data);
double transTime = 0;
double mu = depBody.get_refGravParameter();
double E = 0;
double theta = 0;
theta = x1.angle(x2);
orbit.e = (r2 - r1) / (r1 - r2 * Math.Cos(theta));
if ( (orbit.e > - 1) && (orbit.e < 1) )
{
orbit.a = r1 / (1 - orbit.e);
double n = Math.Sqrt(mu / orbit.a) / orbit.a;
double tgE2 = Math.Sqrt((1 - orbit.e) / (1 + orbit.e)) * Math.Tan(theta / 2);
E = 2 * Math.Atan(tgE2);
double M = E - orbit.e * Math.Sin(E);
transTime = M / n;
}
if (orbit.e == 1)
{
orbit.a = 2*r1;
transTime = r1*Math.Sqrt(2*r1/mu)*(Math.Tan(theta/2) + Math.Pow(Math.Tan(theta/2), 3)/3);
}
if (orbit.e > 1)
{
orbit.a = r1 / (orbit.e - 1);
double n = Math.Sqrt(mu / orbit.a) / orbit.a;
double thE2 = Math.Sqrt((orbit.e - 1) / (orbit.e + 1)) * Math.Tan(theta / 2);
double H = Math.Log((1 + thE2) / (1 - thE2));
double M = orbit.e * Math.Sinh(H) - H;
transTime = M / n;
}
return transTime;
}
//---------------------------------------------------------------------
//
//---------------------------------------------------------------------
public static double LambdaErr(double t,
CelestialBody arivBody,
CelestialBody destBody,
double phi)
{
Orbit orbit = new Orbit();
double destLambda = 0;
double transTime = get_transfer_orbit(t, arivBody, destBody, phi, ref orbit, ref destLambda);
OrbitPos pos = new OrbitPos();
EclipticPos epos = new EclipticPos();
destBody.get_position(t + transTime, ref pos);
destBody.get_ecliptic_coords(pos.theta, ref epos);
return get_phase(epos.lambda - destLambda);
}
//---------------------------------------------------------------------
// Get orbit by two positions x1 = r(t1), x2 = r(t2)
// r = r(t) - body radius-vector
//---------------------------------------------------------------------
public static bool get_orbit(Vector3D x1,
Vector3D x2,
double t1,
double t2,
double mu,
ref Orbit orbit)
{
// Radius-vectors lenght calculation
r1 = x1.lenght();
r2 = x2.lenght();
// Thue anomalies difference calculation
double dV = x1.angle(x2);
// Distance calculation
Vector3D dx = x1 - x2;
s = dx.lenght();
// Lambert theorem parameter
tau = Math.Sqrt(mu) * (t2 - t1);
// Newton solver input data
double [] x = new double [3]{dV, dV, r1};
double eps = 1e-3;
double [] err = new double [3] { eps, eps, eps };
// Solve Lambert equations
if (!EQs.newton_solver(f, Jacoby, err, ref x))
return false;
orbit.a = x[2];
// Eccentricity calculation
double L = (x[0] - x[1])/2;
double R = (r1 + r2) / 4 / orbit.a;
double P = (r2 - r1) / 2 / orbit.a / Math.Sin(L);
double Q = (1 - 2 * R) / Math.Cos(L);
orbit.e = Math.Sqrt(P * P + Q * Q);
// Eccentric anomalies calculation
double sin_K = P / orbit.e;
double cos_K = Q / orbit.e;
double K = math.arg(sin_K, cos_K);
double E1 = K - L;
double E2 = K + L;
// Mean anomalies calculation
double M1 = E1 - orbit.e * Math.Sin(E1);
double M2 = E2 - orbit.e * Math.Sin(E2);
// Mean anomaly at epoch calculation
double n = (M2 - M1) / (t2 - t1);
double n_test = Math.Sqrt(mu / orbit.a) / orbit.a;
orbit.M0 = math.Trunc2PiN(M1 - n * t1);
if (orbit.M0 < 0)
orbit.M0 = 2 * Math.PI + orbit.M0;
// True anomalies calculation
double sin_V1 = Math.Sqrt(1 - orbit.e * orbit.e) * Math.Sin(E1) / (1 - orbit.e * Math.Cos(E1));
double cos_V1 = (Math.Cos(E1) - orbit.e) / (1 - orbit.e * Math.Cos(E1));
double V1 = math.arg(sin_V1, cos_V1);
double V1_deg = V1 / math.RAD;
double sin_V2 = Math.Sqrt(1 - orbit.e * orbit.e) * Math.Sin(E2) / (1 - orbit.e * Math.Cos(E2));
double cos_V2 = (Math.Cos(E2) - orbit.e) / (1 - orbit.e * Math.Cos(E2));
double V2 = math.arg(sin_V2, cos_V2);
double V2_deg = V2 / math.RAD;
// Orbit orientation calculation (high accuracity!!!)
// Radius-vectors orts
DVector3D ex1 = new DVector3D(Convert.ToDecimal(x1.ort().x), Convert.ToDecimal(x1.ort().y), Convert.ToDecimal(x1.ort().z));
DVector3D ex2 = new DVector3D(Convert.ToDecimal(x2.ort().x), Convert.ToDecimal(x2.ort().y), Convert.ToDecimal(x2.ort().z));
// Orbit plane normal
DVector3D en = (ex1 & ex2).ort();
// Orbit inclination
decimal i = DMath.acos(en.z);
orbit.i = Convert.ToDouble(i) / math.RAD;
// Accenting node longtitude
decimal sin_Omega = en.x / DMath.sin(i);
decimal cos_Omega = -en.y / DMath.sin(i);
decimal Omega = DMath.arg(sin_Omega, cos_Omega);
orbit.Omega = Convert.ToDouble(Omega) / math.RAD;
// Argument of latitude
decimal sin_u = ex1.z / DMath.sin(i);
decimal cos_u = (ex1.x + sin_Omega * en.z * sin_u) / cos_Omega;
decimal u = DMath.arg(sin_u, cos_u);
// Argument of periapsis
orbit.omega = (Convert.ToDouble(u) - V1) / math.RAD;
return true;
}
