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

   1 /* ode-initval/rk2imp.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 2, Gaussian implicit. Also known as the implicit
  21    midpoint rule. */
  22
  23 /* Author:  G. Jungman */
  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    The method is also described in eg. this reference.
  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 *Y1;
  43   double *y0;
  44   double *ytmp;
  45   double *y_onestep;
  46   double *y0_orig;
  47 }
  48 rk2imp_state_t;
  49
  50 static void *
  51 rk2imp_alloc (size_t dim)
  52 {
  53   rk2imp_state_t *state = (rk2imp_state_t *) malloc (sizeof (rk2imp_state_t));
  54
  55   if (state == 0)
  56     {
  57       GSL_ERROR_NULL ("failed to allocate space for rk2imp_state",
  58                       GSL_ENOMEM);
  59     }
  60
  61   state->Y1 = (double *) malloc (dim * sizeof (double));
  62
  63   if (state->Y1 == 0)
  64     {
  65       free (state);
  66       GSL_ERROR_NULL ("failed to allocate space for Y1", GSL_ENOMEM);
  67     }
  68
  69   state->ytmp = (double *) malloc (dim * sizeof (double));
  70
  71   if (state->ytmp == 0)
  72     {
  73       free (state->Y1);
  74       free (state);
  75       GSL_ERROR_NULL ("failed to allocate space for ytmp", GSL_ENOMEM);
  76     }
  77
  78   state->y0 = (double *) malloc (dim * sizeof (double));
  79
  80   if (state->y0 == 0)
  81     {
  82       free (state->Y1);
  83       free (state->ytmp);
  84       free (state);
  85       GSL_ERROR_NULL ("failed to allocate space for y0", GSL_ENOMEM);
  86     }
  87
  88   state->y_onestep = (double *) malloc (dim * sizeof (double));
  89
  90   if (state->y_onestep == 0)
  91     {
  92       free (state->Y1);
  93       free (state->ytmp);
  94       free (state->y0);
  95       free (state);
  96       GSL_ERROR_NULL ("failed to allocate space for y_onestep", GSL_ENOMEM);
  97     }
  98
  99   state->y0_orig = (double *) malloc (dim * sizeof (double));
 100
 101   if (state->y0_orig == 0)
 102     {
 103       free (state->y_onestep);
 104       free (state->Y1);
 105       free (state->ytmp);
 106       free (state->y0);
 107       free (state);
 108       GSL_ERROR_NULL ("failed to allocate space for y0_orig", GSL_ENOMEM);
 109     }
 110
 111   return state;
 112 }
 113
 114 static int
 115 rk2imp_step (double *y, rk2imp_state_t *state,
 116              const double h, const double t,
 117              const size_t dim, const gsl_odeiv_system *sys)
 118 {
 119   /* Makes a Runge-Kutta 2nd order implicit advance with step size h.
 120      y0 is initial values of variables y.
 121
 122      The implicit matrix equations to solve are:
 123
 124      Y1 = y0 + h/2 * f(t + h/2, Y1)
 125
 126      y = y0 + h * f(t + h/2, Y1)
 127   */
 128
 129   const double *y0 = state->y0;
 130   double *Y1 = state->Y1;
 131   double *ytmp = state->ytmp;
 132   int max_iter=3;
 133   int nu;
 134   size_t i;
 135
 136   /* iterative solution of Y1 = y0 + h/2 * f(t + h/2, Y1)
 137      Y1 should include initial values at call.
 138
 139      Note: This method does not check for convergence of the
 140      iterative solution!
 141   */
 142
 143   for (nu = 0; nu < max_iter; nu++)
 144     {
 145       for (i = 0; i < dim; i++)
 146         {
 147           ytmp[i] = y0[i] + 0.5 * h * Y1[i];
 148         }
 149
 150       {
 151         int s = GSL_ODEIV_FN_EVAL (sys, t + 0.5 * h, ytmp, Y1);
 152
 153         if (s != GSL_SUCCESS)
 154           {
 155             return s;
 156           }
 157       }
 158     }
 159
 160   /* assignment */
 161
 162   for (i = 0; i < dim; i++)
 163     {
 164       y[i] = y0[i] + h * Y1[i];
 165     }
 166
 167   return GSL_SUCCESS;
 168 }
 169
 170 static int
 171 rk2imp_apply (void *vstate,
 172               size_t dim,
 173               double t,
 174               double h,
 175               double y[],
 176               double yerr[],
 177               const double dydt_in[],
 178               double dydt_out[], const gsl_odeiv_system * sys)
 179 {
 180   rk2imp_state_t *state = (rk2imp_state_t *) vstate;
 181
 182   size_t i;
 183
 184   double *Y1 = state->Y1;
 185   double *y0 = state->y0;
 186   double *y_onestep = state->y_onestep;
 187   double *y0_orig = state->y0_orig;
 188
 189   /* Error estimation is done by step doubling procedure */
 190
 191   /* initialization step */
 192
 193   DBL_MEMCPY (y0, y, dim);
 194
 195   /* Save initial values for possible failures */
 196   DBL_MEMCPY (y0_orig, y, dim);
 197
 198   if (dydt_in != NULL)
 199     {
 200       DBL_MEMCPY (Y1, dydt_in, dim);
 201     }
 202
 203   else
 204     {
 205       int s = GSL_ODEIV_FN_EVAL (sys, t, y, Y1);
 206
 207       if (s != GSL_SUCCESS)
 208         {
 209           return s;
 210         }
 211     }
 212
 213   /* First traverse h with one step (save to y_onestep) */
 214
 215   DBL_MEMCPY (y_onestep, y, dim);
 216
 217   {
 218     int s = rk2imp_step (y_onestep, state, h, t, dim, sys);
 219
 220     if (s != GSL_SUCCESS)
 221       {
 222         return s;
 223       }
 224   }
 225
 226  /* Then with two steps with half step length (save to y) */
 227
 228   {
 229     int s = rk2imp_step (y, state, h / 2.0, t, dim, sys);
 230
 231     if (s != GSL_SUCCESS)
 232       {
 233         /* Restore original y vector */
 234         DBL_MEMCPY (y, y0_orig, dim);
 235
 236         return s;
 237       }
 238   }
 239
 240   DBL_MEMCPY (y0, y, dim);
 241
 242   {
 243     int s = GSL_ODEIV_FN_EVAL (sys, t + h / 2.0, y, Y1);
 244
 245     if (s != GSL_SUCCESS)
 246       {
 247         /* Restore original y vector */
 248         DBL_MEMCPY (y, y0_orig, dim);
 249
 250         return s;
 251       }
 252   }
 253
 254   {
 255     int s = rk2imp_step (y, state, h / 2.0, t + h / 2.0, dim, sys);
 256
 257     if (s != GSL_SUCCESS)
 258       {
 259         /* Restore original y vector */
 260         DBL_MEMCPY (y, y0_orig, dim);
 261
 262         return s;
 263       }
 264   }
 265
 266   /* Derivatives at output */
 267
 268   if (dydt_out != NULL)
 269     {
 270       int s = GSL_ODEIV_FN_EVAL (sys, t + h, y, dydt_out);
 271
 272       if (s != GSL_SUCCESS)
 273         {
 274           /* Restore original y vector */
 275           DBL_MEMCPY (y, y0_orig, dim);
 276
 277           return s;
 278         }
 279     }
 280
 281   /* Error estimation */
 282
 283   for (i = 0; i < dim; i++)
 284     {
 285       yerr[i] = 4.0 * (y[i] - y_onestep[i]) / 3.0;
 286     }
 287
 288   return GSL_SUCCESS;
 289 }
 290
 291 static int
 292 rk2imp_reset (void *vstate, size_t dim)
 293 {
 294   rk2imp_state_t *state = (rk2imp_state_t *) vstate;
 295
 296   DBL_ZERO_MEMSET (state->Y1, dim);
 297   DBL_ZERO_MEMSET (state->ytmp, dim);
 298   DBL_ZERO_MEMSET (state->y0, dim);
 299   DBL_ZERO_MEMSET (state->y_onestep, dim);
 300   DBL_ZERO_MEMSET (state->y0_orig, dim);
 301
 302   return GSL_SUCCESS;
 303 }
 304
 305 static unsigned int
 306 rk2imp_order (void *vstate)
 307 {
 308   rk2imp_state_t *state = (rk2imp_state_t *) vstate;
 309   state = 0; /* prevent warnings about unused parameters */
 310   return 2;
 311 }
 312
 313 static void
 314 rk2imp_free (void *vstate)
 315 {
 316   rk2imp_state_t *state = (rk2imp_state_t *) vstate;
 317
 318   free (state->Y1);
 319   free (state->ytmp);
 320   free (state->y0);
 321   free (state->y_onestep);
 322   free (state->y0_orig);
 323   free (state);
 324 }
 325
 326 static const gsl_odeiv_step_type rk2imp_type = { "rk2imp",      /* name */
 327   1,                            /* can use dydt_in */
 328   1,                            /* gives exact dydt_out */
 329   &rk2imp_alloc,
 330   &rk2imp_apply,
 331   &rk2imp_reset,
 332   &rk2imp_order,
 333   &rk2imp_free
 334 };
 335
 336 const gsl_odeiv_step_type *gsl_odeiv_step_rk2imp = &rk2imp_type;