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