final double[] yTmp = new double[y.length];
final double[] predictedScaled = new double[y.length];
Array2DRowRealMatrix nordsieckTmp = null;
// set up two interpolators 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);
double hNew = stepSize;
interpolator.rescale(hNew);
isLastStep = false;
do {
double error = 10;
while (error >= 1.0) {
stepSize = hNew;
// predict a first estimate of the state at step end (P in the PECE sequence)
final double stepEnd = stepStart + stepSize;
interpolator.setInterpolatedTime(stepEnd);
final ExpandableStatefulODE expandable = getExpandable();
final EquationsMapper primary = expandable.getPrimaryMapper();
primary.insertEquationData(interpolator.getInterpolatedState(), yTmp);
int index = 0;
for (final EquationsMapper secondary : expandable.getSecondaryMappers()) {
secondary.insertEquationData(interpolator.getInterpolatedSecondaryState(index), yTmp);
++index;
}
// evaluate a first estimate of the derivative (first E in the PECE sequence)
computeDerivatives(stepEnd, yTmp, yDot);
// update Nordsieck vector
for (int j = 0; j < y0.length; ++j) {
predictedScaled[j] = stepSize * yDot[j];
}
nordsieckTmp = updateHighOrderDerivativesPhase1(nordsieck);
updateHighOrderDerivativesPhase2(scaled, predictedScaled, nordsieckTmp);
// apply correction (C in the PECE sequence)
error = nordsieckTmp.walkInOptimizedOrder(new Corrector(y, predictedScaled, yTmp));
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);
}
}
// evaluate a final estimate of the derivative (second E in the PECE sequence)
final double stepEnd = stepStart + stepSize;
computeDerivatives(stepEnd, yTmp, yDot);
// update Nordsieck vector
final double[] correctedScaled = new double[y0.length];
for (int j = 0; j < y0.length; ++j) {
correctedScaled[j] = stepSize * yDot[j];
}
updateHighOrderDerivativesPhase2(predictedScaled, correctedScaled, nordsieckTmp);
// discrete events handling
System.arraycopy(yTmp, 0, y, 0, y.length);
interpolator.reinitialize(stepEnd, stepSize, correctedScaled, nordsieckTmp);
interpolator.storeTime(stepStart);
interpolator.shift();
interpolator.storeTime(stepEnd);
stepStart = acceptStep(interpolator, y, yDot, t);
scaled = correctedScaled;
nordsieck = nordsieckTmp;
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);
// dispatch results