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 }