GraggBulirschStoerIntegrator.java

  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.  */

  18. package org.apache.commons.math4.legacy.ode.nonstiff;

  20. import org.apache.commons.math4.legacy.analysis.solvers.UnivariateSolver;
  21. import org.apache.commons.math4.legacy.exception.DimensionMismatchException;
  22. import org.apache.commons.math4.legacy.exception.MaxCountExceededException;
  23. import org.apache.commons.math4.legacy.exception.NoBracketingException;
  24. import org.apache.commons.math4.legacy.exception.NumberIsTooSmallException;
  25. import org.apache.commons.math4.legacy.ode.ExpandableStatefulODE;
  26. import org.apache.commons.math4.legacy.ode.events.EventHandler;
  27. import org.apache.commons.math4.legacy.ode.sampling.AbstractStepInterpolator;
  28. import org.apache.commons.math4.legacy.ode.sampling.StepHandler;
  29. import org.apache.commons.math4.core.jdkmath.JdkMath;

  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.  *
  65.  * <table border="" style="text-align: center; background-color: #E0E0E0">
  66.  * <caption>odex redistribution policy</caption>
  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.  */

  96. public class GraggBulirschStoerIntegrator extends AdaptiveStepsizeIntegrator {

  98.     /** Integrator method name. */
  99.     private static final String METHOD_NAME = "Gragg-Bulirsch-Stoer";

  101.     /** maximal order. */
  102.     private int maxOrder;

  104.     /** step size sequence. */
  105.     private int[] sequence;

  107.     /** overall cost of applying step reduction up to iteration k+1, in number of calls. */
  108.     private int[] costPerStep;

  110.     /** cost per unit step. */
  111.     private double[] costPerTimeUnit;

  113.     /** optimal steps for each order. */
  114.     private double[] optimalStep;

  116.     /** extrapolation coefficients. */
  117.     private double[][] coeff;

  119.     /** stability check enabling parameter. */
  120.     private boolean performTest;

  122.     /** maximal number of checks for each iteration. */
  123.     private int maxChecks;

  125.     /** maximal number of iterations for which checks are performed. */
  126.     private int maxIter;

  128.     /** stepsize reduction factor in case of stability check failure. */
  129.     private double stabilityReduction;

  131.     /** first stepsize control factor. */
  132.     private double stepControl1;

  134.     /** second stepsize control factor. */
  135.     private double stepControl2;

  137.     /** third stepsize control factor. */
  138.     private double stepControl3;

  140.     /** fourth stepsize control factor. */
  141.     private double stepControl4;

  143.     /** first order control factor. */
  144.     private double orderControl1;

  146.     /** second order control factor. */
  147.     private double orderControl2;

  149.     /** use interpolation error in stepsize control. */
  150.     private boolean useInterpolationError;

  152.     /** interpolation order control parameter. */
  153.     private int mudif;

  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.    */
  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.   }

  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.     setStabilityCheck(true, -1, -1, -1);
  196.     setControlFactors(-1, -1, -1, -1);
  197.     setOrderControl(-1, -1, -1);
  198.     setInterpolationControl(true, -1);
  199.   }

  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.    */
  219.   public void setStabilityCheck(final boolean performStabilityCheck,
  220.                                 final int maxNumIter, final int maxNumChecks,
  221.                                 final double stepsizeReductionFactor) {

  223.     this.performTest = performStabilityCheck;
  224.     this.maxIter     = (maxNumIter   <= 0) ? 2 : maxNumIter;
  225.     this.maxChecks   = (maxNumChecks <= 0) ? 1 : maxNumChecks;

  227.     if (stepsizeReductionFactor < 0.0001 || stepsizeReductionFactor > 0.9999) {
  228.       this.stabilityReduction = 0.5;
  229.     } else {
  230.       this.stabilityReduction = stepsizeReductionFactor;
  231.     }
  232.   }

  234.   /** Set the step size control factors.

  236.    * <p>The new step size hNew is computed from the old one h by:
  237.    * <pre>
  238.    * hNew = h * stepControl2 / (err/stepControl1)^(1/(2k+1))
  239.    * </pre>
  240.    * where err is the scaled error and k the iteration number of the
  241.    * extrapolation scheme (counting from 0). The default values are
  242.    * 0.65 for stepControl1 and 0.94 for stepControl2.
  243.    * <p>The step size is subject to the restriction:
  244.    * <pre>
  245.    * stepControl3^(1/(2k+1))/stepControl4 &lt;= hNew/h &lt;= 1/stepControl3^(1/(2k+1))
  246.    * </pre>
  247.    * The default values are 0.02 for stepControl3 and 4.0 for
  248.    * stepControl4.
  249.    * @param control1 first stepsize control factor (the factor is
  250.    * reset to default if lower than 0.0001 or greater than 0.9999)
  251.    * @param control2 second stepsize control factor (the factor
  252.    * is reset to default if lower than 0.0001 or greater than 0.9999)
  253.    * @param control3 third stepsize control factor (the factor is
  254.    * reset to default if lower than 0.0001 or greater than 0.9999)
  255.    * @param control4 fourth stepsize control factor (the factor
  256.    * is reset to default if lower than 1.0001 or greater than 999.9)
  257.    */
  258.   public void setControlFactors(final double control1, final double control2,
  259.                                 final double control3, final double control4) {

  261.     if (control1 < 0.0001 || control1 > 0.9999) {
  262.       this.stepControl1 = 0.65;
  263.     } else {
  264.       this.stepControl1 = control1;
  265.     }

  267.     if (control2 < 0.0001 || control2 > 0.9999) {
  268.       this.stepControl2 = 0.94;
  269.     } else {
  270.       this.stepControl2 = control2;
  271.     }

  273.     if (control3 < 0.0001 || control3 > 0.9999) {
  274.       this.stepControl3 = 0.02;
  275.     } else {
  276.       this.stepControl3 = control3;
  277.     }

  279.     if (control4 < 1.0001 || control4 > 999.9) {
  280.       this.stepControl4 = 4.0;
  281.     } else {
  282.       this.stepControl4 = control4;
  283.     }
  284.   }

  286.   /** Set the order control parameters.
  287.    * <p>The Gragg-Bulirsch-Stoer method changes both the step size and
  288.    * the order during integration, in order to minimize computation
  289.    * cost. Each extrapolation step increases the order by 2, so the
  290.    * maximal order that will be used is always even, it is twice the
  291.    * maximal number of columns in the extrapolation table.</p>
  292.    * <pre>
  293.    * order is decreased if w(k-1) &lt;= w(k)   * orderControl1
  294.    * order is increased if w(k)   &lt;= w(k-1) * orderControl2
  295.    * </pre>
  296.    * <p>where w is the table of work per unit step for each order
  297.    * (number of function calls divided by the step length), and k is
  298.    * the current order.</p>
  299.    * <p>The default maximal order after construction is 18 (i.e. the
  300.    * maximal number of columns is 9). The default values are 0.8 for
  301.    * orderControl1 and 0.9 for orderControl2.</p>
  302.    * @param maximalOrder maximal order in the extrapolation table (the
  303.    * maximal order is reset to default if order &lt;= 6 or odd)
  304.    * @param control1 first order control factor (the factor is
  305.    * reset to default if lower than 0.0001 or greater than 0.9999)
  306.    * @param control2 second order control factor (the factor
  307.    * is reset to default if lower than 0.0001 or greater than 0.9999)
  308.    */
  309.   public void setOrderControl(final int maximalOrder,
  310.                               final double control1, final double control2) {

  312.     if (maximalOrder <= 6 || (maximalOrder & 1) != 0) {
  313.       this.maxOrder = 18;
  314.     }

  316.     if (control1 < 0.0001 || control1 > 0.9999) {
  317.       this.orderControl1 = 0.8;
  318.     } else {
  319.       this.orderControl1 = control1;
  320.     }

  322.     if (control2 < 0.0001 || control2 > 0.9999) {
  323.       this.orderControl2 = 0.9;
  324.     } else {
  325.       this.orderControl2 = control2;
  326.     }

  328.     // reinitialize the arrays
  329.     initializeArrays();
  330.   }

  332.   /** {@inheritDoc} */
  333.   @Override
  334.   public void addStepHandler (final StepHandler handler) {

  336.     super.addStepHandler(handler);

  338.     // reinitialize the arrays
  339.     initializeArrays();
  340.   }

  342.   /** {@inheritDoc} */
  343.   @Override
  344.   public void addEventHandler(final EventHandler function,
  345.                               final double maxCheckInterval,
  346.                               final double convergence,
  347.                               final int maxIterationCount,
  348.                               final UnivariateSolver solver) {
  349.     super.addEventHandler(function, maxCheckInterval, convergence,
  350.                           maxIterationCount, solver);

  352.     // reinitialize the arrays
  353.     initializeArrays();
  354.   }

  356.   /** Initialize the integrator internal arrays. */
  357.   private void initializeArrays() {

  359.     final int size = maxOrder / 2;

  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.     }

  370.     // step size sequence: 2, 6, 10, 14, ...
  371.     for (int k = 0; k < size; ++k) {
  372.         sequence[k] = 4 * k + 2;
  373.     }

  375.     // initialize the order selection cost array
  376.     // (number of function calls for each column of the extrapolation table)
  377.     costPerStep[0] = sequence[0] + 1;
  378.     for (int k = 1; k < size; ++k) {
  379.       costPerStep[k] = costPerStep[k-1] + sequence[k];
  380.     }

  382.     // initialize the extrapolation tables
  383.     for (int k = 0; k < size; ++k) {
  384.       coeff[k] = (k > 0) ? new double[k] : null;
  385.       for (int l = 0; l < k; ++l) {
  386.         final double ratio = ((double) sequence[k]) / sequence[k-l-1];
  387.         coeff[k][l] = 1.0 / (ratio * ratio - 1.0);
  388.       }
  389.     }
  390.   }

  392.   /** Set the interpolation order control parameter.
  393.    * The interpolation order for dense output is 2k - mudif + 1. The
  394.    * default value for mudif is 4 and the interpolation error is used
  395.    * in stepsize control by default.

  397.    * @param useInterpolationErrorForControl if true, interpolation error is used
  398.    * for stepsize control
  399.    * @param mudifControlParameter interpolation order control parameter (the parameter
  400.    * is reset to default if &lt;= 0 or &gt;= 7)
  401.    */
  402.   public void setInterpolationControl(final boolean useInterpolationErrorForControl,
  403.                                       final int mudifControlParameter) {

  405.     this.useInterpolationError = useInterpolationErrorForControl;

  407.     if (mudifControlParameter <= 0 || mudifControlParameter >= 7) {
  408.       this.mudif = 4;
  409.     } else {
  410.       this.mudif = mudifControlParameter;
  411.     }
  412.   }

  414.   /** Update scaling array.
  415.    * @param y1 first state vector to use for scaling
  416.    * @param y2 second state vector to use for scaling
  417.    * @param scale scaling array to update (can be shorter than state)
  418.    */
  419.   private void rescale(final double[] y1, final double[] y2, final double[] scale) {
  420.     if (vecAbsoluteTolerance == null) {
  421.       for (int i = 0; i < scale.length; ++i) {
  422.         final double yi = JdkMath.max(JdkMath.abs(y1[i]), JdkMath.abs(y2[i]));
  423.         scale[i] = scalAbsoluteTolerance + scalRelativeTolerance * yi;
  424.       }
  425.     } else {
  426.       for (int i = 0; i < scale.length; ++i) {
  427.         final double yi = JdkMath.max(JdkMath.abs(y1[i]), JdkMath.abs(y2[i]));
  428.         scale[i] = vecAbsoluteTolerance[i] + vecRelativeTolerance[i] * yi;
  429.       }
  430.     }
  431.   }

  433.   /** Perform integration over one step using substeps of a modified
  434.    * midpoint method.
  435.    * @param t0 initial time
  436.    * @param y0 initial value of the state vector at t0
  437.    * @param step global step
  438.    * @param k iteration number (from 0 to sequence.length - 1)
  439.    * @param scale scaling array (can be shorter than state)
  440.    * @param f placeholder where to put the state vector derivatives at each substep
  441.    *          (element 0 already contains initial derivative)
  442.    * @param yMiddle placeholder where to put the state vector at the middle of the step
  443.    * @param yEnd placeholder where to put the state vector at the end
  444.    * @param yTmp placeholder for one state vector
  445.    * @return true if computation was done properly,
  446.    *         false if stability check failed before end of computation
  447.    * @exception MaxCountExceededException if the number of functions evaluations is exceeded
  448.    * @exception DimensionMismatchException if arrays dimensions do not match equations settings
  449.    */
  450.   private boolean tryStep(final double t0, final double[] y0, final double step, final int k,
  451.                           final double[] scale, final double[][] f,
  452.                           final double[] yMiddle, final double[] yEnd,
  453.                           final double[] yTmp)
  454.       throws MaxCountExceededException, DimensionMismatchException {

  456.     final int    n        = sequence[k];
  457.     final double subStep  = step / n;
  458.     final double subStep2 = 2 * subStep;

  460.     // first substep
  461.     double t = t0 + subStep;
  462.     for (int i = 0; i < y0.length; ++i) {
  463.       yTmp[i] = y0[i];
  464.       yEnd[i] = y0[i] + subStep * f[0][i];
  465.     }
  466.     computeDerivatives(t, yEnd, f[1]);

  468.     // other substeps
  469.     for (int j = 1; j < n; ++j) {

  471.       if (2 * j == n) {
  472.         // save the point at the middle of the step
  473.         System.arraycopy(yEnd, 0, yMiddle, 0, y0.length);
  474.       }

  476.       t += subStep;
  477.       for (int i = 0; i < y0.length; ++i) {
  478.         final double middle = yEnd[i];
  479.         yEnd[i]       = yTmp[i] + subStep2 * f[j][i];
  480.         yTmp[i]       = middle;
  481.       }

  483.       computeDerivatives(t, yEnd, f[j+1]);

  485.       // stability check
  486.       if (performTest && j <= maxChecks && k < maxIter) {
  487.         double initialNorm = 0.0;
  488.         for (int l = 0; l < scale.length; ++l) {
  489.           final double ratio = f[0][l] / scale[l];
  490.           initialNorm += ratio * ratio;
  491.         }
  492.         double deltaNorm = 0.0;
  493.         for (int l = 0; l < scale.length; ++l) {
  494.           final double ratio = (f[j+1][l] - f[0][l]) / scale[l];
  495.           deltaNorm += ratio * ratio;
  496.         }
  497.         if (deltaNorm > 4 * JdkMath.max(1.0e-15, initialNorm)) {
  498.           return false;
  499.         }
  500.       }
  501.     }

  503.     // correction of the last substep (at t0 + step)
  504.     for (int i = 0; i < y0.length; ++i) {
  505.       yEnd[i] = 0.5 * (yTmp[i] + yEnd[i] + subStep * f[n][i]);
  506.     }

  508.     return true;
  509.   }

  511.   /** Extrapolate a vector.
  512.    * @param offset offset to use in the coefficients table
  513.    * @param k index of the last updated point
  514.    * @param diag working diagonal of the Aitken-Neville's
  515.    * triangle, without the last element
  516.    * @param last last element
  517.    */
  518.   private void extrapolate(final int offset, final int k,
  519.                            final double[][] diag, final double[] last) {

  521.     // update the diagonal
  522.     for (int j = 1; j < k; ++j) {
  523.       for (int i = 0; i < last.length; ++i) {
  524.         // Aitken-Neville's recursive formula
  525.         diag[k-j-1][i] = diag[k-j][i] +
  526.                          coeff[k+offset][j-1] * (diag[k-j][i] - diag[k-j-1][i]);
  527.       }
  528.     }

  530.     // update the last element
  531.     for (int i = 0; i < last.length; ++i) {
  532.       // Aitken-Neville's recursive formula
  533.       last[i] = diag[0][i] + coeff[k+offset][k-1] * (diag[0][i] - last[i]);
  534.     }
  535.   }

  537.   /** {@inheritDoc} */
  538.   @Override
  539.   public void integrate(final ExpandableStatefulODE equations, final double t)
  540.       throws NumberIsTooSmallException, DimensionMismatchException,
  541.              MaxCountExceededException, NoBracketingException {

  543.     sanityChecks(equations, t);
  544.     setEquations(equations);
  545.     final boolean forward = t > equations.getTime();

  547.     // create some internal working arrays
  548.     final double[] y0      = equations.getCompleteState();
  549.     final double[] y       = y0.clone();
  550.     final double[] yDot0   = new double[y.length];
  551.     final double[] y1      = new double[y.length];
  552.     final double[] yTmp    = new double[y.length];
  553.     final double[] yTmpDot = new double[y.length];

  555.     final double[][] diagonal = new double[sequence.length-1][];
  556.     final double[][] y1Diag = new double[sequence.length-1][];
  557.     for (int k = 0; k < sequence.length-1; ++k) {
  558.       diagonal[k] = new double[y.length];
  559.       y1Diag[k] = new double[y.length];
  560.     }

  562.     final double[][][] fk  = new double[sequence.length][][];
  563.     for (int k = 0; k < sequence.length; ++k) {

  565.       fk[k]    = new double[sequence[k] + 1][];

  567.       // all substeps start at the same point, so share the first array
  568.       fk[k][0] = yDot0;

  570.       for (int l = 0; l < sequence[k]; ++l) {
  571.         fk[k][l+1] = new double[y0.length];
  572.       }
  573.     }

  575.     if (y != y0) {
  576.       System.arraycopy(y0, 0, y, 0, y0.length);
  577.     }

  579.     final double[] yDot1 = new double[y0.length];
  580.     final double[][] yMidDots = new double[1 + 2 * sequence.length][y0.length];

  582.     // initial scaling
  583.     final double[] scale = new double[mainSetDimension];
  584.     rescale(y, y, scale);

  586.     // initial order selection
  587.     final double tol =
  588.         (vecRelativeTolerance == null) ? scalRelativeTolerance : vecRelativeTolerance[0];
  589.     final double log10R = JdkMath.log10(JdkMath.max(1.0e-10, tol));
  590.     int targetIter = JdkMath.max(1,
  591.                               JdkMath.min(sequence.length - 2,
  592.                                        (int) JdkMath.floor(0.5 - 0.6 * log10R)));

  594.     // set up an interpolator sharing the integrator arrays
  595.     final AbstractStepInterpolator interpolator =
  596.             new GraggBulirschStoerStepInterpolator(y, yDot0,
  597.                                                    y1, yDot1,
  598.                                                    yMidDots, forward,
  599.                                                    equations.getPrimaryMapper(),
  600.                                                    equations.getSecondaryMappers());
  601.     interpolator.storeTime(equations.getTime());

  603.     stepStart = equations.getTime();
  604.     double  hNew             = 0;
  605.     double  maxError         = Double.MAX_VALUE;
  606.     boolean previousRejected = false;
  607.     boolean firstTime        = true;
  608.     boolean newStep          = true;
  609.     boolean firstStepAlreadyComputed = false;
  610.     initIntegration(equations.getTime(), y0, t);
  611.     costPerTimeUnit[0] = 0;
  612.     isLastStep = false;
  613.     do {

  615.       double error;
  616.       boolean reject = false;

  618.       if (newStep) {

  620.         interpolator.shift();

  622.         // first evaluation, at the beginning of the step
  623.         if (! firstStepAlreadyComputed) {
  624.           computeDerivatives(stepStart, y, yDot0);
  625.         }

  627.         if (firstTime) {
  628.           hNew = initializeStep(forward, 2 * targetIter + 1, scale,
  629.                                 stepStart, y, yDot0, yTmp, yTmpDot);
  630.         }

  632.         newStep = false;
  633.       }

  635.       stepSize = hNew;

  637.       // step adjustment near bounds
  638.       if ((forward && (stepStart + stepSize > t)) ||
  639.           ((! forward) && (stepStart + stepSize < t))) {
  640.         stepSize = t - stepStart;
  641.       }
  642.       final double nextT = stepStart + stepSize;
  643.       isLastStep = forward ? (nextT >= t) : (nextT <= t);

  645.       // iterate over several substep sizes
  646.       int k = -1;
  647.       for (boolean loop = true; loop;) {

  649.         ++k;

  651.         // modified midpoint integration with the current substep
  652.         if ( ! tryStep(stepStart, y, stepSize, k, scale, fk[k],
  653.                        (k == 0) ? yMidDots[0] : diagonal[k-1],
  654.                        (k == 0) ? y1 : y1Diag[k-1],
  655.                        yTmp)) {

  657.           // the stability check failed, we reduce the global step
  658.           hNew   = JdkMath.abs(filterStep(stepSize * stabilityReduction, forward, false));
  659.           reject = true;
  660.           loop   = false;
  661.         } else {

  663.           // the substep was computed successfully
  664.           if (k > 0) {

  666.             // extrapolate the state at the end of the step
  667.             // using last iteration data
  668.             extrapolate(0, k, y1Diag, y1);
  669.             rescale(y, y1, scale);

  671.             // estimate the error at the end of the step.
  672.             error = 0;
  673.             for (int j = 0; j < mainSetDimension; ++j) {
  674.               final double e = JdkMath.abs(y1[j] - y1Diag[0][j]) / scale[j];
  675.               error += e * e;
  676.             }
  677.             error = JdkMath.sqrt(error / mainSetDimension);

  679.             if (error > 1.0e15 || (k > 1 && error > maxError)) {
  680.               // error is too big, we reduce the global step
  681.               hNew   = JdkMath.abs(filterStep(stepSize * stabilityReduction, forward, false));
  682.               reject = true;
  683.               loop   = false;
  684.             } else {

  686.               maxError = JdkMath.max(4 * error, 1.0);

  688.               // compute optimal stepsize for this order
  689.               final double exp = 1.0 / (2 * k + 1);
  690.               double fac = stepControl2 / JdkMath.pow(error / stepControl1, exp);
  691.               final double pow = JdkMath.pow(stepControl3, exp);
  692.               fac = JdkMath.max(pow / stepControl4, JdkMath.min(1 / pow, fac));
  693.               optimalStep[k]     = JdkMath.abs(filterStep(stepSize * fac, forward, true));
  694.               costPerTimeUnit[k] = costPerStep[k] / optimalStep[k];

  696.               // check convergence
  697.               switch (k - targetIter) {

  699.               case -1 :
  700.                 if (targetIter > 1 && !previousRejected) {

  702.                   // check if we can stop iterations now
  703.                   if (error <= 1.0) {
  704.                     // convergence have been reached just before targetIter
  705.                     loop = false;
  706.                   } else {
  707.                     // estimate if there is a chance convergence will
  708.                     // be reached on next iteration, using the
  709.                     // asymptotic evolution of error
  710.                     final double ratio = ((double) sequence [targetIter] * sequence[targetIter + 1]) /
  711.                                          (sequence[0] * sequence[0]);
  712.                     if (error > ratio * ratio) {
  713.                       // we don't expect to converge on next iteration
  714.                       // we reject the step immediately and reduce order
  715.                       reject = true;
  716.                       loop   = false;
  717.                       targetIter = k;
  718.                       if (targetIter > 1 &&
  719.                           costPerTimeUnit[targetIter-1] <
  720.                           orderControl1 * costPerTimeUnit[targetIter]) {
  721.                         --targetIter;
  722.                       }
  723.                       hNew = optimalStep[targetIter];
  724.                     }
  725.                   }
  726.                 }
  727.                 break;

  729.               case 0:
  730.                 if (error <= 1.0) {
  731.                   // convergence has been reached exactly at targetIter
  732.                   loop = false;
  733.                 } else {
  734.                   // estimate if there is a chance convergence will
  735.                   // be reached on next iteration, using the
  736.                   // asymptotic evolution of error
  737.                   final double ratio = ((double) sequence[k+1]) / sequence[0];
  738.                   if (error > ratio * ratio) {
  739.                     // we don't expect to converge on next iteration
  740.                     // we reject the step immediately
  741.                     reject = true;
  742.                     loop = false;
  743.                     if (targetIter > 1 &&
  744.                         costPerTimeUnit[targetIter-1] <
  745.                         orderControl1 * costPerTimeUnit[targetIter]) {
  746.                       --targetIter;
  747.                     }
  748.                     hNew = optimalStep[targetIter];
  749.                   }
  750.                 }
  751.                 break;

  753.               case 1 :
  754.                 if (error > 1.0) {
  755.                   reject = true;
  756.                   if (targetIter > 1 &&
  757.                       costPerTimeUnit[targetIter-1] <
  758.                       orderControl1 * costPerTimeUnit[targetIter]) {
  759.                     --targetIter;
  760.                   }
  761.                   hNew = optimalStep[targetIter];
  762.                 }
  763.                 loop = false;
  764.                 break;

  766.               default :
  767.                 if ((firstTime || isLastStep) && error <= 1.0) {
  768.                   loop = false;
  769.                 }
  770.                 break;
  771.               }
  772.             }
  773.           }
  774.         }
  775.       }

  777.       if (! reject) {
  778.           // derivatives at end of step
  779.           computeDerivatives(stepStart + stepSize, y1, yDot1);
  780.       }

  782.       // dense output handling
  783.       double hInt = getMaxStep();
  784.       if (! reject) {

  786.         // extrapolate state at middle point of the step
  787.         for (int j = 1; j <= k; ++j) {
  788.           extrapolate(0, j, diagonal, yMidDots[0]);
  789.         }

  791.         final int mu = 2 * k - mudif + 3;

  793.         for (int l = 0; l < mu; ++l) {

  795.           // derivative at middle point of the step
  796.           final int l2 = l / 2;
  797.           double factor = JdkMath.pow(0.5 * sequence[l2], l);
  798.           int middleIndex = fk[l2].length / 2;
  799.           for (int i = 0; i < y0.length; ++i) {
  800.             yMidDots[l+1][i] = factor * fk[l2][middleIndex + l][i];
  801.           }
  802.           for (int j = 1; j <= k - l2; ++j) {
  803.             factor = JdkMath.pow(0.5 * sequence[j + l2], l);
  804.             middleIndex = fk[l2+j].length / 2;
  805.             for (int i = 0; i < y0.length; ++i) {
  806.               diagonal[j-1][i] = factor * fk[l2+j][middleIndex+l][i];
  807.             }
  808.             extrapolate(l2, j, diagonal, yMidDots[l+1]);
  809.           }
  810.           for (int i = 0; i < y0.length; ++i) {
  811.             yMidDots[l+1][i] *= stepSize;
  812.           }

  814.           // compute centered differences to evaluate next derivatives
  815.           for (int j = (l + 1) / 2; j <= k; ++j) {
  816.             for (int m = fk[j].length - 1; m >= 2 * (l + 1); --m) {
  817.               for (int i = 0; i < y0.length; ++i) {
  818.                 fk[j][m][i] -= fk[j][m-2][i];
  819.               }
  820.             }
  821.           }
  822.         }

  824.         if (mu >= 0) {

  826.           // estimate the dense output coefficients
  827.           final GraggBulirschStoerStepInterpolator gbsInterpolator
  828.             = (GraggBulirschStoerStepInterpolator) interpolator;
  829.           gbsInterpolator.computeCoefficients(mu, stepSize);

  831.           if (useInterpolationError) {
  832.             // use the interpolation error to limit stepsize
  833.             final double interpError = gbsInterpolator.estimateError(scale);
  834.             hInt = JdkMath.abs(stepSize / JdkMath.max(JdkMath.pow(interpError, 1.0 / (mu+4)),
  835.                                                 0.01));
  836.             if (interpError > 10.0) {
  837.               hNew = hInt;
  838.               reject = true;
  839.             }
  840.           }
  841.         }
  842.       }

  844.       if (! reject) {

  846.         // Discrete events handling
  847.         interpolator.storeTime(stepStart + stepSize);
  848.         stepStart = acceptStep(interpolator, y1, yDot1, t);

  850.         // prepare next step
  851.         interpolator.storeTime(stepStart);
  852.         System.arraycopy(y1, 0, y, 0, y0.length);
  853.         System.arraycopy(yDot1, 0, yDot0, 0, y0.length);
  854.         firstStepAlreadyComputed = true;

  856.         int optimalIter;
  857.         if (k == 1) {
  858.           optimalIter = 2;
  859.           if (previousRejected) {
  860.             optimalIter = 1;
  861.           }
  862.         } else if (k <= targetIter) {
  863.           optimalIter = k;
  864.           if (costPerTimeUnit[k-1] < orderControl1 * costPerTimeUnit[k]) {
  865.             optimalIter = k-1;
  866.           } else if (costPerTimeUnit[k] < orderControl2 * costPerTimeUnit[k-1]) {
  867.             optimalIter = JdkMath.min(k+1, sequence.length - 2);
  868.           }
  869.         } else {
  870.           optimalIter = k - 1;
  871.           if (k > 2 &&
  872.               costPerTimeUnit[k-2] < orderControl1 * costPerTimeUnit[k-1]) {
  873.             optimalIter = k - 2;
  874.           }
  875.           if (costPerTimeUnit[k] < orderControl2 * costPerTimeUnit[optimalIter]) {
  876.             optimalIter = JdkMath.min(k, sequence.length - 2);
  877.           }
  878.         }

  880.         if (previousRejected) {
  881.           // after a rejected step neither order nor stepsize
  882.           // should increase
  883.           targetIter = JdkMath.min(optimalIter, k);
  884.           hNew = JdkMath.min(JdkMath.abs(stepSize), optimalStep[targetIter]);
  885.         } else {
  886.           // stepsize control
  887.           if (optimalIter <= k) {
  888.             hNew = optimalStep[optimalIter];
  889.           } else {
  890.             if (k < targetIter &&
  891.                 costPerTimeUnit[k] < orderControl2 * costPerTimeUnit[k-1]) {
  892.               hNew = filterStep(optimalStep[k] * costPerStep[optimalIter+1] / costPerStep[k],
  893.                                forward, false);
  894.             } else {
  895.               hNew = filterStep(optimalStep[k] * costPerStep[optimalIter] / costPerStep[k],
  896.                                 forward, false);
  897.             }
  898.           }

  900.           targetIter = optimalIter;
  901.         }

  903.         newStep = true;
  904.       }

  906.       hNew = JdkMath.min(hNew, hInt);
  907.       if (! forward) {
  908.         hNew = -hNew;
  909.       }

  911.       firstTime = false;

  913.       if (reject) {
  914.         isLastStep = false;
  915.         previousRejected = true;
  916.       } else {
  917.         previousRejected = false;
  918.       }
  919.     } while (!isLastStep);

  921.     // dispatch results
  922.     equations.setTime(stepStart);
  923.     equations.setCompleteState(y);

  925.     resetInternalState();
  926.   }
  927. }
Created with JaCoCo 0.8.10.202304240956Code Coverage Report for Apache Commons Math 4.0-SNAPSHOT