public static Orbit OrbitFromStateVectors(Vector3d pos, Vector3d vel, CelestialBody body, double UT)
{
Orbit ret = new Orbit();
ret.UpdateFromStateVectors(OrbitExtensions.SwapYZ(pos - body.position), OrbitExtensions.SwapYZ(vel), body, UT);
return(ret);
}
public static Orbit OrbitFromStateVectors(Vector3d pos, Vector3d vel, CelestialBody body, double UT)
{
Orbit ret = new Orbit();
ret.UpdateFromStateVectors(OrbitExtensions.SwapYZ(pos - body.position), OrbitExtensions.SwapYZ(vel), body, UT);
if (double.IsNaN(ret.argumentOfPeriapsis))
{
Vector3d vectorToAN = Quaternion.AngleAxis(-(float)ret.LAN, Planetarium.up) * Planetarium.right;
Vector3d vectorToPe = OrbitExtensions.SwapYZ(ret.eccVec);
double cosArgumentOfPeriapsis = Vector3d.Dot(vectorToAN, vectorToPe) / (vectorToAN.magnitude * vectorToPe.magnitude);
//Squad's UpdateFromStateVectors is missing these checks, which are needed due to finite precision arithmetic:
if (cosArgumentOfPeriapsis > 1)
{
ret.argumentOfPeriapsis = 0;
}
else if (cosArgumentOfPeriapsis < -1)
{
ret.argumentOfPeriapsis = 180;
}
else
{
ret.argumentOfPeriapsis = Math.Acos(cosArgumentOfPeriapsis);
}
}
return(ret);
}
public override void OnFixedUpdate()
{
if (!warping)
{
return;
}
if (warpTarget == WarpTarget.SuicideBurn)
{
try
{
targetUT = OrbitExtensions.SuicideBurnCountdown(orbit, vesselState, vessel) + vesselState.time;
}
catch
{
warping = false;
}
}
double target = targetUT - leadTime;
if (target < vesselState.time + 1)
{
core.warp.MinimumWarp(true);
warping = false;
}
else
{
core.warp.WarpToUT(target);
}
}
public string SuicideBurnCountdown()
{
try
{
return(GuiUtils.TimeToDHMS(OrbitExtensions.SuicideBurnCountdown(orbit, vesselState, vessel)));
}
catch
{
return("N/A");
}
}
override public void afterOnFixedUpdate()
{
//Check the end of the action
if (this.isStarted() && !this.isExecuted() && !warping && startTime == 0f)
{
startTime = Time.time;
}
if (this.isStarted() && !this.isExecuted() && startTime > 0)
{
this.spendTime = initTime - (int)(Math.Round(Time.time - startTime)); //Add the end action timer
if (this.spendTime <= 0)
{
this.endAction();
}
}
if (!warping)
{
return;
}
if (warpTarget == WarpTarget.SuicideBurn)
{
try
{
targetUT = OrbitExtensions.SuicideBurnCountdown(this.scriptModule.orbit, this.scriptModule.vesselState, this.scriptModule.vessel) + this.scriptModule.vesselState.time;
}
catch
{
warping = false;
}
}
double target = targetUT - leadTime;
if (target < this.scriptModule.vesselState.time + 1)
{
core.warp.MinimumWarp(true);
warping = false;
}
else
{
core.warp.WarpToUT(target);
}
}
override public void activateAction()
{
base.activateAction();
warping = true;
Orbit orbit = this.scriptModule.orbit;
VesselState vesselState = this.scriptModule.vesselState;
Vessel vessel = FlightGlobals.ActiveVessel;
switch (warpTarget)
{
case WarpTarget.Periapsis:
targetUT = orbit.NextPeriapsisTime(vesselState.time);
break;
case WarpTarget.Apoapsis:
if (orbit.eccentricity < 1)
{
targetUT = orbit.NextApoapsisTime(vesselState.time);
}
break;
case WarpTarget.SoI:
if (orbit.patchEndTransition != Orbit.PatchTransitionType.FINAL)
{
targetUT = orbit.EndUT;
}
break;
case WarpTarget.Node:
if (vessel.patchedConicsUnlocked() && vessel.patchedConicSolver.maneuverNodes.Any())
{
targetUT = vessel.patchedConicSolver.maneuverNodes[0].UT;
}
break;
case WarpTarget.Time:
targetUT = vesselState.time + timeOffset;
break;
case WarpTarget.PhaseAngleT:
if (core.target.NormalTargetExists)
{
Orbit reference;
if (core.target.TargetOrbit.referenceBody == orbit.referenceBody)
{
reference = orbit; // we orbit arround the same body
}
else
{
reference = orbit.referenceBody.orbit;
}
// From Kerbal Alarm Clock
double angleChangePerSec = (360 / core.target.TargetOrbit.period) - (360 / reference.period);
double currentAngle = reference.PhaseAngle(core.target.TargetOrbit, vesselState.time);
double angleDigff = currentAngle - phaseAngle;
if (angleDigff > 0 && angleChangePerSec > 0)
{
angleDigff -= 360;
}
if (angleDigff < 0 && angleChangePerSec < 0)
{
angleDigff += 360;
}
double TimeToTarget = Math.Floor(Math.Abs(angleDigff / angleChangePerSec));
targetUT = vesselState.time + TimeToTarget;
}
break;
case WarpTarget.AtmosphericEntry:
try
{
targetUT = OrbitExtensions.NextTimeOfRadius(vessel.orbit, vesselState.time, vesselState.mainBody.Radius + vesselState.mainBody.RealMaxAtmosphereAltitude());
}
catch
{
warping = false;
}
break;
case WarpTarget.SuicideBurn:
try
{
targetUT = OrbitExtensions.SuicideBurnCountdown(orbit, vesselState, vessel) + vesselState.time;
}
catch
{
warping = false;
}
break;
default:
targetUT = vesselState.time;
break;
}
}
protected override void WindowGUI(int windowID)
{
GUILayout.BeginVertical();
GUILayout.BeginHorizontal();
GUILayout.Label(Localizer.Format("#MechJeb_WarpHelper_label1"), GUILayout.ExpandWidth(false));//"Warp to: "
warpTarget = (WarpTarget)GuiUtils.ComboBox.Box((int)warpTarget, warpTargetStrings, this);
GUILayout.EndHorizontal();
if (warpTarget == WarpTarget.Time)
{
GUILayout.BeginHorizontal();
GUILayout.Label(Localizer.Format("#MechJeb_WarpHelper_label2"), GUILayout.ExpandWidth(true));//"Warp for: "
timeOffset.text = GUILayout.TextField(timeOffset.text, GUILayout.Width(100));
GUILayout.EndHorizontal();
}
else if (warpTarget == WarpTarget.PhaseAngleT)
{
// I wonder if I should check for target that don't make sense
if (!core.target.NormalTargetExists)
{
GUILayout.Label(Localizer.Format("#MechJeb_WarpHelper_label3"));//"You need a target"
}
else
{
GuiUtils.SimpleTextBox(Localizer.Format("#MechJeb_WarpHelper_label4"), phaseAngle, "º", 60);//"Phase Angle:"
}
}
GUILayout.BeginHorizontal();
GuiUtils.SimpleTextBox(Localizer.Format("#MechJeb_WarpHelper_label5"), leadTime, "");//"Lead time: "
if (warping)
{
if (GUILayout.Button(Localizer.Format("#MechJeb_WarpHelper_button1")))//"Abort"
{
warping = false;
core.warp.MinimumWarp(true);
}
}
else
{
if (GUILayout.Button(Localizer.Format("#MechJeb_WarpHelper_button2")))//"Warp"
{
warping = true;
switch (warpTarget)
{
case WarpTarget.Periapsis:
targetUT = orbit.NextPeriapsisTime(vesselState.time);
break;
case WarpTarget.Apoapsis:
if (orbit.eccentricity < 1)
{
targetUT = orbit.NextApoapsisTime(vesselState.time);
}
break;
case WarpTarget.SoI:
if (orbit.patchEndTransition != Orbit.PatchTransitionType.FINAL)
{
targetUT = orbit.EndUT;
}
break;
case WarpTarget.Node:
if (vessel.patchedConicsUnlocked() && vessel.patchedConicSolver.maneuverNodes.Any())
{
targetUT = vessel.patchedConicSolver.maneuverNodes[0].UT;
}
break;
case WarpTarget.Time:
targetUT = vesselState.time + timeOffset;
break;
case WarpTarget.PhaseAngleT:
if (core.target.NormalTargetExists)
{
Orbit reference;
if (core.target.TargetOrbit.referenceBody == orbit.referenceBody)
{
reference = orbit; // we orbit arround the same body
}
else
{
reference = orbit.referenceBody.orbit;
}
// From Kerbal Alarm Clock
double angleChangePerSec = (360 / core.target.TargetOrbit.period) - (360 / reference.period);
double currentAngle = reference.PhaseAngle(core.target.TargetOrbit, vesselState.time);
double angleDigff = currentAngle - phaseAngle;
if (angleDigff > 0 && angleChangePerSec > 0)
{
angleDigff -= 360;
}
if (angleDigff < 0 && angleChangePerSec < 0)
{
angleDigff += 360;
}
double TimeToTarget = Math.Floor(Math.Abs(angleDigff / angleChangePerSec));
targetUT = vesselState.time + TimeToTarget;
}
break;
case WarpTarget.AtmosphericEntry:
try
{
targetUT = OrbitExtensions.NextTimeOfRadius(vessel.orbit, vesselState.time, vesselState.mainBody.Radius + vesselState.mainBody.RealMaxAtmosphereAltitude());
}
catch
{
warping = false;
}
break;
case WarpTarget.SuicideBurn:
try
{
targetUT = OrbitExtensions.SuicideBurnCountdown(orbit, vesselState, vessel) + vesselState.time;
}
catch
{
warping = false;
}
break;
default:
targetUT = vesselState.time;
break;
}
}
}
GUILayout.EndHorizontal();
core.warp.useQuickWarpInfoItem();
if (warping)
{
GUILayout.Label(Localizer.Format("#MechJeb_WarpHelper_label6") + (leadTime > 0 ? GuiUtils.TimeToDHMS(leadTime) + " before " : "") + warpTargetStrings[(int)warpTarget] + "."); //"Warping to "
}
core.warp.ControlWarpButton();
GUILayout.EndVertical();
base.WindowGUI(windowID);
}
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);
}
