/* ======================================================================
 * catmullRom.c - Catmull-Rom interpolating spline function.
 * Copyright (C) 1993 by George Wolberg
 *
 * Written by: George Wolberg, 1993
 * ======================================================================
 */

#include "meshwarp.h"

/* ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
 * catmullRom:
 *
 * Compute a Catmull-Rom spline passing through the len1 points in arrays
 * x1, y1, where y1 = f(x1)
 * len2 positions on the spline are to be computed. Their positions are
 * given in x2. The spline values are stored in y2.
 */
void
catmullRom(x1, y1, len1, x2, y2, len2)
float *x1, *y1, *x2, *y2;
int  len1, len2;
{
 int i, j, dir, j1, j2;
 double x,  dx1, dx2;
 double dx, dy, yd1, yd2, p1, p2, p3;
 double a0y, a1y, a2y, a3y;
 

 /* find direction of monotonic x1; skip ends */
 if(x1[0] < x1[1]) { /* increasing */
  if(x2[0]<x1[0] || x2[len2-1]>x1[len1-1]) dir=0;
  else dir = 1;
 } else {  /* decreasing */
  if(x2[0]>x1[0] || x2[len2-1]<x1[len1-1]) dir=0;
  else dir = -1;
 }
 if(dir == 0) {   /* error */
  printf("catmullRom: Output x-coord out of range of input\n");
  return;
 }

 /* p1 is first endpoint of interval
  * p2 is resampling position
  * p3 is second endpoint of interval
  * j  is input index for current interval
  */

 /* force coefficient initialization */
 if(dir==1) p3 = x2[0] - 1;
 else  p3 = x2[0] + 1;

 for(i=0; i<len2; i++) {
  /* check if in new interval */
  p2 = x2[i];
  if((dir==1 && p2>p3) || (dir== -1 && p2<p3)) {
   /* find the interval which contains p2 */
   if(dir) {
    for(j=0; j<len1 && p2>x1[j]; j++);
    if(p2 < x1[j]) j--;
   } else {
    for(j=0; j<len1 && p2<x1[j]; j++);
    if(p2 > x1[j]) j--;
   }

   p1 = x1[j];  /* update 1st endpt */
   p3 = x1[j+1];  /* update 2nd endpt */

   /* clamp indices for endpoint interpolation */
   j1 = MAX(j-1, 0);
   j2 = MIN(j+2, len1-1);
 
   /* compute spline coefficients */
   dx  = 1.0 / (p3 - p1);
   dx1 = 1.0 / (p3 - x1[j1]);
   dx2 = 1.0 / (x1[j2] - p1);
   dy  = (y1[j+1] - y1[ j ]) * dx;
   yd1 = (y1[j+1] - y1[ j1]) * dx1;
   yd2 = (y1[j2 ] - y1[ j ]) * dx2;
   a0y =  y1[j];
   a1y =  yd1;
   a2y =  dx *  ( 3*dy - 2*yd1 - yd2);
   a3y =  dx*dx*(-2*dy +   yd1 + yd2);
  }
  /* use Horner's rule to calculate cubic polynomial */
  x = p2 - p1;
  y2[i] = ((a3y*x + a2y)*x + a1y)*x + a0y;
 }
}