From dd95067b3a6baf7b9233204835028f084ea4ed6b Mon Sep 17 00:00:00 2001 From: Lamont Granquist Date: Wed, 3 May 2023 18:05:13 -0700 Subject: [PATCH] Basic minstep/maxstep logic Signed-off-by: Lamont Granquist --- MechJeb2/MechJebLib/Core/ODE/AbstractIVP.cs | 10 +++--- .../MechJebLib/Core/ODE/AbstractRungeKutta.cs | 32 ++++++++++++------- .../MechJebLib/Core/ODE/BogackiShampine32.cs | 2 +- .../MechJebLib/Core/ODE/DormandPrince54.cs | 2 +- 4 files changed, 28 insertions(+), 18 deletions(-) diff --git a/MechJeb2/MechJebLib/Core/ODE/AbstractIVP.cs b/MechJeb2/MechJebLib/Core/ODE/AbstractIVP.cs index e8302194..a331a6fc 100644 --- a/MechJeb2/MechJebLib/Core/ODE/AbstractIVP.cs +++ b/MechJeb2/MechJebLib/Core/ODE/AbstractIVP.cs @@ -27,7 +27,7 @@ public abstract class AbstractIVP /// /// Maximum h step. /// - public double Hmax { get; set; } + public double Hmax { get; set; } = double.PositiveInfinity; /// /// Maximum number of steps. @@ -80,7 +80,7 @@ public abstract class AbstractIVP try { N = y0.Count; - Initialize(); + Init(); _Solve(f, y0, yf, t0, tf, interpolant, events); } finally @@ -142,7 +142,7 @@ public abstract class AbstractIVP using var yinterp = Vn.Rent(N); using var finterp = Vn.Rent(N); - PrepareInterpolant(habs, direction, y, dy, ynew, dynew); + InitInterpolant(habs, direction, y, dy, ynew, dynew); Interpolate(tinterp, t, h, y, yinterp); f(yinterp, tinterp, finterp); interpolant?.Add(tinterp, yinterp, finterp); @@ -172,9 +172,9 @@ public abstract class AbstractIVP protected abstract double Step(IVPFunc f, double t, double habs, int direction, Vn y, Vn dy, Vn ynew, Vn dynew); protected abstract double SelectInitialStep(double t0, double tf); - protected abstract void PrepareInterpolant(double habs, int direction, Vn y, Vn dy, Vn ynew, Vn dynew); + protected abstract void InitInterpolant(double habs, int direction, Vn y, Vn dy, Vn ynew, Vn dynew); protected abstract void Interpolate(double x, double t, double h, Vn y, Vn yout); - protected abstract void Initialize(); + protected abstract void Init(); protected abstract void Cleanup(); } } diff --git a/MechJeb2/MechJebLib/Core/ODE/AbstractRungeKutta.cs b/MechJeb2/MechJebLib/Core/ODE/AbstractRungeKutta.cs index 4659cbf2..571ae2f9 100644 --- a/MechJeb2/MechJebLib/Core/ODE/AbstractRungeKutta.cs +++ b/MechJeb2/MechJebLib/Core/ODE/AbstractRungeKutta.cs @@ -7,6 +7,7 @@ using System; using System.Collections.Generic; +using JetBrains.Annotations; using MechJebLib.Primitives; using static MechJebLib.Utils.Statics; @@ -33,20 +34,28 @@ public double Beta set => _beta = value * (ErrorEstimatorOrder + 1.0); } - private double _alpha => 1.0 / (ErrorEstimatorOrder + 1.0) - 0.75 * Beta; - private double _lastErrorNorm = 1e-4; + private double _alpha => 1.0 / (ErrorEstimatorOrder + 1.0) - 0.75 * Beta; + private double _lastErrorNorm = 1e-4; - protected readonly List K = new List(); + protected readonly List K = new List(); protected override double Step(IVPFunc f, double t, double habs, int direction, Vn y, Vn dy, Vn ynew, Vn dynew) { int n = y.Count; using var err = Vn.Rent(n); + double minStep = Hmin; + double maxStep = Hmax; + bool previouslyRejected = false; while (true) { + if (habs > maxStep) + habs = minStep; + else if (habs < minStep) + habs = minStep; + RKStep(f, t, habs, direction, y, dy, ynew, dynew, err); double errorNorm = ScaledErrorNorm(y, ynew, err); @@ -57,7 +66,7 @@ protected override double Step(IVPFunc f, double t, double habs, int direction, if (errorNorm == 0) factor = MAX_FACTOR; else - factor = Math.Min(MAX_FACTOR,SAFETY*Math.Pow(errorNorm, -_alpha)*Math.Pow(_lastErrorNorm, Beta)); + factor = Math.Min(MAX_FACTOR, SAFETY * Math.Pow(errorNorm, -_alpha) * Math.Pow(_lastErrorNorm, Beta)); if (previouslyRejected) factor = Math.Min(1.0, factor); @@ -86,26 +95,27 @@ protected override double SelectInitialStep(double t0, double tf) return 0.001 * v; } - protected override void Initialize() + protected override void Init() { _lastErrorNorm = 1e-4; K.Clear(); - // we create an extra K[0] which we do not use - for (int i = 0; i <= Stages+1; i++) + // we create an extra K[0] which we do not use, because the literature uses 1-indexed K's + for (int i = 0; i <= Stages + 1; i++) K.Add(Vn.Rent(N)); } protected override void Cleanup() { - for (int i = 0; i <= Stages+1; i++) + for (int i = 0; i <= Stages + 1; i++) K[i].Dispose(); K.Clear(); } - protected abstract void RKStep(IVPFunc f, double t, double habs, int direction, Vn y, Vn dy, Vn ynew, Vn dynew, Vn err); + protected abstract void RKStep(IVPFunc f, double t, double habs, int direction, Vn y, Vn dy, Vn ynew, Vn dynew, Vn err); - private double ScaledErrorNorm(Vn y, Vn ynew, Vn err) + [UsedImplicitly] + protected virtual double ScaledErrorNorm(Vn y, Vn ynew, Vn err) { int n = err.Count; @@ -114,7 +124,7 @@ private double ScaledErrorNorm(Vn y, Vn ynew, Vn err) for (int i = 0; i < n; i++) { double scale = Atol + Rtol * Math.Max(Math.Abs(y[i]), Math.Abs(ynew[i])); - error += Powi(err[i]/scale, 2); + error += Powi(err[i] / scale, 2); } return Math.Sqrt(error / n); diff --git a/MechJeb2/MechJebLib/Core/ODE/BogackiShampine32.cs b/MechJeb2/MechJebLib/Core/ODE/BogackiShampine32.cs index 5ee5ef91..7cd8cdac 100644 --- a/MechJeb2/MechJebLib/Core/ODE/BogackiShampine32.cs +++ b/MechJeb2/MechJebLib/Core/ODE/BogackiShampine32.cs @@ -70,7 +70,7 @@ protected override void RKStep(IVPFunc f, double t, double habs, int direction, K[4].CopyTo(dynew); } - protected override void PrepareInterpolant(double habs, int direction, Vn y, Vn dy, Vn ynew, Vn dynew) + protected override void InitInterpolant(double habs, int direction, Vn y, Vn dy, Vn ynew, Vn dynew) { // intentionally left blank for now } diff --git a/MechJeb2/MechJebLib/Core/ODE/DormandPrince54.cs b/MechJeb2/MechJebLib/Core/ODE/DormandPrince54.cs index 7fed83bd..0c67c771 100644 --- a/MechJeb2/MechJebLib/Core/ODE/DormandPrince54.cs +++ b/MechJeb2/MechJebLib/Core/ODE/DormandPrince54.cs @@ -97,7 +97,7 @@ protected override void RKStep(IVPFunc f, double t, double habs, int direction, K[7].CopyTo(dynew); } - protected override void PrepareInterpolant(double habs, int direction, Vn y, Vn dy, Vn ynew, Vn dynew) + protected override void InitInterpolant(double habs, int direction, Vn y, Vn dy, Vn ynew, Vn dynew) { // intentionally left blank for now }