lib/splines.c

   1 #include "xmath.h"
   2
   3 /*
   4 #ifdef __APPLE__
   5 #include <stdlib.h>
   6 #endif
   7 */
   8
   9 #include <stdlib.h>
  10
  11 /* natural cubic spline interpolation based on Numerical Recipes in C */
  12
  13 /* calculate second derivatives; assumes strictly increasing x values */
  14 static void
  15 find_spline_derivs(double *x, double *y, size_t n,
  16                    double *y2, double *u)
  17 {
  18   long i, k;
  19   double p, sig;
  20
  21   y2[0] = u[0] = 0.0;  /* lower boundary condition for natural spline */
  22
  23   /* decomposition loop for the tridiagonal algorithm */
  24   for (i = 1; i < n - 1; i++) {
  25     y2[i] = u[i] = 0.0; /* set in case a zero test is failed */
  26     if (x[i - 1] < x[i] && x[i] < x[i + 1]) {
  27       sig = (x[i] - x[i - 1]) / (x[i + 1] - x[i - 1]);
  28       p = sig * y2[i - 1] + 2.0;
  29       if (p != 0.0) {
  30         y2[i] = (sig - 1.0) / p;
  31         u[i] = (y[i + 1] - y[i]) / (x[i + 1] - x[i])
  32              - (y[i] - y[i - 1]) / (x[i] - x[i - 1]);
  33         u[i] = (6.0 * u[i] / (x[i + 1] - x[i - 1]) - sig * u[i - 1]) / p;
  34       }
  35     }
  36   }
  37
  38   /* upper boundary condition for natural spline */
  39   y2[n - 1] = 0.0;
  40
  41   /* backsubstitution loop of the tridiagonal algorithm */
  42   for (k = n - 2; k >= 0; k--)
  43     y2[k] = y2[k] * y2[k + 1] + u[k];
  44 }
  45
  46 /* interpolate or extrapolate value at x using results of find_spline_derivs */
  47 static void
  48 spline_interp(double *xa, double *ya, double *y2a,
  49               size_t n, double x, double *y)
  50 {
  51   long klo, khi, k;
  52   double h, b, a;
  53
  54   if (x <= xa[0]) {
  55     h = xa[1] - xa[0];
  56     b = (h > 0.0) ? (ya[1] - ya[0]) / h - h * y2a[1] / 6.0 : 0.0;
  57     *y = ya[0] + b * (x - xa[0]);
  58   }
  59   else if (x >= xa[n - 1]) {
  60     h = xa[n - 1] - xa[n - 2];
  61     b = (h > 0.0) ? (ya[n - 1] - ya[n - 2]) / h + h * y2a[n - 2] / 6.0 : 0.0;
  62     *y = ya[n - 1] + b * (x - xa[n - 1]);
  63   }
  64   else {
  65     /* try a linear interpolation for equally spaced x values */
  66     k = (n - 1) * (x - xa[0]) / (xa[n - 1] - xa[0]);
  67
  68     /* make sure the range is right */
  69     if (k < 0) k = 0;
  70     if (k > n - 2) k = n - 2;
  71
  72     /* bisect if necessary until the bracketing interval is found */
  73     klo = (x >= xa[k]) ? k : 0;
  74     khi = (x < xa[k + 1]) ? k + 1 : n - 1;
  75     while (khi - klo > 1) {
  76       k = (khi + klo) / 2;
  77       if (xa[k] > x) khi = k;
  78       else klo = k;
  79     }
  80
  81     /* interpolate */
  82     h = xa[khi] - xa[klo];
  83     if (h > 0.0) {
  84       a = (xa[khi] - x) / h;
  85       b = (x - xa[klo]) / h;
  86       *y = a * ya[klo] + b * ya[khi]
  87          + ((a * a * a - a) * y2a[klo] + (b * b * b - b) * y2a[khi]) * (h * h) / 6.0;
  88     }
  89     else *y = (ya[klo] + ya[khi]) / 2.0; /* should not be needed */
  90   }
  91 }
  92
  93 int
  94 fit_spline(size_t n, double *x, double *y,
  95            size_t ns, double *xs, double *ys, double *work)
  96 {
  97   size_t i;
  98   double *y2, *u;
  99
 100   y2 = work; u = work + n;
 101
 102   if (n < 2 || ns < 1) return (1); /* signal an error */
 103   for (i = 1; i < n; i++)
 104     if (x[i - 1] >= x[i]) return(1); /* signal an error */
 105
 106   find_spline_derivs(x, y, n, y2, u);
 107
 108   for (i = 0; i < ns; i++)
 109     spline_interp(x, y, y2, n, xs[i], &ys[i]);
 110
 111   return(0);
 112 }