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
018package org.apache.commons.math3.ode.nonstiff;
019
020import org.apache.commons.math3.analysis.solvers.UnivariateSolver;
021import org.apache.commons.math3.exception.DimensionMismatchException;
022import org.apache.commons.math3.exception.MaxCountExceededException;
023import org.apache.commons.math3.exception.NoBracketingException;
024import org.apache.commons.math3.exception.NumberIsTooSmallException;
025import org.apache.commons.math3.ode.ExpandableStatefulODE;
026import org.apache.commons.math3.ode.events.EventHandler;
027import org.apache.commons.math3.ode.sampling.AbstractStepInterpolator;
028import org.apache.commons.math3.ode.sampling.StepHandler;
029import 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
097public 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}