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    }