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; }