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    }