odesolve.c
| Line | Branch | Exec | Source |
|---|---|---|---|
| 1 | //----------------------------------------------------------------------------- | ||
| 2 | // odesolve.c | ||
| 3 | // | ||
| 4 | // Fifth-order Runge-Kutta integration with adaptive step size control | ||
| 5 | // based on code from Numerical Recipes in C (Cambridge University | ||
| 6 | // Press, 1992). | ||
| 7 | // | ||
| 8 | // Date: 11/15/06 | ||
| 9 | // Author: L. Rossman | ||
| 10 | //----------------------------------------------------------------------------- | ||
| 11 | |||
| 12 | #include <stdlib.h> | ||
| 13 | #include <math.h> | ||
| 14 | #include "odesolve.h" | ||
| 15 | |||
| 16 | #define MAXSTP 10000 | ||
| 17 | #define TINY 1.0e-30 | ||
| 18 | #define SAFETY 0.9 | ||
| 19 | #define PGROW -0.2 | ||
| 20 | #define PSHRNK -0.25 | ||
| 21 | #define ERRCON 1.89e-4 // = (5/SAFETY)^(1/PGROW) | ||
| 22 | |||
| 23 | |||
| 24 | //----------------------------------------------------------------------------- | ||
| 25 | // Local declarations | ||
| 26 | //----------------------------------------------------------------------------- | ||
| 27 | int nmax; // max. number of equations | ||
| 28 | double* y; // dependent variable | ||
| 29 | double* yscal; // scaling factors | ||
| 30 | double* yerr; // integration errors | ||
| 31 | double* ytemp; // temporary values of y | ||
| 32 | double* dydx; // derivatives of y | ||
| 33 | double* ak; // derivatives at intermediate points | ||
| 34 | |||
| 35 | |||
| 36 | // function that integrates over an error-controlled stepsize | ||
| 37 | int rkqs(double* x, int n, double htry, double eps, double* hdid, | ||
| 38 | double* hnext, void (*derivs)(double, double*, double*)); | ||
| 39 | |||
| 40 | // function that performs the Runge-Kutta integration step | ||
| 41 | void rkck(double x, int n, double h, void (*derivs)(double, double*, double*)); | ||
| 42 | |||
| 43 | |||
| 44 | //----------------------------------------------------------------------------- | ||
| 45 | // open the ODE solver to solve system of n equations | ||
| 46 | // (return 1 if successful, 0 if not) | ||
| 47 | //----------------------------------------------------------------------------- | ||
| 48 | 35 | int odesolve_open(int n) | |
| 49 | { | ||
| 50 | 35 | int n5 = n*5; | |
| 51 | 35 | nmax = 0; | |
| 52 | 35 | y = (double *) calloc(n, sizeof(double)); | |
| 53 | 35 | yscal = (double *) calloc(n, sizeof(double)); | |
| 54 | 35 | dydx = (double *) calloc(n, sizeof(double)); | |
| 55 | 35 | yerr = (double *) calloc(n, sizeof(double)); | |
| 56 | 35 | ytemp = (double *) calloc(n, sizeof(double)); | |
| 57 | 35 | ak = (double *) calloc(n5, sizeof(double)); | |
| 58 |
6/12✓ Branch 0 taken 35 times.
✗ Branch 1 not taken.
✓ Branch 2 taken 35 times.
✗ Branch 3 not taken.
✓ Branch 4 taken 35 times.
✗ Branch 5 not taken.
✓ Branch 6 taken 35 times.
✗ Branch 7 not taken.
✓ Branch 8 taken 35 times.
✗ Branch 9 not taken.
✗ Branch 10 not taken.
✓ Branch 11 taken 35 times.
|
35 | if ( !y || !yscal || !dydx || !yerr || !ytemp || !ak ) return 0; |
| 59 | 35 | nmax = n; | |
| 60 | 35 | return 1; | |
| 61 | } | ||
| 62 | |||
| 63 | |||
| 64 | //----------------------------------------------------------------------------- | ||
| 65 | // close the ODE solver | ||
| 66 | //----------------------------------------------------------------------------- | ||
| 67 | 35 | void odesolve_close() | |
| 68 | { | ||
| 69 |
1/2✓ Branch 0 taken 35 times.
✗ Branch 1 not taken.
|
35 | if ( y ) free(y); |
| 70 | 35 | y = NULL; | |
| 71 |
1/2✓ Branch 0 taken 35 times.
✗ Branch 1 not taken.
|
35 | if ( yscal ) free(yscal); |
| 72 | 35 | yscal = NULL; | |
| 73 |
1/2✓ Branch 0 taken 35 times.
✗ Branch 1 not taken.
|
35 | if ( dydx ) free(dydx); |
| 74 | 35 | dydx = NULL; | |
| 75 |
1/2✓ Branch 0 taken 35 times.
✗ Branch 1 not taken.
|
35 | if ( yerr ) free(yerr); |
| 76 | 35 | yerr = NULL; | |
| 77 |
1/2✓ Branch 0 taken 35 times.
✗ Branch 1 not taken.
|
35 | if ( ytemp ) free(ytemp); |
| 78 | 35 | ytemp = NULL; | |
| 79 |
1/2✓ Branch 0 taken 35 times.
✗ Branch 1 not taken.
|
35 | if ( ak ) free(ak); |
| 80 | 35 | ak = NULL; | |
| 81 | 35 | nmax = 0; | |
| 82 | 35 | } | |
| 83 | |||
| 84 | |||
| 85 | 227701 | int odesolve_integrate(double ystart[], int n, double x1, double x2, | |
| 86 | double eps, double h1, void (*derivs)(double, double*, double*)) | ||
| 87 | //--------------------------------------------------------------- | ||
| 88 | // Driver function for Runge-Kutta integration with adaptive | ||
| 89 | // stepsize control. Integrates starting n values in ystart[] | ||
| 90 | // from x1 to x2 with accuracy eps. h1 is the initial stepsize | ||
| 91 | // guess and derivs is a user-supplied function that computes | ||
| 92 | // derivatives dy/dx of y. On completion, ystart[] contains the | ||
| 93 | // new values of y at the end of the integration interval. | ||
| 94 | //--------------------------------------------------------------- | ||
| 95 | { | ||
| 96 | int i, errcode, nstp; | ||
| 97 | double hdid, hnext; | ||
| 98 | 227701 | double x = x1; | |
| 99 | 227701 | double h = h1; | |
| 100 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 227701 times.
|
227701 | if (nmax < n) return 1; |
| 101 |
2/2✓ Branch 0 taken 253861 times.
✓ Branch 1 taken 227701 times.
|
481562 | for (i=0; i<n; i++) y[i] = ystart[i]; |
| 102 |
1/2✓ Branch 0 taken 238269 times.
✗ Branch 1 not taken.
|
238269 | for (nstp=1; nstp<=MAXSTP; nstp++) |
| 103 | { | ||
| 104 | 238269 | derivs(x,y,dydx); | |
| 105 |
2/2✓ Branch 0 taken 264439 times.
✓ Branch 1 taken 238269 times.
|
502708 | for (i=0; i<n; i++) |
| 106 | 264439 | yscal[i] = fabs(y[i]) + fabs(dydx[i]*h) + TINY; | |
| 107 |
2/2✓ Branch 0 taken 7063 times.
✓ Branch 1 taken 231206 times.
|
238269 | if ((x+h-x2)*(x+h-x1) > 0.0) h = x2 - x; |
| 108 | 238269 | errcode = rkqs(&x,n,h,eps,&hdid,&hnext,derivs); | |
| 109 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 238269 times.
|
238269 | if (errcode) break; |
| 110 |
2/2✓ Branch 0 taken 227701 times.
✓ Branch 1 taken 10568 times.
|
238269 | if ((x-x2)*(x2-x1) >= 0.0) |
| 111 | { | ||
| 112 |
2/2✓ Branch 0 taken 253861 times.
✓ Branch 1 taken 227701 times.
|
481562 | for (i=0; i<n; i++) ystart[i] = y[i]; |
| 113 | 227701 | return 0; | |
| 114 | } | ||
| 115 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 10568 times.
|
10568 | if (fabs(hnext) <= 0.0) return 2; |
| 116 | 10568 | h = hnext; | |
| 117 | } | ||
| 118 | ✗ | return 3; | |
| 119 | } | ||
| 120 | |||
| 121 | |||
| 122 | 238269 | int rkqs(double* x, int n, double htry, double eps, double* hdid, | |
| 123 | double* hnext, void (*derivs)(double, double*, double*)) | ||
| 124 | //--------------------------------------------------------------- | ||
| 125 | // Fifth-order Runge-Kutta integration step with monitoring of | ||
| 126 | // local truncation error to assure accuracy and adjust stepsize. | ||
| 127 | // Inputs are current value of x, trial step size (htry), and | ||
| 128 | // accuracy (eps). Outputs are stepsize taken (hdid) and estimated | ||
| 129 | // next stepsize (hnext). Also updated are the values of y[]. | ||
| 130 | //--------------------------------------------------------------- | ||
| 131 | { | ||
| 132 | int i; | ||
| 133 | 238269 | double err, errmax, h, htemp, xnew, xold = *x; | |
| 134 | |||
| 135 | // --- set initial stepsize | ||
| 136 | 238269 | h = htry; | |
| 137 | for (;;) | ||
| 138 | { | ||
| 139 | // --- take a Runge-Kutta-Cash-Karp step | ||
| 140 | 245573 | rkck(xold, n, h, derivs); | |
| 141 | |||
| 142 | // --- compute scaled maximum error | ||
| 143 | 245573 | errmax = 0.0; | |
| 144 |
2/2✓ Branch 0 taken 271755 times.
✓ Branch 1 taken 245573 times.
|
517328 | for (i=0; i<n; i++) |
| 145 | { | ||
| 146 | 271755 | err = fabs(yerr[i]/yscal[i]); | |
| 147 |
2/2✓ Branch 0 taken 260027 times.
✓ Branch 1 taken 11728 times.
|
271755 | if (err > errmax) errmax = err; |
| 148 | } | ||
| 149 | 245573 | errmax /= eps; | |
| 150 | |||
| 151 | // --- error too large; reduce stepsize & repeat | ||
| 152 |
2/2✓ Branch 0 taken 7304 times.
✓ Branch 1 taken 238269 times.
|
245573 | if (errmax > 1.0) |
| 153 | { | ||
| 154 | 7304 | htemp = SAFETY*h*pow(errmax,PSHRNK); | |
| 155 |
1/2✓ Branch 0 taken 7304 times.
✗ Branch 1 not taken.
|
7304 | if (h >= 0) |
| 156 | { | ||
| 157 |
2/2✓ Branch 0 taken 7248 times.
✓ Branch 1 taken 56 times.
|
7304 | if (htemp > 0.1*h) h = htemp; |
| 158 | 56 | else h = 0.1*h; | |
| 159 | } | ||
| 160 | else | ||
| 161 | { | ||
| 162 | ✗ | if (htemp < 0.1*h) h = htemp; | |
| 163 | ✗ | else h = 0.1*h; | |
| 164 | } | ||
| 165 | 7304 | xnew = xold + h; | |
| 166 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 7304 times.
|
7304 | if (xnew == xold) return 2; |
| 167 | 7304 | continue; | |
| 168 | } | ||
| 169 | |||
| 170 | // --- step succeeded; compute size of next step | ||
| 171 | else | ||
| 172 | { | ||
| 173 |
2/2✓ Branch 0 taken 39842 times.
✓ Branch 1 taken 198427 times.
|
238269 | if (errmax > ERRCON) *hnext = SAFETY*h*pow(errmax,PGROW); |
| 174 | 198427 | else *hnext = 5.0*h; | |
| 175 | 238269 | *x += (*hdid=h); | |
| 176 |
2/2✓ Branch 0 taken 264439 times.
✓ Branch 1 taken 238269 times.
|
502708 | for (i=0; i<n; i++) y[i] = ytemp[i]; |
| 177 | 238269 | return 0; | |
| 178 | } | ||
| 179 | } | ||
| 180 | } | ||
| 181 | |||
| 182 | |||
| 183 | 245573 | void rkck(double x, int n, double h, void (*derivs)(double, double*, double*)) | |
| 184 | //---------------------------------------------------------------------- | ||
| 185 | // Uses the Runge-Kutta-Cash-Karp method to advance y[] at x | ||
| 186 | // over stepsize h. | ||
| 187 | //---------------------------------------------------------------------- | ||
| 188 | { | ||
| 189 | 245573 | double a2=0.2, a3=0.3, a4=0.6, a5=1.0, a6=0.875, | |
| 190 | 245573 | b21=0.2, b31=3.0/40.0, b32=9.0/40.0, b41=0.3, b42= -0.9, b43=1.2, | |
| 191 | 245573 | b51= -11.0/54.0, b52=2.5, b53= -70.0/27.0, b54=35.0/27.0, | |
| 192 | 245573 | b61=1631.0/55296.0, b62=175.0/512.0, b63=575.0/13824.0, | |
| 193 | 245573 | b64=44275.0/110592.0, b65=253.0/4096.0, c1=37.0/378.0, | |
| 194 | 245573 | c3=250.0/621.0, c4=125.0/594.0, c6=512.0/1771.0, | |
| 195 | 245573 | dc5= -277.0/14336.0; | |
| 196 | 245573 | double dc1=c1-2825.0/27648.0, dc3=c3-18575.0/48384.0, | |
| 197 | 245573 | dc4=c4-13525.0/55296.0, dc6=c6-0.25; | |
| 198 | int i; | ||
| 199 | 245573 | int n2 = n*2; | |
| 200 | 245573 | int n3 = n*3; | |
| 201 | 245573 | int n4 = n*4; | |
| 202 | 245573 | double *ak2 = (ak); | |
| 203 | 245573 | double *ak3 = ((ak)+(n)); | |
| 204 | 245573 | double *ak4 = ((ak)+(n2)); | |
| 205 | 245573 | double *ak5 = ((ak)+(n3)); | |
| 206 | 245573 | double *ak6 = ((ak)+(n4)); | |
| 207 | |||
| 208 |
2/2✓ Branch 0 taken 271755 times.
✓ Branch 1 taken 245573 times.
|
517328 | for (i=0; i<n; i++) |
| 209 | 271755 | ytemp[i] = y[i] + b21*h*dydx[i]; | |
| 210 | 245573 | derivs(x+a2*h,ytemp,ak2); | |
| 211 | |||
| 212 |
2/2✓ Branch 0 taken 271755 times.
✓ Branch 1 taken 245573 times.
|
517328 | for (i=0; i<n; i++) |
| 213 | 271755 | ytemp[i] = y[i] + h*(b31*dydx[i]+b32*ak2[i]); | |
| 214 | 245573 | derivs(x+a3*h,ytemp,ak3); | |
| 215 | |||
| 216 |
2/2✓ Branch 0 taken 271755 times.
✓ Branch 1 taken 245573 times.
|
517328 | for (i=0; i<n; i++) |
| 217 | 271755 | ytemp[i] = y[i] + h*(b41*dydx[i]+b42*ak2[i] + b43*ak3[i]); | |
| 218 | 245573 | derivs(x+a4*h,ytemp,ak4); | |
| 219 | |||
| 220 |
2/2✓ Branch 0 taken 271755 times.
✓ Branch 1 taken 245573 times.
|
517328 | for (i=0; i<n; i++) |
| 221 | 271755 | ytemp[i] = y[i] + h*(b51*dydx[i]+b52*ak2[i] + b53*ak3[i] + b54*ak4[i]); | |
| 222 | 245573 | derivs(x+a5*h,ytemp,ak5); | |
| 223 | |||
| 224 |
2/2✓ Branch 0 taken 271755 times.
✓ Branch 1 taken 245573 times.
|
517328 | for (i=0; i<n; i++) |
| 225 | 271755 | ytemp[i] = y[i] + h*(b61*dydx[i]+b62*ak2[i] + b63*ak3[i] + b64*ak4[i] | |
| 226 | 271755 | + b65*ak5[i]); | |
| 227 | 245573 | derivs(x+a6*h,ytemp,ak6); | |
| 228 | |||
| 229 |
2/2✓ Branch 0 taken 271755 times.
✓ Branch 1 taken 245573 times.
|
517328 | for (i=0; i<n; i++) |
| 230 | 271755 | ytemp[i] = y[i] + h*(c1*dydx[i] + c3*ak3[i] + c4*ak4[i] + c6*ak6[i]); | |
| 231 | |||
| 232 |
2/2✓ Branch 0 taken 271755 times.
✓ Branch 1 taken 245573 times.
|
517328 | for (i=0; i<n; i++) |
| 233 | 271755 | yerr[i] = h*(dc1*dydx[i] +dc3*ak3[i] + dc4*ak4[i] + dc5*ak5[i] + dc6*ak6[i]); | |
| 234 | 245573 | } | |
| 235 |