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