// 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++; } }