htswanalysis/MACS/lib/gsl/gsl-1.11/interpolation/cspline.c

   1 /* interpolation/cspline.c
   2  *
   3  * Copyright (C) 1996, 1997, 1998, 1999, 2000, 2001, 2002, 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 /* Author:  G. Jungman
  21  */
  22 #include <config.h>
  23 #include <stdlib.h>
  24 #include <gsl/gsl_errno.h>
  25 #include <gsl/gsl_linalg.h>
  26 #include <gsl/gsl_vector.h>
  27 #include "integ_eval.h"
  28 #include <gsl/gsl_interp.h>
  29
  30 typedef struct
  31 {
  32   double * c;
  33   double * g;
  34   double * diag;
  35   double * offdiag;
  36 } cspline_state_t;
  37
  38
  39 /* common initialization */
  40 static void *
  41 cspline_alloc (size_t size)
  42 {
  43   cspline_state_t * state = (cspline_state_t *) malloc (sizeof (cspline_state_t));
  44
  45   if (state == NULL)
  46     {
  47       GSL_ERROR_NULL("failed to allocate space for state", GSL_ENOMEM);
  48     }
  49
  50   state->c = (double *) malloc (size * sizeof (double));
  51
  52   if (state->c == NULL)
  53     {
  54       free (state);
  55       GSL_ERROR_NULL("failed to allocate space for c", GSL_ENOMEM);
  56     }
  57
  58   state->g = (double *) malloc (size * sizeof (double));
  59
  60   if (state->g == NULL)
  61     {
  62       free (state->c);
  63       free (state);
  64       GSL_ERROR_NULL("failed to allocate space for g", GSL_ENOMEM);
  65     }
  66
  67   state->diag = (double *) malloc (size * sizeof (double));
  68
  69   if (state->diag == NULL)
  70     {
  71       free (state->g);
  72       free (state->c);
  73       free (state);
  74       GSL_ERROR_NULL("failed to allocate space for diag", GSL_ENOMEM);
  75     }
  76
  77   state->offdiag = (double *) malloc (size * sizeof (double));
  78
  79   if (state->offdiag == NULL)
  80     {
  81       free (state->diag);
  82       free (state->g);
  83       free (state->c);
  84       free (state);
  85       GSL_ERROR_NULL("failed to allocate space for offdiag", GSL_ENOMEM);
  86     }
  87
  88   return state;
  89 }
  90
  91
  92 /* natural spline calculation
  93  * see [Engeln-Mullges + Uhlig, p. 254]
  94  */
  95 static int
  96 cspline_init (void * vstate, const double xa[], const double ya[],
  97               size_t size)
  98 {
  99   cspline_state_t *state = (cspline_state_t *) vstate;
 100
 101   size_t i;
 102   size_t num_points = size;
 103   size_t max_index = num_points - 1;  /* Engeln-Mullges + Uhlig "n" */
 104   size_t sys_size = max_index - 1;    /* linear system is sys_size x sys_size */
 105
 106   state->c[0] = 0.0;
 107   state->c[max_index] = 0.0;
 108
 109   for (i = 0; i < sys_size; i++)
 110     {
 111       const double h_i   = xa[i + 1] - xa[i];
 112       const double h_ip1 = xa[i + 2] - xa[i + 1];
 113       const double ydiff_i   = ya[i + 1] - ya[i];
 114       const double ydiff_ip1 = ya[i + 2] - ya[i + 1];
 115       const double g_i = (h_i != 0.0) ? 1.0 / h_i : 0.0;
 116       const double g_ip1 = (h_ip1 != 0.0) ? 1.0 / h_ip1 : 0.0;
 117       state->offdiag[i] = h_ip1;
 118       state->diag[i] = 2.0 * (h_ip1 + h_i);
 119       state->g[i] = 3.0 * (ydiff_ip1 * g_ip1 -  ydiff_i * g_i);
 120     }
 121
 122   if (sys_size == 1)
 123     {
 124       state->c[1] = state->g[0] / state->diag[0];
 125       return GSL_SUCCESS;
 126     }
 127   else
 128     {
 129       gsl_vector_view g_vec = gsl_vector_view_array(state->g, sys_size);
 130       gsl_vector_view diag_vec = gsl_vector_view_array(state->diag, sys_size);
 131       gsl_vector_view offdiag_vec = gsl_vector_view_array(state->offdiag, sys_size - 1);
 132       gsl_vector_view solution_vec = gsl_vector_view_array ((state->c) + 1, sys_size);
 133
 134       int status = gsl_linalg_solve_symm_tridiag(&diag_vec.vector,
 135                                                  &offdiag_vec.vector,
 136                                                  &g_vec.vector,
 137                                                  &solution_vec.vector);
 138       return status;
 139     }
 140 }
 141
 142
 143 /* periodic spline calculation
 144  * see [Engeln-Mullges + Uhlig, p. 256]
 145  */
 146 static int
 147 cspline_init_periodic (void * vstate, const double xa[], const double ya[],
 148                        size_t size)
 149 {
 150   cspline_state_t *state = (cspline_state_t *) vstate;
 151
 152   size_t i;
 153   size_t num_points = size;
 154   size_t max_index = num_points - 1;  /* Engeln-Mullges + Uhlig "n" */
 155   size_t sys_size = max_index;    /* linear system is sys_size x sys_size */
 156
 157   if (sys_size == 2) {
 158     /* solve 2x2 system */
 159
 160     const double h0 = xa[1] - xa[0];
 161     const double h1 = xa[2] - xa[1];
 162
 163     const double A = 2.0*(h0 + h1);
 164     const double B = h0 + h1;
 165     double g[2];
 166     double det;
 167
 168     g[0] = 3.0 * ((ya[2] - ya[1]) / h1 - (ya[1] - ya[0]) / h0);
 169     g[1] = 3.0 * ((ya[1] - ya[2]) / h0 - (ya[2] - ya[1]) / h1);
 170
 171     det = 3.0 * (h0 + h1) * (h0 + h1);
 172     state->c[1] = ( A * g[0] - B * g[1])/det;
 173     state->c[2] = (-B * g[0] + A * g[1])/det;
 174     state->c[0] = state->c[2];
 175
 176     return GSL_SUCCESS;
 177   } else {
 178
 179     for (i = 0; i < sys_size-1; i++) {
 180       const double h_i       = xa[i + 1] - xa[i];
 181       const double h_ip1     = xa[i + 2] - xa[i + 1];
 182       const double ydiff_i   = ya[i + 1] - ya[i];
 183       const double ydiff_ip1 = ya[i + 2] - ya[i + 1];
 184       const double g_i = (h_i != 0.0) ? 1.0 / h_i : 0.0;
 185       const double g_ip1 = (h_ip1 != 0.0) ? 1.0 / h_ip1 : 0.0;
 186       state->offdiag[i] = h_ip1;
 187       state->diag[i] = 2.0 * (h_ip1 + h_i);
 188       state->g[i] = 3.0 * (ydiff_ip1 * g_ip1 - ydiff_i * g_i);
 189     }
 190
 191     i = sys_size - 1;
 192
 193     {
 194       const double h_i       = xa[i + 1] - xa[i];
 195       const double h_ip1     = xa[1] - xa[0];
 196       const double ydiff_i   = ya[i + 1] - ya[i];
 197       const double ydiff_ip1 = ya[1] - ya[0];
 198       const double g_i = (h_i != 0.0) ? 1.0 / h_i : 0.0;
 199       const double g_ip1 = (h_ip1 != 0.0) ? 1.0 / h_ip1 : 0.0;
 200       state->offdiag[i] = h_ip1;
 201       state->diag[i] = 2.0 * (h_ip1 + h_i);
 202       state->g[i] = 3.0 * (ydiff_ip1 * g_ip1 - ydiff_i * g_i);
 203     }
 204
 205     {
 206       gsl_vector_view g_vec = gsl_vector_view_array(state->g, sys_size);
 207       gsl_vector_view diag_vec = gsl_vector_view_array(state->diag, sys_size);
 208       gsl_vector_view offdiag_vec = gsl_vector_view_array(state->offdiag, sys_size);
 209       gsl_vector_view solution_vec = gsl_vector_view_array ((state->c) + 1, sys_size);
 210
 211       int status = gsl_linalg_solve_symm_cyc_tridiag(&diag_vec.vector,
 212                                                      &offdiag_vec.vector,
 213                                                      &g_vec.vector,
 214                                                      &solution_vec.vector);
 215       state->c[0] = state->c[max_index];
 216
 217       return status;
 218     }
 219   }
 220 }
 221
 222
 223 static
 224 void
 225 cspline_free (void * vstate)
 226 {
 227   cspline_state_t *state = (cspline_state_t *) vstate;
 228
 229   free (state->c);
 230   free (state->g);
 231   free (state->diag);
 232   free (state->offdiag);
 233   free (state);
 234 }
 235
 236 /* function for common coefficient determination
 237  */
 238 static inline void
 239 coeff_calc (const double c_array[], double dy, double dx, size_t index,
 240             double * b, double * c, double * d)
 241 {
 242   const double c_i = c_array[index];
 243   const double c_ip1 = c_array[index + 1];
 244   *b = (dy / dx) - dx * (c_ip1 + 2.0 * c_i) / 3.0;
 245   *c = c_i;
 246   *d = (c_ip1 - c_i) / (3.0 * dx);
 247 }
 248
 249
 250 static
 251 int
 252 cspline_eval (const void * vstate,
 253               const double x_array[], const double y_array[], size_t size,
 254               double x,
 255               gsl_interp_accel * a,
 256               double *y)
 257 {
 258   const cspline_state_t *state = (const cspline_state_t *) vstate;
 259
 260   double x_lo, x_hi;
 261   double dx;
 262   size_t index;
 263
 264   if (a != 0)
 265     {
 266       index = gsl_interp_accel_find (a, x_array, size, x);
 267     }
 268   else
 269     {
 270       index = gsl_interp_bsearch (x_array, x, 0, size - 1);
 271     }
 272
 273   /* evaluate */
 274   x_hi = x_array[index + 1];
 275   x_lo = x_array[index];
 276   dx = x_hi - x_lo;
 277   if (dx > 0.0)
 278     {
 279       const double y_lo = y_array[index];
 280       const double y_hi = y_array[index + 1];
 281       const double dy = y_hi - y_lo;
 282       double delx = x - x_lo;
 283       double b_i, c_i, d_i;
 284       coeff_calc(state->c, dy, dx, index,  &b_i, &c_i, &d_i);
 285       *y = y_lo + delx * (b_i + delx * (c_i + delx * d_i));
 286       return GSL_SUCCESS;
 287     }
 288   else
 289     {
 290       *y = 0.0;
 291       return GSL_EINVAL;
 292     }
 293 }
 294
 295
 296 static
 297 int
 298 cspline_eval_deriv (const void * vstate,
 299                     const double x_array[], const double y_array[], size_t size,
 300                     double x,
 301                     gsl_interp_accel * a,
 302                     double *dydx)
 303 {
 304   const cspline_state_t *state = (const cspline_state_t *) vstate;
 305
 306   double x_lo, x_hi;
 307   double dx;
 308   size_t index;
 309
 310   if (a != 0)
 311     {
 312       index = gsl_interp_accel_find (a, x_array, size, x);
 313     }
 314   else
 315     {
 316       index = gsl_interp_bsearch (x_array, x, 0, size - 1);
 317     }
 318
 319   /* evaluate */
 320   x_hi = x_array[index + 1];
 321   x_lo = x_array[index];
 322   dx = x_hi - x_lo;
 323   if (dx > 0.0)
 324     {
 325       const double y_lo = y_array[index];
 326       const double y_hi = y_array[index + 1];
 327       const double dy = y_hi - y_lo;
 328       double delx = x - x_lo;
 329       double b_i, c_i, d_i;
 330       coeff_calc(state->c, dy, dx, index,  &b_i, &c_i, &d_i);
 331       *dydx = b_i + delx * (2.0 * c_i + 3.0 * d_i * delx);
 332       return GSL_SUCCESS;
 333     }
 334   else
 335     {
 336       *dydx = 0.0;
 337       return GSL_FAILURE;
 338     }
 339 }
 340
 341
 342 static
 343 int
 344 cspline_eval_deriv2 (const void * vstate,
 345                      const double x_array[], const double y_array[], size_t size,
 346                      double x,
 347                      gsl_interp_accel * a,
 348                      double * y_pp)
 349 {
 350   const cspline_state_t *state = (const cspline_state_t *) vstate;
 351
 352   double x_lo, x_hi;
 353   double dx;
 354   size_t index;
 355
 356   if (a != 0)
 357     {
 358       index = gsl_interp_accel_find (a, x_array, size, x);
 359     }
 360   else
 361     {
 362       index = gsl_interp_bsearch (x_array, x, 0, size - 1);
 363     }
 364
 365   /* evaluate */
 366   x_hi = x_array[index + 1];
 367   x_lo = x_array[index];
 368   dx = x_hi - x_lo;
 369   if (dx > 0.0)
 370     {
 371       const double y_lo = y_array[index];
 372       const double y_hi = y_array[index + 1];
 373       const double dy = y_hi - y_lo;
 374       double delx = x - x_lo;
 375       double b_i, c_i, d_i;
 376       coeff_calc(state->c, dy, dx, index,  &b_i, &c_i, &d_i);
 377       *y_pp = 2.0 * c_i + 6.0 * d_i * delx;
 378       return GSL_SUCCESS;
 379     }
 380   else
 381     {
 382       *y_pp = 0.0;
 383       return GSL_FAILURE;
 384     }
 385 }
 386
 387
 388 static
 389 int
 390 cspline_eval_integ (const void * vstate,
 391                     const double x_array[], const double y_array[], size_t size,
 392                     gsl_interp_accel * acc,
 393                     double a, double b,
 394                     double * result)
 395 {
 396   const cspline_state_t *state = (const cspline_state_t *) vstate;
 397
 398   size_t i, index_a, index_b;
 399
 400   if (acc != 0)
 401     {
 402       index_a = gsl_interp_accel_find (acc, x_array, size, a);
 403       index_b = gsl_interp_accel_find (acc, x_array, size, b);
 404     }
 405   else
 406     {
 407       index_a = gsl_interp_bsearch (x_array, a, 0, size - 1);
 408       index_b = gsl_interp_bsearch (x_array, b, 0, size - 1);
 409     }
 410
 411   *result = 0.0;
 412
 413   /* interior intervals */
 414   for(i=index_a; i<=index_b; i++) {
 415     const double x_hi = x_array[i + 1];
 416     const double x_lo = x_array[i];
 417     const double y_lo = y_array[i];
 418     const double y_hi = y_array[i + 1];
 419     const double dx = x_hi - x_lo;
 420     const double dy = y_hi - y_lo;
 421     if(dx != 0.0) {
 422       double b_i, c_i, d_i;
 423       coeff_calc(state->c, dy, dx, i,  &b_i, &c_i, &d_i);
 424
 425       if (i == index_a || i == index_b)
 426         {
 427           double x1 = (i == index_a) ? a : x_lo;
 428           double x2 = (i == index_b) ? b : x_hi;
 429           *result += integ_eval(y_lo, b_i, c_i, d_i, x_lo, x1, x2);
 430         }
 431       else
 432         {
 433           *result += dx * (y_lo + dx*(0.5*b_i + dx*(c_i/3.0 + 0.25*d_i*dx)));
 434         }
 435     }
 436     else {
 437       *result = 0.0;
 438       return GSL_FAILURE;
 439     }
 440   }
 441
 442   return GSL_SUCCESS;
 443 }
 444
 445 static const gsl_interp_type cspline_type =
 446 {
 447   "cspline",
 448   3,
 449   &cspline_alloc,
 450   &cspline_init,
 451   &cspline_eval,
 452   &cspline_eval_deriv,
 453   &cspline_eval_deriv2,
 454   &cspline_eval_integ,
 455   &cspline_free
 456 };
 457
 458 const gsl_interp_type * gsl_interp_cspline = &cspline_type;
 459
 460 static const gsl_interp_type cspline_periodic_type =
 461 {
 462   "cspline-periodic",
 463   2,
 464   &cspline_alloc,
 465   &cspline_init_periodic,
 466   &cspline_eval,
 467   &cspline_eval_deriv,
 468   &cspline_eval_deriv2,
 469   &cspline_eval_integ,
 470   &cspline_free
 471 };
 472
 473 const gsl_interp_type * gsl_interp_cspline_periodic = &cspline_periodic_type;
 474
 475