Friday, October 20, 2017

Runge-Kutta-Fehlberg method

'***********************************************************************
'*      Integrate a System of Ordinary Differential Equations By the   *
'*      Runge-Kutta-Fehlberg method (double precision)                 *
'* ------------------------------------------------------------------- *
'* REFERENCE:     H A Watts and L F Shampine,                          *
'*                Sandia Laboratories,                                 *
'*                Albuquerque, New Mexico.                             *
'* ------------------------------------------------------------------- *
'* SAMPLE RUN:                                                         *
'*                                                                     *
'* PROGRAM TRKF45                                                      *
'* Demonstrate the RKF45 ODE integrator.                               *
'*                                                                     *
'* TEST01                                                              *
'* Solve a scalar equation:                                            *
'*                                                                     *
'*  Y' = 0.25 * Y * ( 1 - Y / 20 )                                     *
'*                                                                     *
'*       T          Y       Y_Exact     Error                          *
'*                                                                     *
'*    0.00000    1.00000    1.00000    0.0000000                       *
'*    4.00000    2.50321    2.50322   -0.0000087                       *
'*    8.00000    5.60007    5.60009   -0.0000193                       *
'*   12.00000   10.27774   10.27773   -0.0000069                       *
'*   16.00000   14.83682   14.83683   -0.0000038                       *
'*   20.00000   17.73017   17.73017   -0.0000084                       *
'*                                                                     *
'* TEST02                                                              *
'* Solve a vector equation:                                            *
'*                                                                     *
'*  Y'(1) =   Y(2)                                                     *
'*  Y'(2) = - Y(1)                                                     *
'*                                                                     *
'*       T            Y1            Y2                                 *
'*                                                                     *
'*    0.00000      1.00000       0.00000                               *
'*    0.52360      0.86603      -0.50000                               *
'*    1.04720      0.50000      -0.86603                               *
'*    1.57080      0.00000      -1.00000                               *
'*    2.09440     -0.50000      -0.86603                               *
'*    2.61799     -0.86603      -0.50000                               *
'*    3.14159     -1.00000      -0.00000                               *
'*    3.66519     -0.86603       0.50000                               *
'*    4.18879     -0.50000       0.86603                               *
'*    4.71239     -0.00000       1.00001                               *
'*    5.23599      0.50000       0.86604                               *
'*    5.75959      0.86604       0.50001                               *
'*    6.28319      1.00002       0.00000                               *
'*                                                                     *
'* TEST03                                                              *
'* Solve a vector equation:                                            *
'*                                                                     *
'*  Y'(1) = Y(2)                                                       *
'*  Y'(2) = Y(3)                                                       *
'*  Y'(3) = Y(4)                                                       *
'*  Y'(4) = Y(5)                                                       *
'*  Y'(5) = (45 * Y(3) * Y(4) * Y(5) - 40 * Y(4)^3) / (9 * Y(3)^2)     *
'*                                                                     *
'*       T          Y1         Y2         Y3         Y4         Y5     *
'*                                                                     *
'*    0.00000    1.00000    1.00000    1.00000    1.00000    1.00000   *
'*    0.13636    1.14610    1.14609    1.14587    1.14068    1.05604   *
'*    0.27273    1.31354    1.31340    1.31128    1.28532    1.05248   *
'*    0.40909    1.50538    1.50460    1.49612    1.42333    0.95209   *
'*    0.54545    1.72508    1.72223    1.69844    1.53859    0.71111   *
'*    0.68182    1.97638    1.96840    1.91370    1.60897    0.28809   *
'*    0.81818    2.26328    2.24438    2.13400    1.60781   -0.33918   *
'*    0.95455    2.58984    2.55011    2.34770    1.50801   -1.15027   *
'*    1.09091    2.96003    2.88369    2.53985    1.28946   -2.06094   *
'*    1.22727    3.37739    3.24105    2.69376    0.94820   -2.92046   *
'*    1.36364    3.84475    3.61589    2.79372    0.50379   -3.54302   *
'*    1.50000    4.36396    4.00000    2.82843   -0.00000   -3.77124   *
'* ------------------------------------------------------------------- *
'*                                                                     *
'*                           Basic Release 1.1 By J-P Moreau, Paris.   *
'*                                      (www.jpmoreau.fr)              *
'*                                                                     *
'* Release 1.1: added test #3.                                         *
'***********************************************************************
' LIST OF SUBROUTINES:
' 400: User defined calculation of derivatives (examples #1 and #2)
' 500: Calculate exact solution (example #1)
' 600: Define example #1
' 700: Define example #2
' 800: Define example #3
' 1000: Fehlberg subroutine (one step)
' 1200: Emulation of Fortran routine Sign (with integers)
' 1210: Emulation of Fortran routine Sign (with real numbers)
' 1300: Emulation of Fortran routine Max (with real numbers)
' 1310: Emulation of Fortran routine Min (with real numbers)
' 2000: Runge-Kutta-Fehlberg method (double precision)
'------------------------------------------------------------
DefDbl A-H, O-Z
DefInt I-N

NEQ = 5              'Maximum number of equations
PI = 4# * Atn(1#)    'Constant PI
EPSILON = 2.22E-16   'Small number

Dim y(NEQ), yp(NEQ)
'auxiliary variables used by 400
Dim yy(NEQ), yyp(NEQ)
'work space used by successive calls to 1000 and 2000
Dim f1(NEQ), f2(NEQ), f3(NEQ), f4(NEQ), f5(NEQ)

  Cls
  Print
  Print " PROGRAM TRKF45"
  Print " Demonstrate the RKF45 ODE integrator."

  GoSub 600  'call test01
  Print
  INPUT "Any key to continue... ", R$
  GoSub 700  'call test02
  Print
  INPUT "Any key to continue... ", R$
  GoSub 800  'call test03
  Print
  INPUT "Any key to continue... ", R$

End 'of main program

'User defined system of diff. equations
400 'Subroutine f(tt, yy, yyp)
'--------------------------------------------------------------------
'
'  F evaluates the derivative for the ODE (TESTS #1 and #2).
'
'-------------------------------------------------------------------}
  If num = 1 Then
    yyp(1) = 0.25 * yy(1) * (1# - yy(1) / 20#)
  ElseIf num = 2 Then
    yyp(1) = yy(2):  yyp(2) = -yy(1)
  Else
    yyp(1) = yy(2)
    yyp(2) = yy(3)
    yyp(3) = yy(4)
    yyp(4) = yy(5)
    yyp(5) = (45# * yy(3) * yy(4) * yy(5) - 40# * yy(4) ^ 3) / (9# * yy(3) ^ 2)
  End If
Return

500 'function yexact(tt)
'--------------------------------------------------------------------
'
'  YEXACT evaluates the exact solution of the ODE (For TEST #1).
'
'--------------------------------------------------------------------
  yexact = 20# / (1# + 19# * Exp(-0.25 * tt))
Return


600 'Subroutine test01
'--------------------------------------------------------------------
'
'  TEST01 solves a scalar ODE in double precision.
'
'--------------------------------------------------------------------
  num = 1   'example #1
  neqn = 1  'one equation
  F$ = "####.#####"
  G$ = "####.#######"
  Print
  Print " TEST01"
  Print " Solve a scalar equation:"
  Print
  Print "  Y' = 0.25 * Y * ( 1 - Y / 20 )"
  Print

  abserr = 0.000001
  relerr = 0.000001

  iflag = 1

  tstart = 0#
  tstop = 20#

  nstep = 5

  tout = 0#
  y(1) = 1#

  Print "       T         Y        Y_Exact     Error"
  Print
  tt = tout: GoSub 500 'calculate yexact(tout)
  Print " ";: Print USING; F$; tout;
  Print " ";: Print USING; F$; y(1);
  Print " ";: Print USING; F$; yexact;
  Print " ";: Print USING; G$; y(1) - yexact

  For istep = 1 To nstep

    t = ((nstep - istep + 1) * tstart + (istep - 1) * tstop) / nstep
    tout = ((nstep - istep) * tstart + (istep) * tstop) / nstep

    GoSub 2000 'call rkfs (neqn,y,t,tout,relerr,abserr,iflag,yp,h,f1..f5,savr,save,
               'nfe,kop,init,jflag,kflag)

    tt = tout: GoSub 500 'calculate yexact(tout)
    Print " ";: Print USING; F$; tout;
    Print " ";: Print USING; F$; y(1);
    Print " ";: Print USING; F$; yexact;
    Print " ";: Print USING; G$; y(1) - yexact
 
  Next istep
Return

700 'Subroutine test02
'--------------------------------------------------------------------
'
'  TEST02 solves a vector ODE.
'
'--------------------------------------------------------------------
  num = 2     'Example #2
  neqn = 2    '2 equations

  Print
  Print " TEST02"
  Print " Solve a vector equation:"
  Print
  Print "  Y'(1) =   Y(2)"
  Print "  Y'(2) = - Y(1)"

  abserr = 0.000001
  relerr = 0.000001

  iflag = 1

  tstart = 0#
  tstop = 2# * PI

  nstep = 12

  tout = 0#

  y(1) = 1#
  y(2) = 0#

  Print
  Print "       T          Y1         Y2"
  Print
  Print " ";: Print USING; F$; tout;
  Print " ";: Print USING; F$; y(1);
  Print " ";: Print USING; F$; y(2)

  For istep = 1 To nstep

    t = ((nstep - istep + 1) * tstart + (istep - 1) * tstop) / nstep
    tout = ((nstep - istep) * tstart + (istep * tstop) / nstep)

    GoSub 2000 'call rkfs (neqn,y,t,tout,relerr,abserr,iflag,yp,h,f1..f5,savr,save,
               'nfe,kop,init,jflag,kflag)

    Print " ";: Print USING; F$; tout;
    Print " ";: Print USING; F$; y(1);
    Print " ";: Print USING; F$; y(2)

  Next istep
Return

800 'Subroutine test03
'--------------------------------------------------------------------
'
'  TEST02 solves a vector ODE.
'
'--------------------------------------------------------------------
  num = 3     'Example #3
  neqn = 5    '5 equations

  Print
  Print " TEST03"
  Print " Solve a vector equation:"
  Print
  Print "  Y'(1) = Y(2)"
  Print "  Y'(2) = Y(3)"
  Print "  Y'(3) = Y(4)"
  Print "  Y'(4) = Y(5)"
  Print "  Y'(5) = (45 * Y(3) * Y(4) * Y(5) - 40 * Y(4)^3) / (9 * Y(3)^2)"

  abserr = 0.000001
  relerr = 0.000001

  iflag = 1

  tstart = 0#
  tstop = 1.5

  nstep = 11

  tout = 0#

  y(1) = 1#
  y(2) = 1#
  y(3) = 1#
  y(4) = 1#
  y(5) = 1#

  Print
  Print "       T          Y1         Y2         Y3         Y4         Y5"
  Print
  Print " ";: Print USING; F$; tout;
  Print " ";: Print USING; F$; y(1);
  Print " ";: Print USING; F$; y(2);
  Print " ";: Print USING; F$; y(3);
  Print " ";: Print USING; F$; y(4);
  Print " ";: Print USING; F$; y(5)

  For istep = 1 To nstep

    t = ((nstep - istep + 1) * tstart + (istep - 1) * tstop) / nstep
    tout = ((nstep - istep) * tstart + (istep * tstop) / nstep)

    GoSub 2000 'call rkfs (neqn,y,t,tout,relerr,abserr,iflag,yp,h,f1..f5,savr,save,
               'nfe,kop,init,jflag,kflag)

    Print " ";: Print USING; F$; tout;
    Print " ";: Print USING; F$; y(1);
    Print " ";: Print USING; F$; y(2);
    Print " ";: Print USING; F$; y(3);
    Print " ";: Print USING; F$; y(4);
    Print " ";: Print USING; F$; y(5)

  Next istep
Return

'************************************************************************
'* Collection of Basic subroutines to Integrate a System of Ordinary    *
'* Differential Equations By the Runge-Kutta-Fehlberg method (in double *
'* precision).                                                          *
'************************************************************************

1000 'Subroutine fehl(neqn, y, t, h, yp, f1, f2, f3, f4, f5, s)
'************************************************************************
'
'  FEHL takes one Fehlberg fourth-fifth order step (double precision).
'
'  Discussion:
'
'    FEHL integrates a system of NEQN first order ordinary differential
'    equations of the form
'      dY(i)/dT = F(T,Y(1),---,Y(NEQN))
'    where the initial values Y and the initial derivatives
'    YP are specified at the starting point T.
'
'    FEHL advances the solution over the fixed step H and returns
'    the fifth order (sixth order accurate locally) solution
'    approximation at T+H in array S.
'
'    The formulas have been grouped to control loss of significance.
'    FEHL should be called with an H not smaller than 13 units of
'    roundoff in T so that the various independent arguments can be
'    distinguished.
'
'  Author:
'
'    H A Watts and L F Shampine,
'    Sandia Laboratories,
'    Albuquerque, New Mexico.
'
'    RKF45 is primarily designed to solve non-stiff and mildly stiff
'    differential equations when derivative evaluations are inexpensive.
'
'  Parameters:
'
'    Input, external F, a subroutine of the form
'      Procedure f(t:double; y:VEC; var yp:VEC);
'    to evaluate the derivatives.
'      YP(I) = dY(I) / dT;
'
'    Input, integer NEQN, the number of equations to be integrated.
'
'    Input, double Y(NEQN), the current value of the dependent variable.
'
'    Input, double T, the current value of the independent variable.
'
'    Input, double H, the step size to take.
'
'    Input, double YP(NEQN), the current value of the derivative of the
'    dependent variable.
'
'    Output, double F1(NEQN), double F2(NEQN), double F3(NEQN), double F4(NEQN),
'    double F5(NEQN) are arrays of dimension NEQN which are needed for
'    internal storage.
'
'    Output, double S(NEQN), the computed estimate of the solution at T+H.
'******************************************************************************}

  ch = h / 4#

  For i = 1 To neqn
    f5(i) = y(i) + ch * yp(i)
  Next i

  tt = t + ch:
  For i = 1 To neqn
    yy(i) = f5(i)
  Next i
  GoSub 400  'call f(t+ch, f5, f1)
  For i = 1 To neqn
    f1(i) = yyp(i)
  Next i

  ch = 3# * h / 32#

  For i = 1 To neqn
    f5(i) = y(i) + ch * (yp(i) + 3# * f1(i))
  Next i

  tt = t + 3# * h / 8#
  For i = 1 To neqn
    yy(i) = f5(i)
  Next i
  GoSub 400  'call f(t+3#*h/8#, f5, f2)
  For i = 1 To neqn
    f2(i) = yyp(i)
  Next i

  ch = h / 2197#

  For i = 1 To neqn
    f5(i) = y(i) + ch * (1932# * yp(i) + (7296# * f2(i) - 7200# * f1(i)))
  Next i

  tt = t + 12# * h / 13#
  For i = 1 To neqn
    yy(i) = f5(i)
  Next i
  GoSub 400  'call f(t+12#*h/13#, f5, f3)
  For i = 1 To neqn
    f3(i) = yyp(i)
  Next i

  ch = h / 4104#

  For i = 1 To neqn
    f5(i) = y(i) + ch * ((8341# * yp(i) - 845# * f3(i)) + (29440# * f2(i) - 32832# * f1(i)))
  Next i

  tt = t + h
  For i = 1 To neqn
    yy(i) = f5(i)
  Next i
  GoSub 400:  'call f(t+h, f5, f4)
  For i = 1 To neqn
    f4(i) = yyp(i)
  Next i

  ch = h / 20520#

  For i = 1 To neqn
    tmp = (41040# * f1(i) - 28352# * f2(i))
    f1(i) = y(i) + ch * ((-6080# * yp(i) + (9295# * f3(i) - 5643# * f4(i))) + tmp)
  Next i

  tt = t + h / 2#
  For i = 1 To neqn
    yy(i) = f1(i)
  Next i
  GoSub 400  'call f(t+h/2#, f1, f5)
  For i = 1 To neqn
    f5(i) = yyp(i)
  Next i

' Ready to compute the approximate solution at T+H.

  ch = h / 7618050#

  For i = 1 To neqn
    tmp = (3953664# * f2(i) + 277020# * f5(i))
    f1(i) = y(i) + ch * ((902880# * yp(i) + (3855735# * f3(i) - 1371249# * f4(i))) + tmp)
  Next i

Return

'Emulation of Fortran Intrisic Functions
1200 'Function ISign(ia,ib)
  If ib < 0 Then
    ISign = -Abs(ia)
  Else
    ISign = Abs(ia)
  End If
Return

1210 'Function Sign(a,b)
  If B < 0 Then
    Sign = -Abs(a)
  Else
    Sign = Abs(a)
  End If
Return

1300 'Function XMax(a,b)
  If a >= B Then
    XMax = a
  Else
    XMax = B
  End If
Return

1310 'Function XMin(a,b)
  If a <= B Then
    XMIN = a
  Else
    XMIN = B
  End If
Return

2000 'Subroutine rkfs(neqn, y, t, tout, relerr, abserr, iflag, yp, h, f1, f2, f3, f4,f5,
     '                savr, save, nfe, kop, init, jflag, kflag)
'***************************************************************************************
'
'  RKFS implements the Runge-Kutta-Fehlberg method (double precision).
'
'  Discussion:
'
'    RKFS integrates a system of first order ordinary differential
'    equations as described in the comments for RKF45.
'
'    The arrays yp, f1, f2, f3, f4, and f5 (of dimension neqn) and
'    the variables h, savre, savae, nfe, kop, init, jflag and kflag are used
'    internally by the code and appear in the call list to eliminate
'    local retention of variables between calls.  Accordingly, they
'    should not be altered.  Items of possible interest are
'
'      YP  - the derivative of the solution vector at T;
'      H   - an appropriate stepsize to be used for the next step;
'      NFE - the number of derivative function evaluations.
'
'    The expense is controlled by restricting the number
'    of function evaluations to be approximately MAXNFE.
'    As set, this corresponds to about 500 steps.
'
'    REMIN is the minimum acceptable value of RELERR.  Attempts
'    to obtain higher accuracy with this subroutine are usually
'    very expensive and often unsuccessful.
'***************************************************************************************
'Labels:  25,40,45,50,60,65,80,100,200,260
  remin = 0.000000000001
  maxnfe = 3000

' ihfaild, ioutput: boolean (0 or 1)

'  Check the input parameters

  eps = EPSILON

  If neqn < 1 Then
    iflag = 8
    Return
  End If

  If relerr < 0# Then
    iflag = 8
    Return
  End If

  If abserr < 0# Then
    iflag = 8
    Return
  End If

  mflag = Abs(iflag)

  If Abs(iflag) < 1 Or Abs(iflag) > 8 Then
    iflag = 8
    Return
  End If

'  Is this the first call?

  If mflag = 1 Then GoTo 50

'  Check continuation possibilities

  If t = tout And kflag <> 3 Then
    iflag = 8
    Return
  End If

  If mflag <> 2 Then GoTo 25

'  iflag = +2 or -2

  If kflag = 3 Then GoTo 45
  If init = 0 Then GoTo 45
  If kflag = 4 Then GoTo 40

  If kflag = 5 And abserr = 0# Then End

  If kflag = 6 And relerr <= savr And abserr <= save Then End

  GoTo 50

'  iflag = 3,4,5,6,7 or 8

25 If iflag = 3 Then GoTo 45
   If iflag = 4 Then GoTo 40
   If iflag = 5 And abserr > 0# Then GoTo 45

'  Integration cannot be continued since user did not respond to
'  the instructions pertaining to iflag=5,6,7 or 8.

  End

'  Reset function evaluation counter

40 nfe = 0
  If mflag = 2 Then GoTo 50

'  Reset flag value from previous call

45 iflag = jflag

  If kflag = 3 Then mflag = Abs(iflag)

'  Save input iflag and set continuation flag for subsequent input checking

50 jflag = iflag
   kflag = 0

'  Save relerr and abserr for checking input on subsequent calls

  savr = relerr
  save = abserr

'  Restrict relative error tolerance to be at least as large as
'  2*eps+remin to avoid limiting precision difficulties arising
'  from impossible accuracy requests.

  rer = 2# * EPSILON + remin

'  The relative error tolerance is too small

  If relerr < rer Then
    relerr = rer
    iflag = 3
    kflag = 3
    Return
  End If

  dt = tout - t

  If mflag = 1 Then GoTo 60
  If init = 0 Then GoTo 65
  GoTo 80

'  Initialization:
'    set initialization completion indicator, init
'    set indicator for too many output points, kop
'    evaluate initial derivatives
'    set counter for function evaluations, nfe
'    evaluate initial derivatives
'    set counter for function evaluations, nfe
'    estimate starting stepsize.

60 init = 0
  kop = 0
  a = t
  For i = 1 To neqn
    yy(i) = y(i)
  Next i
  GoSub 400  'call f(a, y, yp)
  For i = 1 To neqn
    yp(i) = yyp(i)
  Next i
  nfe = 1

  If t = tout Then
    iflag = 2
    Return
  End If

65 init = 1
  h = Abs(dt)
  toln = 0#
  For k = 1 To neqn
    tol = relerr * Abs(y(k)) + abserr
    If tol > 0# Then
      toln = tol
      ypk = Abs(yp(k))
      If ypk * h ^ 5 > tol Then
        h = (tol / ypk) ^ 0.2
      End If
    End If
  Next k

  If toln <= 0# Then h = 0#

  'h = XMax(h, 26# * eps * XMax(abs(t), abs(dt)))
  If Abs(t) > Abs(dt) Then
    tmp = Abs(t)
  Else
    tmp = Abs(dt)
  End If
  If h < 26# * eps * tmp Then
    h = 26# * eps * tmp
  End If

  ia = 2: ib = iflag: GoSub 1200
  jflag = ISign

'  Set stepsize for integration in the direction from T to TOUT

80 a = h: B = dt: GoSub 1210
   h = Sign

'  Test to see if RKF45 is being severely impacted by too many output points.

  If Abs(h) >= 2# * Abs(dt) Then kop = kop + 1

'  Unnecessary frequency of output

  If kop = 100 Then
    kop = 0
    iflag = 7
    Return
  End If

'  If too close to output point, extrapolate and return

  If Abs(dt) <= 26# * eps * Abs(t) Then
    For i = 1 To neqn
      y(i) = y(i) + dt * yp(i)
    Next i
    a = tout

    For i = 1 To neqn
      yy(i) = y(i)
    Next i
    GoSub 400  'call f(a, y, yp)
    For i = 1 To neqn
      yp(i) = yyp(i)
    Next i
    nfe = nfe + 1

    t = tout
    iflag = 2
    Return
  End If

'  Initialize output point indicator

  ioutput = 0

'  To avoid premature underflow in the error tolerance function,
'  scale the error tolerances.

  scale = 2# / relerr
  ae = scale * abserr

'  Step by step integration

100:  ihfaild = 0

'  Set smallest allowable stepsize

  hmin = 26# * eps * Abs(t)

'  Adjust stepsize if necessary to hit the output point.
'  Look ahead two steps to avoid drastic changes in the stepsize and
'  thus lessen the impact of output points on the code.

  dt = tout - t
  If Abs(dt) >= 2# * Abs(h) Then GoTo 200

'  The next successful step will complete the integration to the output point.

  If Abs(dt) <= Abs(h) Then
    ioutput = 1
    h = dt
    GoTo 200
  End If

  h = 0.5 * dt     'reduce step

'     Core integrator for taking a single step
'
'     The tolerances have been scaled to avoid premature underflow in
'     computing the error tolerance function ET.
'     To avoid problems with zero crossings, relative error is measured
'     using the average of the magnitudes of the solution at the
'     beginning and end of a step.
'     The error estimate formula has been grouped to control loss of
'     significance.
'
'     To distinguish the various arguments, H is not permitted
'     to become smaller than 26 units of roundoff in T.
'     Practical limits on the change in the stepsize are enforced to
'     smooth the stepsize selection process and to avoid excessive
'     chattering on problems having discontinuities.
'     To prevent unnecessary failures, the code uses 9/10 the stepsize
'     it estimates will succeed.
'
'     After a step failure, the stepsize is not allowed to increase for
'     the next attempted step.  This makes the code more efficient on
'     problems having discontinuities and more effective in general
'     since local extrapolation is being used and extra caution seems
'     warranted.
'
'     Test number of derivative function evaluations.
'     If okay, try to advance the integration from T to T+H.

' Too many function calls.

200 If nfe > maxnfe Then
    iflag = 4
    kflag = 4
    Return
  End If

'  Advance an approximate solution over one step of length H.

  GoSub 1000  'call fehl(neqn, y, t, h, yp, f1, f2, f3, f4, f5, f1)
  nfe = nfe + 5

'  Compute and test allowable tolerances versus local error estimates
'  and remove scaling of tolerances.  Note that relative error is
'  measured with respect to the average of the magnitudes of the
'  solution at the beginning and end of the step.

  eeoet = 0#

  For k = 1 To neqn

    et = Abs(y(k)) + Abs(f1(k)) + ae

    If et <= 0# Then
      iflag = 5
      Return
    End If

    tmp = (22528# * f2(k) - 27360# * f5(k))
    ee = Abs((-2090# * yp(k) + (21970# * f3(k) - 15048# * f4(k))) + tmp)

    'eeoet = XMax(eeoet, ee/et)
    a = eecet: B = ee / et: GoSub 1300: eecet = XMax

  Next k

  esttol = ABS(h) * eeoet * scale / 752400#

  If esttol <= 1# Then GoTo 260

'  Unsuccessful step.  Reduce the stepsize, try again.
'  The decrease is limited to a factor of 1/10.

  ihfaild = 1
  ioutput = 0

  If esttol < 59049# Then
    s = 0.9 / (esttol ^ 0.2)
  Else
    s = 0.1
  End If

  h = s * h

  If Abs(h) < hmin Then
    iflag = 6
    kflag = 6
    Return
  Else
    GoTo 200
  End If

'  Successful step. Store solution at T+H and evaluate derivatives there.

260  t = t + h
  For i = 1 To neqn
    y(i) = f1(i)
  Next i
  a = t

  For i = 1 To neqn
    yy(i) = y(i)
  Next i
  GoSub 400  'call f(a, y, yp)
  For i = 1 To neqn
    yp(i) = yyp(i)
  Next i
  nfe = nfe + 1

'  Choose next stepsize.  The increase is limited to a factor of 5.
'  If step failure has just occurred, next stepsize is not allowed to increase.

  If esttol > 0.0001889568 Then
    s = 0.9 / esttol ^ 0.2
  Else
    s = 5#
  End If

  If ihfaild <> 0 Then
    's = XMin(s, 1#)
    a = s: B = 1#: GoSub 1310: s = XMIN
  End If

  'h = Sign(XMax(s * abs(h), hmin), h)
  a = s * Abs(h): B = hmin: GoSub 1300
  a = XMax: B = h: GoSub 1210: h = Sign

'  End of core integrator

'  Should we take another step?

  If ioutput <> 0 Then
    t = tout
    iflag = 2
  End If

  If iflag > 0 Then GoTo 100

' Integration successfully completed

'  ne-step mode
  iflag = -2

Return

'end of file trkf45.bas

No comments:

Post a Comment