splinterpolator.h

  switch (_order) {
  case 0:
    if (ax < 0.5) val = 1.0;
    break;
  case 1:
    if (ax < 1) val = 1-ax;;
    break;
  case 2:
    if (ax < 0.5) val = 0.75-ax*ax;
    else if (ax < 1.5) val = 0.5*(1.5-ax)*(1.5-ax);
    break;
  case 3:
    if (ax < 1) val = 2.0/3.0 + 0.5*ax*ax*(ax-2);
    else if (ax < 2) { ax = 2-ax; val = (1.0/6.0)*(ax*ax*ax); }
    break;
  case 4:
    if (ax < 0.5) { ax *= ax; val = (115.0/192.0) + ax*((2.0*ax-5.0)/8.0); }
    else if (ax < 1.5) val = (55.0/96.0) + ax*(ax*(ax*((5.0-ax)/6.0) - 1.25) + 5.0/24.0);
    else if (ax < 2.5) { ax -= 2.5; ax *= ax; val = (1.0/24.0)*ax*ax; }
    break;
  case 5:
    if (ax < 1) { double xx = ax*ax; val = 0.55 + xx*(xx*((3.0-ax)/12.0) - 0.5); }
    else if (ax < 2) val = 0.425 + ax*(ax*(ax*(ax*((ax-9.0)/24.0) + 1.25) - 1.75) + 0.625);
    else if (ax < 3) { ax = 3-ax; double xx = ax*ax; val = (1.0/120.0)*ax*xx*xx; }
    break;
  case 6:
    if (ax < 0.5) { ax *= ax; val = (5887.0/11520.0) + ax*(ax*((21.0-4.0*ax)/144.0) -77.0/192.0); }
    else if (ax < 1.5) val = 7861.0/15360.0 + ax*(ax*(ax*(ax*(ax*((ax - 7.0)/48.0) + 0.328125) - 35.0/288.0) - 91.0/256.0) -7.0/768.0);
    else if (ax < 2.5) val = 1379.0/7680.0 + ax*(ax*(ax*(ax*(ax*((14.0-ax)/120.0) - 0.65625) + 133.0/72.0) - 2.5703125) + 1267.0/960.0);
    else if (ax < 3.5) { ax -= 3.5; ax *= ax*ax; val = (1.0/720.0) * ax*ax; }
    break;
  case 7:
    if (ax < 1) { double xx = ax*ax; val = 151.0/315.0 + xx*(xx*(xx*((ax-4.0)/144.0) + 1.0/9.0) - 1.0/3.0); }
    else if (ax < 2) val = 103.0/210.0 + ax*(ax*(ax*(ax*(ax*(ax*((12.0-ax)/240.0) -7.0/30.0) + 0.5) - 7.0/18.0) - 0.1) -7.0/90.0);
    else if (ax < 3) val = ax*(ax*(ax*(ax*(ax*(ax*((ax-20.0)/720.0) + 7.0/30.0) - 19.0/18.0) + 49.0/18.0) - 23.0/6.0) + 217.0/90.0) - 139.0/630.0;
    else if (ax < 4) { ax = 4-ax; double xxx=ax*ax*ax; val = (1.0/5040.0)*ax*xxx*xxx; }
    break;
  default:
    throw SplinterpolatorException("get_wgt: invalid order spline");
  }

  return(val);
}

/////////////////////////////////////////////////////////////////////
//
// Returns the weight for the first derivative of a spline at 
// coordinate x, where x is relative to the centre of the spline.
//
/////////////////////////////////////////////////////////////////////

template<class T>
double Splinterpolator<T>::get_dwgt(double x) const
{
  double val = 0.0;
  double ax = abs(x);                               // Kernels all anti-symmetric
  int    sign = (ax) ? static_cast<int>(x/ax) : 1;  // Arbitrary choice for when x=0

  switch (_order) {
  case 0: case 1:
    throw SplinterpolatorException("get_dwgt: invalid order spline");
  case 2:
    if (ax < 0.5) val = sign * -2.0*ax;
    else if (ax < 1.5) val = sign * (-1.5 + ax);
    break;
  case 3:
    if (ax < 1) val = sign * (1.5*ax*ax - 2.0*ax);
    else if (ax < 2) { ax = 2-ax; val = sign * -0.5*ax*ax; }
    break;
  case 4:
    if (ax < 0.5) val = sign * (ax*ax*ax - 1.25*ax);
    else if (ax < 1.5) val = sign * (5.0/24.0 - ax*(2.5 - ax*(2.5 - (2.0/3.0)*ax)));
    else if (ax < 2.5) { ax -= 2.5; val = sign * (1.0/6.0)*ax*ax*ax; }
    break;
  case 5:
    if (ax < 1) val = sign * ax*(ax*(ax*(1-(5.0/12.0)*ax)) - 1);
    else if (ax < 2) val = sign * (0.625 - ax*(3.5 - ax*(3.75 - ax*(1.5 - (5.0/24.0)*ax))));
    else if (ax < 3) { ax -= 3; ax = ax*ax; val = sign * (-1.0/24.0)*ax*ax; }
    break;
  case 6:
    if (ax < 0.5) { double xx = ax*ax; val = sign * ax*(xx*((7.0/12) - (1.0/6.0)*xx) - (77.0/96.0)); }
    else if (ax < 1.5) {double xx = ax*ax; val = sign * (ax*(xx*(0.1250*xx + 1.3125) - 0.7109375) - xx*((35.0/48.0)*xx + (35.0/96.0)) - (7.0/768.0)); }
    else if (ax < 2.5) { double xx = ax*ax; val = sign * ((1267.0/960.0) - ax*(xx*(0.05*xx + (21.0/8.0)) + (329.0/64.0)) + xx*((7.0/12.0)*xx + (133.0/24.0))); }
    else if (ax < 3.5) { ax -= 3.5; double xx = ax*ax; val = sign * (1.0/120.0)*xx*xx*ax; }
    break;
  case 7:
    if (ax < 1) { double xx = ax*ax; val = sign * ax*(xx*(xx*((7.0/144.0)*ax - (1.0/6.0)) + 4.0/9.0) - 2.0/3.0); }
    else if (ax < 2) { double xx = ax*ax; val = sign * (ax*(xx*(xx*0.3 + 2.0) - 0.2) - xx*(xx*(xx*(7.0/240.0) + (7.0/6.0)) + (7.0/6.0)) - (7.0/90.0)); }
    else if (ax < 3) { double xx = ax*ax; val = sign * (1.0/720.0)*(xx - 4.0*ax + 2.0)*(7.0*xx*xx - 92.0*xx*ax + 458.0*xx - 1024.0*ax + 868.0); }
    else if (ax < 4) { ax = 4-ax; ax = ax*ax*ax; val = sign * (-1.0/720.0)*ax*ax; }
    break;
  default:
    throw SplinterpolatorException("get_dwgt: invalid order spline");
  }

  return(val);
}

template<class T>
inline void Splinterpolator<T>::get_dwgt1(const double * const *wgts, const double * const *dwgts, 
                                          const unsigned int *dd, unsigned int nd, unsigned int k, 
                                          unsigned int l, unsigned int m, double wgt1, double *dwgt1) const
{
  for (unsigned int i=0; i<nd; i++) {
    switch (dd[i]) {
    case 2:
      dwgt1[i] = wgts[4][m] * wgts[3][l] * dwgts[2][k];
      break;
    case 3:
      dwgt1[i] = wgts[4][m] * dwgts[3][l] * wgts[2][k];
      break;
    case 4:
      dwgt1[i] = dwgts[4][m] * wgts[3][l] * wgts[2][k];
      break;
    default:
      dwgt1[i] = wgt1;
      break;
    }
  }
}

template<class T>
inline std::pair<double,double> Splinterpolator<T>::range() const
{
  std::pair<double,double>  rng(0.0,0.0);
  rng.second = static_cast<double>(_order+1.0)/2.0;
  rng.first = - rng.second;
  return(rng);
}
/////////////////////////////////////////////////////////////////////
//
// Returns the value of the coefficient indexed by indx. Unlike the
// public Coef() this routine allows indexing outside the valid
// volume, returning values that are dependent on the extrapolation
// model when these are encountered.
//
// N.B. May change value of input index N.B.
//
/////////////////////////////////////////////////////////////////////

template<class T>
inline unsigned int Splinterpolator<T>::indx2indx(int indx, unsigned int d) const
{
  if (d > (_ndim-1)) return(0);

  // cout << "indx in = " << indx << endl;

  if (indx < 0) {
    switch (_et[d]) {
    case Constant:
      indx = 0;
      break;
    case Zeros: case Mirror:
      indx = (indx%int(_dim[d])) ? -indx%int(_dim[d]) : 0;
      break;
    case Periodic:
      indx = (indx%int(_dim[d])) ? _dim[d]+indx%int(_dim[d]) : 0;
      break;
    default:
      break;
    }
  }
  else if (indx >= static_cast<int>(_dim[d])) {
    switch (_et[d]) {
    case Constant:
      indx = _dim[d]-1;
      break;
    case Zeros: case Mirror:
      indx = 2*_dim[d] - (_dim[d]+indx%int(_dim[d])) - 2;
      break;
    case Periodic:
      indx = indx%int(_dim[d]);
      break;
    default:
      break;
    }
  }

  // cout << "indx out = " << indx << endl;

  return(indx);
}

// The next routine is defunct and will be moved out of this file.

/*
template<class T>
inline unsigned int Splinterpolator<T>::indx2indx(int indx, unsigned int d) const
{
  if (d > (_ndim-1)) return(0);

  if (indx < 0) {
    switch (_et[d]) {
    case Constant:
      return(0);
      break;
    case Zeros: case Mirror:
      return((indx%int(_dim[d])) ? -1-indx%int(_dim[d]) : _dim[d]-1);
      break;
    case Periodic:
      return((indx%int(_dim[d])) ? _dim[d]+indx%int(_dim[d]) : 0);
      break;
    default:
      break;
    }
  }
  else if (indx >= static_cast<int>(_dim[d])) {
    switch (_et[d]) {
    case Constant:
      return(_dim[d]-1);
      break;
    case Zeros: case Mirror:
      return(2*_dim[d] - (_dim[d]+indx%int(_dim[d])) - 1);
      break;
    case Periodic:
      return(indx%int(_dim[d]));
      break;
    default:
      break;
    }
  }
  return(indx);
}
*/

template<class T>
unsigned int Splinterpolator<T>::indx2linear(int k, int l, int m) const
{
  if (_ndim < 3) return(0);

  int lindx = 0;
  if (_ndim>4) lindx = indx2indx(m,4);
  if (_ndim>3) lindx = _dim[3]*lindx + indx2indx(l,3);
  lindx = _dim[0]*_dim[1]*(_dim[2]*lindx + indx2indx(k,2));

  return(lindx);
}

template<class T>
inline unsigned int Splinterpolator<T>::add2linear(unsigned int lin, int j) const
{
  if (_ndim < 2) return(lin);
  else return(lin + _dim[0]*indx2indx(j,1));
}

template<class T>
T Splinterpolator<T>::coef(int *indx) const
{
  // First fix any outside-volume indicies
  for (unsigned int i=0; i<_ndim; i++) {
    if (indx[i] < 0) {
      switch (_et[i]) {
      case Zeros:
	return(static_cast<T>(0));
      case Constant:
	indx[i] = 0;
	break;
      case Mirror:
	indx[i] = 1-indx[i];
	break;
      case Periodic:
	indx[i] = _dim[i]+indx[i];
	break;
      default:
	break;
      }
    }
    else if (indx[i] >= static_cast<int>(_dim[i])) {
      switch (_et[i]) {
      case Zeros:
	return(static_cast<T>(0));
      case Constant:
	indx[i] = _dim[i]-1;
	break;
      case Mirror:
	indx[i] = 2*_dim[i]-indx[i]-1;
	break;
      case Periodic:
	indx[i] = indx[i]-_dim[i];
	break;
      default:
	break;
      }
    }
  }
  // Now make linear index
  unsigned int lindx=indx[_ndim-1];
  for (int i=_ndim-2; i>=0; i--) lindx = _dim[i]*lindx + indx[i];
  return(coef_ptr()[lindx]);
}

template<class T>
bool Splinterpolator<T>::should_be_zero(const double *coord) const
{
  for (unsigned int i=0; i<_ndim; i++) {
    if (_et[i] == Zeros && (coord[i] < 0 || coord[i] > (_dim[i]-1))) return(true);
  }
  return(false);
}

template<class T>
unsigned int Splinterpolator<T>::n_nonzero(const unsigned int *vec) const
{
  unsigned int n=0;
  for (unsigned int i=0; i<_ndim; i++) if (vec[i]) n++;
  return(n);
}

/////////////////////////////////////////////////////////////////////
//
// Takes care of the "common" tasks when constructing a 
// Splinterpolator object. Called by constructors and by .Set()
//
/////////////////////////////////////////////////////////////////////

template<class T>
void Splinterpolator<T>::common_construction(const T *data, const std::vector<unsigned int>& dim, unsigned int order, double prec, const std::vector<ExtrapolationType>& et, bool copy)
{
  if (!dim.size()) throw SplinterpolatorException("common_construction: data has zeros dimensions");
  if (!dim.size() > 5) throw SplinterpolatorException("common_construction: data cannot have more than 5 dimensions");
  if (dim.size() != et.size()) throw SplinterpolatorException("common_construction: dim and et must have the same size");
  for (unsigned int i=0; i<dim.size(); i++) if (!dim[i]) throw SplinterpolatorException("common_construction: data cannot have zeros size in any direction");
  if (order > 7) throw SplinterpolatorException("common_construction: spline order must be lesst than 7");
  if (!data) throw SplinterpolatorException("common_construction: zero data pointer");
  
  _order = order;
  _prec = prec;
  _et = et;
  _dim.resize(5);
  _ndim = dim.size();
  for (unsigned int i=0; i<5; i++) _dim[i]  = (i < dim.size()) ? dim[i] : 1;

  _own_coef = calc_coef(data,copy);

  _valid = true;
}

/////////////////////////////////////////////////////////////////////
//
// Takes care of the "common" tasks when copy-constructing
// and when assigning.
//
/////////////////////////////////////////////////////////////////////

template<class T>
void Splinterpolator<T>::assign(const Splinterpolator<T>& src)
{
  _valid = src._valid;
  _own_coef = src._own_coef;
  _cptr = src._cptr;
  _order = src._order;
  _ndim = src._ndim;
  _prec = src._prec;
  _dim = src._dim;
  _et = src._et;
  
  if (_own_coef) { // If we need to do a deep copy
    unsigned int ts = 1;
    for (unsigned int i=0; i<_ndim; i++) ts *= _dim[i];
    _coef = new T[ts];
    memcpy(_coef,src._coef,ts*sizeof(T));
  }
}

/////////////////////////////////////////////////////////////////////
//
// Performs deconvolution, converting signal to spline coefficients.
//
/////////////////////////////////////////////////////////////////////

template<class T>
bool Splinterpolator<T>::calc_coef(const T *data, bool copy)
{
  if (_order < 2 && !copy) { _cptr = data; return(false); }

  // Allocate memory and put the original data into _coef
  //
  unsigned int ts=1;
  for (unsigned int i=0; i<_dim.size(); i++) ts *= _dim[i];
  _coef = new T[ts];
  memcpy(_coef,data,ts*sizeof(T));

  if (_order < 2) return(true);  // If nearest neighbour or linear, that's all we need 

  // Loop over all non-singleton dimensions and deconvolve along them
  //
  std::vector<unsigned int>  tdim(_dim.size()-1,0); 
  for (unsigned int cdir=0; cdir<_dim.size(); cdir++) {
    if (_dim[cdir] > 1) deconv_along(cdir);
  }

  return(true);
}

/////////////////////////////////////////////////////////////////////
//
// Performs deconvolution along one of the dimensions, visiting
// all points along the other dimensions.
//
/////////////////////////////////////////////////////////////////////

template<class T>
void Splinterpolator<T>::deconv_along(unsigned int dim)
{
  // Set up to reflect "missing" dimension
  //
  std::vector<unsigned int>   rdim(4,1);     // Sizes along remaining dimensions
  std::vector<unsigned int>   rstep(4,1);    // Step-sizes (in "volume") of remaining dimensions
  unsigned int                mdim = 1;      // Size along "missing" dimension
  unsigned int                mstep = 1;     // Step-size along "missing" dimension
  for (unsigned int i=0, j=0, ss=1; i<5; i++) {
    if (i == dim) {  // If it is our "missing" dimension
      mdim = _dim[i];
      mstep = ss;
    }
    else {
      rdim[j] = _dim[i];
      rstep[j++] = ss;
    }
    ss *= _dim[i];
  }

  SplineColumn  col(mdim,mstep);          // Column helps us do the job
  
  for (unsigned int l=0; l<rdim[3]; l++) {
    for (unsigned int k=0; k<rdim[2]; k++) {
      for (unsigned int j=0; j<rdim[1]; j++) {
        T *dp = _coef + l*rstep[3] + k*rstep[2] + j*rstep[1];
	for (unsigned int i=0; i<rdim[0]; i++, dp+=rstep[0]) {
	  col.Get(dp);                          // Extract a column from the volume
          col.Deconv(_order,_et[dim],_prec);    // Deconvolve it
          col.Set(dp);                          // Put back the deconvolved column
	}
      }
    }
  }
  return;
}

/////////////////////////////////////////////////////////////////////
//
// Here starts private member functions for SplineColumn
//
/////////////////////////////////////////////////////////////////////

/////////////////////////////////////////////////////////////////////
//
// This function returns the poles and scale-factors for splines
// of order 2--7. The values correspond to those found in
// table 1 in Unsers 1993 paper: 
// B-spline signal processing. II. Efficiency design and applications
// 
// The actual values have been taken from
// http://bigwww.epfl.ch/thevenaz/interpolation/coeff.c
//
/////////////////////////////////////////////////////////////////////

template<class T>
unsigned int Splinterpolator<T>::SplineColumn::get_poles(unsigned int order, double *z, unsigned int *sf) const
{
  unsigned int   np = 0;                     // # of poles

  switch (order) {
  case 2:
    np = 1;
    z[0] = 2.0*sqrt(2.0) - 3.0;
    *sf = 8;
    break;
  case 3:
    np = 1;
    z[0] = sqrt(3.0) - 2.0;
    *sf = 6;
    break;
  case 4:
    np = 2;
    z[0] = sqrt(664.0 - sqrt(438976.0)) + sqrt(304.0) - 19.0;
    z[1] = sqrt(664.0 + sqrt(438976.0)) - sqrt(304.0) - 19.0;
    *sf = 384;
    break;
  case 5:
    np = 2;
    z[0] = sqrt(135.0 / 2.0 - sqrt(17745.0 / 4.0)) + sqrt(105.0 / 4.0) - 13.0 / 2.0;
    z[1] = sqrt(135.0 / 2.0 + sqrt(17745.0 / 4.0)) - sqrt(105.0 / 4.0) - 13.0 / 2.0;
    *sf = 120;
    break;
  case 6:
    np = 3;
    z[0] = -0.48829458930304475513011803888378906211227916123938;
    z[1] = -0.081679271076237512597937765737059080653379610398148;
    z[2] = -0.0014141518083258177510872439765585925278641690553467;
    *sf = 46080;
    break;
  case 7:
    np = 3;
    z[0] = -0.53528043079643816554240378168164607183392315234269;
    z[1] = -0.12255461519232669051527226435935734360548654942730;
    z[2] = -0.0091486948096082769285930216516478534156925639545994;
    *sf = 5040;
    break;
  default:
    throw SplinterpolatorException("SplineColumn::get_poles: invalid order of spline");
  }
  return(np);
}

/////////////////////////////////////////////////////////////////////
//
// Initialises the first value for the forward sweep. The initialisation
// will always be an approximation (this is where the "infinite" in IIR
// breaks down) so the value will be calculated to a predefined precision.
//
/////////////////////////////////////////////////////////////////////

template<class T>
double Splinterpolator<T>::SplineColumn::init_fwd_sweep(double z, ExtrapolationType et, double prec) const
{
  //
  // Move logs away from here after debugging
  //
  unsigned int n = static_cast<unsigned int>((log(prec)/log(abs(z))) + 1.5);
  n = (n > _sz) ? _sz : n;

  double iv = _col[0];
  if (et == Periodic) {
    double *ptr=&_col[_sz-1];
    double z2i=z;
    for (unsigned int i=1; i<n; i++, ptr--, z2i*=z) iv += z2i * *ptr;
  }
  else {
    double *ptr=&_col[1];
    double z2i=z;
    for (unsigned int i=1; i<n; i++, ptr++, z2i*=z) iv += z2i * *ptr;
  }
  return(iv); 
}

/////////////////////////////////////////////////////////////////////
//
// Initialises the first value for the backward sweep. The initialisation
// will always be an approximation (this is where the "infinite" in IIR
// breaks down) so the value will be calculated to a predefined precision.
//
/////////////////////////////////////////////////////////////////////

template<class T>
double Splinterpolator<T>::SplineColumn::init_bwd_sweep(double z, double lv, ExtrapolationType et, double prec) const
{
  double iv = 0.0;
  if (et == Periodic) {
    unsigned int n = static_cast<unsigned int>((log(prec)/log(abs(z))) + 1.5);
    n = (n > _sz) ? _sz : n;
    iv = z * _col[_sz-1];
    double z2i = z*z;
    double *ptr=_col;
    for (unsigned int i=1; i<n; i++, ptr++, z2i*=z) {
      iv += z2i * *ptr;
    }
    iv /= (z2i-1.0);
  }
  else {
    iv = -z/(1.0-z*z) * (2.0*_col[_sz-1] - lv);
  }
  return(iv);
}

} // End namespace SPLINTERPOLATOR

#endif // End #ifndef splinterpolator.h