SplineGrassMitasova.cpp

Go to the documentation of this file.

/*!
\file terralib/mnt/core/SplineGrassMitasova.cpp

\brief This file contains a class to calculate a grid from Sample using spline grass mitasova interpolator.

*/

#include "SplineGrassMitasova.h"
#include "Utils.h"

#include "../../common/progress/TaskProgress.h"

#include <algorithm>
#include <cmath>
#include <vector>


te::mnt::SplineInterpolationGrassMitasova::SplineInterpolationGrassMitasova(int minPoints, double tension, double smooth)
{
  m_minimoPontos = minPoints * 2;
  m_tension = tension;
  m_smooth = smooth;
}

te::mnt::SplineInterpolationGrassMitasova::~SplineInterpolationGrassMitasova() =
    default;

void te::mnt::SplineInterpolationGrassMitasova::setControlPoints(double xMin, double xMax, double yMin, double yMax, int nMax, double xOrigem, double yOrigem, double dnorm)
{
  std::size_t nVectorPoints = m_dataset.size();

  m_pointsRegion.clear();
  for (std::size_t  i = 0; i<nVectorPoints; i++)
  {
    te::gm::Point pp3d = m_dataset[i].second;
    if ((pp3d.getX() >= xMin) && (pp3d.getX() < xMax) && (pp3d.getY() >= yMin) && (pp3d.getY() < yMax))
    {
      //update new point
      double nX = (pp3d.getX() - xOrigem) / dnorm;
      double nY = (pp3d.getY() - yOrigem) / dnorm;

      te::gm::Point npp3d(0, te::gm::PointZType);
      npp3d.setX(nX);
      npp3d.setY(nY);
      npp3d.setZ(pp3d.getZ());

      //add a new point
      m_pointsRegion.push_back(npp3d);

      if (m_pointsRegion.size() == static_cast<size_t>(nMax))
        break;
    }
  }
}

double te::mnt::SplineInterpolationGrassMitasova::amax1(double arg1, double arg2)
{
  double res;

  if (arg1 >= arg2)
    res = arg1;
  else
    res = arg2;
  return res;
}

double te::mnt::SplineInterpolationGrassMitasova::amin1(double arg1, double arg2)
{
  double res;

  if (arg1 <= arg2)
    res = arg1;
  else
    res = arg2;
  return res;
}

double te::mnt::SplineInterpolationGrassMitasova::IL_crst(double r, double fi)
/*
* Radial basis function - completely regularized spline with
* tension (d=2)
*/
{
  double rfsta2 = fi * fi * r / 4.;

  static double c[4] = { 8.5733287401, 18.0590169730, 8.6347608925,
    0.2677737343 };
  static double b[4] = { 9.5733223454, 25.6329561486, 21.0996530827,
    3.9584969228 };
  double ce = 0.57721566;

  static double u[10] = { 1.e+00, -.25e+00,
    .055555555555556e+00, -.010416666666667e+00, /*fixed bug 415.. repl. by 416..*/
    .166666666666667e-02, -2.31481481481482e-04,
    2.83446712018141e-05, -3.10019841269841e-06,
    3.06192435822065e-07, -2.75573192239859e-08 };
  double x = rfsta2;
  double res;

  double e1, ea, eb;

  if (x < 1.e+00)
  {
    res = x * (u[0] + x * (u[1] + x * (u[2] + x * (u[3] + x * (u[4] + x *
      (u[5] + x * (u[6] + x * (u[7] + x * (u[8] + x * u[9])))))))));
    return (res);
  }

  if (x > 25.e+00)
    e1 = 0.00;
  else
  {
    ea = c[3] + x * (c[2] + x * (c[1] + x * (c[0] + x)));
    eb = b[3] + x * (b[2] + x * (b[1] + x * (b[0] + x)));
    e1 = (ea / eb) / (x * exp(x));
  }
  res = e1 + ce + log(x);
  return (res);
}

void te::mnt::SplineInterpolationGrassMitasova::G_lubksb(std::vector< std::vector<double> > &a, int n, std::vector<int> &indx, std::vector<double> &b)
{
  int i, ii, ip, j;
  double sum;

  ii = -1;
  for (i = 0; i<n; i++)
  {
    ip = indx[static_cast<size_t>(i)];
    sum = b[static_cast<size_t>(ip)];
    b[static_cast<size_t>(ip)] = b[static_cast<size_t>(i)];
    if (ii >= 0)
      for (j = ii; j<i; j++)
        sum -= a[static_cast<size_t>(i)][static_cast<size_t>(j)] * b[static_cast<size_t>(j)];
    else if (sum > 0)
      ii = i;
    b[static_cast<size_t>(i)] = sum;
  }
  for (i = n - 1; i >= 0; i--)
  {
    sum = b[static_cast<size_t>(i)];
    for (j = i + 1; j<n; j++)
      sum -= a[static_cast<size_t>(i)][static_cast<size_t>(j)] * b[static_cast<size_t>(j)];
    b[static_cast<size_t>(i)] = sum / a[static_cast<size_t>(i)][static_cast<size_t>(i)];
  }
}

#define TINY 1.0e-20;

int te::mnt::SplineInterpolationGrassMitasova::G_ludcmp(std::vector< std::vector<double> > &a, int n, std::vector<int> &indx, double &d)
{
  int i, imax = 0, j, k;
  double big, dum, sum, temp;
  std::vector<double> vv;

  vv.resize(static_cast<size_t>(n));
  d = 1.0;
  for (i = 0; i<n; i++)
  {
    big = 0.0;
    for (j = 0; j<n; j++)
      if ((temp = fabs(a[static_cast<size_t>(i)][static_cast<size_t>(j)])) > big)
        big = temp;
    if (big == 0.0)
    {
      d = 0.0;
      vv.clear();
      return 0; /* Singular matrix  */
    }
    vv[static_cast<size_t>(i)] = 1.0 / big;
  }
  for (j = 0; j<n; j++)
  {
    for (i = 0; i<j; i++)
    {
      sum = a[static_cast<size_t>(i)][static_cast<size_t>(j)];
      for (k = 0; k<i; k++)
        sum -= a[static_cast<size_t>(i)][static_cast<size_t>(k)] * a[static_cast<size_t>(k)][static_cast<size_t>(j)];
      a[static_cast<size_t>(i)][static_cast<size_t>(j)] = sum;
    }
    big = 0.0;
    for (i = j; i<n; i++)
    {
      sum = a[static_cast<size_t>(i)][static_cast<size_t>(j)];
      for (k = 0; k<j; k++)
        sum -= a[static_cast<size_t>(i)][static_cast<size_t>(k)] * a[static_cast<size_t>(k)][static_cast<size_t>(j)];
      a[static_cast<size_t>(i)][static_cast<size_t>(j)] = sum;
      if ((dum = vv[static_cast<size_t>(i)] * fabs(sum)) >= big)
      {
        big = dum;
        imax = i;
      }
    }
    if (j != imax)
    {
      for (k = 0; k<n; k++)
      {
        dum = a[static_cast<size_t>(imax)][static_cast<size_t>(k)];
        a[static_cast<size_t>(imax)][static_cast<size_t>(k)] = a[static_cast<size_t>(j)][static_cast<size_t>(k)];
        a[static_cast<size_t>(j)][static_cast<size_t>(k)] = dum;
      }
      d = -(d);
      vv[static_cast<size_t>(imax)] = vv[static_cast<size_t>(j)];
    }
    indx[static_cast<size_t>(j)] = imax;
    if (a[static_cast<size_t>(j)][static_cast<size_t>(j)] == 0.0)
      a[static_cast<size_t>(j)][static_cast<size_t>(j)] = TINY;
    if (j != n)
    {
      dum = 1.0 / (a[static_cast<size_t>(j)][static_cast<size_t>(j)]);
      for (i = j + 1; i<n; i++)
        a[static_cast<size_t>(i)][static_cast<size_t>(j)] *= dum;
    }
  }
  vv.clear();
  return 1;
}

#undef TINY

int te::mnt::SplineInterpolationGrassMitasova::IL_matrix_create(
  std::vector< std::vector <double> > &matrix,           /* matrix */
  std::vector<int> &indx, double fi, double rsm, int KMAX2
  )
  /*
  Creates system of linear equations represented by matrix using given points
  and interpolating function interp()
  */
{
  double xx, yy;
  double r;
  double d;
  std::vector<double> A;
  double RO, amaxa;
  std::size_t i, i1, j, k, k1, k2, l, m;

  std::size_t n_points = m_pointsRegion.size();

  std::size_t n1 = n_points + 1;

  /*
  C
  C      GENERATION OF MATRIX
  C
  C      FIRST COLUMN
  C
  */
  A.resize(static_cast<size_t>(KMAX2 + 2)*static_cast<size_t>(KMAX2 + 2) + 1);

  A[1] = 0;
  for (k = 1; k <= n_points; k++)
  {
    i1 = k + 1;
    A[i1] = 1.;
  }
  /*
  C
  C      OTHER COLUMNS
  C
  */
  RO = -rsm;

  for (k = 1; k <= n_points; k++)
  {
    k1 = k * n1 + 1;
    k2 = k + 1;
    i1 = k1 + k;
    if (rsm >= 0.) /*indicates variable smoothing */
    {
      A[i1] = RO; /* constant smoothing*/
    }

    for (l = k2; l <= n_points; l++)
    {
      te::gm::Point pp3dK = m_pointsRegion[k - 1];
      te::gm::Point pp3dL = m_pointsRegion[l - 1];

      xx = pp3dK.getX() - pp3dL.getX();
      yy = pp3dK.getY() - pp3dL.getY();

      {
        r = xx*xx + yy*yy;
      //  rfsta2 = fstar2 * (xx * xx + yy * yy);
      }

      i1 = k1 + l;
      A[i1] = IL_crst(r, fi);
    }
  }
  /*
  C
  C       SYMMETRISATION
  C
  */
  amaxa = 1.;
  for (k = 1; k <= n1; k++)
  {
    k1 = (k - 1) * n1;
    k2 = k + 1;
    for (l = k2; l <= n1; l++)
    {
      m = (l - 1) * n1 + k;
      A[m] = A[k1 + l];
      amaxa = amax1(A[m], amaxa);
    }
  }
  m = 0;
  for (i = 0; i <= n_points; i++) {
    for (j = 0; j <= n_points; j++) {
      m++;
      matrix[i][j] = A[m];
    }
  }

  if (G_ludcmp(matrix, static_cast<int>(n_points + 1), indx, d) <= 0)
  { /* find the inverse of the matrix */
    A.clear();
    return -1;
  }

  A.clear();
  return 1;
}

bool te::mnt::SplineInterpolationGrassMitasova::calculateGrid()
{
  unsigned int nro_neighb;
  double rx1, ry2;
  unsigned int NCols;
  unsigned int NLines;

  m_tolerance = m_resx / 4.;

  std::unique_ptr<te::rst::Raster> rst = Initialize(true, nro_neighb, rx1, ry2, NCols, NLines);

  double deltx = m_env.getUpperRightX() - m_env.getLowerLeftX();
  double delty = m_env.getUpperRightY() - m_env.getLowerLeftY();

  //divide em x partes overlaping
  int nPartsX = 50;
  int nPartsY = 50;
  double ew_region = deltx / static_cast<double>(nPartsX);
  double ns_region = delty / static_cast<double>(nPartsY);

  double X1 = m_env.getLowerLeftX();
  double Y1 = m_env.getLowerLeftY();

  double percentageDist = 0.5;

  size_t KMAX2 = 2 * 2 * static_cast<size_t>(m_minimoPontos);
  double fi = m_tension;
  double rsm = m_smooth;

  std::size_t NPOINT = m_dataset.size();
  double dnorm = sqrt((deltx * delty * m_minimoPontos) / NPOINT);

  std::size_t nallPoints = m_dataset.size();
  te::gm::Point pp03d = m_dataset[0].second;
  double zmin = pp03d.getZ();
  double zmax = pp03d.getZ();
  for (std::size_t i = 1; i<nallPoints; i++)
  {
    te::gm::Point pp3d = m_dataset[i].second;
    zmin = amin1(zmin, pp3d.getZ());
    zmax = amax1(zmax, pp3d.getZ());
  }

  int nsteps = static_cast<int>(nPartsY*nPartsX + 1);
  te::common::TaskProgress task("Generating DTM...", te::common::TaskProgress::UNDEFINED, nsteps);

  for (int iy = 0; iy < nPartsY; iy++)
  {
    double begin_y = Y1 + (iy*ns_region);
    double end_y = Y1 + ((iy + 1)*ns_region);

    double disty = (end_y - begin_y) * percentageDist;

    for (int jx = 0; jx < nPartsX; jx++)
    {
      if (!task.isActive())
        return false;
      task.pulse();

      //computes the beginning and end area
      double begin_x = X1 + (jx*ew_region);
      double end_x = X1 + ((jx + 1)*ew_region);

      double distx = (end_x - begin_x) * percentageDist;

      int nOverlaping = 1;
      bool poucosPontos = false;
      do
      {
        double overlapDistX = distx*nOverlaping;
        double overlapDistY = disty*nOverlaping;
        //adds control points and the overlaping area
        setControlPoints(begin_x - overlapDistX, end_x + overlapDistX, begin_y - overlapDistY, end_y + overlapDistY, static_cast<int>(KMAX2), begin_x, begin_y, dnorm);

        poucosPontos = m_pointsRegion.size() < static_cast<size_t>(m_minimoPontos);
        nOverlaping++;

      } while (poucosPontos);

      std::vector<int> indx;
      std::vector< std::vector<double> > matrix;
      std::vector<double> b;

      indx.resize(KMAX2 + 1);
      for (int ii = 0; ii < static_cast<int>(KMAX2) + 1; ++ii)
      {
        std::vector<double> maux;
        maux.resize(KMAX2 + 1);
        matrix.push_back(maux);
      }
      b.resize(KMAX2 + 3);

      //create matrix
      if (IL_matrix_create(matrix, indx, fi, rsm, static_cast<int>(KMAX2)) < 0)
      {
        b.clear();
        indx.clear();
        for (int ii = 0; ii < static_cast<int>(KMAX2) + 1; ++ii)
          matrix[static_cast<size_t>(ii)].clear();
        matrix.clear();
        return false;
      }

      std::size_t n_points = m_pointsRegion.size();
      for (std::size_t jj = 0; jj < n_points; jj++)
      {
        te::gm::Point pp3d = m_pointsRegion.at(jj);
        b[jj + 1] = pp3d.getZ();
      }
      b[0] = 0;

      G_lubksb(matrix, static_cast<int>(n_points) + 1, indx, b);
      double errotot = 0.;
      IL_check_at_points_2d(b, errotot, zmin, dnorm, fi);

      indx.clear();
      for (unsigned ii = 0; ii < matrix.size(); ++ii)
        matrix[ii].clear();
      matrix.clear();

      //Computes the grid
      if (!IL_grid_calc(begin_x, end_x, begin_y, end_y, zmin, zmax, static_cast<int>(KMAX2), b, fi, dnorm))
      {
        b.clear();
        return false;
      }
      b.clear();
    }
  }

  rst.release();
  return true;
}

int te::mnt::SplineInterpolationGrassMitasova::IL_check_at_points_2d(
  std::vector<double>& b, /* solution of linear equations */
  double &ertot,  /* total error */
  double zmin,    /* min z-value */
  double /*dnorm*/, double fi
  )

  /*
  * Checks if interpolating function interp() evaluates correct z-values at
  * given points. If smoothing is used calculate the maximum error caused
  * by smoothing.
  */

{
  size_t n_points = m_pointsRegion.size();/* number of points */
  double h, xx, yy, r2, hz, zz, err, r;
  size_t mm, m;

  for (mm = 1; mm <= n_points; mm++)
  {
    te::gm::Point pp3dMM = m_pointsRegion[mm - 1];
    h = b[0];
    for (m = 1; m <= n_points; m++)
    {
      te::gm::Point pp3dM = m_pointsRegion[m - 1];
      xx = pp3dMM.getX() - pp3dM.getX();
      yy = pp3dMM.getY() - pp3dM.getY();

      r2 = yy * yy + xx * xx;
      if (r2 != 0.)
      {
        r = r2;
        h = h + b[m] * IL_crst(r, fi);
      }
    }
 
    hz = h + zmin;
    zz = pp3dMM.getZ() + zmin;
    err = hz - zz;

    (ertot) += err * err;
  }

  return 1;
}

bool te::mnt::SplineInterpolationGrassMitasova::IL_grid_calc(double xMin, double xMax, double yMin, double yMax, double /*zMin*/, double /*zMax*/, int KMAX2, std::vector<double>& b, double fi, double dnorm)
{
  double x_or = xMin;

  double x1 = m_env.getLowerLeftX();
  double y2 = m_env.getUpperRightY();

  double ns_res = m_resy;
  double ew_res = m_resx;

  double deltx = xMax - xMin;
  double delty = yMax - yMin;

  int n_rows = static_cast<int>(delty / ns_res);
  int n_cols = static_cast<int>(deltx / ew_res);

  size_t n_points = m_pointsRegion.size();

  int ngstc = static_cast<int>((x_or - x1) / ew_res + 0.5);
  int nszc = ngstc + n_cols;
  int ngstr = static_cast<int>((y2 - yMax) / ns_res + 0.5);
  int nszr = ngstr + n_rows;

  double stepix = ew_res / dnorm;
  double stepiy = ns_res / dnorm;

  std::vector<double> w2;
  std::vector<double> w;

  w.resize(static_cast<size_t>(KMAX2) + 9);
  w2.resize(static_cast<size_t>(KMAX2) + 9);

  int contadory = 0;

  for (int k = ngstr; k <= nszr; k++)
  {
    double yg = (k - ngstr) * stepiy + stepiy / 2.; // fixed by J.H. in July 01 
    for (size_t m = 1; m <= n_points; m++)
    {
      te::gm::Point pp3d = m_pointsRegion[m - 1];
      double wm = yg - pp3d.getY();
      w[m] = wm;
      w2[m] = wm * wm;
    }

    for (int l = ngstc; l <= nszc; l++)
    {
      double xg = (l - ngstc) * stepix + stepix / 2.; //fixed by J.H. in July 01 

      // compute everything for area which is * not masked out
      double h = b[0];
      for (size_t m = 1; m <= n_points; m++)
      {
        te::gm::Point pp3d = m_pointsRegion[m - 1];
        double xx = xg - pp3d.getX();
        double xx2 = xx * xx;
        double r2 = xx2 + w2[m];
        double r = r2;

        h = h + b[m] * IL_crst(r, fi);
      }

      double zz = h;
      int row = nszr - contadory;
      if (row >= 0 && row < static_cast<int>(m_rst->getNumberOfRows()) &&
        l >= 0 && l < static_cast<int>(m_rst->getNumberOfColumns()))
      {
        if (zz != m_nodatavalue)
        {
          m_max = (zz > m_max) ? zz : m_max;
          m_min = (zz < m_min) ? zz : m_min;
        }
        m_rst->setValue(static_cast<unsigned int>(l), static_cast<unsigned int>(row), zz);
      }
    }
    contadory++;
  }

  w.clear();
  w2.clear();

  return true;
}