private <T extends RealFieldElement<T>> RungeKuttaStepInterpolator convertInterpolator(
final RungeKuttaFieldStepInterpolator<T> fieldInterpolator,
final FirstOrderFieldDifferentialEquations<T> eqn) {
RungeKuttaStepInterpolator regularInterpolator = null;
try {
String interpolatorName = fieldInterpolator.getClass().getName();
String integratorName = interpolatorName.replaceAll("Field", "");
@SuppressWarnings("unchecked")
Class<RungeKuttaStepInterpolator> clz =
(Class<RungeKuttaStepInterpolator>) Class.forName(integratorName);
regularInterpolator = clz.newInstance();
double[][] yDotArray = null;
java.lang.reflect.Field fYD = RungeKuttaFieldStepInterpolator.class.getDeclaredField("yDotK");
fYD.setAccessible(true);
@SuppressWarnings("unchecked")
T[][] fieldYDotk = (T[][]) fYD.get(fieldInterpolator);
yDotArray = new double[fieldYDotk.length][];
for (int i = 0; i < yDotArray.length; ++i) {
yDotArray[i] = new double[fieldYDotk[i].length];
for (int j = 0; j < yDotArray[i].length; ++j) {
yDotArray[i][j] = fieldYDotk[i][j].getReal();
}
}
double[] y = new double[yDotArray[0].length];
EquationsMapper primaryMapper = null;
EquationsMapper[] secondaryMappers = null;
java.lang.reflect.Field fMapper =
AbstractFieldStepInterpolator.class.getDeclaredField("mapper");
fMapper.setAccessible(true);
@SuppressWarnings("unchecked")
FieldEquationsMapper<T> mapper = (FieldEquationsMapper<T>) fMapper.get(fieldInterpolator);
java.lang.reflect.Field fStart = FieldEquationsMapper.class.getDeclaredField("start");
fStart.setAccessible(true);
int[] start = (int[]) fStart.get(mapper);
primaryMapper = new EquationsMapper(start[0], start[1]);
secondaryMappers = new EquationsMapper[mapper.getNumberOfEquations() - 1];
for (int i = 0; i < secondaryMappers.length; ++i) {
secondaryMappers[i] = new EquationsMapper(start[i + 1], start[i + 2]);
}
AbstractIntegrator dummyIntegrator =
new AbstractIntegrator("dummy") {
@Override
public void integrate(ExpandableStatefulODE equations, double t) {
Assert.fail("this method should not be called");
}
@Override
public void computeDerivatives(final double t, final double[] y, final double[] yDot) {
T fieldT = fieldInterpolator.getCurrentState().getTime().getField().getZero().add(t);
T[] fieldY =
MathArrays.buildArray(
fieldInterpolator.getCurrentState().getTime().getField(), y.length);
for (int i = 0; i < y.length; ++i) {
fieldY[i] =
fieldInterpolator.getCurrentState().getTime().getField().getZero().add(y[i]);
}
T[] fieldYDot = eqn.computeDerivatives(fieldT, fieldY);
for (int i = 0; i < yDot.length; ++i) {
yDot[i] = fieldYDot[i].getReal();
}
}
};
regularInterpolator.reinitialize(
dummyIntegrator,
y,
yDotArray,
fieldInterpolator.isForward(),
primaryMapper,
secondaryMappers);
T[] fieldPreviousY = fieldInterpolator.getPreviousState().getState();
for (int i = 0; i < y.length; ++i) {
y[i] = fieldPreviousY[i].getReal();
}
regularInterpolator.storeTime(fieldInterpolator.getPreviousState().getTime().getReal());
regularInterpolator.shift();
T[] fieldCurrentY = fieldInterpolator.getCurrentState().getState();
for (int i = 0; i < y.length; ++i) {
y[i] = fieldCurrentY[i].getReal();
}
regularInterpolator.storeTime(fieldInterpolator.getCurrentState().getTime().getReal());
} catch (ClassNotFoundException cnfe) {
Assert.fail(cnfe.getLocalizedMessage());
} catch (InstantiationException ie) {
Assert.fail(ie.getLocalizedMessage());
} catch (IllegalAccessException iae) {
Assert.fail(iae.getLocalizedMessage());
} catch (NoSuchFieldException nsfe) {
Assert.fail(nsfe.getLocalizedMessage());
} catch (IllegalArgumentException iae) {
Assert.fail(iae.getLocalizedMessage());
}
return regularInterpolator;
}
/** {@inheritDoc} */
@Override
public void integrate(final ExpandableStatefulODE equations, final double t)
throws MathIllegalStateException, MathIllegalArgumentException {
sanityChecks(equations, t);
setEquations(equations);
final boolean forward = t > equations.getTime();
// create some internal working arrays
final double[] y0 = equations.getCompleteState();
final double[] y = y0.clone();
final int stages = c.length + 1;
final double[][] yDotK = new double[stages][y.length];
final double[] yTmp = y0.clone();
final double[] yDotTmp = new double[y.length];
// set up an interpolator sharing the integrator arrays
final RungeKuttaStepInterpolator interpolator = (RungeKuttaStepInterpolator) prototype.copy();
interpolator.reinitialize(
this, yTmp, yDotK, forward, equations.getPrimaryMapper(), equations.getSecondaryMappers());
interpolator.storeTime(equations.getTime());
// set up integration control objects
stepStart = equations.getTime();
double hNew = 0;
boolean firstTime = true;
initIntegration(equations.getTime(), y0, t);
// main integration loop
isLastStep = false;
do {
interpolator.shift();
// iterate over step size, ensuring local normalized error is smaller than 1
double error = 10;
while (error >= 1.0) {
if (firstTime || !fsal) {
// first stage
computeDerivatives(stepStart, y, yDotK[0]);
}
if (firstTime) {
final double[] scale = new double[mainSetDimension];
if (vecAbsoluteTolerance == null) {
for (int i = 0; i < scale.length; ++i) {
scale[i] = scalAbsoluteTolerance + scalRelativeTolerance * FastMath.abs(y[i]);
}
} else {
for (int i = 0; i < scale.length; ++i) {
scale[i] = vecAbsoluteTolerance[i] + vecRelativeTolerance[i] * FastMath.abs(y[i]);
}
}
hNew = initializeStep(forward, getOrder(), scale, stepStart, y, yDotK[0], yTmp, yDotK[1]);
firstTime = false;
}
stepSize = hNew;
if (forward) {
if (stepStart + stepSize >= t) {
stepSize = t - stepStart;
}
} else {
if (stepStart + stepSize <= t) {
stepSize = t - stepStart;
}
}
// next stages
for (int k = 1; k < stages; ++k) {
for (int j = 0; j < y0.length; ++j) {
double sum = a[k - 1][0] * yDotK[0][j];
for (int l = 1; l < k; ++l) {
sum += a[k - 1][l] * yDotK[l][j];
}
yTmp[j] = y[j] + stepSize * sum;
}
computeDerivatives(stepStart + c[k - 1] * stepSize, yTmp, yDotK[k]);
}
// estimate the state at the end of the step
for (int j = 0; j < y0.length; ++j) {
double sum = b[0] * yDotK[0][j];
for (int l = 1; l < stages; ++l) {
sum += b[l] * yDotK[l][j];
}
yTmp[j] = y[j] + stepSize * sum;
}
// estimate the error at the end of the step
error = estimateError(yDotK, y, yTmp, stepSize);
if (error >= 1.0) {
// reject the step and attempt to reduce error by stepsize control
final double factor =
FastMath.min(
maxGrowth, FastMath.max(minReduction, safety * FastMath.pow(error, exp)));
hNew = filterStep(stepSize * factor, forward, false);
}
}
// local error is small enough: accept the step, trigger events and step handlers
interpolator.storeTime(stepStart + stepSize);
System.arraycopy(yTmp, 0, y, 0, y0.length);
System.arraycopy(yDotK[stages - 1], 0, yDotTmp, 0, y0.length);
stepStart = acceptStep(interpolator, y, yDotTmp, t);
System.arraycopy(y, 0, yTmp, 0, y.length);
if (!isLastStep) {
// prepare next step
interpolator.storeTime(stepStart);
if (fsal) {
// save the last evaluation for the next step
System.arraycopy(yDotTmp, 0, yDotK[0], 0, y0.length);
}
// stepsize control for next step
final double factor =
FastMath.min(maxGrowth, FastMath.max(minReduction, safety * FastMath.pow(error, exp)));
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;
}
}
} while (!isLastStep);
// dispatch results
equations.setTime(stepStart);
equations.setCompleteState(y);
resetInternalState();
}