GraggBulirschStoerStateInterpolator.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.interpolators;

  20. import org.hipparchus.ode.EquationsMapper;
  21. import org.hipparchus.ode.ODEStateAndDerivative;
  22. import org.hipparchus.ode.nonstiff.GraggBulirschStoerIntegrator;
  23. import org.hipparchus.ode.sampling.AbstractODEStateInterpolator;
  24. import org.hipparchus.util.FastMath;

  26. /**
  27.  * This class implements an interpolator for the Gragg-Bulirsch-Stoer
  28.  * integrator.
  29.  *
  30.  * <p>This interpolator compute dense output inside the last step
  31.  * produced by a Gragg-Bulirsch-Stoer integrator.</p>
  32.  *
  33.  * <p>
  34.  * This implementation is basically a reimplementation in Java of the
  35.  * <a
  36.  * href="http://www.unige.ch/math/folks/hairer/prog/nonstiff/odex.f">odex</a>
  37.  * fortran code by E. Hairer and G. Wanner. The redistribution policy
  38.  * for this code is available <a
  39.  * href="http://www.unige.ch/~hairer/prog/licence.txt">here</a>, for
  40.  * convenience, it is reproduced below.</p>
  41.  *
  42.  * <blockquote>
  43.  * <p>Copyright (c) 2004, Ernst Hairer</p>
  44.  *
  45.  * <p>Redistribution and use in source and binary forms, with or
  46.  * without modification, are permitted provided that the following
  47.  * conditions are met:</p>
  48.  * <ul>
  49.  *  <li>Redistributions of source code must retain the above copyright
  50.  *      notice, this list of conditions and the following disclaimer.</li>
  51.  *  <li>Redistributions in binary form must reproduce the above copyright
  52.  *      notice, this list of conditions and the following disclaimer in the
  53.  *      documentation and/or other materials provided with the distribution.</li>
  54.  * </ul>
  55.  *
  56.  * <p><strong>THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND
  57.  * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING,
  58.  * BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
  59.  * FOR A  PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE REGENTS OR
  60.  * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
  61.  * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
  62.  * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
  63.  * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
  64.  * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
  65.  * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
  66.  * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.</strong></p>
  67.  * </blockquote>
  68.  *
  69.  * @see GraggBulirschStoerIntegrator
  70.  */

  72. public class GraggBulirschStoerStateInterpolator extends AbstractODEStateInterpolator {

  74.     /** Serializable version identifier. */
  75.     private static final long serialVersionUID = 20160329L;

  77.     /** Scaled derivatives at the middle of the step $\tau$.
  78.      * (element k is $h^{k} d^{k}y(\tau)/dt^{k}$ where h is step size...)
  79.      */
  80.     private final double[][] yMidDots;

  82.     /** Interpolation polynomials. */
  83.     private final double[][] polynomials;

  85.     /** Error coefficients for the interpolation. */
  86.     private final double[] errfac;

  88.     /** Degree of the interpolation polynomials. */
  89.     private final int currentDegree;

  91.     /** Simple constructor.
  92.      * @param forward integration direction indicator
  93.      * @param globalPreviousState start of the global step
  94.      * @param globalCurrentState end of the global step
  95.      * @param softPreviousState start of the restricted step
  96.      * @param softCurrentState end of the restricted step
  97.      * @param mapper equations mapper for the all equations
  98.      * @param yMidDots scaled derivatives at the middle of the step $\tau$
  99.      * (element k is $h^{k} d^{k}y(\tau)/dt^{k}$ where h is step size...)
  100.      * @param mu degree of the interpolation polynomial
  101.      */
  102.     public GraggBulirschStoerStateInterpolator(final boolean forward,
  103.                                                final ODEStateAndDerivative globalPreviousState,
  104.                                                final ODEStateAndDerivative globalCurrentState,
  105.                                                final ODEStateAndDerivative softPreviousState,
  106.                                                final ODEStateAndDerivative softCurrentState,
  107.                                                final EquationsMapper mapper,
  108.                                                final double[][] yMidDots,
  109.                                                final int mu) {
  110.         super(forward, globalPreviousState, globalCurrentState, softPreviousState, softCurrentState, mapper);

  112.         this.yMidDots      = yMidDots.clone();
  113.         this.currentDegree = mu + 4;
  114.         this.polynomials   = new double[currentDegree + 1][getCurrentState().getCompleteStateDimension()];

  116.         // initialize the error factors array for interpolation
  117.         if (currentDegree <= 4) {
  118.             errfac = null;
  119.         } else {
  120.             errfac = new double[currentDegree - 4];
  121.             for (int i = 0; i < errfac.length; ++i) {
  122.                 final int ip5 = i + 5;
  123.                 errfac[i] = 1.0 / (ip5 * ip5);
  124.                 final double e = 0.5 * FastMath.sqrt (((double) (i + 1)) / ip5);
  125.                 for (int j = 0; j <= i; ++j) {
  126.                     errfac[i] *= e / (j + 1);
  127.                 }
  128.             }
  129.         }

  131.         // compute the interpolation coefficients
  132.         computeCoefficients(mu);

  134.     }

  136.     /** {@inheritDoc} */
  137.     @Override
  138.     protected GraggBulirschStoerStateInterpolator create(final boolean newForward,
  139.                                                          final ODEStateAndDerivative newGlobalPreviousState,
  140.                                                          final ODEStateAndDerivative newGlobalCurrentState,
  141.                                                          final ODEStateAndDerivative newSoftPreviousState,
  142.                                                          final ODEStateAndDerivative newSoftCurrentState,
  143.                                                          final EquationsMapper newMapper) {
  144.         return new GraggBulirschStoerStateInterpolator(newForward,
  145.                                                        newGlobalPreviousState, newGlobalCurrentState,
  146.                                                        newSoftPreviousState, newSoftCurrentState,
  147.                                                        newMapper, yMidDots, currentDegree - 4);
  148.     }

  150.     /** Compute the interpolation coefficients for dense output.
  151.      * @param mu degree of the interpolation polynomial
  152.      */
  153.     private void computeCoefficients(final int mu) {

  155.         final double[] y0Dot = getGlobalPreviousState().getCompleteDerivative();
  156.         final double[] y1Dot = getGlobalCurrentState().getCompleteDerivative();
  157.         final double[] y1    = getGlobalCurrentState().getCompleteState();

  159.         final double[] previousState = getGlobalPreviousState().getCompleteState();
  160.         final double h = getGlobalCurrentState().getTime() - getGlobalPreviousState().getTime();
  161.         for (int i = 0; i < previousState.length; ++i) {

  163.             final double yp0   = h * y0Dot[i];
  164.             final double yp1   = h * y1Dot[i];
  165.             final double ydiff = y1[i] - previousState[i];
  166.             final double aspl  = ydiff - yp1;
  167.             final double bspl  = yp0 - ydiff;

  169.             polynomials[0][i] = previousState[i];
  170.             polynomials[1][i] = ydiff;
  171.             polynomials[2][i] = aspl;
  172.             polynomials[3][i] = bspl;

  174.             if (mu < 0) {
  175.                 return;
  176.             }

  178.             // compute the remaining coefficients
  179.             final double ph0 = 0.5 * (previousState[i] + y1[i]) + 0.125 * (aspl + bspl);
  180.             polynomials[4][i] = 16 * (yMidDots[0][i] - ph0);

  182.             if (mu > 0) {
  183.                 final double ph1 = ydiff + 0.25 * (aspl - bspl);
  184.                 polynomials[5][i] = 16 * (yMidDots[1][i] - ph1);

  186.                 if (mu > 1) {
  187.                     final double ph2 = yp1 - yp0;
  188.                     polynomials[6][i] = 16 * (yMidDots[2][i] - ph2 + polynomials[4][i]);

  190.                     if (mu > 2) {
  191.                         final double ph3 = 6 * (bspl - aspl);
  192.                         polynomials[7][i] = 16 * (yMidDots[3][i] - ph3 + 3 * polynomials[5][i]);

  194.                         for (int j = 4; j <= mu; ++j) {
  195.                             final double fac1 = 0.5 * j * (j - 1);
  196.                             final double fac2 = 2 * fac1 * (j - 2) * (j - 3);
  197.                             polynomials[j+4][i] =
  198.                                             16 * (yMidDots[j][i] + fac1 * polynomials[j+2][i] - fac2 * polynomials[j][i]);
  199.                         }

  201.                     }
  202.                 }
  203.             }
  204.         }

  206.     }

  208.     /** Estimate interpolation error.
  209.      * @param scale scaling array
  210.      * @return estimate of the interpolation error
  211.      */
  212.     public double estimateError(final double[] scale) {
  213.         double error = 0;
  214.         if (currentDegree >= 5) {
  215.             for (int i = 0; i < scale.length; ++i) {
  216.                 final double e = polynomials[currentDegree][i] / scale[i];
  217.                 error += e * e;
  218.             }
  219.             error = FastMath.sqrt(error / scale.length) * errfac[currentDegree - 5];
  220.         }
  221.         return error;
  222.     }

  224.     /** {@inheritDoc} */
  225.     @Override
  226.     protected ODEStateAndDerivative computeInterpolatedStateAndDerivatives(final EquationsMapper mapper,
  227.                                                                            final double time, final double theta,
  228.                                                                            final double thetaH, final double oneMinusThetaH) {

  230.         final int dimension = mapper.getTotalDimension();

  232.         final double h             = thetaH / theta;
  233.         final double oneMinusTheta = 1.0 - theta;
  234.         final double theta05       = theta - 0.5;
  235.         final double tOmT          = theta * oneMinusTheta;
  236.         final double t4            = tOmT * tOmT;
  237.         final double t4Dot         = 2 * tOmT * (1 - 2 * theta);
  238.         final double dot1          = 1.0 / h;
  239.         final double dot2          = theta * (2 - 3 * theta) / h;
  240.         final double dot3          = ((3 * theta - 4) * theta + 1) / h;

  242.         final double[] interpolatedState       = new double[dimension];
  243.         final double[] interpolatedDerivatives = new double[dimension];
  244.         for (int i = 0; i < dimension; ++i) {

  246.             final double p0 = polynomials[0][i];
  247.             final double p1 = polynomials[1][i];
  248.             final double p2 = polynomials[2][i];
  249.             final double p3 = polynomials[3][i];
  250.             interpolatedState[i] = p0 + theta * (p1 + oneMinusTheta * (p2 * theta + p3 * oneMinusTheta));
  251.             interpolatedDerivatives[i] = dot1 * p1 + dot2 * p2 + dot3 * p3;

  253.             if (currentDegree > 3) {
  254.                 double cDot = 0;
  255.                 double c = polynomials[currentDegree][i];
  256.                 for (int j = currentDegree - 1; j > 3; --j) {
  257.                     final double d = 1.0 / (j - 3);
  258.                     cDot = d * (theta05 * cDot + c);
  259.                     c = polynomials[j][i] + c * d * theta05;
  260.                 }
  261.                 interpolatedState[i]       += t4 * c;
  262.                 interpolatedDerivatives[i] += (t4 * cDot + t4Dot * c) / h;
  263.             }

  265.         }

  267.         if (h == 0) {
  268.             // in this degenerated case, the previous computation leads to NaN for derivatives
  269.             // we fix this by using the derivatives at midpoint
  270.             System.arraycopy(yMidDots[1], 0, interpolatedDerivatives, 0, dimension);
  271.         }

  273.         return mapper.mapStateAndDerivative(time, interpolatedState, interpolatedDerivatives);

  275.     }

  277. }
Created with JaCoCo 0.8.12.202403310830