Skip to content

Commit

Permalink
Temporary fix for the E is NaN spam.
Browse files Browse the repository at this point in the history
  • Loading branch information
@sarbian
sarbian committed May 10, 2016
1 parent 146e647 commit cd90df0
Show file tree
Hide file tree
Showing 3 changed files with 121 additions and 5 deletions.
8 changes: 4 additions & 4 deletions MechJeb2/Maneuver/TransferCalculator.cs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -352,19 +352,19 @@ public static ManeuverParameters ComputeEjectionManeuver(Vector3d exit_velocity,
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);
sample.UpdateFromStateVectorsMJ(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);
sample.UpdateFromStateVectorsMJ(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);
sample.UpdateFromStateVectorsMJ(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);
Expand Down Expand Up @@ -395,7 +395,7 @@ static void DistanceToTarget(double[] x, double[] fi, object obj)
Vector3d DV = new Vector3d(x[0], x[1], x[2]);

Orbit orbit = new Orbit();
orbit.UpdateFromStateVectors(data.initial_orbit.getRelativePositionAtUT(t), data.initial_orbit.getOrbitalVelocityAtUT(t) + DV.xzy, data.initial_orbit.referenceBody, t);
orbit.UpdateFromStateVectorsMJ(data.initial_orbit.getRelativePositionAtUT(t), data.initial_orbit.getOrbitalVelocityAtUT(t) + DV.xzy, data.initial_orbit.referenceBody, t);
orbit.StartUT = t;

var pars = new PatchedConics.SolverParameters();
Expand Down
2 changes: 1 addition & 1 deletion MechJeb2/MuUtils.cs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -119,7 +119,7 @@ public static double ClampRadiansPi(double angle)
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);
ret.UpdateFromStateVectorsMJ(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;
Expand Down
116 changes: 116 additions & 0 deletions MechJeb2/OrbitExtensions.cs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -564,5 +564,121 @@ public static double SuicideBurnCountdown(Orbit orbit, VesselState vesselState,
}
return impactTime - decelTime / 2 - vesselState.time;
}

// 1.1.2 UpdateFromStateVectors has some problem so we use a copy of the old code for now
public static void UpdateFromStateVectorsMJ(this Orbit o, Vector3d pos, Vector3d vel, CelestialBody refBody, double UT)
{
o.referenceBody = refBody;
o.h = Vector3d.Cross(pos, vel);
o.inclination = Math.Acos(o.h.z / o.h.magnitude) * MathExtensions.Rad2Deg;
if (o.inclination == 0)
{
vel = vel + (Vector3d.forward * 1E-10);
o.h = Vector3d.Cross(pos, vel);
o.inclination = Math.Acos(o.h.z / o.h.magnitude) * MathExtensions.Rad2Deg;
}
o.eccVec = (Vector3d.Cross(vel, o.h) / refBody.gravParameter) - (pos / pos.magnitude);
o.eccentricity = o.eccVec.magnitude;
o.orbitalEnergy = vel.sqrMagnitude * 0.5 - refBody.gravParameter / pos.magnitude;
if (o.eccentricity >= 1)
{
o.semiMajorAxis = -o.semiLatusRectum / (o.eccVec.sqrMagnitude - 1);
}
else
{
o.semiMajorAxis = -refBody.gravParameter / (2 * o.orbitalEnergy);
}
o.an = Vector3d.Cross(Vector3d.forward, o.h);
if (o.an.y < 0)
{
o.LAN = 2 * Math.PI - Math.Acos(o.an.x / o.an.magnitude) * MathExtensions.Rad2Deg;
}
else
{
o.LAN = Math.Acos(o.an.x / o.an.magnitude) * MathExtensions.Rad2Deg;
}
o.argumentOfPeriapsis = Math.Acos(Vector3d.Dot(o.an, o.eccVec) / (o.an.magnitude * o.eccentricity));
if (o.eccVec.z < 0)
{
o.argumentOfPeriapsis = 2 * Math.PI - o.argumentOfPeriapsis;
}
if (o.an == Vector3d.zero)
{
o.LAN = 0;
o.argumentOfPeriapsis = Math.Acos(o.eccVec.x / o.eccentricity);
}
o.LAN = (o.LAN + Planetarium.InverseRotAngle) % 360;
o.argumentOfPeriapsis = o.argumentOfPeriapsis * MathExtensions.Rad2Deg;
o.period = 2 * Math.PI * Math.Sqrt(Math.Pow(Math.Abs(o.semiMajorAxis), 3) / refBody.gravParameter);
o.trueAnomaly = Math.Acos(Vector3d.Dot(o.eccVec, pos) / (o.eccentricity * pos.magnitude));
if (Vector3d.Dot(pos, vel) < 0)
{
o.trueAnomaly = 2 * Math.PI - o.trueAnomaly;
}
if (double.IsNaN(o.trueAnomaly))
{
o.trueAnomaly = Math.PI;
}

o.eccentricAnomaly = o.GetEccentricAnomalyMJ(o.trueAnomaly);

o.meanAnomaly = o.GetMeanAnomaly(o.eccentricAnomaly, o.trueAnomaly);
o.meanAnomalyAtEpoch = o.meanAnomaly;
if (o.eccentricity >= 1)
{
o.ObT = Math.Pow(Math.Pow(-o.semiMajorAxis, 3) / refBody.gravParameter, 0.5) * o.meanAnomaly;
o.timeToPe = -o.ObT;
o.ObTAtEpoch = o.ObT;
}
else
{
o.orbitPercent = o.meanAnomaly / (2 * Math.PI);
o.ObT = o.orbitPercent * o.period;
o.timeToPe = o.period - o.ObT;
o.timeToAp = o.timeToPe - o.period * 0.5;
if (o.timeToAp < 0)
{
o.timeToAp = o.timeToAp + o.period;
}
o.ObTAtEpoch = o.ObT;
}
o.radius = pos.magnitude;
o.altitude = o.radius - refBody.Radius;
o.epoch = UT;
o.pos = pos;
o.vel = vel;
o.debugPos = pos;
o.debugVel = vel;
o.debugH = o.h;
o.debugAN = o.an;
o.debugEccVec = o.eccVec;
}


private static double GetEccentricAnomalyMJ(this Orbit o, double tA)
{
double eccentricAnomaly;
if (o.eccentricity >= 1)
{
eccentricAnomaly = UtilMath.ACosh((o.eccentricity + Math.Cos(tA)) / (1 + o.eccentricity * Math.Cos(tA)));
if (double.IsNaN(eccentricAnomaly))
{
eccentricAnomaly = Math.PI;
}
if (double.IsInfinity(eccentricAnomaly))
{
MechJebCore.print(string.Format("E is Infinity! tA: {0}, e = {1}", tA, o.eccentricity));
}
}
else
{
eccentricAnomaly = Math.Acos((o.eccentricity + Math.Cos(tA)) / (1 + o.eccentricity * Math.Cos(tA)));
if (tA > Math.PI)
{
eccentricAnomaly = 2 * Math.PI - eccentricAnomaly;
}
}
return eccentricAnomaly;
}
}
}

0 comments on commit cd90df0

Please sign in to comment.