GraggBulirschStoerIntegrator.java

  1. /*
  2.  * Licensed to the Hipparchus project 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 Hipparchus project 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.  *      https://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.hipparchus.ode.nonstiff;

  20. import org.hipparchus.exception.MathIllegalArgumentException;
  21. import org.hipparchus.exception.MathIllegalStateException;
  22. import org.hipparchus.ode.ExpandableODE;
  23. import org.hipparchus.ode.LocalizedODEFormats;
  24. import org.hipparchus.ode.ODEState;
  25. import org.hipparchus.ode.ODEStateAndDerivative;
  26. import org.hipparchus.ode.nonstiff.interpolators.GraggBulirschStoerStateInterpolator;
  27. import org.hipparchus.util.FastMath;

  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.  *
  63.  * <blockquote>
  64.  * <p>Copyright (c) 2004, Ernst Hairer</p>
  65.  *
  66.  * <p>Redistribution and use in source and binary forms, with or
  67.  * without modification, are permitted provided that the following
  68.  * conditions are met:</p>
  69.  * <ul>
  70.  *  <li>Redistributions of source code must retain the above copyright
  71.  *      notice, this list of conditions and the following disclaimer.</li>
  72.  *  <li>Redistributions in binary form must reproduce the above copyright
  73.  *      notice, this list of conditions and the following disclaimer in the
  74.  *      documentation and/or other materials provided with the distribution.</li>
  75.  * </ul>
  76.  *
  77.  * <p><strong>THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND
  78.  * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING,
  79.  * BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
  80.  * FOR A  PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE REGENTS OR
  81.  * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
  82.  * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
  83.  * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
  84.  * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
  85.  * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
  86.  * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
  87.  * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.</strong></p>
  88.  * </blockquote>
  89.  *
  90.  */

  92. public class GraggBulirschStoerIntegrator extends AdaptiveStepsizeIntegrator {

  94.     /** Name of integration scheme. */
  95.     public static final String METHOD_NAME = "Gragg-Bulirsch-Stoer";

  97.     /** maximal order. */
  98.     private int maxOrder;

  100.     /** step size sequence. */
  101.     private int[] sequence;

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

  106.     /** cost per unit step. */
  107.     private double[] costPerTimeUnit;

  109.     /** optimal steps for each order. */
  110.     private double[] optimalStep;

  112.     /** extrapolation coefficients. */
  113.     private double[][] coeff;

  115.     /** stability check enabling parameter. */
  116.     private boolean performTest;

  118.     /** maximal number of checks for each iteration. */
  119.     private int maxChecks;

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

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

  127.     /** first stepsize control factor. */
  128.     private double stepControl1;

  130.     /** second stepsize control factor. */
  131.     private double stepControl2;

  133.     /** third stepsize control factor. */
  134.     private double stepControl3;

  136.     /** fourth stepsize control factor. */
  137.     private double stepControl4;

  139.     /** first order control factor. */
  140.     private double orderControl1;

  142.     /** second order control factor. */
  143.     private double orderControl2;

  145.     /** use interpolation error in stepsize control. */
  146.     private boolean useInterpolationError;

  148.     /** interpolation order control parameter. */
  149.     private int mudif;

  151.     /** Simple constructor.
  152.      * Build a Gragg-Bulirsch-Stoer integrator with the given step
  153.      * bounds. All tuning parameters are set to their default
  154.      * values. The default step handler does nothing.
  155.      * @param minStep minimal step (sign is irrelevant, regardless of
  156.      * integration direction, forward or backward), the last step can
  157.      * be smaller than this
  158.      * @param maxStep maximal step (sign is irrelevant, regardless of
  159.      * integration direction, forward or backward), the last step can
  160.      * be smaller than this
  161.      * @param scalAbsoluteTolerance allowed absolute error
  162.      * @param scalRelativeTolerance allowed relative error
  163.      */
  164.     public GraggBulirschStoerIntegrator(final double minStep, final double maxStep,
  165.                                         final double scalAbsoluteTolerance,
  166.                                         final double scalRelativeTolerance) {
  167.         super(METHOD_NAME, minStep, maxStep,
  168.               scalAbsoluteTolerance, scalRelativeTolerance);
  169.         setStabilityCheck(true, -1, -1, -1);
  170.         setControlFactors(-1, -1, -1, -1);
  171.         setOrderControl(-1, -1, -1);
  172.         setInterpolationControl(true, -1);
  173.     }

  175.     /** Simple constructor.
  176.      * Build a Gragg-Bulirsch-Stoer integrator with the given step
  177.      * bounds. All tuning parameters are set to their default
  178.      * values. The default step handler does nothing.
  179.      * @param minStep minimal step (must be positive even for backward
  180.      * integration), the last step can be smaller than this
  181.      * @param maxStep maximal step (must be positive even for backward
  182.      * integration)
  183.      * @param vecAbsoluteTolerance allowed absolute error
  184.      * @param vecRelativeTolerance allowed relative error
  185.      */
  186.     public GraggBulirschStoerIntegrator(final double minStep, final double maxStep,
  187.                                         final double[] vecAbsoluteTolerance,
  188.                                         final double[] vecRelativeTolerance) {
  189.         super(METHOD_NAME, minStep, maxStep,
  190.               vecAbsoluteTolerance, vecRelativeTolerance);
  191.         setStabilityCheck(true, -1, -1, -1);
  192.         setControlFactors(-1, -1, -1, -1);
  193.         setOrderControl(-1, -1, -1);
  194.         setInterpolationControl(true, -1);
  195.     }

  197.     /** Set the stability check controls.
  198.      * <p>The stability check is performed on the first few iterations of
  199.      * the extrapolation scheme. If this test fails, the step is rejected
  200.      * and the stepsize is reduced.</p>
  201.      * <p>By default, the test is performed, at most during two
  202.      * iterations at each step, and at most once for each of these
  203.      * iterations. The default stepsize reduction factor is 0.5.</p>
  204.      * @param performStabilityCheck if true, stability check will be performed,
  205.      if false, the check will be skipped
  206.      * @param maxNumIter maximal number of iterations for which checks are
  207.      * performed (the number of iterations is reset to default if negative
  208.      * or null)
  209.      * @param maxNumChecks maximal number of checks for each iteration
  210.      * (the number of checks is reset to default if negative or null)
  211.      * @param stepsizeReductionFactor stepsize reduction factor in case of
  212.      * failure (the factor is reset to default if lower than 0.0001 or
  213.      * greater than 0.9999)
  214.      */
  215.     public void setStabilityCheck(final boolean performStabilityCheck,
  216.                                   final int maxNumIter, final int maxNumChecks,
  217.                                   final double stepsizeReductionFactor) {

  219.         this.performTest = performStabilityCheck;
  220.         this.maxIter     = (maxNumIter   <= 0) ? 2 : maxNumIter;
  221.         this.maxChecks   = (maxNumChecks <= 0) ? 1 : maxNumChecks;

  223.         if ((stepsizeReductionFactor < 0.0001) || (stepsizeReductionFactor > 0.9999)) {
  224.             this.stabilityReduction = 0.5;
  225.         } else {
  226.             this.stabilityReduction = stepsizeReductionFactor;
  227.         }

  229.     }

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

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

  258.         if ((control1 < 0.0001) || (control1 > 0.9999)) {
  259.             this.stepControl1 = 0.65;
  260.         } else {
  261.             this.stepControl1 = control1;
  262.         }

  264.         if ((control2 < 0.0001) || (control2 > 0.9999)) {
  265.             this.stepControl2 = 0.94;
  266.         } else {
  267.             this.stepControl2 = control2;
  268.         }

  270.         if ((control3 < 0.0001) || (control3 > 0.9999)) {
  271.             this.stepControl3 = 0.02;
  272.         } else {
  273.             this.stepControl3 = control3;
  274.         }

  276.         if ((control4 < 1.0001) || (control4 > 999.9)) {
  277.             this.stepControl4 = 4.0;
  278.         } else {
  279.             this.stepControl4 = control4;
  280.         }

  282.     }

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

  310.         if (maximalOrder > 6 && maximalOrder % 2 == 0) {
  311.             this.maxOrder = maximalOrder;
  312.         } else {
  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();

  331.     }

  333.     /** Initialize the integrator internal arrays. */
  334.     private void initializeArrays() {

  336.         final int size = maxOrder / 2;

  338.         if ((sequence == null) || (sequence.length != size)) {
  339.             // all arrays should be reallocated with the right size
  340.             sequence        = new int[size];
  341.             costPerStep     = new int[size];
  342.             coeff           = new double[size][];
  343.             costPerTimeUnit = new double[size];
  344.             optimalStep     = new double[size];
  345.         }

  347.         // step size sequence: 2, 6, 10, 14, ...
  348.         for (int k = 0; k < size; ++k) {
  349.             sequence[k] = 4 * k + 2;
  350.         }

  352.         // initialize the order selection cost array
  353.         // (number of function calls for each column of the extrapolation table)
  354.         costPerStep[0] = sequence[0] + 1;
  355.         for (int k = 1; k < size; ++k) {
  356.             costPerStep[k] = costPerStep[k - 1] + sequence[k];
  357.         }

  359.         // initialize the extrapolation tables
  360.         for (int k = 0; k < size; ++k) {
  361.             coeff[k] = (k > 0) ? new double[k] : null;
  362.             for (int l = 0; l < k; ++l) {
  363.                 final double ratio = ((double) sequence[k]) / sequence[k - l - 1];
  364.                 coeff[k][l] = 1.0 / (ratio * ratio - 1.0);
  365.             }
  366.         }

  368.     }

  370.     /** Set the interpolation order control parameter.
  371.      * The interpolation order for dense output is 2k - mudif + 1. The
  372.      * default value for mudif is 4 and the interpolation error is used
  373.      * in stepsize control by default.

  375.      * @param useInterpolationErrorForControl if true, interpolation error is used
  376.      * for stepsize control
  377.      * @param mudifControlParameter interpolation order control parameter (the parameter
  378.      * is reset to default if &lt;= 0 or &gt;= 7)
  379.      */
  380.     public void setInterpolationControl(final boolean useInterpolationErrorForControl,
  381.                                         final int mudifControlParameter) {

  383.         this.useInterpolationError = useInterpolationErrorForControl;

  385.         if ((mudifControlParameter <= 0) || (mudifControlParameter >= 7)) {
  386.             this.mudif = 4;
  387.         } else {
  388.             this.mudif = mudifControlParameter;
  389.         }

  391.     }

  393.     /** Update scaling array.
  394.      * @param y1 first state vector to use for scaling
  395.      * @param y2 second state vector to use for scaling
  396.      * @param scale scaling array to update (can be shorter than state)
  397.      */
  398.     private void rescale(final double[] y1, final double[] y2, final double[] scale) {
  399.         final StepsizeHelper helper = getStepSizeHelper();
  400.         for (int i = 0; i < scale.length; ++i) {
  401.             scale[i] = helper.getTolerance(i, FastMath.max(FastMath.abs(y1[i]), FastMath.abs(y2[i])));
  402.         }
  403.     }

  405.     /** Perform integration over one step using substeps of a modified
  406.      * midpoint method.
  407.      * @param t0 initial time
  408.      * @param y0 initial value of the state vector at t0
  409.      * @param step global step
  410.      * @param k iteration number (from 0 to sequence.length - 1)
  411.      * @param scale scaling array (can be shorter than state)
  412.      * @param f placeholder where to put the state vector derivatives at each substep
  413.      *          (element 0 already contains initial derivative)
  414.      * @param yMiddle placeholder where to put the state vector at the middle of the step
  415.      * @param yEnd placeholder where to put the state vector at the end
  416.      * @return true if computation was done properly,
  417.      *         false if stability check failed before end of computation
  418.      * @exception MathIllegalStateException if the number of functions evaluations is exceeded
  419.      * @exception MathIllegalArgumentException if arrays dimensions do not match equations settings
  420.      */
  421.     private boolean tryStep(final double t0, final double[] y0, final double step, final int k,
  422.                             final double[] scale, final double[][] f,
  423.                             final double[] yMiddle, final double[] yEnd)
  424.         throws MathIllegalArgumentException, MathIllegalStateException {

  426.         final int    n        = sequence[k];
  427.         final double subStep  = step / n;
  428.         final double subStep2 = 2 * subStep;

  430.         // first substep
  431.         double t = t0 + subStep;
  432.         for (int i = 0; i < y0.length; ++i) {
  433.             yEnd[i] = y0[i] + subStep * f[0][i];
  434.         }
  435.         f[1] = computeDerivatives(t, yEnd);

  437.         // other substeps
  438.         final double[] yTmp = y0.clone();
  439.         for (int j = 1; j < n; ++j) {

  441.             if (2 * j == n) {
  442.                 // save the point at the middle of the step
  443.                 System.arraycopy(yEnd, 0, yMiddle, 0, y0.length);
  444.             }

  446.             t += subStep;
  447.             for (int i = 0; i < y0.length; ++i) {
  448.                 final double middle = yEnd[i];
  449.                 yEnd[i]       = yTmp[i] + subStep2 * f[j][i];
  450.                 yTmp[i]       = middle;
  451.             }

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

  455.             // stability check
  456.             if (performTest && (j <= maxChecks) && (k < maxIter)) {
  457.                 double initialNorm = 0.0;
  458.                 for (int l = 0; l < scale.length; ++l) {
  459.                     final double ratio = f[0][l] / scale[l];
  460.                     initialNorm += ratio * ratio;
  461.                 }
  462.                 double deltaNorm = 0.0;
  463.                 for (int l = 0; l < scale.length; ++l) {
  464.                     final double ratio = (f[j + 1][l] - f[0][l]) / scale[l];
  465.                     deltaNorm += ratio * ratio;
  466.                 }
  467.                 if (deltaNorm > 4 * FastMath.max(1.0e-15, initialNorm)) {
  468.                     return false;
  469.                 }
  470.             }

  472.         }

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

  479.         return true;

  481.     }

  483.     /** Extrapolate a vector.
  484.      * @param offset offset to use in the coefficients table
  485.      * @param k index of the last updated point
  486.      * @param diag working diagonal of the Aitken-Neville's
  487.      * triangle, without the last element
  488.      * @param last last element
  489.      */
  490.     private void extrapolate(final int offset, final int k,
  491.                              final double[][] diag, final double[] last) {

  493.         // update the diagonal
  494.         for (int j = 1; j < k; ++j) {
  495.             for (int i = 0; i < last.length; ++i) {
  496.                 // Aitken-Neville's recursive formula
  497.                 diag[k - j - 1][i] = diag[k - j][i] +
  498.                                 coeff[k + offset][j - 1] * (diag[k - j][i] - diag[k - j - 1][i]);
  499.             }
  500.         }

  502.         // update the last element
  503.         for (int i = 0; i < last.length; ++i) {
  504.             // Aitken-Neville's recursive formula
  505.             last[i] = diag[0][i] + coeff[k + offset][k - 1] * (diag[0][i] - last[i]);
  506.         }
  507.     }

  509.     /** {@inheritDoc} */
  510.     @Override
  511.     public ODEStateAndDerivative integrate(final ExpandableODE equations,
  512.                                            final ODEState initialState, final double finalTime)
  513.         throws MathIllegalArgumentException, MathIllegalStateException {

  515.         sanityChecks(initialState, finalTime);
  516.         setStepStart(initIntegration(equations, initialState, finalTime));
  517.         final boolean forward = finalTime > initialState.getTime();

  519.         // create some internal working arrays
  520.         double[]         y        = getStepStart().getCompleteState();
  521.         final double[]   y1       = new double[y.length];
  522.         final double[][] diagonal = new double[sequence.length - 1][];
  523.         final double[][] y1Diag   = new double[sequence.length - 1][];
  524.         for (int k = 0; k < sequence.length - 1; ++k) {
  525.             diagonal[k] = new double[y.length];
  526.             y1Diag[k]   = new double[y.length];
  527.         }

  529.         final double[][][] fk = new double[sequence.length][][];
  530.         for (int k = 0; k < sequence.length; ++k) {
  531.             fk[k] = new double[sequence[k] + 1][];
  532.         }

  534.         // scaled derivatives at the middle of the step $\tau$
  535.         // (element k is $h^{k} d^{k}y(\tau)/dt^{k}$ where h is step size...)
  536.         final double[][] yMidDots = new double[1 + 2 * sequence.length][y.length];

  538.         // initial scaling
  539.         final int mainSetDimension = getStepSizeHelper().getMainSetDimension();
  540.         final double[] scale = new double[mainSetDimension];
  541.         rescale(y, y, scale);

  543.         // initial order selection
  544.         final double tol    = getStepSizeHelper().getRelativeTolerance(0);
  545.         final double log10R = FastMath.log10(FastMath.max(1.0e-10, tol));
  546.         int targetIter = FastMath.max(1,
  547.                                       FastMath.min(sequence.length - 2,
  548.                                                    (int) FastMath.floor(0.5 - 0.6 * log10R)));

  550.         double  hNew                     = 0;
  551.         double  maxError                 = Double.MAX_VALUE;
  552.         boolean previousRejected         = false;
  553.         boolean firstTime                = true;
  554.         boolean newStep                  = true;
  555.         costPerTimeUnit[0] = 0;
  556.         setIsLastStep(false);
  557.         do {

  559.             double error;
  560.             boolean reject = false;

  562.             if (newStep) {

  564.                 // first evaluation, at the beginning of the step
  565.                 final double[] yDot0 = getStepStart().getCompleteDerivative();
  566.                 for (int k = 0; k < sequence.length; ++k) {
  567.                     // all sequences start from the same point, so we share the derivatives
  568.                     fk[k][0] = yDot0;
  569.                 }

  571.                 if (firstTime) {
  572.                     hNew = initializeStep(forward, 2 * targetIter + 1, scale,
  573.                                           getStepStart());
  574.                 }

  576.                 newStep = false;

  578.             }

  580.             setStepSize(hNew);

  582.             // step adjustment near bounds
  583.             if (forward) {
  584.                 if (getStepStart().getTime() + getStepSize() >= finalTime) {
  585.                     setStepSize(finalTime - getStepStart().getTime());
  586.                 }
  587.             } else {
  588.                 if (getStepStart().getTime() + getStepSize() <= finalTime) {
  589.                     setStepSize(finalTime - getStepStart().getTime());
  590.                 }
  591.             }
  592.             final double nextT = getStepStart().getTime() + getStepSize();
  593.             setIsLastStep(forward ? (nextT >= finalTime) : (nextT <= finalTime));

  595.             // iterate over several substep sizes
  596.             int k = -1;
  597.             for (boolean loop = true; loop; ) {

  599.                 ++k;

  601.                 // modified midpoint integration with the current substep
  602.                 if ( ! tryStep(getStepStart().getTime(), y, getStepSize(), k, scale, fk[k],
  603.                                (k == 0) ? yMidDots[0] : diagonal[k - 1],
  604.                                (k == 0) ? y1 : y1Diag[k - 1])) {

  606.                     // the stability check failed, we reduce the global step
  607.                     hNew   = FastMath.abs(getStepSizeHelper().filterStep(getStepSize() * stabilityReduction, forward, false));
  608.                     reject = true;
  609.                     loop   = false;

  611.                 } else {

  613.                     // the substep was computed successfully
  614.                     if (k > 0) {

  616.                         // extrapolate the state at the end of the step
  617.                         // using last iteration data
  618.                         extrapolate(0, k, y1Diag, y1);
  619.                         rescale(y, y1, scale);

  621.                         // estimate the error at the end of the step.
  622.                         error = 0;
  623.                         for (int j = 0; j < mainSetDimension; ++j) {
  624.                             final double e = FastMath.abs(y1[j] - y1Diag[0][j]) / scale[j];
  625.                             error += e * e;
  626.                         }
  627.                         error = FastMath.sqrt(error / mainSetDimension);
  628.                         if (Double.isNaN(error)) {
  629.                             throw new MathIllegalStateException(LocalizedODEFormats.NAN_APPEARING_DURING_INTEGRATION,
  630.                                                                 nextT);
  631.                         }

  633.                         if ((error > 1.0e15) || ((k > 1) && (error > maxError))) {
  634.                             // error is too big, we reduce the global step
  635.                             hNew   = FastMath.abs(getStepSizeHelper().filterStep(getStepSize() * stabilityReduction, forward, false));
  636.                             reject = true;
  637.                             loop   = false;
  638.                         } else {

  640.                             maxError = FastMath.max(4 * error, 1.0);

  642.                             // compute optimal stepsize for this order
  643.                             final double exp = 1.0 / (2 * k + 1);
  644.                             double fac = stepControl2 / FastMath.pow(error / stepControl1, exp);
  645.                             final double pow = FastMath.pow(stepControl3, exp);
  646.                             fac = FastMath.max(pow / stepControl4, FastMath.min(1 / pow, fac));
  647.                             final boolean acceptSmall = k < targetIter;
  648.                             optimalStep[k]     = FastMath.abs(getStepSizeHelper().filterStep(getStepSize() * fac, forward, acceptSmall));
  649.                             costPerTimeUnit[k] = costPerStep[k] / optimalStep[k];

  651.                             // check convergence
  652.                             switch (k - targetIter) {

  654.                                 case -1 :
  655.                                     if ((targetIter > 1) && ! previousRejected) {

  657.                                         // check if we can stop iterations now
  658.                                         if (error <= 1.0) {
  659.                                             // convergence have been reached just before targetIter
  660.                                             loop = false;
  661.                                         } else {
  662.                                             // estimate if there is a chance convergence will
  663.                                             // be reached on next iteration, using the
  664.                                             // asymptotic evolution of error
  665.                                             final double ratio = ((double) sequence [targetIter] * sequence[targetIter + 1]) /
  666.                                                             (sequence[0] * sequence[0]);
  667.                                             if (error > ratio * ratio) {
  668.                                                 // we don't expect to converge on next iteration
  669.                                                 // we reject the step immediately and reduce order
  670.                                                 reject = true;
  671.                                                 loop   = false;
  672.                                                 targetIter = k;
  673.                                                 if ((targetIter > 1) &&
  674.                                                     (costPerTimeUnit[targetIter - 1] <
  675.                                                                     orderControl1 * costPerTimeUnit[targetIter])) {
  676.                                                     --targetIter;
  677.                                                 }
  678.                                                 hNew = getStepSizeHelper().filterStep(optimalStep[targetIter], forward, false);
  679.                                             }
  680.                                         }
  681.                                     }
  682.                                     break;

  684.                                 case 0:
  685.                                     if (error <= 1.0) {
  686.                                         // convergence has been reached exactly at targetIter
  687.                                         loop = false;
  688.                                     } else {
  689.                                         // estimate if there is a chance convergence will
  690.                                         // be reached on next iteration, using the
  691.                                         // asymptotic evolution of error
  692.                                         final double ratio = ((double) sequence[k + 1]) / sequence[0];
  693.                                         if (error > ratio * ratio) {
  694.                                             // we don't expect to converge on next iteration
  695.                                             // we reject the step immediately
  696.                                             reject = true;
  697.                                             loop = false;
  698.                                             if ((targetIter > 1) &&
  699.                                                  (costPerTimeUnit[targetIter - 1] <
  700.                                                                  orderControl1 * costPerTimeUnit[targetIter])) {
  701.                                                 --targetIter;
  702.                                             }
  703.                                             hNew = getStepSizeHelper().filterStep(optimalStep[targetIter], forward, false);
  704.                                         }
  705.                                     }
  706.                                     break;

  708.                                 case 1 :
  709.                                     if (error > 1.0) {
  710.                                         reject = true;
  711.                                         if ((targetIter > 1) &&
  712.                                             (costPerTimeUnit[targetIter - 1] <
  713.                                                             orderControl1 * costPerTimeUnit[targetIter])) {
  714.                                             --targetIter;
  715.                                         }
  716.                                         hNew = getStepSizeHelper().filterStep(optimalStep[targetIter], forward, false);
  717.                                     }
  718.                                     loop = false;
  719.                                     break;

  721.                                 default :
  722.                                     if ((firstTime || isLastStep()) && (error <= 1.0)) {
  723.                                         loop = false;
  724.                                     }
  725.                                     break;

  727.                             }

  729.                         }
  730.                     }
  731.                 }
  732.             }

  734.             // dense output handling
  735.             double hInt = getMaxStep();
  736.             final GraggBulirschStoerStateInterpolator interpolator;
  737.             if (! reject) {

  739.                 // extrapolate state at middle point of the step
  740.                 for (int j = 1; j <= k; ++j) {
  741.                     extrapolate(0, j, diagonal, yMidDots[0]);
  742.                 }

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

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

  748.                     // derivative at middle point of the step
  749.                     final int l2 = l / 2;
  750.                     double factor = FastMath.pow(0.5 * sequence[l2], l);
  751.                     int middleIndex = fk[l2].length / 2;
  752.                     for (int i = 0; i < y.length; ++i) {
  753.                         yMidDots[l + 1][i] = factor * fk[l2][middleIndex + l][i];
  754.                     }
  755.                     for (int j = 1; j <= k - l2; ++j) {
  756.                         factor = FastMath.pow(0.5 * sequence[j + l2], l);
  757.                         middleIndex = fk[l2 + j].length / 2;
  758.                         for (int i = 0; i < y.length; ++i) {
  759.                             diagonal[j - 1][i] = factor * fk[l2 + j][middleIndex + l][i];
  760.                         }
  761.                         extrapolate(l2, j, diagonal, yMidDots[l + 1]);
  762.                     }
  763.                     for (int i = 0; i < y.length; ++i) {
  764.                         yMidDots[l + 1][i] *= getStepSize();
  765.                     }

  767.                     // compute centered differences to evaluate next derivatives
  768.                     for (int j = (l + 1) / 2; j <= k; ++j) {
  769.                         for (int m = fk[j].length - 1; m >= 2 * (l + 1); --m) {
  770.                             for (int i = 0; i < y.length; ++i) {
  771.                                 fk[j][m][i] -= fk[j][m - 2][i];
  772.                             }
  773.                         }
  774.                     }

  776.                 }

  778.                 // state at end of step
  779.                 final ODEStateAndDerivative stepEnd =
  780.                     equations.getMapper().mapStateAndDerivative(nextT, y1, computeDerivatives(nextT, y1));

  782.                 // set up interpolator covering the full step
  783.                 interpolator = new GraggBulirschStoerStateInterpolator(forward,
  784.                                                                        getStepStart(), stepEnd,
  785.                                                                        getStepStart(), stepEnd,
  786.                                                                        equations.getMapper(),
  787.                                                                        yMidDots, mu);

  789.                 if (mu >= 0 && useInterpolationError) {
  790.                     // use the interpolation error to limit stepsize
  791.                     final double interpError = interpolator.estimateError(scale);
  792.                     hInt = FastMath.abs(getStepSize() /
  793.                                         FastMath.max(FastMath.pow(interpError, 1.0 / (mu + 4)), 0.01));
  794.                     if (interpError > 10.0) {
  795.                         hNew   = getStepSizeHelper().filterStep(hInt, forward, false);
  796.                         reject = true;
  797.                     }
  798.                 }

  800.             } else {
  801.                 interpolator = null;
  802.             }

  804.             if (! reject) {

  806.                 // Discrete events handling
  807.                 setStepStart(acceptStep(interpolator, finalTime));

  809.                 // prepare next step
  810.                 // beware that y1 is not always valid anymore here,
  811.                 // as some event may have triggered a reset
  812.                 // so we need to copy the new step start set previously
  813.                 y = getStepStart().getCompleteState();

  815.                 int optimalIter;
  816.                 if (k == 1) {
  817.                     optimalIter = 2;
  818.                     if (previousRejected) {
  819.                         optimalIter = 1;
  820.                     }
  821.                 } else if (k <= targetIter) {
  822.                     optimalIter = k;
  823.                     if (costPerTimeUnit[k - 1] < orderControl1 * costPerTimeUnit[k]) {
  824.                         optimalIter = k - 1;
  825.                     } else if (costPerTimeUnit[k] < orderControl2 * costPerTimeUnit[k - 1]) {
  826.                         optimalIter = FastMath.min(k + 1, sequence.length - 2);
  827.                     }
  828.                 } else {
  829.                     optimalIter = k - 1;
  830.                     if ((k > 2) && (costPerTimeUnit[k - 2] < orderControl1 * costPerTimeUnit[k - 1])) {
  831.                         optimalIter = k - 2;
  832.                     }
  833.                     if (costPerTimeUnit[k] < orderControl2 * costPerTimeUnit[optimalIter]) {
  834.                         optimalIter = FastMath.min(k, sequence.length - 2);
  835.                     }
  836.                 }

  838.                 if (previousRejected) {
  839.                     // after a rejected step neither order nor stepsize
  840.                     // should increase
  841.                     targetIter = FastMath.min(optimalIter, k);
  842.                     hNew = FastMath.min(FastMath.abs(getStepSize()), optimalStep[targetIter]);
  843.                 } else {
  844.                     // stepsize control
  845.                     if (optimalIter <= k) {
  846.                         hNew = getStepSizeHelper().filterStep(optimalStep[optimalIter], forward, false);
  847.                     } else {
  848.                         if ((k < targetIter) &&
  849.                                         (costPerTimeUnit[k] < orderControl2 * costPerTimeUnit[k - 1])) {
  850.                             hNew = getStepSizeHelper().
  851.                                    filterStep(optimalStep[k] * costPerStep[optimalIter + 1] / costPerStep[k], forward, false);
  852.                         } else {
  853.                             hNew = getStepSizeHelper().
  854.                                    filterStep(optimalStep[k] * costPerStep[optimalIter] / costPerStep[k], forward, false);
  855.                         }
  856.                     }

  858.                     targetIter = optimalIter;

  860.                 }

  862.                 newStep = true;

  864.             }

  866.             hNew = FastMath.min(hNew, hInt);
  867.             if (! forward) {
  868.                 hNew = -hNew;
  869.             }

  871.             firstTime = false;

  873.             if (reject) {
  874.                 setIsLastStep(false);
  875.                 previousRejected = true;
  876.             } else {
  877.                 previousRejected = false;
  878.             }

  880.         } while (!isLastStep());

  882.         final ODEStateAndDerivative finalState = getStepStart();
  883.         resetInternalState();
  884.         return finalState;

  886.     }

  888. }
Created with JaCoCo 0.8.12.202403310830