1 /*
2 * Licensed to the Hipparchus project 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 Hipparchus project 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 * https://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.hipparchus.ode.nonstiff;
19
20 import org.hipparchus.exception.MathIllegalArgumentException;
21 import org.hipparchus.exception.MathIllegalStateException;
22 import org.hipparchus.ode.ExpandableODE;
23 import org.hipparchus.ode.LocalizedODEFormats;
24 import org.hipparchus.ode.ODEState;
25 import org.hipparchus.ode.ODEStateAndDerivative;
26 import org.hipparchus.ode.nonstiff.interpolators.GraggBulirschStoerStateInterpolator;
27 import org.hipparchus.util.FastMath;
28
29 /**
30 * This class implements a Gragg-Bulirsch-Stoer integrator for
31 * Ordinary Differential Equations.
32 *
33 * <p>The Gragg-Bulirsch-Stoer algorithm is one of the most efficient
34 * ones currently available for smooth problems. It uses Richardson
35 * extrapolation to estimate what would be the solution if the step
36 * size could be decreased down to zero.</p>
37 *
38 * <p>
39 * This method changes both the step size and the order during
40 * integration, in order to minimize computation cost. It is
41 * particularly well suited when a very high precision is needed. The
42 * limit where this method becomes more efficient than high-order
43 * embedded Runge-Kutta methods like {@link DormandPrince853Integrator
44 * Dormand-Prince 8(5,3)} depends on the problem. Results given in the
45 * Hairer, Norsett and Wanner book show for example that this limit
46 * occurs for accuracy around 1e-6 when integrating Saltzam-Lorenz
47 * equations (the authors note this problem is <i>extremely sensitive
48 * to the errors in the first integration steps</i>), and around 1e-11
49 * for a two dimensional celestial mechanics problems with seven
50 * bodies (pleiades problem, involving quasi-collisions for which
51 * <i>automatic step size control is essential</i>).
52 * </p>
53 *
54 * <p>
55 * This implementation is basically a reimplementation in Java of the
56 * <a
57 * href="http://www.unige.ch/math/folks/hairer/prog/nonstiff/odex.f">odex</a>
58 * fortran code by E. Hairer and G. Wanner. The redistribution policy
59 * for this code is available <a
60 * href="http://www.unige.ch/~hairer/prog/licence.txt">here</a>, for
61 * convenience, it is reproduced below.</p>
62 *
63 * <blockquote>
64 * <p>Copyright (c) 2004, Ernst Hairer</p>
65 *
66 * <p>Redistribution and use in source and binary forms, with or
67 * without modification, are permitted provided that the following
68 * conditions are met:</p>
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>
76 *
77 * <p><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></p>
88 * </blockquote>
89 *
90 */
91
92 public class GraggBulirschStoerIntegrator extends AdaptiveStepsizeIntegrator {
93
94 /** Name of integration scheme. */
95 public static final String METHOD_NAME = "Gragg-Bulirsch-Stoer";
96
97 /** maximal order. */
98 private int maxOrder;
99
100 /** step size sequence. */
101 private int[] sequence;
102
103 /** overall cost of applying step reduction up to iteration k + 1, in number of calls. */
104 private int[] costPerStep;
105
106 /** cost per unit step. */
107 private double[] costPerTimeUnit;
108
109 /** optimal steps for each order. */
110 private double[] optimalStep;
111
112 /** extrapolation coefficients. */
113 private double[][] coeff;
114
115 /** stability check enabling parameter. */
116 private boolean performTest;
117
118 /** maximal number of checks for each iteration. */
119 private int maxChecks;
120
121 /** maximal number of iterations for which checks are performed. */
122 private int maxIter;
123
124 /** stepsize reduction factor in case of stability check failure. */
125 private double stabilityReduction;
126
127 /** first stepsize control factor. */
128 private double stepControl1;
129
130 /** second stepsize control factor. */
131 private double stepControl2;
132
133 /** third stepsize control factor. */
134 private double stepControl3;
135
136 /** fourth stepsize control factor. */
137 private double stepControl4;
138
139 /** first order control factor. */
140 private double orderControl1;
141
142 /** second order control factor. */
143 private double orderControl2;
144
145 /** use interpolation error in stepsize control. */
146 private boolean useInterpolationError;
147
148 /** interpolation order control parameter. */
149 private int mudif;
150
151 /** Simple constructor.
152 * Build a Gragg-Bulirsch-Stoer integrator with the given step
153 * bounds. All tuning parameters are set to their default
154 * values. The default step handler does nothing.
155 * @param minStep minimal step (sign is irrelevant, regardless of
156 * integration direction, forward or backward), the last step can
157 * be smaller than this
158 * @param maxStep maximal step (sign is irrelevant, regardless of
159 * integration direction, forward or backward), the last step can
160 * be smaller than this
161 * @param scalAbsoluteTolerance allowed absolute error
162 * @param scalRelativeTolerance allowed relative error
163 */
164 public GraggBulirschStoerIntegrator(final double minStep, final double maxStep,
165 final double scalAbsoluteTolerance,
166 final double scalRelativeTolerance) {
167 super(METHOD_NAME, minStep, maxStep,
168 scalAbsoluteTolerance, scalRelativeTolerance);
169 setStabilityCheck(true, -1, -1, -1);
170 setControlFactors(-1, -1, -1, -1);
171 setOrderControl(-1, -1, -1);
172 setInterpolationControl(true, -1);
173 }
174
175 /** Simple constructor.
176 * Build a Gragg-Bulirsch-Stoer integrator with the given step
177 * bounds. All tuning parameters are set to their default
178 * values. The default step handler does nothing.
179 * @param minStep minimal step (must be positive even for backward
180 * integration), the last step can be smaller than this
181 * @param maxStep maximal step (must be positive even for backward
182 * integration)
183 * @param vecAbsoluteTolerance allowed absolute error
184 * @param vecRelativeTolerance allowed relative error
185 */
186 public GraggBulirschStoerIntegrator(final double minStep, final double maxStep,
187 final double[] vecAbsoluteTolerance,
188 final double[] vecRelativeTolerance) {
189 super(METHOD_NAME, minStep, maxStep,
190 vecAbsoluteTolerance, vecRelativeTolerance);
191 setStabilityCheck(true, -1, -1, -1);
192 setControlFactors(-1, -1, -1, -1);
193 setOrderControl(-1, -1, -1);
194 setInterpolationControl(true, -1);
195 }
196
197 /** Set the stability check controls.
198 * <p>The stability check is performed on the first few iterations of
199 * the extrapolation scheme. If this test fails, the step is rejected
200 * and the stepsize is reduced.</p>
201 * <p>By default, the test is performed, at most during two
202 * iterations at each step, and at most once for each of these
203 * iterations. The default stepsize reduction factor is 0.5.</p>
204 * @param performStabilityCheck if true, stability check will be performed,
205 if false, the check will be skipped
206 * @param maxNumIter maximal number of iterations for which checks are
207 * performed (the number of iterations is reset to default if negative
208 * or null)
209 * @param maxNumChecks maximal number of checks for each iteration
210 * (the number of checks is reset to default if negative or null)
211 * @param stepsizeReductionFactor stepsize reduction factor in case of
212 * failure (the factor is reset to default if lower than 0.0001 or
213 * greater than 0.9999)
214 */
215 public void setStabilityCheck(final boolean performStabilityCheck,
216 final int maxNumIter, final int maxNumChecks,
217 final double stepsizeReductionFactor) {
218
219 this.performTest = performStabilityCheck;
220 this.maxIter = (maxNumIter <= 0) ? 2 : maxNumIter;
221 this.maxChecks = (maxNumChecks <= 0) ? 1 : maxNumChecks;
222
223 if ((stepsizeReductionFactor < 0.0001) || (stepsizeReductionFactor > 0.9999)) {
224 this.stabilityReduction = 0.5;
225 } else {
226 this.stabilityReduction = stepsizeReductionFactor;
227 }
228
229 }
230
231 /** Set the step size control factors.
232
233 * <p>The new step size hNew is computed from the old one h by:
234 * <pre>
235 * hNew = h * stepControl2 / (err/stepControl1)^(1/(2k + 1))
236 * </pre>
237 * <p>where err is the scaled error and k the iteration number of the
238 * extrapolation scheme (counting from 0). The default values are
239 * 0.65 for stepControl1 and 0.94 for stepControl2.</p>
240 * <p>The step size is subject to the restriction:</p>
241 * <pre>
242 * stepControl3^(1/(2k + 1))/stepControl4 <= hNew/h <= 1/stepControl3^(1/(2k + 1))
243 * </pre>
244 * <p>The default values are 0.02 for stepControl3 and 4.0 for
245 * stepControl4.</p>
246 * @param control1 first stepsize control factor (the factor is
247 * reset to default if lower than 0.0001 or greater than 0.9999)
248 * @param control2 second stepsize control factor (the factor
249 * is reset to default if lower than 0.0001 or greater than 0.9999)
250 * @param control3 third stepsize control factor (the factor is
251 * reset to default if lower than 0.0001 or greater than 0.9999)
252 * @param control4 fourth stepsize control factor (the factor
253 * is reset to default if lower than 1.0001 or greater than 999.9)
254 */
255 public void setControlFactors(final double control1, final double control2,
256 final double control3, final double control4) {
257
258 if ((control1 < 0.0001) || (control1 > 0.9999)) {
259 this.stepControl1 = 0.65;
260 } else {
261 this.stepControl1 = control1;
262 }
263
264 if ((control2 < 0.0001) || (control2 > 0.9999)) {
265 this.stepControl2 = 0.94;
266 } else {
267 this.stepControl2 = control2;
268 }
269
270 if ((control3 < 0.0001) || (control3 > 0.9999)) {
271 this.stepControl3 = 0.02;
272 } else {
273 this.stepControl3 = control3;
274 }
275
276 if ((control4 < 1.0001) || (control4 > 999.9)) {
277 this.stepControl4 = 4.0;
278 } else {
279 this.stepControl4 = control4;
280 }
281
282 }
283
284 /** Set the order control parameters.
285 * <p>The Gragg-Bulirsch-Stoer method changes both the step size and
286 * the order during integration, in order to minimize computation
287 * cost. Each extrapolation step increases the order by 2, so the
288 * maximal order that will be used is always even, it is twice the
289 * maximal number of columns in the extrapolation table.</p>
290 * <pre>
291 * order is decreased if w(k - 1) <= w(k) * orderControl1
292 * order is increased if w(k) <= w(k - 1) * orderControl2
293 * </pre>
294 * <p>where w is the table of work per unit step for each order
295 * (number of function calls divided by the step length), and k is
296 * the current order.</p>
297 * <p>The default maximal order after construction is 18 (i.e. the
298 * maximal number of columns is 9). The default values are 0.8 for
299 * orderControl1 and 0.9 for orderControl2.</p>
300 * @param maximalOrder maximal order in the extrapolation table (the
301 * maximal order is reset to default if order <= 6 or odd)
302 * @param control1 first order control factor (the factor is
303 * reset to default if lower than 0.0001 or greater than 0.9999)
304 * @param control2 second order control factor (the factor
305 * is reset to default if lower than 0.0001 or greater than 0.9999)
306 */
307 public void setOrderControl(final int maximalOrder,
308 final double control1, final double control2) {
309
310 if (maximalOrder > 6 && maximalOrder % 2 == 0) {
311 this.maxOrder = maximalOrder;
312 } else {
313 this.maxOrder = 18;
314 }
315
316 if ((control1 < 0.0001) || (control1 > 0.9999)) {
317 this.orderControl1 = 0.8;
318 } else {
319 this.orderControl1 = control1;
320 }
321
322 if ((control2 < 0.0001) || (control2 > 0.9999)) {
323 this.orderControl2 = 0.9;
324 } else {
325 this.orderControl2 = control2;
326 }
327
328 // reinitialize the arrays
329 initializeArrays();
330
331 }
332
333 /** Initialize the integrator internal arrays. */
334 private void initializeArrays() {
335
336 final int size = maxOrder / 2;
337
338 if ((sequence == null) || (sequence.length != size)) {
339 // all arrays should be reallocated with the right size
340 sequence = new int[size];
341 costPerStep = new int[size];
342 coeff = new double[size][];
343 costPerTimeUnit = new double[size];
344 optimalStep = new double[size];
345 }
346
347 // step size sequence: 2, 6, 10, 14, ...
348 for (int k = 0; k < size; ++k) {
349 sequence[k] = 4 * k + 2;
350 }
351
352 // initialize the order selection cost array
353 // (number of function calls for each column of the extrapolation table)
354 costPerStep[0] = sequence[0] + 1;
355 for (int k = 1; k < size; ++k) {
356 costPerStep[k] = costPerStep[k - 1] + sequence[k];
357 }
358
359 // initialize the extrapolation tables
360 for (int k = 0; k < size; ++k) {
361 coeff[k] = (k > 0) ? new double[k] : null;
362 for (int l = 0; l < k; ++l) {
363 final double ratio = ((double) sequence[k]) / sequence[k - l - 1];
364 coeff[k][l] = 1.0 / (ratio * ratio - 1.0);
365 }
366 }
367
368 }
369
370 /** Set the interpolation order control parameter.
371 * The interpolation order for dense output is 2k - mudif + 1. The
372 * default value for mudif is 4 and the interpolation error is used
373 * in stepsize control by default.
374
375 * @param useInterpolationErrorForControl if true, interpolation error is used
376 * for stepsize control
377 * @param mudifControlParameter interpolation order control parameter (the parameter
378 * is reset to default if <= 0 or >= 7)
379 */
380 public void setInterpolationControl(final boolean useInterpolationErrorForControl,
381 final int mudifControlParameter) {
382
383 this.useInterpolationError = useInterpolationErrorForControl;
384
385 if ((mudifControlParameter <= 0) || (mudifControlParameter >= 7)) {
386 this.mudif = 4;
387 } else {
388 this.mudif = mudifControlParameter;
389 }
390
391 }
392
393 /** Update scaling array.
394 * @param y1 first state vector to use for scaling
395 * @param y2 second state vector to use for scaling
396 * @param scale scaling array to update (can be shorter than state)
397 */
398 private void rescale(final double[] y1, final double[] y2, final double[] scale) {
399 final StepsizeHelper helper = getStepSizeHelper();
400 for (int i = 0; i < scale.length; ++i) {
401 scale[i] = helper.getTolerance(i, FastMath.max(FastMath.abs(y1[i]), FastMath.abs(y2[i])));
402 }
403 }
404
405 /** Perform integration over one step using substeps of a modified
406 * midpoint method.
407 * @param t0 initial time
408 * @param y0 initial value of the state vector at t0
409 * @param step global step
410 * @param k iteration number (from 0 to sequence.length - 1)
411 * @param scale scaling array (can be shorter than state)
412 * @param f placeholder where to put the state vector derivatives at each substep
413 * (element 0 already contains initial derivative)
414 * @param yMiddle placeholder where to put the state vector at the middle of the step
415 * @param yEnd placeholder where to put the state vector at the end
416 * @return true if computation was done properly,
417 * false if stability check failed before end of computation
418 * @exception MathIllegalStateException if the number of functions evaluations is exceeded
419 * @exception MathIllegalArgumentException if arrays dimensions do not match equations settings
420 */
421 private boolean tryStep(final double t0, final double[] y0, final double step, final int k,
422 final double[] scale, final double[][] f,
423 final double[] yMiddle, final double[] yEnd)
424 throws MathIllegalArgumentException, MathIllegalStateException {
425
426 final int n = sequence[k];
427 final double subStep = step / n;
428 final double subStep2 = 2 * subStep;
429
430 // first substep
431 double t = t0 + subStep;
432 for (int i = 0; i < y0.length; ++i) {
433 yEnd[i] = y0[i] + subStep * f[0][i];
434 }
435 f[1] = computeDerivatives(t, yEnd);
436
437 // other substeps
438 final double[] yTmp = y0.clone();
439 for (int j = 1; j < n; ++j) {
440
441 if (2 * j == n) {
442 // save the point at the middle of the step
443 System.arraycopy(yEnd, 0, yMiddle, 0, y0.length);
444 }
445
446 t += subStep;
447 for (int i = 0; i < y0.length; ++i) {
448 final double middle = yEnd[i];
449 yEnd[i] = yTmp[i] + subStep2 * f[j][i];
450 yTmp[i] = middle;
451 }
452
453 f[j + 1] = computeDerivatives(t, yEnd);
454
455 // stability check
456 if (performTest && (j <= maxChecks) && (k < maxIter)) {
457 double initialNorm = 0.0;
458 for (int l = 0; l < scale.length; ++l) {
459 final double ratio = f[0][l] / scale[l];
460 initialNorm += ratio * ratio;
461 }
462 double deltaNorm = 0.0;
463 for (int l = 0; l < scale.length; ++l) {
464 final double ratio = (f[j + 1][l] - f[0][l]) / scale[l];
465 deltaNorm += ratio * ratio;
466 }
467 if (deltaNorm > 4 * FastMath.max(1.0e-15, initialNorm)) {
468 return false;
469 }
470 }
471
472 }
473
474 // correction of the last substep (at t0 + step)
475 for (int i = 0; i < y0.length; ++i) {
476 yEnd[i] = 0.5 * (yTmp[i] + yEnd[i] + subStep * f[n][i]);
477 }
478
479 return true;
480
481 }
482
483 /** Extrapolate a vector.
484 * @param offset offset to use in the coefficients table
485 * @param k index of the last updated point
486 * @param diag working diagonal of the Aitken-Neville's
487 * triangle, without the last element
488 * @param last last element
489 */
490 private void extrapolate(final int offset, final int k,
491 final double[][] diag, final double[] last) {
492
493 // update the diagonal
494 for (int j = 1; j < k; ++j) {
495 for (int i = 0; i < last.length; ++i) {
496 // Aitken-Neville's recursive formula
497 diag[k - j - 1][i] = diag[k - j][i] +
498 coeff[k + offset][j - 1] * (diag[k - j][i] - diag[k - j - 1][i]);
499 }
500 }
501
502 // update the last element
503 for (int i = 0; i < last.length; ++i) {
504 // Aitken-Neville's recursive formula
505 last[i] = diag[0][i] + coeff[k + offset][k - 1] * (diag[0][i] - last[i]);
506 }
507 }
508
509 /** {@inheritDoc} */
510 @Override
511 public ODEStateAndDerivative integrate(final ExpandableODE equations,
512 final ODEState initialState, final double finalTime)
513 throws MathIllegalArgumentException, MathIllegalStateException {
514
515 sanityChecks(initialState, finalTime);
516 setStepStart(initIntegration(equations, initialState, finalTime));
517 final boolean forward = finalTime > initialState.getTime();
518
519 // create some internal working arrays
520 double[] y = getStepStart().getCompleteState();
521 final double[] y1 = new double[y.length];
522 final double[][] diagonal = new double[sequence.length - 1][];
523 final double[][] y1Diag = new double[sequence.length - 1][];
524 for (int k = 0; k < sequence.length - 1; ++k) {
525 diagonal[k] = new double[y.length];
526 y1Diag[k] = new double[y.length];
527 }
528
529 final double[][][] fk = new double[sequence.length][][];
530 for (int k = 0; k < sequence.length; ++k) {
531 fk[k] = new double[sequence[k] + 1][];
532 }
533
534 // scaled derivatives at the middle of the step $\tau$
535 // (element k is $h^{k} d^{k}y(\tau)/dt^{k}$ where h is step size...)
536 final double[][] yMidDots = new double[1 + 2 * sequence.length][y.length];
537
538 // initial scaling
539 final int mainSetDimension = getStepSizeHelper().getMainSetDimension();
540 final double[] scale = new double[mainSetDimension];
541 rescale(y, y, scale);
542
543 // initial order selection
544 final double tol = getStepSizeHelper().getRelativeTolerance(0);
545 final double log10R = FastMath.log10(FastMath.max(1.0e-10, tol));
546 int targetIter = FastMath.max(1,
547 FastMath.min(sequence.length - 2,
548 (int) FastMath.floor(0.5 - 0.6 * log10R)));
549
550 double hNew = 0;
551 double maxError = Double.MAX_VALUE;
552 boolean previousRejected = false;
553 boolean firstTime = true;
554 boolean newStep = true;
555 costPerTimeUnit[0] = 0;
556 setIsLastStep(false);
557 do {
558
559 double error;
560 boolean reject = false;
561
562 if (newStep) {
563
564 // first evaluation, at the beginning of the step
565 final double[] yDot0 = getStepStart().getCompleteDerivative();
566 for (int k = 0; k < sequence.length; ++k) {
567 // all sequences start from the same point, so we share the derivatives
568 fk[k][0] = yDot0;
569 }
570
571 if (firstTime) {
572 hNew = initializeStep(forward, 2 * targetIter + 1, scale,
573 getStepStart());
574 }
575
576 newStep = false;
577
578 }
579
580 setStepSize(hNew);
581
582 // step adjustment near bounds
583 if (forward) {
584 if (getStepStart().getTime() + getStepSize() >= finalTime) {
585 setStepSize(finalTime - getStepStart().getTime());
586 }
587 } else {
588 if (getStepStart().getTime() + getStepSize() <= finalTime) {
589 setStepSize(finalTime - getStepStart().getTime());
590 }
591 }
592 final double nextT = getStepStart().getTime() + getStepSize();
593 setIsLastStep(forward ? (nextT >= finalTime) : (nextT <= finalTime));
594
595 // iterate over several substep sizes
596 int k = -1;
597 for (boolean loop = true; loop; ) {
598
599 ++k;
600
601 // modified midpoint integration with the current substep
602 if ( ! tryStep(getStepStart().getTime(), y, getStepSize(), k, scale, fk[k],
603 (k == 0) ? yMidDots[0] : diagonal[k - 1],
604 (k == 0) ? y1 : y1Diag[k - 1])) {
605
606 // the stability check failed, we reduce the global step
607 hNew = FastMath.abs(getStepSizeHelper().filterStep(getStepSize() * stabilityReduction, forward, false));
608 reject = true;
609 loop = false;
610
611 } else {
612
613 // the substep was computed successfully
614 if (k > 0) {
615
616 // extrapolate the state at the end of the step
617 // using last iteration data
618 extrapolate(0, k, y1Diag, y1);
619 rescale(y, y1, scale);
620
621 // estimate the error at the end of the step.
622 error = 0;
623 for (int j = 0; j < mainSetDimension; ++j) {
624 final double e = FastMath.abs(y1[j] - y1Diag[0][j]) / scale[j];
625 error += e * e;
626 }
627 error = FastMath.sqrt(error / mainSetDimension);
628 if (Double.isNaN(error)) {
629 throw new MathIllegalStateException(LocalizedODEFormats.NAN_APPEARING_DURING_INTEGRATION,
630 nextT);
631 }
632
633 if ((error > 1.0e15) || ((k > 1) && (error > maxError))) {
634 // error is too big, we reduce the global step
635 hNew = FastMath.abs(getStepSizeHelper().filterStep(getStepSize() * stabilityReduction, forward, false));
636 reject = true;
637 loop = false;
638 } else {
639
640 maxError = FastMath.max(4 * error, 1.0);
641
642 // compute optimal stepsize for this order
643 final double exp = 1.0 / (2 * k + 1);
644 double fac = stepControl2 / FastMath.pow(error / stepControl1, exp);
645 final double pow = FastMath.pow(stepControl3, exp);
646 fac = FastMath.max(pow / stepControl4, FastMath.min(1 / pow, fac));
647 final boolean acceptSmall = k < targetIter;
648 optimalStep[k] = FastMath.abs(getStepSizeHelper().filterStep(getStepSize() * fac, forward, acceptSmall));
649 costPerTimeUnit[k] = costPerStep[k] / optimalStep[k];
650
651 // check convergence
652 switch (k - targetIter) {
653
654 case -1 :
655 if ((targetIter > 1) && ! previousRejected) {
656
657 // check if we can stop iterations now
658 if (error <= 1.0) {
659 // convergence have been reached just before targetIter
660 loop = false;
661 } else {
662 // estimate if there is a chance convergence will
663 // be reached on next iteration, using the
664 // asymptotic evolution of error
665 final double ratio = ((double) sequence [targetIter] * sequence[targetIter + 1]) /
666 (sequence[0] * sequence[0]);
667 if (error > ratio * ratio) {
668 // we don't expect to converge on next iteration
669 // we reject the step immediately and reduce order
670 reject = true;
671 loop = false;
672 targetIter = k;
673 if ((targetIter > 1) &&
674 (costPerTimeUnit[targetIter - 1] <
675 orderControl1 * costPerTimeUnit[targetIter])) {
676 --targetIter;
677 }
678 hNew = getStepSizeHelper().filterStep(optimalStep[targetIter], forward, false);
679 }
680 }
681 }
682 break;
683
684 case 0:
685 if (error <= 1.0) {
686 // convergence has been reached exactly at targetIter
687 loop = false;
688 } else {
689 // estimate if there is a chance convergence will
690 // be reached on next iteration, using the
691 // asymptotic evolution of error
692 final double ratio = ((double) sequence[k + 1]) / sequence[0];
693 if (error > ratio * ratio) {
694 // we don't expect to converge on next iteration
695 // we reject the step immediately
696 reject = true;
697 loop = false;
698 if ((targetIter > 1) &&
699 (costPerTimeUnit[targetIter - 1] <
700 orderControl1 * costPerTimeUnit[targetIter])) {
701 --targetIter;
702 }
703 hNew = getStepSizeHelper().filterStep(optimalStep[targetIter], forward, false);
704 }
705 }
706 break;
707
708 case 1 :
709 if (error > 1.0) {
710 reject = true;
711 if ((targetIter > 1) &&
712 (costPerTimeUnit[targetIter - 1] <
713 orderControl1 * costPerTimeUnit[targetIter])) {
714 --targetIter;
715 }
716 hNew = getStepSizeHelper().filterStep(optimalStep[targetIter], forward, false);
717 }
718 loop = false;
719 break;
720
721 default :
722 if ((firstTime || isLastStep()) && (error <= 1.0)) {
723 loop = false;
724 }
725 break;
726
727 }
728
729 }
730 }
731 }
732 }
733
734 // dense output handling
735 double hInt = getMaxStep();
736 final GraggBulirschStoerStateInterpolator interpolator;
737 if (! reject) {
738
739 // extrapolate state at middle point of the step
740 for (int j = 1; j <= k; ++j) {
741 extrapolate(0, j, diagonal, yMidDots[0]);
742 }
743
744 final int mu = 2 * k - mudif + 3;
745
746 for (int l = 0; l < mu; ++l) {
747
748 // derivative at middle point of the step
749 final int l2 = l / 2;
750 double factor = FastMath.pow(0.5 * sequence[l2], l);
751 int middleIndex = fk[l2].length / 2;
752 for (int i = 0; i < y.length; ++i) {
753 yMidDots[l + 1][i] = factor * fk[l2][middleIndex + l][i];
754 }
755 for (int j = 1; j <= k - l2; ++j) {
756 factor = FastMath.pow(0.5 * sequence[j + l2], l);
757 middleIndex = fk[l2 + j].length / 2;
758 for (int i = 0; i < y.length; ++i) {
759 diagonal[j - 1][i] = factor * fk[l2 + j][middleIndex + l][i];
760 }
761 extrapolate(l2, j, diagonal, yMidDots[l + 1]);
762 }
763 for (int i = 0; i < y.length; ++i) {
764 yMidDots[l + 1][i] *= getStepSize();
765 }
766
767 // compute centered differences to evaluate next derivatives
768 for (int j = (l + 1) / 2; j <= k; ++j) {
769 for (int m = fk[j].length - 1; m >= 2 * (l + 1); --m) {
770 for (int i = 0; i < y.length; ++i) {
771 fk[j][m][i] -= fk[j][m - 2][i];
772 }
773 }
774 }
775
776 }
777
778 // state at end of step
779 final ODEStateAndDerivative stepEnd =
780 equations.getMapper().mapStateAndDerivative(nextT, y1, computeDerivatives(nextT, y1));
781
782 // set up interpolator covering the full step
783 interpolator = new GraggBulirschStoerStateInterpolator(forward,
784 getStepStart(), stepEnd,
785 getStepStart(), stepEnd,
786 equations.getMapper(),
787 yMidDots, mu);
788
789 if (mu >= 0 && useInterpolationError) {
790 // use the interpolation error to limit stepsize
791 final double interpError = interpolator.estimateError(scale);
792 hInt = FastMath.abs(getStepSize() /
793 FastMath.max(FastMath.pow(interpError, 1.0 / (mu + 4)), 0.01));
794 if (interpError > 10.0) {
795 hNew = getStepSizeHelper().filterStep(hInt, forward, false);
796 reject = true;
797 }
798 }
799
800 } else {
801 interpolator = null;
802 }
803
804 if (! reject) {
805
806 // Discrete events handling
807 setStepStart(acceptStep(interpolator, finalTime));
808
809 // prepare next step
810 // beware that y1 is not always valid anymore here,
811 // as some event may have triggered a reset
812 // so we need to copy the new step start set previously
813 y = getStepStart().getCompleteState();
814
815 int optimalIter;
816 if (k == 1) {
817 optimalIter = 2;
818 if (previousRejected) {
819 optimalIter = 1;
820 }
821 } else if (k <= targetIter) {
822 optimalIter = k;
823 if (costPerTimeUnit[k - 1] < orderControl1 * costPerTimeUnit[k]) {
824 optimalIter = k - 1;
825 } else if (costPerTimeUnit[k] < orderControl2 * costPerTimeUnit[k - 1]) {
826 optimalIter = FastMath.min(k + 1, sequence.length - 2);
827 }
828 } else {
829 optimalIter = k - 1;
830 if ((k > 2) && (costPerTimeUnit[k - 2] < orderControl1 * costPerTimeUnit[k - 1])) {
831 optimalIter = k - 2;
832 }
833 if (costPerTimeUnit[k] < orderControl2 * costPerTimeUnit[optimalIter]) {
834 optimalIter = FastMath.min(k, sequence.length - 2);
835 }
836 }
837
838 if (previousRejected) {
839 // after a rejected step neither order nor stepsize
840 // should increase
841 targetIter = FastMath.min(optimalIter, k);
842 hNew = FastMath.min(FastMath.abs(getStepSize()), optimalStep[targetIter]);
843 } else {
844 // stepsize control
845 if (optimalIter <= k) {
846 hNew = getStepSizeHelper().filterStep(optimalStep[optimalIter], forward, false);
847 } else {
848 if ((k < targetIter) &&
849 (costPerTimeUnit[k] < orderControl2 * costPerTimeUnit[k - 1])) {
850 hNew = getStepSizeHelper().
851 filterStep(optimalStep[k] * costPerStep[optimalIter + 1] / costPerStep[k], forward, false);
852 } else {
853 hNew = getStepSizeHelper().
854 filterStep(optimalStep[k] * costPerStep[optimalIter] / costPerStep[k], forward, false);
855 }
856 }
857
858 targetIter = optimalIter;
859
860 }
861
862 newStep = true;
863
864 }
865
866 hNew = FastMath.min(hNew, hInt);
867 if (! forward) {
868 hNew = -hNew;
869 }
870
871 firstTime = false;
872
873 if (reject) {
874 setIsLastStep(false);
875 previousRejected = true;
876 } else {
877 previousRejected = false;
878 }
879
880 } while (!isLastStep());
881
882 final ODEStateAndDerivative finalState = getStepStart();
883 resetInternalState();
884 return finalState;
885
886 }
887
888 }