final double[] y0 = equations.getCompleteState();
final double[] y = y0.clone();
final double[] yDot = new double[y.length];
// set up an interpolator sharing the integrator arrays
final NordsieckStepInterpolator interpolator = new NordsieckStepInterpolator();
interpolator.reinitialize(y, forward,
equations.getPrimaryMapper(), equations.getSecondaryMappers());
// set up integration control objects
initIntegration(equations.getTime(), y0, t);
// compute the initial Nordsieck vector using the configured starter integrator
start(equations.getTime(), y, t);
interpolator.reinitialize(stepStart, stepSize, scaled, nordsieck);
interpolator.storeTime(stepStart);
final int lastRow = nordsieck.getRowDimension() - 1;
// reuse the step that was chosen by the starter integrator
double hNew = stepSize;
interpolator.rescale(hNew);
// main integration loop
isLastStep = false;
do {
double error = 10;
while (error >= 1.0) {
stepSize = hNew;
// evaluate error using the last term of the Taylor expansion
error = 0;
for (int i = 0; i < mainSetDimension; ++i) {
final double yScale = FastMath.abs(y[i]);
final double tol = (vecAbsoluteTolerance == null) ?
(scalAbsoluteTolerance + scalRelativeTolerance * yScale) :
(vecAbsoluteTolerance[i] + vecRelativeTolerance[i] * yScale);
final double ratio = nordsieck.getEntry(lastRow, i) / tol;
error += ratio * ratio;
}
error = FastMath.sqrt(error / mainSetDimension);
if (error >= 1.0) {
// reject the step and attempt to reduce error by stepsize control
final double factor = computeStepGrowShrinkFactor(error);
hNew = filterStep(stepSize * factor, forward, false);
interpolator.rescale(hNew);
}
}
// predict a first estimate of the state at step end
final double stepEnd = stepStart + stepSize;
interpolator.shift();
interpolator.setInterpolatedTime(stepEnd);
final ExpandableStatefulODE expandable = getExpandable();
final EquationsMapper primary = expandable.getPrimaryMapper();
primary.insertEquationData(interpolator.getInterpolatedState(), y);
int index = 0;
for (final EquationsMapper secondary : expandable.getSecondaryMappers()) {
secondary.insertEquationData(interpolator.getInterpolatedSecondaryState(index), y);
++index;
}
// evaluate the derivative
computeDerivatives(stepEnd, y, yDot);
// update Nordsieck vector
final double[] predictedScaled = new double[y0.length];
for (int j = 0; j < y0.length; ++j) {
predictedScaled[j] = stepSize * yDot[j];
}
final Array2DRowRealMatrix nordsieckTmp = updateHighOrderDerivativesPhase1(nordsieck);
updateHighOrderDerivativesPhase2(scaled, predictedScaled, nordsieckTmp);
interpolator.reinitialize(stepEnd, stepSize, predictedScaled, nordsieckTmp);
// discrete events handling
interpolator.storeTime(stepEnd);
stepStart = acceptStep(interpolator, y, yDot, t);
scaled = predictedScaled;
nordsieck = nordsieckTmp;
interpolator.reinitialize(stepEnd, stepSize, scaled, nordsieck);
if (!isLastStep) {
// prepare next step
interpolator.storeTime(stepStart);
if (resetOccurred) {
// some events handler has triggered changes that
// invalidate the derivatives, we need to restart from scratch
start(stepStart, y, t);
interpolator.reinitialize(stepStart, stepSize, scaled, nordsieck);
}
// stepsize control for next step
final double factor = computeStepGrowShrinkFactor(error);
final double scaledH = stepSize * factor;
final double nextT = stepStart + scaledH;
final boolean nextIsLast = forward ? (nextT >= t) : (nextT <= t);
hNew = filterStep(scaledH, forward, nextIsLast);
final double filteredNextT = stepStart + hNew;
final boolean filteredNextIsLast = forward ? (filteredNextT >= t) : (filteredNextT <= t);
if (filteredNextIsLast) {
hNew = t - stepStart;
}
interpolator.rescale(hNew);
}
} while (!isLastStep);