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