home *** CD-ROM | disk | FTP | other *** search
/ OS/2 Shareware BBS: 10 Tools / 10-Tools.zip / numana01.zip / SRC / INTEGRAT.MOD < prev    next >
Text File  |  1996-08-16  |  20KB  |  569 lines
  1. IMPLEMENTATION MODULE Integration;
  2.  
  3.         (********************************************************)
  4.         (*                                                      *)
  5.         (*              Solving differential equations          *)
  6.         (*                                                      *)
  7.         (*  Programmer:         P. Moylan                       *)
  8.         (*  Last edited:        16 August 1996                  *)
  9.         (*  Status:             Working                         *)
  10.         (*                                                      *)
  11.         (*      Original source: many of the procedures in      *)
  12.         (*      this module are adaptations of Fortran code     *)
  13.         (*      published in the book "Numerical Recipes"       *)
  14.         (*      by Press, Flannery, Teutolsky, and Vetterling.  *)
  15.         (*      The implementation details are a little         *)
  16.         (*      different from their code, but the underlying   *)
  17.         (*      algorithms are pretty much the same.            *)
  18.         (*                                                      *)
  19.         (*      Portability problem: I've had to use an XDS     *)
  20.         (*      language extension (open arrays) here; I        *)
  21.         (*      haven't yet figured out how to do the job       *)
  22.         (*      in ISO standard Modula-2.                       *)
  23.         (*                                                      *)
  24.         (********************************************************)
  25.  
  26. FROM SYSTEM IMPORT
  27.     (* type *)  ADDRESS;
  28.  
  29. FROM Storage IMPORT
  30.     (* proc *)  ALLOCATE, DEALLOCATE;
  31.  
  32. FROM MiscM2 IMPORT
  33.     (* proc *)  Power;
  34.  
  35. FROM Vec IMPORT
  36.     (* type *)  VectorPtr,
  37.     (* proc *)  NewVector, DisposeVector, Add, Sub, ScalarMul, Copy;
  38.  
  39. FROM Mat IMPORT
  40.     (* type *)  ArrayPtr,
  41.     (* proc *)  NewArray, DisposeArray;
  42.  
  43. <* m2extensions+ *>
  44.  
  45. (************************************************************************)
  46.  
  47. TYPE
  48.     DiffEqn = POINTER TO
  49.                     RECORD
  50.                         order: CARDINAL;
  51.                         Derivs: RHSproc;
  52.                         t: LONGREAL;
  53.                         stepmin, step, stepmax: LONGREAL;
  54.                         errbound: LONGREAL;
  55.                         y, dydt, yscale: VectorPtr;
  56.                     END (*RECORD*);
  57.  
  58.     StepDriver = PROCEDURE (DiffEqn);
  59.  
  60. (************************************************************************)
  61. (*              FOURTH ORDER RUNGE-KUTTA, FIXED STEP SIZE               *)
  62. (************************************************************************)
  63.  
  64. PROCEDURE RK4step (DE: DiffEqn;  y, dydt: VectorPtr;
  65.                                 t, h: LONGREAL;
  66.                                 yout: VectorPtr);
  67.  
  68.     (* One step of a 4th order Runge-Kutta integration. *)
  69.  
  70.     VAR N: CARDINAL;
  71.         tmid, hh: LONGREAL;  dym, dyt, yt, temp: VectorPtr;
  72.  
  73.     BEGIN
  74.         N := DE^.order;
  75.         dym := NewVector (N);
  76.         dyt := NewVector (N);
  77.         yt := NewVector (N);
  78.         temp := NewVector (N);
  79.  
  80.         hh := 0.5*h;  tmid := t + hh;
  81.  
  82.         (* Let yt be the first estimate of y at the midpoint. *)
  83.  
  84.         ScalarMul (hh, dydt^, N, temp^);
  85.         Add (y^, temp^, N, yt^);
  86.  
  87.         (* Use this to get an estimate dyt of the derivative at the     *)
  88.         (* midpoint, and from that get an improved yt.                  *)
  89.  
  90.         DE^.Derivs (tmid, yt^, dyt^);
  91.         ScalarMul (hh, dyt^, N, temp^);
  92.         Add (y^, temp^, N, yt^);
  93.  
  94.         (* From the updated yt, get another estimate dym of the         *)
  95.         (* derivative of the midpoint; and from this estimate yt as     *)
  96.         (* the endpoint value of y.                                     *)
  97.  
  98.         DE^.Derivs (tmid, yt^, dym^);
  99.         ScalarMul (h, dym^, N, temp^);
  100.         Add (y^, temp^, N, yt^);
  101.  
  102.         (* Save the sum of dyt and dym, then let dyt be an estimate     *)
  103.         (* of the derivative of the endpoint.                           *)
  104.  
  105.         Add (dyt^, dym^, N, dym^);
  106.         DE^.Derivs (t+h, yt^, dyt^);
  107.  
  108.         (* We now have three estimates of the derivative: dydt, dyt,    *)
  109.         (* and dym.  (Actually, there are four, because by now dym is   *)
  110.         (* the sum of two earlier estimates.)  Use a weighted sum of    *)
  111.         (* these to get the new y.                                      *)
  112.  
  113.         ScalarMul (2.0, dym^, N, temp^);
  114.         Add (dydt^, temp^, N, temp^);
  115.         Add (dyt^, temp^, N, temp^);
  116.         ScalarMul (h/6.0, temp^, N, temp^);
  117.         Add (y^, temp^, N, yout^);
  118.  
  119.         DisposeVector (temp, N);
  120.         DisposeVector (dym, N);
  121.         DisposeVector (dyt, N);
  122.         DisposeVector (yt, N);
  123.  
  124.     END RK4step;
  125.  
  126. (************************************************************************)
  127. (*              FIFTH ORDER RUNGE-KUTTA, VARIABLE STEP SIZE             *)
  128. (************************************************************************)
  129.  
  130. PROCEDURE RK5step (DE: DiffEqn);
  131.  
  132.     (* One step of a Runge-Kutta method with variable step size.        *)
  133.     (* Updates DE^.t, DE^.y, DE^.step.                                  *)
  134.  
  135.     CONST pgrow = -0.2;  pshrink = -0.25;
  136.           safety = 0.9;  errcon = 6.0E-4;
  137.           (* errcon is Power(4.0/safety, 1.0/pgrow) *)
  138.  
  139.     VAR ynew, temp2: VectorPtr;
  140.         tmid, h, hh, errmax, test: LONGREAL;
  141.         i, N: CARDINAL;  satisfied: BOOLEAN;
  142.  
  143.     BEGIN
  144.         N := DE^.order;
  145.         ynew := NewVector (N);
  146.         temp2 := NewVector (N);
  147.         h := DE^.step;
  148.  
  149.         REPEAT
  150.             (* Take two half steps. *)
  151.  
  152.             hh := 0.5*h;
  153.             RK4step (DE, DE^.y, DE^.dydt, DE^.t, hh, ynew);
  154.             tmid := DE^.t + hh;
  155.             DE^.Derivs (tmid, ynew^, temp2^);
  156.             RK4step (DE, ynew, temp2, tmid, hh, ynew);
  157.  
  158.             (* For comparison, take one full step. *)
  159.  
  160.             RK4step (DE, DE^.y, DE^.dydt, DE^.t, h, temp2);
  161.  
  162.             (* Evaluate the error. *)
  163.  
  164.             Sub (ynew^, temp2^, N, temp2^);
  165.             errmax := 0.0;
  166.             FOR i := 0 TO N-1 DO
  167.                 test := ABS(temp2^[i]/DE^.yscale^[i]);
  168.                 IF test > errmax THEN errmax := test END(*IF*);
  169.             END (*FOR*);
  170.             errmax := errmax/DE^.errbound;
  171.             IF errmax <= 1.0 THEN
  172.  
  173.                 (* Error acceptable.  Estimate new value for    *)
  174.                 (* step size, and exit loop.                    *)
  175.  
  176.                 IF errmax > errcon THEN
  177.                     DE^.step := safety * h * Power(errmax,pgrow);
  178.                 ELSE
  179.                     DE^.step := 4.0*h;
  180.                 END (*IF*);
  181.                 IF DE^.step > DE^.stepmax THEN
  182.                     DE^.step := DE^.stepmax;
  183.                 END (*IF*);
  184.                 satisfied := TRUE;
  185.  
  186.             ELSIF h <= DE^.stepmin THEN
  187.  
  188.                 DE^.step := DE^.stepmin;
  189.                 satisfied := TRUE;
  190.  
  191.             ELSE
  192.  
  193.                 (* Error too large, reduce step size. *)
  194.  
  195.                 h := safety * h * Power(errmax,pshrink);
  196.                 IF h < DE^.stepmin THEN
  197.                     h := DE^.stepmin;
  198.                 END (*IF*);
  199.                 satisfied := FALSE;
  200.  
  201.             END (*IF*);
  202.  
  203.         UNTIL satisfied;
  204.  
  205.         (* Final correction to the answer, to get fifth-order accuracy. *)
  206.  
  207.         ScalarMul (1.0/15.0, temp2^, N, temp2^);
  208.         Add (ynew^, temp2^, N, DE^.y^);
  209.         DE^.t := DE^.t + h;
  210.  
  211.         DisposeVector (ynew, N);  DisposeVector (temp2, N);
  212.  
  213.     END RK5step;
  214.  
  215. (************************************************************************)
  216. (*              MODIFIED MIDPOINT METHOD, FIXED STEP SIZE               *)
  217. (************************************************************************)
  218.  
  219. PROCEDURE MMid (DE: DiffEqn;  ystart: VectorPtr;
  220.                         htotal: LONGREAL;  NumberOfSteps: CARDINAL;
  221.                         yout: VectorPtr);
  222.  
  223.     (* Modified midpoint method of solving a DE.  The initial value for *)
  224.     (* y is ystart^, and the initial values for t, and dy/dt are        *)
  225.     (* specified as part of the first parameter.  By the time we've     *)
  226.     (* returned yout^ is the solution at a time htotal later.  The      *)
  227.     (* number of substeps to be used is specified as NumberOfSteps.     *)
  228.  
  229.     VAR ym, yn, temp: VectorPtr;
  230.         h, t: LONGREAL;
  231.         j, N: CARDINAL;
  232.  
  233.     BEGIN
  234.         N := DE^.order;
  235.         ym := NewVector (N);
  236.         yn := NewVector (N);
  237.         temp := NewVector (N);
  238.         h := htotal/VAL(LONGREAL,NumberOfSteps);
  239.  
  240.         (* First step.  Throughout this computation yn holds the        *)
  241.         (* latest output estimate, and ym holds the previous estimate.  *)
  242.  
  243.         Copy (ystart^, N, ym^);
  244.         ScalarMul (h, DE^.dydt^, N, yn^);
  245.         Add (yn^, ystart^, N, yn^);
  246.  
  247.         (* We temporarily use yout as a temporary variable to   *)
  248.         (* hold derivative information.                         *)
  249.  
  250.         t := DE^.t + h;
  251.         DE^.Derivs (t, yn^, yout^);
  252.         FOR j := 2 TO NumberOfSteps DO
  253.             ScalarMul (2.0*h, yout^, N, temp^);
  254.             Add (ym^, temp^, N, temp^);
  255.             Copy (yn^, N, ym^);
  256.             Copy (temp^, N, yn^);
  257.             t := t + h;
  258.             DE^.Derivs (t, yn^, yout^);
  259.         END (*FOR*);
  260.  
  261.         (* Last step. *)
  262.  
  263.         ScalarMul (h, yout^, N, yout^);
  264.         Add (yout^, yn^, N, yout^);
  265.         Add (yout^, ym^, N, yout^);
  266.         ScalarMul (0.5, yout^, N, yout^);
  267.  
  268.         DisposeVector (ym, N);
  269.         DisposeVector (yn, N);
  270.         DisposeVector (temp, N);
  271.  
  272.     END MMid;
  273.  
  274. (************************************************************************)
  275. (*              BULIRSCH-STOER METHOD, VARIABLE STEP SIZE               *)
  276. (*                                                                      *)
  277. (* It looks to me as if I actually have the Bulirsch-Stoer method       *)
  278. (* working, but the quality of the coding is still pretty poor.         *)
  279. (* (I started the job by translating some old Fortran code, and it      *)
  280. (* shows.)                                                              *)
  281. (************************************************************************)
  282.  
  283. CONST nuse = 7;
  284.  
  285. PROCEDURE RZextract (trynumber: CARDINAL;  stepsize: LONGREAL;
  286.                                 yest, yz, yerror: VectorPtr;
  287.                                 nv: CARDINAL;
  288.                                 history: ArrayPtr);
  289.  
  290.     (* Rational function extrapolation.  This is intended to be called  *)
  291.     (* repeatedly with increasing values of trynumber, and              *)
  292.     (* correspondingly smaller and smaller values of stepsize.  The     *)
  293.     (* input datum is yest^, and this is combined with values from      *)
  294.     (* earlier calls to produce an extrapolated value yz^ and an error  *)
  295.     (* estimate yerror^.                                                *)
  296.     (* nv is the number of rows of the vector result.  The procedure    *)
  297.     (* uses at most the last nuse estimates.                            *)
  298.     (* Array "history" is for remembering the results of earlier calls. *)
  299.     (* (In effect it is a local static variable of this procedure, but  *)
  300.     (* the space has to be allocated by the caller so that information  *)
  301.     (* is saved across calls.)  The last row of this array holds the    *)
  302.     (* sequence of squares of step sizes.  In the remaining rows, the   *)
  303.     (* first column holds a projected y vector, and the remaining       *)
  304.     (* columns accumulate a set of correction terms.                    *)
  305.  
  306.     VAR M1, j, k: CARDINAL;
  307.         yy, v, c, ddy, b1, b: LONGREAL;
  308.         fx: ARRAY [1..nuse-1] OF LONGREAL;
  309.  
  310.     BEGIN
  311.         stepsize := stepsize*stepsize;
  312.         history^[nv,trynumber] := stepsize;
  313.         IF trynumber = 0 THEN
  314.             FOR j := 0 TO nv-1 DO
  315.                 b := yest^[j];
  316.                 yz^[j] := b;
  317.                 yerror^[j] := b;
  318.                 history^[j,0] := b;
  319.             END (*FOR*);
  320.         ELSE
  321.             M1 := trynumber;
  322.             IF M1 >= nuse THEN M1 := nuse-1 END(*IF*);
  323.             FOR k := 1 TO M1 DO
  324.                 fx[k] := history^[nv,trynumber-k]/stepsize;
  325.             END (*FOR*);
  326.             FOR j := 0 TO nv-1 DO
  327.                 yy := yest^[j];
  328.                 v := history^[j,0];
  329.                 c := yy;
  330.                 history^[j,0] := yy;
  331.                 FOR k := 1 TO M1 DO
  332.                     b1 := fx[k] * v;
  333.                     b := b1 - c;
  334.                     IF b <> 0.0 THEN
  335.                         b := (c - v)/b;
  336.                         ddy := c*b;
  337.                         c := b1*b;
  338.                     ELSE
  339.                         ddy := v;
  340.                     END (*IF*);
  341.                     IF k <> M1 THEN
  342.                         v := history^[j,k];
  343.                     END (*IF*);
  344.                     history^[j,k] := ddy;
  345.                     yy := yy + ddy;
  346.                 END (*FOR*);
  347.                 yerror^[j] := ddy;
  348.                 yz^[j] := yy;
  349.             END (*FOR*);
  350.         END (*IF*);
  351.     END RZextract;
  352.  
  353. (************************************************************************)
  354.  
  355. PROCEDURE BSstep (DE: DiffEqn);
  356.  
  357.     (* One step of the Bulirsch-Stoer method.  The basic idea is to     *)
  358.     (* apply the modified midpoint method with successively finer and   *)
  359.     (* finer subdivisions, and extrapolate the results to an estimate   *)
  360.     (* of what should happen with zero stepsize.                        *)
  361.  
  362.     CONST imax = 10;
  363.           shrink = 0.95;  grow = 1.2;
  364.  
  365.     TYPE StepSequence = ARRAY [0..imax] OF CARDINAL;
  366.     CONST Nseq = StepSequence {2, 4, 6, 8, 12, 16, 24, 32, 48, 64, 96};
  367.  
  368.     VAR ysave, yseq, yerr: VectorPtr;  workspace: ArrayPtr;
  369.         h, errmax, temp: LONGREAL;
  370.         i, j, nv: CARDINAL;  converged: BOOLEAN;
  371.  
  372.     BEGIN
  373.         WITH DE^ DO
  374.             nv := order;
  375.             yseq := NewVector (nv);
  376.             ysave := NewVector (nv);
  377.             yerr := NewVector (nv);
  378.             workspace := NewArray (nv+1, nuse);
  379.             h := step;
  380.             Copy (y^, nv, ysave^);
  381.             converged := FALSE;
  382.             REPEAT
  383.                 i := 0;
  384.                 LOOP
  385.                     MMid (DE, ysave, h, Nseq[i], yseq);
  386.                     RZextract (i, h/VAL(LONGREAL,Nseq[i]), yseq, y, yerr, nv, workspace);
  387.                     errmax := 0.0;
  388.                     FOR j := 0 TO nv-1 DO
  389.                         temp := ABS(yerr^[j]/yscale^[j]);
  390.                         IF temp > errmax THEN
  391.                             errmax := temp;
  392.                         END (*IF*);
  393.                     END (*FOR*);
  394.                     IF errmax < errbound THEN
  395.  
  396.                         (* We've met the prescribed error tolerance.  Adjust the step   *)
  397.                         (* size so that in the long term we'll tend to use about "nuse" *)
  398.                         (* iterations per step.                                         *)
  399.  
  400.                         t := t + h;
  401.                         IF i = nuse-1 THEN
  402.                             step := h*shrink;
  403.                         ELSIF i = nuse-2 THEN
  404.                             step := h*grow;
  405.                         ELSE
  406.                             step := h*VAL(LONGREAL,Nseq[nuse-1])
  407.                                         /VAL(LONGREAL,Nseq[i]);
  408.                         END (*IF*);
  409.                         IF step < stepmin THEN step := stepmin
  410.                         ELSIF step > stepmax THEN step := stepmax
  411.                         END (*IF*);
  412.                         converged := TRUE;
  413.                         EXIT (*LOOP*);
  414.  
  415.                     ELSIF i = imax THEN
  416.  
  417.                         (* If we reach here, the step has failed (which *)
  418.                         (* should not happen often).  Try again with a  *)
  419.                         (* smaller step size.                           *)
  420.  
  421.                         IF h = stepmin THEN
  422.                             converged := TRUE;
  423.                         ELSE
  424.                             h := 0.0625*h;
  425.                             IF h < stepmin THEN h := stepmin END(*IF*);
  426.                         END (*IF*);
  427.                         EXIT(*LOOP*);
  428.  
  429.                     ELSE
  430.                         INC(i);
  431.                     END (*IF*);
  432.  
  433.                 END (*LOOP*);
  434.  
  435.             UNTIL converged;
  436.  
  437.         END (*WITH*);
  438.         DisposeVector (ysave, nv);
  439.         DisposeVector (yseq, nv);
  440.         DisposeArray (workspace, nv, nuse);
  441.         DisposeVector (yerr, nv);
  442.  
  443.     END BSstep;
  444.  
  445. (************************************************************************)
  446. (*              STEP DRIVERS FOR EACH SOLUTION METHOD                   *)
  447. (************************************************************************)
  448.  
  449. PROCEDURE SetScalingVector (DE: DiffEqn);
  450.  
  451.     (* Computes the scaling vector for error control. *)
  452.  
  453.     VAR j: CARDINAL;
  454.  
  455.     BEGIN
  456.         WITH DE^ DO
  457.             FOR j := 0 TO order-1 DO
  458.                 yscale^[j] := ABS(y^[j]) + step * ABS(dydt^[j]);
  459.             END (*FOR*);
  460.         END (*WITH*);
  461.     END SetScalingVector;
  462.  
  463. (************************************************************************)
  464.  
  465. PROCEDURE DoRK4step (DE: DiffEqn);
  466.  
  467.     BEGIN
  468.         WITH DE^ DO
  469.             RK4step (DE, y, dydt, t, step, y);
  470.             t := t + step;
  471.         END (*WITH*);
  472.     END DoRK4step;
  473.  
  474. (************************************************************************)
  475.  
  476. PROCEDURE DoRK5step (DE: DiffEqn);
  477.  
  478.     BEGIN
  479.         SetScalingVector (DE);
  480.         RK5step (DE);
  481.     END DoRK5step;
  482.  
  483. (************************************************************************)
  484.  
  485. PROCEDURE DoMMstep (DE: DiffEqn);
  486.  
  487.     VAR nstep: CARDINAL;
  488.  
  489.     BEGIN
  490.         WITH DE^ DO
  491.             nstep := TRUNC (stepmax/stepmin);
  492.             MMid (DE, y, stepmax, nstep, y);
  493.             t := t + stepmax;
  494.         END (*WITH*);
  495.     END DoMMstep;
  496.  
  497. (************************************************************************)
  498.  
  499. PROCEDURE DoBSstep (DE: DiffEqn);
  500.  
  501.     BEGIN
  502.         SetScalingVector (DE);
  503.         BSstep (DE);
  504.     END DoBSstep;
  505.  
  506. (************************************************************************)
  507. (*              THE USER-CALLABLE SOLUTION PROCEDURE                    *)
  508. (************************************************************************)
  509.  
  510. PROCEDURE Solve (N: CARDINAL;  RHS: RHSproc;  method: SolutionMethod;
  511.                         Plot: PlotProc;  Extra: ADDRESS;
  512.                         t0, tf, minstep, maxstep: LONGREAL;
  513.                         y0: ARRAY OF LONGREAL;  eps: LONGREAL);
  514.  
  515.     (* See definition module for the meanings of the parameters. *)
  516.  
  517.     VAR DE: DiffEqn;  DoOneStep: StepDriver;
  518.  
  519.     BEGIN
  520.         NEW (DE);
  521.         WITH DE^ DO
  522.  
  523.             (* Set up the parameters and initial conditions. *)
  524.  
  525.             order := N;
  526.             Derivs := RHS;
  527.             t := t0;
  528.             y := NewVector (N);  Copy (y0, N, y^);
  529.             dydt := NewVector (N);
  530.             yscale := NewVector (N);
  531.             stepmin := minstep;  stepmax := maxstep;
  532.             step := maxstep;
  533.             errbound := eps;
  534.             CASE method OF
  535.                 | RK4:  DoOneStep := DoRK4step;
  536.                 | RK5:  DoOneStep := DoRK5step;
  537.                 | MM:   DoOneStep := DoMMstep;
  538.                 | BS:   DoOneStep := DoBSstep;
  539.             END (*CASE*);
  540.  
  541.             (* Now for the main solution loop. *)
  542.  
  543.             LOOP
  544.                 Plot (Extra, t, y^);
  545.                 IF t >= tf THEN EXIT(*LOOP*) END(*IF*);
  546.                 IF t + step > tf THEN
  547.                     step := tf - t;
  548.                     stepmax := step;
  549.                 END (*IF*);
  550.                 Derivs (t, y^, dydt^);
  551.                 DoOneStep (DE);
  552.             END (*LOOP*);
  553.  
  554.             (* Throw away all temporary data. *)
  555.  
  556.             DisposeVector (y, N);
  557.             DisposeVector (dydt, N);
  558.             DisposeVector (yscale, N);
  559.  
  560.         END (*WITH*);
  561.         DISPOSE (DE);
  562.  
  563.     END Solve;
  564.  
  565. (************************************************************************)
  566.  
  567. END Integration.
  568.  
  569.