View Javadoc

1   /*
2    * Licensed to the Apache Software Foundation (ASF) under one or more
3    * contributor license agreements.  See the NOTICE file distributed with
4    * this work for additional information regarding copyright ownership.
5    * The ASF licenses this file to You under the Apache License, Version 2.0
6    * (the "License"); you may not use this file except in compliance with
7    * the License.  You may obtain a copy of the License at
8    *
9    *      http://www.apache.org/licenses/LICENSE-2.0
10   *
11   * Unless required by applicable law or agreed to in writing, software
12   * distributed under the License is distributed on an "AS IS" BASIS,
13   * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
14   * See the License for the specific language governing permissions and
15   * limitations under the License.
16   */
17  
18  package org.apache.commons.math.ode.nonstiff;
19  
20  import org.apache.commons.math.ode.DerivativeException;
21  import org.apache.commons.math.ode.FirstOrderDifferentialEquations;
22  import org.apache.commons.math.ode.IntegratorException;
23  import org.apache.commons.math.ode.events.EventHandler;
24  import org.apache.commons.math.ode.sampling.AbstractStepInterpolator;
25  import org.apache.commons.math.ode.sampling.DummyStepInterpolator;
26  import org.apache.commons.math.ode.sampling.StepHandler;
27  
28  /**
29   * This class implements a Gragg-Bulirsch-Stoer integrator for
30   * Ordinary Differential Equations.
31   *
32   * <p>The Gragg-Bulirsch-Stoer algorithm is one of the most efficient
33   * ones currently available for smooth problems. It uses Richardson
34   * extrapolation to estimate what would be the solution if the step
35   * size could be decreased down to zero.</p>
36   *
37   * <p>
38   * This method changes both the step size and the order during
39   * integration, in order to minimize computation cost. It is
40   * particularly well suited when a very high precision is needed. The
41   * limit where this method becomes more efficient than high-order
42   * embedded Runge-Kutta methods like {@link DormandPrince853Integrator
43   * Dormand-Prince 8(5,3)} depends on the problem. Results given in the
44   * Hairer, Norsett and Wanner book show for example that this limit
45   * occurs for accuracy around 1e-6 when integrating Saltzam-Lorenz
46   * equations (the authors note this problem is <i>extremely sensitive
47   * to the errors in the first integration steps</i>), and around 1e-11
48   * for a two dimensional celestial mechanics problems with seven
49   * bodies (pleiades problem, involving quasi-collisions for which
50   * <i>automatic step size control is essential</i>).
51   * </p>
52   *
53   * <p>
54   * This implementation is basically a reimplementation in Java of the
55   * <a
56   * href="http://www.unige.ch/math/folks/hairer/prog/nonstiff/odex.f">odex</a>
57   * fortran code by E. Hairer and G. Wanner. The redistribution policy
58   * for this code is available <a
59   * href="http://www.unige.ch/~hairer/prog/licence.txt">here</a>, for
60   * convenience, it is reproduced below.</p>
61   * </p>
62   *
63   * <table border="0" width="80%" cellpadding="10" align="center" bgcolor="#E0E0E0">
64   * <tr><td>Copyright (c) 2004, Ernst Hairer</td></tr>
65   *
66   * <tr><td>Redistribution and use in source and binary forms, with or
67   * without modification, are permitted provided that the following
68   * conditions are met:
69   * <ul>
70   *  <li>Redistributions of source code must retain the above copyright
71   *      notice, this list of conditions and the following disclaimer.</li>
72   *  <li>Redistributions in binary form must reproduce the above copyright
73   *      notice, this list of conditions and the following disclaimer in the
74   *      documentation and/or other materials provided with the distribution.</li>
75   * </ul></td></tr>
76   *
77   * <tr><td><strong>THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND
78   * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING,
79   * BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
80   * FOR A  PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE REGENTS OR
81   * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
82   * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
83   * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
84   * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
85   * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
86   * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
87   * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.</strong></td></tr>
88   * </table>
89   *
90   * @author E. Hairer and G. Wanner (fortran version)
91   * @version $Revision: 786881 $ $Date: 2009-06-20 14:53:08 -0400 (Sat, 20 Jun 2009) $
92   * @since 1.2
93   */
94  
95  public class GraggBulirschStoerIntegrator extends AdaptiveStepsizeIntegrator {
96  
97    /** Integrator method name. */
98    private static final String METHOD_NAME = "Gragg-Bulirsch-Stoer";
99  
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 }