---

interpolation.c

Go to the documentation of this file.

#include "utilf.h"
#include "interpolation.h"


static polynom3 Px1, Px2, Py1, Py2;

static real dist2(real *t)
{
  real dx, dy;
  dx = splint(Px1, t[1]) - splint(Px2, t[2]);
  dy = splint(Py1, t[1]) - splint(Py2, t[2]);
  return sq(dx) + sq(dy);
}


static void ddist2(real *t, real *df)
{
  real dx, dy;
  dx = splint(Px1, t[1]) - splint(Px2, t[2]);
  dy = splint(Py1, t[1]) - splint(Py2, t[2]);
  df[1] = 2.*(splint1(Px1, t[1])*dx + splint1(Py1, t[1])*dy);
  df[2] = -2.*(splint1(Px2, t[2])*dx + splint1(Py2, t[2])*dy);
}


void spline(real1D x, real1D y, int n, real yp1, real ypn, real1D d2y,
            polynom3 *poly, int flag)
{
  int i, k;
  real p, qn, sig, un, *u;
  real x1, y1, z1, x2, y2, z2, h;
  
  u = (real1D) malloc(sizeof(real) * (n - 1));

  switch(flag) {
  case SP_NATURAL: d2y[0] = u[0] = 0.0; break;
  case SP_DERIVATIVE:
    d2y[0] = -0.5;
    u[0] = (3.0 / (x[1] - x[0])) * ((y[1] - y[0]) / (x[1] - x[0]) - yp1);
    break;
  default:
    fprintf(stderr, "spline(): Bad flag parameter\n");
    exit(1);
  }

  for (i = 1; i < n - 1; i++) {
    sig = (x[i] - x[i-1]) / (x[i+1] - x[i-1]);
    p = sig * d2y[i-1] + 2.0;
    d2y[i] = (sig - 1.0) / p;
    u[i] = (y[i+1] - y[i]) / (x[i+1] - x[i]) - (y[i] - y[i-1]) 
      / (x[i] - x[i-1]);
    u[i] = (6.0 * u[i] / (x[i+1] - x[i-1]) - sig * u[i-1]) / p;
  }

  switch(flag) {
  case SP_NATURAL: d2y[n - 1] = 0.0; 
    break;
  case SP_DERIVATIVE:
    qn = 0.5;
    un = (3.0 / (x[n-1] - x[n-2])) * (ypn - (y[n-1] - y[n-2]) 
                                      / (x[n-1] - x[n-2]));
    d2y[n - 1] = (un - qn * u[n-2]) / (qn * d2y[n-2] + 1.0);
    break;
  }

  for (k = n - 2; k >= 0; k--)
    d2y[k] = d2y[k] * d2y[k+1] + u[k];
  free(u);

  /* Fill the coefficients of the interpolating polynomial for each segment */
  for (i = 0; i < n - 1; i++)
    {
      x1 = x[i]; y1 = y[i]; z1 = d2y[i];
      x2 = x[i+1]; y2 = y[i+1]; z2 = d2y[i+1];
      h = x2 - x1;
      poly[i].a = (x2*y1 - x1*y2)/h + h*(x1*z2 - x2*z1)/6. 
        + (x2*x2*x2*z1 - x1*x1*x1*z2)/(6.*h);
      poly[i].b = (y2 - y1)/h + h*(z1 - z2)/6. 
        + (x1*x1*z2 - x2*x2*z1)/(2.*h);
      poly[i].c = (x2*z1 - x1*z2)/(2.*h);
      poly[i].d = (z2 - z1)/(6.*h);
    }
}


void tridag(real1D a, real1D b, real1D c, real1D r, real1D u, int n)
{
  int j;
  real bet, *gam;
  
  gam = (real *)malloc(n*sizeof(real));
  bet = b[0];
  u[0] = r[0]/bet;
  for (j = 1; j < n; j++) {
    gam[j] = c[j-1]/bet;
    bet = b[j] - a[j]*gam[j];
    u[j] = (r[j] - a[j]*u[j-1])/bet;
  }
  for (j = n - 2; j >= 0; j--)
    u[j] -= gam[j+1]*u[j+1];
  free(gam);
}


void Pspline(real1D x, real1D y, real1D d2y, real lp, int n, polynom3 *poly)
{
  real1D a, b, c, r, z, u;
  real alpha, gamma, fact, x1, x2, y1, y2, z1, z2, h;
  int i;

  a = (real1D) malloc(n*sizeof(real));
  b = (real1D) malloc(n*sizeof(real));
  c = (real1D) malloc(n*sizeof(real));
  r = (real1D) malloc(n*sizeof(real));
  z = (real1D) malloc(n*sizeof(real));
  u = (real1D) malloc(n*sizeof(real));

  b[0] = x[1] - x[n-1] + lp;
  c[0] = 0.5*(x[1] - x[0]);
  r[0] = 3.0*((y[1] - y[0])/(x[1] - x[0]) - 
              (y[0] - y[n-1])/(x[0] - x[n-1] + lp));
  for (i = 1; i < n - 1; i++) {
    a[i] = 0.5*(x[i] - x[i-1]);
    b[i] = x[i+1] - x[i-1];
    c[i] = 0.5*(x[i+1] - x[i]);

    r[i] = 3.0*((y[i+1] - y[i])/(x[i+1] - x[i]) - 
                (y[i] - y[i-1])/(x[i] - x[i-1]));
  }    
  a[n-1] = 0.5*(x[n-1] - x[n-2]);
  b[n-1] = x[0] - x[n-2] + lp;
  r[n-1] = 3.0*((y[0] - y[n-1])/(x[0] - x[n-1] + lp) - 
                (y[n-1] - y[n-2])/(x[n-1] - x[n-2]));
  alpha = 0.5*(x[0] - x[n-1] + lp);

  gamma = -b[0];
  b[0] -= gamma;
  b[n-1] -= alpha*alpha/gamma;  
  tridag(a, b, c, r, d2y, n);

  u[0] = gamma;
  u[n-1] = alpha;
  for (i = 1; i < n - 1; i++)
    u[i] = 0.0;
  tridag(a, b, c, u, z, n);
    
  fact = (d2y[0] + alpha*d2y[n-1]/gamma)/(1.0 + z[0] + alpha*z[n-1]/gamma);
  for (i = 0; i < n; i++)
    d2y[i] -= fact*z[i];

  /* Fill the coefficients of the interpolating polynomial for each segment */
  for (i = 0; i < n - 1; i++)
    {
      x1 = x[i]; y1 = y[i]; z1 = d2y[i];
      x2 = x[i+1]; y2 = y[i+1]; z2 = d2y[i+1];
      h = x2 - x1;
      poly[i].a = (x2*y1 - x1*y2)/h + h*(x1*z2 - x2*z1)/6. 
        + (x2*x2*x2*z1 - x1*x1*x1*z2)/(6.*h);
      poly[i].b = (y2 - y1)/h + h*(z1 - z2)/6. 
        + (x1*x1*z2 - x2*x2*z1)/(2.*h);
      poly[i].c = (x2*z1 - x1*z2)/(2.*h);
      poly[i].d = (z2 - z1)/(6.*h);
    }
  x1 = x[n - 1]; y1 = y[n - 1]; z1 = d2y[n - 1];
  x2 = x[0] + lp; y2 = y[0]; z2 = d2y[0];
  h = x2 - x1;
  poly[n - 1].a = (x2*y1 - x1*y2)/h + h*(x1*z2 - x2*z1)/6. 
    + (x2*x2*x2*z1 - x1*x1*x1*z2)/(6.*h);
  poly[n - 1].b = (y2 - y1)/h + h*(z1 - z2)/6. 
    + (x1*x1*z2 - x2*x2*z1)/(2.*h);
  poly[n - 1].c = (x2*z1 - x1*z2)/(2.*h);
  poly[n - 1].d = (z2 - z1)/(6.*h);

  free(a); free(b); free(c); free(r); free(z); free(u);
}


real splint_find(real1D xa, polynom3 *poly, int n, real x)
{
  int klo, khi, k;
  
  /* then search for the nearest lower point */
  klo = 0;
  khi = n - 1;
  while (khi - klo > 1) {
    k = (khi + klo) >> 1;
    if (xa[k] > x) khi = k;
    else klo = k;
  }
  return poly[klo].a + x*(poly[klo].b + x*(poly[klo].c + x*poly[klo].d));
}


int distmin(polynom3 px, polynom3 py, real xp, real yp, real *tmin,
            real t1, real t2, real prec)
{
  real a, b, c, d, e, f;
  real guess = 0.5*(t1 + t2), xh, xl, fl, fh, dxold, dx, f1, df, temp;
  int niter = 0;

  a = px.a*px.b + py.a*py.b - px.b*xp - py.b*yp;
  b = sq(px.b) + sq(py.b) + 2.*(px.a*px.c + py.a*py.c - px.c*xp - py.c*yp);
  c = 3.*(px.b*px.c + py.b*py.c + px.a*px.d + py.a*py.d - px.d*xp - py.d*yp);
  d = 2.*(sq(px.c) + sq(py.c) + 2.*px.b*px.d + 2.*py.b*py.d);
  e = 5.*(px.c*px.d + py.c*py.d);
  f = 3.*(sq(px.d) + sq(py.d));
#define dist(x) (a+(x)*(b+(x)*(c+(x)*(d+(x)*(e+(x)*f)))))
#define dist1(x) (b+(x)*(2.*c+(x)*(3.*d+(x)*(4.*e+(x)*5.*f))))

  fl = dist(t1);
  fh = dist(t2);
  if (fabs(fl) < prec) { *tmin = t1; return 1; }
  if (fabs(fh) < prec) { *tmin = t2; return 1; }
  if (fl < 0.0) {
    xl = t1;
    xh = t2;
  } 
  else {
    xl = t2;
    xh = t1;
  }
  dxold = fabs(t2 - t1);
  dx = dxold;
  f1 = dist(guess);
  df = dist1(guess);
  while (niter++ < POLYMAX) {
    if ((((guess - xh)*df - f1)*((guess - xl)*df - f1) >= 0.0) ||
        (fabs(2.0*f1) > fabs(dxold*df))) {
      dxold = dx;
      dx = 0.5*(xh - xl);
      guess = xl + dx;
    }
    else {
      dxold = dx;
      dx = f1 / df;
      temp = guess;
      guess -= dx;
    }
    f1 = dist(guess);
    if (fabs(f1) < prec) { if (guess > t2 || guess < t1) return 0; 
                           *tmin = guess; return 1; }
    df = dist1(guess);
    if (f1 < 0.0)
      xl = guess;
    else
      xh = guess;
  }
  return 0;

#undef dist
#undef dist1
}


int polyroot(polynom3 poly, real x1, real x2, real *x, real prec)
{
  real guess = 0.5*(x1 + x2), xh, xl, fl, fh, dxold, dx, f, df, temp;
  int niter = 0;
  
  /* Solving for the polynomial root using a combination of
     bissection and Newton's solver */
  fl = splint(poly, x1);
  fh = splint(poly, x2);
  if (fl == 0.0) {
    *x = x1;
    return 0;
  }    
  if (fh == 0.0) {
    *x = x2;
    return 0;
  }
  if (fl < 0.0) {
    xl = x1;
    xh = x2;
  } 
  else {
    xl = x2;
    xh = x1;
  }
  dxold = fabs(x2 - x1);
  dx = dxold;
  f = splint(poly, guess);
  df = splint1(poly, guess);
  while (niter++ < POLYMAX) 
    {
      if ((((guess - xh)*df - f)*((guess - xl)*df - f) >= 0.0) ||
          (fabs(2.0*f) > fabs(dxold*df))) {
        dxold = dx;
        dx = 0.5*(xh - xl);
        guess = xl + dx;
      }
      else {
        dxold = dx;
        dx = f / df;
        temp = guess;
        guess -= dx;
      }
      f = splint(poly, guess);
      if (fabs(f) < prec) {
        *x = guess;
        return niter;
      }
      df = splint1(poly, guess);
      if (f < 0.0)
        xl = guess;
      else
        xh = guess;
    }
  fprintf(stderr, "WARNING polyroot(): Can not find a root in [%g,%g]\n", x1, x2);
  return POLYMAX + 1;
}


int polyroots(polynom3 poly, real x1, real x2, real *x, real y, real prec)
{
  real delta, a, b, c, t1, t2, y1, y2, yt1, yt2;
  int n = 0;

  poly.a -= y;

  if (x1 > x2) { a = x1; x1 = x2; x2 = a; }
  y1 = splint(poly, x1);
  y2 = splint(poly, x2);
  /* find the extrema contained in {x1, x2} (if any) */
  a = 3.*poly.d;  
  b = 2.*poly.c;  
  c = poly.b;
  delta = sq(b) - 4.*a*c;
  if (delta < 0.0)          /* no extremum anywhere -> one root maximum */
    t1 = t2 = x2 + 1.;
  else if (delta == 0.0) {  /* one extremum -> two roots maximum */    
    t1 = -b / (2.*a);
    t2 = x2 + 1.;
    if (t1 > t2) {a = t1; t1 = t2; t2 = a; }
  }
  else {                    /* two extrema -> three roots maximum */ 
    delta = sqrt(delta);
    t1 = (-b + delta) / (2.*a);
    t2 = (-b - delta) / (2.*a);
    if (t1 > t2) {a = t1; t1 = t2; t2 = a; }
  }
  
  /* no extremum in [x1,x2] -> one root maximum in [x1,x2] */
  if ((t1 < x1 && t2 > x2) || t1 > x2 || t2 < x1) {
    if (y1*y2 <= 0.0) {
      polyroot(poly, x1, x2, x, prec);
      return 1;
    }
    return 0;
  }

  yt1 = splint(poly, t1);
  yt2 = splint(poly, t2);

  /* one extremum near x1 */
  if (t1 >= x1 && y1*yt1 <= 0.0)
    polyroot(poly, x1, t1, &(x[n++]), prec);
  if (t1 < x1 && t2 <= x2 && y1*yt2 <= 0.0)
    polyroot(poly, x1, t2, &(x[n++]), prec);

  /* one extremum near x2 */
  if (t2 <= x2 && y2*yt2 <= 0.0)
    polyroot(poly, t2, x2, &(x[n++]), prec);
  if (t2 > x2 && t1 <= x2 && y2*yt1 <= 0.0)
    polyroot(poly, t1, x2, &(x[n++]), prec);  

  /* two extrema in [x1,x2] */
  if (t1 >= x1 && t2 <= x2 && yt1*yt2 <= 0.0)
    polyroot(poly, t1, t2, &(x[n++]), prec);

  return n;
} 


/* bicubic interpolation */
void bcucof(real2D f, int i, int j, real *c, int nx, int ny)
{
  static int wt[16][16]= {
    {1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0},
    {0,0,0,0,0,0,0,0,1,0,0,0,0,0,0,0},
    {-3,0,0,3,0,0,0,0,-2,0,0,-1,0,0,0,0},
    {2,0,0,-2,0,0,0,0,1,0,0,1,0,0,0,0},
    {0,0,0,0,1,0,0,0,0,0,0,0,0,0,0,0},
    {0,0,0,0,0,0,0,0,0,0,0,0,1,0,0,0},
    {0,0,0,0,-3,0,0,3,0,0,0,0,-2,0,0,-1},
    {0,0,0,0,2,0,0,-2,0,0,0,0,1,0,0,1},
    {-3,3,0,0,-2,-1,0,0,0,0,0,0,0,0,0,0},
    {0,0,0,0,0,0,0,0,-3,3,0,0,-2,-1,0,0},
    {9,-9,9,-9,6,3,-3,-6,6,-6,-3,3,4,2,1,2},
    {-6,6,-6,6,-4,-2,2,4,-3,3,3,-3,-2,-1,-1,-2},
    {2,-2,0,0,1,1,0,0,0,0,0,0,0,0,0,0},
    {0,0,0,0,0,0,0,0,2,-2,0,0,1,1,0,0},
    {-6,6,-6,6,-3,-3,3,3,-4,4,2,-2,-2,-2,-1,-1},
    {4,-4,4,-4,2,2,-2,-2,2,-2,-2,2,1,1,1,1}
  };
  int k;
  real xx, x[16];
  
  /* function */
  x[0] = f[i][j]; x[1] = f[i+1][j]; x[2] = f[i+1][j+1]; x[3] = f[i][j+1];

  /* x component of the gradient */
  x[4] = 0.5*(f[i+1][j] - f[i-1][j]);
  if (i+2 <= nx) {
    x[5] = 0.5*(f[i+2][j] - f[i][j]);
    x[6] = 0.5*(f[i+2][j+1] - f[i][j+1]);
  }
  else { /* periodic boundary conditions */
    x[5] = 0.5*(f[i+2 - nx + 2][j] - f[i][j]);
    x[6] = 0.5*(f[i+2 - nx + 2][j+1] - f[i][j+1]);
  }
  x[7] = 0.5*(f[i+1][j+1] - f[i-1][j+1]);

  /* y component of the gradient */
  x[8] = 0.5*(f[i][j+1] - f[i][j-1]);
  x[9] = 0.5*(f[i+1][j+1] - f[i+1][j-1]);
  if (j+2 <= ny) {
    x[10] = 0.5*(f[i+1][j+2] - f[i+1][j]);
    x[11] = 0.5*(f[i][j+2] - f[i][j]);
  } 
  else /* zero gradient */
    x[10] = x[11] = 0.0;

  /* cross derivatives */       
  x[12] = 0.25*(f[i+1][j+1] - f[i+1][j-1] - f[i-1][j+1] + f[i-1][j-1]);
  if (i+2 <= nx) {
    x[13] = 0.25*(f[i+2][j+1] - f[i+2][j-1] - f[i][j+1] + f[i][j-1]);
    if (j+2 <= ny) {
      x[14] = 0.25*(f[i+2][j+2] - f[i+2][j] - f[i][j+2] + f[i][j]);
      x[15] = 0.25*(f[i+1][j+2] - f[i+1][j] - f[i-1][j+2] + f[i-1][j]);
    }
    else /* zero gradient */
      x[14] = x[15] = 0.0;
  }
  else { /* periodic boundary conditions */
    x[13] = 0.25*(f[i+2 - nx + 2][j+1] - f[i+2 - nx + 2][j-1] 
                  - f[i][j+1] + f[i][j-1]);
    if (j+2 <= ny) {
      x[14] = 0.25*(f[i+2-nx+2][j+2] - f[i+2-nx+2][j] - f[i][j+2] + f[i][j]);
      x[15] = 0.25*(f[i+1][j+2] - f[i+1][j] - f[i-1][j+2] + f[i-1][j]);
    }
    else /* zero gradient */
      x[14] = x[15] = 0.0;
  }

  for (i = 0; i <= 15; i++) {
    xx = 0.0;
    for (k = 0; k <= 15; k++) xx += wt[i][k]*x[k];
    c[i] = xx;
  }
}


real bcuint(real t, real u, real *c)
{
  int i;
  real ans = 0.0;

  for (i = 3; i >= 0; i--)
    ans = t*(ans) + ((c[4*i + 3]*u + c[4*i + 2])*u + c[4*i + 1])*u + c[4*i];
  return ans;
}


/* For the cubic interpolation, the following function takes the
coordinates of the four points and gives back the three coefficients
for each x(t) and y(t) polynomial.
        x(t) = ex*t^3 + fx*t^2 + gx*t + x0
        y(t) = ey*t^3 + fy*t^2 + gy*t + y0
d0 = (P0,P1), d1 = d0 + (P1,P2), d2 = d1 + (P2,P3)
        x(d0) = x1  x(d1) = x2 etc .... 
        *rd2 is equal to d2 */
void CubicPolynom(real x0, real y0, real x1, real y1, 
                  real x2, real y2, real x3, real y3, 
                  real *ex, real *fx, real *gx,
                  real *ey, real *fy, real *gy,
                  real *rd2)
{
  real d0, d1, d2, det;
  real a1, a2, a3;

  d0 = sqrt(sq(x1 - x0) + sq(y1 - y0));
  d1 = d0 + sqrt(sq(x2 - x1) + sq(y2 - y1));
  d2 = d1 + sqrt(sq(x3 - x2) + sq(y3 - y2));
  
  det = (d0*d1*(-d0 + d1)*d2*(-d0 + d2)*(-d1 + d2));

  a1 = sq(d0)*(x0*(d1 - d2) + d2*x2 - d1*x3);
  a2 = sq(d1)*(x0*(d2 - d0) - d2*x1 + d0*x3);
  a3 = sq(d2)*(x0*(d0 - d1) + d1*x1 - d0*x2);
  
  *ex = (a1 + a2 + a3) / det;
  *fx = - (d0*a1 + d1*a2 + d2*a3) / det;
  *gx = (sq(d0)*sq(d2)*(d2 - d0)*
         (d0*d0*d0*(x0 - x2) + d1*d1*d1*(x1 - x0)) + 
         sq(d0)*sq(d1)*(d0 - d1)*
         (d0*d0*d0*(x0 - x3) + d2*d2*d2*(x1 - x0)))
    / (d0*d0*d0*det);

  a1 = sq(d0)*(y0*(d1 - d2) + d2*y2 - d1*y3);
  a2 = sq(d1)*(y0*(d2 - d0) - d2*y1 + d0*y3);
  a3 = sq(d2)*(y0*(d0 - d1) + d1*y1 - d0*y2);
  
  *ey = (a1 + a2 + a3) / det;
  *fy = - (d0*a1 + d1*a2 + d2*a3) / det;
  *gy = (sq(d0)*sq(d2)*(d2 - d0)*
         (d0*d0*d0*(y0 - y2) + d1*d1*d1*(y1 - y0)) + 
         sq(d0)*sq(d1)*(d0 - d1)*
         (d0*d0*d0*(y0 - y3) + d2*d2*d2*(y1 - y0)))
    / (d0*d0*d0*det);
  
  *rd2 = d2;
}


#define NPTS 9
/* return the extrapolated value of the points of coordinate (x,y) obtained
  using a polynomial interpolation of the type
  f(x,y)=a00 + a01*y + a02*y^2 + 
         a10*x + a11*xy + a12*xy^2 +
         a20*x^2 + a21*x^2y + a22*x^2*y^2
  In order to solve the resulting system of equations,
  at most 3 points can have the same x or y coordinate  */
real polynomial(real *xp, real *yp, real *z, real x, real y)
{
  int i, j, k, i1;
  real a[NPTS][NPTS], lambda[NPTS], max, temp, xp2, yp2;

  for (i = 0; i < NPTS; i++) {
    xp2 = xp[i]*xp[i]; yp2 = yp[i]*yp[i];
    a[i][0] = 1.0;
    a[i][1] = yp[i];
    a[i][2] = yp2;
    a[i][3] = xp[i];
    a[i][4] = xp[i]*yp[i];
    a[i][5] = xp[i]*yp2;
    a[i][6] = xp2;
    a[i][7] = xp2*yp[i];
    a[i][8] = xp2*yp2;
  }
  
  /* solve the system of equations using gauss pivoting */
  for (j = 0; j <= NPTS - 2; j++) {
    i1 = j;
    max = fabs(a[j][j]);
    for (i = j + 1; i < NPTS; i++) {
      if (fabs(a[i][j]) >= max) {
        i1 = i;
        max = fabs(a[i][j]);
      }
    }
    for (k = j; k < NPTS; k++) {
      max = a[j][k]; a[j][k] = a[i1][k]; a[i1][k] = max;
    }
    max = z[j]; z[j] = z[i1]; z[i1] = max;
    for (i = j + 1; i < NPTS; i++) {
      if (a[j][j] == 0.) {
        printf("polynomial(%g, %g): no solution\n", x, y);
        for (i = 0; i < NPTS; i++)
          printf("%g %g\n", xp[i], yp[i]);
        return 0.0;
      }
      else
        temp = a[i][j] / a[j][j];
      for (k = j; k < NPTS; k++)
        a[i][k] -= temp * a[j][k];
      z[i] -= temp * z[j];
    }
  }
  if (a[NPTS - 1][NPTS - 1] == 0.) {
    printf("polynomial(%g, %g): no solution\n", x, y);
    for (i = 0; i < NPTS; i++)
      printf("%g %g\n", xp[i], yp[i]);
    return 0.0;
  }
  else
    lambda[NPTS - 1] = z[NPTS - 1] / a[NPTS - 1][NPTS - 1];
  for (i = NPTS - 2; i >= 0; i--) {
    lambda[i] = z[i];
    for (k = i + 1; k < NPTS; k++)
      lambda[i] -= a[i][k] * lambda[k];
    lambda[i] /= a[i][i];
  }
  
  /* compute the value for coordinate (x,y) 
     using the approximating function */
  xp2 = x*x;
  yp2 = y*y;
  return lambda[0] + lambda[1]*y + lambda[2]*yp2 +
         x*(lambda[3] + lambda[4]*y + lambda[5]*yp2) +
         xp2*(lambda[6] + lambda[7]*y + lambda[8]*yp2);
}
#undef NPTS


real trapzd(real (*func)(polynom3, polynom3, real), 
            polynom3 px, polynom3 py,
            real a, real b, int n)
{
  real x,tnm,sum,del;
  static real s;
  int it,j;
  
  if (n == 1) {
    return (s=0.5*(b-a)*((*func)(px,py,a)+(*func)(px,py,b)));
  } else {
    for (it=1,j=1;j<n-1;j++) it <<= 1;
    tnm=it;
    del=(b-a)/tnm;
    x=a+0.5*del;
    for (sum=0.0,j=1;j<=it;j++,x+=del) sum += (*func)(px,py,x);
    s=0.5*(s+(b-a)*sum/tnm);
    return s;
  }
}


real qtrap(real (*func)(polynom3, polynom3, real), 
           polynom3 px, polynom3 py,
           real a, real b, real eps)
{
  int j;
  real s,olds;

  olds = -1.0e30;
  for (j = 1; j <= 20; j++) {
    s = trapzd(func,px,py,a,b,j);
    if (fabs(s - olds) < eps*fabs(olds)) {
      printf("niter: %d\n", j);
      return s;
    }
    olds = s;
  }  
  fprintf(stderr, "WARNING qtrap(): too many steps\n");
  return s;
}


/*
real polydistmin(polynom3 px1, polynom3 px2, polynom3 py1, polynom3 py2,
                 real *t1, real *t2, real ftol)
{
  real t[3], fret;
  int niter;

  Px1 = px1; Px2 = px2; Py1 = py1; Py2 = py2;
  t[1] = *t1; t[2] = *t2;
  frprmn(t, 2, ftol, &niter, &fret, dist2, ddist2);
  *t1 = t[1]; *t2 = t[2];
  return sqrt(fret);
}
*/

#if 0
int main(int argc, char *argv[])
{
  real1D x, y, d2y;
  real t;
  polynom3 *poly;
  int i, n = 10;

  x = (real1D)malloc(n*sizeof(real));
  y = (real1D)malloc(n*sizeof(real));
  d2y = (real1D)malloc(n*sizeof(real));
  poly = (polynom3 *)malloc(n*sizeof(polynom3));

  for (i = 0; i < n; i++) {
    x[i] = i*6.28318530718/n;
    y[i] = sin(x[i]);
    printf("%g %g\n", x[i], y[i]);
  }    

  Pspline(x, y, d2y, 6.28318530718, n, poly);
  printf("&\n");
  
  for (i = 0; i < n - 1; i++) {
    for (t = x[i]; t <= x[i+1]; t += 0.01)
      printf("%g %g\n", t, splint(poly[i], t));
  }
  for (t = x[n-1]; t <= x[0] + 6.28318530718; t += 0.01)
    printf("%g %g\n", t, splint(poly[n-1], t));
}
#endif

---