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.math.ode.nonstiff; 19 20 import org.apache.commons.math.ode.DerivativeException; 21 import org.apache.commons.math.ode.FirstOrderDifferentialEquations; 22 import org.apache.commons.math.ode.IntegratorException; 23 import org.apache.commons.math.ode.events.EventHandler; 24 import org.apache.commons.math.ode.sampling.AbstractStepInterpolator; 25 import org.apache.commons.math.ode.sampling.DummyStepInterpolator; 26 import org.apache.commons.math.ode.sampling.StepHandler; 27 import org.apache.commons.math.util.FastMath; 28 29 /** 30 * This class implements a Gragg-Bulirsch-Stoer integrator for 31 * Ordinary Differential Equations. 32 * 33 * <p>The Gragg-Bulirsch-Stoer algorithm is one of the most efficient 34 * ones currently available for smooth problems. It uses Richardson 35 * extrapolation to estimate what would be the solution if the step 36 * size could be decreased down to zero.</p> 37 * 38 * <p> 39 * This method changes both the step size and the order during 40 * integration, in order to minimize computation cost. It is 41 * particularly well suited when a very high precision is needed. The 42 * limit where this method becomes more efficient than high-order 43 * embedded Runge-Kutta methods like {@link DormandPrince853Integrator 44 * Dormand-Prince 8(5,3)} depends on the problem. Results given in the 45 * Hairer, Norsett and Wanner book show for example that this limit 46 * occurs for accuracy around 1e-6 when integrating Saltzam-Lorenz 47 * equations (the authors note this problem is <i>extremely sensitive 48 * to the errors in the first integration steps</i>), and around 1e-11 49 * for a two dimensional celestial mechanics problems with seven 50 * bodies (pleiades problem, involving quasi-collisions for which 51 * <i>automatic step size control is essential</i>). 52 * </p> 53 * 54 * <p> 55 * This implementation is basically a reimplementation in Java of the 56 * <a 57 * href="http://www.unige.ch/math/folks/hairer/prog/nonstiff/odex.f">odex</a> 58 * fortran code by E. Hairer and G. Wanner. The redistribution policy 59 * for this code is available <a 60 * href="http://www.unige.ch/~hairer/prog/licence.txt">here</a>, for 61 * convenience, it is reproduced below.</p> 62 * </p> 63 * 64 * <table border="0" width="80%" cellpadding="10" align="center" bgcolor="#E0E0E0"> 65 * <tr><td>Copyright (c) 2004, Ernst Hairer</td></tr> 66 * 67 * <tr><td>Redistribution and use in source and binary forms, with or 68 * without modification, are permitted provided that the following 69 * conditions are met: 70 * <ul> 71 * <li>Redistributions of source code must retain the above copyright 72 * notice, this list of conditions and the following disclaimer.</li> 73 * <li>Redistributions in binary form must reproduce the above copyright 74 * notice, this list of conditions and the following disclaimer in the 75 * documentation and/or other materials provided with the distribution.</li> 76 * </ul></td></tr> 77 * 78 * <tr><td><strong>THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND 79 * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, 80 * BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 81 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE REGENTS OR 82 * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, 83 * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, 84 * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 85 * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 86 * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 87 * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 88 * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.</strong></td></tr> 89 * </table> 90 * 91 * @version $Revision: 1073158 $ $Date: 2011-02-21 22:46:52 +0100 (lun. 21 févr. 2011) $ 92 * @since 1.2 93 */ 94 95 public class GraggBulirschStoerIntegrator extends AdaptiveStepsizeIntegrator { 96 97 /** Integrator method name. */ 98 private static final String METHOD_NAME = "Gragg-Bulirsch-Stoer"; 99 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 */ GraggBulirschStoerIntegrator(final double minStep, final double maxStep, final double scalAbsoluteTolerance, final double scalRelativeTolerance)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 */ GraggBulirschStoerIntegrator(final double minStep, final double maxStep, final double[] vecAbsoluteTolerance, final double[] vecRelativeTolerance)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 */ setStabilityCheck(final boolean performStabilityCheck, final int maxNumIter, final int maxNumChecks, final double stepsizeReductionFactor)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 */ setStepsizeControl(final double control1, final double control2, final double control3, final double control4)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 */ setOrderControl(final int maximalOrder, final double control1, final double control2)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 addStepHandler(final StepHandler handler)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 addEventHandler(final EventHandler function, final double maxCheckInterval, final double convergence, final int maxIterationCount)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. */ initializeArrays()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 */ setInterpolationControl(final boolean useInterpolationErrorForControl, final int mudifControlParameter)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 */ rescale(final double[] y1, final double[] y2, final double[] scale)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 */ 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)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 */ extrapolate(final int offset, final int k, final double[][] diag, final double[] last)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 integrate(final FirstOrderDifferentialEquations equations, final double t0, final double[] y0, final double t, final double[] y)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 } 964