InterpUtil.cpp

Go to the documentation of this file.

//------------------------------------------------------------------------------
//------------------------------------------------------------------------------

//----- Included files ---------------------------------------------------------

// 1. Precompiled header

// 2. My own header
#include <xmsinterp/interpolate/InterpUtil.h>

// 3. Standard library headers

// 4. External library headers
#include <xmscore/math/math.h>

// 5. Shared code headers

// 6. Non-shared code headers

//----- Forward declarations ---------------------------------------------------

//----- External globals -------------------------------------------------------

//----- Namespace declaration --------------------------------------------------
namespace xms
{
//----- Constants / Enumerations -----------------------------------------------

//----- Classes / Structs ------------------------------------------------------

//----- Internal functions -----------------------------------------------------

//----- Class / Function definitions -------------------------------------------

//------------------------------------------------------------------------------
//------------------------------------------------------------------------------
void inNodalFuncSetUpMatrixAndVector(double xk,
                                     double yk,
                                     double fk,
                                     const std::vector<InterpPtInfo>& closest,
                                     double** M,
                                     double* VV)
{
  int i, j, n;
  double v1[6], v2[6]; // fg_vv is made up of two summations
  double wi, xi, yi, fi;

  n = (int)closest.size();
  // initialize matrix and vector.
  for (i = 1; i <= 5; i++)
  {
    v1[i] = 0.0;
    v2[i] = 0.0;
    VV[i - 1] = 0.0;
    for (j = 1; j <= 5; j++)
      M[i - 1][j - 1] = 0.0;
  }

  for (i = 0; i < n; i++)
  { // weights will all be zero when i=n-1
    wi = closest[i].m_weight;
    xi = closest[i].m_loc.x - xk;
    yi = closest[i].m_loc.y - yk;
    fi = closest[i].m_scalar;

    // set up matrix in banded form.
    M[0][0] = M[0][0] + wi * xi * xi;
    M[0][1] = M[0][1] + wi * xi * yi;
    M[0][2] = M[0][2] + wi * xi * xi * xi;
    M[0][3] = M[0][3] + wi * xi * xi * yi;
    M[0][4] = M[0][4] + wi * xi * yi * yi;
    v1[1] = v1[1] + wi * xi * fi;
    v2[1] = v2[1] + wi * xi;

    M[1][0] = M[1][0] + wi * yi * yi;
    M[1][1] = M[1][1] + wi * xi * xi * yi;
    M[1][2] = M[1][2] + wi * xi * yi * yi;
    M[1][3] = M[1][3] + wi * yi * yi * yi;
    v1[2] = v1[2] + wi * yi * fi;
    v2[2] = v2[2] + wi * yi;

    M[2][0] = M[2][0] + wi * xi * xi * xi * xi;
    M[2][1] = M[2][1] + wi * xi * xi * xi * yi;
    M[2][2] = M[2][2] + wi * xi * xi * yi * yi;
    v1[3] = v1[3] + wi * xi * xi * fi;
    v2[3] = v2[3] + wi * xi * xi;

    M[3][0] = M[3][0] + wi * xi * xi * yi * yi;
    M[3][1] = M[3][1] + wi * xi * yi * yi * yi;
    v1[4] = v1[4] + wi * xi * yi * fi;
    v2[4] = v2[4] + wi * xi * yi;

    M[4][0] = M[4][0] + wi * yi * yi * yi * yi;
    v1[5] = v1[5] + wi * yi * yi * fi;
    v2[5] = v2[5] + wi * yi * yi;
  }

  for (i = 1; i <= 5; i++)
    VV[i - 1] = v1[i] - fk * v2[i];

} // inNodalFuncSetUpMatrixAndVector
//------------------------------------------------------------------------------
//------------------------------------------------------------------------------
void inNodalFuncSetUpMatrixAndVector3d(double xk,
                                       double yk,
                                       double zk,
                                       double fk,
                                       const std::vector<InterpPtInfo>& closest,
                                       double** M,
                                       double* vv)
{
  int i, j;
  double v1[10], v2[10]; // vv is made up of two summations
  double wi, xi, yi, zi, fi;
  double xyi, xzi, yzi, x2i, y2i, z2i, wXi;
  double** m = M;

  // initialize to matrix and vector.
  // NOTE: arrays are defined from 1,not 0: this is so I'm compatible with
  // the function solve_banded_matrix in matrices.c
  for (i = 1; i < 10; i++)
  {
    v1[i] = 0.0;
    v2[i] = 0.0;
    vv[i - 1] = 0.0;
    for (j = 1; j < 10; j++)
      m[i - 1][j - 1] = 0.0;
  }

  for (i = 0; i < (int)closest.size(); ++i)
  {
    wi = closest[i].m_weight;
    xi = closest[i].m_loc.x - xk;
    yi = closest[i].m_loc.y - yk;
    zi = closest[i].m_loc.z - zk;
    fi = closest[i].m_scalar;

    // set up matrix in banded form.
    xyi = xi * yi;
    xzi = xi * zi;
    yzi = yi * zi;
    x2i = xi * xi;
    y2i = yi * yi;
    z2i = zi * zi;

    wXi = wi * xi;
    m[0][0] += (wXi * xi);
    m[0][1] += (wXi * yi);
    m[0][2] += (wXi * zi);
    m[0][3] += (wXi * xyi);
    m[0][4] += (wXi * xzi);
    m[0][5] += (wXi * yzi);
    m[0][6] += (wXi * x2i);
    m[0][7] += (wXi * y2i);
    m[0][8] += (wXi * z2i);
    v1[1] += (wXi * fi);
    v2[1] += wXi;

    wXi = wi * yi;
    m[1][0] += (wXi * yi);
    m[1][1] += (wXi * zi);
    m[1][2] += (wXi * xyi);
    m[1][3] += (wXi * xzi);
    m[1][4] += (wXi * yzi);
    m[1][5] += (wXi * x2i);
    m[1][6] += (wXi * y2i);
    m[1][7] += (wXi * z2i);
    v1[2] += (wXi * fi);
    v2[2] += wXi;

    wXi = wi * zi;
    m[2][0] += (wXi * zi);
    m[2][1] += (wXi * xyi);
    m[2][2] += (wXi * xzi);
    m[2][3] += (wXi * yzi);
    m[2][4] += (wXi * x2i);
    m[2][5] += (wXi * y2i);
    m[2][6] += (wXi * z2i);
    v1[3] += (wXi * fi);
    v2[3] += wXi;

    wXi = wi * xyi;
    m[3][0] += (wXi * xyi);
    m[3][1] += (wXi * xzi);
    m[3][2] += (wXi * yzi);
    m[3][3] += (wXi * x2i);
    m[3][4] += (wXi * y2i);
    m[3][5] += (wXi * z2i);
    v1[4] += (wXi * fi);
    v2[4] += wXi;

    wXi = wi * xzi;
    m[4][0] += (wXi * xzi);
    m[4][1] += (wXi * yzi);
    m[4][2] += (wXi * x2i);
    m[4][3] += (wXi * y2i);
    m[4][4] += (wXi * z2i);
    v1[5] += (wXi * fi);
    v2[5] += wXi;

    wXi = wi * yzi;
    m[5][0] += (wXi * yzi);
    m[5][1] += (wXi * x2i);
    m[5][2] += (wXi * y2i);
    m[5][3] += (wXi * z2i);
    v1[6] += (wXi * fi);
    v2[6] += wXi;

    wXi = wi * x2i;
    m[6][0] += (wXi * x2i);
    m[6][1] += (wXi * y2i);
    m[6][2] += (wXi * z2i);
    v1[7] += (wXi * fi);
    v2[7] += wXi;

    wXi = wi * y2i;
    m[7][0] += (wXi * y2i);
    m[7][1] += (wXi * z2i);
    v1[8] += (wXi * fi);
    v2[8] += wXi;

    wXi = wi * z2i;
    m[8][0] += (wXi * z2i);
    v1[9] += (wXi * fi);
    v2[9] += wXi;
  }

  for (i = 1; i <= 9; i++)
    vv[i - 1] = v1[i] - fk * v2[i];
} // inNodalFuncSetUpMatrixAndVector3d
//------------------------------------------------------------------------------
//------------------------------------------------------------------------------
void inDistanceSquared(const Pt3d& a_pt,
                       const std::vector<int>& a_ptIdxs,
                       const std::vector<Pt3d>& a_ptLoc,
                       bool a_2d,
                       std::vector<double>& a_d2)
{
  bool useAll = a_ptIdxs.empty();
  size_t nPtIdxs = useAll ? a_ptLoc.size() : a_ptIdxs.size();
  a_d2.resize(0);
  a_d2.reserve(nPtIdxs);
  size_t j;
  double d2;
  for (size_t i = 0; i < nPtIdxs; ++i)
  {
    j = useAll ? i : a_ptIdxs[i];
    d2 = xms::sqr(a_pt.x - a_ptLoc[j].x) + xms::sqr(a_pt.y - a_ptLoc[j].y);
    if (!a_2d)
      d2 += xms::sqr(a_pt.z - a_ptLoc[j].z);
    a_d2.push_back(d2);
  }
} // inDistanceSquared
//------------------------------------------------------------------------------
//------------------------------------------------------------------------------
void inIdwWeights(const std::vector<double>& a_distSquare,
                  double a_power,
                  bool a_modifiedShepardWeights,
                  std::vector<double>& a_w)
{
  std::vector<double> d2(a_distSquare);
  double distSum(0), dExp(a_power / 2), r, d;
  a_w.resize(0);
  a_w.assign(d2.size(), 0);
  r = sqrt(*std::max_element(d2.begin(), d2.end()));
  for (size_t i = 0; i < d2.size(); ++i)
  { // this is basically at the same location as our point
    if (d2[i] < 1e-20)
      d2[i] = 1e10;
  }
  for (size_t i = 0; i < d2.size(); ++i)
  {
    if (a_modifiedShepardWeights)
    { // Franke, R., and Neilson, G. 1980. Smooth interpolation of large sets
      // of scattered data. International Journal of Numerical Methods
      // in Engineering 15, 1691-1704.

      // r is the distance to the furthest point, d is the distance to the
      // current point
      d = sqrt(d2[i]);
      d2[i] = xms::sqr((r - d) / (r * d));
    }
    else
    { // classic weighting function
      if (dExp == 1)
      { // 1 over distance squared
        d2[i] = 1 / d2[i];
      }
      else
      { // if the m_power variable is not 2 then we have to use the "pow" function
        d2[i] = 1 / pow(d2[i], dExp);
      }
    }
    distSum += d2[i];
  }
  if (0.0 == distSum)
  {
    d2 = a_distSquare;
    for (size_t i = 0; i < d2.size(); ++i)
      distSum += d2[i];
  }
  for (size_t i = 0; i < d2.size(); ++i)
  {
    a_w[i] = d2[i] / distSum;
  }
} // inIdwWeights
//------------------------------------------------------------------------------
//------------------------------------------------------------------------------
bool inAllScalarsEqual(const std::vector<float>& a_scalars, const DynBitset& a_act)
{
  bool allScalarsSame(true);
  if (a_scalars.empty())
    return allScalarsSame;

  if (a_act.empty() || a_act.size() != a_scalars.size())
  {
    float f = a_scalars[0];
    for (size_t i = 1; allScalarsSame && i < a_scalars.size(); ++i)
    {
      if (f != a_scalars[i])
        allScalarsSame = false;
    }
    return allScalarsSame;
  }

  // get first active value
  size_t idx(0);
  if (!a_act[0])
  {
    for (size_t i = 1; idx == 0 && i < a_act.size(); ++i)
    {
      if (a_act[i])
        idx = i;
    }
  }

  float f = a_scalars[idx];
  for (size_t i = idx + 1; allScalarsSame && i < a_scalars.size(); ++i)
  {
    if (a_act[i] && f != a_scalars[i])
      allScalarsSame = false;
  }
  return allScalarsSame;
} // inAllScalarsEqual

} // namespace xms