// set up integration control objects
for (StepHandler handler : stepHandlers) {
handler.reset();
}
CombinedEventsManager manager = addEndTimeChecker(t0, t, eventsHandlersManager);
// compute the initial Nordsieck vector using the configured starter integrator
start(t0, y, t);
interpolator.reinitialize(stepStart, stepSize, scaled, nordsieck);
interpolator.storeTime(stepStart);
double hNew = stepSize;
interpolator.rescale(hNew);
boolean lastStep = false;
while (!lastStep) {
// shift all data
interpolator.shift();
double error = 0;
for (boolean loop = true; loop;) {
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);
System.arraycopy(interpolator.getInterpolatedState(), 0, yTmp, 0, y0.length);
// evaluate a first estimate of the derivative (first E in the PECE sequence)
computeDerivatives(stepEnd, yTmp, 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);
// apply correction (C in the PECE sequence)
error = nordsieckTmp.walkInOptimizedOrder(new Corrector(y, predictedScaled, yTmp));
if (error <= 1.0) {
// evaluate a final estimate of the derivative (second E in the PECE sequence)
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
interpolatorTmp.reinitialize(stepEnd, stepSize, correctedScaled, nordsieckTmp);
interpolatorTmp.storeTime(stepStart);
interpolatorTmp.shift();
interpolatorTmp.storeTime(stepEnd);
if (manager.evaluateStep(interpolatorTmp)) {
final double dt = manager.getEventTime() - stepStart;
if (Math.abs(dt) <= Math.ulp(stepStart)) {
// we cannot simply truncate the step, reject the current computation
// and let the loop compute another state with the truncated step.
// it is so small (much probably exactly 0 due to limited accuracy)
// that the code above would fail handling it.
// So we set up an artificial 0 size step by copying states
interpolator.storeTime(stepStart);
System.arraycopy(y, 0, yTmp, 0, y0.length);
hNew = 0;
stepSize = 0;
loop = false;
} else {
// reject the step to match exactly the next switch time
hNew = dt;
interpolator.rescale(hNew);
}
} else {
// accept the step
scaled = correctedScaled;
nordsieck = nordsieckTmp;
interpolator.reinitialize(stepEnd, stepSize, scaled, nordsieck);
loop = false;
}
} else {
// 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);
}
}
// the step has been accepted (may have been truncated)
final double nextStep = stepStart + stepSize;
System.arraycopy(yTmp, 0, y, 0, n);
interpolator.storeTime(nextStep);
manager.stepAccepted(nextStep, y);
lastStep = manager.stop();
// provide the step data to the step handler
for (StepHandler handler : stepHandlers) {
interpolator.setInterpolatedTime(nextStep);
handler.handleStep(interpolator, lastStep);
}
stepStart = nextStep;
if (!lastStep && manager.reset(stepStart, y)) {
// 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);