htswanalysis/MACS/lib/gsl/gsl-1.11/ode-initval/rk4.c

   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 <config.h>
  33 #include <stdlib.h>
  34 #include <string.h>
  35 #include <gsl/gsl_errno.h>
  36 #include <gsl/gsl_odeiv.h>
  37
  38 #include "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 *
  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
 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
 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
 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
 336 rk4_order (void *vstate)
 337 {
 338   rk4_state_t *state = (rk4_state_t *) vstate;
 339   state = 0; /* prevent warnings about unused parameters */
 340   return 4;
 341 }
 342
 343 static void
 344 rk4_free (void *vstate)
 345 {
 346   rk4_state_t *state = (rk4_state_t *) vstate;
 347   free (state->k);
 348   free (state->k1);
 349   free (state->y0);
 350   free (state->ytmp);
 351   free (state->y_onestep);
 352   free (state);
 353 }
 354
 355 static const gsl_odeiv_step_type rk4_type = { "rk4",    /* name */
 356   1,                            /* can use dydt_in */
 357   1,                            /* gives exact dydt_out */
 358   &rk4_alloc,
 359   &rk4_apply,
 360   &rk4_reset,
 361   &rk4_order,
 362   &rk4_free
 363 };
 364
 365 const gsl_odeiv_step_type *gsl_odeiv_step_rk4 = &rk4_type;