/// <summary>Modifies solution by adding time step between consequtive points at the end of
/// system state vector</summary>
/// <param name="solution">Solution sequence</param>
/// <returns>Solution with appended time step</returns>
public static IEnumerable <SolPoint> AppendStep(this IEnumerable <SolPoint> solution)
{
var en = solution.GetEnumerator();
if (!en.MoveNext())
{
yield break;
}
SolPoint prev = en.Current;
double[] temp = new double[prev.X.Length + 1];
Array.Copy(prev.X, temp, prev.X.Length);
yield return(new SolPoint(prev.T, new Vector((double[])temp.Clone())));
while (en.MoveNext())
{
SolPoint curr = en.Current;
Array.Copy(curr.X, temp, curr.X.Length);
temp[curr.X.Length] = curr.T - prev.T;
yield return(new SolPoint(curr.T, new Vector((double[])temp.Clone())));
prev = curr;
}
yield break;
}
/// <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;
}
}
static Vector RK4OneStep(ref SolPoint sp0, double dt, Func <double, Vector, Vector> f)
{
Vector x0 = sp0.X;
double t0 = sp0.T;
Vector f1 = f(t0, x0);
Vector xx = x0 + f1 * (dt / 2.0);
Vector f2 = f(t0 + dt / 2.0, xx);
xx = x0 + f2 * (dt / 2.0);
Vector f3 = f(t0 + dt / 2.0, xx);
xx = x0 + f3 * dt;
Vector f4 = f(t0 + dt, xx);
return(x0 + (dt / 6.0) * (f1 + 2.0 * f2 + 2.0 * f3 + f4)); //at t = t0 + dt
}
//yi+1 = yi + h (55y'i - 59y'i-1 + 37y'i-2 - 9y'i-3)/24 .
public static IEnumerable <SolPoint> Adams4(double t0, Vector y0, Func <double, Vector, Vector> f, Options opts)
{
const double ci = 55d / 24d, c1 = -59d / 24d, c2 = 37d / 24d, c3 = -9d / 24d;
Vector y_3 = y0;
double t_3 = t0;
int n = y0.Length;
double dt = opts.InitialStep;
if (dt == 0.0)
{
dt = defaultInitStep;
opts.InitialStep = dt;
}
//Output initial point
Vector f_3 = f(t_3, y_3);
var sp_3 = new SolPoint(t_3, y_3);
yield return(sp_3);
Vector y_2 = RK4OneStep(ref sp_3, dt, f);
double t_2 = t_3 + dt;
var f_2 = f(t_2, y_2);
var sp_2 = new SolPoint(t_2, y_2);
yield return(sp_2);
Vector y_1 = RK4OneStep(ref sp_2, dt, f);
double t_1 = t_2 + dt;
var f_1 = f(t_1, y_1);
var sp_1 = new SolPoint(t_1, y_1);
yield return(sp_1);
Vector y_i = RK4OneStep(ref sp_1, dt, f);
double t_i = t_1 + dt;
var f_i = f(t_i, y_i);
var sp_i = new SolPoint(t_i, y_i);
yield return(sp_i);
double t_i1 = t_i;
do
{
Vector y_i1 = y_i + dt * (ci * f_i + c1 * f_1 + c2 * f_2 + c3 * f_3);
t_i1 += dt;
var f_i1 = f(t_i1, y_i1);
var sp_i1 = new SolPoint(t_i1, y_i1);
yield return(sp_i1);
Vector.Swap(ref y_i1, ref y_i);
Vector.Swap(ref f_2, ref f_3);
Vector.Swap(ref f_1, ref f_2);
Vector.Swap(ref f_i, ref f_1);
Vector.Swap(ref f_i1, ref f_i);
} while(true);
//Vector f2 =
//первые три точки создаем при помощи РК4
//while(true) // Can produce any number of solution points
//{
// Vector xx = x + x1 * (dt / 2.0);
// Vector x2 = f(t + dt / 2.0,xx);
// xx = x + x2 * (dt / 2.0);
// Vector x3 = f(t + dt / 2.0,xx);
// xx = x + x3 * dt;
// Vector x4 = f(t + dt,xx);
// x = x + (dt / 6.0) * (x1 + 2.0 * x2 + 2.0 * x3 + x4);
// t = t + dt; //Go to the next point of our time grid
// x1 = f(t,x);
// yield return new SolPoint(t,x);
//}
}