public void ChangeStep()
{
dt = dt / 2.0;
if (dt < epsilon)
{
throw new ArgumentException("Cannot generate numerical solution");
}
zn = NordsieckState.Rescale(zn, 0.5d);
}
/// <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>
private void InternalInitialize(double t0, double[] x0, Action <double, double[], double[]> f, GearsBDFOptions opts)
{
if (null == _denseJacobianEvaluation && null == _sparseJacobianEvaluation)
{
throw new InvalidProgramException("Ooops, how could this happen?");
}
double t = t0;
var x = (double[])x0.Clone();
n = x0.Length;
this.f = f;
this.opts = opts;
//Initial step size.
_dydt = _dydt ?? new double[n];
var dt = EvaluateRatesAndGetDt(t0, x0, _dydt);
var resdt = dt;
int qmax = 5;
int qcurr = 2;
_zn_saved = new DoubleMatrix(n, qmax + 1);
currstate = new NordsieckState(n, qmax, qcurr, dt, t, x0, _dydt);
isIterationFailed = false;
tout = t0;
xout = (double[])x0.Clone();
// ---------------------------------------------------------------------------------------------------
// End of initialize
// ---------------------------------------------------------------------------------------------------
// Firstly, return initial point
// EvaluateInternally(t0, true, out t0, xout);
_initializationState = InitializationState.NotInitialized;
}
/// <summary>
/// Execute predictor-corrector scheme for Nordsieck's method
/// </summary>
/// <param name="flag"></param>
/// <param name="f">Evaluation of the deriatives. First argument is time, second arg are the state variables, and 3rd arg is the array to accomodate the derivatives.</param>
/// <param name="denseJacobianEvaluation">Evaluation of the jacobian.</param>
/// <param name="sparseJacobianEvaluation">Evaluation of the jacobian as a sparse matrix. Either this or the previous arg must be valid.</param>
/// <param name="opts">current options</param>
/// <returns>en - current error vector</returns>
internal void PredictorCorrectorScheme(
ref bool flag,
Action <double, double[], double[]> f,
Func <double, double[], IROMatrix <double> > denseJacobianEvaluation,
Func <double, double[], SparseDoubleMatrix> sparseJacobianEvaluation,
GearsBDFOptions opts
)
{
NordsieckState currstate = this;
NordsieckState newstate = this;
int n = currstate._xn.Length;
VectorMath.Copy(currstate._en, ecurr);
VectorMath.Copy(currstate._xn, xcurr);
var x0 = currstate._xn;
MatrixMath.Copy(currstate._zn, zcurr); // zcurr now is old nordsieck matrix
var qcurr = currstate._qn; // current degree
var qmax = currstate._qmax; // max degree
var dt = currstate._dt;
var t = currstate._tn;
MatrixMath.Copy(currstate._zn, z0); // save Nordsieck matrix
//Tolerance computation factors
double Cq = Math.Pow(qcurr + 1, -1.0);
double tau = 1.0 / (Cq * Factorial(qcurr) * l[qcurr - 1][qcurr]);
int count = 0;
double Dq = 0.0, DqUp = 0.0, DqDown = 0.0;
double delta = 0.0;
//Scaling factors for the step size changing
//with new method order q' = q, q + 1, q - 1, respectively
double rSame, rUp, rDown;
if (null != denseJacobianEvaluation)
{
var J = denseJacobianEvaluation(t + dt, xcurr);
if (J.GetType() != P?.GetType())
{
AllocatePMatrixForJacobian(J);
}
do
{
MatrixMath.MapIndexed(J, dt * b[qcurr - 1], (i, j, aij, factor) => (i == j ? 1 : 0) - aij * factor, P, Zeros.AllowSkip); // P = Identity - J*dt*b[qcurr-1]
VectorMath.Copy(xcurr, xprev);
f(t + dt, xcurr, ftdt);
MatrixMath.CopyColumn(z0, 1, colExtract); // 1st derivative/dt
VectorMath.Map(ftdt, colExtract, ecurr, dt, (ff, c, e, local_dt) => local_dt * ff - c - e, gm); // gm = dt * f(t + dt, xcurr) - z0.GetColumn(1) - ecurr;
gaussSolver.SolveDestructive(P, gm, tmpVec1);
VectorMath.Add(ecurr, tmpVec1, ecurr); // ecurr = ecurr + P.SolveGE(gm);
VectorMath.Map(x0, ecurr, b[qcurr - 1], (x, e, local_b) => x + e * local_b, xcurr); // xcurr = x0 + b[qcurr - 1] * ecurr;
//Row dimension is smaller than zcurr has
int M_Rows = ecurr.Length;
int M_Columns = l[qcurr - 1].Length;
//So, "expand" the matrix
MatrixMath.MapIndexed(z0, (i, j, z) => z + (i < M_Rows && j < M_Columns ? ecurr[i] * l[qcurr - 1][j] : 0.0d), zcurr);
Dq = ToleranceNorm(ecurr, opts.RelativeTolerance, opts.AbsoluteTolerance, xprev);
var factor_deltaE = (1.0 / (qcurr + 2) * l[qcurr - 1][qcurr - 1]);
VectorMath.Map(ecurr, currstate._en, factor_deltaE, (e, c, factor) => (e - c) * factor, deltaE); // deltaE = (ecurr - currstate.en)*(1.0 / (qcurr + 2) * l[qcurr - 1][qcurr - 1])
DqUp = ToleranceNorm(deltaE, opts.RelativeTolerance, opts.AbsoluteTolerance, xcurr);
zcurr.CopyColumn(qcurr - 1, colExtract);
DqDown = ToleranceNorm(colExtract, opts.RelativeTolerance, opts.AbsoluteTolerance, xcurr);
delta = Dq / (tau / (2 * (qcurr + 2)));
count++;
} while (delta > 1.0d && count < opts.NumberOfIterations);
}
else if (null != sparseJacobianEvaluation)
{
SparseDoubleMatrix J = sparseJacobianEvaluation(t + dt, xcurr);
var P = new SparseDoubleMatrix(J.RowCount, J.ColumnCount);
do
{
J.MapSparseIncludingDiagonal((x, i, j) => (i == j ? 1 : 0) - x * dt * b[qcurr - 1], P);
VectorMath.Copy(xcurr, xprev);
f(t + dt, xcurr, ftdt);
MatrixMath.CopyColumn(z0, 1, colExtract);
VectorMath.Map(ftdt, colExtract, ecurr, (ff, c, e) => dt * ff - c - e, gm); // gm = dt * f(t + dt, xcurr) - z0.GetColumn(1) - ecurr;
gaussSolver.SolveDestructive(P, gm, tmpVec1);
VectorMath.Add(ecurr, tmpVec1, ecurr); // ecurr = ecurr + P.SolveGE(gm);
VectorMath.Map(x0, ecurr, (x, e) => x + e * b[qcurr - 1], xcurr); // xcurr = x0 + b[qcurr - 1] * ecurr;
//Row dimension is smaller than zcurr has
int M_Rows = ecurr.Length;
int M_Columns = l[qcurr - 1].Length;
//So, "expand" the matrix
MatrixMath.MapIndexed(z0, (i, j, z) => z + (i < M_Rows && j < M_Columns ? ecurr[i] * l[qcurr - 1][j] : 0.0d), zcurr);
Dq = ToleranceNorm(ecurr, opts.RelativeTolerance, opts.AbsoluteTolerance, xprev);
var factor_deltaE = (1.0 / (qcurr + 2) * l[qcurr - 1][qcurr - 1]);
VectorMath.Map(ecurr, currstate._en, (e, c) => (e - c) * factor_deltaE, deltaE); // deltaE = (ecurr - currstate.en)*(1.0 / (qcurr + 2) * l[qcurr - 1][qcurr - 1])
DqUp = ToleranceNorm(deltaE, opts.RelativeTolerance, opts.AbsoluteTolerance, xcurr);
DqDown = ToleranceNorm(zcurr.GetColumn(qcurr - 1), opts.RelativeTolerance, opts.AbsoluteTolerance, xcurr);
delta = Dq / (tau / (2 * (qcurr + 2)));
count++;
} while (delta > 1.0d && count < opts.NumberOfIterations);
}
else // neither denseJacobianEvaluation nor sparseJacobianEvaluation valid
{
throw new ArgumentNullException(nameof(denseJacobianEvaluation), "Either denseJacobianEvaluation or sparseJacobianEvaluation must be set!");
}
//======================================
var nsuccess = count < opts.NumberOfIterations ? currstate._nsuccess + 1 : 0;
if (count < opts.NumberOfIterations)
{
flag = false;
MatrixMath.Copy(zcurr, newstate._zn);
zcurr.CopyColumn(0, newstate._xn);
VectorMath.Copy(ecurr, newstate._en);
}
else
{
flag = true;
// MatrixMath.Copy(currstate.zn, newstate.zn); // null operation since currstate and newstate are identical
currstate._zn.CopyColumn(0, newstate._xn);
VectorMath.Copy(currstate._en, newstate._en); // null operation since currstate and newstate are identical
}
//Compute step size scaling factors
rUp = 0.0;
if (currstate._qn < currstate._qmax)
{
rUp = rUp = 1.0 / 1.4 / (Math.Pow(DqUp, 1.0 / (qcurr + 2)) + 1e-6);
}
rSame = 1.0 / 1.2 / (Math.Pow(Dq, 1.0 / (qcurr + 1)) + 1e-6);
rDown = 0.0;
if (currstate._qn > 1)
{
rDown = 1.0 / 1.3 / (Math.Pow(DqDown, 1.0 / (qcurr)) + 1e-6);
}
//======================================
_nsuccess = nsuccess >= _qn ? 0 : nsuccess;
//Step size scale operations
if (rSame >= rUp)
{
if (rSame <= rDown && nsuccess >= _qn && _qn > 1)
{
_qn = _qn - 1;
_Dq = DqDown;
for (int i = 0; i < n; i++)
{
for (int j = newstate._qn + 1; j < qmax + 1; j++)
{
_zn[i, j] = 0.0;
}
}
nsuccess = 0;
_rFactor = rDown;
}
else
{
// _qn = _qn;
_Dq = Dq;
_rFactor = rSame;
}
}
else
{
if (rUp >= rDown)
{
if (rUp >= rSame && nsuccess >= _qn && _qn < _qmax)
{
_qn = _qn + 1;
_Dq = DqUp;
_rFactor = rUp;
nsuccess = 0;
}
else
{
// _qn = _qn;
_Dq = Dq;
_rFactor = rSame;
}
}
else
{
if (nsuccess >= _qn && _qn > 1)
{
_qn = _qn - 1;
_Dq = DqDown;
for (int i = 0; i < n; i++)
{
for (int j = newstate._qn + 1; j < qmax + 1; j++)
{
_zn[i, j] = 0.0;
}
}
nsuccess = 0;
_rFactor = rDown;
}
else
{
// _qn = _qn;
_Dq = Dq;
_rFactor = rSame;
}
}
}
_dt = dt;
_tn = t;
}
/// <summary>
/// Execute predictor-corrector scheme for Nordsieck's method
/// </summary>
/// <param name="currstate"></param>
/// <param name="flag"></param>
/// <param name="f">right parts vector</param>
/// <param name="opts">current options</param>
/// <returns>en - current error vector</returns>
private static NordsieckState PredictorCorrectorScheme(NordsieckState currstate, ref bool flag, Func <double, Vector, Vector> f, Options opts)
{
int n = currstate.xn.Length;
var newstate = new NordsieckState();
var ecurr = currstate.en;
newstate.en = ecurr.Clone();
var xcurr = currstate.xn;
var x0 = currstate.xn;
var zcurr = currstate.zn.Clone();
var qcurr = currstate.qn;
var qmax = currstate.qmax;
var dt = currstate.dt;
var t = currstate.tn;
var z0 = currstate.zn.Clone();
//Tolerance computation factors
double Cq = Math.Pow(qcurr + 1, -1.0);
double tau = 1.0 / (Cq * Factorial(qcurr) * l[qcurr - 1][qcurr]);
int count = 0;
double Dq = 0.0, DqUp = 0.0, DqDown = 0.0;
double delta = 0.0;
//Scaling factors for the step size changing
//with new method order q' = q, q + 1, q - 1, respectively
double rSame, rUp, rDown;
var xprev = Vector.Zeros(n);
var gm = Vector.Zeros(n);
var deltaE = Vector.Zeros(n);
var M = Matrix.Identity(n, qmax - 1);
if (opts.SparseJacobian == null)
{
Matrix J = opts.Jacobian == null?NordsieckState.Jacobian(f, xcurr, t + dt) : opts.Jacobian;
Matrix P = Matrix.Identity(n, n) - J * dt * b[qcurr - 1];
do
{
xprev = xcurr.Clone();
gm = dt * f(t + dt, xcurr) - z0.CloneColumn(1) - ecurr;
ecurr = ecurr + P.SolveGE(gm);
xcurr = x0 + b[qcurr - 1] * ecurr;
//Row dimension is smaller than zcurr has
M = ecurr & l[qcurr - 1];
//So, "expand" the matrix
var MBig = Matrix.Identity(zcurr.RowDimension, zcurr.ColumnDimension);
for (int i = 0; i < zcurr.RowDimension; i++)
{
for (int j = 0; j < zcurr.ColumnDimension; j++)
{
MBig[i, j] = i < M.RowDimension && j < M.ColumnDimension ? M[i, j] : 0.0d;
}
}
zcurr = z0 + MBig;
Dq = ecurr.ToleranceNorm(opts.RelativeTolerance, opts.AbsoluteTolerance, xprev);
deltaE = ecurr - currstate.en;
deltaE *= (1.0 / (qcurr + 2) * l[qcurr - 1][qcurr - 1]);
DqUp = deltaE.ToleranceNorm(opts.RelativeTolerance, opts.AbsoluteTolerance, xcurr);
DqDown = zcurr.CloneColumn(qcurr - 1).ToleranceNorm(opts.RelativeTolerance, opts.AbsoluteTolerance, xcurr);
delta = Dq / (tau / (2 * (qcurr + 2)));
count++;
} while (delta > 1.0d && count < opts.NumberOfIterations);
}
else
{
SparseMatrix J = opts.SparseJacobian;
SparseMatrix P = SparseMatrix.Identity(n, n) - J * dt * b[qcurr - 1];
do
{
xprev = xcurr.Clone();
gm = dt * f(t + dt, xcurr) - z0.CloneColumn(1) - ecurr;
ecurr = ecurr + P.SolveGE(gm);
xcurr = x0 + b[qcurr - 1] * ecurr;
//Row dimension is smaller than zcurr has
M = ecurr & l[qcurr - 1];
//So, "expand" the matrix
var MBig = Matrix.Identity(zcurr.RowDimension, zcurr.ColumnDimension);
for (int i = 0; i < zcurr.RowDimension; i++)
{
for (int j = 0; j < zcurr.ColumnDimension; j++)
{
MBig[i, j] = i < M.RowDimension && j < M.ColumnDimension ? M[i, j] : 0.0d;
}
}
zcurr = z0 + MBig;
Dq = ecurr.ToleranceNorm(opts.RelativeTolerance, opts.AbsoluteTolerance, xprev);
deltaE = ecurr - currstate.en;
deltaE *= (1.0 / (qcurr + 2) * l[qcurr - 1][qcurr - 1]);
DqUp = deltaE.ToleranceNorm(opts.RelativeTolerance, opts.AbsoluteTolerance, xcurr);
DqDown = zcurr.CloneColumn(qcurr - 1).ToleranceNorm(opts.RelativeTolerance, opts.AbsoluteTolerance, xcurr);
delta = Dq / (tau / (2 * (qcurr + 2)));
count++;
} while (delta > 1.0d && count < opts.NumberOfIterations);
}
//======================================
var nsuccess = count < opts.NumberOfIterations ? currstate.nsuccess + 1 : 0;
if (count < opts.NumberOfIterations)
{
flag = false;
newstate.zn = zcurr.Clone();
newstate.xn = zcurr.CloneColumn(0);
newstate.en = ecurr.Clone();
}
else
{
flag = true;
newstate.zn = currstate.zn.Clone();
newstate.xn = currstate.zn.CloneColumn(0);
newstate.en = currstate.en.Clone();
}
//Compute step size scaling factors
rUp = 0.0;
if (currstate.qn < currstate.qmax)
{
rUp = rUp = 1.0 / 1.4 / (Math.Pow(DqUp, 1.0 / (qcurr + 2)) + 1e-6);
}
rSame = 1.0 / 1.2 / (Math.Pow(Dq, 1.0 / (qcurr + 1)) + 1e-6);
rDown = 0.0;
if (currstate.qn > 1)
{
rDown = 1.0 / 1.3 / (Math.Pow(DqDown, 1.0 / (qcurr)) + 1e-6);
}
//======================================
newstate.nsuccess = nsuccess >= currstate.qn ? 0 : nsuccess;
//Step size scale operations
if (rSame >= rUp)
{
if (rSame <= rDown && nsuccess >= currstate.qn && currstate.qn > 1)
{
newstate.qn = currstate.qn - 1;
newstate.Dq = DqDown;
for (int i = 0; i < n; i++)
{
for (int j = newstate.qn + 1; j < qmax + 1; j++)
{
newstate.zn[i, j] = 0.0;
}
}
nsuccess = 0;
newstate.rFactor = rDown;
}
else
{
newstate.qn = currstate.qn;
newstate.Dq = Dq;
newstate.rFactor = rSame;
}
}
else
{
if (rUp >= rDown)
{
if (rUp >= rSame && nsuccess >= currstate.qn && currstate.qn < currstate.qmax)
{
newstate.qn = currstate.qn + 1;
newstate.Dq = DqUp;
newstate.rFactor = rUp;
nsuccess = 0;
}
else
{
newstate.qn = currstate.qn;
newstate.Dq = Dq;
newstate.rFactor = rSame;
}
}
else
{
if (nsuccess >= currstate.qn && currstate.qn > 1)
{
newstate.qn = currstate.qn - 1;
newstate.Dq = DqDown;
for (int i = 0; i < n; i++)
{
for (int j = newstate.qn + 1; j < qmax + 1; j++)
{
newstate.zn[i, j] = 0.0;
}
}
nsuccess = 0;
newstate.rFactor = rDown;
}
else
{
newstate.qn = currstate.qn;
newstate.Dq = Dq;
newstate.rFactor = rSame;
}
}
}
newstate.qmax = qmax;
newstate.dt = dt;
newstate.tn = t;
return(newstate);
}
/// <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 <SolutionPoint> 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;
var 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 SolutionPoint(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 + dx[i] * dx[i] / (ywt[i] * ywt[i]);
}
dt = Math.Sqrt(tol / (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;
var 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);
var currstate = new NordsieckState
{
delta = 0.0d,
Dq = 0.0d,
dt = dt,
en = eold,
tn = t,
xn = x0,
qn = qcurr,
qmax = qmax,
nsuccess = 0,
zn = zn,
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 SolutionPoint(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 SolutionPoint(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);
}
}
}
}
