// Dormand Prince 5(4)7FM ODE integrator (aka DOPRI5)
//
// We prefer this over Bogacki–Shampine in order to obtain the free 4th order interpolant.
//
// f - the ODE function to integrate
// o - object paramter which is passed through to the ODE function
// y0 - array of starting y0 values
// n - dimensionality of the problem
// xtbl - array of x values to evaluate at (2 minimum for start + end)
// ytbl - output jagged array of y values at the x evaluation points (n x 2 minimum)
// eps - accuracy
// h - starting step-size (can be zero for automatic guess)
// xlist - output list of all intermediate x values (user allocates, can be null or omitted)
// ylist - output list of all intermediate y values (user allocates, can be null or omitted)
// hmin - minimum h step (may be violated on the last step)
// hmax - maximum h step
// maxiter - maximum number of steps
// EvtFuns - array of event functions
//
// ref: https://github.com/scipy/scipy/blob/master/scipy/integrate/dop/dopri5.f
//
public static void RKDP547FM(ODEFun f, object o, double[] y0, int n, Span <double> xtbl, Span <double[]> ytbl, double eps, double hstart, List <double> xlist = null, List <double []> ylist = null, double hmin = 0, double hmax = 0, int maxiter = 0, EvtFun[] EvtFuns = null)
{
Span <double> k1 = stackalloc double[n];
Span <double> k2 = stackalloc double[n];
Span <double> k3 = stackalloc double[n];
Span <double> k4 = stackalloc double[n];
Span <double> k5 = stackalloc double[n];
Span <double> k6 = stackalloc double[n];
Span <double> k7 = stackalloc double[n];
// accumulator
Span <double> a = stackalloc double[n];
// error
Span <double> err = stackalloc double[n];
double h = hstart;
double x = xtbl[0];
Span <double> y = stackalloc double[n];
if (ylist != null && xlist != null)
{
ylist.Clear();
xlist.Clear();
}
y0.CopyTo(y);
// auto-guess starting value of h based on smallest dx in xtbl
if (h == 0)
{
double v = Math.Abs(xtbl[1] - xtbl[0]);
for (int i = 1; i < xtbl.Length - 1; i++)
{
v = Math.Min(v, Math.Abs(xtbl[i + 1] - xtbl[i]));
}
h = 0.001 * v;
}
// xtbl controls the direcction of integration, the sign of h is not relevant
h = Math.Sign(xtbl[1] - xtbl[0]) * Math.Abs(h);
// copy initial conditions to ybtl output
int j = 0;
for (int i = 0; i < n; i++)
{
ytbl[i][j] = y[i];
}
j++;
// copy initial conditions to full xlist/ylist output
if (xlist != null && ylist != null)
{
double[] ydup2 = new double[n];
y.CopyTo(ydup2);
ylist.Add(ydup2);
xlist.Add(x);
}
bool fsal = false;
bool at_hmin;
double h_restart = 0;
double niter = 0;
while (j < xtbl.Length)
{
double xf = xtbl[j];
while ((h > 0) ? x <xf : x> xf)
{
at_hmin = false;
if (hmax > 0 && Math.Abs(h) > hmax)
{
h = hmax * Math.Sign(h);
}
if (hmin > 0 && Math.Abs(h) < hmin)
{
h = hmin * Math.Sign(h);
at_hmin = true;
}
// we may violate hmin in order to exactly hit the boundary conditions
if (Math.Abs(h) > Math.Abs(xf - x))
{
h = xf - x;
}
if (fsal)
{
// FIXME: tricks to make this copy go away?
for (int i = 0; i < n; i++)
{
k1[i] = k7[i];
}
}
else
{
f(y, x, k1, n, o);
}
for (int i = 0; i < n; i++)
{
a[i] = y[i] + h * (a21 * k1[i]);
}
f(a, x + c2 * h, k2, n, o);
for (int i = 0; i < n; i++)
{
a[i] = y[i] + h * (a31 * k1[i] + a32 * k2[i]);
}
f(a, x + c3 * h, k3, n, o);
for (int i = 0; i < n; i++)
{
a[i] = y[i] + h * (a41 * k1[i] + a42 * k2[i] + a43 * k3[i]);
}
f(a, x + c4 * h, k4, n, o);
for (int i = 0; i < n; i++)
{
a[i] = y[i] + h * (a51 * k1[i] + a52 * k2[i] + a53 * k3[i] + a54 * k4[i]);
}
f(a, x + c5 * h, k5, n, o);
for (int i = 0; i < n; i++)
{
a[i] = y[i] + h * (a61 * k1[i] + a62 * k2[i] + a63 * k3[i] + a64 * k4[i] + a65 * k5[i]);
}
f(a, x + c6 * h, k6, n, o);
for (int i = 0; i < n; i++)
{
a[i] = y[i] + h * (a71 * k1[i] + a73 * k3[i] + a74 * k4[i] + a75 * k5[i] + a76 * k6[i]);
}
f(a, x + c7 * h, k7, n, o);
for (int i = 0; i < n; i++)
{
err[i] = k1[i] * (b1 - b1p) + k3[i] * (b3 - b3p) + k4[i] * (b4 - b4p) + k5[i] * (b5 - b5p) + k6[i] * (b6 - b6p) + k7[i] * (b7 - b7p);
}
double error = 0;
for (int i = 0; i < n; i++)
{
// FIXME: look at dopri fortran code to see how they generate this
error = Math.Max(error, Math.Abs(err[i]));
}
double s = 0.84 * Math.Pow(eps / error, 1.0 / 5.0);
int evt = -1;
if (error < eps || at_hmin || h_restart != 0)
{
int sign = 0;
if (EvtFuns != null)
{
for (int i = 0; i < EvtFuns.Length; i++)
{
EvtFun e = EvtFuns[i];
double e1 = e(y, x, n, o);
double e2 = e(a, x + h, n, o);
if (e1 * e2 < 0)
{
evt = i;
// sign is passed to Brent to ensure we select a value on the other side of the event
sign = Math.Sign(e2);
break;
}
}
}
if (evt < 0 || h_restart != 0)
{
fsal = true; // advancing so use k7 for k1 next time
for (int i = 0; i < n; i++)
{
y[i] = a[i]; // FSAL
}
x = x + h;
if (xlist != null && ylist != null)
{
double[] ydup = new double[n];
for (int i = 0; i < n; i++)
{
ydup[i] = y[i];
}
ylist.Add(ydup);
xlist.Add(x);
}
if (h_restart != 0)
{
h = h_restart;
s = 1;
h_restart = 0;
}
}
else
{
fsal = false;
s = 1;
if (h_restart == 0)
{
h_restart = h;
}
Brent.Root((newh, a1, a2, a3, a4, o2) => EvtWrapper(EvtFuns[evt], newh, x, a1, a2, x + h, a3, a4, n, o2), 0, h, 1e-15, out h, out _, o, y, k1, a, k7, sign: sign);
}
}
else
{
fsal = false; // rewinding so k7 is no longer valid
}
if (s < 0.1)
{
s = 0.1;
}
if (s > 4)
{
s = 4;
}
h = h * s;
if (maxiter > 0 && niter++ >= maxiter)
{
throw new ArgumentException("maximum iterations exceeded");
}
}
for (int i = 0; i < n; i++)
{
ytbl[i][j] = y[i];
}
j++;
}
}
public void RKF45(ODEFun f, object o)
{
double[] k1 = new double[n];
double[] k2 = new double[n];
double[] k3 = new double[n];
double[] k4 = new double[n];
double[] k5 = new double[n];
double[] k6 = new double[n];
double[] a = new double[n]; // accumulator
double[] err = new double[n]; // error
double[] z = new double[n]; // left as zeros
double h = hstart;
double x = xtbl[0];
double[] y = new double[n];
double[] dy = new double[n];
ytbl = new double[n][]; // output
for (int i = 0; i < n; i++)
{
ytbl[i] = new double[xtbl.Length];
}
y0.CopyTo(y, 0);
// auto-guess starting value of h based on smallest dx in xtbl
if (h == 0)
{
double v = Math.Abs(xtbl[1] - xtbl[0]);
for (int i = 1; i < xtbl.Length - 1; i++)
{
v = Math.Min(v, Math.Abs(xtbl[i + 1] - xtbl[i]));
}
h = 0.001 * v;
}
// xtbl controls the direcction of integration, the sign of h is not relevant
h = Math.Sign(xtbl[1] - xtbl[0]) * Math.Abs(h);
bool at_hmin;
double niter = 0;
int j = 0;
// add the initial conditions
for (int i = 0; i < n; i++)
{
ytbl[i][j] = y[i];
}
double[] ydup2 = new double[n];
y.CopyTo(ydup2, 0);
ylist.Add(ydup2);
xlist.Add(x);
j++;
while (j < xtbl.Length)
{
double xf = xtbl[j];
while ((h > 0) ? x <xf : x> xf)
{
at_hmin = false;
if (hmax > 0 && Math.Abs(h) > hmax)
{
h = hmax * Math.Sign(h);
}
if (hmin > 0 && Math.Abs(h) < hmin)
{
h = hmin * Math.Sign(h);
at_hmin = true;
}
// we may violate hmin in order to exactly hit the boundary conditions
if (Math.Abs(h) > Math.Abs(xf - x))
{
h = xf - x;
}
z.CopyTo(k1, 0);
f(y, x, dy, o);
rkadd(k1, dy, h);
y.CopyTo(a, 0);
rkadd(a, k1, 0.25);
z.CopyTo(k2, 0);
f(a, x + 0.25 * h, dy, o);
rkadd(k2, dy, h);
y.CopyTo(a, 0);
rkadd(a, k1, 3.0 / 32.0);
rkadd(a, k2, 9.0 / 32.0);
z.CopyTo(k3, 0);
f(a, x + 3 * h / 8, dy, o);
rkadd(k3, dy, h);
y.CopyTo(a, 0);
rkadd(a, k1, 1932.0 / 2197.0);
rkadd(a, k2, -7200.0 / 2197.0);
rkadd(a, k3, 7296.0 / 2197.0);
z.CopyTo(k4, 0);
f(a, x + 12 * h / 13, dy, o);
rkadd(k4, dy, h);
y.CopyTo(a, 0);
rkadd(a, k1, 439.0 / 216.0);
rkadd(a, k2, -8.0);
rkadd(a, k3, 3680.0 / 513.0);
rkadd(a, k4, -845.0 / 4104.0);
z.CopyTo(k5, 0);
f(a, x + h, dy, o);
rkadd(k5, dy, h);
y.CopyTo(a, 0);
rkadd(a, k1, -8.0 / 27.0);
rkadd(a, k2, 2.0);
rkadd(a, k3, -3544.0 / 2565.0);
rkadd(a, k4, 1859.0 / 4104.0);
rkadd(a, k5, -11.0 / 40.0);
z.CopyTo(k6, 0);
f(a, x + 0.5 * h, dy, o);
rkadd(k6, dy, h);
z.CopyTo(err, 0);
rkadd(err, k1, 1.0 / 360.0);
rkadd(err, k3, -128.0 / 4275.0);
rkadd(err, k4, -2197.0 / 75240.0);
rkadd(err, k5, 1.0 / 50.0);
rkadd(err, k6, 2.0 / 55.0);
double error = 0;
for (int i = 0; i < err.Length; i++)
{
error = Math.Max(error, Math.Abs(err[i] / h));
}
double s = Math.Pow(0.5 * eps / error, 0.25);
if (error < eps || at_hmin)
{
rkadd(y, k1, 25.0 / 216.0);
rkadd(y, k3, 1408.0 / 2565.0);
rkadd(y, k4, 2197.0 / 4104.0);
rkadd(y, k5, -0.2);
x = x + h;
if (allvals)
{
double[] ydup = new double[n];
Array.Copy(y, ydup, n);
ylist.Add(ydup);
xlist.Add(x);
}
}
if (s < 0.1)
{
s = 0.1;
}
if (s > 4)
{
s = 4;
}
h = h * s;
if (maxiter > 0 && niter++ >= maxiter)
{
throw new ArgumentException("maximum iterations exceeded");
}
}
for (int i = 0; i < n; i++)
{
ytbl[i][j] = y[i];
}
j++;
}
}
