//TODO 1.1 changed trueAnomaly to rad but MJ ext stil uses deg. Should change for consistency
//Originally by Zool, revised by The_Duck
//Converts a true anomaly into an eccentric anomaly.
//For elliptical orbits this returns a value between 0 and 2pi
//For hyperbolic orbits the returned value can be any number.
//NOTE: For a hyperbolic orbit, if a true anomaly is requested that does not exist (a true anomaly
//past the true anomaly of the asymptote) then an ArgumentException is thrown
public static double GetEccentricAnomalyAtTrueAnomaly(this Orbit o, double trueAnomaly)
{
double e = o.eccentricity;
trueAnomaly = MuUtils.ClampDegrees360(trueAnomaly);
trueAnomaly = trueAnomaly * (Math.PI / 180);
if (e < 1) //elliptical orbits
{
double cosE = (e + Math.Cos(trueAnomaly)) / (1 + e * Math.Cos(trueAnomaly));
double sinE = Math.Sqrt(1 - (cosE * cosE));
if (trueAnomaly > Math.PI)
{
sinE *= -1;
}
return(MuUtils.ClampRadiansTwoPi(Math.Atan2(sinE, cosE)));
}
else //hyperbolic orbits
{
double coshE = (e + Math.Cos(trueAnomaly)) / (1 + e * Math.Cos(trueAnomaly));
if (coshE < 1)
{
throw new ArgumentException("OrbitExtensions.GetEccentricAnomalyAtTrueAnomaly: True anomaly of " + trueAnomaly + " radians is not attained by orbit with eccentricity " + o.eccentricity);
}
double E = MuUtils.Acosh(coshE);
if (trueAnomaly > Math.PI)
{
E *= -1;
}
return(E);
}
}
//The mean anomaly of the orbit.
//For elliptical orbits, the value return is always between 0 and 2pi
//For hyperbolic orbits, the value can be any number.
public static double MeanAnomalyAtUT(this Orbit o, double UT)
{
double ret = o.meanAnomalyAtEpoch + o.MeanMotion() * (UT - o.epoch);
if (o.eccentricity < 1)
{
ret = MuUtils.ClampRadiansTwoPi(ret);
}
return(ret);
}
//The next time at which the orbiting object will reach the given mean anomaly.
//For elliptical orbits, this will be a time between UT and UT + o.period
//For hyperbolic orbits, this can be any time, including a time in the past, if
//the given mean anomaly occurred in the past
public static double UTAtMeanAnomaly(this Orbit o, double meanAnomaly, double UT)
{
double currentMeanAnomaly = o.MeanAnomalyAtUT(UT);
double meanDifference = meanAnomaly - currentMeanAnomaly;
if (o.eccentricity < 1)
{
meanDifference = MuUtils.ClampRadiansTwoPi(meanDifference);
}
return(UT + meanDifference / o.MeanMotion());
}
//The mean anomaly of the orbit.
//For elliptical orbits, the value return is always between 0 and 2pi
//For hyperbolic orbits, the value can be any number.
public static double MeanAnomalyAtUT(this Orbit o, double UT)
{
// We use ObtAtEpoch and not meanAnomalyAtEpoch because somehow meanAnomalyAtEpoch
// can be wrong when using the RealSolarSystem mod. ObtAtEpoch is always correct.
double ret = (o.ObTAtEpoch + (UT - o.epoch)) * o.MeanMotion();
if (o.eccentricity < 1)
{
ret = MuUtils.ClampRadiansTwoPi(ret);
}
return(ret);
}
//Originally by Zool, revised by The_Duck
//Converts an eccentric anomaly into a mean anomaly.
//For an elliptical orbit, the returned value is between 0 and 2pi
//For a hyperbolic orbit, the returned value is any number
public static double GetMeanAnomalyAtEccentricAnomaly(this Orbit o, double E)
{
double e = o.eccentricity;
if (e < 1) //elliptical orbits
{
return(MuUtils.ClampRadiansTwoPi(E - (e * Math.Sin(E))));
}
else //hyperbolic orbits
{
return((e * Math.Sinh(E)) - E);
}
}
public ManeuverParameters ComputeEjectionManeuver(Vector3d exit_velocity, Orbit initial_orbit, double UT_0)
{
double GM = initial_orbit.referenceBody.gravParameter;
double C3 = exit_velocity.sqrMagnitude;
double Mh = initial_orbit.referenceBody.sphereOfInfluence;
double Rpe = initial_orbit.semiMajorAxis;
double isma = 2 / Mh - C3 / GM; //inverted Semi-major Axis, will work for parabolic orbit
double ecc = 1.0 - Rpe * isma;
//double vstart = Math.Sqrt(GM * (2 / Rpe - isma)); //{ total start boost}
//double slat = Rpe * Rpe * vstart * vstart / GM;
//the problem here should be R for circular orbit instead of Rpe
double slat = 2 * Rpe - isma * Rpe * Rpe; //but we don't know start point (yet) in elliptic orbit
double theta = Math.Acos((slat / Mh - 1) / ecc);
/*
* //old way infinity hyperbolic:
* //problems: it's not necessary hyperbolic (in case of low speed exit_velocity),
* //and exit_velocity appears not infinite far from celestial body, but only sphereOfInfluence far
* //i.e. Mh in previous statements(theta, isma) is not infinity!
*
* double sma = -GM / C3;
*
* double ecc = 1 - Rpe / sma;
* double theta = Math.Acos(-1 / ecc);
*
*/
Vector3d intersect_1;
Vector3d intersect_2;
// intersect_1 is the optimal position on the orbit assuming the orbit is circular and the sphere of influence is infinite
IntersectConePlane(exit_velocity, theta, initial_orbit.h, out intersect_1, out intersect_2);
Vector3d eccvec = initial_orbit.GetEccVector();
double true_anomaly = AngleAboutAxis(eccvec, intersect_1, initial_orbit.h);
// GetMeanAnomalyAtTrueAnomaly expects degrees and returns radians
double mean_anomaly = initial_orbit.GetMeanAnomalyAtTrueAnomaly(true_anomaly * UtilMath.Rad2Deg);
double delta_mean_anomaly = MuUtils.ClampRadiansPi(mean_anomaly - initial_orbit.MeanAnomalyAtUT(UT_0));
double UT = UT_0 + delta_mean_anomaly / initial_orbit.MeanMotion();
Vector3d V_0 = initial_orbit.getOrbitalVelocityAtUT(UT);
Vector3d pos = initial_orbit.getRelativePositionAtUT(UT);
double final_vel = Math.Sqrt(C3 + 2 * GM / pos.magnitude);
Vector3d x = pos.normalized;
Vector3d z = Vector3d.Cross(x, exit_velocity).normalized;
Vector3d y = Vector3d.Cross(z, x);
theta = Math.PI / 2;
double dtheta = 0.001;
Orbit sample = new Orbit();
double theta_err = double.MaxValue;
for (int iteration = 0; iteration < 50; ++iteration)
{
Vector3d V_1 = final_vel * (Math.Cos(theta) * x + Math.Sin(theta) * y);
sample.UpdateFromStateVectors(pos, V_1, initial_orbit.referenceBody, UT);
theta_err = AngleAboutAxis(exit_velocity, sample.getOrbitalVelocityAtUT(OrbitExtensions.NextTimeOfRadius(sample, UT, sample.referenceBody.sphereOfInfluence)), z);
if (double.IsNaN(theta_err))
{
return(null);
}
if (Math.Abs(theta_err) <= 0.1 * UtilMath.Deg2Rad)
{
return(new ManeuverParameters((V_1 - V_0).xzy, UT));
}
V_1 = final_vel * (Math.Cos(theta + dtheta) * x + Math.Sin(theta + dtheta) * y);
sample.UpdateFromStateVectors(pos, V_1, initial_orbit.referenceBody, UT);
double theta_err_2 = AngleAboutAxis(exit_velocity, sample.getOrbitalVelocityAtUT(OrbitExtensions.NextTimeOfRadius(sample, UT, sample.referenceBody.sphereOfInfluence)), z);
V_1 = final_vel * (Math.Cos(theta - dtheta) * x + Math.Sin(theta - dtheta) * y);
sample.UpdateFromStateVectors(pos, V_1, initial_orbit.referenceBody, UT);
double theta_err_3 = AngleAboutAxis(exit_velocity, sample.getOrbitalVelocityAtUT(OrbitExtensions.NextTimeOfRadius(sample, UT, sample.referenceBody.sphereOfInfluence)), z);
double derr = MuUtils.ClampRadiansPi(theta_err_2 - theta_err_3) / (2 * dtheta);
theta = MuUtils.ClampRadiansTwoPi(theta - theta_err / derr);
// if theta > pi, replace with 2pi - theta, the function ejection_angle=f(theta) is symmetrc wrt theta=pi
if (theta > Math.PI)
{
theta = 2 * Math.PI - theta;
}
}
return(null);
}
static ManeuverParameters ComputeEjectionManeuver(Vector3d exit_velocity, Orbit initial_orbit, double UT_0)
{
double GM = initial_orbit.referenceBody.gravParameter;
double C3 = exit_velocity.sqrMagnitude;
double sma = -GM / C3;
double Rpe = initial_orbit.semiMajorAxis;
double ecc = 1 - Rpe / sma;
double theta = Math.Acos(-1 / ecc);
Vector3d intersect_1;
Vector3d intersect_2;
// intersect_1 is the optimal position on the orbit assuming the orbit is circular and the sphere of influence is infinite
IntersectConePlane(exit_velocity, theta, initial_orbit.h, out intersect_1, out intersect_2);
Vector3d eccvec = initial_orbit.GetEccVector();
double true_anomaly = AngleAboutAxis(eccvec, intersect_1, initial_orbit.h);
// GetMeanAnomalyAtTrueAnomaly expects degrees and returns radians
double mean_anomaly = initial_orbit.GetMeanAnomalyAtTrueAnomaly(true_anomaly * 180 / Math.PI);
double delta_mean_anomaly = MuUtils.ClampRadiansPi(mean_anomaly - initial_orbit.MeanAnomalyAtUT(UT_0));
double UT = UT_0 + delta_mean_anomaly / initial_orbit.MeanMotion();
Vector3d V_0 = initial_orbit.getOrbitalVelocityAtUT(UT);
Vector3d pos = initial_orbit.getRelativePositionAtUT(UT);
double final_vel = Math.Sqrt(C3 + 2 * GM / pos.magnitude);
Vector3d x = pos.normalized;
Vector3d z = Vector3d.Cross(x, exit_velocity).normalized;
Vector3d y = Vector3d.Cross(z, x);
theta = Math.PI / 2;
double dtheta = 0.001;
Orbit sample = new Orbit();
double theta_err = Double.MaxValue;
for (int iteration = 0; iteration < 50; ++iteration)
{
Vector3d V_1 = final_vel * (Math.Cos(theta) * x + Math.Sin(theta) * y);
sample.UpdateFromStateVectors(pos, V_1, initial_orbit.referenceBody, UT);
theta_err = AngleAboutAxis(exit_velocity, sample.getOrbitalVelocityAtUT(OrbitExtensions.NextTimeOfRadius(sample, UT, sample.referenceBody.sphereOfInfluence)), z);
if (double.IsNaN(theta_err))
{
return(null);
}
if (Math.Abs(theta_err) <= Math.PI / 1800)
{
return(new ManeuverParameters((V_1 - V_0).xzy, UT));
}
V_1 = final_vel * (Math.Cos(theta + dtheta) * x + Math.Sin(theta + dtheta) * y);
sample.UpdateFromStateVectors(pos, V_1, initial_orbit.referenceBody, UT);
double theta_err_2 = AngleAboutAxis(exit_velocity, sample.getOrbitalVelocityAtUT(OrbitExtensions.NextTimeOfRadius(sample, UT, sample.referenceBody.sphereOfInfluence)), z);
V_1 = final_vel * (Math.Cos(theta - dtheta) * x + Math.Sin(theta - dtheta) * y);
sample.UpdateFromStateVectors(pos, V_1, initial_orbit.referenceBody, UT);
double theta_err_3 = AngleAboutAxis(exit_velocity, sample.getOrbitalVelocityAtUT(OrbitExtensions.NextTimeOfRadius(sample, UT, sample.referenceBody.sphereOfInfluence)), z);
double derr = MuUtils.ClampRadiansPi(theta_err_2 - theta_err_3) / (2 * dtheta);
theta = MuUtils.ClampRadiansTwoPi(theta - theta_err / derr);
// if theta > pi, replace with 2pi - theta, the function ejection_angle=f(theta) is symmetrc wrt theta=pi
if (theta > Math.PI)
{
theta = 2 * Math.PI - theta;
}
}
return(null);
}
