myquadratic.h

#include "utils.h"

typedef struct {
  coord o; //
  int n, m;
#if dimension == 2 // m = a[0] + a[1]*x + a[2]*y + a[3]*x*y + a[4]*x^2 + a[5]*y^2
  double ** M, rhs[6], a[6];
#else // dimension == 3, m = a[0] + a[1]*x + a[2]*y + a[3]*z
      //                     a[4]*xy + a[5]*xz + a[6]*yz +
      //                     a[7]*x^2 + a[8]*y^2 + a[9]*z^2
  double ** M, rhs[10], a[10];
#endif // dimension
  bool linear; // Force linear extrapolation instead of quadratic
} QuadraticFit;

#if dimension == 2
int neigh = 3; // Number of neighbors to use above matrix dimension to improve matrix conditioning
#else // dimension == 3
int neigh = 5;
#endif // dimension
double pivtol = 1.e-10; // Tolerance for matrix inversion

static void quadratic_fit_init (QuadraticFit * p, coord o, bool linear)
{
  // Origin
  foreach_dimension()
    p->o.x = o.x;
#if dimension == 2
  p->n = 6;
  p->m = 6; // Dimension of reduced matrix if necessary
#else // dimension == 3
  p->n = 10;
  p->m = 10; // Dimension of reduced matrix if necessary
#endif // dimension
  // Matrix
  p->M = (double **) matrix_new (p->n, p->n, sizeof(double));  
  for (int i = 0; i < p->n; i++) {
    for (int j = 0; j < p->n; j++)
      p->M[i][j] = 0.;
    p->rhs[i] = 0.;
  }
  // Linear or quadratic extrapolation
  p->linear = linear;
}

static void quadratic_fit_add (QuadraticFit * p, coord o, double m)
{

We update here the coefficients in the lower triangle matrix.

#if dimension == 2
  // Relative coordinates to the origin
  double x1 = o.x - p->o.x, y1 = o.y - p->o.y;
  // Useful variables
  double x2 = x1*x1, y2 = y1*y1, xy = x1*y1;
  double x3 = x1*x2, y3 = y1*y2, x2y = x1*xy, xy2 = y1*xy;
  double x4 = x1*x3, y4 = y1*y3, x3y = x1*x2y, xy3 = y1*xy2, x2y2 = xy*xy;
  // Fill the matrix X^TX
  p->M[0][0] += 1.;
  p->M[1][0] += x1; p->M[1][1] += x2;
  p->M[2][0] += y1; p->M[2][1] += xy;   p->M[2][2] += y2;
  p->M[3][0] += xy; p->M[3][1] += x2y;  p->M[3][2] += xy2;  p->M[3][3] += x2y2;
  p->M[4][0] += x2; p->M[4][1] += x3;   p->M[4][2] += x2y;  p->M[4][3] += x3y;  p->M[4][4] += x4;
  p->M[5][0] += y2; p->M[5][1] += xy2;  p->M[5][2] += y3;   p->M[5][3] += xy3;  p->M[5][4] += x2y2; p->M[5][5] += y4;
  p->rhs[0]  += m;  p->rhs[1]  += x1*m; p->rhs[2]  += y1*m; p->rhs[3]  += xy*m; p->rhs[4]  += x2*m; p->rhs[5] += y2*m;
#else // dimension == 3
  // Relative coordinates to the origin
  double x1 = o.x - p->o.x, y1 = o.y - p->o.y, z1 = o.z - p->o.z;
  // Useful variables
  double x2 = x1*x1, y2 = y1*y1, z2 = z1*z1, xy = x1*y1, xz = x1*z1, yz = y1*z1;
  double x3 = x1*x2, y3 = y1*y2, z3 = z1*z2, x2y = x1*xy, x2z = x1*xz, xy2 = y1*xy, y2z = y1*yz, xz2 = z1*xz, yz2 = z1*yz, xyz = x1*y1*z1;
  double x4 = x1*x3, y4 = y1*y3, z4 = z1*z3, x3y = x1*x2y, x3z = x1*x2z, xy3 = y1*xy2, y3z = y1*y2z, xz3 = z1*xz2, yz3 = z1*yz2, x2y2 = xy*xy, x2z2 = xz*xz, y2z2 = yz*yz, xyz2 = xy*z2, xy2z = xz*y2, x2yz = x2*yz;
  // Fill the matrix X^TX
  p->M[0][0] += 1.;
  p->M[1][0] += x1; p->M[1][1] += x2;
  p->M[2][0] += y1; p->M[2][1] += xy;  p->M[2][2] += y2;
  p->M[3][0] += z1; p->M[3][1] += xz;  p->M[3][2] += yz;  p->M[3][3] += z2;
  p->M[4][0] += xy; p->M[4][1] += x2y; p->M[4][2] += xy2; p->M[4][3] += xyz; p->M[4][4] += x2y2;
  p->M[5][0] += xz; p->M[5][1] += x2z; p->M[5][2] += xyz; p->M[5][3] += xz2; p->M[5][4] += x2yz; p->M[5][5] += x2z2;
  p->M[6][0] += yz; p->M[6][1] += xyz; p->M[6][2] += y2z; p->M[6][3] += yz2; p->M[6][4] += xy2z; p->M[6][5] += xyz2; p->M[6][6] += y2z2;
  p->M[7][0] += x2; p->M[7][1] += x3;  p->M[7][2] += x2y; p->M[7][3] += x2z; p->M[7][4] += x3y;  p->M[7][5] += x3z;  p->M[7][6] += x2yz; p->M[7][7] += x4;
  p->M[8][0] += y2; p->M[8][1] += xy2; p->M[8][2] += y3;  p->M[8][3] += y2z; p->M[8][4] += xy3;  p->M[8][5] += xy2z; p->M[8][6] += y3z;  p->M[8][7] += x2y2; p->M[8][8] += y4;
  p->M[9][0] += z2; p->M[9][1] += xz2; p->M[9][2] += yz2; p->M[9][3] += z3;  p->M[9][4] += xyz2; p->M[9][5] += xz3;  p->M[9][6] += yz3;  p->M[9][7] += x2z2; p->M[9][8] += y2z2; p->M[9][9] += z4;
  p->rhs[0]  += m;  p->rhs[1] += x1*m; p->rhs[2] += y1*m; p->rhs[3] += z1*m; p->rhs[4] += xy*m;  p->rhs[5] += xz*m;  p->rhs[6] += yz*m;  p->rhs[7] += x2*m;  p->rhs[8] += y2*m;  p->rhs[9] += z2*m;
#endif // dimension  
}

static void quadratic_fit_set (QuadraticFit * p, const int nc)
{

We set the symmetric coefficients (upper triangle matrix)

  for (int i = 0; i < p->n; i++)
    for (int j = i + 1; j < p->n; j++)
      p->M[i][j] = p->M[j][i];

For degenereate cases, when not enough neighbors are available, we use a linear fit or simple injection and therefore set the quadratic (and linear for injection) matrix coefficient to 0.

Note that we switch to a linear fit if the number of neighbors available is smaller than dimension + neigh. This is to avoid inverting a matrix with a large condition number, which would generate a inaccurate solution. An alternative would be to use a QR decomposition instead of matrix inversion.

  // Injection
  if (nc < (dimension + 1))
    p->m = 1;
  // Linear interpolation/extrapolation
  else if (nc < p->n + neigh || p->linear)
    p->m = dimension + 1;

  if (p->m != p->n)
    for (int i = 0; i < p->n; i++)
      for (int j = p->m; j < p->n; j++)
	p->M[i][j] = p->M[j][i] = 0.;
}

static double quadratic_fit_solve (QuadraticFit * p, const int nc)
{
  quadratic_fit_set (p, nc);
  double pivmin = matrix_inverse (p->M, p->m, pivtol);
  if (pivmin)
    for (int i = 0; i < p->n; i++) {
      p->a[i] = 0.;
      for (int j = 0; j < p->n; j++)
	p->a[i] += p->M[i][j]*p->rhs[j];
    }
  else /* this may be a degenerate/isolated interface fragment */  
    for (int i = 0; i < p->n; i++)
      p->a[i] = 0.;
  matrix_free (p->M);
  return pivmin;
}