home *** CD-ROM | disk | FTP | other *** search
/ Power-Programmierung / CD2.mdf / c / library / dos / math / cephes / ode / adams3.c next >
Encoding:
C/C++ Source or Header  |  1992-11-17  |  7.6 KB  |  397 lines
  1. /* Adams-Bashforth-Moulton integration formulas.
  2.  *
  3.  * Reference:
  4.  * Shampine, L. F. and M. K. Gordon, _Computer Solution of
  5.  * Ordinary Differential Equations_, W. H. Freeman, 1975.
  6.  *
  7.  * Program by Steve Moshier.
  8.  */
  9.  
  10. #include "int.h"
  11. /* Divided differences */
  12. #define N 19
  13.  
  14. /*Predictor coefficients*/
  15. double precof[N] = {
  16. 1.0,
  17. 1.0 / 2.0,
  18. 5.0 / 12.0,
  19. 3.0 / 8.0,
  20. 251.0 / 720.0,
  21. 95.0 / 288.0,
  22. 19087.0 / 60480.0,
  23. 5257.0 / 17280.0,
  24. 1070017.0 / 3628800.0,
  25. 25713.0 / 89600.0,
  26. 26842253.0 / 95800320.0,
  27. 4777223.0 / 17418240.0,
  28. 703604254357.0 / 2615348736000.0,
  29. 106364763817.0 / 402361344000.0,
  30. 1166309819657.0 / 4483454976000.0,
  31. 2.5221445e7 / 9.8402304e7,
  32. 8.092989203533249e15 / 3.201186852864e16,
  33. 8.5455477715379e13 / 3.4237292544e14,
  34. 1.2600467236042756559e19 / 5.109094217170944e19,
  35. };
  36.  
  37. /*Corrector coefficients*/
  38. double corcof[N] = {
  39.  1.0,
  40.  -1.0 / 2.0,
  41.  -1.0 / 12.0,
  42.  -1.0 / 24.0,
  43.  -19.0 / 720.0,
  44.  -3.0 / 160.0,
  45.  -863.0 / 60480.0,
  46.  -275.0 / 24192.0,
  47.  -33953.0 / 3628800.0,
  48.  -8183.0 / 1036800.0,
  49.  -3250433.0 / 479001600.0,
  50.  -4671.0 / 788480.0,
  51.  -13695779093.0 / 2615348736000.0,
  52.  -2224234463.0 / 475517952000.0,
  53.  -132282840127.0 / 31384184832000.0,
  54.  -2639651053.0 / 689762304000.0,
  55.  1.11956703448001e14 / 3.201186852864e16,
  56.  5.0188465e7 / 1.5613165568e10,
  57.  2.334028946344463e15 / 7.86014494949376e17,
  58. };
  59.  
  60. /* Compute zeroth through kth backward differences
  61.  * of the data in the input array
  62.  */
  63. divdif(vec , k, diffn)
  64. double vec[]; /* input array of k+1 data items */
  65. double *diffn; /* output array of ith differences */
  66. int k;
  67. {
  68. double diftbl[N];
  69. double *p, *q;
  70. double y;
  71. int i, o;
  72.  
  73. /* Copy the given data (zeroth difference) into temp array
  74.  */
  75. p = diftbl;
  76. q = vec;
  77. for( i=0; i<=k; i++ )
  78.     *p++ = *q++;
  79.  
  80. /* On the first outer loop, k-1 first differences are calculated.
  81.  * These overwrite the original data in the temp array.
  82.  */
  83. o = k;
  84. for( o=k; o>0; o-- )
  85.     {
  86.     p = diftbl;
  87.     q = p;
  88.     for( i=0; i<o; i++ )
  89.         {
  90.         y = *p++;
  91.         *q++ = *p - y;
  92.         }
  93.     *diffn++ = *p; /* copy out the last (undifferenced) item */
  94. #if DEBUG
  95.     printf( "%.5e ", *p );
  96. #endif
  97.     }
  98. #if DEBUG
  99.     printf( "%.5e\n", *(q-1) );
  100. #endif
  101. *diffn++ = *(q-1);
  102. }
  103.  
  104.  
  105. /* Update array of differences, given new data value.
  106.  * diffn is an array of k+1 differences, starting with the
  107.  * zeroth difference (the previous original data value).
  108.  */
  109. dupdate( diffn, k, f )
  110. register double *diffn;  /* input and output array of differences */
  111. int k; /* max order of differences */
  112. double f; /* new data point (zeroth difference) */
  113. {
  114. double new, old;
  115. int i;
  116.  
  117. new = f;
  118. for( i=0; i<k; i++ )
  119.     {
  120.     old = *diffn;
  121.     *diffn++ = new;
  122. #if DEBUG
  123.     printf( "%.5e ", new );
  124. #endif
  125.     new = new - old;
  126.     }
  127. #if DEBUG
  128.     printf( "%.5e\n", new );
  129. #endif
  130. *diffn++ = new;
  131. }
  132.  
  133.  
  134.  
  135.  
  136. /* Evaluate the interpolating polynomial
  137.  *
  138.  *              (x - x )
  139.  *                    n    1
  140.  * P(x) = f  +  --------  D f   +  ...
  141.  *         n       h         n
  142.  *
  143.  *     (x - x )(x - x   )...(x - x     )
  144.  *           n       n-1          n+2-k    k-1
  145.  *  +  ---------------------------------  D    f
  146.  *                   k-1                        n
  147.  *                  h     (k-1)!
  148.  *
  149.  *
  150.  *         j
  151.  *  where D denotes the jth backward difference, see dupdate(), and
  152.  *
  153.  *  f   =   f( x , y(x ) )  is the interpolated derivative y'(x ) .
  154.  *   n          n     n                                        n
  155.  *
  156.  * The subroutine argument t is linearly scaled so that t = 1.0
  157.  * will evaluate the polynomial at x = x_n + h,
  158.  * t = 0.0 corresponds to x = x_n, etc.
  159.  */
  160. double difpol( diffn, k, t )
  161. double *diffn;
  162. int k; /* differences go up to order k-1 */
  163. double t; /* scaled argument */
  164. {
  165. double f, fac, s, u;
  166. int i;
  167.  
  168. f = *diffn++; /* the zeroth difference = nth data point */
  169. u = 1.0;
  170. /*s = x/h - n;*/
  171. s = 1.0;    /* to evaluate the polynomial at x = xn + h */
  172. fac = 1.0;
  173. for( i=1; i<k; i++ )
  174.     {
  175.     if( s == 0.0 )
  176.         break;
  177.     u *= s / fac;
  178.     f += u  *  (*diffn++);
  179.     fac += 1.0;
  180.     s += 1.0;
  181.     }
  182. return( f );
  183. }
  184.  
  185.  
  186. /* Integrate the interpolating polynomial from x[n] to x[n+1]
  187.  * to obtain the change in the integrated function from y[n] to y[n+1]
  188.  * given by:
  189.  *
  190.  *                     k-1
  191.  *                      -       i
  192.  *  y    =  y   +   h   >   c  D  f    .
  193.  *   n+1     n          -    i     n
  194.  *                     i=0
  195.  *
  196.  *  This subroutine returns the summation term, not multiplied by h.
  197.  *  The coefficients c_i of the integration formula are either
  198.  *  precof[] or corcof[], given above.
  199.  */
  200. double intpol( diffn, coeffs, k )
  201. double *diffn; /* array of backward differences */
  202. double *coeffs; /* coefficients of integration formula */
  203. int k; /* differences used go up to order k-1 */
  204. {
  205. double s;
  206. int i;
  207.  
  208. s = 0.0;
  209. coeffs += k;
  210. diffn += k;
  211. for( i=0; i<k; i++ )
  212.     {
  213.     s += (*--coeffs) * (*--diffn);
  214.     }
  215. return( s );
  216. }
  217.  
  218.  
  219.  
  220. /* Copy array of n elements from p to q.
  221.  */
  222. vcopy( p, q, n )
  223. register double *p, *q;
  224. register int n;
  225. {
  226. do
  227.     *q++ = *p++;
  228. while( --n );
  229. }
  230.  
  231.  
  232. /* Adams initialization program.
  233.  */
  234.  
  235. /* Addresses within the work array */
  236. static double *dv;
  237. static double *dvp;
  238. static double *vp;
  239. static double *yp;
  240. static double *vn;
  241. static double *delta;
  242. static double *sdelta;
  243. static double *y0;
  244.  
  245. static double ccor;
  246. static double hstep; /* step size (constant) */
  247. static int order; /* Order of the prediction formula */
  248. static int ordp1;
  249. static int asiz;
  250. static int dsiz;
  251. static int jstep; /* counts steps taken */
  252.  
  253. /* Initialize pointers in work array, compute derivatives at
  254.  * initial position, and start the difference tables.
  255.  * neq >= nequat in adstep() below.  If neq > nequat, unused space
  256.  * is left for extra variables that are not actually integrated.
  257.  */
  258. adstart( h, yn, work, neq, ord, t )
  259. double h;
  260. double yn[], work[];
  261. int neq, ord;
  262. double t; 
  263. {
  264. double *p;
  265. int j;
  266.  
  267. hstep = h;
  268. ccor = hstep * precof[ord];
  269. jstep = 0;
  270. dsiz = ord + 2;
  271. asiz = neq * dsiz;
  272. order = ord;
  273. ordp1 = ord + 1;
  274. p = work;
  275. dv = p;
  276. p += asiz;
  277. dvp = p;
  278. p += dsiz;
  279. vp = p;
  280. p += neq;
  281. yp = p;
  282. p += neq;
  283. vn = p;
  284. p += neq;
  285. delta = p;
  286. p += neq;
  287. sdelta = p;
  288. p += neq;
  289. y0 = p;
  290.  
  291.  
  292. func( t, yn, vn );
  293.  
  294. p = dv;
  295. for( j=0; j<neq; j++ )
  296.     {
  297.     dupdate( p, 0, vn[j] );
  298.     p += dsiz;
  299.     }
  300. }
  301.  
  302.  
  303.  
  304.  
  305.  
  306. /* Adams-Bashforth-Moulton step.
  307.  */
  308.  
  309. double adstep( t, yn, nequat )
  310. double *t;
  311. double yn[];
  312. int nequat;
  313. {
  314. double e, e0, time;
  315. double *pdv;
  316. int i, j;
  317. double intpol();
  318.  
  319. time = *t;
  320. jstep += 1;
  321.  
  322. /* Do Runge-Kutta for the first ord + 1 steps.
  323.  */
  324. if( jstep <= ordp1 )
  325.     {
  326.     rungek(  nequat, time, yn, hstep, yn, delta );
  327.     func( time+hstep, yn, vn );
  328.     pdv = dv;
  329.     for( j=0; j<nequat; j++ )
  330.         {
  331.         dupdate( pdv, jstep, vn[j] );
  332.         pdv += dsiz;
  333.         }
  334.     e = 0.0;
  335.     goto done;
  336.     }
  337.  
  338.  
  339.  
  340. /* Predict the next position
  341.  * based on current y' difference table dv[].
  342.  */
  343.     pdv = dv;
  344.     for( i=0; i<nequat; i++ )
  345.         {
  346.         yp[i] = yn[i] + hstep * intpol( pdv, precof, order );
  347.         pdv += dsiz;
  348.         }
  349. /* Evaluate derivatives at the predicted position
  350.  */
  351.     func( time+hstep, yp, vp );
  352.  
  353. /* Correct the predicted position and velocity using the derivatives
  354.  * evalutated at yp.
  355.  */
  356.     pdv = dv;
  357.     for( i=0; i<nequat; i++ )
  358.         {
  359.         vcopy( pdv, dvp, dsiz ); /* y' difference table */
  360.         dupdate( dvp, ordp1, vp[i] );
  361. /* Note, the following line is equivalent to:
  362.  *        yn[i] = yn[i] + hstep * intpol( dap, corcof, ordp1 );
  363.  * where e is the corrected value of the next vn.
  364.  */
  365.         yn[i] = yp[i] + ccor * dvp[order];
  366.         pdv += dsiz;
  367.         }
  368.  
  369. /* Evaluate derivative at the final position
  370.  */
  371.     func( time+hstep, yn, vn );
  372.  
  373.     e = 0.0;
  374.     pdv = dv;
  375.     for( i=0; i<nequat; i++ )
  376.         {
  377.         dupdate( pdv, ordp1, vn[i] );
  378. /* Note, the following line is equivalent to:
  379.  *        yn[i] = yn[i] + hstep * intpol( dap, corcof, ordp1 );
  380.  * where e is the corrected value of the next vn.
  381.  */
  382.         yn[i] = yp[i] + ccor * pdv[order];
  383. /* Estimate the error
  384.  */
  385.         e0 = hstep * pdv[order] * corcof[order];
  386.         if( e0 < 0.0 )
  387.             e0 = -e0;
  388.         if( e0 > e )
  389.             e = e0;
  390.         pdv += dsiz;
  391.         }
  392. done:
  393. time += hstep;
  394. *t = time;
  395. return( e );
  396. }
  397.