graggbulirschstoerintegrator.java

Apache的common math数学软件包

    for (int k = 0; k < size; ++k) {
      coeff[k] = (k > 0) ? new double[k] : null;
      for (int l = 0; l < k; ++l) {
        double ratio = ((double) sequence[k]) / sequence[k-l-1];
        coeff[k][l] = 1.0 / (ratio * ratio - 1.0);
      }
    }

  }

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

   * @param useInterpolationError if true, interpolation error is used
   * for stepsize control
   * @param mudif interpolation order control parameter (the parameter
   * is reset to default if <= 0 or >= 7)
   */
  public void setInterpolationControl(boolean useInterpolationError,
                                      int mudif) {

    this.useInterpolationError = useInterpolationError;

    if ((mudif <= 0) || (mudif >= 7)) {
      this.mudif = 4;
    } else {
      this.mudif = mudif;
    }

  }

  /** Get the name of the method.
   * @return name of the method
   */
  public String getName() {
    return methodName;
  }

  /** Update scaling array.
   * @param y1 first state vector to use for scaling
   * @param y2 second state vector to use for scaling
   * @param scale scaling array to update
   */
  private void rescale(double[] y1, double[] y2, double[] scale) {
    if (vecAbsoluteTolerance == null) {
      for (int i = 0; i < scale.length; ++i) {
        double yi = Math.max(Math.abs(y1[i]), Math.abs(y2[i]));
        scale[i] = scalAbsoluteTolerance + scalRelativeTolerance * yi;
      }
    } else {
      for (int i = 0; i < scale.length; ++i) {
        double yi = Math.max(Math.abs(y1[i]), Math.abs(y2[i]));
        scale[i] = vecAbsoluteTolerance[i] + vecRelativeTolerance[i] * yi;
      }
    }
  }

  /** Perform integration over one step using substeps of a modified
   * midpoint method.
   * @param equations differential equations to integrate
   * @param t0 initial time
   * @param y0 initial value of the state vector at t0
   * @param step global step
   * @param k iteration number (from 0 to sequence.length - 1)
   * @param scale scaling array
   * @param f placeholder where to put the state vector derivatives at each substep
   *          (element 0 already contains initial derivative)
   * @param yMiddle placeholder where to put the state vector at the middle of the step
   * @param yEnd placeholder where to put the state vector at the end
   * @param yTmp placeholder for one state vector
   * @return true if computation was done properly,
   *         false if stability check failed before end of computation
   * @throws DerivativeException this exception is propagated to the caller if the
   * underlying user function triggers one
   */
  private boolean tryStep(FirstOrderDifferentialEquations equations,
                          double t0, double[] y0, double step, int k,
                          double[] scale, double[][] f,
                          double[] yMiddle, double[] yEnd,
                          double[] yTmp)
    throws DerivativeException {

    int    n        = sequence[k];
    double subStep  = step / n;
    double subStep2 = 2 * subStep;

    // first substep
    double t = t0 + subStep;
    for (int i = 0; i < y0.length; ++i) {
      yTmp[i] = y0[i];
      yEnd[i] = y0[i] + subStep * f[0][i];
    }
    equations.computeDerivatives(t, yEnd, f[1]);

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

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

      t += subStep;
      for (int i = 0; i < y0.length; ++i) {
        double middle = yEnd[i];
        yEnd[i]       = yTmp[i] + subStep2 * f[j][i];
        yTmp[i]       = middle;
      }

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

      // stability check
      if (performTest && (j <= maxChecks) && (k < maxIter)) {
        double initialNorm = 0.0;
        for (int l = 0; l < y0.length; ++l) {
          double ratio = f[0][l] / scale[l];
          initialNorm += ratio * ratio;
        }
        double deltaNorm = 0.0;
        for (int l = 0; l < y0.length; ++l) {
          double ratio = (f[j+1][l] - f[0][l]) / scale[l];
          deltaNorm += ratio * ratio;
        }
        if (deltaNorm > 4 * Math.max(1.0e-15, initialNorm)) {
          return false;
        }
      }

    }

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

    return true;

  }

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

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

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

  /** Integrate the differential equations up to the given time.
   * <p>This method solves an Initial Value Problem (IVP).</p>
   * <p>Since this method stores some internal state variables made
   * available in its public interface during integration ({@link
   * #getCurrentSignedStepsize()}), it is <em>not</em> thread-safe.</p>
   * @param equations differential equations to integrate
   * @param t0 initial time
   * @param y0 initial value of the state vector at t0
   * @param t target time for the integration
   * (can be set to a value smaller than <code>t0</code> for backward integration)
   * @param y placeholder where to put the state vector at each successful
   *  step (and hence at the end of integration), can be the same object as y0
   * @throws IntegratorException if the integrator cannot perform integration
   * @throws DerivativeException this exception is propagated to the caller if
   * the underlying user function triggers one
   */
  public void integrate(FirstOrderDifferentialEquations equations,
                        double t0, double[] y0, double t, double[] y)
  throws DerivativeException, IntegratorException {

    sanityChecks(equations, t0, y0, t, y);
    boolean forward = (t > t0);

    // create some internal working arrays
    double[] yDot0   = new double[y0.length];
    double[] y1      = new double[y0.length];
    double[] yTmp    = new double[y0.length];
    double[] yTmpDot = new double[y0.length];

    double[][] diagonal = new double[sequence.length-1][];
    double[][] y1Diag = new double[sequence.length-1][];
    for (int k = 0; k < sequence.length-1; ++k) {
      diagonal[k] = new double[y0.length];
      y1Diag[k] = new double[y0.length];
    }

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

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

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

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

    }

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

    double[] yDot1      = null;
    double[][] yMidDots = null;
    if (denseOutput) {
      yDot1    = new double[y0.length];
      yMidDots = new double[1 + 2 * sequence.length][];
      for (int j = 0; j < yMidDots.length; ++j) {
        yMidDots[j] = new double[y0.length];
      }
    } else {
      yMidDots    = new double[1][];
      yMidDots[0] = new double[y0.length];
    }

    // initial scaling
    double[] scale = new double[y0.length];
    rescale(y, y, scale);

    // initial order selection
    double tol =
        (vecRelativeTolerance == null) ? scalRelativeTolerance : vecRelativeTolerance[0];
    double log10R = Math.log(Math.max(1.0e-10, tol)) / Math.log(10.0);
    int targetIter = Math.max(1,
                              Math.min(sequence.length - 2,
                                       (int) Math.floor(0.5 - 0.6 * log10R)));
    // set up an interpolator sharing the integrator arrays
    AbstractStepInterpolator interpolator = null;
    if (denseOutput || (! switchesHandler.isEmpty())) {
      interpolator = new GraggBulirschStoerStepInterpolator(y, yDot0,
                                                            y1, yDot1,
                                                            yMidDots, forward);
    } else {
      interpolator = new DummyStepInterpolator(y, forward);
    }
    interpolator.storeTime(t0);

    stepStart = t0;
    double  hNew             = 0;
    double  maxError         = Double.MAX_VALUE;
    boolean previousRejected = false;
    boolean firstTime        = true;
    boolean newStep          = true;
    boolean lastStep         = false;
    boolean firstStepAlreadyComputed = false;
    handler.reset();
    costPerTimeUnit[0] = 0;
    while (! lastStep) {

      double error;
      boolean reject = false;

      if (newStep) {

        interpolator.shift();

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

        if (firstTime) {

          hNew = initializeStep(equations, forward,
                                2 * targetIter + 1, scale,
                                stepStart, y, yDot0, yTmp, yTmpDot);

          if (! forward) {
            hNew = -hNew;
          }

        }

        newStep = false;

      }

      stepSize = hNew;

      // step adjustment near bounds
      if ((forward && (stepStart + stepSize > t)) ||
          ((! forward) && (stepStart + stepSize < t))) {
        stepSize = t - stepStart;
      }
      double nextT = stepStart + stepSize;
      lastStep = forward ? (nextT >= t) : (nextT <= t);

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

        ++k;

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

          // the stability check failed, we reduce the global step
          hNew   = Math.abs(filterStep(stepSize * stabilityReduction, false));
          reject = true;
          loop   = false;

        } else {

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

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

            // estimate the error at the end of the step.
            error = 0;
            for (int j = 0; j < y0.length; ++j) {
              double e = Math.abs(y1[j] - y1Diag[0][j]) / scale[j];
              error += e * e;
            }
            error = Math.sqrt(error / y0.length);