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

   1 /* ode-initval/rk4imp.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 4, Gaussian implicit */
  21
  22 /* Author:  G. Jungman
  23 */
  24
  25 /* Error estimation by step doubling, see eg. Ascher, U.M., Petzold,
  26    L.R., Computer methods for ordinary differential and
  27    differential-algebraic equations, SIAM, Philadelphia, 1998.
  28    Method coefficients can also be found in it.
  29 */
  30
  31 #include <config.h>
  32 #include <stdlib.h>
  33 #include <string.h>
  34 #include <gsl/gsl_math.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 *k1nu;
  43   double *k2nu;
  44   double *ytmp1;
  45   double *ytmp2;
  46   double *y0;
  47   double *y0_orig;
  48   double *y_onestep;
  49 }
  50 rk4imp_state_t;
  51
  52 static void *
  53 rk4imp_alloc (size_t dim)
  54 {
  55   rk4imp_state_t *state = (rk4imp_state_t *) malloc (sizeof (rk4imp_state_t));
  56
  57   if (state == 0)
  58     {
  59       GSL_ERROR_NULL ("failed to allocate space for rk4imp_state",
  60                       GSL_ENOMEM);
  61     }
  62
  63   state->k1nu = (double *) malloc (dim * sizeof (double));
  64
  65   if (state->k1nu == 0)
  66     {
  67       free (state);
  68       GSL_ERROR_NULL ("failed to allocate space for k1nu", GSL_ENOMEM);
  69     }
  70
  71   state->k2nu = (double *) malloc (dim * sizeof (double));
  72
  73   if (state->k2nu == 0)
  74     {
  75       free (state->k1nu);
  76       free (state);
  77       GSL_ERROR_NULL ("failed to allocate space for k2nu", GSL_ENOMEM);
  78     }
  79
  80   state->ytmp1 = (double *) malloc (dim * sizeof (double));
  81
  82   if (state->ytmp1 == 0)
  83     {
  84       free (state->k2nu);
  85       free (state->k1nu);
  86       free (state);
  87       GSL_ERROR_NULL ("failed to allocate space for ytmp1", GSL_ENOMEM);
  88     }
  89
  90   state->ytmp2 = (double *) malloc (dim * sizeof (double));
  91
  92   if (state->ytmp2 == 0)
  93     {
  94       free (state->ytmp1);
  95       free (state->k2nu);
  96       free (state->k1nu);
  97       free (state);
  98       GSL_ERROR_NULL ("failed to allocate space for ytmp2", GSL_ENOMEM);
  99     }
 100
 101   state->y0 = (double *) malloc (dim * sizeof (double));
 102
 103   if (state->y0 == 0)
 104     {
 105       free (state->ytmp2);
 106       free (state->ytmp1);
 107       free (state->k2nu);
 108       free (state->k1nu);
 109       free (state);
 110       GSL_ERROR_NULL ("failed to allocate space for y0", GSL_ENOMEM);
 111     }
 112
 113   state->y0_orig = (double *) malloc (dim * sizeof (double));
 114
 115   if (state->y0_orig == 0)
 116     {
 117       free (state->y0);
 118       free (state->ytmp2);
 119       free (state->ytmp1);
 120       free (state->k2nu);
 121       free (state->k1nu);
 122       free (state);
 123       GSL_ERROR_NULL ("failed to allocate space for y0_orig", GSL_ENOMEM);
 124     }
 125
 126   state->y_onestep = (double *) malloc (dim * sizeof (double));
 127
 128   if (state->y_onestep == 0)
 129     {
 130       free (state->y0_orig);
 131       free (state->y0);
 132       free (state->ytmp2);
 133       free (state->ytmp1);
 134       free (state->k2nu);
 135       free (state->k1nu);
 136       free (state);
 137       GSL_ERROR_NULL ("failed to allocate space for y_onestep", GSL_ENOMEM);
 138     }
 139
 140   return state;
 141 }
 142
 143 static int
 144 rk4imp_step (double *y, rk4imp_state_t *state,
 145              const double h, const double t,
 146              const size_t dim, const gsl_odeiv_system *sys)
 147 {
 148   /* Makes a Runge-Kutta 4th order implicit advance with step size h.
 149      y0 is initial values of variables y.
 150
 151      The implicit matrix equations to solve are:
 152
 153      Y1 = y0 + h * a11 * f(t + h * c1, Y1) + h * a12 * f(t + h * c2, Y2)
 154      Y2 = y0 + h * a21 * f(t + h * c1, Y1) + h * a22 * f(t + h * c2, Y2)
 155
 156      y = y0 + h * b1 * f(t + h * c1, Y1) + h * b2 * f(t + h * c2, Y2)
 157
 158      with constant coefficients a, b and c. For this method
 159      they are: b=[0.5 0.5] c=[(3-sqrt(3))/6 (3+sqrt(3))/6]
 160      a11=1/4, a12=(3-2*sqrt(3))/12, a21=(3+2*sqrt(3))/12 and a22=1/4
 161   */
 162
 163   const double ir3 = 1.0 / M_SQRT3;
 164   const int iter_steps = 3;
 165   int nu;
 166   size_t i;
 167
 168   double *const k1nu = state->k1nu;
 169   double *const k2nu = state->k2nu;
 170   double *const ytmp1 = state->ytmp1;
 171   double *const ytmp2 = state->ytmp2;
 172
 173   /* iterative solution of Y1 and Y2.
 174
 175      Note: This method does not check for convergence of the
 176      iterative solution!
 177   */
 178
 179   for (nu = 0; nu < iter_steps; nu++)
 180     {
 181       for (i = 0; i < dim; i++)
 182         {
 183           ytmp1[i] =
 184             y[i] + h * (0.25 * k1nu[i] + 0.5 * (0.5 - ir3) * k2nu[i]);
 185           ytmp2[i] =
 186             y[i] + h * (0.25 * k2nu[i] + 0.5 * (0.5 + ir3) * k1nu[i]);
 187         }
 188       {
 189         int s =
 190           GSL_ODEIV_FN_EVAL (sys, t + 0.5 * h * (1.0 - ir3), ytmp1, k1nu);
 191
 192         if (s != GSL_SUCCESS)
 193           {
 194             return s;
 195           }
 196       }
 197       {
 198         int s =
 199           GSL_ODEIV_FN_EVAL (sys, t + 0.5 * h * (1.0 + ir3), ytmp2, k2nu);
 200
 201         if (s != GSL_SUCCESS)
 202           {
 203             return s;
 204           }
 205       }
 206     }
 207
 208   /* assignment */
 209
 210   for (i = 0; i < dim; i++)
 211     {
 212       const double d_i = 0.5 * (k1nu[i] + k2nu[i]);
 213       y[i] += h * d_i;
 214     }
 215
 216   return GSL_SUCCESS;
 217 }
 218
 219 static int
 220 rk4imp_apply (void *vstate,
 221               size_t dim,
 222               double t,
 223               double h,
 224               double y[],
 225               double yerr[],
 226               const double dydt_in[],
 227               double dydt_out[],
 228               const gsl_odeiv_system * sys)
 229 {
 230   rk4imp_state_t *state = (rk4imp_state_t *) vstate;
 231
 232   size_t i;
 233
 234   double *y0 = state->y0;
 235   double *y0_orig = state->y0_orig;
 236   double *y_onestep = state->y_onestep;
 237   double *k1nu = state->k1nu;
 238   double *k2nu = state->k2nu;
 239
 240   /* Initialization step */
 241   DBL_MEMCPY (y0, y, dim);
 242
 243   /* Save initial values in case of failure */
 244   DBL_MEMCPY (y0_orig, y, dim);
 245
 246   if (dydt_in != 0)
 247     {
 248       DBL_MEMCPY (k1nu, dydt_in, dim);
 249     }
 250   else
 251     {
 252       int s = GSL_ODEIV_FN_EVAL (sys, t, y, k1nu);
 253
 254       if (s != GSL_SUCCESS)
 255         {
 256           return s;
 257         }
 258     }
 259
 260   DBL_MEMCPY (k2nu, k1nu, dim);
 261
 262   /* First traverse h with one step (save to y_onestep) */
 263
 264   DBL_MEMCPY (y_onestep, y, dim);
 265
 266   {
 267     int s = rk4imp_step (y_onestep, state, h, t, dim, sys);
 268
 269     if (s != GSL_SUCCESS)
 270       {
 271         return s;
 272       }
 273   }
 274
 275  /* Then with two steps with half step length (save to y) */
 276
 277   {
 278     int s = rk4imp_step (y, state, h/2.0, t, dim, sys);
 279
 280     if (s != GSL_SUCCESS)
 281       {
 282         /* Restore original y vector */
 283         DBL_MEMCPY (y, y0_orig, dim);
 284         return s;
 285       }
 286   }
 287
 288   DBL_MEMCPY (y0, y, dim);
 289
 290   {
 291     int s = GSL_ODEIV_FN_EVAL (sys, t + h/2.0, y, k1nu);
 292
 293     if (s != GSL_SUCCESS)
 294       {
 295         /* Restore original y vector */
 296         DBL_MEMCPY (y, y0_orig, dim);
 297         return s;
 298       }
 299   }
 300
 301   DBL_MEMCPY (k2nu, k1nu, dim);
 302
 303   {
 304     int s = rk4imp_step (y, state, h/2.0, t + h/2.0, dim, sys);
 305
 306     if (s != GSL_SUCCESS)
 307       {
 308         /* Restore original y vector */
 309         DBL_MEMCPY (y, y0_orig, dim);
 310         return s;
 311       }
 312   }
 313
 314   /* Derivatives at output */
 315
 316   if (dydt_out != NULL)
 317     {
 318       int s = GSL_ODEIV_FN_EVAL (sys, t + h, y, dydt_out);
 319
 320       if (s != GSL_SUCCESS) {
 321         /* Restore original y vector */
 322         DBL_MEMCPY (y, y0_orig, dim);
 323         return s;
 324       }
 325     }
 326
 327   /* Error estimation */
 328
 329   /* Denominator in step doubling error equation
 330    *  yerr = 0.5 * | y(onestep) - y(twosteps) | / (2^order - 1)
 331    */
 332
 333   for (i = 0; i < dim; i++)
 334     {
 335       yerr[i] = 8.0 * 0.5 * (y[i] - y_onestep[i]) / 15.0;
 336     }
 337
 338   return GSL_SUCCESS;
 339 }
 340
 341 static int
 342 rk4imp_reset (void *vstate, size_t dim)
 343 {
 344   rk4imp_state_t *state = (rk4imp_state_t *) vstate;
 345
 346   DBL_ZERO_MEMSET (state->y_onestep, dim);
 347   DBL_ZERO_MEMSET (state->y0_orig, dim);
 348   DBL_ZERO_MEMSET (state->y0, dim);
 349   DBL_ZERO_MEMSET (state->k1nu, dim);
 350   DBL_ZERO_MEMSET (state->k2nu, dim);
 351   DBL_ZERO_MEMSET (state->ytmp1, dim);
 352   DBL_ZERO_MEMSET (state->ytmp2, dim);
 353
 354   return GSL_SUCCESS;
 355 }
 356
 357 static unsigned int
 358 rk4imp_order (void *vstate)
 359 {
 360   rk4imp_state_t *state = (rk4imp_state_t *) vstate;
 361   state = 0; /* prevent warnings about unused parameters */
 362   return 4;
 363 }
 364
 365 static void
 366 rk4imp_free (void *vstate)
 367 {
 368   rk4imp_state_t *state = (rk4imp_state_t *) vstate;
 369
 370   free (state->y_onestep);
 371   free (state->y0_orig);
 372   free (state->y0);
 373   free (state->k1nu);
 374   free (state->k2nu);
 375   free (state->ytmp1);
 376   free (state->ytmp2);
 377   free (state);
 378 }
 379
 380 static const gsl_odeiv_step_type rk4imp_type = { "rk4imp",      /* name */
 381   1,                             /* can use dydt_in? */
 382   1,                             /* gives exact dydt_out? */
 383   &rk4imp_alloc,
 384   &rk4imp_apply,
 385   &rk4imp_reset,
 386   &rk4imp_order,
 387   &rk4imp_free
 388 };
 389
 390 const gsl_odeiv_step_type *gsl_odeiv_step_rk4imp = &rk4imp_type;