/// <summary>Interpolates solution at points with specified time step</summary> /// <param name="solution">Solution</param> /// <param name="delta">Time step</param> /// <returns>New sequence of solution points at moments i * delta</returns> /// <remarks>Linear intepolation is used to find phase vector between two solution points</remarks> public static IEnumerable <SolPoint> WithStep(this IEnumerable <SolPoint> solution, double delta) { var en = solution.GetEnumerator(); if (!en.MoveNext()) { yield break; } SolPoint prev = en.Current; double n = Math.Ceiling(prev.T / delta); double tout = n * delta; if (tout == prev.T) { yield return(prev); } n++; tout = n * delta; while (true) { if (!en.MoveNext()) { yield break; } SolPoint current = en.Current; while (current.T >= tout) { yield return(new SolPoint(tout, Vector.Lerp(tout, prev.T, prev.X, current.T, current.X))); n++; tout = n * delta; } prev = current; } }
/// <summary>Implementation of Runge-Kutta algoritm with per-point accurancy control from /// Dormand and Prince article. /// (J.R.Dormand, P.J.Prince, A family of embedded Runge-Kuttae formulae)</summary> /// <param name="t0">Left end of current time span</param> /// <param name="x0">Initial phase vector value</param> /// <param name="f">System right parts vector function</param> /// <param name="opts">Options used by solver</param> /// <example>Let our problem will be: /// dx/dt=y+1, /// dy/dt=-x+2. /// x(0)=0, y(0)=1. /// To solve it, we just have to write /// <code> /// var sol=Ode.RK547M(0,new Vector(0,1),(t,x)=>new Vector(y+1,-x+2)); /// </code> /// and then enumerate solution point from <see cref="System.IEnumerable"/> 'sol'. /// </example> /// <returns>Endless sequence of solution points</returns> public static IEnumerable <SolPoint> RK547M(double t0, Vector x0, Func <double, Vector, Vector> f, Options opts) { // Safety factors for automatic step control // See Solving Ordinary Differential Equations I Non stiff problems by E. Hairer, S.P. Norsett, G.Wanner // 2nd edition, Springer. // Safety factor is recommended on page 168 const double SafetyFactor = 0.8; const double MaxFactor = 5.0d; // Maximum possible step increase const double MinFactor = 0.2d; // Maximum possible step decrease int Refine = 4; Vector S = Vector.Zeros(Refine - 1); for (int i = 0; i < Refine - 1; i++) { S[i] = (double)(i + 1) / Refine; } double t = t0; Vector x = x0.Clone(); // Lower order approximation Vector x1 = x0.Clone(); // Higher order approximation int n = x0.Length; // Dimensions of the system // Initial step value. During computational process, we modify it. double dt = opts.InitialStep; // Create formulae parameters(a's,b's and c's) // In original article, a is a step array. double[][] a = new double[6][]; a[0] = new double[] { 1 / 5d }; a[1] = new double[] { 3 / 40d, 9 / 40d }; a[2] = new double[] { 44 / 45d, -56 / 15d, 32 / 9d }; a[3] = new double[] { 19372 / 6561d, -25360 / 2187d, 64448 / 6561d, -212 / 729d }; a[4] = new double[] { 9017 / 3168d, -355 / 33d, 46732 / 5247d, 49 / 176d, -5103 / 18656d }; a[5] = new double[] { 35 / 384d, 0, 500 / 1113d, 125 / 192d, -2187 / 6784d, 11 / 84d }; Vector c = new Vector(0, 1 / 5d, 3 / 10d, 4 / 5d, 8 / 9d, 1, 1); // Coeffs for higher order Vector b1 = new Vector(5179 / 57600d, 0, 7571 / 16695d, 393 / 640d, -92097 / 339200d, 187 / 2100d, 1 / 40d); // Coeffs for lower order Vector b = new Vector(35 / 384d, 0, 500 / 1113d, 125 / 192d, -2187 / 6784d, 11 / 84d, 0); const int s = 7; // == c.Length double dt0; // Compute initial step (see E. Hairer book) if (opts.InitialStep == 0) { double d0 = 0.0d, d1 = 0.0d; double[] sc = new double[n]; var f0 = f(t0, x0); for (int i = 0; i < n; i++) { sc[i] = opts.AbsoluteTolerance + opts.RelativeTolerance * Math.Abs(x0[i]); d0 = Math.Max(d0, Math.Abs(x0[i]) / sc[i]); d1 = Math.Max(d1, Math.Abs(f0[i]) / sc[i]); } var h0 = Math.Min(d0, d1) < 1e-5 ? 1e-6 : 1e-2 * (d0 / d1); var f1 = f(t0 + h0, x0 + h0 * f0); double d2 = 0; for (int i = 0; i < n; i++) { d2 = Math.Max(d2, Math.Abs(f0[i] - f1[i]) / sc[i] / h0); } dt = Math.Max(d1, d2) <= 1e-15 ? Math.Max(1e-6, h0 * 1e-3) : Math.Pow(1e-2 / Math.Max(d1, d2), 1 / 5d); if (dt > 100 * h0) { dt = 100 * h0; } } else { dt = opts.InitialStep; } dt0 = dt; // Output initial point double tout = t0; Vector xout = x0.Clone(); if (opts.OutputStep > 0) // Store previous solution point if OutputStep is specified (non-zero) { tout += opts.OutputStep; } yield return(new SolPoint(t0, x0.Clone())); // Pre-allocate arrays Vector[] k = new Vector[s]; Vector[] x2 = new Vector[s - 1]; for (int i = 0; i < s - 1; i++) { x2[i] = Vector.Zeros(n); } Vector prevX = Vector.Zeros(n); double prevErr = 1.0d; // Previous error, used for step PI-filtering double prevDt; while (true) // Main loop - produces numerical solution { Vector.Copy(x1, prevX); double e = 0.0d; // error factor, should be < 1 //int ii = 0; do { prevDt = dt; Vector.Copy(prevX, x1); // Compute internal method variables. k[0] = dt * f(t, x1); for (int i = 1; i < s; i++) { Vector.Copy(x1, x2[i - 1]); for (int j = 0; j < i; j++) { x2[i - 1].MulAdd(k[j], a[i - 1][j]); } k[i] = dt * f(t + dt * c[i], x2[i - 1]); } //Try to compute X in the next time point. Vector.Copy(prevX, x); for (int l = 0; l < s; l++) { x.MulAdd(k[l], b[l]); x1.MulAdd(k[l], b1[l]); } // Compute error (see p. 168 of book indicated above) // error compulation in L-infinity norm is commented e = Math.Abs(x[0] - x1[0]) / Math.Max(opts.AbsoluteTolerance, opts.RelativeTolerance * Math.Max(Math.Abs(prevX[0]), Math.Abs(x1[0]))); for (int i = 1; i < n; i++) { e = Math.Max(e, Math.Abs(x[i] - x1[i]) / Math.Max(opts.AbsoluteTolerance, opts.RelativeTolerance * Math.Max(Math.Abs(prevX[i]), Math.Abs(x1[i])))); } // PI-filter. Beta = 0.08 dt = e == 0 ? dt : dt *Math.Min(MaxFactor, Math.Max(MinFactor, SafetyFactor *Math.Pow(1.0d / e, 1.0d / 5.0d) * Math.Pow(prevErr, 0.08d))); if (opts.MaxStep < Double.MaxValue) { dt = Math.Min(dt, opts.MaxStep); } //if (opts.MinStep > 0) dt = Math.Max(dt, opts.MinStep); if (Double.IsNaN(dt)) { throw new ArgumentException("Derivatives function returned NaN"); } if (dt < 1e-12) { throw new ArgumentException("Cannot generate numerical solution"); } } while(e > 1.0d); // Repeat until solving vector euclidean norm doesn't satisfy break condition prevErr = e; // Output data if (opts.OutputStep > 0) // Output points with specified step { while (t <= tout && tout <= t + prevDt) { yield return(new SolPoint(tout, Vector.Lerp(tout, t, xout, t + prevDt, x1))); tout += opts.OutputStep; } } else if (Refine > 1) // Output interpolated points set by Refine property { Vector ts = Vector.Zeros(S.Length); for (int i = 0; i < S.Length; i++) { ts[i] = t + prevDt * S[i]; } var ys = RKinterp(S, xout, k); for (int i = 0; i < S.Length; i++) { yield return(new SolPoint(ts[i], ys[i])); } } yield return(new SolPoint(t + prevDt, x1.Clone())); // Update current time and state t = t + prevDt; Vector.Copy(x1, xout); } }
/// <summary> /// Implementation of Gear's BDF method with dynamically changed step size and order. Order changes between 1 and 3. /// </summary> /// <param name="t0">Initial time point</param> /// <param name="x0">Initial phase vector</param> /// <param name="f">Right parts of the system</param> /// <param name="opts">Options for accuracy control and initial step size</param> /// <returns>Sequence of infinite number of solution points.</returns> public static IEnumerable <SolPoint> GearBDF(double t0, Vector x0, Func <double, Vector, Vector> f, Options opts) { double t = t0; Vector x = x0.Clone(); int n = x0.Length; double tout = t0; Vector xout = new Vector(); if (opts.OutputStep > 0) // Store previous solution point if OutputStep is specified (non-zero) { xout = x0.Clone(); tout += opts.OutputStep; } // Firstly, return initial point yield return(new SolPoint(t0, x0.Clone())); //Initial step size. Vector dx = f(t0, x0).Clone(); double dt; if (opts.InitialStep != 0) { dt = opts.InitialStep; } else { var tol = opts.RelativeTolerance; var ewt = Vector.Zeros(n); var ywt = Vector.Zeros(n); var sum = 0.0; for (int i = 0; i < n; i++) { ewt[i] = opts.RelativeTolerance * Math.Abs(x[i]) + opts.AbsoluteTolerance; ywt[i] = ewt[i] / tol; sum = sum + (double)dx[i] * dx[i] / (ywt[i] * ywt[i]); } dt = Math.Sqrt(tol / ((double)1.0d / (ywt[0] * ywt[0]) + sum / n)); } dt = Math.Min(dt, opts.MaxStep); var resdt = dt; int qmax = 5; int qcurr = 2; //Compute Nordstieck's history matrix at t=t0; Matrix zn = new Matrix(n, qmax + 1); for (int i = 0; i < n; i++) { zn[i, 0] = x[i]; zn[i, 1] = dt * dx[i]; for (int j = qcurr; j < qmax + 1; j++) { zn[i, j] = 0.0d; } } var eold = Vector.Zeros(n); NordsieckState currstate = new NordsieckState(); currstate.delta = 0.0d; currstate.Dq = 0.0d; currstate.dt = dt; currstate.en = eold; currstate.tn = t; currstate.xn = x0; currstate.qn = qcurr; currstate.qmax = qmax; currstate.nsuccess = 0; currstate.zn = zn; currstate.rFactor = 1.0d; bool isIterationFailed = false; //Can produce any number of solution points while (true) { // Reset fail flag isIterationFailed = false; // Predictor step var z0 = currstate.zn.Clone(); currstate.zn = NordsieckState.ZNew(currstate.zn); currstate.en = Vector.Zeros(n); currstate.xn = currstate.zn.CloneColumn(0); // Corrector step currstate = PredictorCorrectorScheme(currstate, ref isIterationFailed, f, opts); if (isIterationFailed) // If iterations are not finished - bad convergence { currstate.zn = z0; currstate.nsuccess = 0; currstate.ChangeStep(); } else // Iterations finished { var r = Math.Min(1.1d, Math.Max(0.2d, currstate.rFactor)); if (currstate.delta >= 1.0d) { if (opts.MaxStep < Double.MaxValue) { r = Math.Min(r, opts.MaxStep / currstate.dt); } if (opts.MinStep > 0) { r = Math.Max(r, opts.MinStep / currstate.dt); } r = Math.Min(r, opts.MaxScale); r = Math.Max(r, opts.MinScale); currstate.dt = currstate.dt * r; // Decrease step currstate.zn = NordsieckState.Rescale(currstate.zn, r); } else { // Output data if (opts.OutputStep > 0) // Output points with specified step { while (currstate.tn <= tout && tout <= currstate.tn + currstate.dt) { yield return(new SolPoint(tout, Vector.Lerp(tout, currstate.tn, xout, currstate.tn + currstate.dt, currstate.xn))); tout += opts.OutputStep; } Vector.Copy(currstate.xn, xout); } else // Output each point { yield return(new SolPoint(currstate.tn + currstate.dt, currstate.xn)); } currstate.tn = currstate.tn + currstate.dt; if (opts.MaxStep < Double.MaxValue) { r = Math.Min(r, opts.MaxStep / currstate.dt); } if (opts.MinStep > 0) { r = Math.Max(r, opts.MinStep / currstate.dt); } r = Math.Min(r, opts.MaxScale); r = Math.Max(r, opts.MinScale); currstate.dt = currstate.dt * r; currstate.zn = NordsieckState.Rescale(currstate.zn, r); } } } }