Use ChangeInclination out of MJLib

Signed-off-by: Lamont Granquist <lamont@scriptkiddie.org>

MechJeb2/MechJebLib/Maneuvers/Simple.cs

@@ -35,6 +35,13 @@ public static V3 DeltaVToCircularize(double mu, V3 r, V3 v)
 
         public static V3 DeltaVToEllipticize(double mu, V3 r, V3 v, double newPeR, double newApR)
         {
+            Check.Finite(mu);
+            Check.Finite(r);
+            Check.Finite(v);
+            Check.Positive(mu);
+            Check.Positive(newPeR);
+            Check.Finite(newApR);
+
             double rm = r.magnitude;
             // orbital energy
             double e = -mu / (newPeR + newApR);
@@ -53,6 +60,9 @@ public static V3 DeltaVToEllipticize(double mu, V3 r, V3 v, double newPeR, doubl
             V3 one = vtransverse * transversehat + vradial * radialhat - v;
             V3 two = vtransverse * transversehat - vradial * radialhat - v;
 
+            Check.Finite(one);
+            Check.Finite(two);
+
             return one.magnitude < two.magnitude ? one : two;
         }
 

MechJeb2/OrbitalManeuverCalculator.cs

@@ -8,6 +8,7 @@
 using UnityEngine;
 using Debug = UnityEngine.Debug;
 using Random = System.Random;
+using static MechJebLib.Utils.Statics;
 
 namespace MuMech
 {
@@ -101,21 +102,9 @@ 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.
-        public static double HeadingForInclination(double inclinationDegrees, double latitudeDegrees)
+        private static double HeadingForInclination(double inclinationDegrees, double latitudeDegrees)
         {
-            double cosDesiredSurfaceAngle = Math.Cos(inclinationDegrees * UtilMath.Deg2Rad) / Math.Cos(latitudeDegrees * UtilMath.Deg2Rad);
-            if (Math.Abs(cosDesiredSurfaceAngle) > 1.0)
-            {
-                //If inclination < latitude, we get this case: the desired inclination is impossible
-                if (Math.Abs(MuUtils.ClampDegrees180(inclinationDegrees)) < 90) return 90;
-                return 270;
-            }
-
-            double angleFromEast = UtilMath.Rad2Deg * Math.Acos(cosDesiredSurfaceAngle); //an angle between 0 and 180
-            if (inclinationDegrees < 0) angleFromEast *= -1;
-            //now angleFromEast is between -180 and 180
-
-            return MuUtils.ClampDegrees360(90 - angleFromEast);
+            return Rad2Deg(Maths.HeadingForInclination(Deg2Rad(inclinationDegrees), Deg2Rad(latitudeDegrees)));
         }
 
         //See #676
@@ -199,17 +188,13 @@ public static double HeadingForLaunchInclination(Vessel vessel, VesselState vess
         //   - first, clamp newInclination to the range -180, 180
         //   - if newInclination > 0, do the cheaper burn to set that inclination
         //   - if newInclination < 0, do the more expensive burn to set that inclination
-        public static Vector3d DeltaVToChangeInclination(Orbit o, double UT, double newInclination)
+        public static Vector3d DeltaVToChangeInclination(Orbit o, double ut, double newInclination)
         {
-            double latitude = o.referenceBody.GetLatitude(o.WorldPositionAtUT(UT));
-            double desiredHeading = HeadingForInclination(newInclination, latitude);
-            var actualHorizontalVelocity = Vector3d.Exclude(o.Up(UT), o.WorldOrbitalVelocityAtUT(UT));
-            Vector3d eastComponent = actualHorizontalVelocity.magnitude * Math.Sin(UtilMath.Deg2Rad * desiredHeading) * o.East(UT);
-            Vector3d northComponent = actualHorizontalVelocity.magnitude * Math.Cos(UtilMath.Deg2Rad * desiredHeading) * o.North(UT);
-            if (Vector3d.Dot(actualHorizontalVelocity, northComponent) < 0) northComponent *= -1;
-            if (MuUtils.ClampDegrees180(newInclination) < 0) northComponent                *= -1;
-            Vector3d desiredHorizontalVelocity = eastComponent + northComponent;
-            return desiredHorizontalVelocity - actualHorizontalVelocity;
+            (V3 r, V3 v) = o.RightHandedStateVectorsAtUT(ut);
+
+            V3 dv = Simple.DeltaVToChangeInclination(r, v, Deg2Rad(newInclination));
+
+            return dv.V3ToWorld();
         }
 
         //Computes the delta-V and time of a burn to match planes with the target orbit. The output burnUT

MechJebLibTest/Maneuvers/Simple.cs

@@ -59,8 +59,36 @@ public void DeltaVToEllipticizeTest()
 
                 V3 dv = MechJebLib.Maneuvers.Simple.DeltaVToEllipticize(mu, r, v, newPeR, newApR);
 
-                MechJebLib.Core.Maths.PeriapsisFromStateVectors(mu, r, v + dv).ShouldEqual(newPeR, 1e-5);
-                MechJebLib.Core.Maths.ApoapsisFromStateVectors(mu, r, v + dv).ShouldEqual(newApR, 1e-5);
+                MechJebLib.Core.Maths.PeriapsisFromStateVectors(mu, r, v + dv).ShouldEqual(newPeR, 1e-4);
+                MechJebLib.Core.Maths.ApoapsisFromStateVectors(mu, r, v + dv).ShouldEqual(newApR, 1e-4);
+            }
+        }
+
+        [Fact]
+        public void DeltaVToChangeInclinationTest()
+        {
+            const int NTRIALS = 50;
+
+            var random = new Random();
+
+            for (int i = 0; i < NTRIALS; i++)
+            {
+                var r = new V3(4 * random.NextDouble() - 2, 4 * random.NextDouble() - 2, 4 * random.NextDouble() - 2);
+                var v = new V3(4 * random.NextDouble() - 2, 4 * random.NextDouble() - 2, 4 * random.NextDouble() - 2);
+
+                double rscale = random.NextDouble() * 1.5e8 + 1;
+                double vscale = random.NextDouble() * 3e4 + 1;
+                r      *= rscale;
+                v      *= vscale;
+
+                double plusOrMinusOne = random.Next(0, 2) * 2 - 1;
+                double lat = MechJebLib.Core.Maths.LatitudeFromBCI(r);
+                double newInc = Abs(lat) + random.NextDouble() * (PI - 2 * Abs(lat));
+                newInc *= plusOrMinusOne;
+
+                V3 dv = MechJebLib.Maneuvers.Simple.DeltaVToChangeInclination(r, v, newInc);
+
+                MechJebLib.Core.Maths.IncFromStateVectors(r, v + dv).ShouldEqual(Abs(newInc), 1e-4);
             }
         }