Better RK initial stepsize

Probably won't make much of a difference at all, but it makes me
feel like the ODE suite is more grown up.

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

MechJeb2/MechJebLib/Core/ODE/AbstractIVP.cs

@@ -126,7 +126,6 @@ private double EventFuncWrapper(double x, object? o)
             IReadOnlyList<Event>? events)
         {
             Direction = t0 != tf ? Math.Sign(tf - t0) : 1;
-            Habs      = SelectInitialStep(t0, tf);
             MaxStep   = Hmax;
             MinStep   = Hmin;
 
@@ -137,6 +136,8 @@ private double EventFuncWrapper(double x, object? o)
 
             f(Y, T, Dy);
 
+            Habs = SelectInitialStep(f, T, Y, Dy, Direction);
+
             interpolant?.Add(T, Y, Dy);
 
             if (events != null)
@@ -263,7 +264,7 @@ private int FillInterpolant(IVPFunc f, double t0, double tf, Hn interpolant, int
         }
 
         protected abstract (double, double) Step(IVPFunc f);
-        protected abstract double           SelectInitialStep(double t0, double tf);
+        protected abstract double           SelectInitialStep(IVPFunc f, double t0, IReadOnlyList<double> y0, IReadOnlyList<double> f0, int direction);
         protected abstract void             InitInterpolant();
         protected abstract void             Interpolate(double x, Vn yout);
         protected abstract void             Init();

MechJeb2/MechJebLib/Core/ODE/AbstractRungeKutta.cs

@@ -10,6 +10,7 @@
 using JetBrains.Annotations;
 using MechJebLib.Primitives;
 using static MechJebLib.Utils.Statics;
+using static System.Math;
 
 namespace MechJebLib.Core.ODE
 {
@@ -63,31 +64,54 @@ protected override (double, double) Step(IVPFunc f)
                     if (errorNorm == 0)
                         factor = MAX_FACTOR;
                     else
-                        factor = Math.Min(MAX_FACTOR, SAFETY * Math.Pow(errorNorm, -_alpha) * Math.Pow(_lastErrorNorm, Beta));
+                        factor = Min(MAX_FACTOR, SAFETY * Pow(errorNorm, -_alpha) * Pow(_lastErrorNorm, Beta));
 
                     if (previouslyRejected)
-                        factor = Math.Min(1.0, factor);
+                        factor = Min(1.0, factor);
 
                     Tnew = T + Habs * Direction;
 
-                    _lastErrorNorm = Math.Max(errorNorm, 1e-4);
+                    _lastErrorNorm = Max(errorNorm, 1e-4);
 
                     return (Habs, Habs * factor);
                 }
 
-                Habs *= Math.Max(MIN_FACTOR, SAFETY * Math.Pow(errorNorm, -_alpha));
+                Habs *= Max(MIN_FACTOR, SAFETY * Pow(errorNorm, -_alpha));
 
                 previouslyRejected = true;
             }
         }
 
-        protected override double SelectInitialStep(double t0, double tf)
+        // https://github.com/scipy/scipy/blob/c374ca7fdfa32dd3817cbec0f1863e01640279eb/scipy/integrate/_ivp/common.py#L66-L121
+        // E. Hairer, S. P. Norsett G. Wanner, "Solving Ordinary Differential Equations I: Nonstiff Problems", Sec. II.4.
+        protected override double SelectInitialStep(IVPFunc f, double t0, IReadOnlyList<double> y0, IReadOnlyList<double> dy, int direction)
         {
-            if (Hstart > 0)
-                return Hstart;
+            using Vn y = Vn.Rent(N).CopyFrom(y0);
+            using Vn f0 = Vn.Rent(N).CopyFrom(dy);
+            using Vn scale = y.Abs().MutTimes(Rtol).MutAdd(Atol);
 
-            double v = Math.Abs(tf - t0);
-            return 0.001 * v;
+            double d0 = y.magnitude / scale.magnitude;
+            double d1 = f0.magnitude / scale.magnitude;
+
+            double h0;
+            if (d0 < 1e-5 || d1 < 1e-5)
+                h0 = 1e-6;
+            else
+                h0 = 0.01 * d0 / d1;
+
+            using Vn y1 = f0.Dup().MutTimes(h0).MutTimes(direction).MutAdd(y0);
+
+            using var f1 = Vn.Rent(N);
+            f(f1, t0 + h0 * direction, y1);
+            double d2 = f1.MutSub(f0).MutDiv(scale).magnitude / h0;
+
+            double h1;
+            if (d1 <= 1e-15 && d2 <= 1e-15)
+                h1 = Max(1e-6, h0 * 1e-3);
+            else
+                h1 = Pow(0.01 / Max(d1, d2), 1.0 / (Order + 1));
+
+            return Min(100 * h0, h1);
         }
 
         protected override void Init()
@@ -118,11 +142,11 @@ protected virtual double ScaledErrorNorm(Vn err)
 
             for (int i = 0; i < n; i++)
             {
-                double scale = Atol + Rtol * Math.Max(Math.Abs(Y[i]), Math.Abs(Ynew[i]));
+                double scale = Atol + Rtol * Max(Abs(Y[i]), Abs(Ynew[i]));
                 error += Powi(err[i] / scale, 2);
             }
 
-            return Math.Sqrt(error / n);
+            return Sqrt(error / n);
         }
     }
 }