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