Skip to content

Commit

Permalink
MJLib version of HeadingForLaunchInclination
Browse files Browse the repository at this point in the history 
Signed-off-by: Lamont Granquist <lamont@scriptkiddie.org>
  • Loading branch information
@lamont-granquist
lamont-granquist committed Sep 1, 2023
1 parent 4806676 commit 0fb3a01
Show file tree
Hide file tree
Showing 5 changed files with 105 additions and 109 deletions.
17 changes: 5 additions & 12 deletions MechJeb2/CelestialBodyExtensions.cs
View file
Expand Up @@ -2,10 +2,8 @@
{
public static class CelestialBodyExtensions
{
public static double TerrainAltitude(this CelestialBody body, Vector3d worldPosition)
{
return body.TerrainAltitude(body.GetLatitude(worldPosition), body.GetLongitude(worldPosition));
}
public static double TerrainAltitude(this CelestialBody body, Vector3d worldPosition) =>
body.TerrainAltitude(body.GetLatitude(worldPosition), body.GetLongitude(worldPosition));

//The KSP drag law is dv/dt = -b * v^2 where b is proportional to the air density and
//the ship's drag coefficient. In this equation b has units of inverse length. So 1/b
Expand All @@ -23,15 +21,10 @@ public static double DragLength(this CelestialBody body, Vector3d pos, double dr
return mass / (0.0005 * PhysicsGlobals.DragMultiplier * airDensity * dragCoeff);
}

public static double DragLength(this CelestialBody body, double altitudeASL, double dragCoeff, double mass)
{
return body.DragLength(body.GetWorldSurfacePosition(0, 0, altitudeASL), dragCoeff, mass);
}
public static double DragLength(this CelestialBody body, double altitudeASL, double dragCoeff, double mass) =>
body.DragLength(body.GetWorldSurfacePosition(0, 0, altitudeASL), dragCoeff, mass);

public static double RealMaxAtmosphereAltitude(this CelestialBody body)
{
return !body.atmosphere ? 0 : body.atmosphereDepth;
}
public static double RealMaxAtmosphereAltitude(this CelestialBody body) => !body.atmosphere ? 0 : body.atmosphereDepth;

public static double AltitudeForPressure(this CelestialBody body, double pressure)
{
Expand Down
23 changes: 18 additions & 5 deletions MechJeb2/MechJebLib/Core/Maths.cs
View file
Expand Up @@ -221,13 +221,19 @@ public static V3 ENUToECI(V3 pos, V3 vec)
return m * vec;
}

public static double HeadingForVelocity(V3 r, V3 v)
{
V3 venu = ECIToENU(r, v);
return Clamp2Pi(Atan2(venu[0], venu[1]));
}

public static V3 VelocityForHeading(V3 r, V3 v, double newHeading)
{
V3 vtemp = ECIToENU(r, v);
double hmag = new V3(vtemp.x, vtemp.y).magnitude;
vtemp[0] = hmag * Sin(newHeading);
vtemp[1] = hmag * Cos(newHeading);
return ENUToECI(r, vtemp);
V3 venu = ECIToENU(r, v);
double hmag = new V3(venu.x, venu.y).magnitude;
venu[0] = hmag * Sin(newHeading);
venu[1] = hmag * Cos(newHeading);
return ENUToECI(r, venu);
}

public static V3 ENUHeadingForInclination(double inc, V3 r) => ENUHeadingForInclination(inc, LatitudeFromBCI(r));
Expand Down Expand Up @@ -289,6 +295,13 @@ public static V3 ECIToENU(V3 r, V3 v)
return m * v;
}

public static V3 EscapeVelocityForInclination(double mu, V3 r, double newInc)
{
V3 vf = ENUHeadingForInclination(newInc, r) * EscapeVelocity(mu, r.magnitude);
vf = ENUToECI(r, vf);
return vf;
}

public static V3 VelocityForInclination(V3 r, V3 v, double newInc)
{
V3 v0 = ECIToENU(r, v);
Expand Down
101 changes: 74 additions & 27 deletions MechJeb2/MechJebLib/Maneuvers/Simple.cs
View file
Expand Up @@ -11,35 +11,30 @@ public class Simple
{
public static V3 DeltaVRelativeToCircularVelocity(double mu, V3 r, V3 v, double n = 1.0)
{
Check.Finite(mu);
Check.Finite(r);
Check.PositiveFinite(mu);
Check.Finite(v);
Check.Positive(mu);
Check.NonZero(r);
Check.NonZeroFinite(r);

var h = V3.Cross(r, v);
return n * Maths.CircularVelocityFromHvec(mu, r, h) - v;
}

public static V3 DeltaVToCircularize(double mu, V3 r, V3 v)
{
Check.Finite(mu);
Check.Finite(r);
Check.PositiveFinite(mu);
Check.Finite(v);
Check.Positive(mu);
Check.NonZero(r);
Check.NonZeroFinite(r);

var h = V3.Cross(r, v);
return Maths.CircularVelocityFromHvec(mu, r, h) - v;
}

public static V3 DeltaVToEllipticize(double mu, V3 r, V3 v, double newPeR, double newApR)
{
Check.Finite(mu);
Check.Finite(r);
Check.PositiveFinite(mu);
Check.NonZeroFinite(r);
Check.Finite(v);
Check.Positive(mu);
Check.Positive(newPeR);
Check.PositiveFinite(newPeR);
Check.Finite(newApR);

double rm = r.magnitude;
Expand Down Expand Up @@ -68,12 +63,10 @@ public static V3 DeltaVToEllipticize(double mu, V3 r, V3 v, double newPeR, doubl

public static V3 DeltaVToCircularizeAfterTime(double mu, V3 r, V3 v, double dt)
{
Check.Finite(mu);
Check.Finite(r);
Check.PositiveFinite(mu);
Check.NonZeroFinite(r);
Check.Finite(v);
Check.Finite(dt);
Check.Positive(mu);
Check.NonZero(r);

(V3 r1, V3 v1) = Shepperd.Solve(mu, dt, r, v);
var h = V3.Cross(r1, v1);
Expand All @@ -82,11 +75,9 @@ public static V3 DeltaVToCircularizeAfterTime(double mu, V3 r, V3 v, double dt)

public static (V3 dv, double dt) ManeuverToCircularizeAtPeriapsis(double mu, V3 r, V3 v)
{
Check.Finite(mu);
Check.Finite(r);
Check.PositiveFinite(mu);
Check.NonZeroFinite(r);
Check.Finite(v);
Check.Positive(mu);
Check.NonZero(r);

double dt = Maths.TimeToNextPeriapsis(mu, r, v);

Expand All @@ -97,21 +88,77 @@ public static (V3 dv, double dt) ManeuverToCircularizeAtPeriapsis(double mu, V3

public static (V3 dv, double dt) ManeuverToCircularizeAtApoapsis(double mu, V3 r, V3 v)
{
Check.Finite(mu);
Check.Finite(r);
Check.PositiveFinite(mu);
Check.NonZeroFinite(r);
Check.Finite(v);
Check.Positive(mu);
Check.NonZero(r);

double dt = Maths.TimeToNextApoapsis(mu, r, v);
V3 dv = DeltaVToCircularizeAfterTime(mu, r, v, dt);

Check.Finite(dv);
Check.Finite(dt);

return (DeltaVToCircularizeAfterTime(mu, r, v, dt), dt);
return (dv, dt);
}

// simple proportional yaw guidance for non-hyperbolic target orbits
public static V3 VelocityForLaunchInclination(double mu, V3 r, V3 v, double newInc, double rotFreq)
{
Check.PositiveFinite(mu);
Check.NonZeroFinite(r);
Check.Finite(v);
Check.Finite(newInc);

// as long as we're not launching to hyperbolic orbits, this vgo will always point
// in 'front' of us.
V3 v1 = Maths.EscapeVelocityForInclination(mu, r, newInc);
V3 v2 = Maths.EscapeVelocityForInclination(mu, r, -newInc);
V3 dv1 = v1 - v;
V3 dv2 = v2 - v;

V3 dv = dv1.magnitude < dv2.magnitude ? dv1 : dv2;

// if the surface horizontal velocity is very small and the dv magnitudes are nearly equal, then
// assume we are doing vertical rise from launch and the sign of newInc determines if we take the
// northward or southward going track.
V3 rhat = r.normalized;
V3 vsurf = v - V3.Cross(rotFreq * V3.northpole, r);
V3 vhoriz = vsurf - V3.Dot(vsurf, rhat) * rhat;
if (vhoriz.magnitude / v1.magnitude < 0.05 && Abs(dv1.magnitude - dv2.magnitude) / dv1.magnitude < 0.05)
dv = dv1;

Check.Finite(dv);

return dv;
}

public static V3 DeltaVToChangeInclination(V3 r, V3 v, double newInc) => Maths.VelocityForInclination(r, v, newInc) - v;
public static double HeadingForLaunchInclination(double mu, V3 r, V3 v, double newInc, double rotFreq) =>
Maths.HeadingForVelocity(r, VelocityForLaunchInclination(mu, r, v, newInc, rotFreq));

public static V3 DeltaVToChangeFPA(V3 r, V3 v, double newFPA) => Maths.VelocityForFPA(r, v, newFPA) - v;
public static V3 DeltaVToChangeInclination(V3 r, V3 v, double newInc)
{
Check.NonZeroFinite(r);
Check.Finite(v);
Check.Finite(newInc);

V3 dv = Maths.VelocityForInclination(r, v, newInc) - v;

Check.Finite(dv);

return dv;
}

public static V3 DeltaVToChangeFPA(V3 r, V3 v, double newFPA)
{
Check.NonZeroFinite(r);
Check.Finite(v);
Check.Finite(newFPA);

V3 dv = Maths.VelocityForFPA(r, v, newFPA) - v;

Check.Finite(dv);

return dv;
}
}
}
2 changes: 1 addition & 1 deletion MechJeb2/MechJebModuleAscentBaseAutopilot.cs
View file
Expand Up @@ -336,7 +336,7 @@ protected float ThrottleToRaiseApoapsis(double currentApR, double finalApR)
// this provides ground track heading based on desired inclination and is what most consumers should call
protected void AttitudeTo(double desiredPitch)
{
double desiredHeading = OrbitalManeuverCalculator.HeadingForLaunchInclination(Vessel, VesselState, AscentSettings.DesiredInclination);
double desiredHeading = OrbitalManeuverCalculator.HeadingForLaunchInclination(Vessel.orbit, AscentSettings.DesiredInclination);
AttitudeTo(desiredPitch, desiredHeading);
}

Expand Down
71 changes: 7 additions & 64 deletions MechJeb2/OrbitalManeuverCalculator.cs
View file
Expand Up @@ -6,9 +6,9 @@
using MechJebLib.Primitives;
using Smooth.Pools;
using UnityEngine;
using static MechJebLib.Statics;
using Debug = UnityEngine.Debug;
using Random = System.Random;
using static MechJebLib.Statics;

namespace MuMech
{
Expand Down Expand Up @@ -102,10 +102,8 @@ public static Vector3d DeltaVForSemiMajorAxis(Orbit o, double ut, double newSMA)
//Returned heading is in degrees and in the range 0 to 360.
//If the given latitude is too large, so that an orbit with a given inclination never attains the
//given latitude, then this function returns either 90 (if -90 < inclination < 90) or 270.
private static double HeadingForInclination(double inclinationDegrees, double latitudeDegrees)
{
return Rad2Deg(Maths.HeadingForInclination(Deg2Rad(inclinationDegrees), Deg2Rad(latitudeDegrees)));
}
private static double HeadingForInclination(double inclinationDegrees, double latitudeDegrees) =>
Rad2Deg(Maths.HeadingForInclination(Deg2Rad(inclinationDegrees), Deg2Rad(latitudeDegrees)));

//See #676
//Computes the heading for a ground launch at the specified latitude accounting for the body rotation.
Expand All @@ -117,67 +115,12 @@ private static double HeadingForInclination(double inclinationDegrees, double la
//Returned heading is in degrees and in the range 0 to 360.
//If the given latitude is too large, so that an orbit with a given inclination never attains the
//given latitude, then this function returns either 90 (if -90 < inclination < 90) or 270.
public static double HeadingForLaunchInclination(Vessel vessel, VesselState vesselState, double inclinationDegrees)
public static double HeadingForLaunchInclination(Orbit o, double inclinationDegrees)
{
CelestialBody body = vessel.mainBody;
double latitudeDegrees = vesselState.latitude;
double orbVel = vessel.orbit.CircularOrbitSpeed();
double headingOne = HeadingForInclination(inclinationDegrees, latitudeDegrees) * UtilMath.Deg2Rad;
double headingTwo = HeadingForInclination(-inclinationDegrees, latitudeDegrees) * UtilMath.Deg2Rad;
double now = Planetarium.GetUniversalTime();
Orbit o = vessel.orbit;

Vector3d north = vesselState.north;
Vector3d east = vesselState.east;

var actualHorizontalVelocity = Vector3d.Exclude(o.Up(now), o.WorldOrbitalVelocityAtUT(now));
Vector3d desiredHorizontalVelocityOne = orbVel * (Math.Sin(headingOne) * east + Math.Cos(headingOne) * north);
Vector3d desiredHorizontalVelocityTwo = orbVel * (Math.Sin(headingTwo) * east + Math.Cos(headingTwo) * north);

Vector3d deltaHorizontalVelocityOne = desiredHorizontalVelocityOne - actualHorizontalVelocity;
Vector3d deltaHorizontalVelocityTwo = desiredHorizontalVelocityTwo - actualHorizontalVelocity;

Vector3d desiredHorizontalVelocity;
Vector3d deltaHorizontalVelocity;

if (vesselState.speedSurfaceHorizontal < 200)
{
// at initial launch we have to head the direction the user specifies (90 north instead of -90 south).
// 200 m/s of surface velocity also defines a 'grace period' where someone can catch a rocket that they meant
// to launch at -90 and typed 90 into the inclination box fast after it started to initiate the turn.
// if the rocket gets outside of the 200 m/s surface velocity envelope, then there is no way to tell MJ to
// take a south travelling rocket and turn north or vice versa.
desiredHorizontalVelocity = desiredHorizontalVelocityOne;
deltaHorizontalVelocity = deltaHorizontalVelocityOne;
}
else
{
// now in order to get great circle tracks correct we pick the side which gives the lowest delta-V, which will get
// ground tracks that cross the maximum (or minimum) latitude of a great circle correct.
if (deltaHorizontalVelocityOne.magnitude < deltaHorizontalVelocityTwo.magnitude)
{
desiredHorizontalVelocity = desiredHorizontalVelocityOne;
deltaHorizontalVelocity = deltaHorizontalVelocityOne;
}
else
{
desiredHorizontalVelocity = desiredHorizontalVelocityTwo;
deltaHorizontalVelocity = deltaHorizontalVelocityTwo;
}
}

// if you circularize in one burn, towards the end deltaHorizontalVelocity will whip around, but we want to
// fall back to tracking desiredHorizontalVelocity
if (Vector3d.Dot(desiredHorizontalVelocity.normalized, deltaHorizontalVelocity.normalized) < 0.90)
{
// it is important that we do NOT do the fracReserveDV math here, we want to ignore the deltaHV entirely at ths point
return MuUtils.ClampDegrees360(UtilMath.Rad2Deg *
Math.Atan2(Vector3d.Dot(desiredHorizontalVelocity, east),
Vector3d.Dot(desiredHorizontalVelocity, north)));
}
(V3 r, V3 v) = o.RightHandedStateVectorsAtUT(Planetarium.GetUniversalTime());
double rotFreq = TAU / o.referenceBody.rotationPeriod;

return MuUtils.ClampDegrees360(UtilMath.Rad2Deg *
Math.Atan2(Vector3d.Dot(deltaHorizontalVelocity, east), Vector3d.Dot(deltaHorizontalVelocity, north)));
return Rad2Deg(Simple.HeadingForLaunchInclination(o.referenceBody.gravParameter, r, v, Deg2Rad(inclinationDegrees), rotFreq));
}

//Computes the delta-V of the burn required to change an orbit's inclination to a given value
Expand Down

0 comments on commit 0fb3a01

Please sign in to comment.