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

   1 /* ode-initval/rk2.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(3), Euler-Cauchy */
  21
  22 /* Author:  G. Jungman
  23  */
  24
  25 /* Reference: Abramowitz & Stegun, section 25.5. Runge-Kutta 2nd (25.5.7)
  26    and 3rd (25.5.8) order methods */
  27
  28 #include <config.h>
  29 #include <stdlib.h>
  30 #include <string.h>
  31 #include <gsl/gsl_errno.h>
  32 #include <gsl/gsl_odeiv.h>
  33
  34 #include "odeiv_util.h"
  35
  36 typedef struct
  37 {
  38   double *k1;
  39   double *k2;
  40   double *k3;
  41   double *ytmp;
  42 }
  43 rk2_state_t;
  44
  45 static void *
  46 rk2_alloc (size_t dim)
  47 {
  48   rk2_state_t *state = (rk2_state_t *) malloc (sizeof (rk2_state_t));
  49
  50   if (state == 0)
  51     {
  52       GSL_ERROR_NULL ("failed to allocate space for rk2_state", GSL_ENOMEM);
  53     }
  54
  55   state->k1 = (double *) malloc (dim * sizeof (double));
  56
  57   if (state->k1 == 0)
  58     {
  59       free (state);
  60       GSL_ERROR_NULL ("failed to allocate space for k1", GSL_ENOMEM);
  61     }
  62
  63   state->k2 = (double *) malloc (dim * sizeof (double));
  64
  65   if (state->k2 == 0)
  66     {
  67       free (state->k1);
  68       free (state);
  69       GSL_ERROR_NULL ("failed to allocate space for k2", GSL_ENOMEM);
  70     }
  71
  72   state->k3 = (double *) malloc (dim * sizeof (double));
  73
  74   if (state->k3 == 0)
  75     {
  76       free (state->k2);
  77       free (state->k1);
  78       free (state);
  79       GSL_ERROR_NULL ("failed to allocate space for k2", GSL_ENOMEM);
  80     }
  81
  82   state->ytmp = (double *) malloc (dim * sizeof (double));
  83
  84   if (state->ytmp == 0)
  85     {
  86       free (state->k3);
  87       free (state->k2);
  88       free (state->k1);
  89       free (state);
  90       GSL_ERROR_NULL ("failed to allocate space for k2", GSL_ENOMEM);
  91     }
  92
  93   return state;
  94 }
  95
  96
  97 static int
  98 rk2_apply (void *vstate,
  99            size_t dim,
 100            double t,
 101            double h,
 102            double y[],
 103            double yerr[],
 104            const double dydt_in[],
 105            double dydt_out[],
 106            const gsl_odeiv_system * sys)
 107 {
 108   rk2_state_t *state = (rk2_state_t *) vstate;
 109
 110   size_t i;
 111
 112   double *const k1 = state->k1;
 113   double *const k2 = state->k2;
 114   double *const k3 = state->k3;
 115   double *const ytmp = state->ytmp;
 116
 117   /* k1 step */
 118   /* k1 = f(t,y) */
 119
 120   if (dydt_in != NULL)
 121     {
 122       DBL_MEMCPY (k1, dydt_in, dim);
 123     }
 124   else
 125     {
 126       int s = GSL_ODEIV_FN_EVAL (sys, t, y, k1);
 127
 128       if (s != GSL_SUCCESS)
 129         {
 130           return s;
 131         }
 132     }
 133
 134   /* k2 step */
 135   /* k2 = f(t + 0.5*h, y + 0.5*k1) */
 136
 137   for (i = 0; i < dim; i++)
 138     {
 139       ytmp[i] = y[i] + 0.5 * h * k1[i];
 140     }
 141
 142   {
 143     int s = GSL_ODEIV_FN_EVAL (sys, t + 0.5 * h, ytmp, k2);
 144
 145     if (s != GSL_SUCCESS)
 146       {
 147         return s;
 148       }
 149   }
 150
 151   /* k3 step */
 152   /* for 3rd order estimates, is used for error estimation
 153      k3 = f(t + h, y - k1 + 2*k2) */
 154
 155   for (i = 0; i < dim; i++)
 156     {
 157       ytmp[i] = y[i] + h * (-k1[i] + 2.0 * k2[i]);
 158     }
 159
 160   {
 161     int s = GSL_ODEIV_FN_EVAL (sys, t + h, ytmp, k3);
 162
 163     if (s != GSL_SUCCESS)
 164       {
 165         return s;
 166       }
 167   }
 168
 169   /* final sum */
 170
 171   for (i = 0; i < dim; i++)
 172     {
 173       /* Save original values if derivative evaluation below fails */
 174       ytmp[i] = y[i];
 175
 176       {
 177         const double ksum3 = (k1[i] + 4.0 * k2[i] + k3[i]) / 6.0;
 178         y[i] += h * ksum3;
 179       }
 180     }
 181
 182   /* Derivatives at output */
 183
 184   if (dydt_out != NULL)
 185     {
 186       int s = GSL_ODEIV_FN_EVAL (sys, t + h, y, dydt_out);
 187
 188       if (s != GSL_SUCCESS)
 189         {
 190           /* Restore original values */
 191           DBL_MEMCPY (y, ytmp, dim);
 192
 193           return s;
 194         }
 195     }
 196
 197   /* Error estimation */
 198
 199   for (i = 0; i < dim; i++)
 200     {
 201       const double ksum3 = (k1[i] + 4.0 * k2[i] + k3[i]) / 6.0;
 202       yerr[i] = h * (k2[i] - ksum3);
 203     }
 204
 205   return GSL_SUCCESS;
 206 }
 207
 208 static int
 209 rk2_reset (void *vstate, size_t dim)
 210 {
 211   rk2_state_t *state = (rk2_state_t *) vstate;
 212
 213   DBL_ZERO_MEMSET (state->k1, dim);
 214   DBL_ZERO_MEMSET (state->k2, dim);
 215   DBL_ZERO_MEMSET (state->k3, dim);
 216   DBL_ZERO_MEMSET (state->ytmp, dim);
 217
 218   return GSL_SUCCESS;
 219 }
 220
 221 static unsigned int
 222 rk2_order (void *vstate)
 223 {
 224   rk2_state_t *state = (rk2_state_t *) vstate;
 225   state = 0; /* prevent warnings about unused parameters */
 226   return 2;
 227 }
 228
 229 static void
 230 rk2_free (void *vstate)
 231 {
 232   rk2_state_t *state = (rk2_state_t *) vstate;
 233   free (state->k1);
 234   free (state->k2);
 235   free (state->k3);
 236   free (state->ytmp);
 237   free (state);
 238 }
 239
 240 static const gsl_odeiv_step_type rk2_type = { "rk2",    /* name */
 241   1,                           /* can use dydt_in */
 242   1,                           /* gives exact dydt_out */
 243   &rk2_alloc,
 244   &rk2_apply,
 245   &rk2_reset,
 246   &rk2_order,
 247   &rk2_free
 248 };
 249
 250 const gsl_odeiv_step_type *gsl_odeiv_step_rk2 = &rk2_type;