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     * @version $Revision: 919479 $ $Date: 2010-03-05 11:35:56 -0500 (Fri, 05 Mar 2010) $
091     * @since 1.2
092     */
093    
094    public class GraggBulirschStoerIntegrator extends AdaptiveStepsizeIntegrator {
095    
096        /** Integrator method name. */
097        private static final String METHOD_NAME = "Gragg-Bulirsch-Stoer";
098    
099        /** maximal order. */
100        private int maxOrder;
101    
102        /** step size sequence. */
103        private int[] sequence;
104    
105        /** overall cost of applying step reduction up to iteration k+1, in number of calls. */
106        private int[] costPerStep;
107    
108        /** cost per unit step. */
109        private double[] costPerTimeUnit;
110    
111        /** optimal steps for each order. */
112        private double[] optimalStep;
113    
114        /** extrapolation coefficients. */
115        private double[][] coeff;
116    
117        /** stability check enabling parameter. */
118        private boolean performTest;
119    
120        /** maximal number of checks for each iteration. */
121        private int maxChecks;
122    
123        /** maximal number of iterations for which checks are performed. */
124        private int maxIter;
125    
126        /** stepsize reduction factor in case of stability check failure. */
127        private double stabilityReduction;
128    
129        /** first stepsize control factor. */
130        private double stepControl1;
131    
132        /** second stepsize control factor. */
133        private double stepControl2;
134    
135        /** third stepsize control factor. */
136        private double stepControl3;
137    
138        /** fourth stepsize control factor. */
139        private double stepControl4;
140    
141        /** first order control factor. */
142        private double orderControl1;
143    
144        /** second order control factor. */
145        private double orderControl2;
146    
147        /** dense outpute required. */
148        private boolean denseOutput;
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 (must be positive even for backward
161       * integration), the last step can be smaller than this
162       * @param maxStep maximal step (must be positive even for backward
163       * integration)
164       * @param scalAbsoluteTolerance allowed absolute error
165       * @param scalRelativeTolerance allowed relative error
166       */
167      public GraggBulirschStoerIntegrator(final double minStep, final double maxStep,
168                                          final double scalAbsoluteTolerance,
169                                          final double scalRelativeTolerance) {
170        super(METHOD_NAME, minStep, maxStep,
171              scalAbsoluteTolerance, scalRelativeTolerance);
172        denseOutput = requiresDenseOutput() || (! eventsHandlersManager.isEmpty());
173        setStabilityCheck(true, -1, -1, -1);
174        setStepsizeControl(-1, -1, -1, -1);
175        setOrderControl(-1, -1, -1);
176        setInterpolationControl(true, -1);
177      }
178    
179      /** Simple constructor.
180       * Build a Gragg-Bulirsch-Stoer integrator with the given step
181       * bounds. All tuning parameters are set to their default
182       * values. The default step handler does nothing.
183       * @param minStep minimal step (must be positive even for backward
184       * integration), the last step can be smaller than this
185       * @param maxStep maximal step (must be positive even for backward
186       * integration)
187       * @param vecAbsoluteTolerance allowed absolute error
188       * @param vecRelativeTolerance allowed relative error
189       */
190      public GraggBulirschStoerIntegrator(final double minStep, final double maxStep,
191                                          final double[] vecAbsoluteTolerance,
192                                          final double[] vecRelativeTolerance) {
193        super(METHOD_NAME, minStep, maxStep,
194              vecAbsoluteTolerance, vecRelativeTolerance);
195        denseOutput = requiresDenseOutput() || (! eventsHandlersManager.isEmpty());
196        setStabilityCheck(true, -1, -1, -1);
197        setStepsizeControl(-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 setStepsizeControl(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        denseOutput = requiresDenseOutput() || (! eventsHandlersManager.isEmpty());
342    
343        // reinitialize the arrays
344        initializeArrays();
345    
346      }
347    
348      /** {@inheritDoc} */
349      @Override
350      public void addEventHandler(final EventHandler function,
351                                  final double maxCheckInterval,
352                                  final double convergence,
353                                  final int maxIterationCount) {
354        super.addEventHandler(function, maxCheckInterval, convergence, maxIterationCount);
355        denseOutput = requiresDenseOutput() || (! eventsHandlersManager.isEmpty());
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        if (denseOutput) {
377          // step size sequence: 2, 6, 10, 14, ...
378          for (int k = 0; k < size; ++k) {
379            sequence[k] = 4 * k + 2;
380          }
381        } else {
382          // step size sequence: 2, 4, 6, 8, ...
383          for (int k = 0; k < size; ++k) {
384            sequence[k] = 2 * (k + 1);
385          }
386        }
387    
388        // initialize the order selection cost array
389        // (number of function calls for each column of the extrapolation table)
390        costPerStep[0] = sequence[0] + 1;
391        for (int k = 1; k < size; ++k) {
392          costPerStep[k] = costPerStep[k-1] + sequence[k];
393        }
394    
395        // initialize the extrapolation tables
396        for (int k = 0; k < size; ++k) {
397          coeff[k] = (k > 0) ? new double[k] : null;
398          for (int l = 0; l < k; ++l) {
399            final double ratio = ((double) sequence[k]) / sequence[k-l-1];
400            coeff[k][l] = 1.0 / (ratio * ratio - 1.0);
401          }
402        }
403    
404      }
405    
406      /** Set the interpolation order control parameter.
407       * The interpolation order for dense output is 2k - mudif + 1. The
408       * default value for mudif is 4 and the interpolation error is used
409       * in stepsize control by default.
410    
411       * @param useInterpolationErrorForControl if true, interpolation error is used
412       * for stepsize control
413       * @param mudifControlParameter interpolation order control parameter (the parameter
414       * is reset to default if <= 0 or >= 7)
415       */
416      public void setInterpolationControl(final boolean useInterpolationErrorForControl,
417                                          final int mudifControlParameter) {
418    
419        this.useInterpolationError = useInterpolationErrorForControl;
420    
421        if ((mudifControlParameter <= 0) || (mudifControlParameter >= 7)) {
422          this.mudif = 4;
423        } else {
424          this.mudif = mudifControlParameter;
425        }
426    
427      }
428    
429      /** Update scaling array.
430       * @param y1 first state vector to use for scaling
431       * @param y2 second state vector to use for scaling
432       * @param scale scaling array to update
433       */
434      private void rescale(final double[] y1, final double[] y2, final double[] scale) {
435        if (vecAbsoluteTolerance == null) {
436          for (int i = 0; i < scale.length; ++i) {
437            final double yi = Math.max(Math.abs(y1[i]), Math.abs(y2[i]));
438            scale[i] = scalAbsoluteTolerance + scalRelativeTolerance * yi;
439          }
440        } else {
441          for (int i = 0; i < scale.length; ++i) {
442            final double yi = Math.max(Math.abs(y1[i]), Math.abs(y2[i]));
443            scale[i] = vecAbsoluteTolerance[i] + vecRelativeTolerance[i] * yi;
444          }
445        }
446      }
447    
448      /** Perform integration over one step using substeps of a modified
449       * midpoint method.
450       * @param t0 initial time
451       * @param y0 initial value of the state vector at t0
452       * @param step global step
453       * @param k iteration number (from 0 to sequence.length - 1)
454       * @param scale scaling array
455       * @param f placeholder where to put the state vector derivatives at each substep
456       *          (element 0 already contains initial derivative)
457       * @param yMiddle placeholder where to put the state vector at the middle of the step
458       * @param yEnd placeholder where to put the state vector at the end
459       * @param yTmp placeholder for one state vector
460       * @return true if computation was done properly,
461       *         false if stability check failed before end of computation
462       * @throws DerivativeException this exception is propagated to the caller if the
463       * underlying user function triggers one
464       */
465      private boolean tryStep(final double t0, final double[] y0, final double step, final int k,
466                              final double[] scale, final double[][] f,
467                              final double[] yMiddle, final double[] yEnd,
468                              final double[] yTmp)
469        throws DerivativeException {
470    
471        final int    n        = sequence[k];
472        final double subStep  = step / n;
473        final double subStep2 = 2 * subStep;
474    
475        // first substep
476        double t = t0 + subStep;
477        for (int i = 0; i < y0.length; ++i) {
478          yTmp[i] = y0[i];
479          yEnd[i] = y0[i] + subStep * f[0][i];
480        }
481        computeDerivatives(t, yEnd, f[1]);
482    
483        // other substeps
484        for (int j = 1; j < n; ++j) {
485    
486          if (2 * j == n) {
487            // save the point at the middle of the step
488            System.arraycopy(yEnd, 0, yMiddle, 0, y0.length);
489          }
490    
491          t += subStep;
492          for (int i = 0; i < y0.length; ++i) {
493            final double middle = yEnd[i];
494            yEnd[i]       = yTmp[i] + subStep2 * f[j][i];
495            yTmp[i]       = middle;
496          }
497    
498          computeDerivatives(t, yEnd, f[j+1]);
499    
500          // stability check
501          if (performTest && (j <= maxChecks) && (k < maxIter)) {
502            double initialNorm = 0.0;
503            for (int l = 0; l < y0.length; ++l) {
504              final double ratio = f[0][l] / scale[l];
505              initialNorm += ratio * ratio;
506            }
507            double deltaNorm = 0.0;
508            for (int l = 0; l < y0.length; ++l) {
509              final double ratio = (f[j+1][l] - f[0][l]) / scale[l];
510              deltaNorm += ratio * ratio;
511            }
512            if (deltaNorm > 4 * Math.max(1.0e-15, initialNorm)) {
513              return false;
514            }
515          }
516    
517        }
518    
519        // correction of the last substep (at t0 + step)
520        for (int i = 0; i < y0.length; ++i) {
521          yEnd[i] = 0.5 * (yTmp[i] + yEnd[i] + subStep * f[n][i]);
522        }
523    
524        return true;
525    
526      }
527    
528      /** Extrapolate a vector.
529       * @param offset offset to use in the coefficients table
530       * @param k index of the last updated point
531       * @param diag working diagonal of the Aitken-Neville's
532       * triangle, without the last element
533       * @param last last element
534       */
535      private void extrapolate(final int offset, final int k,
536                               final double[][] diag, final double[] last) {
537    
538        // update the diagonal
539        for (int j = 1; j < k; ++j) {
540          for (int i = 0; i < last.length; ++i) {
541            // Aitken-Neville's recursive formula
542            diag[k-j-1][i] = diag[k-j][i] +
543                             coeff[k+offset][j-1] * (diag[k-j][i] - diag[k-j-1][i]);
544          }
545        }
546    
547        // update the last element
548        for (int i = 0; i < last.length; ++i) {
549          // Aitken-Neville's recursive formula
550          last[i] = diag[0][i] + coeff[k+offset][k-1] * (diag[0][i] - last[i]);
551        }
552      }
553    
554      /** {@inheritDoc} */
555      @Override
556      public double integrate(final FirstOrderDifferentialEquations equations,
557                              final double t0, final double[] y0, final double t, final double[] y)
558      throws DerivativeException, IntegratorException {
559    
560        sanityChecks(equations, t0, y0, t, y);
561        setEquations(equations);
562        resetEvaluations();
563        final boolean forward = t > t0;
564    
565        // create some internal working arrays
566        final double[] yDot0   = new double[y0.length];
567        final double[] y1      = new double[y0.length];
568        final double[] yTmp    = new double[y0.length];
569        final double[] yTmpDot = new double[y0.length];
570    
571        final double[][] diagonal = new double[sequence.length-1][];
572        final double[][] y1Diag = new double[sequence.length-1][];
573        for (int k = 0; k < sequence.length-1; ++k) {
574          diagonal[k] = new double[y0.length];
575          y1Diag[k] = new double[y0.length];
576        }
577    
578        final double[][][] fk  = new double[sequence.length][][];
579        for (int k = 0; k < sequence.length; ++k) {
580    
581          fk[k]    = new double[sequence[k] + 1][];
582    
583          // all substeps start at the same point, so share the first array
584          fk[k][0] = yDot0;
585    
586          for (int l = 0; l < sequence[k]; ++l) {
587            fk[k][l+1] = new double[y0.length];
588          }
589    
590        }
591    
592        if (y != y0) {
593          System.arraycopy(y0, 0, y, 0, y0.length);
594        }
595    
596        double[] yDot1      = null;
597        double[][] yMidDots = null;
598        if (denseOutput) {
599          yDot1    = new double[y0.length];
600          yMidDots = new double[1 + 2 * sequence.length][];
601          for (int j = 0; j < yMidDots.length; ++j) {
602            yMidDots[j] = new double[y0.length];
603          }
604        } else {
605          yMidDots    = new double[1][];
606          yMidDots[0] = new double[y0.length];
607        }
608    
609        // initial scaling
610        final double[] scale = new double[y0.length];
611        rescale(y, y, scale);
612    
613        // initial order selection
614        final double tol =
615            (vecRelativeTolerance == null) ? scalRelativeTolerance : vecRelativeTolerance[0];
616        final double log10R = Math.log(Math.max(1.0e-10, tol)) / Math.log(10.0);
617        int targetIter = Math.max(1,
618                                  Math.min(sequence.length - 2,
619                                           (int) Math.floor(0.5 - 0.6 * log10R)));
620        // set up an interpolator sharing the integrator arrays
621        AbstractStepInterpolator interpolator = null;
622        if (denseOutput || (! eventsHandlersManager.isEmpty())) {
623          interpolator = new GraggBulirschStoerStepInterpolator(y, yDot0,
624                                                                y1, yDot1,
625                                                                yMidDots, forward);
626        } else {
627          interpolator = new DummyStepInterpolator(y, yDot1, forward);
628        }
629        interpolator.storeTime(t0);
630    
631        stepStart = t0;
632        double  hNew             = 0;
633        double  maxError         = Double.MAX_VALUE;
634        boolean previousRejected = false;
635        boolean firstTime        = true;
636        boolean newStep          = true;
637        boolean lastStep         = false;
638        boolean firstStepAlreadyComputed = false;
639        for (StepHandler handler : stepHandlers) {
640            handler.reset();
641        }
642        costPerTimeUnit[0] = 0;
643        while (! lastStep) {
644    
645          double error;
646          boolean reject = false;
647    
648          if (newStep) {
649    
650            interpolator.shift();
651    
652            // first evaluation, at the beginning of the step
653            if (! firstStepAlreadyComputed) {
654              computeDerivatives(stepStart, y, yDot0);
655            }
656    
657            if (firstTime) {
658    
659              hNew = initializeStep(equations, forward,
660                                    2 * targetIter + 1, scale,
661                                    stepStart, y, yDot0, yTmp, yTmpDot);
662    
663              if (! forward) {
664                hNew = -hNew;
665              }
666    
667            }
668    
669            newStep = false;
670    
671          }
672    
673          stepSize = hNew;
674    
675          // step adjustment near bounds
676          if ((forward && (stepStart + stepSize > t)) ||
677              ((! forward) && (stepStart + stepSize < t))) {
678            stepSize = t - stepStart;
679          }
680          final double nextT = stepStart + stepSize;
681          lastStep = forward ? (nextT >= t) : (nextT <= t);
682    
683          // iterate over several substep sizes
684          int k = -1;
685          for (boolean loop = true; loop; ) {
686    
687            ++k;
688    
689            // modified midpoint integration with the current substep
690            if ( ! tryStep(stepStart, y, stepSize, k, scale, fk[k],
691                           (k == 0) ? yMidDots[0] : diagonal[k-1],
692                           (k == 0) ? y1 : y1Diag[k-1],
693                           yTmp)) {
694    
695              // the stability check failed, we reduce the global step
696              hNew   = Math.abs(filterStep(stepSize * stabilityReduction, forward, false));
697              reject = true;
698              loop   = false;
699    
700            } else {
701    
702              // the substep was computed successfully
703              if (k > 0) {
704    
705                // extrapolate the state at the end of the step
706                // using last iteration data
707                extrapolate(0, k, y1Diag, y1);
708                rescale(y, y1, scale);
709    
710                // estimate the error at the end of the step.
711                error = 0;
712                for (int j = 0; j < y0.length; ++j) {
713                  final double e = Math.abs(y1[j] - y1Diag[0][j]) / scale[j];
714                  error += e * e;
715                }
716                error = Math.sqrt(error / y0.length);
717    
718                if ((error > 1.0e15) || ((k > 1) && (error > maxError))) {
719                  // error is too big, we reduce the global step
720                  hNew   = Math.abs(filterStep(stepSize * stabilityReduction, forward, false));
721                  reject = true;
722                  loop   = false;
723                } else {
724    
725                  maxError = Math.max(4 * error, 1.0);
726    
727                  // compute optimal stepsize for this order
728                  final double exp = 1.0 / (2 * k + 1);
729                  double fac = stepControl2 / Math.pow(error / stepControl1, exp);
730                  final double pow = Math.pow(stepControl3, exp);
731                  fac = Math.max(pow / stepControl4, Math.min(1 / pow, fac));
732                  optimalStep[k]     = Math.abs(filterStep(stepSize * fac, forward, true));
733                  costPerTimeUnit[k] = costPerStep[k] / optimalStep[k];
734    
735                  // check convergence
736                  switch (k - targetIter) {
737    
738                  case -1 :
739                    if ((targetIter > 1) && ! previousRejected) {
740    
741                      // check if we can stop iterations now
742                      if (error <= 1.0) {
743                        // convergence have been reached just before targetIter
744                        loop = false;
745                      } else {
746                        // estimate if there is a chance convergence will
747                        // be reached on next iteration, using the
748                        // asymptotic evolution of error
749                        final double ratio = ((double) sequence [targetIter] * sequence[targetIter + 1]) /
750                                             (sequence[0] * sequence[0]);
751                        if (error > ratio * ratio) {
752                          // we don't expect to converge on next iteration
753                          // we reject the step immediately and reduce order
754                          reject = true;
755                          loop   = false;
756                          targetIter = k;
757                          if ((targetIter > 1) &&
758                              (costPerTimeUnit[targetIter-1] <
759                               orderControl1 * costPerTimeUnit[targetIter])) {
760                            --targetIter;
761                          }
762                          hNew = optimalStep[targetIter];
763                        }
764                      }
765                    }
766                    break;
767    
768                  case 0:
769                    if (error <= 1.0) {
770                      // convergence has been reached exactly at targetIter
771                      loop = false;
772                    } else {
773                      // estimate if there is a chance convergence will
774                      // be reached on next iteration, using the
775                      // asymptotic evolution of error
776                      final double ratio = ((double) sequence[k+1]) / sequence[0];
777                      if (error > ratio * ratio) {
778                        // we don't expect to converge on next iteration
779                        // we reject the step immediately
780                        reject = true;
781                        loop = false;
782                        if ((targetIter > 1) &&
783                            (costPerTimeUnit[targetIter-1] <
784                             orderControl1 * costPerTimeUnit[targetIter])) {
785                          --targetIter;
786                        }
787                        hNew = optimalStep[targetIter];
788                      }
789                    }
790                    break;
791    
792                  case 1 :
793                    if (error > 1.0) {
794                      reject = true;
795                      if ((targetIter > 1) &&
796                          (costPerTimeUnit[targetIter-1] <
797                           orderControl1 * costPerTimeUnit[targetIter])) {
798                        --targetIter;
799                      }
800                      hNew = optimalStep[targetIter];
801                    }
802                    loop = false;
803                    break;
804    
805                  default :
806                    if ((firstTime || lastStep) && (error <= 1.0)) {
807                      loop = false;
808                    }
809                    break;
810    
811                  }
812    
813                }
814              }
815            }
816          }
817    
818          // dense output handling
819          double hInt = getMaxStep();
820          if (denseOutput && ! reject) {
821    
822            // extrapolate state at middle point of the step
823            for (int j = 1; j <= k; ++j) {
824              extrapolate(0, j, diagonal, yMidDots[0]);
825            }
826    
827            // derivative at end of step
828            computeDerivatives(stepStart + stepSize, y1, yDot1);
829    
830            final int mu = 2 * k - mudif + 3;
831    
832            for (int l = 0; l < mu; ++l) {
833    
834              // derivative at middle point of the step
835              final int l2 = l / 2;
836              double factor = Math.pow(0.5 * sequence[l2], l);
837              int middleIndex = fk[l2].length / 2;
838              for (int i = 0; i < y0.length; ++i) {
839                yMidDots[l+1][i] = factor * fk[l2][middleIndex + l][i];
840              }
841              for (int j = 1; j <= k - l2; ++j) {
842                factor = Math.pow(0.5 * sequence[j + l2], l);
843                middleIndex = fk[l2+j].length / 2;
844                for (int i = 0; i < y0.length; ++i) {
845                  diagonal[j-1][i] = factor * fk[l2+j][middleIndex+l][i];
846                }
847                extrapolate(l2, j, diagonal, yMidDots[l+1]);
848              }
849              for (int i = 0; i < y0.length; ++i) {
850                yMidDots[l+1][i] *= stepSize;
851              }
852    
853              // compute centered differences to evaluate next derivatives
854              for (int j = (l + 1) / 2; j <= k; ++j) {
855                for (int m = fk[j].length - 1; m >= 2 * (l + 1); --m) {
856                  for (int i = 0; i < y0.length; ++i) {
857                    fk[j][m][i] -= fk[j][m-2][i];
858                  }
859                }
860              }
861    
862            }
863    
864            if (mu >= 0) {
865    
866              // estimate the dense output coefficients
867              final GraggBulirschStoerStepInterpolator gbsInterpolator
868                = (GraggBulirschStoerStepInterpolator) interpolator;
869              gbsInterpolator.computeCoefficients(mu, stepSize);
870    
871              if (useInterpolationError) {
872                // use the interpolation error to limit stepsize
873                final double interpError = gbsInterpolator.estimateError(scale);
874                hInt = Math.abs(stepSize / Math.max(Math.pow(interpError, 1.0 / (mu+4)),
875                                                    0.01));
876                if (interpError > 10.0) {
877                  hNew = hInt;
878                  reject = true;
879                }
880              }
881    
882              // Discrete events handling
883              if (!reject) {
884                interpolator.storeTime(stepStart + stepSize);
885                if (eventsHandlersManager.evaluateStep(interpolator)) {
886                    final double dt = eventsHandlersManager.getEventTime() - stepStart;
887                    if (Math.abs(dt) > Math.ulp(stepStart)) {
888                        // reject the step to match exactly the next switch time
889                        hNew = Math.abs(dt);
890                        reject = true;
891                    }
892                }
893              }
894    
895            }
896    
897            if (!reject) {
898              // we will reuse the slope for the beginning of next step
899              firstStepAlreadyComputed = true;
900              System.arraycopy(yDot1, 0, yDot0, 0, y0.length);
901            }
902    
903          }
904    
905          if (! reject) {
906    
907            // store end of step state
908            final double nextStep = stepStart + stepSize;
909            System.arraycopy(y1, 0, y, 0, y0.length);
910    
911            eventsHandlersManager.stepAccepted(nextStep, y);
912            if (eventsHandlersManager.stop()) {
913              lastStep = true;
914            }
915    
916            // provide the step data to the step handler
917            interpolator.storeTime(nextStep);
918            for (StepHandler handler : stepHandlers) {
919                handler.handleStep(interpolator, lastStep);
920            }
921            stepStart = nextStep;
922    
923            if (eventsHandlersManager.reset(stepStart, y) && ! lastStep) {
924              // some switching function has triggered changes that
925              // invalidate the derivatives, we need to recompute them
926              firstStepAlreadyComputed = false;
927            }
928    
929            int optimalIter;
930            if (k == 1) {
931              optimalIter = 2;
932              if (previousRejected) {
933                optimalIter = 1;
934              }
935            } else if (k <= targetIter) {
936              optimalIter = k;
937              if (costPerTimeUnit[k-1] < orderControl1 * costPerTimeUnit[k]) {
938                optimalIter = k-1;
939              } else if (costPerTimeUnit[k] < orderControl2 * costPerTimeUnit[k-1]) {
940                optimalIter = Math.min(k+1, sequence.length - 2);
941              }
942            } else {
943              optimalIter = k - 1;
944              if ((k > 2) &&
945                  (costPerTimeUnit[k-2] < orderControl1 * costPerTimeUnit[k-1])) {
946                optimalIter = k - 2;
947              }
948              if (costPerTimeUnit[k] < orderControl2 * costPerTimeUnit[optimalIter]) {
949                optimalIter = Math.min(k, sequence.length - 2);
950              }
951            }
952    
953            if (previousRejected) {
954              // after a rejected step neither order nor stepsize
955              // should increase
956              targetIter = Math.min(optimalIter, k);
957              hNew = Math.min(Math.abs(stepSize), optimalStep[targetIter]);
958            } else {
959              // stepsize control
960              if (optimalIter <= k) {
961                hNew = optimalStep[optimalIter];
962              } else {
963                if ((k < targetIter) &&
964                    (costPerTimeUnit[k] < orderControl2 * costPerTimeUnit[k-1])) {
965                  hNew = filterStep(optimalStep[k] * costPerStep[optimalIter+1] / costPerStep[k],
966                                   forward, false);
967                } else {
968                  hNew = filterStep(optimalStep[k] * costPerStep[optimalIter] / costPerStep[k],
969                                    forward, false);
970                }
971              }
972    
973              targetIter = optimalIter;
974    
975            }
976    
977            newStep = true;
978    
979          }
980    
981          hNew = Math.min(hNew, hInt);
982          if (! forward) {
983            hNew = -hNew;
984          }
985    
986          firstTime = false;
987    
988          if (reject) {
989            lastStep = false;
990            previousRejected = true;
991          } else {
992            previousRejected = false;
993          }
994    
995        }
996    
997        return stepStart;
998    
999      }
1000    
1001    }