    001    /*
    002     * Licensed to the Apache Software Foundation (ASF) under one or more
    003     * contributor license agreements.  See the NOTICE file distributed with
    004     * this work for additional information regarding copyright ownership.
    005     * The ASF licenses this file to You under the Apache License, Version 2.0
    006     * (the "License"); you may not use this file except in compliance with
    007     * the License.  You may obtain a copy of the License at
    008     *
    009     *      http://www.apache.org/licenses/LICENSE-2.0
    010     *
    011     * Unless required by applicable law or agreed to in writing, software
    012     * distributed under the License is distributed on an "AS IS" BASIS,
    013     * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
    014     * See the License for the specific language governing permissions and
    015     * limitations under the License.
    016     */
    017
    018    package org.apache.commons.math.ode.nonstiff;
    019
    020    import org.apache.commons.math.ode.DerivativeException;
    021    import org.apache.commons.math.ode.FirstOrderDifferentialEquations;
    022    import org.apache.commons.math.ode.IntegratorException;
    023    import org.apache.commons.math.ode.events.EventHandler;
    024    import org.apache.commons.math.ode.sampling.AbstractStepInterpolator;
    025    import org.apache.commons.math.ode.sampling.DummyStepInterpolator;
    026    import org.apache.commons.math.ode.sampling.StepHandler;
    027    import org.apache.commons.math.util.FastMath;
    028
    029    /**
    030     * This class implements a Gragg-Bulirsch-Stoer integrator for
    031     * Ordinary Differential Equations.
    032     *
    033     * <p>The Gragg-Bulirsch-Stoer algorithm is one of the most efficient
    034     * ones currently available for smooth problems. It uses Richardson
    035     * extrapolation to estimate what would be the solution if the step
    036     * size could be decreased down to zero.</p>
    037     *
    038     * <p>
    039     * This method changes both the step size and the order during
    040     * integration, in order to minimize computation cost. It is
    041     * particularly well suited when a very high precision is needed. The
    042     * limit where this method becomes more efficient than high-order
    043     * embedded Runge-Kutta methods like {@link DormandPrince853Integrator
    044     * Dormand-Prince 8(5,3)} depends on the problem. Results given in the
    045     * Hairer, Norsett and Wanner book show for example that this limit
    046     * occurs for accuracy around 1e-6 when integrating Saltzam-Lorenz
    047     * equations (the authors note this problem is <i>extremely sensitive
    048     * to the errors in the first integration steps</i>), and around 1e-11
    049     * for a two dimensional celestial mechanics problems with seven
    050     * bodies (pleiades problem, involving quasi-collisions for which
    051     * <i>automatic step size control is essential</i>).
    052     * </p>
    053     *
    054     * <p>
    055     * This implementation is basically a reimplementation in Java of the
    056     * <a
    057     * href="http://www.unige.ch/math/folks/hairer/prog/nonstiff/odex.f">odex</a>
    058     * fortran code by E. Hairer and G. Wanner. The redistribution policy
    059     * for this code is available <a
    060     * href="http://www.unige.ch/~hairer/prog/licence.txt">here</a>, for
    061     * convenience, it is reproduced below.</p>
    062     * </p>
    063     *
    064     * <table border="0" width="80%" cellpadding="10" align="center" bgcolor="#E0E0E0">
    065     * <tr><td>Copyright (c) 2004, Ernst Hairer</td></tr>
    066     *
    067     * <tr><td>Redistribution and use in source and binary forms, with or
    068     * without modification, are permitted provided that the following
    069     * conditions are met:
    070     * <ul>
    071     *  <li>Redistributions of source code must retain the above copyright
    072     *      notice, this list of conditions and the following disclaimer.</li>
    073     *  <li>Redistributions in binary form must reproduce the above copyright
    074     *      notice, this list of conditions and the following disclaimer in the
    075     *      documentation and/or other materials provided with the distribution.</li>
    076     * </ul></td></tr>
    077     *
    078     * <tr><td><strong>THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND
    079     * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING,
    080     * BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
    081     * FOR A  PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE REGENTS OR
    082     * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
    083     * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
    084     * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
    085     * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
    086     * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
    087     * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
    088     * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.</strong></td></tr>
    089     * </table>
    090     *
    091     * @version $Revision: 1073158 $ $Date: 2011-02-21 22:46:52 +0100 (lun. 21 f??vr. 2011) $
    092     * @since 1.2
    093     */
    094
    095    public class GraggBulirschStoerIntegrator extends AdaptiveStepsizeIntegrator {
    096
    097        /** Integrator method name. */
    098        private static final String METHOD_NAME = "Gragg-Bulirsch-Stoer";
    099
    100        /** maximal order. */
    101        private int maxOrder;
    102
    103        /** step size sequence. */
    104        private int[] sequence;
    105
    106        /** overall cost of applying step reduction up to iteration k+1, in number of calls. */
    107        private int[] costPerStep;
    108
    109        /** cost per unit step. */
    110        private double[] costPerTimeUnit;
    111
    112        /** optimal steps for each order. */
    113        private double[] optimalStep;
    114
    115        /** extrapolation coefficients. */
    116        private double[][] coeff;
    117
    118        /** stability check enabling parameter. */
    119        private boolean performTest;
    120
    121        /** maximal number of checks for each iteration. */
    122        private int maxChecks;
    123
    124        /** maximal number of iterations for which checks are performed. */
    125        private int maxIter;
    126
    127        /** stepsize reduction factor in case of stability check failure. */
    128        private double stabilityReduction;
    129
    130        /** first stepsize control factor. */
    131        private double stepControl1;
    132
    133        /** second stepsize control factor. */
    134        private double stepControl2;
    135
    136        /** third stepsize control factor. */
    137        private double stepControl3;
    138
    139        /** fourth stepsize control factor. */
    140        private double stepControl4;
    141
    142        /** first order control factor. */
    143        private double orderControl1;
    144
    145        /** second order control factor. */
    146        private double orderControl2;
    147
    148        /** use interpolation error in stepsize control. */
    149        private boolean useInterpolationError;
    150
    151        /** interpolation order control parameter. */
    152        private int mudif;
    153
    154      /** Simple constructor.
    155       * Build a Gragg-Bulirsch-Stoer integrator with the given step
    156       * bounds. All tuning parameters are set to their default
    157       * values. The default step handler does nothing.
    158       * @param minStep minimal step (must be positive even for backward
    159       * integration), the last step can be smaller than this
    160       * @param maxStep maximal step (must be positive even for backward
    161       * integration)
    162       * @param scalAbsoluteTolerance allowed absolute error
    163       * @param scalRelativeTolerance allowed relative error
    164       */
    165      public GraggBulirschStoerIntegrator(final double minStep, final double maxStep,
    166                                          final double scalAbsoluteTolerance,
    167                                          final double scalRelativeTolerance) {
    168        super(METHOD_NAME, minStep, maxStep,
    169              scalAbsoluteTolerance, scalRelativeTolerance);
    170        setStabilityCheck(true, -1, -1, -1);
    171        setStepsizeControl(-1, -1, -1, -1);
    172        setOrderControl(-1, -1, -1);
    173        setInterpolationControl(true, -1);
    174      }
    175
    176      /** Simple constructor.
    177       * Build a Gragg-Bulirsch-Stoer integrator with the given step
    178       * bounds. All tuning parameters are set to their default
    179       * values. The default step handler does nothing.
    180       * @param minStep minimal step (must be positive even for backward
    181       * integration), the last step can be smaller than this
    182       * @param maxStep maximal step (must be positive even for backward
    183       * integration)
    184       * @param vecAbsoluteTolerance allowed absolute error
    185       * @param vecRelativeTolerance allowed relative error
    186       */
    187      public GraggBulirschStoerIntegrator(final double minStep, final double maxStep,
    188                                          final double[] vecAbsoluteTolerance,
    189                                          final double[] vecRelativeTolerance) {
    190        super(METHOD_NAME, minStep, maxStep,
    191              vecAbsoluteTolerance, vecRelativeTolerance);
    192        setStabilityCheck(true, -1, -1, -1);
    193        setStepsizeControl(-1, -1, -1, -1);
    194        setOrderControl(-1, -1, -1);
    195        setInterpolationControl(true, -1);
    196      }
    197
    198      /** Set the stability check controls.
    199       * <p>The stability check is performed on the first few iterations of
    200       * the extrapolation scheme. If this test fails, the step is rejected
    201       * and the stepsize is reduced.</p>
    202       * <p>By default, the test is performed, at most during two
    203       * iterations at each step, and at most once for each of these
    204       * iterations. The default stepsize reduction factor is 0.5.</p>
    205       * @param performStabilityCheck if true, stability check will be performed,
    206         if false, the check will be skipped
    207       * @param maxNumIter maximal number of iterations for which checks are
    208       * performed (the number of iterations is reset to default if negative
    209       * or null)
    210       * @param maxNumChecks maximal number of checks for each iteration
    211       * (the number of checks is reset to default if negative or null)
    212       * @param stepsizeReductionFactor stepsize reduction factor in case of
    213       * failure (the factor is reset to default if lower than 0.0001 or
    214       * greater than 0.9999)
    215       */
    216      public void setStabilityCheck(final boolean performStabilityCheck,
    217                                    final int maxNumIter, final int maxNumChecks,
    218                                    final double stepsizeReductionFactor) {
    219
    220        this.performTest = performStabilityCheck;
    221        this.maxIter     = (maxNumIter   <= 0) ? 2 : maxNumIter;
    222        this.maxChecks   = (maxNumChecks <= 0) ? 1 : maxNumChecks;
    223
    224        if ((stepsizeReductionFactor < 0.0001) || (stepsizeReductionFactor > 0.9999)) {
    225          this.stabilityReduction = 0.5;
    226        } else {
    227          this.stabilityReduction = stepsizeReductionFactor;
    228        }
    229
    230      }
    231
    232      /** Set the step size control factors.
    233
    234       * <p>The new step size hNew is computed from the old one h by:
    235       * <pre>
    236       * hNew = h * stepControl2 / (err/stepControl1)^(1/(2k+1))
    237       * </pre>
    238       * where err is the scaled error and k the iteration number of the
    239       * extrapolation scheme (counting from 0). The default values are
    240       * 0.65 for stepControl1 and 0.94 for stepControl2.</p>
    241       * <p>The step size is subject to the restriction:
    242       * <pre>
    243       * stepControl3^(1/(2k+1))/stepControl4 <= hNew/h <= 1/stepControl3^(1/(2k+1))
    244       * </pre>
    245       * The default values are 0.02 for stepControl3 and 4.0 for
    246       * stepControl4.</p>
    247       * @param control1 first stepsize control factor (the factor is
    248       * reset to default if lower than 0.0001 or greater than 0.9999)
    249       * @param control2 second stepsize control factor (the factor
    250       * is reset to default if lower than 0.0001 or greater than 0.9999)
    251       * @param control3 third stepsize control factor (the factor is
    252       * reset to default if lower than 0.0001 or greater than 0.9999)
    253       * @param control4 fourth stepsize control factor (the factor
    254       * is reset to default if lower than 1.0001 or greater than 999.9)
    255       */
    256      public void setStepsizeControl(final double control1, final double control2,
    257                                     final double control3, final double control4) {
    258
    259        if ((control1 < 0.0001) || (control1 > 0.9999)) {
    260          this.stepControl1 = 0.65;
    261        } else {
    262          this.stepControl1 = control1;
    263        }
    264
    265        if ((control2 < 0.0001) || (control2 > 0.9999)) {
    266          this.stepControl2 = 0.94;
    267        } else {
    268          this.stepControl2 = control2;
    269        }
    270
    271        if ((control3 < 0.0001) || (control3 > 0.9999)) {
    272          this.stepControl3 = 0.02;
    273        } else {
    274          this.stepControl3 = control3;
    275        }
    276
    277        if ((control4 < 1.0001) || (control4 > 999.9)) {
    278          this.stepControl4 = 4.0;
    279        } else {
    280          this.stepControl4 = control4;
    281        }
    282
    283      }
    284
    285      /** Set the order control parameters.
    286       * <p>The Gragg-Bulirsch-Stoer method changes both the step size and
    287       * the order during integration, in order to minimize computation
    288       * cost. Each extrapolation step increases the order by 2, so the
    289       * maximal order that will be used is always even, it is twice the
    290       * maximal number of columns in the extrapolation table.</p>
    291       * <pre>
    292       * order is decreased if w(k-1) <= w(k)   * orderControl1
    293       * order is increased if w(k)   <= w(k-1) * orderControl2
    294       * </pre>
    295       * <p>where w is the table of work per unit step for each order
    296       * (number of function calls divided by the step length), and k is
    297       * the current order.</p>
    298       * <p>The default maximal order after construction is 18 (i.e. the
    299       * maximal number of columns is 9). The default values are 0.8 for
    300       * orderControl1 and 0.9 for orderControl2.</p>
    301       * @param maximalOrder maximal order in the extrapolation table (the
    302       * maximal order is reset to default if order <= 6 or odd)
    303       * @param control1 first order control factor (the factor is
    304       * reset to default if lower than 0.0001 or greater than 0.9999)
    305       * @param control2 second order control factor (the factor
    306       * is reset to default if lower than 0.0001 or greater than 0.9999)
    307       */
    308      public void setOrderControl(final int maximalOrder,
    309                                  final double control1, final double control2) {
    310
    311        if ((maximalOrder <= 6) || (maximalOrder % 2 != 0)) {
    312          this.maxOrder = 18;
    313        }
    314
    315        if ((control1 < 0.0001) || (control1 > 0.9999)) {
    316          this.orderControl1 = 0.8;
    317        } else {
    318          this.orderControl1 = control1;
    319        }
    320
    321        if ((control2 < 0.0001) || (control2 > 0.9999)) {
    322          this.orderControl2 = 0.9;
    323        } else {
    324          this.orderControl2 = control2;
    325        }
    326
    327        // reinitialize the arrays
    328        initializeArrays();
    329
    330      }
    331
    332      /** {@inheritDoc} */
    333      @Override
    334      public void addStepHandler (final StepHandler handler) {
    335
    336        super.addStepHandler(handler);
    337
    338        // reinitialize the arrays
    339        initializeArrays();
    340
    341      }
    342
    343      /** {@inheritDoc} */
    344      @Override
    345      public void addEventHandler(final EventHandler function,
    346                                  final double maxCheckInterval,
    347                                  final double convergence,
    348                                  final int maxIterationCount) {
    349        super.addEventHandler(function, maxCheckInterval, convergence, maxIterationCount);
    350
    351        // reinitialize the arrays
    352        initializeArrays();
    353
    354      }
    355
    356      /** Initialize the integrator internal arrays. */
    357      private void initializeArrays() {
    358
    359        final int size = maxOrder / 2;
    360
    361        if ((sequence == null) || (sequence.length != size)) {
    362          // all arrays should be reallocated with the right size
    363          sequence        = new int[size];
    364          costPerStep     = new int[size];
    365          coeff           = new double[size][];
    366          costPerTimeUnit = new double[size];
    367          optimalStep     = new double[size];
    368        }
    369
    370        if (requiresDenseOutput()) {
    371          // step size sequence: 2, 6, 10, 14, ...
    372          for (int k = 0; k < size; ++k) {
    373            sequence[k] = 4 * k + 2;
    374          }
    375        } else {
    376          // step size sequence: 2, 4, 6, 8, ...
    377          for (int k = 0; k < size; ++k) {
    378            sequence[k] = 2 * (k + 1);
    379          }
    380        }
    381
    382        // initialize the order selection cost array
    383        // (number of function calls for each column of the extrapolation table)
    384        costPerStep[0] = sequence[0] + 1;
    385        for (int k = 1; k < size; ++k) {
    386          costPerStep[k] = costPerStep[k-1] + sequence[k];
    387        }
    388
    389        // initialize the extrapolation tables
    390        for (int k = 0; k < size; ++k) {
    391          coeff[k] = (k > 0) ? new double[k] : null;
    392          for (int l = 0; l < k; ++l) {
    393            final double ratio = ((double) sequence[k]) / sequence[k-l-1];
    394            coeff[k][l] = 1.0 / (ratio * ratio - 1.0);
    395          }
    396        }
    397
    398      }
    399
    400      /** Set the interpolation order control parameter.
    401       * The interpolation order for dense output is 2k - mudif + 1. The
    402       * default value for mudif is 4 and the interpolation error is used
    403       * in stepsize control by default.
    404       *
    405       * @param useInterpolationErrorForControl if true, interpolation error is used
    406       * for stepsize control
    407       * @param mudifControlParameter interpolation order control parameter (the parameter
    408       * is reset to default if <= 0 or >= 7)
    409       */
    410      public void setInterpolationControl(final boolean useInterpolationErrorForControl,
    411                                          final int mudifControlParameter) {
    412
    413        this.useInterpolationError = useInterpolationErrorForControl;
    414
    415        if ((mudifControlParameter <= 0) || (mudifControlParameter >= 7)) {
    416          this.mudif = 4;
    417        } else {
    418          this.mudif = mudifControlParameter;
    419        }
    420
    421      }
    422
    423      /** Update scaling array.
    424       * @param y1 first state vector to use for scaling
    425       * @param y2 second state vector to use for scaling
    426       * @param scale scaling array to update (can be shorter than state)
    427       */
    428      private void rescale(final double[] y1, final double[] y2, final double[] scale) {
    429        if (vecAbsoluteTolerance == null) {
    430          for (int i = 0; i < scale.length; ++i) {
    431            final double yi = FastMath.max(FastMath.abs(y1[i]), FastMath.abs(y2[i]));
    432            scale[i] = scalAbsoluteTolerance + scalRelativeTolerance * yi;
    433          }
    434        } else {
    435          for (int i = 0; i < scale.length; ++i) {
    436            final double yi = FastMath.max(FastMath.abs(y1[i]), FastMath.abs(y2[i]));
    437            scale[i] = vecAbsoluteTolerance[i] + vecRelativeTolerance[i] * yi;
    438          }
    439        }
    440      }
    441
    442      /** Perform integration over one step using substeps of a modified
    443       * midpoint method.
    444       * @param t0 initial time
    445       * @param y0 initial value of the state vector at t0
    446       * @param step global step
    447       * @param k iteration number (from 0 to sequence.length - 1)
    448       * @param scale scaling array (can be shorter than state)
    449       * @param f placeholder where to put the state vector derivatives at each substep
    450       *          (element 0 already contains initial derivative)
    451       * @param yMiddle placeholder where to put the state vector at the middle of the step
    452       * @param yEnd placeholder where to put the state vector at the end
    453       * @param yTmp placeholder for one state vector
    454       * @return true if computation was done properly,
    455       *         false if stability check failed before end of computation
    456       * @throws DerivativeException this exception is propagated to the caller if the
    457       * underlying user function triggers one
    458       */
    459      private boolean tryStep(final double t0, final double[] y0, final double step, final int k,
    460                              final double[] scale, final double[][] f,
    461                              final double[] yMiddle, final double[] yEnd,
    462                              final double[] yTmp)
    463        throws DerivativeException {
    464
    465        final int    n        = sequence[k];
    466        final double subStep  = step / n;
    467        final double subStep2 = 2 * subStep;
    468
    469        // first substep
    470        double t = t0 + subStep;
    471        for (int i = 0; i < y0.length; ++i) {
    472          yTmp[i] = y0[i];
    473          yEnd[i] = y0[i] + subStep * f[0][i];
    474        }
    475        computeDerivatives(t, yEnd, f[1]);
    476
    477        // other substeps
    478        for (int j = 1; j < n; ++j) {
    479
    480          if (2 * j == n) {
    481            // save the point at the middle of the step
    482            System.arraycopy(yEnd, 0, yMiddle, 0, y0.length);
    483          }
    484
    485          t += subStep;
    486          for (int i = 0; i < y0.length; ++i) {
    487            final double middle = yEnd[i];
    488            yEnd[i]       = yTmp[i] + subStep2 * f[j][i];
    489            yTmp[i]       = middle;
    490          }
    491
    492          computeDerivatives(t, yEnd, f[j+1]);
    493
    494          // stability check
    495          if (performTest && (j <= maxChecks) && (k < maxIter)) {
    496            double initialNorm = 0.0;
    497            for (int l = 0; l < scale.length; ++l) {
    498              final double ratio = f[0][l] / scale[l];
    499              initialNorm += ratio * ratio;
    500            }
    501            double deltaNorm = 0.0;
    502            for (int l = 0; l < scale.length; ++l) {
    503              final double ratio = (f[j+1][l] - f[0][l]) / scale[l];
    504              deltaNorm += ratio * ratio;
    505            }
    506            if (deltaNorm > 4 * FastMath.max(1.0e-15, initialNorm)) {
    507              return false;
    508            }
    509          }
    510
    511        }
    512
    513        // correction of the last substep (at t0 + step)
    514        for (int i = 0; i < y0.length; ++i) {
    515          yEnd[i] = 0.5 * (yTmp[i] + yEnd[i] + subStep * f[n][i]);
    516        }
    517
    518        return true;
    519
    520      }
    521
    522      /** Extrapolate a vector.
    523       * @param offset offset to use in the coefficients table
    524       * @param k index of the last updated point
    525       * @param diag working diagonal of the Aitken-Neville's
    526       * triangle, without the last element
    527       * @param last last element
    528       */
    529      private void extrapolate(final int offset, final int k,
    530                               final double[][] diag, final double[] last) {
    531
    532        // update the diagonal
    533        for (int j = 1; j < k; ++j) {
    534          for (int i = 0; i < last.length; ++i) {
    535            // Aitken-Neville's recursive formula
    536            diag[k-j-1][i] = diag[k-j][i] +
    537                             coeff[k+offset][j-1] * (diag[k-j][i] - diag[k-j-1][i]);
    538          }
    539        }
    540
    541        // update the last element
    542        for (int i = 0; i < last.length; ++i) {
    543          // Aitken-Neville's recursive formula
    544          last[i] = diag[0][i] + coeff[k+offset][k-1] * (diag[0][i] - last[i]);
    545        }
    546      }
    547
    548      /** {@inheritDoc} */
    549      @Override
    550      public double integrate(final FirstOrderDifferentialEquations equations,
    551                              final double t0, final double[] y0, final double t, final double[] y)
    552          throws DerivativeException, IntegratorException {
    553
    554        sanityChecks(equations, t0, y0, t, y);
    555        setEquations(equations);
    556        resetEvaluations();
    557        final boolean forward = t > t0;
    558
    559        // create some internal working arrays
    560        final double[] yDot0   = new double[y0.length];
    561        final double[] y1      = new double[y0.length];
    562        final double[] yTmp    = new double[y0.length];
    563        final double[] yTmpDot = new double[y0.length];
    564
    565        final double[][] diagonal = new double[sequence.length-1][];
    566        final double[][] y1Diag = new double[sequence.length-1][];
    567        for (int k = 0; k < sequence.length-1; ++k) {
    568          diagonal[k] = new double[y0.length];
    569          y1Diag[k] = new double[y0.length];
    570        }
    571
    572        final double[][][] fk  = new double[sequence.length][][];
    573        for (int k = 0; k < sequence.length; ++k) {
    574
    575          fk[k]    = new double[sequence[k] + 1][];
    576
    577          // all substeps start at the same point, so share the first array
    578          fk[k][0] = yDot0;
    579
    580          for (int l = 0; l < sequence[k]; ++l) {
    581            fk[k][l+1] = new double[y0.length];
    582          }
    583
    584        }
    585
    586        if (y != y0) {
    587          System.arraycopy(y0, 0, y, 0, y0.length);
    588        }
    589
    590        double[] yDot1      = new double[y0.length];
    591        double[][] yMidDots = null;
    592        final boolean denseOutput = requiresDenseOutput();
    593        if (denseOutput) {
    594          yMidDots = new double[1 + 2 * sequence.length][];
    595          for (int j = 0; j < yMidDots.length; ++j) {
    596            yMidDots[j] = new double[y0.length];
    597          }
    598        } else {
    599          yMidDots    = new double[1][];
    600          yMidDots[0] = new double[y0.length];
    601        }
    602
    603        // initial scaling
    604        final double[] scale = new double[mainSetDimension];
    605        rescale(y, y, scale);
    606
    607        // initial order selection
    608        final double tol =
    609            (vecRelativeTolerance == null) ? scalRelativeTolerance : vecRelativeTolerance[0];
    610        final double log10R = FastMath.log10(FastMath.max(1.0e-10, tol));
    611        int targetIter = FastMath.max(1,
    612                                  FastMath.min(sequence.length - 2,
    613                                           (int) FastMath.floor(0.5 - 0.6 * log10R)));
    614        // set up an interpolator sharing the integrator arrays
    615        AbstractStepInterpolator interpolator = null;
    616        if (denseOutput) {
    617          interpolator = new GraggBulirschStoerStepInterpolator(y, yDot0,
    618                                                                y1, yDot1,
    619                                                                yMidDots, forward);
    620        } else {
    621          interpolator = new DummyStepInterpolator(y, yDot1, forward);
    622        }
    623        interpolator.storeTime(t0);
    624
    625        stepStart = t0;
    626        double  hNew             = 0;
    627        double  maxError         = Double.MAX_VALUE;
    628        boolean previousRejected = false;
    629        boolean firstTime        = true;
    630        boolean newStep          = true;
    631        boolean firstStepAlreadyComputed = false;
    632        for (StepHandler handler : stepHandlers) {
    633            handler.reset();
    634        }
    635        setStateInitialized(false);
    636        costPerTimeUnit[0] = 0;
    637        isLastStep = false;
    638        do {
    639
    640          double error;
    641          boolean reject = false;
    642
    643          if (newStep) {
    644
    645            interpolator.shift();
    646
    647            // first evaluation, at the beginning of the step
    648            if (! firstStepAlreadyComputed) {
    649              computeDerivatives(stepStart, y, yDot0);
    650            }
    651
    652            if (firstTime) {
    653              hNew = initializeStep(equations, forward,
    654                                    2 * targetIter + 1, scale,
    655                                    stepStart, y, yDot0, yTmp, yTmpDot);
    656            }
    657
    658            newStep = false;
    659
    660          }
    661
    662          stepSize = hNew;
    663
    664          // step adjustment near bounds
    665          if ((forward && (stepStart + stepSize > t)) ||
    666              ((! forward) && (stepStart + stepSize < t))) {
    667            stepSize = t - stepStart;
    668          }
    669          final double nextT = stepStart + stepSize;
    670          isLastStep = forward ? (nextT >= t) : (nextT <= t);
    671
    672          // iterate over several substep sizes
    673          int k = -1;
    674          for (boolean loop = true; loop; ) {
    675
    676            ++k;
    677
    678            // modified midpoint integration with the current substep
    679            if ( ! tryStep(stepStart, y, stepSize, k, scale, fk[k],
    680                           (k == 0) ? yMidDots[0] : diagonal[k-1],
    681                           (k == 0) ? y1 : y1Diag[k-1],
    682                           yTmp)) {
    683
    684              // the stability check failed, we reduce the global step
    685              hNew   = FastMath.abs(filterStep(stepSize * stabilityReduction, forward, false));
    686              reject = true;
    687              loop   = false;
    688
    689            } else {
    690
    691              // the substep was computed successfully
    692              if (k > 0) {
    693
    694                // extrapolate the state at the end of the step
    695                // using last iteration data
    696                extrapolate(0, k, y1Diag, y1);
    697                rescale(y, y1, scale);
    698
    699                // estimate the error at the end of the step.
    700                error = 0;
    701                for (int j = 0; j < mainSetDimension; ++j) {
    702                  final double e = FastMath.abs(y1[j] - y1Diag[0][j]) / scale[j];
    703                  error += e * e;
    704                }
    705                error = FastMath.sqrt(error / mainSetDimension);
    706
    707                if ((error > 1.0e15) || ((k > 1) && (error > maxError))) {
    708                  // error is too big, we reduce the global step
    709                  hNew   = FastMath.abs(filterStep(stepSize * stabilityReduction, forward, false));
    710                  reject = true;
    711                  loop   = false;
    712                } else {
    713
    714                  maxError = FastMath.max(4 * error, 1.0);
    715
    716                  // compute optimal stepsize for this order
    717                  final double exp = 1.0 / (2 * k + 1);
    718                  double fac = stepControl2 / FastMath.pow(error / stepControl1, exp);
    719                  final double pow = FastMath.pow(stepControl3, exp);
    720                  fac = FastMath.max(pow / stepControl4, FastMath.min(1 / pow, fac));
    721                  optimalStep[k]     = FastMath.abs(filterStep(stepSize * fac, forward, true));
    722                  costPerTimeUnit[k] = costPerStep[k] / optimalStep[k];
    723
    724                  // check convergence
    725                  switch (k - targetIter) {
    726
    727                  case -1 :
    728                    if ((targetIter > 1) && ! previousRejected) {
    729
    730                      // check if we can stop iterations now
    731                      if (error <= 1.0) {
    732                        // convergence have been reached just before targetIter
    733                        loop = false;
    734                      } else {
    735                        // estimate if there is a chance convergence will
    736                        // be reached on next iteration, using the
    737                        // asymptotic evolution of error
    738                        final double ratio = ((double) sequence [targetIter] * sequence[targetIter + 1]) /
    739                                             (sequence[0] * sequence[0]);
    740                        if (error > ratio * ratio) {
    741                          // we don't expect to converge on next iteration
    742                          // we reject the step immediately and reduce order
    743                          reject = true;
    744                          loop   = false;
    745                          targetIter = k;
    746                          if ((targetIter > 1) &&
    747                              (costPerTimeUnit[targetIter-1] <
    748                               orderControl1 * costPerTimeUnit[targetIter])) {
    749                            --targetIter;
    750                          }
    751                          hNew = optimalStep[targetIter];
    752                        }
    753                      }
    754                    }
    755                    break;
    756
    757                  case 0:
    758                    if (error <= 1.0) {
    759                      // convergence has been reached exactly at targetIter
    760                      loop = false;
    761                    } else {
    762                      // estimate if there is a chance convergence will
    763                      // be reached on next iteration, using the
    764                      // asymptotic evolution of error
    765                      final double ratio = ((double) sequence[k+1]) / sequence[0];
    766                      if (error > ratio * ratio) {
    767                        // we don't expect to converge on next iteration
    768                        // we reject the step immediately
    769                        reject = true;
    770                        loop = false;
    771                        if ((targetIter > 1) &&
    772                            (costPerTimeUnit[targetIter-1] <
    773                             orderControl1 * costPerTimeUnit[targetIter])) {
    774                          --targetIter;
    775                        }
    776                        hNew = optimalStep[targetIter];
    777                      }
    778                    }
    779                    break;
    780
    781                  case 1 :
    782                    if (error > 1.0) {
    783                      reject = true;
    784                      if ((targetIter > 1) &&
    785                          (costPerTimeUnit[targetIter-1] <
    786                           orderControl1 * costPerTimeUnit[targetIter])) {
    787                        --targetIter;
    788                      }
    789                      hNew = optimalStep[targetIter];
    790                    }
    791                    loop = false;
    792                    break;
    793
    794                  default :
    795                    if ((firstTime || isLastStep) && (error <= 1.0)) {
    796                      loop = false;
    797                    }
    798                    break;
    799
    800                  }
    801
    802                }
    803              }
    804            }
    805          }
    806
    807          if (! reject) {
    808              // derivatives at end of step
    809              computeDerivatives(stepStart + stepSize, y1, yDot1);
    810          }
    811
    812          // dense output handling
    813          double hInt = getMaxStep();
    814          if (denseOutput && ! reject) {
    815
    816            // extrapolate state at middle point of the step
    817            for (int j = 1; j <= k; ++j) {
    818              extrapolate(0, j, diagonal, yMidDots[0]);
    819            }
    820
    821            final int mu = 2 * k - mudif + 3;
    822
    823            for (int l = 0; l < mu; ++l) {
    824
    825              // derivative at middle point of the step
    826              final int l2 = l / 2;
    827              double factor = FastMath.pow(0.5 * sequence[l2], l);
    828              int middleIndex = fk[l2].length / 2;
    829              for (int i = 0; i < y0.length; ++i) {
    830                yMidDots[l+1][i] = factor * fk[l2][middleIndex + l][i];
    831              }
    832              for (int j = 1; j <= k - l2; ++j) {
    833                factor = FastMath.pow(0.5 * sequence[j + l2], l);
    834                middleIndex = fk[l2+j].length / 2;
    835                for (int i = 0; i < y0.length; ++i) {
    836                  diagonal[j-1][i] = factor * fk[l2+j][middleIndex+l][i];
    837                }
    838                extrapolate(l2, j, diagonal, yMidDots[l+1]);
    839              }
    840              for (int i = 0; i < y0.length; ++i) {
    841                yMidDots[l+1][i] *= stepSize;
    842              }
    843
    844              // compute centered differences to evaluate next derivatives
    845              for (int j = (l + 1) / 2; j <= k; ++j) {
    846                for (int m = fk[j].length - 1; m >= 2 * (l + 1); --m) {
    847                  for (int i = 0; i < y0.length; ++i) {
    848                    fk[j][m][i] -= fk[j][m-2][i];
    849                  }
    850                }
    851              }
    852
    853            }
    854
    855            if (mu >= 0) {
    856
    857              // estimate the dense output coefficients
    858              final GraggBulirschStoerStepInterpolator gbsInterpolator
    859                = (GraggBulirschStoerStepInterpolator) interpolator;
    860              gbsInterpolator.computeCoefficients(mu, stepSize);
    861
    862              if (useInterpolationError) {
    863                // use the interpolation error to limit stepsize
    864                final double interpError = gbsInterpolator.estimateError(scale);
    865                hInt = FastMath.abs(stepSize / FastMath.max(FastMath.pow(interpError, 1.0 / (mu+4)),
    866                                                    0.01));
    867                if (interpError > 10.0) {
    868                  hNew = hInt;
    869                  reject = true;
    870                }
    871              }
    872
    873            }
    874
    875          }
    876
    877          if (! reject) {
    878
    879            // Discrete events handling
    880            interpolator.storeTime(stepStart + stepSize);
    881            stepStart = acceptStep(interpolator, y1, yDot1, t);
    882
    883            // prepare next step
    884            interpolator.storeTime(stepStart);
    885            System.arraycopy(y1, 0, y, 0, y0.length);
    886            System.arraycopy(yDot1, 0, yDot0, 0, y0.length);
    887            firstStepAlreadyComputed = true;
    888
    889            int optimalIter;
    890            if (k == 1) {
    891              optimalIter = 2;
    892              if (previousRejected) {
    893                optimalIter = 1;
    894              }
    895            } else if (k <= targetIter) {
    896              optimalIter = k;
    897              if (costPerTimeUnit[k-1] < orderControl1 * costPerTimeUnit[k]) {
    898                optimalIter = k-1;
    899              } else if (costPerTimeUnit[k] < orderControl2 * costPerTimeUnit[k-1]) {
    900                optimalIter = FastMath.min(k+1, sequence.length - 2);
    901              }
    902            } else {
    903              optimalIter = k - 1;
    904              if ((k > 2) &&
    905                  (costPerTimeUnit[k-2] < orderControl1 * costPerTimeUnit[k-1])) {
    906                optimalIter = k - 2;
    907              }
    908              if (costPerTimeUnit[k] < orderControl2 * costPerTimeUnit[optimalIter]) {
    909                optimalIter = FastMath.min(k, sequence.length - 2);
    910              }
    911            }
    912
    913            if (previousRejected) {
    914              // after a rejected step neither order nor stepsize
    915              // should increase
    916              targetIter = FastMath.min(optimalIter, k);
    917              hNew = FastMath.min(FastMath.abs(stepSize), optimalStep[targetIter]);
    918            } else {
    919              // stepsize control
    920              if (optimalIter <= k) {
    921                hNew = optimalStep[optimalIter];
    922              } else {
    923                if ((k < targetIter) &&
    924                    (costPerTimeUnit[k] < orderControl2 * costPerTimeUnit[k-1])) {
    925                  hNew = filterStep(optimalStep[k] * costPerStep[optimalIter+1] / costPerStep[k],
    926                                   forward, false);
    927                } else {
    928                  hNew = filterStep(optimalStep[k] * costPerStep[optimalIter] / costPerStep[k],
    929                                    forward, false);
    930                }
    931              }
    932
    933              targetIter = optimalIter;
    934
    935            }
    936
    937            newStep = true;
    938
    939          }
    940
    941          hNew = FastMath.min(hNew, hInt);
    942          if (! forward) {
    943            hNew = -hNew;
    944          }
    945
    946          firstTime = false;
    947
    948          if (reject) {
    949            isLastStep = false;
    950            previousRejected = true;
    951          } else {
    952            previousRejected = false;
    953          }
    954
    955        } while (!isLastStep);
    956
    957        final double stopTime = stepStart;
    958        resetInternalState();
    959        return stopTime;
    960
    961      }
    962
    963    }