Repository: commons-math Updated Branches: refs/heads/field-ode 04feb9960 -> d3fb47063
Intermediate level implementations of variable-step Runge-Kutta methods. Project: http://git-wip-us.apache.org/repos/asf/commons-math/repo Commit: http://git-wip-us.apache.org/repos/asf/commons-math/commit/d3fb4706 Tree: http://git-wip-us.apache.org/repos/asf/commons-math/tree/d3fb4706 Diff: http://git-wip-us.apache.org/repos/asf/commons-math/diff/d3fb4706 Branch: refs/heads/field-ode Commit: d3fb47063e63a316b76f1665be3300eb7c4362c8 Parents: 04feb99 Author: Luc Maisonobe <l...@apache.org> Authored: Sun Nov 15 15:56:52 2015 +0100 Committer: Luc Maisonobe <l...@apache.org> Committed: Sun Nov 15 15:56:52 2015 +0100 ---------------------------------------------------------------------- .../AdaptiveStepsizeFieldIntegrator.java | 366 ++++++++++++++++++ .../EmbeddedRungeKuttaFieldIntegrator.java | 379 +++++++++++++++++++ 2 files changed, 745 insertions(+) ---------------------------------------------------------------------- http://git-wip-us.apache.org/repos/asf/commons-math/blob/d3fb4706/src/main/java/org/apache/commons/math3/ode/nonstiff/AdaptiveStepsizeFieldIntegrator.java ---------------------------------------------------------------------- diff --git a/src/main/java/org/apache/commons/math3/ode/nonstiff/AdaptiveStepsizeFieldIntegrator.java b/src/main/java/org/apache/commons/math3/ode/nonstiff/AdaptiveStepsizeFieldIntegrator.java new file mode 100644 index 0000000..75d4b51 --- /dev/null +++ b/src/main/java/org/apache/commons/math3/ode/nonstiff/AdaptiveStepsizeFieldIntegrator.java @@ -0,0 +1,366 @@ +/* + * Licensed to the Apache Software Foundation (ASF) under one or more + * contributor license agreements. See the NOTICE file distributed with + * this work for additional information regarding copyright ownership. + * The ASF licenses this file to You under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with + * the License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +package org.apache.commons.math3.ode.nonstiff; + +import org.apache.commons.math3.Field; +import org.apache.commons.math3.RealFieldElement; +import org.apache.commons.math3.exception.DimensionMismatchException; +import org.apache.commons.math3.exception.MaxCountExceededException; +import org.apache.commons.math3.exception.NumberIsTooSmallException; +import org.apache.commons.math3.exception.util.LocalizedFormats; +import org.apache.commons.math3.ode.AbstractFieldIntegrator; +import org.apache.commons.math3.ode.FieldEquationsMapper; +import org.apache.commons.math3.ode.FieldODEState; +import org.apache.commons.math3.ode.FieldODEStateAndDerivative; +import org.apache.commons.math3.util.FastMath; +import org.apache.commons.math3.util.MathArrays; +import org.apache.commons.math3.util.MathUtils; + +/** + * This abstract class holds the common part of all adaptive + * stepsize integrators for Ordinary Differential Equations. + * + * <p>These algorithms perform integration with stepsize control, which + * means the user does not specify the integration step but rather a + * tolerance on error. The error threshold is computed as + * <pre> + * threshold_i = absTol_i + relTol_i * max (abs (ym), abs (ym+1)) + * </pre> + * where absTol_i is the absolute tolerance for component i of the + * state vector and relTol_i is the relative tolerance for the same + * component. The user can also use only two scalar values absTol and + * relTol which will be used for all components. + * </p> + * <p> + * Note that <em>only</em> the {@link FieldODEState#getState() main part} + * of the state vector is used for stepsize control. The {@link + * FieldODEState#getSecondaryState(int) secondary parts} of the state + * vector are explicitly ignored for stepsize control. + * </p> + * + * <p>If the estimated error for ym+1 is such that + * <pre> + * sqrt((sum (errEst_i / threshold_i)^2 ) / n) < 1 + * </pre> + * + * (where n is the main set dimension) then the step is accepted, + * otherwise the step is rejected and a new attempt is made with a new + * stepsize.</p> + * + * @param <T> the type of the field elements + * @since 3.6 + * + */ + +public abstract class AdaptiveStepsizeFieldIntegrator<T extends RealFieldElement<T>> + extends AbstractFieldIntegrator<T> { + + /** Allowed absolute scalar error. */ + protected double scalAbsoluteTolerance; + + /** Allowed relative scalar error. */ + protected double scalRelativeTolerance; + + /** Allowed absolute vectorial error. */ + protected double[] vecAbsoluteTolerance; + + /** Allowed relative vectorial error. */ + protected double[] vecRelativeTolerance; + + /** Main set dimension. */ + protected int mainSetDimension; + + /** User supplied initial step. */ + private T initialStep; + + /** Minimal step. */ + private T minStep; + + /** Maximal step. */ + private T maxStep; + + /** Build an integrator with the given stepsize bounds. + * The default step handler does nothing. + * @param field field to which the time and state vector elements belong + * @param name name of the method + * @param minStep minimal step (sign is irrelevant, regardless of + * integration direction, forward or backward), the last step can + * be smaller than this + * @param maxStep maximal step (sign is irrelevant, regardless of + * integration direction, forward or backward), the last step can + * be smaller than this + * @param scalAbsoluteTolerance allowed absolute error + * @param scalRelativeTolerance allowed relative error + */ + public AdaptiveStepsizeFieldIntegrator(final Field<T> field, final String name, + final double minStep, final double maxStep, + final double scalAbsoluteTolerance, + final double scalRelativeTolerance) { + + super(field, name); + setStepSizeControl(minStep, maxStep, scalAbsoluteTolerance, scalRelativeTolerance); + resetInternalState(); + + } + + /** Build an integrator with the given stepsize bounds. + * The default step handler does nothing. + * @param field field to which the time and state vector elements belong + * @param name name of the method + * @param minStep minimal step (sign is irrelevant, regardless of + * integration direction, forward or backward), the last step can + * be smaller than this + * @param maxStep maximal step (sign is irrelevant, regardless of + * integration direction, forward or backward), the last step can + * be smaller than this + * @param vecAbsoluteTolerance allowed absolute error + * @param vecRelativeTolerance allowed relative error + */ + public AdaptiveStepsizeFieldIntegrator(final Field<T> field, final String name, + final double minStep, final double maxStep, + final double[] vecAbsoluteTolerance, + final double[] vecRelativeTolerance) { + + super(field, name); + setStepSizeControl(minStep, maxStep, vecAbsoluteTolerance, vecRelativeTolerance); + resetInternalState(); + + } + + /** Set the adaptive step size control parameters. + * <p> + * A side effect of this method is to also reset the initial + * step so it will be automatically computed by the integrator + * if {@link #setInitialStepSize(double) setInitialStepSize} + * is not called by the user. + * </p> + * @param minimalStep minimal step (must be positive even for backward + * integration), the last step can be smaller than this + * @param maximalStep maximal step (must be positive even for backward + * integration) + * @param absoluteTolerance allowed absolute error + * @param relativeTolerance allowed relative error + */ + public void setStepSizeControl(final double minimalStep, final double maximalStep, + final double absoluteTolerance, + final double relativeTolerance) { + + minStep = getField().getZero().add(FastMath.abs(minimalStep)); + maxStep = getField().getZero().add(FastMath.abs(maximalStep)); + initialStep = getField().getOne().negate(); + + scalAbsoluteTolerance = absoluteTolerance; + scalRelativeTolerance = relativeTolerance; + vecAbsoluteTolerance = null; + vecRelativeTolerance = null; + + } + + /** Set the adaptive step size control parameters. + * <p> + * A side effect of this method is to also reset the initial + * step so it will be automatically computed by the integrator + * if {@link #setInitialStepSize(double) setInitialStepSize} + * is not called by the user. + * </p> + * @param minimalStep minimal step (must be positive even for backward + * integration), the last step can be smaller than this + * @param maximalStep maximal step (must be positive even for backward + * integration) + * @param absoluteTolerance allowed absolute error + * @param relativeTolerance allowed relative error + */ + public void setStepSizeControl(final double minimalStep, final double maximalStep, + final double[] absoluteTolerance, + final double[] relativeTolerance) { + + minStep = getField().getZero().add(FastMath.abs(minimalStep)); + maxStep = getField().getZero().add(FastMath.abs(maximalStep)); + initialStep = getField().getOne().negate(); + + scalAbsoluteTolerance = 0; + scalRelativeTolerance = 0; + vecAbsoluteTolerance = absoluteTolerance.clone(); + vecRelativeTolerance = relativeTolerance.clone(); + + } + + /** Set the initial step size. + * <p>This method allows the user to specify an initial positive + * step size instead of letting the integrator guess it by + * itself. If this method is not called before integration is + * started, the initial step size will be estimated by the + * integrator.</p> + * @param initialStepSize initial step size to use (must be positive even + * for backward integration ; providing a negative value or a value + * outside of the min/max step interval will lead the integrator to + * ignore the value and compute the initial step size by itself) + */ + public void setInitialStepSize(final T initialStepSize) { + if (initialStepSize.subtract(minStep).getReal() < 0 || + initialStepSize.subtract(maxStep).getReal() > 0) { + initialStep = getField().getOne().negate(); + } else { + initialStep = initialStepSize; + } + } + + /** {@inheritDoc} */ + @Override + protected void sanityChecks(final FieldODEState<T> eqn, final T t) + throws DimensionMismatchException, NumberIsTooSmallException { + + super.sanityChecks(eqn, t); + + mainSetDimension = eqn.getState().length; + + if (vecAbsoluteTolerance != null && vecAbsoluteTolerance.length != mainSetDimension) { + throw new DimensionMismatchException(mainSetDimension, vecAbsoluteTolerance.length); + } + + if (vecRelativeTolerance != null && vecRelativeTolerance.length != mainSetDimension) { + throw new DimensionMismatchException(mainSetDimension, vecRelativeTolerance.length); + } + + } + + /** Initialize the integration step. + * @param forward forward integration indicator + * @param order order of the method + * @param scale scaling vector for the state vector (can be shorter than state vector) + * @param state0 state at integration start time + * @param mapper mapper for all the equations + * @return first integration step + * @exception MaxCountExceededException if the number of functions evaluations is exceeded + * @exception DimensionMismatchException if arrays dimensions do not match equations settings + */ + public T initializeStep(final boolean forward, final int order, final T[] scale, + final FieldODEStateAndDerivative<T> state0, + final FieldEquationsMapper<T> mapper) + throws MaxCountExceededException, DimensionMismatchException { + + if (initialStep.getReal() > 0) { + // use the user provided value + return forward ? initialStep : initialStep.negate(); + } + + // very rough first guess : h = 0.01 * ||y/scale|| / ||y'/scale|| + // this guess will be used to perform an Euler step + final T[] y0 = mapper.mapState(state0); + final T[] yDot0 = mapper.mapDerivative(state0); + T yOnScale2 = getField().getZero(); + T yDotOnScale2 = getField().getZero(); + for (int j = 0; j < scale.length; ++j) { + final T ratio = y0[j].divide(scale[j]); + yOnScale2 = yOnScale2.add(ratio.multiply(ratio)); + final T ratioDot = yDot0[j].divide(scale[j]); + yDotOnScale2 = yDotOnScale2.add(ratioDot.multiply(ratioDot)); + } + + T h = (yOnScale2.getReal() < 1.0e-10 || yDotOnScale2.getReal() < 1.0e-10) ? + getField().getZero().add(1.0e-6) : + yOnScale2.divide(yDotOnScale2).sqrt().multiply(0.01); + if (! forward) { + h = h.negate(); + } + + // perform an Euler step using the preceding rough guess + final T[] y1 = MathArrays.buildArray(getField(), y0.length); + for (int j = 0; j < y0.length; ++j) { + y1[j] = y0[j].add(yDot0[j].multiply(h)); + } + final T[] yDot1 = computeDerivatives(state0.getTime().add(h), y1); + + // estimate the second derivative of the solution + T yDDotOnScale = getField().getZero(); + for (int j = 0; j < scale.length; ++j) { + final T ratioDotDot = yDot1[j].subtract(yDot0[j]).divide(scale[j]); + yDDotOnScale = yDDotOnScale.add(ratioDotDot.multiply(ratioDotDot)); + } + yDDotOnScale = yDDotOnScale.sqrt().divide(h); + + // step size is computed such that + // h^order * max (||y'/tol||, ||y''/tol||) = 0.01 + final T maxInv2 = MathUtils.max(yDotOnScale2.sqrt(), yDDotOnScale); + final T h1 = maxInv2.getReal() < 1.0e-15 ? + MathUtils.max(getField().getZero().add(1.0e-6), h.abs().multiply(0.001)) : + maxInv2.multiply(100).reciprocal().pow(1.0 / order); + h = MathUtils.min(h.abs().multiply(100), h1); + h = MathUtils.max(h, state0.getTime().abs().multiply(1.0e-12)); // avoids cancellation when computing t1 - t0 + h = MathUtils.max(minStep, MathUtils.min(maxStep, h)); + if (! forward) { + h = h.negate(); + } + + return h; + + } + + /** Filter the integration step. + * @param h signed step + * @param forward forward integration indicator + * @param acceptSmall if true, steps smaller than the minimal value + * are silently increased up to this value, if false such small + * steps generate an exception + * @return a bounded integration step (h if no bound is reach, or a bounded value) + * @exception NumberIsTooSmallException if the step is too small and acceptSmall is false + */ + protected T filterStep(final T h, final boolean forward, final boolean acceptSmall) + throws NumberIsTooSmallException { + + T filteredH = h; + if (h.abs().subtract(minStep).getReal() < 0) { + if (acceptSmall) { + filteredH = forward ? minStep : minStep.negate(); + } else { + throw new NumberIsTooSmallException(LocalizedFormats.MINIMAL_STEPSIZE_REACHED_DURING_INTEGRATION, + h.abs().getReal(), minStep.getReal(), true); + } + } + + if (filteredH.subtract(maxStep).getReal() > 0) { + filteredH = maxStep; + } else if (filteredH.add(maxStep).getReal() < 0) { + filteredH = maxStep.negate(); + } + + return filteredH; + + } + + /** Reset internal state to dummy values. */ + protected void resetInternalState() { + stepStart = null; + stepSize = minStep.multiply(maxStep).sqrt(); + } + + /** Get the minimal step. + * @return minimal step + */ + public T getMinStep() { + return minStep; + } + + /** Get the maximal step. + * @return maximal step + */ + public T getMaxStep() { + return maxStep; + } + +} http://git-wip-us.apache.org/repos/asf/commons-math/blob/d3fb4706/src/main/java/org/apache/commons/math3/ode/nonstiff/EmbeddedRungeKuttaFieldIntegrator.java ---------------------------------------------------------------------- diff --git a/src/main/java/org/apache/commons/math3/ode/nonstiff/EmbeddedRungeKuttaFieldIntegrator.java b/src/main/java/org/apache/commons/math3/ode/nonstiff/EmbeddedRungeKuttaFieldIntegrator.java new file mode 100644 index 0000000..0852aff --- /dev/null +++ b/src/main/java/org/apache/commons/math3/ode/nonstiff/EmbeddedRungeKuttaFieldIntegrator.java @@ -0,0 +1,379 @@ +/* + * Licensed to the Apache Software Foundation (ASF) under one or more + * contributor license agreements. See the NOTICE file distributed with + * this work for additional information regarding copyright ownership. + * The ASF licenses this file to You under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with + * the License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +package org.apache.commons.math3.ode.nonstiff; + +import org.apache.commons.math3.Field; +import org.apache.commons.math3.RealFieldElement; +import org.apache.commons.math3.exception.DimensionMismatchException; +import org.apache.commons.math3.exception.MaxCountExceededException; +import org.apache.commons.math3.exception.NoBracketingException; +import org.apache.commons.math3.exception.NumberIsTooSmallException; +import org.apache.commons.math3.ode.FieldExpandableODE; +import org.apache.commons.math3.ode.FieldODEState; +import org.apache.commons.math3.ode.FieldODEStateAndDerivative; +import org.apache.commons.math3.util.MathArrays; +import org.apache.commons.math3.util.MathUtils; + +/** + * This class implements the common part of all embedded Runge-Kutta + * integrators for Ordinary Differential Equations. + * + * <p>These methods are embedded explicit Runge-Kutta methods with two + * sets of coefficients allowing to estimate the error, their Butcher + * arrays are as follows : + * <pre> + * 0 | + * c2 | a21 + * c3 | a31 a32 + * ... | ... + * cs | as1 as2 ... ass-1 + * |-------------------------- + * | b1 b2 ... bs-1 bs + * | b'1 b'2 ... b's-1 b's + * </pre> + * </p> + * + * <p>In fact, we rather use the array defined by ej = bj - b'j to + * compute directly the error rather than computing two estimates and + * then comparing them.</p> + * + * <p>Some methods are qualified as <i>fsal</i> (first same as last) + * methods. This means the last evaluation of the derivatives in one + * step is the same as the first in the next step. Then, this + * evaluation can be reused from one step to the next one and the cost + * of such a method is really s-1 evaluations despite the method still + * has s stages. This behaviour is true only for successful steps, if + * the step is rejected after the error estimation phase, no + * evaluation is saved. For an <i>fsal</i> method, we have cs = 1 and + * asi = bi for all i.</p> + * + * @param <T> the type of the field elements + * @since 3.6 + */ + +public abstract class EmbeddedRungeKuttaFieldIntegrator<T extends RealFieldElement<T>> + extends AdaptiveStepsizeFieldIntegrator<T> { + + /** Indicator for <i>fsal</i> methods. */ + private final boolean fsal; + + /** Time steps from Butcher array (without the first zero). */ + private final double[] c; + + /** Internal weights from Butcher array (without the first empty row). */ + private final double[][] a; + + /** External weights for the high order method from Butcher array. */ + private final double[] b; + + /** Prototype of the step interpolator. */ + private final RungeKuttaFieldStepInterpolator<T> prototype; + + /** Stepsize control exponent. */ + private final double exp; + + /** Safety factor for stepsize control. */ + private T safety; + + /** Minimal reduction factor for stepsize control. */ + private T minReduction; + + /** Maximal growth factor for stepsize control. */ + private T maxGrowth; + + /** Build a Runge-Kutta integrator with the given Butcher array. + * @param field field to which the time and state vector elements belong + * @param name name of the method + * @param fsal indicate that the method is an <i>fsal</i> + * @param c time steps from Butcher array (without the first zero) + * @param a internal weights from Butcher array (without the first empty row) + * @param b propagation weights for the high order method from Butcher array + * @param prototype prototype of the step interpolator to use + * @param minStep minimal step (sign is irrelevant, regardless of + * integration direction, forward or backward), the last step can + * be smaller than this + * @param maxStep maximal step (sign is irrelevant, regardless of + * integration direction, forward or backward), the last step can + * be smaller than this + * @param scalAbsoluteTolerance allowed absolute error + * @param scalRelativeTolerance allowed relative error + */ + protected EmbeddedRungeKuttaFieldIntegrator(final Field<T> field, final String name, final boolean fsal, + final double[] c, final double[][] a, final double[] b, + final RungeKuttaFieldStepInterpolator<T> prototype, + final double minStep, final double maxStep, + final double scalAbsoluteTolerance, + final double scalRelativeTolerance) { + + super(field, name, minStep, maxStep, scalAbsoluteTolerance, scalRelativeTolerance); + + this.fsal = fsal; + this.c = c; + this.a = a; + this.b = b; + this.prototype = prototype; + + exp = -1.0 / getOrder(); + + // set the default values of the algorithm control parameters + setSafety(field.getZero().add(0.9)); + setMinReduction(field.getZero().add(0.2)); + setMaxGrowth(field.getZero().add(10.0)); + + } + + /** Build a Runge-Kutta integrator with the given Butcher array. + * @param field field to which the time and state vector elements belong + * @param name name of the method + * @param fsal indicate that the method is an <i>fsal</i> + * @param c time steps from Butcher array (without the first zero) + * @param a internal weights from Butcher array (without the first empty row) + * @param b propagation weights for the high order method from Butcher array + * @param prototype prototype of the step interpolator to use + * @param minStep minimal step (must be positive even for backward + * integration), the last step can be smaller than this + * @param maxStep maximal step (must be positive even for backward + * integration) + * @param vecAbsoluteTolerance allowed absolute error + * @param vecRelativeTolerance allowed relative error + */ + protected EmbeddedRungeKuttaFieldIntegrator(final Field<T> field, final String name, final boolean fsal, + final double[] c, final double[][] a, final double[] b, + final RungeKuttaFieldStepInterpolator<T> prototype, + final double minStep, final double maxStep, + final double[] vecAbsoluteTolerance, + final double[] vecRelativeTolerance) { + + super(field, name, minStep, maxStep, vecAbsoluteTolerance, vecRelativeTolerance); + + this.fsal = fsal; + this.c = c; + this.a = a; + this.b = b; + this.prototype = prototype; + + exp = -1.0 / getOrder(); + + // set the default values of the algorithm control parameters + setSafety(field.getZero().add(0.9)); + setMinReduction(field.getZero().add(0.2)); + setMaxGrowth(field.getZero().add(10.0)); + + } + + /** Get the order of the method. + * @return order of the method + */ + public abstract int getOrder(); + + /** Get the safety factor for stepsize control. + * @return safety factor + */ + public T getSafety() { + return safety; + } + + /** Set the safety factor for stepsize control. + * @param safety safety factor + */ + public void setSafety(final T safety) { + this.safety = safety; + } + + /** {@inheritDoc} */ + @Override + 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); + stepStart = 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 an interpolator sharing the integrator arrays + final RungeKuttaFieldStepInterpolator<T> interpolator = (RungeKuttaFieldStepInterpolator<T>) prototype.copy(); + interpolator.reinitialize(this, y0, yDotK, forward, equations.getMapper()); + interpolator.storeState(stepStart); + + // set up integration control objects + T hNew = getField().getZero(); + boolean firstTime = true; + + // main integration loop + isLastStep = false; + do { + + interpolator.shift(); + + // 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 + yDotK[0] = stepStart.getDerivative(); + + 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, stepStart, equations.getMapper()); + firstTime = false; + } + + stepSize = hNew; + if (forward) { + if (stepStart.getTime().add(stepSize).subtract(finalTime).getReal() >= 0) { + stepSize = finalTime.subtract(stepStart.getTime()); + } + } else { + if (stepStart.getTime().add(stepSize).subtract(finalTime).getReal() <= 0) { + stepSize = finalTime.subtract(stepStart.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(stepSize.multiply(sum)); + } + + yDotK[k] = computeDerivatives(stepStart.getTime().add(stepSize.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(stepSize.multiply(sum)); + } + + // estimate the error at the end of the step + error = estimateError(yDotK, y, yTmp, stepSize); + if (error.subtract(1.0).getReal() >= 0) { + // reject the step and attempt to reduce error by stepsize control + final T factor = MathUtils.min(maxGrowth, + MathUtils.max(minReduction, safety.multiply(error.pow(exp)))); + hNew = filterStep(stepSize.multiply(factor), forward, false); + } + + } + final T stepEnd = stepStart.getTime().add(stepSize); + final T[] yDotTmp = fsal ? yDotK[stages - 1] : computeDerivatives(stepEnd, yTmp); + final FieldODEStateAndDerivative<T> stateTmp = new FieldODEStateAndDerivative<T>(stepEnd, yTmp, yDotTmp); + + // local error is small enough: accept the step, trigger events and step handlers + interpolator.storeState(stateTmp); + System.arraycopy(yTmp, 0, y, 0, y0.length); + stepStart = acceptStep(interpolator, finalTime); + System.arraycopy(y, 0, yTmp, 0, y.length); + + if (!isLastStep) { + + // prepare next step + interpolator.storeState(stepStart); + + // stepsize control for next step + final T factor = MathUtils.min(maxGrowth, + MathUtils.max(minReduction, safety.multiply(error.pow(exp)))); + final T scaledH = stepSize.multiply(factor); + final T nextT = stepStart.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 = stepStart.getTime().add(hNew); + final boolean filteredNextIsLast = forward ? + filteredNextT.subtract(finalTime).getReal() >= 0 : + filteredNextT.subtract(finalTime).getReal() <= 0; + if (filteredNextIsLast) { + hNew = finalTime.subtract(stepStart.getTime()); + } + + } + + } while (!isLastStep); + + final FieldODEStateAndDerivative<T> finalState = stepStart; + resetInternalState(); + return finalState; + + } + + /** Get the minimal reduction factor for stepsize control. + * @return minimal reduction factor + */ + public T getMinReduction() { + return minReduction; + } + + /** Set the minimal reduction factor for stepsize control. + * @param minReduction minimal reduction factor + */ + public void setMinReduction(final T minReduction) { + this.minReduction = minReduction; + } + + /** Get the maximal growth factor for stepsize control. + * @return maximal growth factor + */ + public T getMaxGrowth() { + return maxGrowth; + } + + /** Set the maximal growth factor for stepsize control. + * @param maxGrowth maximal growth factor + */ + public void setMaxGrowth(final T maxGrowth) { + this.maxGrowth = maxGrowth; + } + + /** Compute the error ratio. + * @param yDotK derivatives computed during the first stages + * @param y0 estimate of the step at the start of the step + * @param y1 estimate of the step at the end of the step + * @param h current step + * @return error ratio, greater than 1 if step should be rejected + */ + protected abstract T estimateError(T[][] yDotK, T[] y0, T[] y1, T h); + +}
