GraggBulirschStoerIntegrator.java
- /*
- * Licensed to the Hipparchus project under one or more
- * contributor license agreements. See the NOTICE file distributed with
- * this work for additional information regarding copyright ownership.
- * The Hipparchus project 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
- *
- * https://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.hipparchus.ode.nonstiff;
- import org.hipparchus.exception.MathIllegalArgumentException;
- import org.hipparchus.exception.MathIllegalStateException;
- import org.hipparchus.ode.ExpandableODE;
- import org.hipparchus.ode.LocalizedODEFormats;
- import org.hipparchus.ode.ODEState;
- import org.hipparchus.ode.ODEStateAndDerivative;
- import org.hipparchus.util.FastMath;
- /**
- * This class implements a Gragg-Bulirsch-Stoer integrator for
- * Ordinary Differential Equations.
- *
- * <p>The Gragg-Bulirsch-Stoer algorithm is one of the most efficient
- * ones currently available for smooth problems. It uses Richardson
- * extrapolation to estimate what would be the solution if the step
- * size could be decreased down to zero.</p>
- *
- * <p>
- * This method changes both the step size and the order during
- * integration, in order to minimize computation cost. It is
- * particularly well suited when a very high precision is needed. The
- * limit where this method becomes more efficient than high-order
- * embedded Runge-Kutta methods like {@link DormandPrince853Integrator
- * Dormand-Prince 8(5,3)} depends on the problem. Results given in the
- * Hairer, Norsett and Wanner book show for example that this limit
- * occurs for accuracy around 1e-6 when integrating Saltzam-Lorenz
- * equations (the authors note this problem is <i>extremely sensitive
- * to the errors in the first integration steps</i>), and around 1e-11
- * for a two dimensional celestial mechanics problems with seven
- * bodies (pleiades problem, involving quasi-collisions for which
- * <i>automatic step size control is essential</i>).
- * </p>
- *
- * <p>
- * This implementation is basically a reimplementation in Java of the
- * <a
- * href="http://www.unige.ch/math/folks/hairer/prog/nonstiff/odex.f">odex</a>
- * fortran code by E. Hairer and G. Wanner. The redistribution policy
- * for this code is available <a
- * href="http://www.unige.ch/~hairer/prog/licence.txt">here</a>, for
- * convenience, it is reproduced below.</p>
- *
- * <blockquote>
- * <p>Copyright (c) 2004, Ernst Hairer</p>
- *
- * <p>Redistribution and use in source and binary forms, with or
- * without modification, are permitted provided that the following
- * conditions are met:</p>
- * <ul>
- * <li>Redistributions of source code must retain the above copyright
- * notice, this list of conditions and the following disclaimer.</li>
- * <li>Redistributions in binary form must reproduce the above copyright
- * notice, this list of conditions and the following disclaimer in the
- * documentation and/or other materials provided with the distribution.</li>
- * </ul>
- *
- * <p><strong>THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND
- * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING,
- * BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE REGENTS OR
- * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
- * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
- * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
- * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
- * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
- * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
- * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.</strong></p>
- * </blockquote>
- *
- */
- public class GraggBulirschStoerIntegrator extends AdaptiveStepsizeIntegrator {
- /** Name of integration scheme. */
- public static final String METHOD_NAME = "Gragg-Bulirsch-Stoer";
- /** maximal order. */
- private int maxOrder;
- /** step size sequence. */
- private int[] sequence;
- /** overall cost of applying step reduction up to iteration k + 1, in number of calls. */
- private int[] costPerStep;
- /** cost per unit step. */
- private double[] costPerTimeUnit;
- /** optimal steps for each order. */
- private double[] optimalStep;
- /** extrapolation coefficients. */
- private double[][] coeff;
- /** stability check enabling parameter. */
- private boolean performTest;
- /** maximal number of checks for each iteration. */
- private int maxChecks;
- /** maximal number of iterations for which checks are performed. */
- private int maxIter;
- /** stepsize reduction factor in case of stability check failure. */
- private double stabilityReduction;
- /** first stepsize control factor. */
- private double stepControl1;
- /** second stepsize control factor. */
- private double stepControl2;
- /** third stepsize control factor. */
- private double stepControl3;
- /** fourth stepsize control factor. */
- private double stepControl4;
- /** first order control factor. */
- private double orderControl1;
- /** second order control factor. */
- private double orderControl2;
- /** use interpolation error in stepsize control. */
- private boolean useInterpolationError;
- /** interpolation order control parameter. */
- private int mudif;
- /** Simple constructor.
- * Build a Gragg-Bulirsch-Stoer integrator with the given step
- * bounds. All tuning parameters are set to their default
- * values. The default step handler does nothing.
- * @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 GraggBulirschStoerIntegrator(final double minStep, final double maxStep,
- final double scalAbsoluteTolerance,
- final double scalRelativeTolerance) {
- super(METHOD_NAME, minStep, maxStep,
- scalAbsoluteTolerance, scalRelativeTolerance);
- setStabilityCheck(true, -1, -1, -1);
- setControlFactors(-1, -1, -1, -1);
- setOrderControl(-1, -1, -1);
- setInterpolationControl(true, -1);
- }
- /** Simple constructor.
- * Build a Gragg-Bulirsch-Stoer integrator with the given step
- * bounds. All tuning parameters are set to their default
- * values. The default step handler does nothing.
- * @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
- */
- public GraggBulirschStoerIntegrator(final double minStep, final double maxStep,
- final double[] vecAbsoluteTolerance,
- final double[] vecRelativeTolerance) {
- super(METHOD_NAME, minStep, maxStep,
- vecAbsoluteTolerance, vecRelativeTolerance);
- setStabilityCheck(true, -1, -1, -1);
- setControlFactors(-1, -1, -1, -1);
- setOrderControl(-1, -1, -1);
- setInterpolationControl(true, -1);
- }
- /** Set the stability check controls.
- * <p>The stability check is performed on the first few iterations of
- * the extrapolation scheme. If this test fails, the step is rejected
- * and the stepsize is reduced.</p>
- * <p>By default, the test is performed, at most during two
- * iterations at each step, and at most once for each of these
- * iterations. The default stepsize reduction factor is 0.5.</p>
- * @param performStabilityCheck if true, stability check will be performed,
- if false, the check will be skipped
- * @param maxNumIter maximal number of iterations for which checks are
- * performed (the number of iterations is reset to default if negative
- * or null)
- * @param maxNumChecks maximal number of checks for each iteration
- * (the number of checks is reset to default if negative or null)
- * @param stepsizeReductionFactor stepsize reduction factor in case of
- * failure (the factor is reset to default if lower than 0.0001 or
- * greater than 0.9999)
- */
- public void setStabilityCheck(final boolean performStabilityCheck,
- final int maxNumIter, final int maxNumChecks,
- final double stepsizeReductionFactor) {
- this.performTest = performStabilityCheck;
- this.maxIter = (maxNumIter <= 0) ? 2 : maxNumIter;
- this.maxChecks = (maxNumChecks <= 0) ? 1 : maxNumChecks;
- if ((stepsizeReductionFactor < 0.0001) || (stepsizeReductionFactor > 0.9999)) {
- this.stabilityReduction = 0.5;
- } else {
- this.stabilityReduction = stepsizeReductionFactor;
- }
- }
- /** Set the step size control factors.
- * <p>The new step size hNew is computed from the old one h by:
- * <pre>
- * hNew = h * stepControl2 / (err/stepControl1)^(1/(2k + 1))
- * </pre>
- * <p>where err is the scaled error and k the iteration number of the
- * extrapolation scheme (counting from 0). The default values are
- * 0.65 for stepControl1 and 0.94 for stepControl2.</p>
- * <p>The step size is subject to the restriction:</p>
- * <pre>
- * stepControl3^(1/(2k + 1))/stepControl4 <= hNew/h <= 1/stepControl3^(1/(2k + 1))
- * </pre>
- * <p>The default values are 0.02 for stepControl3 and 4.0 for
- * stepControl4.</p>
- * @param control1 first stepsize control factor (the factor is
- * reset to default if lower than 0.0001 or greater than 0.9999)
- * @param control2 second stepsize control factor (the factor
- * is reset to default if lower than 0.0001 or greater than 0.9999)
- * @param control3 third stepsize control factor (the factor is
- * reset to default if lower than 0.0001 or greater than 0.9999)
- * @param control4 fourth stepsize control factor (the factor
- * is reset to default if lower than 1.0001 or greater than 999.9)
- */
- public void setControlFactors(final double control1, final double control2,
- final double control3, final double control4) {
- if ((control1 < 0.0001) || (control1 > 0.9999)) {
- this.stepControl1 = 0.65;
- } else {
- this.stepControl1 = control1;
- }
- if ((control2 < 0.0001) || (control2 > 0.9999)) {
- this.stepControl2 = 0.94;
- } else {
- this.stepControl2 = control2;
- }
- if ((control3 < 0.0001) || (control3 > 0.9999)) {
- this.stepControl3 = 0.02;
- } else {
- this.stepControl3 = control3;
- }
- if ((control4 < 1.0001) || (control4 > 999.9)) {
- this.stepControl4 = 4.0;
- } else {
- this.stepControl4 = control4;
- }
- }
- /** Set the order control parameters.
- * <p>The Gragg-Bulirsch-Stoer method changes both the step size and
- * the order during integration, in order to minimize computation
- * cost. Each extrapolation step increases the order by 2, so the
- * maximal order that will be used is always even, it is twice the
- * maximal number of columns in the extrapolation table.</p>
- * <pre>
- * order is decreased if w(k - 1) <= w(k) * orderControl1
- * order is increased if w(k) <= w(k - 1) * orderControl2
- * </pre>
- * <p>where w is the table of work per unit step for each order
- * (number of function calls divided by the step length), and k is
- * the current order.</p>
- * <p>The default maximal order after construction is 18 (i.e. the
- * maximal number of columns is 9). The default values are 0.8 for
- * orderControl1 and 0.9 for orderControl2.</p>
- * @param maximalOrder maximal order in the extrapolation table (the
- * maximal order is reset to default if order <= 6 or odd)
- * @param control1 first order control factor (the factor is
- * reset to default if lower than 0.0001 or greater than 0.9999)
- * @param control2 second order control factor (the factor
- * is reset to default if lower than 0.0001 or greater than 0.9999)
- */
- public void setOrderControl(final int maximalOrder,
- final double control1, final double control2) {
- if (maximalOrder > 6 && maximalOrder % 2 == 0) {
- this.maxOrder = maximalOrder;
- } else {
- this.maxOrder = 18;
- }
- if ((control1 < 0.0001) || (control1 > 0.9999)) {
- this.orderControl1 = 0.8;
- } else {
- this.orderControl1 = control1;
- }
- if ((control2 < 0.0001) || (control2 > 0.9999)) {
- this.orderControl2 = 0.9;
- } else {
- this.orderControl2 = control2;
- }
- // reinitialize the arrays
- initializeArrays();
- }
- /** Initialize the integrator internal arrays. */
- private void initializeArrays() {
- final int size = maxOrder / 2;
- if ((sequence == null) || (sequence.length != size)) {
- // all arrays should be reallocated with the right size
- sequence = new int[size];
- costPerStep = new int[size];
- coeff = new double[size][];
- costPerTimeUnit = new double[size];
- optimalStep = new double[size];
- }
- // step size sequence: 2, 6, 10, 14, ...
- for (int k = 0; k < size; ++k) {
- sequence[k] = 4 * k + 2;
- }
- // initialize the order selection cost array
- // (number of function calls for each column of the extrapolation table)
- costPerStep[0] = sequence[0] + 1;
- for (int k = 1; k < size; ++k) {
- costPerStep[k] = costPerStep[k - 1] + sequence[k];
- }
- // initialize the extrapolation tables
- for (int k = 0; k < size; ++k) {
- coeff[k] = (k > 0) ? new double[k] : null;
- for (int l = 0; l < k; ++l) {
- final double ratio = ((double) sequence[k]) / sequence[k - l - 1];
- coeff[k][l] = 1.0 / (ratio * ratio - 1.0);
- }
- }
- }
- /** Set the interpolation order control parameter.
- * The interpolation order for dense output is 2k - mudif + 1. The
- * default value for mudif is 4 and the interpolation error is used
- * in stepsize control by default.
- * @param useInterpolationErrorForControl if true, interpolation error is used
- * for stepsize control
- * @param mudifControlParameter interpolation order control parameter (the parameter
- * is reset to default if <= 0 or >= 7)
- */
- public void setInterpolationControl(final boolean useInterpolationErrorForControl,
- final int mudifControlParameter) {
- this.useInterpolationError = useInterpolationErrorForControl;
- if ((mudifControlParameter <= 0) || (mudifControlParameter >= 7)) {
- this.mudif = 4;
- } else {
- this.mudif = mudifControlParameter;
- }
- }
- /** Update scaling array.
- * @param y1 first state vector to use for scaling
- * @param y2 second state vector to use for scaling
- * @param scale scaling array to update (can be shorter than state)
- */
- private void rescale(final double[] y1, final double[] y2, final double[] scale) {
- final StepsizeHelper helper = getStepSizeHelper();
- for (int i = 0; i < scale.length; ++i) {
- scale[i] = helper.getTolerance(i, FastMath.max(FastMath.abs(y1[i]), FastMath.abs(y2[i])));
- }
- }
- /** Perform integration over one step using substeps of a modified
- * midpoint method.
- * @param t0 initial time
- * @param y0 initial value of the state vector at t0
- * @param step global step
- * @param k iteration number (from 0 to sequence.length - 1)
- * @param scale scaling array (can be shorter than state)
- * @param f placeholder where to put the state vector derivatives at each substep
- * (element 0 already contains initial derivative)
- * @param yMiddle placeholder where to put the state vector at the middle of the step
- * @param yEnd placeholder where to put the state vector at the end
- * @return true if computation was done properly,
- * false if stability check failed before end of computation
- * @exception MathIllegalStateException if the number of functions evaluations is exceeded
- * @exception MathIllegalArgumentException if arrays dimensions do not match equations settings
- */
- private boolean tryStep(final double t0, final double[] y0, final double step, final int k,
- final double[] scale, final double[][] f,
- final double[] yMiddle, final double[] yEnd)
- throws MathIllegalArgumentException, MathIllegalStateException {
- final int n = sequence[k];
- final double subStep = step / n;
- final double subStep2 = 2 * subStep;
- // first substep
- double t = t0 + subStep;
- for (int i = 0; i < y0.length; ++i) {
- yEnd[i] = y0[i] + subStep * f[0][i];
- }
- f[1] = computeDerivatives(t, yEnd);
- // other substeps
- final double[] yTmp = y0.clone();
- for (int j = 1; j < n; ++j) {
- if (2 * j == n) {
- // save the point at the middle of the step
- System.arraycopy(yEnd, 0, yMiddle, 0, y0.length);
- }
- t += subStep;
- for (int i = 0; i < y0.length; ++i) {
- final double middle = yEnd[i];
- yEnd[i] = yTmp[i] + subStep2 * f[j][i];
- yTmp[i] = middle;
- }
- f[j + 1] = computeDerivatives(t, yEnd);
- // stability check
- if (performTest && (j <= maxChecks) && (k < maxIter)) {
- double initialNorm = 0.0;
- for (int l = 0; l < scale.length; ++l) {
- final double ratio = f[0][l] / scale[l];
- initialNorm += ratio * ratio;
- }
- double deltaNorm = 0.0;
- for (int l = 0; l < scale.length; ++l) {
- final double ratio = (f[j + 1][l] - f[0][l]) / scale[l];
- deltaNorm += ratio * ratio;
- }
- if (deltaNorm > 4 * FastMath.max(1.0e-15, initialNorm)) {
- return false;
- }
- }
- }
- // correction of the last substep (at t0 + step)
- for (int i = 0; i < y0.length; ++i) {
- yEnd[i] = 0.5 * (yTmp[i] + yEnd[i] + subStep * f[n][i]);
- }
- return true;
- }
- /** Extrapolate a vector.
- * @param offset offset to use in the coefficients table
- * @param k index of the last updated point
- * @param diag working diagonal of the Aitken-Neville's
- * triangle, without the last element
- * @param last last element
- */
- private void extrapolate(final int offset, final int k,
- final double[][] diag, final double[] last) {
- // update the diagonal
- for (int j = 1; j < k; ++j) {
- for (int i = 0; i < last.length; ++i) {
- // Aitken-Neville's recursive formula
- diag[k - j - 1][i] = diag[k - j][i] +
- coeff[k + offset][j - 1] * (diag[k - j][i] - diag[k - j - 1][i]);
- }
- }
- // update the last element
- for (int i = 0; i < last.length; ++i) {
- // Aitken-Neville's recursive formula
- last[i] = diag[0][i] + coeff[k + offset][k - 1] * (diag[0][i] - last[i]);
- }
- }
- /** {@inheritDoc} */
- @Override
- public ODEStateAndDerivative integrate(final ExpandableODE equations,
- final ODEState initialState, final double finalTime)
- throws MathIllegalArgumentException, MathIllegalStateException {
- sanityChecks(initialState, finalTime);
- setStepStart(initIntegration(equations, initialState, finalTime));
- final boolean forward = finalTime > initialState.getTime();
- // create some internal working arrays
- double[] y = getStepStart().getCompleteState();
- final double[] y1 = new double[y.length];
- final double[][] diagonal = new double[sequence.length - 1][];
- final double[][] y1Diag = new double[sequence.length - 1][];
- for (int k = 0; k < sequence.length - 1; ++k) {
- diagonal[k] = new double[y.length];
- y1Diag[k] = new double[y.length];
- }
- final double[][][] fk = new double[sequence.length][][];
- for (int k = 0; k < sequence.length; ++k) {
- fk[k] = new double[sequence[k] + 1][];
- }
- // scaled derivatives at the middle of the step $\tau$
- // (element k is $h^{k} d^{k}y(\tau)/dt^{k}$ where h is step size...)
- final double[][] yMidDots = new double[1 + 2 * sequence.length][y.length];
- // initial scaling
- final int mainSetDimension = getStepSizeHelper().getMainSetDimension();
- final double[] scale = new double[mainSetDimension];
- rescale(y, y, scale);
- // initial order selection
- final double tol = getStepSizeHelper().getRelativeTolerance(0);
- final double log10R = FastMath.log10(FastMath.max(1.0e-10, tol));
- int targetIter = FastMath.max(1,
- FastMath.min(sequence.length - 2,
- (int) FastMath.floor(0.5 - 0.6 * log10R)));
- double hNew = 0;
- double maxError = Double.MAX_VALUE;
- boolean previousRejected = false;
- boolean firstTime = true;
- boolean newStep = true;
- costPerTimeUnit[0] = 0;
- setIsLastStep(false);
- do {
- double error;
- boolean reject = false;
- if (newStep) {
- // first evaluation, at the beginning of the step
- final double[] yDot0 = getStepStart().getCompleteDerivative();
- for (int k = 0; k < sequence.length; ++k) {
- // all sequences start from the same point, so we share the derivatives
- fk[k][0] = yDot0;
- }
- if (firstTime) {
- hNew = initializeStep(forward, 2 * targetIter + 1, scale,
- getStepStart());
- }
- newStep = false;
- }
- setStepSize(hNew);
- // step adjustment near bounds
- if (forward) {
- if (getStepStart().getTime() + getStepSize() >= finalTime) {
- setStepSize(finalTime - getStepStart().getTime());
- }
- } else {
- if (getStepStart().getTime() + getStepSize() <= finalTime) {
- setStepSize(finalTime - getStepStart().getTime());
- }
- }
- final double nextT = getStepStart().getTime() + getStepSize();
- setIsLastStep(forward ? (nextT >= finalTime) : (nextT <= finalTime));
- // iterate over several substep sizes
- int k = -1;
- for (boolean loop = true; loop; ) {
- ++k;
- // modified midpoint integration with the current substep
- if ( ! tryStep(getStepStart().getTime(), y, getStepSize(), k, scale, fk[k],
- (k == 0) ? yMidDots[0] : diagonal[k - 1],
- (k == 0) ? y1 : y1Diag[k - 1])) {
- // the stability check failed, we reduce the global step
- hNew = FastMath.abs(getStepSizeHelper().filterStep(getStepSize() * stabilityReduction, forward, false));
- reject = true;
- loop = false;
- } else {
- // the substep was computed successfully
- if (k > 0) {
- // extrapolate the state at the end of the step
- // using last iteration data
- extrapolate(0, k, y1Diag, y1);
- rescale(y, y1, scale);
- // estimate the error at the end of the step.
- error = 0;
- for (int j = 0; j < mainSetDimension; ++j) {
- final double e = FastMath.abs(y1[j] - y1Diag[0][j]) / scale[j];
- error += e * e;
- }
- error = FastMath.sqrt(error / mainSetDimension);
- if (Double.isNaN(error)) {
- throw new MathIllegalStateException(LocalizedODEFormats.NAN_APPEARING_DURING_INTEGRATION,
- nextT);
- }
- if ((error > 1.0e15) || ((k > 1) && (error > maxError))) {
- // error is too big, we reduce the global step
- hNew = FastMath.abs(getStepSizeHelper().filterStep(getStepSize() * stabilityReduction, forward, false));
- reject = true;
- loop = false;
- } else {
- maxError = FastMath.max(4 * error, 1.0);
- // compute optimal stepsize for this order
- final double exp = 1.0 / (2 * k + 1);
- double fac = stepControl2 / FastMath.pow(error / stepControl1, exp);
- final double pow = FastMath.pow(stepControl3, exp);
- fac = FastMath.max(pow / stepControl4, FastMath.min(1 / pow, fac));
- final boolean acceptSmall = k < targetIter;
- optimalStep[k] = FastMath.abs(getStepSizeHelper().filterStep(getStepSize() * fac, forward, acceptSmall));
- costPerTimeUnit[k] = costPerStep[k] / optimalStep[k];
- // check convergence
- switch (k - targetIter) {
- case -1 :
- if ((targetIter > 1) && ! previousRejected) {
- // check if we can stop iterations now
- if (error <= 1.0) {
- // convergence have been reached just before targetIter
- loop = false;
- } else {
- // estimate if there is a chance convergence will
- // be reached on next iteration, using the
- // asymptotic evolution of error
- final double ratio = ((double) sequence [targetIter] * sequence[targetIter + 1]) /
- (sequence[0] * sequence[0]);
- if (error > ratio * ratio) {
- // we don't expect to converge on next iteration
- // we reject the step immediately and reduce order
- reject = true;
- loop = false;
- targetIter = k;
- if ((targetIter > 1) &&
- (costPerTimeUnit[targetIter - 1] <
- orderControl1 * costPerTimeUnit[targetIter])) {
- --targetIter;
- }
- hNew = getStepSizeHelper().filterStep(optimalStep[targetIter], forward, false);
- }
- }
- }
- break;
- case 0:
- if (error <= 1.0) {
- // convergence has been reached exactly at targetIter
- loop = false;
- } else {
- // estimate if there is a chance convergence will
- // be reached on next iteration, using the
- // asymptotic evolution of error
- final double ratio = ((double) sequence[k + 1]) / sequence[0];
- if (error > ratio * ratio) {
- // we don't expect to converge on next iteration
- // we reject the step immediately
- reject = true;
- loop = false;
- if ((targetIter > 1) &&
- (costPerTimeUnit[targetIter - 1] <
- orderControl1 * costPerTimeUnit[targetIter])) {
- --targetIter;
- }
- hNew = getStepSizeHelper().filterStep(optimalStep[targetIter], forward, false);
- }
- }
- break;
- case 1 :
- if (error > 1.0) {
- reject = true;
- if ((targetIter > 1) &&
- (costPerTimeUnit[targetIter - 1] <
- orderControl1 * costPerTimeUnit[targetIter])) {
- --targetIter;
- }
- hNew = getStepSizeHelper().filterStep(optimalStep[targetIter], forward, false);
- }
- loop = false;
- break;
- default :
- if ((firstTime || isLastStep()) && (error <= 1.0)) {
- loop = false;
- }
- break;
- }
- }
- }
- }
- }
- // dense output handling
- double hInt = getMaxStep();
- final GraggBulirschStoerStateInterpolator interpolator;
- if (! reject) {
- // extrapolate state at middle point of the step
- for (int j = 1; j <= k; ++j) {
- extrapolate(0, j, diagonal, yMidDots[0]);
- }
- final int mu = 2 * k - mudif + 3;
- for (int l = 0; l < mu; ++l) {
- // derivative at middle point of the step
- final int l2 = l / 2;
- double factor = FastMath.pow(0.5 * sequence[l2], l);
- int middleIndex = fk[l2].length / 2;
- for (int i = 0; i < y.length; ++i) {
- yMidDots[l + 1][i] = factor * fk[l2][middleIndex + l][i];
- }
- for (int j = 1; j <= k - l2; ++j) {
- factor = FastMath.pow(0.5 * sequence[j + l2], l);
- middleIndex = fk[l2 + j].length / 2;
- for (int i = 0; i < y.length; ++i) {
- diagonal[j - 1][i] = factor * fk[l2 + j][middleIndex + l][i];
- }
- extrapolate(l2, j, diagonal, yMidDots[l + 1]);
- }
- for (int i = 0; i < y.length; ++i) {
- yMidDots[l + 1][i] *= getStepSize();
- }
- // compute centered differences to evaluate next derivatives
- for (int j = (l + 1) / 2; j <= k; ++j) {
- for (int m = fk[j].length - 1; m >= 2 * (l + 1); --m) {
- for (int i = 0; i < y.length; ++i) {
- fk[j][m][i] -= fk[j][m - 2][i];
- }
- }
- }
- }
- // state at end of step
- final ODEStateAndDerivative stepEnd =
- equations.getMapper().mapStateAndDerivative(nextT, y1, computeDerivatives(nextT, y1));
- // set up interpolator covering the full step
- interpolator = new GraggBulirschStoerStateInterpolator(forward,
- getStepStart(), stepEnd,
- getStepStart(), stepEnd,
- equations.getMapper(),
- yMidDots, mu);
- if (mu >= 0 && useInterpolationError) {
- // use the interpolation error to limit stepsize
- final double interpError = interpolator.estimateError(scale);
- hInt = FastMath.abs(getStepSize() /
- FastMath.max(FastMath.pow(interpError, 1.0 / (mu + 4)), 0.01));
- if (interpError > 10.0) {
- hNew = getStepSizeHelper().filterStep(hInt, forward, false);
- reject = true;
- }
- }
- } else {
- interpolator = null;
- }
- if (! reject) {
- // Discrete events handling
- setStepStart(acceptStep(interpolator, finalTime));
- // prepare next step
- // beware that y1 is not always valid anymore here,
- // as some event may have triggered a reset
- // so we need to copy the new step start set previously
- y = getStepStart().getCompleteState();
- int optimalIter;
- if (k == 1) {
- optimalIter = 2;
- if (previousRejected) {
- optimalIter = 1;
- }
- } else if (k <= targetIter) {
- optimalIter = k;
- if (costPerTimeUnit[k - 1] < orderControl1 * costPerTimeUnit[k]) {
- optimalIter = k - 1;
- } else if (costPerTimeUnit[k] < orderControl2 * costPerTimeUnit[k - 1]) {
- optimalIter = FastMath.min(k + 1, sequence.length - 2);
- }
- } else {
- optimalIter = k - 1;
- if ((k > 2) && (costPerTimeUnit[k - 2] < orderControl1 * costPerTimeUnit[k - 1])) {
- optimalIter = k - 2;
- }
- if (costPerTimeUnit[k] < orderControl2 * costPerTimeUnit[optimalIter]) {
- optimalIter = FastMath.min(k, sequence.length - 2);
- }
- }
- if (previousRejected) {
- // after a rejected step neither order nor stepsize
- // should increase
- targetIter = FastMath.min(optimalIter, k);
- hNew = FastMath.min(FastMath.abs(getStepSize()), optimalStep[targetIter]);
- } else {
- // stepsize control
- if (optimalIter <= k) {
- hNew = getStepSizeHelper().filterStep(optimalStep[optimalIter], forward, false);
- } else {
- if ((k < targetIter) &&
- (costPerTimeUnit[k] < orderControl2 * costPerTimeUnit[k - 1])) {
- hNew = getStepSizeHelper().
- filterStep(optimalStep[k] * costPerStep[optimalIter + 1] / costPerStep[k], forward, false);
- } else {
- hNew = getStepSizeHelper().
- filterStep(optimalStep[k] * costPerStep[optimalIter] / costPerStep[k], forward, false);
- }
- }
- targetIter = optimalIter;
- }
- newStep = true;
- }
- hNew = FastMath.min(hNew, hInt);
- if (! forward) {
- hNew = -hNew;
- }
- firstTime = false;
- if (reject) {
- setIsLastStep(false);
- previousRejected = true;
- } else {
- previousRejected = false;
- }
- } while (!isLastStep());
- final ODEStateAndDerivative finalState = getStepStart();
- resetInternalState();
- return finalState;
- }
- }