1 /* ode-initval/rk4.c
2 *
3 * Copyright (C) 1996, 1997, 1998, 1999, 2000 Gerard Jungman
4 *
5 * This program is free software; you can redistribute it and/or modify
6 * it under the terms of the GNU General Public License as published by
7 * the Free Software Foundation; either version 3 of the License, or (at
8 * your option) any later version.
9 *
10 * This program is distributed in the hope that it will be useful, but
11 * WITHOUT ANY WARRANTY; without even the implied warranty of
12 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
13 * General Public License for more details.
14 *
15 * You should have received a copy of the GNU General Public License
16 * along with this program; if not, write to the Free Software
17 * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
18 */
19
20 /* Runge-Kutta 4th order, Classical */
21
22 /* Author: G. Jungman
23 */
24
25 /* Reference: Abramowitz & Stegun, section 25.5. equation 25.5.10
26
27 Error estimation by step doubling, see eg. Ascher, U.M., Petzold,
28 L.R., Computer methods for ordinary differential and
29 differential-algebraic equations, SIAM, Philadelphia, 1998.
30 */
31
32 #include "gsl__config.h"
33 #include <stdlib.h>
34 #include <string.h>
35 #include "gsl_errno.h"
36 #include "gsl_odeiv.h"
37
38 #include "gsl_ode-initval__odeiv_util.h"
39
40 typedef struct
41 {
42 double *k;
43 double *k1;
44 double *y0;
45 double *ytmp;
46 double *y_onestep;
47 }
48 rk4_state_t;
49
50 static void *
rk4_alloc(size_t dim)51 rk4_alloc (size_t dim)
52 {
53 rk4_state_t *state = (rk4_state_t *) malloc (sizeof (rk4_state_t));
54
55 if (state == 0)
56 {
57 GSL_ERROR_NULL ("failed to allocate space for rk4_state", GSL_ENOMEM);
58 }
59
60 state->k = (double *) malloc (dim * sizeof (double));
61
62 if (state->k == 0)
63 {
64 free (state);
65 GSL_ERROR_NULL ("failed to allocate space for k", GSL_ENOMEM);
66 }
67
68 state->k1 = (double *) malloc (dim * sizeof (double));
69
70 if (state->k1 == 0)
71 {
72 free (state);
73 free (state->k);
74 GSL_ERROR_NULL ("failed to allocate space for k1", GSL_ENOMEM);
75 }
76
77 state->y0 = (double *) malloc (dim * sizeof (double));
78
79 if (state->y0 == 0)
80 {
81 free (state->k);
82 free (state->k1);
83 free (state);
84 GSL_ERROR_NULL ("failed to allocate space for y0", GSL_ENOMEM);
85 }
86
87 state->ytmp = (double *) malloc (dim * sizeof (double));
88
89 if (state->ytmp == 0)
90 {
91 free (state->y0);
92 free (state->k);
93 free (state->k1);
94 free (state);
95 GSL_ERROR_NULL ("failed to allocate space for ytmp", GSL_ENOMEM);
96 }
97
98 state->y_onestep = (double *) malloc (dim * sizeof (double));
99
100 if (state->y_onestep == 0)
101 {
102 free (state->ytmp);
103 free (state->y0);
104 free (state->k);
105 free (state->k1);
106 free (state);
107 GSL_ERROR_NULL ("failed to allocate space for ytmp", GSL_ENOMEM);
108 }
109
110 return state;
111 }
112
113 static int
rk4_step(double * y,const rk4_state_t * state,const double h,const double t,const size_t dim,const gsl_odeiv_system * sys)114 rk4_step (double *y, const rk4_state_t *state,
115 const double h, const double t, const size_t dim,
116 const gsl_odeiv_system *sys)
117 {
118 /* Makes a Runge-Kutta 4th order advance with step size h. */
119
120 /* initial values of variables y. */
121 const double *y0 = state->y0;
122
123 /* work space */
124 double *ytmp = state->ytmp;
125
126 /* Runge-Kutta coefficients. Contains values of coefficient k1
127 in the beginning
128 */
129 double *k = state->k;
130
131 size_t i;
132
133 /* k1 step */
134
135 for (i = 0; i < dim; i++)
136 {
137 y[i] += h / 6.0 * k[i];
138 ytmp[i] = y0[i] + 0.5 * h * k[i];
139 }
140
141 /* k2 step */
142 {
143 int s = GSL_ODEIV_FN_EVAL (sys, t + 0.5 * h, ytmp, k);
144
145 if (s != GSL_SUCCESS)
146 {
147 return s;
148 }
149 }
150
151 for (i = 0; i < dim; i++)
152 {
153 y[i] += h / 3.0 * k[i];
154 ytmp[i] = y0[i] + 0.5 * h * k[i];
155 }
156
157 /* k3 step */
158 {
159 int s = GSL_ODEIV_FN_EVAL (sys, t + 0.5 * h, ytmp, k);
160
161 if (s != GSL_SUCCESS)
162 {
163 return s;
164 }
165 }
166
167 for (i = 0; i < dim; i++)
168 {
169 y[i] += h / 3.0 * k[i];
170 ytmp[i] = y0[i] + h * k[i];
171 }
172
173 /* k4 step */
174 {
175 int s = GSL_ODEIV_FN_EVAL (sys, t + h, ytmp, k);
176
177 if (s != GSL_SUCCESS)
178 {
179 return s;
180 }
181 }
182
183 for (i = 0; i < dim; i++)
184 {
185 y[i] += h / 6.0 * k[i];
186 }
187
188 return GSL_SUCCESS;
189 }
190
191
192 static int
rk4_apply(void * vstate,size_t dim,double t,double h,double y[],double yerr[],const double dydt_in[],double dydt_out[],const gsl_odeiv_system * sys)193 rk4_apply (void *vstate,
194 size_t dim,
195 double t,
196 double h,
197 double y[],
198 double yerr[],
199 const double dydt_in[],
200 double dydt_out[],
201 const gsl_odeiv_system * sys)
202 {
203 rk4_state_t *state = (rk4_state_t *) vstate;
204
205 size_t i;
206
207 double *const k = state->k;
208 double *const k1 = state->k1;
209 double *const y0 = state->y0;
210 double *const y_onestep = state->y_onestep;
211
212 DBL_MEMCPY (y0, y, dim);
213
214 if (dydt_in != NULL)
215 {
216 DBL_MEMCPY (k, dydt_in, dim);
217 }
218 else
219 {
220 int s = GSL_ODEIV_FN_EVAL (sys, t, y0, k);
221
222 if (s != GSL_SUCCESS)
223 {
224 return s;
225 }
226 }
227
228 /* Error estimation is done by step doubling procedure */
229
230 /* Save first point derivatives*/
231
232 DBL_MEMCPY (k1, k, dim);
233
234 /* First traverse h with one step (save to y_onestep) */
235
236 DBL_MEMCPY (y_onestep, y, dim);
237
238 {
239 int s = rk4_step (y_onestep, state, h, t, dim, sys);
240
241 if (s != GSL_SUCCESS)
242 {
243 return s;
244 }
245 }
246
247 /* Then with two steps with half step length (save to y) */
248
249 DBL_MEMCPY (k, k1, dim);
250
251 {
252 int s = rk4_step (y, state, h/2.0, t, dim, sys);
253
254 if (s != GSL_SUCCESS)
255 {
256 /* Restore original values */
257 DBL_MEMCPY (y, y0, dim);
258 return s;
259 }
260 }
261
262 /* Update before second step */
263 {
264 int s = GSL_ODEIV_FN_EVAL (sys, t + h/2.0, y, k);
265
266 if (s != GSL_SUCCESS)
267 {
268 /* Restore original values */
269 DBL_MEMCPY (y, y0, dim);
270 return s;
271 }
272 }
273
274 /* Save original y0 to k1 for possible failures */
275 DBL_MEMCPY (k1, y0, dim);
276
277 /* Update y0 for second step */
278 DBL_MEMCPY (y0, y, dim);
279
280 {
281 int s = rk4_step (y, state, h/2.0, t + h/2.0, dim, sys);
282
283 if (s != GSL_SUCCESS)
284 {
285 /* Restore original values */
286 DBL_MEMCPY (y, k1, dim);
287 return s;
288 }
289 }
290
291 /* Derivatives at output */
292
293 if (dydt_out != NULL) {
294 int s = GSL_ODEIV_FN_EVAL (sys, t + h, y, dydt_out);
295
296 if (s != GSL_SUCCESS)
297 {
298 /* Restore original values */
299 DBL_MEMCPY (y, k1, dim);
300 return s;
301 }
302 }
303
304 /* Error estimation
305
306 yerr = C * 0.5 * | y(onestep) - y(twosteps) | / (2^order - 1)
307
308 constant C is approximately 8.0 to ensure 90% of samples lie within
309 the error (assuming a gaussian distribution with prior p(sigma)=1/sigma.)
310
311 */
312
313 for (i = 0; i < dim; i++)
314 {
315 yerr[i] = 4.0 * (y[i] - y_onestep[i]) / 15.0;
316 }
317
318 return GSL_SUCCESS;
319 }
320
321 static int
rk4_reset(void * vstate,size_t dim)322 rk4_reset (void *vstate, size_t dim)
323 {
324 rk4_state_t *state = (rk4_state_t *) vstate;
325
326 DBL_ZERO_MEMSET (state->k, dim);
327 DBL_ZERO_MEMSET (state->k1, dim);
328 DBL_ZERO_MEMSET (state->y0, dim);
329 DBL_ZERO_MEMSET (state->ytmp, dim);
330 DBL_ZERO_MEMSET (state->y_onestep, dim);
331
332 return GSL_SUCCESS;
333 }
334
335 static unsigned int
rk4_order(void * vstate)336 rk4_order (void *vstate)
337 {
338 return 4;
339 }
340
341 static void
rk4_free(void * vstate)342 rk4_free (void *vstate)
343 {
344 rk4_state_t *state = (rk4_state_t *) vstate;
345 free (state->k);
346 free (state->k1);
347 free (state->y0);
348 free (state->ytmp);
349 free (state->y_onestep);
350 free (state);
351 }
352
353 static const gsl_odeiv_step_type rk4_type = { "rk4", /* name */
354 1, /* can use dydt_in */
355 1, /* gives exact dydt_out */
356 &rk4_alloc,
357 &rk4_apply,
358 &rk4_reset,
359 &rk4_order,
360 &rk4_free
361 };
362
363 const gsl_odeiv_step_type *gsl_odeiv_step_rk4 = &rk4_type;
364