in commons-math-legacy/src/main/java/org/apache/commons/math4/legacy/ode/nonstiff/EmbeddedRungeKuttaFieldIntegrator.java [213:338]
public FieldODEStateAndDerivative<T> integrate(final FieldExpandableODE<T> equations,
final FieldODEState<T> initialState, final T finalTime)
throws NumberIsTooSmallException, DimensionMismatchException,
MaxCountExceededException, NoBracketingException {
sanityChecks(initialState, finalTime);
final T t0 = initialState.getTime();
final T[] y0 = equations.getMapper().mapState(initialState);
setStepStart(initIntegration(equations, t0, y0, finalTime));
final boolean forward = finalTime.subtract(initialState.getTime()).getReal() > 0;
// create some internal working arrays
final int stages = c.length + 1;
T[] y = y0;
final T[][] yDotK = MathArrays.buildArray(getField(), stages, -1);
final T[] yTmp = MathArrays.buildArray(getField(), y0.length);
// set up integration control objects
T hNew = getField().getZero();
boolean firstTime = true;
// main integration loop
setIsLastStep(false);
do {
// iterate over step size, ensuring local normalized error is smaller than 1
T error = getField().getZero().add(10);
while (error.subtract(1.0).getReal() >= 0) {
// first stage
y = equations.getMapper().mapState(getStepStart());
yDotK[0] = equations.getMapper().mapDerivative(getStepStart());
if (firstTime) {
final T[] scale = MathArrays.buildArray(getField(), mainSetDimension);
if (vecAbsoluteTolerance == null) {
for (int i = 0; i < scale.length; ++i) {
scale[i] = y[i].abs().multiply(scalRelativeTolerance).add(scalAbsoluteTolerance);
}
} else {
for (int i = 0; i < scale.length; ++i) {
scale[i] = y[i].abs().multiply(vecRelativeTolerance[i]).add(vecAbsoluteTolerance[i]);
}
}
hNew = initializeStep(forward, getOrder(), scale, getStepStart(), equations.getMapper());
firstTime = false;
}
setStepSize(hNew);
if (forward) {
if (getStepStart().getTime().add(getStepSize()).subtract(finalTime).getReal() >= 0) {
setStepSize(finalTime.subtract(getStepStart().getTime()));
}
} else {
if (getStepStart().getTime().add(getStepSize()).subtract(finalTime).getReal() <= 0) {
setStepSize(finalTime.subtract(getStepStart().getTime()));
}
}
// next stages
for (int k = 1; k < stages; ++k) {
for (int j = 0; j < y0.length; ++j) {
T sum = yDotK[0][j].multiply(a[k-1][0]);
for (int l = 1; l < k; ++l) {
sum = sum.add(yDotK[l][j].multiply(a[k-1][l]));
}
yTmp[j] = y[j].add(getStepSize().multiply(sum));
}
yDotK[k] = computeDerivatives(getStepStart().getTime().add(getStepSize().multiply(c[k-1])), yTmp);
}
// estimate the state at the end of the step
for (int j = 0; j < y0.length; ++j) {
T sum = yDotK[0][j].multiply(b[0]);
for (int l = 1; l < stages; ++l) {
sum = sum.add(yDotK[l][j].multiply(b[l]));
}
yTmp[j] = y[j].add(getStepSize().multiply(sum));
}
// estimate the error at the end of the step
error = estimateError(yDotK, y, yTmp, getStepSize());
if (error.subtract(1.0).getReal() >= 0) {
// reject the step and attempt to reduce error by stepsize control
final T factor = RealFieldElement.min(maxGrowth,
RealFieldElement.max(minReduction, safety.multiply(error.pow(exp))));
hNew = filterStep(getStepSize().multiply(factor), forward, false);
}
}
final T stepEnd = getStepStart().getTime().add(getStepSize());
final T[] yDotTmp = (fsal >= 0) ? yDotK[fsal] : computeDerivatives(stepEnd, yTmp);
final FieldODEStateAndDerivative<T> stateTmp = new FieldODEStateAndDerivative<>(stepEnd, yTmp, yDotTmp);
// local error is small enough: accept the step, trigger events and step handlers
System.arraycopy(yTmp, 0, y, 0, y0.length);
setStepStart(acceptStep(createInterpolator(forward, yDotK, getStepStart(), stateTmp, equations.getMapper()),
finalTime));
if (!isLastStep()) {
// stepsize control for next step
final T factor = RealFieldElement.min(maxGrowth,
RealFieldElement.max(minReduction, safety.multiply(error.pow(exp))));
final T scaledH = getStepSize().multiply(factor);
final T nextT = getStepStart().getTime().add(scaledH);
final boolean nextIsLast = forward ?
nextT.subtract(finalTime).getReal() >= 0 :
nextT.subtract(finalTime).getReal() <= 0;
hNew = filterStep(scaledH, forward, nextIsLast);
final T filteredNextT = getStepStart().getTime().add(hNew);
final boolean filteredNextIsLast = forward ?
filteredNextT.subtract(finalTime).getReal() >= 0 :
filteredNextT.subtract(finalTime).getReal() <= 0;
if (filteredNextIsLast) {
hNew = finalTime.subtract(getStepStart().getTime());
}
}
} while (!isLastStep());
final FieldODEStateAndDerivative<T> finalState = getStepStart();
resetInternalState();
return finalState;
}