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

   1 /* ode-initval/rk2simp.c
   2  *
   3  * Copyright (C) 2004 Tuomo Keskitalo
   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 2, Gaussian implicit. Also known as implicit midpoint rule.
  21
  22    Non-linear equations solved by linearization, LU-decomposition
  23    and matrix inversion. For reference, see eg.
  24
  25    Ascher, U.M., Petzold, L.R., Computer methods for ordinary
  26    differential and differential-algebraic equations, SIAM,
  27    Philadelphia, 1998.
  28  */
  29
  30 #include <config.h>
  31 #include <stdlib.h>
  32 #include <string.h>
  33 #include <gsl/gsl_math.h>
  34 #include <gsl/gsl_errno.h>
  35 #include <gsl/gsl_odeiv.h>
  36 #include <gsl/gsl_linalg.h>
  37
  38 #include "odeiv_util.h"
  39
  40 typedef struct
  41 {
  42   double *Y1;
  43   double *y0;
  44   double *y0_orig;
  45   double *ytmp;
  46   double *dfdy;                 /* Jacobian */
  47   double *dfdt;                 /* time derivatives, not used */
  48   double *y_onestep;
  49   gsl_permutation *p;
  50 }
  51 rk2simp_state_t;
  52
  53 static void *
  54 rk2simp_alloc (size_t dim)
  55 {
  56   rk2simp_state_t *state =
  57     (rk2simp_state_t *) malloc (sizeof (rk2simp_state_t));
  58
  59   if (state == 0)
  60     {
  61       GSL_ERROR_NULL ("failed to allocate space for rk2simp_state",
  62                       GSL_ENOMEM);
  63     }
  64
  65   state->Y1 = (double *) malloc (dim * sizeof (double));
  66
  67   if (state->Y1 == 0)
  68     {
  69       free (state);
  70       GSL_ERROR_NULL ("failed to allocate space for Y1", GSL_ENOMEM);
  71     }
  72
  73   state->y0 = (double *) malloc (dim * sizeof (double));
  74
  75   if (state->y0 == 0)
  76     {
  77       free (state->Y1);
  78       free (state);
  79       GSL_ERROR_NULL ("failed to allocate space for y0", GSL_ENOMEM);
  80     }
  81
  82   state->y0_orig = (double *) malloc (dim * sizeof (double));
  83
  84   if (state->y0_orig == 0)
  85     {
  86       free (state->Y1);
  87       free (state->y0);
  88       free (state);
  89       GSL_ERROR_NULL ("failed to allocate space for y0_orig", GSL_ENOMEM);
  90     }
  91
  92   state->ytmp = (double *) malloc (dim * sizeof (double));
  93
  94   if (state->ytmp == 0)
  95     {
  96       free (state->Y1);
  97       free (state->y0);
  98       free (state->y0_orig);
  99       free (state);
 100       GSL_ERROR_NULL ("failed to allocate space for ytmp", GSL_ENOMEM);
 101     }
 102
 103   state->dfdy = (double *) malloc (dim * dim * sizeof (double));
 104
 105   if (state->dfdy == 0)
 106     {
 107       free (state->Y1);
 108       free (state->y0);
 109       free (state->y0_orig);
 110       free (state->ytmp);
 111       free (state);
 112       GSL_ERROR_NULL ("failed to allocate space for dfdy", GSL_ENOMEM);
 113     }
 114
 115   state->dfdt = (double *) malloc (dim * sizeof (double));
 116
 117   if (state->dfdt == 0)
 118     {
 119       free (state->Y1);
 120       free (state->y0);
 121       free (state->y0_orig);
 122       free (state->ytmp);
 123       free (state->dfdy);
 124       free (state);
 125       GSL_ERROR_NULL ("failed to allocate space for dfdt", GSL_ENOMEM);
 126     }
 127
 128   state->y_onestep = (double *) malloc (dim * sizeof (double));
 129
 130   if (state->y_onestep == 0)
 131     {
 132       free (state->Y1);
 133       free (state->y0);
 134       free (state->y0_orig);
 135       free (state->ytmp);
 136       free (state->dfdy);
 137       free (state->dfdt);
 138       free (state);
 139       GSL_ERROR_NULL ("failed to allocate space for y_onestep", GSL_ENOMEM);
 140     }
 141
 142   state->p = gsl_permutation_alloc (dim);
 143
 144   if (state->p == 0)
 145     {
 146       free (state->Y1);
 147       free (state->y0);
 148       free (state->y0_orig);
 149       free (state->ytmp);
 150       free (state->dfdy);
 151       free (state->dfdt);
 152       free (state);
 153       GSL_ERROR_NULL ("failed to allocate space for p", GSL_ENOMEM);
 154     }
 155
 156   return state;
 157 }
 158
 159
 160 static int
 161 rk2simp_step (double *y, rk2simp_state_t * state,
 162               const double h, const double t,
 163               const size_t dim, const gsl_odeiv_system * sys)
 164 {
 165   /* Makes a Runge-Kutta 2nd order semi-implicit advance with step size h.
 166      y0 is initial values of variables y.
 167
 168      The linearized semi-implicit equations to calculate are:
 169
 170      Y1 = y0 + h/2 * (1 - h/2 * df/dy)^(-1) * f(t + h/2, y0)
 171
 172      y = y0 + h * f(t + h/2, Y1)
 173    */
 174
 175   const double *y0 = state->y0;
 176   double *Y1 = state->Y1;
 177   double *ytmp = state->ytmp;
 178
 179   size_t i;
 180   int s, ps;
 181
 182   gsl_matrix_view J = gsl_matrix_view_array (state->dfdy, dim, dim);
 183
 184   /* First solve Y1.
 185      Calculate the inverse matrix (1 - h/2 * df/dy)^-1
 186    */
 187
 188   /* Create matrix to J */
 189
 190   s = GSL_ODEIV_JA_EVAL (sys, t, y0, state->dfdy, state->dfdt);
 191
 192   if (s != GSL_SUCCESS)
 193     {
 194       return s;
 195     }
 196
 197   gsl_matrix_scale (&J.matrix, -h / 2.0);
 198   gsl_matrix_add_diagonal(&J.matrix, 1.0);
 199
 200   /* Invert it by LU-decomposition to invmat */
 201
 202   s += gsl_linalg_LU_decomp (&J.matrix, state->p, &ps);
 203
 204   if (s != GSL_SUCCESS)
 205     {
 206       return GSL_EFAILED;
 207     }
 208
 209   /* Evaluate f(t + h/2, y0) */
 210
 211   s = GSL_ODEIV_FN_EVAL (sys, t + 0.5 * h, y0, ytmp);
 212
 213   if (s != GSL_SUCCESS)
 214     {
 215       return s;
 216     }
 217
 218   /* Calculate Y1 = y0 + h/2 * ((1-h/2 * df/dy)^-1) ytmp */
 219
 220   {
 221     gsl_vector_const_view y0_view = gsl_vector_const_view_array(y0, dim);
 222     gsl_vector_view ytmp_view = gsl_vector_view_array(ytmp, dim);
 223     gsl_vector_view Y1_view = gsl_vector_view_array(Y1, dim);
 224
 225     s = gsl_linalg_LU_solve (&J.matrix, state->p,
 226                              &ytmp_view.vector, &Y1_view.vector);
 227
 228     gsl_vector_scale (&Y1_view.vector, 0.5 * h);
 229     gsl_vector_add (&Y1_view.vector, &y0_view.vector);
 230   }
 231
 232   /* And finally evaluation of f(t + h/2, Y1) and calculation of y */
 233
 234   s = GSL_ODEIV_FN_EVAL (sys, t + 0.5 * h, Y1, ytmp);
 235
 236   if (s != GSL_SUCCESS)
 237     {
 238       return s;
 239     }
 240
 241   for (i = 0; i < dim; i++)
 242     {
 243       y[i] = y0[i] + h * ytmp[i];
 244     }
 245
 246   return s;
 247 }
 248
 249 static int
 250 rk2simp_apply (void *vstate, size_t dim, double t, double h,
 251                double y[], double yerr[], const double dydt_in[],
 252                double dydt_out[], const gsl_odeiv_system * sys)
 253 {
 254   rk2simp_state_t *state = (rk2simp_state_t *) vstate;
 255
 256   size_t i;
 257
 258   double *y0 = state->y0;
 259   double *y0_orig = state->y0_orig;
 260   double *y_onestep = state->y_onestep;
 261
 262   /* Error estimation is done by step doubling procedure */
 263
 264   DBL_MEMCPY (y0, y, dim);
 265
 266   /* Save initial values in case of failure */
 267   DBL_MEMCPY (y0_orig, y, dim);
 268
 269   /* First traverse h with one step (save to y_onestep) */
 270   DBL_MEMCPY (y_onestep, y, dim);
 271
 272   {
 273     int s = rk2simp_step (y_onestep, state, h, t, dim, sys);
 274
 275     if (s != GSL_SUCCESS)
 276       {
 277         return s;
 278       }
 279   }
 280
 281   /* Then with two steps with half step length (save to y) */
 282
 283   {
 284     int s = rk2simp_step (y, state, h / 2.0, t, dim, sys);
 285
 286     if (s != GSL_SUCCESS)
 287       {
 288         /* Restore original y vector */
 289         DBL_MEMCPY (y, y0_orig, dim);
 290         return s;
 291       }
 292   }
 293
 294   DBL_MEMCPY (y0, y, dim);
 295
 296   {
 297     int s = rk2simp_step (y, state, h / 2.0, t + h / 2.0, dim, sys);
 298
 299     if (s != GSL_SUCCESS)
 300       {
 301         /* Restore original y vector */
 302         DBL_MEMCPY (y, y0_orig, dim);
 303         return s;
 304       }
 305   }
 306
 307   /* Derivatives at output */
 308
 309   if (dydt_out != NULL)
 310     {
 311       int s = GSL_ODEIV_FN_EVAL (sys, t + h, y, dydt_out);
 312
 313       if (s != GSL_SUCCESS)
 314         {
 315           /* Restore original y vector */
 316           DBL_MEMCPY (y, y0_orig, dim);
 317           return s;
 318         }
 319     }
 320
 321   /* Error estimation */
 322
 323   for (i = 0; i < dim; i++)
 324     {
 325       yerr[i] = 4.0 * (y[i] - y_onestep[i]) / 3.0;
 326     }
 327
 328   return GSL_SUCCESS;
 329 }
 330
 331
 332 static int
 333 rk2simp_reset (void *vstate, size_t dim)
 334 {
 335   rk2simp_state_t *state = (rk2simp_state_t *) vstate;
 336
 337   DBL_ZERO_MEMSET (state->Y1, dim);
 338   DBL_ZERO_MEMSET (state->y0, dim);
 339   DBL_ZERO_MEMSET (state->y0_orig, dim);
 340   DBL_ZERO_MEMSET (state->ytmp, dim);
 341   DBL_ZERO_MEMSET (state->dfdt, dim * dim);
 342   DBL_ZERO_MEMSET (state->dfdt, dim);
 343   DBL_ZERO_MEMSET (state->y_onestep, dim);
 344
 345   return GSL_SUCCESS;
 346 }
 347
 348 static unsigned int
 349 rk2simp_order (void *vstate)
 350 {
 351   rk2simp_state_t *state = (rk2simp_state_t *) vstate;
 352   state = 0;                    /* prevent warnings about unused parameters */
 353   return 2;
 354 }
 355
 356 static void
 357 rk2simp_free (void *vstate)
 358 {
 359   rk2simp_state_t *state = (rk2simp_state_t *) vstate;
 360   free (state->Y1);
 361   free (state->y0);
 362   free (state->y0_orig);
 363   free (state->ytmp);
 364   free (state->dfdy);
 365   free (state->dfdt);
 366   free (state->y_onestep);
 367   gsl_permutation_free (state->p);
 368   free (state);
 369 }
 370
 371 static const gsl_odeiv_step_type rk2simp_type = {
 372   "rk2simp",                    /* name */
 373   0,                            /* can use dydt_in? */
 374   1,                            /* gives exact dydt_out? */
 375   &rk2simp_alloc,
 376   &rk2simp_apply,
 377   &rk2simp_reset,
 378   &rk2simp_order,
 379   &rk2simp_free
 380 };
 381
 382 const gsl_odeiv_step_type *gsl_odeiv_step_rk2simp = &rk2simp_type;