Skip to content

Commit

Permalink
Hang state off the IVP object
Browse files Browse the repository at this point in the history 
This reduces the method signatures but is less functional.

I was starting down the road of passing t, tnew, y, ynew and
dy, dynew to half of the callback methods, which starts to
look like litter.

Amazingly this fixes the Shepperd tests which seems to have been
a real bug fixed by the new Habs/_habsNext handling (probably in
better handling of Tnew and the isWithin() comparison when
pulling off the interpolated values?)

Signed-off-by: Lamont Granquist <lamont@scriptkiddie.org>
  • Loading branch information
@lamont-granquist
lamont-granquist committed May 4, 2023
1 parent 2cb9292 commit d277897
Show file tree
Hide file tree
Showing 4 changed files with 107 additions and 94 deletions.
95 changes: 57 additions & 38 deletions MechJeb2/MechJebLib/Core/ODE/AbstractIVP.cs
View file
Expand Up @@ -59,7 +59,7 @@ public abstract class AbstractIVP
/// </summary>
public bool ThrowOnMaxIter { get; set; } = true;

public CancellationToken CancellationToken { get; set; }
public CancellationToken CancellationToken { get; }

protected int N;

Expand All @@ -75,55 +75,73 @@ public abstract class AbstractIVP
/// <param name="events"></param>
/// <exception cref="ArgumentException"></exception>
public void Solve(IVPFunc f, IReadOnlyList<double> y0, IList<double> yf, double t0, double tf, Hn? interpolant = null,
List<IVPEvent>? events = null)
IReadOnlyCollection<IVPEvent>? events = null)
{
try
{
N = y0.Count;
N = y0.Count;
Ynew = Vn.Rent(N);
Dynew = Vn.Rent(N);
Dy = Vn.Rent(N);
Y = Vn.Rent(N);

Init();
_Solve(f, y0, yf, t0, tf, interpolant, events);
}
finally
{
Y.Dispose();
Dy.Dispose();
Ynew.Dispose();
Dynew.Dispose();
Y = Ynew = Dy = Dynew = null!;

Cleanup();
}
}

protected Vn Y = null!;
protected Vn Ynew = null!;
protected Vn Dy = null!;
protected Vn Dynew = null!;
protected double Habs;
protected int Direction;
protected double T, Tnew;
protected double MaxStep;
protected double MinStep;
private double _habsNext;

private void _Solve(IVPFunc f, IReadOnlyList<double> y0, IList<double> yf, double t0, double tf, Hn? interpolant,
List<IVPEvent>? events)
IReadOnlyCollection<IVPEvent>? events)
{
using var ynew = Vn.Rent(N);
using var dynew = Vn.Rent(N);
using var dy = Vn.Rent(N);
using var y = Vn.Rent(N);
Direction = t0 != tf ? Math.Sign(tf - t0) : 1;
Habs = SelectInitialStep(t0, tf);
MaxStep = Hmax;
MinStep = Hmin;

int direction = t0 != tf ? Math.Sign(tf - t0) : 1;
double habs = SelectInitialStep(t0, tf);

double t = t0;
y.CopyFrom(y0);
T = t0;
Y.CopyFrom(y0);
double niter = 0;
int interpCount = 1;

f(y, t, dy);
f(Y, T, Dy);

interpolant?.Add(t, y, dy);
interpolant?.Add(T, Y, Dy);

while (t != tf)
while (T != tf)
{
CancellationToken.ThrowIfCancellationRequested();

double h = habs * direction;

double tnew = t + h;
double tnext = T + Habs * Direction;

if (direction * (tnew - tf) > 0)
tnew = tf;
if (Direction * (tnext - tf) > 0)
MaxStep = Habs = Math.Abs(tf - T);
else
Habs = Math.Abs(tnext - T);

h = tnew - t;
habs = Math.Abs(h);
(Habs, _habsNext) = Step(f);

habs = Step(f, t, habs, direction, y, dy, ynew, dynew);
Tnew = T + Habs * Direction;

if (events != null)
{
Expand All @@ -136,24 +154,25 @@ public abstract class AbstractIVP
{
double tinterp = t0 + (tf - t0) * interpCount / Interpnum;

if (!tinterp.IsWithin(t, tnew))
if (!tinterp.IsWithin(T, Tnew))
break;

using var yinterp = Vn.Rent(N);
using var finterp = Vn.Rent(N);

InitInterpolant(habs, direction, y, dy, ynew, dynew);
Interpolate(tinterp, t, h, y, yinterp);
InitInterpolant();
Interpolate(tinterp, yinterp);
f(yinterp, tinterp, finterp);
interpolant?.Add(tinterp, yinterp, finterp);
interpCount++;
}
}

// take a step
ynew.CopyTo(y);
dynew.CopyTo(dy);
t = tnew;
Y.CopyFrom(Ynew);
Dy.CopyFrom(Dynew);
T = Tnew;
Habs = _habsNext;

// handle max iterations
if (Maxiter > 0 && niter++ > Maxiter)
Expand All @@ -165,16 +184,16 @@ public abstract class AbstractIVP
}
}

interpolant?.Add(t, y, dy);
interpolant?.Add(T, Y, Dy);

y.CopyTo(yf);
Y.CopyTo(yf);
}

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 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 Init();
protected abstract void Cleanup();
protected abstract (double, double) Step(IVPFunc f);
protected abstract double SelectInitialStep(double t0, double tf);
protected abstract void InitInterpolant();
protected abstract void Interpolate(double x, Vn yout);
protected abstract void Init();
protected abstract void Cleanup();
}
}
36 changes: 16 additions & 20 deletions MechJeb2/MechJebLib/Core/ODE/AbstractRungeKutta.cs
View file
Expand Up @@ -39,26 +39,24 @@ public double Beta

protected readonly List<Vn> K = new List<Vn>();

protected override double Step(IVPFunc f, double t, double habs, int direction, Vn y, Vn dy, Vn ynew, Vn dynew)
protected override (double, double) Step(IVPFunc f)
{
int n = y.Count;
using var err = Vn.Rent(n);

double minStep = Hmin;
double maxStep = Hmax;
using var err = Vn.Rent(N);

bool previouslyRejected = false;

while (true)
{
if (habs > maxStep)
habs = minStep;
else if (habs < minStep)
habs = minStep;
CancellationToken.ThrowIfCancellationRequested();

if (Habs > MaxStep)
Habs = MaxStep;
else if (Habs < MinStep)
Habs = MinStep;

RKStep(f, t, habs, direction, y, dy, ynew, dynew, err);
RKStep(f, err);

double errorNorm = ScaledErrorNorm(y, ynew, err);
double errorNorm = ScaledErrorNorm(err);

if (errorNorm < 1)
{
Expand All @@ -71,19 +69,17 @@ protected override double Step(IVPFunc f, double t, double habs, int direction,
if (previouslyRejected)
factor = Math.Min(1.0, factor);

habs *= factor;
Tnew = T + Habs * Direction;

_lastErrorNorm = Math.Max(errorNorm, 1e-4);

break;
return (Habs, Habs * factor);
}

habs *= Math.Max(MIN_FACTOR, SAFETY * Math.Pow(errorNorm, -_alpha));
Habs *= Math.Max(MIN_FACTOR, SAFETY * Math.Pow(errorNorm, -_alpha));

previouslyRejected = true;
}

return habs;
}

protected override double SelectInitialStep(double t0, double tf)
Expand Down Expand Up @@ -112,18 +108,18 @@ protected override void Cleanup()
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, Vn err);

[UsedImplicitly]
protected virtual double ScaledErrorNorm(Vn y, Vn ynew, Vn err)
protected virtual double ScaledErrorNorm(Vn err)
{
int n = err.Count;

double error = 0.0;

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 * Math.Max(Math.Abs(Y[i]), Math.Abs(Ynew[i]));
error += Powi(err[i] / scale, 2);
}

Expand Down
29 changes: 13 additions & 16 deletions MechJeb2/MechJebLib/Core/ODE/BogackiShampine32.cs
View file
Expand Up @@ -37,41 +37,38 @@ public class BogackiShampine32 : AbstractRungeKutta

#endregion

protected override void RKStep(IVPFunc f, double t, double habs, int direction, Vn y, Vn dy, Vn ynew, Vn dynew, Vn err)
protected override void RKStep(IVPFunc f, Vn err)
{
double h = habs * direction;
double h = Habs * Direction;

dy.CopyTo(K[1]);
Dy.CopyTo(K[1]);

for (int i = 0; i < N; i++)
ynew[i] = y[i] + h * (A21 * dy[i]);
f(ynew, t + C2 * h, K[2]);
Ynew[i] = Y[i] + h * (A21 * Dy[i]);
f(Ynew, T + C2 * h, K[2]);

for (int i = 0; i < N; i++)
ynew[i] = y[i] + h * (A32 * K[2][i]);
f(ynew, t + C3 * h, K[3]);
Ynew[i] = Y[i] + h * (A32 * K[2][i]);
f(Ynew, T + C3 * h, K[3]);

for (int i = 0; i < N; i++)
ynew[i] = y[i] + h * (A41 * K[1][i] + A42 * K[2][i] + A43 * K[3][i]);
f(ynew, t + h, K[4]);
Ynew[i] = Y[i] + h * (A41 * K[1][i] + A42 * K[2][i] + A43 * K[3][i]);
f(Ynew, T + h, K[4]);

for (int i = 0; i < N; i++)
err[i] = K[1][i] * E1 + K[2][i] * E2 + K[3][i] * E3 + K[4][i] * E4;

K[4].CopyTo(dynew);
K[4].CopyTo(Dynew);
}

protected override void InitInterpolant(double habs, int direction, Vn y, Vn dy, Vn ynew, Vn dynew)
protected override void InitInterpolant()
{
// intentionally left blank for now
// intentionally left blank
}

protected override void Interpolate(double x, double t, double h, Vn y, Vn yout)
protected override void Interpolate(double x, Vn yout)
{
/*
var data = (RKData)o;
int n = y.Count;
double s = (x - t) / h;
double s2 = s * s;
double s3 = s * s2;
Expand Down
41 changes: 21 additions & 20 deletions MechJeb2/MechJebLib/Core/ODE/DormandPrince54.cs
View file
Expand Up @@ -96,52 +96,53 @@ public class DormandPrince54 : AbstractRungeKutta

#endregion

protected override void RKStep(IVPFunc f, double t, double habs, int direction, Vn y, Vn dy, Vn ynew, Vn dynew, Vn err)
protected override void RKStep(IVPFunc f, Vn err)
{
double h = habs * direction;
double h = Habs * Direction;

dy.CopyTo(K[1]);
Dy.CopyTo(K[1]);

for (int i = 0; i < N; i++)
ynew[i] = y[i] + h * (A21 * dy[i]);
f(ynew, t + C2 * h, K[2]);
Ynew[i] = Y[i] + h * (A21 * Dy[i]);
f(Ynew, T + C2 * h, K[2]);

for (int i = 0; i < N; i++)
ynew[i] = y[i] + h * (A31 * K[1][i] + A32 * K[2][i]);
f(ynew, t + C3 * h, K[3]);
Ynew[i] = Y[i] + h * (A31 * K[1][i] + A32 * K[2][i]);
f(Ynew, T + C3 * h, K[3]);

for (int i = 0; i < N; i++)
ynew[i] = y[i] + h * (A41 * K[1][i] + A42 * K[2][i] + A43 * K[3][i]);
f(ynew, t + C4 * h, K[4]);
Ynew[i] = Y[i] + h * (A41 * K[1][i] + A42 * K[2][i] + A43 * K[3][i]);
f(Ynew, T + C4 * h, K[4]);

for (int i = 0; i < N; i++)
ynew[i] = y[i] + h * (A51 * K[1][i] + A52 * K[2][i] + A53 * K[3][i] + A54 * K[4][i]);
f(ynew, t + C5 * h, K[5]);
Ynew[i] = Y[i] + h * (A51 * K[1][i] + A52 * K[2][i] + A53 * K[3][i] + A54 * K[4][i]);
f(Ynew, T + C5 * h, K[5]);

for (int i = 0; i < N; i++)
ynew[i] = y[i] + h * (A61 * K[1][i] + A62 * K[2][i] + A63 * K[3][i] + A64 * K[4][i] + A65 * K[5][i]);
f(ynew, t + h, K[6]);
Ynew[i] = Y[i] + h * (A61 * K[1][i] + A62 * K[2][i] + A63 * K[3][i] + A64 * K[4][i] + A65 * K[5][i]);
f(Ynew, T + h, K[6]);

for (int i = 0; i < N; i++)
ynew[i] = y[i] + h * (A71 * K[1][i] + A73 * K[3][i] + A74 * K[4][i] + A75 * K[5][i] + A76 * K[6][i]);
Ynew[i] = Y[i] + h * (A71 * K[1][i] + A73 * K[3][i] + A74 * K[4][i] + A75 * K[5][i] + A76 * K[6][i]);

f(ynew, t + h, K[7]);
f(Ynew, T + h, K[7]);

for (int i = 0; i < N; i++)
err[i] = K[1][i] * E1 + K[3][i] * E3 + K[4][i] * E4 + K[5][i] * E5 + K[6][i] * E6 + K[7][i] * E7;

K[7].CopyTo(dynew);
K[7].CopyTo(Dynew);
}

protected override void InitInterpolant(double habs, int direction, Vn y, Vn dy, Vn ynew, Vn dynew)
protected override void InitInterpolant()
{
// intentionally left blank
}

// https://doi.org/10.1016/0898-1221(86)90025-8
protected override void Interpolate(double x, double t, double h, Vn y, Vn yout)
protected override void Interpolate(double x, Vn yout)
{
double s = (x - t) / h;
double h = Habs * Direction;
double s = (x - T) / h;
double s2 = s * s;
double s3 = s * s2;
double s4 = s2 * s2;
Expand All @@ -155,7 +156,7 @@ protected override void Interpolate(double x, double t, double h, Vn y, Vn yout)

for (int i = 0; i < N; i++)
{
yout[i] = y[i] + h * s * (bs1 * K[1][i] + bs3 * K[3][i] + bs4 * K[4][i] + bs5 * K[5][i] + bs6 * K[6][i] + bs7 * K[7][i]);
yout[i] = Y[i] + h * s * (bs1 * K[1][i] + bs3 * K[3][i] + bs4 * K[4][i] + bs5 * K[5][i] + bs6 * K[6][i] + bs7 * K[7][i]);
}
}
}
Expand Down

0 comments on commit d277897

Please sign in to comment.