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

   1 /* ode-initval/bsimp.c
   2  *
   3  * Copyright (C) 1996, 1997, 1998, 1999, 2000, 2004 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 /* Bulirsch-Stoer Implicit */
  21
  22 /* Author:  G. Jungman
  23  */
  24 /* Bader-Deuflhard implicit extrapolative stepper.
  25  * [Numer. Math., 41, 373 (1983)]
  26  */
  27 #include <config.h>
  28 #include <stdlib.h>
  29 #include <string.h>
  30 #include <gsl/gsl_math.h>
  31 #include <gsl/gsl_errno.h>
  32 #include <gsl/gsl_linalg.h>
  33 #include <gsl/gsl_odeiv.h>
  34
  35 #include "odeiv_util.h"
  36
  37 #define SEQUENCE_COUNT 8
  38 #define SEQUENCE_MAX   7
  39
  40 /* Bader-Deuflhard extrapolation sequence */
  41 static const int bd_sequence[SEQUENCE_COUNT] =
  42   { 2, 6, 10, 14, 22, 34, 50, 70 };
  43
  44 typedef struct
  45 {
  46   gsl_matrix *d;                /* workspace for extrapolation         */
  47   gsl_matrix *a_mat;            /* workspace for linear system matrix  */
  48   gsl_permutation *p_vec;       /* workspace for LU permutation        */
  49
  50   double x[SEQUENCE_MAX];       /* workspace for extrapolation */
  51
  52   /* state info */
  53   size_t k_current;
  54   size_t k_choice;
  55   double h_next;
  56   double eps;
  57
  58   /* workspace for extrapolation step */
  59   double *yp;
  60   double *y_save;
  61   double *yerr_save;
  62   double *y_extrap_save;
  63   double *y_extrap_sequence;
  64   double *extrap_work;
  65   double *dfdt;
  66   double *y_temp;
  67   double *delta_temp;
  68   double *weight;
  69   gsl_matrix *dfdy;
  70
  71   /* workspace for the basic stepper */
  72   double *rhs_temp;
  73   double *delta;
  74
  75   /* order of last step */
  76   size_t order;
  77 }
  78 bsimp_state_t;
  79
  80 /* Compute weighting factor */
  81
  82 static void
  83 compute_weights (const double y[], double w[], size_t dim)
  84 {
  85   size_t i;
  86   for (i = 0; i < dim; i++)
  87     {
  88       double u = fabs(y[i]);
  89       w[i] = (u > 0.0) ? u : 1.0;
  90     }
  91 }
  92
  93 /* Calculate a choice for the "order" of the method, using the
  94  * Deuflhard criteria.
  95  */
  96
  97 static size_t
  98 bsimp_deuf_kchoice (double eps, size_t dimension)
  99 {
 100   const double safety_f = 0.25;
 101   const double small_eps = safety_f * eps;
 102
 103   double a_work[SEQUENCE_COUNT];
 104   double alpha[SEQUENCE_MAX][SEQUENCE_MAX];
 105
 106   int i, k;
 107
 108   a_work[0] = bd_sequence[0] + 1.0;
 109
 110   for (k = 0; k < SEQUENCE_MAX; k++)
 111     {
 112       a_work[k + 1] = a_work[k] + bd_sequence[k + 1];
 113     }
 114
 115   for (i = 0; i < SEQUENCE_MAX; i++)
 116     {
 117       alpha[i][i] = 1.0;
 118       for (k = 0; k < i; k++)
 119         {
 120           const double tmp1 = a_work[k + 1] - a_work[i + 1];
 121           const double tmp2 = (a_work[i + 1] - a_work[0] + 1.0) * (2 * k + 1);
 122           alpha[k][i] = pow (small_eps, tmp1 / tmp2);
 123         }
 124     }
 125
 126   a_work[0] += dimension;
 127
 128   for (k = 0; k < SEQUENCE_MAX; k++)
 129     {
 130       a_work[k + 1] = a_work[k] + bd_sequence[k + 1];
 131     }
 132
 133   for (k = 0; k < SEQUENCE_MAX - 1; k++)
 134     {
 135       if (a_work[k + 2] > a_work[k + 1] * alpha[k][k + 1])
 136         break;
 137     }
 138
 139   return k;
 140 }
 141
 142 static void
 143 poly_extrap (gsl_matrix * d,
 144              const double x[],
 145              const unsigned int i_step,
 146              const double x_i,
 147              const double y_i[],
 148              double y_0[], double y_0_err[], double work[], const size_t dim)
 149 {
 150   size_t j, k;
 151
 152   DBL_MEMCPY (y_0_err, y_i, dim);
 153   DBL_MEMCPY (y_0, y_i, dim);
 154
 155   if (i_step == 0)
 156     {
 157       for (j = 0; j < dim; j++)
 158         {
 159           gsl_matrix_set (d, 0, j, y_i[j]);
 160         }
 161     }
 162   else
 163     {
 164       DBL_MEMCPY (work, y_i, dim);
 165
 166       for (k = 0; k < i_step; k++)
 167         {
 168           double delta = 1.0 / (x[i_step - k - 1] - x_i);
 169           const double f1 = delta * x_i;
 170           const double f2 = delta * x[i_step - k - 1];
 171
 172           for (j = 0; j < dim; j++)
 173             {
 174               const double q_kj = gsl_matrix_get (d, k, j);
 175               gsl_matrix_set (d, k, j, y_0_err[j]);
 176               delta = work[j] - q_kj;
 177               y_0_err[j] = f1 * delta;
 178               work[j] = f2 * delta;
 179               y_0[j] += y_0_err[j];
 180             }
 181         }
 182
 183       for (j = 0; j < dim; j++)
 184         {
 185           gsl_matrix_set (d, i_step, j, y_0_err[j]);
 186         }
 187     }
 188 }
 189
 190 /* Basic implicit Bulirsch-Stoer step.  Divide the step h_total into
 191  * n_step smaller steps and do the Bader-Deuflhard semi-implicit
 192  * iteration.  */
 193
 194 static int
 195 bsimp_step_local (void *vstate,
 196                   size_t dim,
 197                   const double t0,
 198                   const double h_total,
 199                   const unsigned int n_step,
 200                   const double y[],
 201                   const double yp[],
 202                   const double dfdt[],
 203                   const gsl_matrix * dfdy,
 204                   double y_out[],
 205                   const gsl_odeiv_system * sys)
 206 {
 207   bsimp_state_t *state = (bsimp_state_t *) vstate;
 208
 209   gsl_matrix *const a_mat = state->a_mat;
 210   gsl_permutation *const p_vec = state->p_vec;
 211
 212   double *const delta = state->delta;
 213   double *const y_temp = state->y_temp;
 214   double *const delta_temp = state->delta_temp;
 215   double *const rhs_temp = state->rhs_temp;
 216   double *const w = state->weight;
 217
 218   gsl_vector_view y_temp_vec = gsl_vector_view_array (y_temp, dim);
 219   gsl_vector_view delta_temp_vec = gsl_vector_view_array (delta_temp, dim);
 220   gsl_vector_view rhs_temp_vec = gsl_vector_view_array (rhs_temp, dim);
 221
 222   const double h = h_total / n_step;
 223   double t = t0 + h;
 224
 225   double sum;
 226
 227   /* This is the factor sigma referred to in equation 3.4 of the
 228      paper.  A relative change in y exceeding sigma indicates a
 229      runaway behavior. According to the authors suitable values for
 230      sigma are >>1.  I have chosen a value of 100*dim. BJG */
 231
 232   const double max_sum = 100.0 * dim;
 233
 234   int signum, status;
 235   size_t i, j;
 236   size_t n_inter;
 237
 238   /* Calculate the matrix for the linear system. */
 239   for (i = 0; i < dim; i++)
 240     {
 241       for (j = 0; j < dim; j++)
 242         {
 243           gsl_matrix_set (a_mat, i, j, -h * gsl_matrix_get (dfdy, i, j));
 244         }
 245       gsl_matrix_set (a_mat, i, i, gsl_matrix_get (a_mat, i, i) + 1.0);
 246     }
 247
 248   /* LU decomposition for the linear system. */
 249
 250   gsl_linalg_LU_decomp (a_mat, p_vec, &signum);
 251
 252   /* Compute weighting factors */
 253
 254   compute_weights (y, w, dim);
 255
 256   /* Initial step. */
 257
 258   for (i = 0; i < dim; i++)
 259     {
 260       y_temp[i] = h * (yp[i] + h * dfdt[i]);
 261     }
 262
 263   gsl_linalg_LU_solve (a_mat, p_vec, &y_temp_vec.vector, &delta_temp_vec.vector);
 264
 265   sum = 0.0;
 266
 267   for (i = 0; i < dim; i++)
 268     {
 269       const double di = delta_temp[i];
 270       delta[i] = di;
 271       y_temp[i] = y[i] + di;
 272       sum += fabs(di) / w[i];
 273     }
 274
 275   if (sum > max_sum)
 276     {
 277       return GSL_EFAILED ;
 278     }
 279
 280   /* Intermediate steps. */
 281
 282   status = GSL_ODEIV_FN_EVAL (sys, t, y_temp, y_out);
 283
 284   if (status)
 285     {
 286       return status;
 287     }
 288
 289   for (n_inter = 1; n_inter < n_step; n_inter++)
 290     {
 291       for (i = 0; i < dim; i++)
 292         {
 293           rhs_temp[i] = h * y_out[i] - delta[i];
 294         }
 295
 296       gsl_linalg_LU_solve (a_mat, p_vec, &rhs_temp_vec.vector, &delta_temp_vec.vector);
 297
 298       sum = 0.0;
 299
 300       for (i = 0; i < dim; i++)
 301         {
 302           delta[i] += 2.0 * delta_temp[i];
 303           y_temp[i] += delta[i];
 304           sum += fabs(delta[i]) / w[i];
 305         }
 306
 307       if (sum > max_sum)
 308         {
 309           return GSL_EFAILED ;
 310         }
 311
 312       t += h;
 313
 314       status = GSL_ODEIV_FN_EVAL (sys, t, y_temp, y_out);
 315
 316       if (status)
 317         {
 318           return status;
 319         }
 320     }
 321
 322
 323   /* Final step. */
 324
 325   for (i = 0; i < dim; i++)
 326     {
 327       rhs_temp[i] = h * y_out[i] - delta[i];
 328     }
 329
 330   gsl_linalg_LU_solve (a_mat, p_vec, &rhs_temp_vec.vector, &delta_temp_vec.vector);
 331
 332   sum = 0.0;
 333
 334   for (i = 0; i < dim; i++)
 335     {
 336       y_out[i] = y_temp[i] + delta_temp[i];
 337       sum += fabs(delta_temp[i]) / w[i];
 338     }
 339
 340   if (sum > max_sum)
 341     {
 342       return GSL_EFAILED ;
 343     }
 344
 345   return GSL_SUCCESS;
 346 }
 347
 348
 349 static void *
 350 bsimp_alloc (size_t dim)
 351 {
 352   bsimp_state_t *state = (bsimp_state_t *) malloc (sizeof (bsimp_state_t));
 353
 354   state->d = gsl_matrix_alloc (SEQUENCE_MAX, dim);
 355   state->a_mat = gsl_matrix_alloc (dim, dim);
 356   state->p_vec = gsl_permutation_alloc (dim);
 357
 358   state->yp = (double *) malloc (dim * sizeof (double));
 359   state->y_save = (double *) malloc (dim * sizeof (double));
 360   state->yerr_save = (double *) malloc (dim * sizeof (double));
 361   state->y_extrap_save = (double *) malloc (dim * sizeof (double));
 362   state->y_extrap_sequence = (double *) malloc (dim * sizeof (double));
 363   state->extrap_work = (double *) malloc (dim * sizeof (double));
 364   state->dfdt = (double *) malloc (dim * sizeof (double));
 365   state->y_temp = (double *) malloc (dim * sizeof (double));
 366   state->delta_temp = (double *) malloc (dim * sizeof(double));
 367   state->weight = (double *) malloc (dim * sizeof(double));
 368
 369   state->dfdy = gsl_matrix_alloc (dim, dim);
 370
 371   state->rhs_temp = (double *) malloc (dim * sizeof(double));
 372   state->delta = (double *) malloc (dim * sizeof (double));
 373
 374   {
 375     size_t k_choice = bsimp_deuf_kchoice (GSL_SQRT_DBL_EPSILON, dim); /*FIXME: choice of epsilon? */
 376     state->k_choice = k_choice;
 377     state->k_current = k_choice;
 378     state->order = 2 * k_choice;
 379   }
 380
 381   state->h_next = -GSL_SQRT_DBL_MAX;
 382
 383   return state;
 384 }
 385
 386 /* Perform the basic semi-implicit extrapolation
 387  * step, of size h, at a Deuflhard determined order.
 388  */
 389 static int
 390 bsimp_apply (void *vstate,
 391              size_t dim,
 392              double t,
 393              double h,
 394              double y[],
 395              double yerr[],
 396              const double dydt_in[],
 397              double dydt_out[],
 398              const gsl_odeiv_system * sys)
 399 {
 400   bsimp_state_t *state = (bsimp_state_t *) vstate;
 401
 402   double *const x = state->x;
 403   double *const yp = state->yp;
 404   double *const y_save = state->y_save;
 405   double *const yerr_save = state->yerr_save;
 406   double *const y_extrap_sequence = state->y_extrap_sequence;
 407   double *const y_extrap_save = state->y_extrap_save;
 408   double *const extrap_work = state->extrap_work;
 409   double *const dfdt = state->dfdt;
 410   gsl_matrix *d = state->d;
 411   gsl_matrix *dfdy = state->dfdy;
 412
 413   const double t_local = t;
 414   size_t i, k;
 415
 416   if (h + t_local == t_local)
 417     {
 418       return GSL_EUNDRFLW;      /* FIXME: error condition */
 419     }
 420
 421   DBL_MEMCPY (y_extrap_save, y, dim);
 422
 423   /* Save inputs */
 424   DBL_MEMCPY (y_save, y, dim);
 425   DBL_MEMCPY (yerr_save, yerr, dim);
 426
 427   /* Evaluate the derivative. */
 428   if (dydt_in != NULL)
 429     {
 430       DBL_MEMCPY (yp, dydt_in, dim);
 431     }
 432   else
 433     {
 434       int s = GSL_ODEIV_FN_EVAL (sys, t_local, y, yp);
 435
 436       if (s != GSL_SUCCESS)
 437         {
 438           return s;
 439         }
 440     }
 441
 442   /* Evaluate the Jacobian for the system. */
 443   {
 444     int s = GSL_ODEIV_JA_EVAL (sys, t_local, y, dfdy->data, dfdt);
 445
 446     if (s != GSL_SUCCESS)
 447       {
 448         return s;
 449       }
 450   }
 451
 452   /* Make a series of refined extrapolations,
 453    * up to the specified maximum order, which
 454    * was calculated based on the Deuflhard
 455    * criterion upon state initialization.  */
 456   for (k = 0; k <= state->k_current; k++)
 457     {
 458       const unsigned int N = bd_sequence[k];
 459       const double r = (h / N);
 460       const double x_k = r * r;
 461
 462       int status = bsimp_step_local (state,
 463                                      dim, t_local, h, N,
 464                                      y_extrap_save, yp,
 465                                      dfdt, dfdy,
 466                                      y_extrap_sequence,
 467                                      sys);
 468
 469       if (status == GSL_EFAILED)
 470         {
 471           /* If the local step fails, set the error to infinity in
 472              order to force a reduction in the step size */
 473
 474           for (i = 0; i < dim; i++)
 475             {
 476               yerr[i] = GSL_POSINF;
 477             }
 478
 479           break;
 480         }
 481
 482       else if (status != GSL_SUCCESS)
 483         {
 484           return status;
 485         }
 486
 487       x[k] = x_k;
 488
 489       poly_extrap (d, x, k, x_k, y_extrap_sequence, y, yerr, extrap_work, dim);
 490     }
 491
 492   /* Evaluate dydt_out[]. */
 493
 494   if (dydt_out != NULL)
 495     {
 496       int s = GSL_ODEIV_FN_EVAL (sys, t + h, y, dydt_out);
 497
 498       if (s != GSL_SUCCESS)
 499         {
 500           DBL_MEMCPY (y, y_save, dim);
 501           DBL_MEMCPY (yerr, yerr_save, dim);
 502           return s;
 503         }
 504     }
 505
 506   return GSL_SUCCESS;
 507 }
 508
 509 static unsigned int
 510 bsimp_order (void *vstate)
 511 {
 512   bsimp_state_t *state = (bsimp_state_t *) vstate;
 513   return state->order;
 514 }
 515
 516 static int
 517 bsimp_reset (void *vstate, size_t dim)
 518 {
 519   bsimp_state_t *state = (bsimp_state_t *) vstate;
 520
 521   state->h_next = 0;
 522
 523   DBL_ZERO_MEMSET (state->yp, dim);
 524
 525   return GSL_SUCCESS;
 526 }
 527
 528
 529 static void
 530 bsimp_free (void * vstate)
 531 {
 532   bsimp_state_t *state = (bsimp_state_t *) vstate;
 533
 534   free (state->delta);
 535   free (state->rhs_temp);
 536
 537   gsl_matrix_free (state->dfdy);
 538
 539   free (state->weight);
 540   free (state->delta_temp);
 541   free (state->y_temp);
 542   free (state->dfdt);
 543   free (state->extrap_work);
 544   free (state->y_extrap_sequence);
 545   free (state->y_extrap_save);
 546   free (state->y_save);
 547   free (state->yerr_save);
 548   free (state->yp);
 549
 550   gsl_permutation_free (state->p_vec);
 551   gsl_matrix_free (state->a_mat);
 552   gsl_matrix_free (state->d);
 553   free (state);
 554 }
 555
 556 static const gsl_odeiv_step_type bsimp_type = {
 557   "bsimp",                      /* name */
 558   1,                            /* can use dydt_in */
 559   1,                            /* gives exact dydt_out */
 560   &bsimp_alloc,
 561   &bsimp_apply,
 562   &bsimp_reset,
 563   &bsimp_order,
 564   &bsimp_free
 565 };
 566
 567 const gsl_odeiv_step_type *gsl_odeiv_step_bsimp = &bsimp_type;