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