protected <T extends RealFieldElement<T>> void doNonFieldInterpolatorConsistency(
final Field<T> field,
double epsilonSin,
double epsilonCos,
double epsilonSinDot,
double epsilonCosDot) {
FirstOrderFieldDifferentialEquations<T> eqn = new SinCos<T>(field);
RungeKuttaFieldStepInterpolator<T> fieldInterpolator =
setUpInterpolator(field, eqn, 0.0, new double[] {0.0, 1.0}, 0.125);
RungeKuttaStepInterpolator regularInterpolator = convertInterpolator(fieldInterpolator, eqn);
int n = 100;
double maxErrorSin = 0;
double maxErrorCos = 0;
double maxErrorSinDot = 0;
double maxErrorCosDot = 0;
for (int i = 0; i <= n; ++i) {
T t =
fieldInterpolator
.getPreviousState()
.getTime()
.multiply(n - i)
.add(fieldInterpolator.getCurrentState().getTime().multiply(i))
.divide(n);
FieldODEStateAndDerivative<T> state = fieldInterpolator.getInterpolatedState(t);
T[] fieldY = state.getState();
T[] fieldYDot = state.getDerivative();
regularInterpolator.setInterpolatedTime(t.getReal());
double[] regularY = regularInterpolator.getInterpolatedState();
double[] regularYDot = regularInterpolator.getInterpolatedDerivatives();
maxErrorSin = FastMath.max(maxErrorSin, fieldY[0].subtract(regularY[0]).abs().getReal());
maxErrorCos = FastMath.max(maxErrorCos, fieldY[1].subtract(regularY[1]).abs().getReal());
maxErrorSinDot =
FastMath.max(maxErrorSinDot, fieldYDot[0].subtract(regularYDot[0]).abs().getReal());
maxErrorCosDot =
FastMath.max(maxErrorCosDot, fieldYDot[1].subtract(regularYDot[1]).abs().getReal());
}
Assert.assertEquals(0.0, maxErrorSin, epsilonSin);
Assert.assertEquals(0.0, maxErrorCos, epsilonCos);
Assert.assertEquals(0.0, maxErrorSinDot, epsilonSinDot);
Assert.assertEquals(0.0, maxErrorCosDot, epsilonCosDot);
}
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();
}
/** {@inheritDoc} */
@Override
public double integrate(
final FirstOrderDifferentialEquations equations,
final double t0,
final double[] y0,
final double t,
final double[] y)
throws DerivativeException, IntegratorException {
sanityChecks(equations, t0, y0, t, y);
setEquations(equations);
resetEvaluations();
final boolean forward = (t > t0);
// create some internal working arrays
final int stages = c.length + 1;
if (y != y0) {
System.arraycopy(y0, 0, y, 0, y0.length);
}
final double[][] yDotK = new double[stages][y0.length];
final double[] yTmp = new double[y0.length];
// set up an interpolator sharing the integrator arrays
AbstractStepInterpolator interpolator;
if (requiresDenseOutput() || (!eventsHandlersManager.isEmpty())) {
final RungeKuttaStepInterpolator rki = (RungeKuttaStepInterpolator) prototype.copy();
rki.reinitialize(this, yTmp, yDotK, forward);
interpolator = rki;
} else {
interpolator = new DummyStepInterpolator(yTmp, forward);
}
interpolator.storeTime(t0);
// set up integration control objects
stepStart = t0;
double hNew = 0;
boolean firstTime = true;
for (StepHandler handler : stepHandlers) {
handler.reset();
}
CombinedEventsManager manager = addEndTimeChecker(t0, t, eventsHandlersManager);
boolean lastStep = false;
// main integration loop
while (!lastStep) {
interpolator.shift();
double error = 0;
for (boolean loop = true; loop; ) {
if (firstTime || !fsal) {
// first stage
computeDerivatives(stepStart, y, yDotK[0]);
}
if (firstTime) {
final double[] scale;
if (vecAbsoluteTolerance != null) {
scale = vecAbsoluteTolerance;
} else {
scale = new double[y0.length];
Arrays.fill(scale, scalAbsoluteTolerance);
}
hNew =
initializeStep(
equations, forward, getOrder(), scale, stepStart, y, yDotK[0], yTmp, yDotK[1]);
firstTime = false;
}
stepSize = hNew;
// 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) {
// discrete events handling
interpolator.storeTime(stepStart + stepSize);
if (manager.evaluateStep(interpolator)) {
final double dt = manager.getEventTime() - stepStart;
if (Math.abs(dt) <= Math.ulp(stepStart)) {
// rejecting the step would lead to a too small next step, we accept it
loop = false;
} else {
// reject the step to match exactly the next switch time
hNew = dt;
}
} else {
// accept the step
loop = false;
}
} else {
// reject the step and attempt to reduce error by stepsize control
final double factor =
Math.min(maxGrowth, Math.max(minReduction, safety * Math.pow(error, exp)));
hNew = filterStep(stepSize * factor, forward, false);
}
}
// the step has been accepted
final double nextStep = stepStart + stepSize;
System.arraycopy(yTmp, 0, y, 0, y0.length);
manager.stepAccepted(nextStep, y);
lastStep = manager.stop();
// provide the step data to the step handler
interpolator.storeTime(nextStep);
for (StepHandler handler : stepHandlers) {
handler.handleStep(interpolator, lastStep);
}
stepStart = nextStep;
if (fsal) {
// save the last evaluation for the next step
System.arraycopy(yDotK[stages - 1], 0, yDotK[0], 0, y0.length);
}
if (manager.reset(stepStart, y) && !lastStep) {
// some event handler has triggered changes that
// invalidate the derivatives, we need to recompute them
computeDerivatives(stepStart, y, yDotK[0]);
}
if (!lastStep) {
// in some rare cases we may get here with stepSize = 0, for example
// when an event occurs at integration start, reducing the first step
// to zero; we have to reset the step to some safe non zero value
stepSize = filterStep(stepSize, forward, true);
// stepsize control for next step
final double factor =
Math.min(maxGrowth, Math.max(minReduction, safety * Math.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 stopTime = stepStart;
resetInternalState();
return stopTime;
}
