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