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