The barest start at mass costate integration

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

MechJeb2/MechJebLib/PVG/InputLayout.cs

@@ -20,6 +20,7 @@ public struct InputLayout
         public const int VY_INDEX         = 4;
         public const int VZ_INDEX         = 5;
         public const int M_INDEX          = 12;
+        public const int PM_INDEX         = 13;
         public const int BT_INDEX         = 14;
         public const int INPUT_LAYOUT_LEN = 15;
 
@@ -56,7 +57,7 @@ public void CopyTo(IList<double> other)
             PV.CopyTo(other, 6);
             PR.CopyTo(other, 9);
             other[M_INDEX]  = M;
-            other[13]       = Pm;
+            other[PM_INDEX] = Pm;
             other[BT_INDEX] = Bt;
         }
 
@@ -67,7 +68,7 @@ public void CopyFrom(IList<double> other)
             PV.CopyFrom(other, 6);
             PR.CopyFrom(other, 9);
             M  = other[M_INDEX];
-            Pm = other[13];
+            Pm = other[PM_INDEX];
             Bt = other[BT_INDEX];
         }
 

MechJeb2/MechJebLib/PVG/Optimizer.cs

@@ -316,6 +316,30 @@ public Optimizer Run()
             return this;
         }
 
+        private void IntegrateMassCostate()
+        {
+            double pm = 1;
+
+            for (int p = lastPhase; p >= 0; p--)
+            {
+                _terminal[p][OutputLayout.PM_INDEX] = pm;
+
+                // FIXME: okay this is harder than I thought, so here's what needs to get done I think:
+                // - need dense output high fidelity interpolant from the integrator for just the Pv 3-vector.
+                // - need to write a real simple 1-dimensional integration kernel for Pm that uses that
+                //   interpolant to integrate the Pm backwards.
+                // - then need to figure out the jump condition due to mass loss between stages.
+                // - alternatively it might be possible to use analyic expressions for Pv to integrate backwards
+                //   in closed-form without any interpolant or using RK here.
+                // - obviously, when using analytic thrust integrals it makes sense to do exactly that, so maybe
+                //   that is really the first step?
+
+                _initial[p][InputLayout.PM_INDEX] = pm;
+
+                pm += 0; // replace with jump condition between stages
+            }
+        }
+
         private void Shooting(Solution? solution = null)
         {
             using var integArray = Vn.Rent(OutputLayout.OUTPUT_LAYOUT_LEN);
@@ -359,6 +383,8 @@ private void Shooting(Solution? solution = null)
 
                 t0 += bt;
             }
+
+            IntegrateMassCostate();
         }
 
         public Optimizer Bootstrap(V3 pv0, V3 pr0)
@@ -413,6 +439,10 @@ public Optimizer Bootstrap(V3 pv0, V3 pr0)
                 t0 += tf;
             }
 
+            IntegrateMassCostate();
+
+            CalculateResiduals();
+
             for (int p = 0; p <= lastPhase; p++)
             {
                 Log(_phases[p].ToString());
@@ -499,6 +529,8 @@ public Optimizer Bootstrap(Solution solution)
                 t0 += tf;
             }
 
+            IntegrateMassCostate();
+
             CalculateResiduals();
 
             for (int p = 0; p <= lastPhase; p++)

MechJeb2/MechJebLib/PVG/OutputLayout.cs

@@ -13,6 +13,9 @@ namespace MechJebLib.PVG
 {
     public struct OutputLayout
     {
+        public const int M_INDEX           = 12;
+        public const int PM_INDEX          = 13;
+        public const int DV_INDEX          = 14;
         public const int OUTPUT_LAYOUT_LEN = 15;
 
         public V3 R;
@@ -58,9 +61,9 @@ public void CopyTo(IList<double> other)
             V.CopyTo(other, 3);
             PV.CopyTo(other, 6);
             PR.CopyTo(other, 9);
-            other[12] = M;
-            other[13] = Pm;
-            other[14] = DV;
+            other[M_INDEX]  = M;
+            other[PM_INDEX] = Pm;
+            other[DV_INDEX] = DV;
         }
 
         public void CopyFrom(IList<double> other)
@@ -69,9 +72,9 @@ public void CopyFrom(IList<double> other)
             V.CopyFrom(other, 3);
             PV.CopyFrom(other, 6);
             PR.CopyFrom(other, 9);
-            M  = other[12];
-            Pm = other[13];
-            DV = other[14];
+            M  = other[M_INDEX];
+            Pm = other[PM_INDEX];
+            DV = other[DV_INDEX];
         }
 
         public static OutputLayout CreateFrom(IList<double> other)