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 }