fix "launch to plane" again again

this time with feeling.

since we want to fully match the angular momentum vector for inc, lan
and one component that sort of corresponds to half of sma/ecc or
ApR/PeR pairs fully specify that.  then fix the flight path angle.
then take only one of velocity or radius to match (the other one
is fully constrained by the magnitude of the h-vec so is not free).

then use the argp transfersality conditions.

the result is that now the optimizer stops flailing and settles down
to 8-10 iterations when converged.

also added the znorm to the status output.

i don't think this has a singularity, but we'll have to see.

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

MechJeb2/MechJebModuleAscentGuidance.cs

@@ -357,6 +357,9 @@ protected override void WindowGUI(int windowID)
                         GUILayout.Label("n: " + core.guidance.last_lm_iteration_count + "(" + core.guidance.max_lm_iteration_count + ")", GUILayout.Width(100));
                         GUILayout.Label("staleness: " + GuiUtils.TimeToDHMS(core.guidance.staleness));
                         GUILayout.EndHorizontal();
+                        GUILayout.BeginHorizontal();
+                        GUILayout.Label(String.Format("znorm: {0:G5}", core.guidance.last_znorm));
+                        GUILayout.EndHorizontal();
                         if ( core.guidance.last_failure_cause != null )
                         {
                             GUIStyle s = new GUIStyle(GUI.skin.label);

MechJeb2/MechJebModuleGuidanceController.cs

@@ -36,6 +36,7 @@ public class MechJebModuleGuidanceController : ComputerModule
         public int max_lm_iteration_count { get { return ( p != null ) ? p.max_lm_iteration_count : 0; } }
         public int last_lm_iteration_count { get { return ( p != null ) ? p.last_lm_iteration_count : 0; } }
         public int last_lm_status { get { return ( p != null ) ? p.last_lm_status : 0; } }
+        public double last_znorm { get { return ( p != null ) ? p.last_znorm : 0; } }
         public String last_failure_cause { get { return ( p != null ) ? p.last_failure_cause : null; } }
         public double last_success_time = 0.0;
         public double staleness { get { return ( last_success_time > 0 ) ? vesselState.time - last_success_time : 0; } }

MechJeb2/Pontryagin/PontryaginBase.cs

@@ -389,6 +389,7 @@ public void AddSegment(List<double> t, List<double[]> y, Arc a)
         public int max_lm_iteration_count = 0;
         public int last_lm_iteration_count = 0;
         public int last_lm_status = 0;
+        public double last_znorm = 0;
         public String last_failure_cause = null;
         public double last_success_time = 0;
 
@@ -1021,10 +1022,11 @@ public bool runOptimizer(List<Arc> arcs)
                 //Debug.Log("z[" + i + "] = " + z[i]);
             }
 
-            znorm = Math.Sqrt(znorm);
-            //Debug.Log("znorm = " + znorm);
+            last_znorm = Math.Sqrt(znorm);
 
-            // this comes first because after max-iterations we may still have an acceptable solution
+            // this comes first because after max-iterations we may still have an acceptable solution.
+            // we check the largest z-value rather than znorm because for a lot of dimensions several slightly
+            // off z values can add up to failure when they're all acceptable tolerances.
             if (max_z < 1e-5)
             {
                 y0 = y0_new;

MechJeb2/Pontryagin/PontryaginLaunch.cs

@@ -37,29 +37,16 @@ private void flightangle5constraint(double[] yT, double[] z)
             Vector3d prf = new Vector3d(yT[9], yT[10], yT[11]);
             Vector3d hf = Vector3d.Cross(rf, vf);
 
-            // rot takes hT and rotates it to the south pole ([ 0, -1, 0 ] in xzy coordinates in KSP)
-            Quaternion rot = Quaternion.FromToRotation(hT, Vector3d.down);
-            // now rotate our projected final vector by the same amount
-            Vector3d hfp = rot * hf.normalized;
+            Vector3d hmiss = hf - hT;
 
-            /* 5 constraints */
+            // 5 constraints
             z[0] = ( rf.magnitude * rf.magnitude - rTm * rTm ) / 2.0;
-            z[1] = ( vf.magnitude * vf.magnitude - vTm * vTm ) / 2.0;
-            z[2] = Vector3d.Dot(rf, vf) - rf.magnitude * vf.magnitude * Math.Sin(gamma);
-
-            // stereographic projection onto the Riemann sphere ( hfp[1] is z-axis in KSP )
-            z[3] = hfp[0] / ( 1.0 - hfp[1] );
-            z[4] = hfp[2] / ( 1.0 - hfp[1] );
-
-            // for an almost precisely 180 degree flip of the orbital angular momentum vector remap the infinity to something the
-            // least squares algorithm can digest.
-            if ( !z[3].IsFinite() || !z[4].IsFinite() || Math.Abs(z[3]) > Math.Sqrt(Double.MaxValue) || Math.Abs(z[4]) > Math.Sqrt(Double.MaxValue) )
-            {
-                z[3] = Math.Sqrt(Double.MaxValue);
-                z[4] = Math.Sqrt(Double.MaxValue);
-            }
+            z[1] = Vector3d.Dot(rf, vf) - rf.magnitude * vf.magnitude * Math.Sin(gamma);
+            z[2] = hmiss[0];
+            z[3] = hmiss[1];
+            z[4] = hmiss[2];
 
-            /* transversality - free argp */
+            // transversality - free argp
             z[5] = Vector3d.Dot(Vector3d.Cross(prf, rf) + Vector3d.Cross(pvf, vf), hT);
         }
 
@@ -128,6 +115,12 @@ public override void Bootstrap(double t0)
             yf = new double[arcs.Count*13];
             multipleIntegrate(y0, yf, arcs, initialize: true);
 
+            //Debug.Log("optimizer hT = " + hT.magnitude * r_scale * v_scale);
+            //Debug.Log("r_scale = " + r_scale);
+            //Debug.Log("v_scale = " + v_scale);
+            //Debug.Log("rTm = " + rTm * r_scale + " " + rTm);
+            //Debug.Log("vTm = " + vTm * v_scale + " " + vTm);
+
             //for(int j = 0; j < y0.Length; j++)
             //    Debug.Log("bootstrap - y0[" + j + "] = " + y0[j]);
 
@@ -244,11 +237,6 @@ public override void Bootstrap(double t0)
             //for(int k = 0; k < yf.Length; k++)
                 //Debug.Log("new yf[" + k + "] = " + yf[k]);
 
-            //Debug.Log("optimizer hT = " + hT.magnitude * r_scale * v_scale);
-            //Debug.Log("r_scale = " + r_scale);
-            //Debug.Log("v_scale = " + v_scale);
-            //Debug.Log("rTm = " + rTm * r_scale + " " + rTm);
-            //Debug.Log("vTm = " + vTm * v_scale + " " + vTm);
         }
 
         public override void Optimize(double t0)