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