TabulatedFunction.cc

Go to the documentation of this file.

#include <iomanip>
#include <sstream>
#include <cmath>
#if !defined(__clang_major__) || __clang_major__ < 5
#  include <ext/algorithm>
#endif
#include <utl/TabulatedFunction.h>
#include <utl/AugerException.h>
#include <utl/ErrorLogger.h>

using namespace std;
using namespace utl;

typedef TabulatedFunction::Array Array;


void
TabulatedFunction::FillTable(const Array& xValues,
                             const Array& yValues)
{
  const unsigned int xSize = xValues.size();

  if (xSize != yValues.size()) {
    ostringstream err;
    err << "Size of x and y arrays are different.";
    ERROR(err);
    throw OutOfBoundException(err.str());
  }

  fX = xValues;
  fY = yValues;

  if (is_sorted(fX.begin(), fX.end()))
    return;

  // insertion sort
  for (unsigned int i = 0; i < xSize; ++i) {
    unsigned int mini = i;
    double minx = fX[i];
    for (unsigned int j = i+1; j < xSize; ++j) {
      const double x = fX[j];
      if (x < minx) {
        minx = x;
        mini = j;
      }
    }
    if (mini != i) {
      using std::swap;
      swap(fX[i], fX[mini]);
      swap(fY[i], fY[mini]);
    }
  }
}


void
TabulatedFunction::PushBack(const double x, const double y)
{
  // Check that same X value is not accidentally pushed twice in a row
  // (a common error)
  if (fX.size() && fabs(fX.back() - x) < 1./numeric_limits<float>::max()) {
    ostringstream warn;
    warn << "Refusing to push the same x value " << x << " twice (y = " << y << ')';
    WARNING(warn);
    return;
  }

  fX.push_back(x);
  fY.push_back(y);
}


double
TabulatedFunction::LinearInterpolateY(double x)
  const
{
  const unsigned int size = fX.size();

  if (!size) {
    ostringstream msg;
    msg << "No points in table.";
    ERROR(msg);
    throw OutOfBoundException(msg.str());
  }

  if (size == 1)
    return fY[0];

  if (x < fX[0] || x > fX[size-1]) {

    if (x < fX[0] && (fX[0] - x) / (fX[1] - fX[2]) < fBoundaryTolerance)
      x = fX[0];
    else if (x > fX[size-1] &&
             (x - fX[size-1]) / (fX[size-1] - fX[size-2]) < fBoundaryTolerance)
      x = fX[size-1];
    else {
      boost::format err("Value x=%.10f is out of table bounds [%.10f, %.12f].");
      err % x % fX[0] % fX[size-1];
      ERROR(err);
      throw OutOfBoundException(err.str());
    }

  }

  const Array::const_iterator xBegin = fX.begin();
  const Array::const_iterator xIt = upper_bound(xBegin, fX.end(), x);
  int index = xIt - xBegin - 1;
  const double x1 = fX[index];
  const double y1 = fY[index];
  if (x1 == x)
    return y1;
  ++index;
  const double x2 = fX[index];
  const double y2 = fY[index];

  return y1 + (y2 - y1)*(x - x1)/(x2 - x1);
}


double
TabulatedFunction::InterpolateY(const double x, const unsigned int polyDegree)
  const
{
  const int nn = fX.size();

  if (nn == 1 && x == fX.front())
    return fY.front();

  if (nn < 2 || polyDegree < 1) {
    ostringstream msg;
    msg << "Not enough points in table or wrong polynomial degree.";
    ERROR(msg);
    throw OutOfBoundException(msg.str());
  }

  const unsigned int mmax = 10;
  const int m = min(min(polyDegree, mmax), (unsigned int)(nn-1));

  if (m == 1)
    return LinearInterpolateY(x);

  double d[20];
  double t[20];

  const Array::const_iterator xBegin = fX.begin();
  const int ix = upper_bound(xBegin, fX.end(), x) - xBegin - 1;

  const int mplus = m + 1;

  // Copy reordered interpolation points into (t[i],d[i]),
  // setting 'extra' to true if m+2 points to be used.
  int npts = m + 2 - m % 2;
  t[0] = fX[ix];
  d[0] = fY[ix];
  for (int ip = 1, i = 1; ip < npts; ++i)
    for (int sign = 1; sign >= -1 && ip < npts; sign -= 2) {
      const int isub = ix + i*sign;
      if (0 <= isub && isub < nn) {
        t[ip] = fX[isub];
        d[ip] = fY[isub];
        ++ip;
      } else
        npts = mplus;
    }

  const bool extra = (npts != mplus);
  // Replace d by the leading diagonal of a divided-difference table,
  // Supplemented by an extra line if 'extra' is true.
  for (int i = 1; i <= m; ++i) {
    if (extra) {
      const int isub = mplus - i;
      d[mplus] = (d[mplus] - d[m - 1]) / (t[mplus] - t[isub - 1]);
    }
    for (int k = mplus; k > i; --k) {
      const int k1 = k - 1;
      const int isub = k - i;
      d[k1] = (d[k1] - d[k - 2]) / (t[k1] - t[isub - 1]);
    }
  }

  // Evaluate the Newton interpolation formula at x, averaging two
  // values of last difference if 'extra' is true.
  double sum = d[m];
  if (extra)
    sum = (sum + d[mplus])/2;
  for (int j = m; j; --j)
    sum = d[j - 1] + (x - t[j - 1])*sum;

  return sum;
}


ConstTabulatedFunctionIterator
TabulatedFunction::FindX(const double x)
  const
{
  const Array::const_iterator xBegin = fX.begin();
  const Array::const_iterator xEnd = fX.end();
  const Array::const_iterator xIt = lower_bound(xBegin, xEnd, x);
  if (xIt != xEnd && x == *xIt) {
    const Array::const_iterator yIt = fY.begin() + (xIt - xBegin);
    return ConstTabulatedFunctionIterator(xIt, yIt);
  } else
    return ConstTabulatedFunctionIterator(xEnd, fY.end());
}


ConstTabulatedFunctionIterator
TabulatedFunction::FindY(const double y)
  const
{
  const Array::const_iterator yIt = find(fY.begin(), fY.end(), y);
  const Array::const_iterator xIt = yIt != fY.end() ?
    fX.begin() + (yIt - fY.begin()) : fX.end();
  return ConstTabulatedFunctionIterator(xIt, yIt);
}


TabulatedFunctionIterator
TabulatedFunction::Insert(const double x, const double y)
{
  const Array::iterator xIt = lower_bound(fX.begin(), fX.end(), x);
  const Array::iterator yIt = fY.begin() + (xIt - fX.begin());
  const Array::iterator ixIt = fX.insert(xIt, x);
  const Array::iterator iyIt = fY.insert(yIt, y);
  return TabulatedFunctionIterator(ixIt, iyIt);
}