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