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 * @author E. Hairer and G. Wanner (fortran version) 091 * @version $Revision: 786881 $ $Date: 2009-06-20 14:53:08 -0400 (Sat, 20 Jun 2009) $ 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 /** Simple constructor. 101 * Build a Gragg-Bulirsch-Stoer integrator with the given step 102 * bounds. All tuning parameters are set to their default 103 * values. The default step handler does nothing. 104 * @param minStep minimal step (must be positive even for backward 105 * integration), the last step can be smaller than this 106 * @param maxStep maximal step (must be positive even for backward 107 * integration) 108 * @param scalAbsoluteTolerance allowed absolute error 109 * @param scalRelativeTolerance allowed relative error 110 */ 111 public GraggBulirschStoerIntegrator(final double minStep, final double maxStep, 112 final double scalAbsoluteTolerance, 113 final double scalRelativeTolerance) { 114 super(METHOD_NAME, minStep, maxStep, 115 scalAbsoluteTolerance, scalRelativeTolerance); 116 denseOutput = requiresDenseOutput() || (! eventsHandlersManager.isEmpty()); 117 setStabilityCheck(true, -1, -1, -1); 118 setStepsizeControl(-1, -1, -1, -1); 119 setOrderControl(-1, -1, -1); 120 setInterpolationControl(true, -1); 121 } 122 123 /** Simple constructor. 124 * Build a Gragg-Bulirsch-Stoer integrator with the given step 125 * bounds. All tuning parameters are set to their default 126 * values. The default step handler does nothing. 127 * @param minStep minimal step (must be positive even for backward 128 * integration), the last step can be smaller than this 129 * @param maxStep maximal step (must be positive even for backward 130 * integration) 131 * @param vecAbsoluteTolerance allowed absolute error 132 * @param vecRelativeTolerance allowed relative error 133 */ 134 public GraggBulirschStoerIntegrator(final double minStep, final double maxStep, 135 final double[] vecAbsoluteTolerance, 136 final double[] vecRelativeTolerance) { 137 super(METHOD_NAME, minStep, maxStep, 138 vecAbsoluteTolerance, vecRelativeTolerance); 139 denseOutput = requiresDenseOutput() || (! eventsHandlersManager.isEmpty()); 140 setStabilityCheck(true, -1, -1, -1); 141 setStepsizeControl(-1, -1, -1, -1); 142 setOrderControl(-1, -1, -1); 143 setInterpolationControl(true, -1); 144 } 145 146 /** Set the stability check controls. 147 * <p>The stability check is performed on the first few iterations of 148 * the extrapolation scheme. If this test fails, the step is rejected 149 * and the stepsize is reduced.</p> 150 * <p>By default, the test is performed, at most during two 151 * iterations at each step, and at most once for each of these 152 * iterations. The default stepsize reduction factor is 0.5.</p> 153 * @param performTest if true, stability check will be performed, 154 if false, the check will be skipped 155 * @param maxIter maximal number of iterations for which checks are 156 * performed (the number of iterations is reset to default if negative 157 * or null) 158 * @param maxChecks maximal number of checks for each iteration 159 * (the number of checks is reset to default if negative or null) 160 * @param stabilityReduction stepsize reduction factor in case of 161 * failure (the factor is reset to default if lower than 0.0001 or 162 * greater than 0.9999) 163 */ 164 public void setStabilityCheck(final boolean performTest, 165 final int maxIter, final int maxChecks, 166 final double stabilityReduction) { 167 168 this.performTest = performTest; 169 this.maxIter = (maxIter <= 0) ? 2 : maxIter; 170 this.maxChecks = (maxChecks <= 0) ? 1 : maxChecks; 171 172 if ((stabilityReduction < 0.0001) || (stabilityReduction > 0.9999)) { 173 this.stabilityReduction = 0.5; 174 } else { 175 this.stabilityReduction = stabilityReduction; 176 } 177 178 } 179 180 /** Set the step size control factors. 181 182 * <p>The new step size hNew is computed from the old one h by: 183 * <pre> 184 * hNew = h * stepControl2 / (err/stepControl1)^(1/(2k+1)) 185 * </pre> 186 * where err is the scaled error and k the iteration number of the 187 * extrapolation scheme (counting from 0). The default values are 188 * 0.65 for stepControl1 and 0.94 for stepControl2.</p> 189 * <p>The step size is subject to the restriction: 190 * <pre> 191 * stepControl3^(1/(2k+1))/stepControl4 <= hNew/h <= 1/stepControl3^(1/(2k+1)) 192 * </pre> 193 * The default values are 0.02 for stepControl3 and 4.0 for 194 * stepControl4.</p> 195 * @param stepControl1 first stepsize control factor (the factor is 196 * reset to default if lower than 0.0001 or greater than 0.9999) 197 * @param stepControl2 second stepsize control factor (the factor 198 * is reset to default if lower than 0.0001 or greater than 0.9999) 199 * @param stepControl3 third stepsize control factor (the factor is 200 * reset to default if lower than 0.0001 or greater than 0.9999) 201 * @param stepControl4 fourth stepsize control factor (the factor 202 * is reset to default if lower than 1.0001 or greater than 999.9) 203 */ 204 public void setStepsizeControl(final double stepControl1, final double stepControl2, 205 final double stepControl3, final double stepControl4) { 206 207 if ((stepControl1 < 0.0001) || (stepControl1 > 0.9999)) { 208 this.stepControl1 = 0.65; 209 } else { 210 this.stepControl1 = stepControl1; 211 } 212 213 if ((stepControl2 < 0.0001) || (stepControl2 > 0.9999)) { 214 this.stepControl2 = 0.94; 215 } else { 216 this.stepControl2 = stepControl2; 217 } 218 219 if ((stepControl3 < 0.0001) || (stepControl3 > 0.9999)) { 220 this.stepControl3 = 0.02; 221 } else { 222 this.stepControl3 = stepControl3; 223 } 224 225 if ((stepControl4 < 1.0001) || (stepControl4 > 999.9)) { 226 this.stepControl4 = 4.0; 227 } else { 228 this.stepControl4 = stepControl4; 229 } 230 231 } 232 233 /** Set the order control parameters. 234 * <p>The Gragg-Bulirsch-Stoer method changes both the step size and 235 * the order during integration, in order to minimize computation 236 * cost. Each extrapolation step increases the order by 2, so the 237 * maximal order that will be used is always even, it is twice the 238 * maximal number of columns in the extrapolation table.</p> 239 * <pre> 240 * order is decreased if w(k-1) <= w(k) * orderControl1 241 * order is increased if w(k) <= w(k-1) * orderControl2 242 * </pre> 243 * <p>where w is the table of work per unit step for each order 244 * (number of function calls divided by the step length), and k is 245 * the current order.</p> 246 * <p>The default maximal order after construction is 18 (i.e. the 247 * maximal number of columns is 9). The default values are 0.8 for 248 * orderControl1 and 0.9 for orderControl2.</p> 249 * @param maxOrder maximal order in the extrapolation table (the 250 * maximal order is reset to default if order <= 6 or odd) 251 * @param orderControl1 first order control factor (the factor is 252 * reset to default if lower than 0.0001 or greater than 0.9999) 253 * @param orderControl2 second order control factor (the factor 254 * is reset to default if lower than 0.0001 or greater than 0.9999) 255 */ 256 public void setOrderControl(final int maxOrder, 257 final double orderControl1, final double orderControl2) { 258 259 if ((maxOrder <= 6) || (maxOrder % 2 != 0)) { 260 this.maxOrder = 18; 261 } 262 263 if ((orderControl1 < 0.0001) || (orderControl1 > 0.9999)) { 264 this.orderControl1 = 0.8; 265 } else { 266 this.orderControl1 = orderControl1; 267 } 268 269 if ((orderControl2 < 0.0001) || (orderControl2 > 0.9999)) { 270 this.orderControl2 = 0.9; 271 } else { 272 this.orderControl2 = orderControl2; 273 } 274 275 // reinitialize the arrays 276 initializeArrays(); 277 278 } 279 280 /** {@inheritDoc} */ 281 @Override 282 public void addStepHandler (final StepHandler handler) { 283 284 super.addStepHandler(handler); 285 denseOutput = requiresDenseOutput() || (! eventsHandlersManager.isEmpty()); 286 287 // reinitialize the arrays 288 initializeArrays(); 289 290 } 291 292 /** {@inheritDoc} */ 293 @Override 294 public void addEventHandler(final EventHandler function, 295 final double maxCheckInterval, 296 final double convergence, 297 final int maxIterationCount) { 298 super.addEventHandler(function, maxCheckInterval, convergence, maxIterationCount); 299 denseOutput = requiresDenseOutput() || (! eventsHandlersManager.isEmpty()); 300 301 // reinitialize the arrays 302 initializeArrays(); 303 304 } 305 306 /** Initialize the integrator internal arrays. */ 307 private void initializeArrays() { 308 309 final int size = maxOrder / 2; 310 311 if ((sequence == null) || (sequence.length != size)) { 312 // all arrays should be reallocated with the right size 313 sequence = new int[size]; 314 costPerStep = new int[size]; 315 coeff = new double[size][]; 316 costPerTimeUnit = new double[size]; 317 optimalStep = new double[size]; 318 } 319 320 if (denseOutput) { 321 // step size sequence: 2, 6, 10, 14, ... 322 for (int k = 0; k < size; ++k) { 323 sequence[k] = 4 * k + 2; 324 } 325 } else { 326 // step size sequence: 2, 4, 6, 8, ... 327 for (int k = 0; k < size; ++k) { 328 sequence[k] = 2 * (k + 1); 329 } 330 } 331 332 // initialize the order selection cost array 333 // (number of function calls for each column of the extrapolation table) 334 costPerStep[0] = sequence[0] + 1; 335 for (int k = 1; k < size; ++k) { 336 costPerStep[k] = costPerStep[k-1] + sequence[k]; 337 } 338 339 // initialize the extrapolation tables 340 for (int k = 0; k < size; ++k) { 341 coeff[k] = (k > 0) ? new double[k] : null; 342 for (int l = 0; l < k; ++l) { 343 final double ratio = ((double) sequence[k]) / sequence[k-l-1]; 344 coeff[k][l] = 1.0 / (ratio * ratio - 1.0); 345 } 346 } 347 348 } 349 350 /** Set the interpolation order control parameter. 351 * The interpolation order for dense output is 2k - mudif + 1. The 352 * default value for mudif is 4 and the interpolation error is used 353 * in stepsize control by default. 354 355 * @param useInterpolationError if true, interpolation error is used 356 * for stepsize control 357 * @param mudif interpolation order control parameter (the parameter 358 * is reset to default if <= 0 or >= 7) 359 */ 360 public void setInterpolationControl(final boolean useInterpolationError, 361 final int mudif) { 362 363 this.useInterpolationError = useInterpolationError; 364 365 if ((mudif <= 0) || (mudif >= 7)) { 366 this.mudif = 4; 367 } else { 368 this.mudif = mudif; 369 } 370 371 } 372 373 /** Update scaling array. 374 * @param y1 first state vector to use for scaling 375 * @param y2 second state vector to use for scaling 376 * @param scale scaling array to update 377 */ 378 private void rescale(final double[] y1, final double[] y2, final double[] scale) { 379 if (vecAbsoluteTolerance == null) { 380 for (int i = 0; i < scale.length; ++i) { 381 final double yi = Math.max(Math.abs(y1[i]), Math.abs(y2[i])); 382 scale[i] = scalAbsoluteTolerance + scalRelativeTolerance * yi; 383 } 384 } else { 385 for (int i = 0; i < scale.length; ++i) { 386 final double yi = Math.max(Math.abs(y1[i]), Math.abs(y2[i])); 387 scale[i] = vecAbsoluteTolerance[i] + vecRelativeTolerance[i] * yi; 388 } 389 } 390 } 391 392 /** Perform integration over one step using substeps of a modified 393 * midpoint method. 394 * @param t0 initial time 395 * @param y0 initial value of the state vector at t0 396 * @param step global step 397 * @param k iteration number (from 0 to sequence.length - 1) 398 * @param scale scaling array 399 * @param f placeholder where to put the state vector derivatives at each substep 400 * (element 0 already contains initial derivative) 401 * @param yMiddle placeholder where to put the state vector at the middle of the step 402 * @param yEnd placeholder where to put the state vector at the end 403 * @param yTmp placeholder for one state vector 404 * @return true if computation was done properly, 405 * false if stability check failed before end of computation 406 * @throws DerivativeException this exception is propagated to the caller if the 407 * underlying user function triggers one 408 */ 409 private boolean tryStep(final double t0, final double[] y0, final double step, final int k, 410 final double[] scale, final double[][] f, 411 final double[] yMiddle, final double[] yEnd, 412 final double[] yTmp) 413 throws DerivativeException { 414 415 final int n = sequence[k]; 416 final double subStep = step / n; 417 final double subStep2 = 2 * subStep; 418 419 // first substep 420 double t = t0 + subStep; 421 for (int i = 0; i < y0.length; ++i) { 422 yTmp[i] = y0[i]; 423 yEnd[i] = y0[i] + subStep * f[0][i]; 424 } 425 computeDerivatives(t, yEnd, f[1]); 426 427 // other substeps 428 for (int j = 1; j < n; ++j) { 429 430 if (2 * j == n) { 431 // save the point at the middle of the step 432 System.arraycopy(yEnd, 0, yMiddle, 0, y0.length); 433 } 434 435 t += subStep; 436 for (int i = 0; i < y0.length; ++i) { 437 final double middle = yEnd[i]; 438 yEnd[i] = yTmp[i] + subStep2 * f[j][i]; 439 yTmp[i] = middle; 440 } 441 442 computeDerivatives(t, yEnd, f[j+1]); 443 444 // stability check 445 if (performTest && (j <= maxChecks) && (k < maxIter)) { 446 double initialNorm = 0.0; 447 for (int l = 0; l < y0.length; ++l) { 448 final double ratio = f[0][l] / scale[l]; 449 initialNorm += ratio * ratio; 450 } 451 double deltaNorm = 0.0; 452 for (int l = 0; l < y0.length; ++l) { 453 final double ratio = (f[j+1][l] - f[0][l]) / scale[l]; 454 deltaNorm += ratio * ratio; 455 } 456 if (deltaNorm > 4 * Math.max(1.0e-15, initialNorm)) { 457 return false; 458 } 459 } 460 461 } 462 463 // correction of the last substep (at t0 + step) 464 for (int i = 0; i < y0.length; ++i) { 465 yEnd[i] = 0.5 * (yTmp[i] + yEnd[i] + subStep * f[n][i]); 466 } 467 468 return true; 469 470 } 471 472 /** Extrapolate a vector. 473 * @param offset offset to use in the coefficients table 474 * @param k index of the last updated point 475 * @param diag working diagonal of the Aitken-Neville's 476 * triangle, without the last element 477 * @param last last element 478 */ 479 private void extrapolate(final int offset, final int k, 480 final double[][] diag, final double[] last) { 481 482 // update the diagonal 483 for (int j = 1; j < k; ++j) { 484 for (int i = 0; i < last.length; ++i) { 485 // Aitken-Neville's recursive formula 486 diag[k-j-1][i] = diag[k-j][i] + 487 coeff[k+offset][j-1] * (diag[k-j][i] - diag[k-j-1][i]); 488 } 489 } 490 491 // update the last element 492 for (int i = 0; i < last.length; ++i) { 493 // Aitken-Neville's recursive formula 494 last[i] = diag[0][i] + coeff[k+offset][k-1] * (diag[0][i] - last[i]); 495 } 496 } 497 498 /** {@inheritDoc} */ 499 @Override 500 public double integrate(final FirstOrderDifferentialEquations equations, 501 final double t0, final double[] y0, final double t, final double[] y) 502 throws DerivativeException, IntegratorException { 503 504 sanityChecks(equations, t0, y0, t, y); 505 setEquations(equations); 506 resetEvaluations(); 507 final boolean forward = (t > t0); 508 509 // create some internal working arrays 510 final double[] yDot0 = new double[y0.length]; 511 final double[] y1 = new double[y0.length]; 512 final double[] yTmp = new double[y0.length]; 513 final double[] yTmpDot = new double[y0.length]; 514 515 final double[][] diagonal = new double[sequence.length-1][]; 516 final double[][] y1Diag = new double[sequence.length-1][]; 517 for (int k = 0; k < sequence.length-1; ++k) { 518 diagonal[k] = new double[y0.length]; 519 y1Diag[k] = new double[y0.length]; 520 } 521 522 final double[][][] fk = new double[sequence.length][][]; 523 for (int k = 0; k < sequence.length; ++k) { 524 525 fk[k] = new double[sequence[k] + 1][]; 526 527 // all substeps start at the same point, so share the first array 528 fk[k][0] = yDot0; 529 530 for (int l = 0; l < sequence[k]; ++l) { 531 fk[k][l+1] = new double[y0.length]; 532 } 533 534 } 535 536 if (y != y0) { 537 System.arraycopy(y0, 0, y, 0, y0.length); 538 } 539 540 double[] yDot1 = null; 541 double[][] yMidDots = null; 542 if (denseOutput) { 543 yDot1 = new double[y0.length]; 544 yMidDots = new double[1 + 2 * sequence.length][]; 545 for (int j = 0; j < yMidDots.length; ++j) { 546 yMidDots[j] = new double[y0.length]; 547 } 548 } else { 549 yMidDots = new double[1][]; 550 yMidDots[0] = new double[y0.length]; 551 } 552 553 // initial scaling 554 final double[] scale = new double[y0.length]; 555 rescale(y, y, scale); 556 557 // initial order selection 558 final double tol = 559 (vecRelativeTolerance == null) ? scalRelativeTolerance : vecRelativeTolerance[0]; 560 final double log10R = Math.log(Math.max(1.0e-10, tol)) / Math.log(10.0); 561 int targetIter = Math.max(1, 562 Math.min(sequence.length - 2, 563 (int) Math.floor(0.5 - 0.6 * log10R))); 564 // set up an interpolator sharing the integrator arrays 565 AbstractStepInterpolator interpolator = null; 566 if (denseOutput || (! eventsHandlersManager.isEmpty())) { 567 interpolator = new GraggBulirschStoerStepInterpolator(y, yDot0, 568 y1, yDot1, 569 yMidDots, forward); 570 } else { 571 interpolator = new DummyStepInterpolator(y, forward); 572 } 573 interpolator.storeTime(t0); 574 575 stepStart = t0; 576 double hNew = 0; 577 double maxError = Double.MAX_VALUE; 578 boolean previousRejected = false; 579 boolean firstTime = true; 580 boolean newStep = true; 581 boolean lastStep = false; 582 boolean firstStepAlreadyComputed = false; 583 for (StepHandler handler : stepHandlers) { 584 handler.reset(); 585 } 586 costPerTimeUnit[0] = 0; 587 while (! lastStep) { 588 589 double error; 590 boolean reject = false; 591 592 if (newStep) { 593 594 interpolator.shift(); 595 596 // first evaluation, at the beginning of the step 597 if (! firstStepAlreadyComputed) { 598 computeDerivatives(stepStart, y, yDot0); 599 } 600 601 if (firstTime) { 602 603 hNew = initializeStep(equations, forward, 604 2 * targetIter + 1, scale, 605 stepStart, y, yDot0, yTmp, yTmpDot); 606 607 if (! forward) { 608 hNew = -hNew; 609 } 610 611 } 612 613 newStep = false; 614 615 } 616 617 stepSize = hNew; 618 619 // step adjustment near bounds 620 if ((forward && (stepStart + stepSize > t)) || 621 ((! forward) && (stepStart + stepSize < t))) { 622 stepSize = t - stepStart; 623 } 624 final double nextT = stepStart + stepSize; 625 lastStep = forward ? (nextT >= t) : (nextT <= t); 626 627 // iterate over several substep sizes 628 int k = -1; 629 for (boolean loop = true; loop; ) { 630 631 ++k; 632 633 // modified midpoint integration with the current substep 634 if ( ! tryStep(stepStart, y, stepSize, k, scale, fk[k], 635 (k == 0) ? yMidDots[0] : diagonal[k-1], 636 (k == 0) ? y1 : y1Diag[k-1], 637 yTmp)) { 638 639 // the stability check failed, we reduce the global step 640 hNew = Math.abs(filterStep(stepSize * stabilityReduction, forward, false)); 641 reject = true; 642 loop = false; 643 644 } else { 645 646 // the substep was computed successfully 647 if (k > 0) { 648 649 // extrapolate the state at the end of the step 650 // using last iteration data 651 extrapolate(0, k, y1Diag, y1); 652 rescale(y, y1, scale); 653 654 // estimate the error at the end of the step. 655 error = 0; 656 for (int j = 0; j < y0.length; ++j) { 657 final double e = Math.abs(y1[j] - y1Diag[0][j]) / scale[j]; 658 error += e * e; 659 } 660 error = Math.sqrt(error / y0.length); 661 662 if ((error > 1.0e15) || ((k > 1) && (error > maxError))) { 663 // error is too big, we reduce the global step 664 hNew = Math.abs(filterStep(stepSize * stabilityReduction, forward, false)); 665 reject = true; 666 loop = false; 667 } else { 668 669 maxError = Math.max(4 * error, 1.0); 670 671 // compute optimal stepsize for this order 672 final double exp = 1.0 / (2 * k + 1); 673 double fac = stepControl2 / Math.pow(error / stepControl1, exp); 674 final double pow = Math.pow(stepControl3, exp); 675 fac = Math.max(pow / stepControl4, Math.min(1 / pow, fac)); 676 optimalStep[k] = Math.abs(filterStep(stepSize * fac, forward, true)); 677 costPerTimeUnit[k] = costPerStep[k] / optimalStep[k]; 678 679 // check convergence 680 switch (k - targetIter) { 681 682 case -1 : 683 if ((targetIter > 1) && ! previousRejected) { 684 685 // check if we can stop iterations now 686 if (error <= 1.0) { 687 // convergence have been reached just before targetIter 688 loop = false; 689 } else { 690 // estimate if there is a chance convergence will 691 // be reached on next iteration, using the 692 // asymptotic evolution of error 693 final double ratio = ((double) sequence [k] * sequence[k+1]) / 694 (sequence[0] * sequence[0]); 695 if (error > ratio * ratio) { 696 // we don't expect to converge on next iteration 697 // we reject the step immediately and reduce order 698 reject = true; 699 loop = false; 700 targetIter = k; 701 if ((targetIter > 1) && 702 (costPerTimeUnit[targetIter-1] < 703 orderControl1 * costPerTimeUnit[targetIter])) { 704 --targetIter; 705 } 706 hNew = optimalStep[targetIter]; 707 } 708 } 709 } 710 break; 711 712 case 0: 713 if (error <= 1.0) { 714 // convergence has been reached exactly at targetIter 715 loop = false; 716 } else { 717 // estimate if there is a chance convergence will 718 // be reached on next iteration, using the 719 // asymptotic evolution of error 720 final double ratio = ((double) sequence[k+1]) / sequence[0]; 721 if (error > ratio * ratio) { 722 // we don't expect to converge on next iteration 723 // we reject the step immediately 724 reject = true; 725 loop = false; 726 if ((targetIter > 1) && 727 (costPerTimeUnit[targetIter-1] < 728 orderControl1 * costPerTimeUnit[targetIter])) { 729 --targetIter; 730 } 731 hNew = optimalStep[targetIter]; 732 } 733 } 734 break; 735 736 case 1 : 737 if (error > 1.0) { 738 reject = true; 739 if ((targetIter > 1) && 740 (costPerTimeUnit[targetIter-1] < 741 orderControl1 * costPerTimeUnit[targetIter])) { 742 --targetIter; 743 } 744 hNew = optimalStep[targetIter]; 745 } 746 loop = false; 747 break; 748 749 default : 750 if ((firstTime || lastStep) && (error <= 1.0)) { 751 loop = false; 752 } 753 break; 754 755 } 756 757 } 758 } 759 } 760 } 761 762 // dense output handling 763 double hInt = getMaxStep(); 764 if (denseOutput && ! reject) { 765 766 // extrapolate state at middle point of the step 767 for (int j = 1; j <= k; ++j) { 768 extrapolate(0, j, diagonal, yMidDots[0]); 769 } 770 771 // derivative at end of step 772 computeDerivatives(stepStart + stepSize, y1, yDot1); 773 774 final int mu = 2 * k - mudif + 3; 775 776 for (int l = 0; l < mu; ++l) { 777 778 // derivative at middle point of the step 779 final int l2 = l / 2; 780 double factor = Math.pow(0.5 * sequence[l2], l); 781 int middleIndex = fk[l2].length / 2; 782 for (int i = 0; i < y0.length; ++i) { 783 yMidDots[l+1][i] = factor * fk[l2][middleIndex + l][i]; 784 } 785 for (int j = 1; j <= k - l2; ++j) { 786 factor = Math.pow(0.5 * sequence[j + l2], l); 787 middleIndex = fk[l2+j].length / 2; 788 for (int i = 0; i < y0.length; ++i) { 789 diagonal[j-1][i] = factor * fk[l2+j][middleIndex+l][i]; 790 } 791 extrapolate(l2, j, diagonal, yMidDots[l+1]); 792 } 793 for (int i = 0; i < y0.length; ++i) { 794 yMidDots[l+1][i] *= stepSize; 795 } 796 797 // compute centered differences to evaluate next derivatives 798 for (int j = (l + 1) / 2; j <= k; ++j) { 799 for (int m = fk[j].length - 1; m >= 2 * (l + 1); --m) { 800 for (int i = 0; i < y0.length; ++i) { 801 fk[j][m][i] -= fk[j][m-2][i]; 802 } 803 } 804 } 805 806 } 807 808 if (mu >= 0) { 809 810 // estimate the dense output coefficients 811 final GraggBulirschStoerStepInterpolator gbsInterpolator 812 = (GraggBulirschStoerStepInterpolator) interpolator; 813 gbsInterpolator.computeCoefficients(mu, stepSize); 814 815 if (useInterpolationError) { 816 // use the interpolation error to limit stepsize 817 final double interpError = gbsInterpolator.estimateError(scale); 818 hInt = Math.abs(stepSize / Math.max(Math.pow(interpError, 1.0 / (mu+4)), 819 0.01)); 820 if (interpError > 10.0) { 821 hNew = hInt; 822 reject = true; 823 } 824 } 825 826 // Discrete events handling 827 if (!reject) { 828 interpolator.storeTime(stepStart + stepSize); 829 if (eventsHandlersManager.evaluateStep(interpolator)) { 830 final double dt = eventsHandlersManager.getEventTime() - stepStart; 831 if (Math.abs(dt) > Math.ulp(stepStart)) { 832 // reject the step to match exactly the next switch time 833 hNew = Math.abs(dt); 834 reject = true; 835 } 836 } 837 } 838 839 } 840 841 if (!reject) { 842 // we will reuse the slope for the beginning of next step 843 firstStepAlreadyComputed = true; 844 System.arraycopy(yDot1, 0, yDot0, 0, y0.length); 845 } 846 847 } 848 849 if (! reject) { 850 851 // store end of step state 852 final double nextStep = stepStart + stepSize; 853 System.arraycopy(y1, 0, y, 0, y0.length); 854 855 eventsHandlersManager.stepAccepted(nextStep, y); 856 if (eventsHandlersManager.stop()) { 857 lastStep = true; 858 } 859 860 // provide the step data to the step handler 861 interpolator.storeTime(nextStep); 862 for (StepHandler handler : stepHandlers) { 863 handler.handleStep(interpolator, lastStep); 864 } 865 stepStart = nextStep; 866 867 if (eventsHandlersManager.reset(stepStart, y) && ! lastStep) { 868 // some switching function has triggered changes that 869 // invalidate the derivatives, we need to recompute them 870 firstStepAlreadyComputed = false; 871 } 872 873 int optimalIter; 874 if (k == 1) { 875 optimalIter = 2; 876 if (previousRejected) { 877 optimalIter = 1; 878 } 879 } else if (k <= targetIter) { 880 optimalIter = k; 881 if (costPerTimeUnit[k-1] < orderControl1 * costPerTimeUnit[k]) { 882 optimalIter = k-1; 883 } else if (costPerTimeUnit[k] < orderControl2 * costPerTimeUnit[k-1]) { 884 optimalIter = Math.min(k+1, sequence.length - 2); 885 } 886 } else { 887 optimalIter = k - 1; 888 if ((k > 2) && 889 (costPerTimeUnit[k-2] < orderControl1 * costPerTimeUnit[k-1])) { 890 optimalIter = k - 2; 891 } 892 if (costPerTimeUnit[k] < orderControl2 * costPerTimeUnit[optimalIter]) { 893 optimalIter = Math.min(k, sequence.length - 2); 894 } 895 } 896 897 if (previousRejected) { 898 // after a rejected step neither order nor stepsize 899 // should increase 900 targetIter = Math.min(optimalIter, k); 901 hNew = Math.min(Math.abs(stepSize), optimalStep[targetIter]); 902 } else { 903 // stepsize control 904 if (optimalIter <= k) { 905 hNew = optimalStep[optimalIter]; 906 } else { 907 if ((k < targetIter) && 908 (costPerTimeUnit[k] < orderControl2 * costPerTimeUnit[k-1])) { 909 hNew = filterStep(optimalStep[k] * costPerStep[optimalIter+1] / costPerStep[k], 910 forward, false); 911 } else { 912 hNew = filterStep(optimalStep[k] * costPerStep[optimalIter] / costPerStep[k], 913 forward, false); 914 } 915 } 916 917 targetIter = optimalIter; 918 919 } 920 921 newStep = true; 922 923 } 924 925 hNew = Math.min(hNew, hInt); 926 if (! forward) { 927 hNew = -hNew; 928 } 929 930 firstTime = false; 931 932 if (reject) { 933 lastStep = false; 934 previousRejected = true; 935 } else { 936 previousRejected = false; 937 } 938 939 } 940 941 return stepStart; 942 943 } 944 945 /** maximal order. */ 946 private int maxOrder; 947 948 /** step size sequence. */ 949 private int[] sequence; 950 951 /** overall cost of applying step reduction up to iteration k+1, 952 * in number of calls. 953 */ 954 private int[] costPerStep; 955 956 /** cost per unit step. */ 957 private double[] costPerTimeUnit; 958 959 /** optimal steps for each order. */ 960 private double[] optimalStep; 961 962 /** extrapolation coefficients. */ 963 private double[][] coeff; 964 965 /** stability check enabling parameter. */ 966 private boolean performTest; 967 968 /** maximal number of checks for each iteration. */ 969 private int maxChecks; 970 971 /** maximal number of iterations for which checks are performed. */ 972 private int maxIter; 973 974 /** stepsize reduction factor in case of stability check failure. */ 975 private double stabilityReduction; 976 977 /** first stepsize control factor. */ 978 private double stepControl1; 979 980 /** second stepsize control factor. */ 981 private double stepControl2; 982 983 /** third stepsize control factor. */ 984 private double stepControl3; 985 986 /** fourth stepsize control factor. */ 987 private double stepControl4; 988 989 /** first order control factor. */ 990 private double orderControl1; 991 992 /** second order control factor. */ 993 private double orderControl2; 994 995 /** dense outpute required. */ 996 private boolean denseOutput; 997 998 /** use interpolation error in stepsize control. */ 999 private boolean useInterpolationError; 1000 1001 /** interpolation order control parameter. */ 1002 private int mudif; 1003 1004 }