protected <T extends RealFieldElement<T>> void doInterpolationInside(
final Field<T> field, double epsilonSin, double epsilonCos) {
RungeKuttaFieldStepInterpolator<T> interpolator =
setUpInterpolator(field, new SinCos<T>(field), 0.0, new double[] {0.0, 1.0}, 0.0125);
int n = 100;
double maxErrorSin = 0;
double maxErrorCos = 0;
for (int i = 0; i <= n; ++i) {
T t =
interpolator
.getPreviousState()
.getTime()
.multiply(n - i)
.add(interpolator.getCurrentState().getTime().multiply(i))
.divide(n);
FieldODEStateAndDerivative<T> state = interpolator.getInterpolatedState(t);
maxErrorSin =
FastMath.max(maxErrorSin, state.getState()[0].subtract(t.sin()).abs().getReal());
maxErrorCos =
FastMath.max(maxErrorCos, state.getState()[1].subtract(t.cos()).abs().getReal());
}
Assert.assertEquals(0.0, maxErrorSin, epsilonSin);
Assert.assertEquals(0.0, maxErrorCos, epsilonCos);
}
protected <T extends RealFieldElement<T>> void doInterpolationAtBounds(
final Field<T> field, double epsilon) {
RungeKuttaFieldStepInterpolator<T> interpolator =
setUpInterpolator(field, new SinCos<T>(field), 0.0, new double[] {0.0, 1.0}, 0.125);
Assert.assertEquals(0.0, interpolator.getPreviousState().getTime().getReal(), 1.0e-15);
for (int i = 0; i < 2; ++i) {
Assert.assertEquals(
interpolator.getPreviousState().getState()[i].getReal(),
interpolator
.getInterpolatedState(interpolator.getPreviousState().getTime())
.getState()[i]
.getReal(),
epsilon);
}
Assert.assertEquals(0.125, interpolator.getCurrentState().getTime().getReal(), 1.0e-15);
for (int i = 0; i < 2; ++i) {
Assert.assertEquals(
interpolator.getCurrentState().getState()[i].getReal(),
interpolator
.getInterpolatedState(interpolator.getCurrentState().getTime())
.getState()[i]
.getReal(),
epsilon);
}
}
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;
}
