private void EvaluateInternally(double?tout, out double t_result, double[] result)
{
if (_initializationState == InitializationState.NotInitialized) // not initialized so far
{
_initializationState = InitializationState.InitialValueReturned;
if (null == tout)
{
last_tout = t_result = currstate._tn;
currstate._zn.CopyColumn(0, result);
return;
}
}
else if (_initializationState == InitializationState.Initialized)
{
// we have to clone some of the code from below to here
// this is no good style, but a goto statement with a jump inside another code block will not work here.
if (tout.HasValue)
{
// Output data, but only if (i) we have requested a certain time point,
// and ii) as long as we can interpolate this point from the previous point and the current point
if (currstate._tn <= tout.Value && tout.Value <= currstate._tn + currstate._dt)
{
// VectorMath.Lerp(tout.Value, currstate.tn, xout, currstate.tn + currstate.dt, currstate.xn, result);
currstate.EvaluateYAtTime(tout.Value, result);
last_tout = t_result = tout.Value;
return;
}
}
else
{
if (currstate._tn == last_tout)
{
last_tout = t_result = currstate._tn + currstate._dt;
VectorMath.Copy(currstate._xn, result);
return;
}
}
VectorMath.Copy(currstate._xn, xout); // save x of this step
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.Rescale(r);
}
_initializationState = InitializationState.Initialized;
//Can produce any number of solution points
while (true)
{
// Reset fail flag
isIterationFailed = false;
// Predictor step
_zn_saved.CopyFrom(currstate._zn);
currstate.ZNew();
VectorMath.FillWith(currstate._en, 0); // TODO find out if this statement is neccessary
currstate._zn.CopyColumn(0, currstate._xn);
// Corrector step
currstate.PredictorCorrectorScheme(ref isIterationFailed, f, _denseJacobianEvaluation, _sparseJacobianEvaluation, opts);
if (isIterationFailed) // If iterations are not finished - bad convergence
{
currstate._zn.CopyFrom(_zn_saved); // copy saved state back
currstate._nsuccess = 0;
currstate.DivideStepBy2();
}
else // Iterations finished, i.e. did not fail
{
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.Rescale(r);
}
else // Iteration finished successfully
{
// Output data
if (tout.HasValue)
{
if (currstate._tn <= tout.Value && tout.Value <= currstate._tn + currstate._dt)
{
// VectorMath.Lerp(tout.Value, currstate.tn, xout, currstate.tn + currstate.dt, currstate.xn, result);
currstate.EvaluateYAtTime(tout.Value, result);
t_result = tout.Value;
return;
}
}
else
{
VectorMath.Copy(currstate._xn, result);
t_result = last_tout = currstate._tn + currstate._dt;
return;
}
VectorMath.Copy(currstate._xn, xout);
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.Rescale(r);
}
}
}
}
/// <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);
}
}
}
}