GraggBulirschStoerIntegrator.java

  1. /*
  2.  * Licensed to the Hipparchus project under one or more
  3.  * contributor license agreements.  See the NOTICE file distributed with
  4.  * this work for additional information regarding copyright ownership.
  5.  * The Hipparchus project licenses this file to You under the Apache License, Version 2.0
  6.  * (the "License"); you may not use this file except in compliance with
  7.  * the License.  You may obtain a copy of the License at
  8.  *
  9.  *      https://www.apache.org/licenses/LICENSE-2.0
  10.  *
  11.  * Unless required by applicable law or agreed to in writing, software
  12.  * distributed under the License is distributed on an "AS IS" BASIS,
  13.  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  14.  * See the License for the specific language governing permissions and
  15.  * limitations under the License.
  16.  */

  17. package org.hipparchus.ode.nonstiff;

  18. import org.hipparchus.exception.MathIllegalArgumentException;
  19. import org.hipparchus.exception.MathIllegalStateException;
  20. import org.hipparchus.ode.ExpandableODE;
  21. import org.hipparchus.ode.LocalizedODEFormats;
  22. import org.hipparchus.ode.ODEState;
  23. import org.hipparchus.ode.ODEStateAndDerivative;
  24. import org.hipparchus.util.FastMath;

  25. /**
  26.  * This class implements a Gragg-Bulirsch-Stoer integrator for
  27.  * Ordinary Differential Equations.
  28.  *
  29.  * <p>The Gragg-Bulirsch-Stoer algorithm is one of the most efficient
  30.  * ones currently available for smooth problems. It uses Richardson
  31.  * extrapolation to estimate what would be the solution if the step
  32.  * size could be decreased down to zero.</p>
  33.  *
  34.  * <p>
  35.  * This method changes both the step size and the order during
  36.  * integration, in order to minimize computation cost. It is
  37.  * particularly well suited when a very high precision is needed. The
  38.  * limit where this method becomes more efficient than high-order
  39.  * embedded Runge-Kutta methods like {@link DormandPrince853Integrator
  40.  * Dormand-Prince 8(5,3)} depends on the problem. Results given in the
  41.  * Hairer, Norsett and Wanner book show for example that this limit
  42.  * occurs for accuracy around 1e-6 when integrating Saltzam-Lorenz
  43.  * equations (the authors note this problem is <i>extremely sensitive
  44.  * to the errors in the first integration steps</i>), and around 1e-11
  45.  * for a two dimensional celestial mechanics problems with seven
  46.  * bodies (pleiades problem, involving quasi-collisions for which
  47.  * <i>automatic step size control is essential</i>).
  48.  * </p>
  49.  *
  50.  * <p>
  51.  * This implementation is basically a reimplementation in Java of the
  52.  * <a
  53.  * href="http://www.unige.ch/math/folks/hairer/prog/nonstiff/odex.f">odex</a>
  54.  * fortran code by E. Hairer and G. Wanner. The redistribution policy
  55.  * for this code is available <a
  56.  * href="http://www.unige.ch/~hairer/prog/licence.txt">here</a>, for
  57.  * convenience, it is reproduced below.</p>
  58.  *
  59.  * <blockquote>
  60.  * <p>Copyright (c) 2004, Ernst Hairer</p>
  61.  *
  62.  * <p>Redistribution and use in source and binary forms, with or
  63.  * without modification, are permitted provided that the following
  64.  * conditions are met:</p>
  65.  * <ul>
  66.  *  <li>Redistributions of source code must retain the above copyright
  67.  *      notice, this list of conditions and the following disclaimer.</li>
  68.  *  <li>Redistributions in binary form must reproduce the above copyright
  69.  *      notice, this list of conditions and the following disclaimer in the
  70.  *      documentation and/or other materials provided with the distribution.</li>
  71.  * </ul>
  72.  *
  73.  * <p><strong>THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND
  74.  * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING,
  75.  * BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
  76.  * FOR A  PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE REGENTS OR
  77.  * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
  78.  * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
  79.  * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
  80.  * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
  81.  * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
  82.  * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
  83.  * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.</strong></p>
  84.  * </blockquote>
  85.  *
  86.  */

  87. public class GraggBulirschStoerIntegrator extends AdaptiveStepsizeIntegrator {

  88.     /** Name of integration scheme. */
  89.     public static final String METHOD_NAME = "Gragg-Bulirsch-Stoer";

  90.     /** maximal order. */
  91.     private int maxOrder;

  92.     /** step size sequence. */
  93.     private int[] sequence;

  94.     /** overall cost of applying step reduction up to iteration k + 1, in number of calls. */
  95.     private int[] costPerStep;

  96.     /** cost per unit step. */
  97.     private double[] costPerTimeUnit;

  98.     /** optimal steps for each order. */
  99.     private double[] optimalStep;

  100.     /** extrapolation coefficients. */
  101.     private double[][] coeff;

  102.     /** stability check enabling parameter. */
  103.     private boolean performTest;

  104.     /** maximal number of checks for each iteration. */
  105.     private int maxChecks;

  106.     /** maximal number of iterations for which checks are performed. */
  107.     private int maxIter;

  108.     /** stepsize reduction factor in case of stability check failure. */
  109.     private double stabilityReduction;

  110.     /** first stepsize control factor. */
  111.     private double stepControl1;

  112.     /** second stepsize control factor. */
  113.     private double stepControl2;

  114.     /** third stepsize control factor. */
  115.     private double stepControl3;

  116.     /** fourth stepsize control factor. */
  117.     private double stepControl4;

  118.     /** first order control factor. */
  119.     private double orderControl1;

  120.     /** second order control factor. */
  121.     private double orderControl2;

  122.     /** use interpolation error in stepsize control. */
  123.     private boolean useInterpolationError;

  124.     /** interpolation order control parameter. */
  125.     private int mudif;

  126.     /** Simple constructor.
  127.      * Build a Gragg-Bulirsch-Stoer integrator with the given step
  128.      * bounds. All tuning parameters are set to their default
  129.      * values. The default step handler does nothing.
  130.      * @param minStep minimal step (sign is irrelevant, regardless of
  131.      * integration direction, forward or backward), the last step can
  132.      * be smaller than this
  133.      * @param maxStep maximal step (sign is irrelevant, regardless of
  134.      * integration direction, forward or backward), the last step can
  135.      * be smaller than this
  136.      * @param scalAbsoluteTolerance allowed absolute error
  137.      * @param scalRelativeTolerance allowed relative error
  138.      */
  139.     public GraggBulirschStoerIntegrator(final double minStep, final double maxStep,
  140.                                         final double scalAbsoluteTolerance,
  141.                                         final double scalRelativeTolerance) {
  142.         super(METHOD_NAME, minStep, maxStep,
  143.               scalAbsoluteTolerance, scalRelativeTolerance);
  144.         setStabilityCheck(true, -1, -1, -1);
  145.         setControlFactors(-1, -1, -1, -1);
  146.         setOrderControl(-1, -1, -1);
  147.         setInterpolationControl(true, -1);
  148.     }

  149.     /** Simple constructor.
  150.      * Build a Gragg-Bulirsch-Stoer integrator with the given step
  151.      * bounds. All tuning parameters are set to their default
  152.      * values. The default step handler does nothing.
  153.      * @param minStep minimal step (must be positive even for backward
  154.      * integration), the last step can be smaller than this
  155.      * @param maxStep maximal step (must be positive even for backward
  156.      * integration)
  157.      * @param vecAbsoluteTolerance allowed absolute error
  158.      * @param vecRelativeTolerance allowed relative error
  159.      */
  160.     public GraggBulirschStoerIntegrator(final double minStep, final double maxStep,
  161.                                         final double[] vecAbsoluteTolerance,
  162.                                         final double[] vecRelativeTolerance) {
  163.         super(METHOD_NAME, minStep, maxStep,
  164.               vecAbsoluteTolerance, vecRelativeTolerance);
  165.         setStabilityCheck(true, -1, -1, -1);
  166.         setControlFactors(-1, -1, -1, -1);
  167.         setOrderControl(-1, -1, -1);
  168.         setInterpolationControl(true, -1);
  169.     }

  170.     /** Set the stability check controls.
  171.      * <p>The stability check is performed on the first few iterations of
  172.      * the extrapolation scheme. If this test fails, the step is rejected
  173.      * and the stepsize is reduced.</p>
  174.      * <p>By default, the test is performed, at most during two
  175.      * iterations at each step, and at most once for each of these
  176.      * iterations. The default stepsize reduction factor is 0.5.</p>
  177.      * @param performStabilityCheck if true, stability check will be performed,
  178.      if false, the check will be skipped
  179.      * @param maxNumIter maximal number of iterations for which checks are
  180.      * performed (the number of iterations is reset to default if negative
  181.      * or null)
  182.      * @param maxNumChecks maximal number of checks for each iteration
  183.      * (the number of checks is reset to default if negative or null)
  184.      * @param stepsizeReductionFactor stepsize reduction factor in case of
  185.      * failure (the factor is reset to default if lower than 0.0001 or
  186.      * greater than 0.9999)
  187.      */
  188.     public void setStabilityCheck(final boolean performStabilityCheck,
  189.                                   final int maxNumIter, final int maxNumChecks,
  190.                                   final double stepsizeReductionFactor) {

  191.         this.performTest = performStabilityCheck;
  192.         this.maxIter     = (maxNumIter   <= 0) ? 2 : maxNumIter;
  193.         this.maxChecks   = (maxNumChecks <= 0) ? 1 : maxNumChecks;

  194.         if ((stepsizeReductionFactor < 0.0001) || (stepsizeReductionFactor > 0.9999)) {
  195.             this.stabilityReduction = 0.5;
  196.         } else {
  197.             this.stabilityReduction = stepsizeReductionFactor;
  198.         }

  199.     }

  200.     /** Set the step size control factors.

  201.      * <p>The new step size hNew is computed from the old one h by:
  202.      * <pre>
  203.      * hNew = h * stepControl2 / (err/stepControl1)^(1/(2k + 1))
  204.      * </pre>
  205.      * <p>where err is the scaled error and k the iteration number of the
  206.      * extrapolation scheme (counting from 0). The default values are
  207.      * 0.65 for stepControl1 and 0.94 for stepControl2.</p>
  208.      * <p>The step size is subject to the restriction:</p>
  209.      * <pre>
  210.      * stepControl3^(1/(2k + 1))/stepControl4 &lt;= hNew/h &lt;= 1/stepControl3^(1/(2k + 1))
  211.      * </pre>
  212.      * <p>The default values are 0.02 for stepControl3 and 4.0 for
  213.      * stepControl4.</p>
  214.      * @param control1 first stepsize control factor (the factor is
  215.      * reset to default if lower than 0.0001 or greater than 0.9999)
  216.      * @param control2 second stepsize control factor (the factor
  217.      * is reset to default if lower than 0.0001 or greater than 0.9999)
  218.      * @param control3 third stepsize control factor (the factor is
  219.      * reset to default if lower than 0.0001 or greater than 0.9999)
  220.      * @param control4 fourth stepsize control factor (the factor
  221.      * is reset to default if lower than 1.0001 or greater than 999.9)
  222.      */
  223.     public void setControlFactors(final double control1, final double control2,
  224.                                   final double control3, final double control4) {

  225.         if ((control1 < 0.0001) || (control1 > 0.9999)) {
  226.             this.stepControl1 = 0.65;
  227.         } else {
  228.             this.stepControl1 = control1;
  229.         }

  230.         if ((control2 < 0.0001) || (control2 > 0.9999)) {
  231.             this.stepControl2 = 0.94;
  232.         } else {
  233.             this.stepControl2 = control2;
  234.         }

  235.         if ((control3 < 0.0001) || (control3 > 0.9999)) {
  236.             this.stepControl3 = 0.02;
  237.         } else {
  238.             this.stepControl3 = control3;
  239.         }

  240.         if ((control4 < 1.0001) || (control4 > 999.9)) {
  241.             this.stepControl4 = 4.0;
  242.         } else {
  243.             this.stepControl4 = control4;
  244.         }

  245.     }

  246.     /** Set the order control parameters.
  247.      * <p>The Gragg-Bulirsch-Stoer method changes both the step size and
  248.      * the order during integration, in order to minimize computation
  249.      * cost. Each extrapolation step increases the order by 2, so the
  250.      * maximal order that will be used is always even, it is twice the
  251.      * maximal number of columns in the extrapolation table.</p>
  252.      * <pre>
  253.      * order is decreased if w(k - 1) &lt;= w(k)     * orderControl1
  254.      * order is increased if w(k)     &lt;= w(k - 1) * orderControl2
  255.      * </pre>
  256.      * <p>where w is the table of work per unit step for each order
  257.      * (number of function calls divided by the step length), and k is
  258.      * the current order.</p>
  259.      * <p>The default maximal order after construction is 18 (i.e. the
  260.      * maximal number of columns is 9). The default values are 0.8 for
  261.      * orderControl1 and 0.9 for orderControl2.</p>
  262.      * @param maximalOrder maximal order in the extrapolation table (the
  263.      * maximal order is reset to default if order &lt;= 6 or odd)
  264.      * @param control1 first order control factor (the factor is
  265.      * reset to default if lower than 0.0001 or greater than 0.9999)
  266.      * @param control2 second order control factor (the factor
  267.      * is reset to default if lower than 0.0001 or greater than 0.9999)
  268.      */
  269.     public void setOrderControl(final int maximalOrder,
  270.                                 final double control1, final double control2) {

  271.         if (maximalOrder > 6 && maximalOrder % 2 == 0) {
  272.             this.maxOrder = maximalOrder;
  273.         } else {
  274.             this.maxOrder = 18;
  275.         }

  276.         if ((control1 < 0.0001) || (control1 > 0.9999)) {
  277.             this.orderControl1 = 0.8;
  278.         } else {
  279.             this.orderControl1 = control1;
  280.         }

  281.         if ((control2 < 0.0001) || (control2 > 0.9999)) {
  282.             this.orderControl2 = 0.9;
  283.         } else {
  284.             this.orderControl2 = control2;
  285.         }

  286.         // reinitialize the arrays
  287.         initializeArrays();

  288.     }

  289.     /** Initialize the integrator internal arrays. */
  290.     private void initializeArrays() {

  291.         final int size = maxOrder / 2;

  292.         if ((sequence == null) || (sequence.length != size)) {
  293.             // all arrays should be reallocated with the right size
  294.             sequence        = new int[size];
  295.             costPerStep     = new int[size];
  296.             coeff           = new double[size][];
  297.             costPerTimeUnit = new double[size];
  298.             optimalStep     = new double[size];
  299.         }

  300.         // step size sequence: 2, 6, 10, 14, ...
  301.         for (int k = 0; k < size; ++k) {
  302.             sequence[k] = 4 * k + 2;
  303.         }

  304.         // initialize the order selection cost array
  305.         // (number of function calls for each column of the extrapolation table)
  306.         costPerStep[0] = sequence[0] + 1;
  307.         for (int k = 1; k < size; ++k) {
  308.             costPerStep[k] = costPerStep[k - 1] + sequence[k];
  309.         }

  310.         // initialize the extrapolation tables
  311.         for (int k = 0; k < size; ++k) {
  312.             coeff[k] = (k > 0) ? new double[k] : null;
  313.             for (int l = 0; l < k; ++l) {
  314.                 final double ratio = ((double) sequence[k]) / sequence[k - l - 1];
  315.                 coeff[k][l] = 1.0 / (ratio * ratio - 1.0);
  316.             }
  317.         }

  318.     }

  319.     /** Set the interpolation order control parameter.
  320.      * The interpolation order for dense output is 2k - mudif + 1. The
  321.      * default value for mudif is 4 and the interpolation error is used
  322.      * in stepsize control by default.

  323.      * @param useInterpolationErrorForControl if true, interpolation error is used
  324.      * for stepsize control
  325.      * @param mudifControlParameter interpolation order control parameter (the parameter
  326.      * is reset to default if &lt;= 0 or &gt;= 7)
  327.      */
  328.     public void setInterpolationControl(final boolean useInterpolationErrorForControl,
  329.                                         final int mudifControlParameter) {

  330.         this.useInterpolationError = useInterpolationErrorForControl;

  331.         if ((mudifControlParameter <= 0) || (mudifControlParameter >= 7)) {
  332.             this.mudif = 4;
  333.         } else {
  334.             this.mudif = mudifControlParameter;
  335.         }

  336.     }

  337.     /** Update scaling array.
  338.      * @param y1 first state vector to use for scaling
  339.      * @param y2 second state vector to use for scaling
  340.      * @param scale scaling array to update (can be shorter than state)
  341.      */
  342.     private void rescale(final double[] y1, final double[] y2, final double[] scale) {
  343.         final StepsizeHelper helper = getStepSizeHelper();
  344.         for (int i = 0; i < scale.length; ++i) {
  345.             scale[i] = helper.getTolerance(i, FastMath.max(FastMath.abs(y1[i]), FastMath.abs(y2[i])));
  346.         }
  347.     }

  348.     /** Perform integration over one step using substeps of a modified
  349.      * midpoint method.
  350.      * @param t0 initial time
  351.      * @param y0 initial value of the state vector at t0
  352.      * @param step global step
  353.      * @param k iteration number (from 0 to sequence.length - 1)
  354.      * @param scale scaling array (can be shorter than state)
  355.      * @param f placeholder where to put the state vector derivatives at each substep
  356.      *          (element 0 already contains initial derivative)
  357.      * @param yMiddle placeholder where to put the state vector at the middle of the step
  358.      * @param yEnd placeholder where to put the state vector at the end
  359.      * @return true if computation was done properly,
  360.      *         false if stability check failed before end of computation
  361.      * @exception MathIllegalStateException if the number of functions evaluations is exceeded
  362.      * @exception MathIllegalArgumentException if arrays dimensions do not match equations settings
  363.      */
  364.     private boolean tryStep(final double t0, final double[] y0, final double step, final int k,
  365.                             final double[] scale, final double[][] f,
  366.                             final double[] yMiddle, final double[] yEnd)
  367.         throws MathIllegalArgumentException, MathIllegalStateException {

  368.         final int    n        = sequence[k];
  369.         final double subStep  = step / n;
  370.         final double subStep2 = 2 * subStep;

  371.         // first substep
  372.         double t = t0 + subStep;
  373.         for (int i = 0; i < y0.length; ++i) {
  374.             yEnd[i] = y0[i] + subStep * f[0][i];
  375.         }
  376.         f[1] = computeDerivatives(t, yEnd);

  377.         // other substeps
  378.         final double[] yTmp = y0.clone();
  379.         for (int j = 1; j < n; ++j) {

  380.             if (2 * j == n) {
  381.                 // save the point at the middle of the step
  382.                 System.arraycopy(yEnd, 0, yMiddle, 0, y0.length);
  383.             }

  384.             t += subStep;
  385.             for (int i = 0; i < y0.length; ++i) {
  386.                 final double middle = yEnd[i];
  387.                 yEnd[i]       = yTmp[i] + subStep2 * f[j][i];
  388.                 yTmp[i]       = middle;
  389.             }

  390.             f[j + 1] = computeDerivatives(t, yEnd);

  391.             // stability check
  392.             if (performTest && (j <= maxChecks) && (k < maxIter)) {
  393.                 double initialNorm = 0.0;
  394.                 for (int l = 0; l < scale.length; ++l) {
  395.                     final double ratio = f[0][l] / scale[l];
  396.                     initialNorm += ratio * ratio;
  397.                 }
  398.                 double deltaNorm = 0.0;
  399.                 for (int l = 0; l < scale.length; ++l) {
  400.                     final double ratio = (f[j + 1][l] - f[0][l]) / scale[l];
  401.                     deltaNorm += ratio * ratio;
  402.                 }
  403.                 if (deltaNorm > 4 * FastMath.max(1.0e-15, initialNorm)) {
  404.                     return false;
  405.                 }
  406.             }

  407.         }

  408.         // correction of the last substep (at t0 + step)
  409.         for (int i = 0; i < y0.length; ++i) {
  410.             yEnd[i] = 0.5 * (yTmp[i] + yEnd[i] + subStep * f[n][i]);
  411.         }

  412.         return true;

  413.     }

  414.     /** Extrapolate a vector.
  415.      * @param offset offset to use in the coefficients table
  416.      * @param k index of the last updated point
  417.      * @param diag working diagonal of the Aitken-Neville's
  418.      * triangle, without the last element
  419.      * @param last last element
  420.      */
  421.     private void extrapolate(final int offset, final int k,
  422.                              final double[][] diag, final double[] last) {

  423.         // update the diagonal
  424.         for (int j = 1; j < k; ++j) {
  425.             for (int i = 0; i < last.length; ++i) {
  426.                 // Aitken-Neville's recursive formula
  427.                 diag[k - j - 1][i] = diag[k - j][i] +
  428.                                 coeff[k + offset][j - 1] * (diag[k - j][i] - diag[k - j - 1][i]);
  429.             }
  430.         }

  431.         // update the last element
  432.         for (int i = 0; i < last.length; ++i) {
  433.             // Aitken-Neville's recursive formula
  434.             last[i] = diag[0][i] + coeff[k + offset][k - 1] * (diag[0][i] - last[i]);
  435.         }
  436.     }

  437.     /** {@inheritDoc} */
  438.     @Override
  439.     public ODEStateAndDerivative integrate(final ExpandableODE equations,
  440.                                            final ODEState initialState, final double finalTime)
  441.         throws MathIllegalArgumentException, MathIllegalStateException {

  442.         sanityChecks(initialState, finalTime);
  443.         setStepStart(initIntegration(equations, initialState, finalTime));
  444.         final boolean forward = finalTime > initialState.getTime();

  445.         // create some internal working arrays
  446.         double[]         y        = getStepStart().getCompleteState();
  447.         final double[]   y1       = new double[y.length];
  448.         final double[][] diagonal = new double[sequence.length - 1][];
  449.         final double[][] y1Diag   = new double[sequence.length - 1][];
  450.         for (int k = 0; k < sequence.length - 1; ++k) {
  451.             diagonal[k] = new double[y.length];
  452.             y1Diag[k]   = new double[y.length];
  453.         }

  454.         final double[][][] fk = new double[sequence.length][][];
  455.         for (int k = 0; k < sequence.length; ++k) {
  456.             fk[k] = new double[sequence[k] + 1][];
  457.         }

  458.         // scaled derivatives at the middle of the step $\tau$
  459.         // (element k is $h^{k} d^{k}y(\tau)/dt^{k}$ where h is step size...)
  460.         final double[][] yMidDots = new double[1 + 2 * sequence.length][y.length];

  461.         // initial scaling
  462.         final int mainSetDimension = getStepSizeHelper().getMainSetDimension();
  463.         final double[] scale = new double[mainSetDimension];
  464.         rescale(y, y, scale);

  465.         // initial order selection
  466.         final double tol    = getStepSizeHelper().getRelativeTolerance(0);
  467.         final double log10R = FastMath.log10(FastMath.max(1.0e-10, tol));
  468.         int targetIter = FastMath.max(1,
  469.                                       FastMath.min(sequence.length - 2,
  470.                                                    (int) FastMath.floor(0.5 - 0.6 * log10R)));

  471.         double  hNew                     = 0;
  472.         double  maxError                 = Double.MAX_VALUE;
  473.         boolean previousRejected         = false;
  474.         boolean firstTime                = true;
  475.         boolean newStep                  = true;
  476.         costPerTimeUnit[0] = 0;
  477.         setIsLastStep(false);
  478.         do {

  479.             double error;
  480.             boolean reject = false;

  481.             if (newStep) {

  482.                 // first evaluation, at the beginning of the step
  483.                 final double[] yDot0 = getStepStart().getCompleteDerivative();
  484.                 for (int k = 0; k < sequence.length; ++k) {
  485.                     // all sequences start from the same point, so we share the derivatives
  486.                     fk[k][0] = yDot0;
  487.                 }

  488.                 if (firstTime) {
  489.                     hNew = initializeStep(forward, 2 * targetIter + 1, scale,
  490.                                           getStepStart());
  491.                 }

  492.                 newStep = false;

  493.             }

  494.             setStepSize(hNew);

  495.             // step adjustment near bounds
  496.             if (forward) {
  497.                 if (getStepStart().getTime() + getStepSize() >= finalTime) {
  498.                     setStepSize(finalTime - getStepStart().getTime());
  499.                 }
  500.             } else {
  501.                 if (getStepStart().getTime() + getStepSize() <= finalTime) {
  502.                     setStepSize(finalTime - getStepStart().getTime());
  503.                 }
  504.             }
  505.             final double nextT = getStepStart().getTime() + getStepSize();
  506.             setIsLastStep(forward ? (nextT >= finalTime) : (nextT <= finalTime));

  507.             // iterate over several substep sizes
  508.             int k = -1;
  509.             for (boolean loop = true; loop; ) {

  510.                 ++k;

  511.                 // modified midpoint integration with the current substep
  512.                 if ( ! tryStep(getStepStart().getTime(), y, getStepSize(), k, scale, fk[k],
  513.                                (k == 0) ? yMidDots[0] : diagonal[k - 1],
  514.                                (k == 0) ? y1 : y1Diag[k - 1])) {

  515.                     // the stability check failed, we reduce the global step
  516.                     hNew   = FastMath.abs(getStepSizeHelper().filterStep(getStepSize() * stabilityReduction, forward, false));
  517.                     reject = true;
  518.                     loop   = false;

  519.                 } else {

  520.                     // the substep was computed successfully
  521.                     if (k > 0) {

  522.                         // extrapolate the state at the end of the step
  523.                         // using last iteration data
  524.                         extrapolate(0, k, y1Diag, y1);
  525.                         rescale(y, y1, scale);

  526.                         // estimate the error at the end of the step.
  527.                         error = 0;
  528.                         for (int j = 0; j < mainSetDimension; ++j) {
  529.                             final double e = FastMath.abs(y1[j] - y1Diag[0][j]) / scale[j];
  530.                             error += e * e;
  531.                         }
  532.                         error = FastMath.sqrt(error / mainSetDimension);
  533.                         if (Double.isNaN(error)) {
  534.                             throw new MathIllegalStateException(LocalizedODEFormats.NAN_APPEARING_DURING_INTEGRATION,
  535.                                                                 nextT);
  536.                         }

  537.                         if ((error > 1.0e15) || ((k > 1) && (error > maxError))) {
  538.                             // error is too big, we reduce the global step
  539.                             hNew   = FastMath.abs(getStepSizeHelper().filterStep(getStepSize() * stabilityReduction, forward, false));
  540.                             reject = true;
  541.                             loop   = false;
  542.                         } else {

  543.                             maxError = FastMath.max(4 * error, 1.0);

  544.                             // compute optimal stepsize for this order
  545.                             final double exp = 1.0 / (2 * k + 1);
  546.                             double fac = stepControl2 / FastMath.pow(error / stepControl1, exp);
  547.                             final double pow = FastMath.pow(stepControl3, exp);
  548.                             fac = FastMath.max(pow / stepControl4, FastMath.min(1 / pow, fac));
  549.                             final boolean acceptSmall = k < targetIter;
  550.                             optimalStep[k]     = FastMath.abs(getStepSizeHelper().filterStep(getStepSize() * fac, forward, acceptSmall));
  551.                             costPerTimeUnit[k] = costPerStep[k] / optimalStep[k];

  552.                             // check convergence
  553.                             switch (k - targetIter) {

  554.                                 case -1 :
  555.                                     if ((targetIter > 1) && ! previousRejected) {

  556.                                         // check if we can stop iterations now
  557.                                         if (error <= 1.0) {
  558.                                             // convergence have been reached just before targetIter
  559.                                             loop = false;
  560.                                         } else {
  561.                                             // estimate if there is a chance convergence will
  562.                                             // be reached on next iteration, using the
  563.                                             // asymptotic evolution of error
  564.                                             final double ratio = ((double) sequence [targetIter] * sequence[targetIter + 1]) /
  565.                                                             (sequence[0] * sequence[0]);
  566.                                             if (error > ratio * ratio) {
  567.                                                 // we don't expect to converge on next iteration
  568.                                                 // we reject the step immediately and reduce order
  569.                                                 reject = true;
  570.                                                 loop   = false;
  571.                                                 targetIter = k;
  572.                                                 if ((targetIter > 1) &&
  573.                                                     (costPerTimeUnit[targetIter - 1] <
  574.                                                                     orderControl1 * costPerTimeUnit[targetIter])) {
  575.                                                     --targetIter;
  576.                                                 }
  577.                                                 hNew = getStepSizeHelper().filterStep(optimalStep[targetIter], forward, false);
  578.                                             }
  579.                                         }
  580.                                     }
  581.                                     break;

  582.                                 case 0:
  583.                                     if (error <= 1.0) {
  584.                                         // convergence has been reached exactly at targetIter
  585.                                         loop = false;
  586.                                     } else {
  587.                                         // estimate if there is a chance convergence will
  588.                                         // be reached on next iteration, using the
  589.                                         // asymptotic evolution of error
  590.                                         final double ratio = ((double) sequence[k + 1]) / sequence[0];
  591.                                         if (error > ratio * ratio) {
  592.                                             // we don't expect to converge on next iteration
  593.                                             // we reject the step immediately
  594.                                             reject = true;
  595.                                             loop = false;
  596.                                             if ((targetIter > 1) &&
  597.                                                  (costPerTimeUnit[targetIter - 1] <
  598.                                                                  orderControl1 * costPerTimeUnit[targetIter])) {
  599.                                                 --targetIter;
  600.                                             }
  601.                                             hNew = getStepSizeHelper().filterStep(optimalStep[targetIter], forward, false);
  602.                                         }
  603.                                     }
  604.                                     break;

  605.                                 case 1 :
  606.                                     if (error > 1.0) {
  607.                                         reject = true;
  608.                                         if ((targetIter > 1) &&
  609.                                             (costPerTimeUnit[targetIter - 1] <
  610.                                                             orderControl1 * costPerTimeUnit[targetIter])) {
  611.                                             --targetIter;
  612.                                         }
  613.                                         hNew = getStepSizeHelper().filterStep(optimalStep[targetIter], forward, false);
  614.                                     }
  615.                                     loop = false;
  616.                                     break;

  617.                                 default :
  618.                                     if ((firstTime || isLastStep()) && (error <= 1.0)) {
  619.                                         loop = false;
  620.                                     }
  621.                                     break;

  622.                             }

  623.                         }
  624.                     }
  625.                 }
  626.             }

  627.             // dense output handling
  628.             double hInt = getMaxStep();
  629.             final GraggBulirschStoerStateInterpolator interpolator;
  630.             if (! reject) {

  631.                 // extrapolate state at middle point of the step
  632.                 for (int j = 1; j <= k; ++j) {
  633.                     extrapolate(0, j, diagonal, yMidDots[0]);
  634.                 }

  635.                 final int mu = 2 * k - mudif + 3;

  636.                 for (int l = 0; l < mu; ++l) {

  637.                     // derivative at middle point of the step
  638.                     final int l2 = l / 2;
  639.                     double factor = FastMath.pow(0.5 * sequence[l2], l);
  640.                     int middleIndex = fk[l2].length / 2;
  641.                     for (int i = 0; i < y.length; ++i) {
  642.                         yMidDots[l + 1][i] = factor * fk[l2][middleIndex + l][i];
  643.                     }
  644.                     for (int j = 1; j <= k - l2; ++j) {
  645.                         factor = FastMath.pow(0.5 * sequence[j + l2], l);
  646.                         middleIndex = fk[l2 + j].length / 2;
  647.                         for (int i = 0; i < y.length; ++i) {
  648.                             diagonal[j - 1][i] = factor * fk[l2 + j][middleIndex + l][i];
  649.                         }
  650.                         extrapolate(l2, j, diagonal, yMidDots[l + 1]);
  651.                     }
  652.                     for (int i = 0; i < y.length; ++i) {
  653.                         yMidDots[l + 1][i] *= getStepSize();
  654.                     }

  655.                     // compute centered differences to evaluate next derivatives
  656.                     for (int j = (l + 1) / 2; j <= k; ++j) {
  657.                         for (int m = fk[j].length - 1; m >= 2 * (l + 1); --m) {
  658.                             for (int i = 0; i < y.length; ++i) {
  659.                                 fk[j][m][i] -= fk[j][m - 2][i];
  660.                             }
  661.                         }
  662.                     }

  663.                 }

  664.                 // state at end of step
  665.                 final ODEStateAndDerivative stepEnd =
  666.                     equations.getMapper().mapStateAndDerivative(nextT, y1, computeDerivatives(nextT, y1));

  667.                 // set up interpolator covering the full step
  668.                 interpolator = new GraggBulirschStoerStateInterpolator(forward,
  669.                                                                        getStepStart(), stepEnd,
  670.                                                                        getStepStart(), stepEnd,
  671.                                                                        equations.getMapper(),
  672.                                                                        yMidDots, mu);

  673.                 if (mu >= 0 && useInterpolationError) {
  674.                     // use the interpolation error to limit stepsize
  675.                     final double interpError = interpolator.estimateError(scale);
  676.                     hInt = FastMath.abs(getStepSize() /
  677.                                         FastMath.max(FastMath.pow(interpError, 1.0 / (mu + 4)), 0.01));
  678.                     if (interpError > 10.0) {
  679.                         hNew   = getStepSizeHelper().filterStep(hInt, forward, false);
  680.                         reject = true;
  681.                     }
  682.                 }

  683.             } else {
  684.                 interpolator = null;
  685.             }

  686.             if (! reject) {

  687.                 // Discrete events handling
  688.                 setStepStart(acceptStep(interpolator, finalTime));

  689.                 // prepare next step
  690.                 // beware that y1 is not always valid anymore here,
  691.                 // as some event may have triggered a reset
  692.                 // so we need to copy the new step start set previously
  693.                 y = getStepStart().getCompleteState();

  694.                 int optimalIter;
  695.                 if (k == 1) {
  696.                     optimalIter = 2;
  697.                     if (previousRejected) {
  698.                         optimalIter = 1;
  699.                     }
  700.                 } else if (k <= targetIter) {
  701.                     optimalIter = k;
  702.                     if (costPerTimeUnit[k - 1] < orderControl1 * costPerTimeUnit[k]) {
  703.                         optimalIter = k - 1;
  704.                     } else if (costPerTimeUnit[k] < orderControl2 * costPerTimeUnit[k - 1]) {
  705.                         optimalIter = FastMath.min(k + 1, sequence.length - 2);
  706.                     }
  707.                 } else {
  708.                     optimalIter = k - 1;
  709.                     if ((k > 2) && (costPerTimeUnit[k - 2] < orderControl1 * costPerTimeUnit[k - 1])) {
  710.                         optimalIter = k - 2;
  711.                     }
  712.                     if (costPerTimeUnit[k] < orderControl2 * costPerTimeUnit[optimalIter]) {
  713.                         optimalIter = FastMath.min(k, sequence.length - 2);
  714.                     }
  715.                 }

  716.                 if (previousRejected) {
  717.                     // after a rejected step neither order nor stepsize
  718.                     // should increase
  719.                     targetIter = FastMath.min(optimalIter, k);
  720.                     hNew = FastMath.min(FastMath.abs(getStepSize()), optimalStep[targetIter]);
  721.                 } else {
  722.                     // stepsize control
  723.                     if (optimalIter <= k) {
  724.                         hNew = getStepSizeHelper().filterStep(optimalStep[optimalIter], forward, false);
  725.                     } else {
  726.                         if ((k < targetIter) &&
  727.                                         (costPerTimeUnit[k] < orderControl2 * costPerTimeUnit[k - 1])) {
  728.                             hNew = getStepSizeHelper().
  729.                                    filterStep(optimalStep[k] * costPerStep[optimalIter + 1] / costPerStep[k], forward, false);
  730.                         } else {
  731.                             hNew = getStepSizeHelper().
  732.                                    filterStep(optimalStep[k] * costPerStep[optimalIter] / costPerStep[k], forward, false);
  733.                         }
  734.                     }

  735.                     targetIter = optimalIter;

  736.                 }

  737.                 newStep = true;

  738.             }

  739.             hNew = FastMath.min(hNew, hInt);
  740.             if (! forward) {
  741.                 hNew = -hNew;
  742.             }

  743.             firstTime = false;

  744.             if (reject) {
  745.                 setIsLastStep(false);
  746.                 previousRejected = true;
  747.             } else {
  748.                 previousRejected = false;
  749.             }

  750.         } while (!isLastStep());

  751.         final ODEStateAndDerivative finalState = getStepStart();
  752.         resetInternalState();
  753.         return finalState;

  754.     }

  755. }