/*=========================================================================

  Program:   Visualization Toolkit
  Module:    vtkQuadraticPyramid.cxx

  Copyright (c) Ken Martin, Will Schroeder, Bill Lorensen
  All rights reserved.
  See Copyright.txt or http://www.kitware.com/Copyright.htm for details.

     This software is distributed WITHOUT ANY WARRANTY; without even
     the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
     PURPOSE.  See the above copyright notice for more information.

=========================================================================*/

#include "vtkQuadraticPyramid.h"

#include "vtkCellData.h"
#include "vtkDoubleArray.h"
#include "vtkMath.h"
#include "vtkObjectFactory.h"
#include "vtkPointData.h"
#include "vtkPoints.h"
#include "vtkPyramid.h"
#include "vtkQuadraticEdge.h"
#include "vtkQuadraticQuad.h"
#include "vtkQuadraticTriangle.h"
#include "vtkTetra.h"

vtkStandardNewMacro(vtkQuadraticPyramid);

//------------------------------------------------------------------------------
// Construct the pyramid with 13 points + 1 extra point for internal
// computation.
//
vtkQuadraticPyramid::vtkQuadraticPyramid()
{
  this->PointIds->SetNumberOfIds(13);
  this->Points->SetNumberOfPoints(13);
  for (int i = 0; i < 13; i++)
  {
    this->Points->SetPoint(i, 0.0, 0.0, 0.0);
    this->PointIds->SetId(i, 0);
  }

  this->Edge = vtkQuadraticEdge::New();
  this->Face = vtkQuadraticQuad::New();
  this->TriangleFace = vtkQuadraticTriangle::New();
  this->Tetra = vtkTetra::New();
  this->Pyramid = vtkPyramid::New();

  this->PointData = vtkPointData::New();
  this->CellData = vtkCellData::New();
  this->CellScalars = vtkDoubleArray::New();
  this->CellScalars->SetNumberOfTuples(14);
  this->Scalars = vtkDoubleArray::New();
  this->Scalars->SetNumberOfTuples(5); // num of vertices
}

//------------------------------------------------------------------------------
vtkQuadraticPyramid::~vtkQuadraticPyramid()
{
  this->Edge->Delete();
  this->Face->Delete();
  this->TriangleFace->Delete();
  this->Tetra->Delete();
  this->Pyramid->Delete();

  this->PointData->Delete();
  this->CellData->Delete();
  this->Scalars->Delete();
  this->CellScalars->Delete();
}

//------------------------------------------------------------------------------
// point id 13 is created to make it easier to subdivide this cell and it is
// located at the center of the quadrilateral face of the pyramid. LinearPyramid
// is only used by the Subdivide(), Contour() and Clip() methods.
static int LinearPyramids[10][5] = {
  // 6 pyramids
  { 0, 5, 13, 8, 9 },
  { 5, 1, 6, 13, 10 },
  { 8, 13, 7, 3, 12 },
  { 13, 6, 2, 7, 11 },
  { 9, 10, 11, 12, 4 },
  { 9, 12, 11, 10, 13 },
  // 4 tets
  { 5, 13, 9, 10, 0 },
  { 6, 13, 10, 11, 0 },
  { 7, 13, 11, 12, 0 },
  { 8, 13, 12, 9, 0 },
};

static constexpr vtkIdType PyramidFaces[5][8] = {
  { 0, 3, 2, 1, 8, 7, 6, 5 },
  { 0, 1, 4, 5, 10, 9, 0, 0 },
  { 1, 2, 4, 6, 11, 10, 0, 0 },
  { 2, 3, 4, 7, 12, 11, 0, 0 },
  { 3, 0, 4, 8, 9, 12, 0, 0 },
};

static constexpr vtkIdType PyramidEdges[8][3] = {
  { 0, 1, 5 },
  { 1, 2, 6 },
  { 2, 3, 7 },
  { 3, 0, 8 },
  { 0, 4, 9 },
  { 1, 4, 10 },
  { 2, 4, 11 },
  { 3, 4, 12 },
};

//------------------------------------------------------------------------------
const vtkIdType* vtkQuadraticPyramid::GetEdgeArray(vtkIdType edgeId)
{
  return PyramidEdges[edgeId];
}
//------------------------------------------------------------------------------
const vtkIdType* vtkQuadraticPyramid::GetFaceArray(vtkIdType faceId)
{
  return PyramidFaces[faceId];
}

//------------------------------------------------------------------------------
vtkCell* vtkQuadraticPyramid::GetEdge(int edgeId)
{
  edgeId = (edgeId < 0 ? 0 : (edgeId > 7 ? 7 : edgeId));

  for (int i = 0; i < 3; i++)
  {
    this->Edge->PointIds->SetId(i, this->PointIds->GetId(PyramidEdges[edgeId][i]));
    this->Edge->Points->SetPoint(i, this->Points->GetPoint(PyramidEdges[edgeId][i]));
  }

  return this->Edge;
}

//------------------------------------------------------------------------------
vtkCell* vtkQuadraticPyramid::GetFace(int faceId)
{
  faceId = (faceId < 0 ? 0 : (faceId > 4 ? 4 : faceId));

  // load point id's and coordinates
  // be careful with the first one:

  if (faceId > 0)
  {
    for (int i = 0; i < 6; i++)
    {
      this->TriangleFace->PointIds->SetId(i, this->PointIds->GetId(PyramidFaces[faceId][i]));
      this->TriangleFace->Points->SetPoint(i, this->Points->GetPoint(PyramidFaces[faceId][i]));
    }
    return this->TriangleFace;
  }
  else
  {
    for (int i = 0; i < 8; i++)
    {
      this->Face->PointIds->SetId(i, this->PointIds->GetId(PyramidFaces[faceId][i]));
      this->Face->Points->SetPoint(i, this->Points->GetPoint(PyramidFaces[faceId][i]));
    }
    return this->Face;
  }
}

//------------------------------------------------------------------------------
namespace
{
const double VTK_DIVERGED = 1.e6;
const int VTK_PYRAMID_MAX_ITERATION = 20;
const double VTK_PYRAMID_CONVERGED = 1.e-03;
}

int vtkQuadraticPyramid::EvaluatePosition(const double* x, double closestPoint[3], int& subId,
  double pcoords[3], double& dist2, double weights[])
{
  subId = 0;
  // There are problems searching for the apex point so we check if
  // we are there first before doing the full parametric inversion.
  vtkPoints* points = this->GetPoints();
  double apexPoint[3];
  points->GetPoint(4, apexPoint);
  dist2 = vtkMath::Distance2BetweenPoints(apexPoint, x);
  double baseMidpoint[3];
  points->GetPoint(0, baseMidpoint);
  for (int i = 1; i < 4; i++)
  {
    double tmp[3];
    points->GetPoint(i, tmp);
    for (int j = 0; j < 3; j++)
    {
      baseMidpoint[j] += tmp[j];
    }
  }
  for (int i = 0; i < 3; i++)
  {
    baseMidpoint[i] /= 4.;
  }

  double length2 = vtkMath::Distance2BetweenPoints(apexPoint, baseMidpoint);
  // we use .001 as the relative tolerance here since that is the same
  // that is used for the interior cell check below but we need to
  // square it here because we're looking at dist2^2.
  if (dist2 == 0. || (length2 != 0. && dist2 / length2 < 1.e-6))
  {
    pcoords[0] = pcoords[1] = 0;
    pcoords[2] = 1;
    vtkQuadraticPyramid::InterpolationFunctions(pcoords, weights);
    if (closestPoint)
    {
      memcpy(closestPoint, x, 3 * sizeof(double));
      dist2 = 0.;
    }
    return 1;
  }

  double derivs[3 * 13];

  // compute a bound on the volume to get a scale for an acceptable determinant
  double longestEdge = 0;
  for (int i = 0; i < 8; i++)
  {
    double pt0[3], pt1[3];
    points->GetPoint(PyramidEdges[i][0], pt0);
    points->GetPoint(PyramidEdges[i][1], pt1);
    double d2 = vtkMath::Distance2BetweenPoints(pt0, pt1);
    if (longestEdge < d2)
    {
      longestEdge = d2;
    }
  }
  // longestEdge value is already squared
  double volumeBound = pow(longestEdge, 1.5);
  double determinantTolerance = 1e-20 < .00001 * volumeBound ? 1e-20 : .00001 * volumeBound;

  //  set initial position for Newton's method
  subId = 0;
  pcoords[0] = pcoords[1] = pcoords[2] = 0.5;
  double params[3] = { 0.5, 0.5, 0.5 };

  //  enter iteration loop
  int converged = 0;
  for (int iteration = 0; !converged && (iteration < VTK_PYRAMID_MAX_ITERATION); iteration++)
  {
    //  calculate element interpolation functions and derivatives
    vtkQuadraticPyramid::InterpolationFunctions(pcoords, weights);
    vtkQuadraticPyramid::InterpolationDerivs(pcoords, derivs);

    //  calculate newton functions
    double fcol[3] = { 0, 0, 0 }, rcol[3] = { 0, 0, 0 }, scol[3] = { 0, 0, 0 },
           tcol[3] = { 0, 0, 0 };
    for (int i = 0; i < 13; i++)
    {
      double pt[3];
      this->Points->GetPoint(i, pt);
      for (int j = 0; j < 3; j++)
      {
        fcol[j] += pt[j] * weights[i];
        rcol[j] += pt[j] * derivs[i];
        scol[j] += pt[j] * derivs[i + 13];
        tcol[j] += pt[j] * derivs[i + 26];
      }
    }

    for (int i = 0; i < 3; i++)
    {
      fcol[i] -= x[i];
    }

    //  compute determinants and generate improvements
    double d = vtkMath::Determinant3x3(rcol, scol, tcol);
    if (fabs(d) < determinantTolerance)
    {
      vtkDebugMacro(<< "Determinant incorrect, iteration " << iteration);
      return -1;
    }

    pcoords[0] = params[0] - 0.5 * vtkMath::Determinant3x3(fcol, scol, tcol) / d;
    pcoords[1] = params[1] - 0.5 * vtkMath::Determinant3x3(rcol, fcol, tcol) / d;
    pcoords[2] = params[2] - 0.5 * vtkMath::Determinant3x3(rcol, scol, fcol) / d;

    //  check for convergence
    if (((fabs(pcoords[0] - params[0])) < VTK_PYRAMID_CONVERGED) &&
      ((fabs(pcoords[1] - params[1])) < VTK_PYRAMID_CONVERGED) &&
      ((fabs(pcoords[2] - params[2])) < VTK_PYRAMID_CONVERGED))
    {
      converged = 1;
    }

    // Test for bad divergence (S.Hirschberg 11.12.2001)
    else if ((fabs(pcoords[0]) > VTK_DIVERGED) || (fabs(pcoords[1]) > VTK_DIVERGED) ||
      (fabs(pcoords[2]) > VTK_DIVERGED))
    {
      return -1;
    }

    //  if not converged, repeat
    else
    {
      params[0] = pcoords[0];
      params[1] = pcoords[1];
      params[2] = pcoords[2];
    }
  }

  //  if not converged, set the parametric coordinates to arbitrary values
  //  outside of element
  if (!converged)
  {
    return -1;
  }

  vtkQuadraticPyramid::InterpolationFunctions(pcoords, weights);

  // This is correct in that the XY parametric coordinate plane "shrinks"
  // while Z increases and X and Y always are between 0 and 1.
  if (pcoords[0] >= -0.001 && pcoords[0] <= 1.001 && pcoords[1] >= -0.001 && pcoords[1] <= 1.001 &&
    pcoords[2] >= -0.001 && pcoords[2] <= 1.001)
  {
    if (closestPoint)
    {
      closestPoint[0] = x[0];
      closestPoint[1] = x[1];
      closestPoint[2] = x[2];
      dist2 = 0.0; // inside pyramid
    }
    return 1;
  }
  else
  {
    double pc[3], w[13];
    if (closestPoint)
    {
      for (int i = 0; i < 3; i++) // only approximate, not really true for warped hexa
      {
        if (pcoords[i] < 0.0)
        {
          pc[i] = 0.0;
        }
        else if (pcoords[i] > 1.0)
        {
          pc[i] = 1.0;
        }
        else
        {
          pc[i] = pcoords[i];
        }
      }
      this->EvaluateLocation(subId, pc, closestPoint, static_cast<double*>(w));
      dist2 = vtkMath::Distance2BetweenPoints(closestPoint, x);
    }

    return 0;
  }
}

//------------------------------------------------------------------------------
void vtkQuadraticPyramid::EvaluateLocation(
  int& vtkNotUsed(subId), const double pcoords[3], double x[3], double* weights)
{
  int i, j;
  double pt[3];

  vtkQuadraticPyramid::InterpolationFunctions(pcoords, weights);

  x[0] = x[1] = x[2] = 0.0;
  for (i = 0; i < 13; i++)
  {
    this->Points->GetPoint(i, pt);
    for (j = 0; j < 3; j++)
    {
      x[j] += pt[j] * weights[i];
    }
  }
}

//------------------------------------------------------------------------------
int vtkQuadraticPyramid::CellBoundary(int subId, const double pcoords[3], vtkIdList* pts)
{
  return this->Pyramid->CellBoundary(subId, pcoords, pts);
}

//------------------------------------------------------------------------------
void vtkQuadraticPyramid::Subdivide(
  vtkPointData* inPd, vtkCellData* inCd, vtkIdType cellId, vtkDataArray* cellScalars)
{
  double weights[13];

  // Copy point and cell attribute data, first make sure it's empty:
  this->PointData->Initialize();
  this->CellData->Initialize();
  this->ResizeArrays(14);
  // Make sure to copy ALL arrays. These field data have to be
  // identical to the input field data. Otherwise, CopyData
  // that occurs later may not work because the output field
  // data was initialized (CopyAllocate) with the input field
  // data.
  this->PointData->CopyAllOn();
  this->CellData->CopyAllOn();
  this->PointData->CopyAllocate(inPd, 14);
  this->CellData->CopyAllocate(inCd, 10);
  for (int i = 0; i < 13; i++)
  {
    this->PointData->CopyData(inPd, this->PointIds->GetId(i), i);
    this->CellScalars->SetValue(i, cellScalars->GetTuple1(i));
  }
  for (int i = 0; i < 10; i++)
  {
    this->CellData->CopyData(inCd, cellId, i);
  }

  // Interpolate new value at midpoint
  double p[3];

  // New midpoint is at center of the quadrilateral face
  double midPoint[3] = { 0.5, 0.5, 0.0 };
  vtkQuadraticPyramid::InterpolationFunctions(midPoint, weights);

  double x[3] = { 0., 0., 0. };
  double s = 0.0;
  for (int i = 0; i < 13; i++)
  {
    this->Points->GetPoint(i, p);
    for (int j = 0; j < 3; j++)
    {
      x[j] += p[j] * weights[i];
    }
    s += cellScalars->GetTuple1(i) * weights[i];
  }
  this->Points->SetPoint(13, x);
  this->CellScalars->SetValue(13, s);
  this->PointData->InterpolatePoint(inPd, 13, this->PointIds, weights);
}

//------------------------------------------------------------------------------
void vtkQuadraticPyramid::ResizeArrays(vtkIdType newSize)
{
  if (newSize != 13 && newSize != 14)
  {
    vtkWarningMacro("Incorrect resize value for member arrays.");
  }
  else
  {
    this->Points->Resize(newSize);
    this->PointIds->Resize(newSize);
  }
}

//------------------------------------------------------------------------------
void vtkQuadraticPyramid::Contour(double value, vtkDataArray* cellScalars,
  vtkIncrementalPointLocator* locator, vtkCellArray* verts, vtkCellArray* lines,
  vtkCellArray* polys, vtkPointData* inPd, vtkPointData* outPd, vtkCellData* inCd, vtkIdType cellId,
  vtkCellData* outCd)
{
  int i;
  // subdivide into 6 linear pyramids + 4 tetras
  this->Subdivide(inPd, inCd, cellId, cellScalars);

  // contour each linear pyramid separately
  this->Scalars->SetNumberOfTuples(5); // num of vertices
  for (i = 0; i < 6; i++)              // for each pyramid
  {
    for (int j = 0; j < 5; j++) // for each point of pyramid
    {
      this->Pyramid->Points->SetPoint(j, this->Points->GetPoint(LinearPyramids[i][j]));
      this->Pyramid->PointIds->SetId(j, LinearPyramids[i][j]);
      this->Scalars->SetValue(j, this->CellScalars->GetValue(LinearPyramids[i][j]));
    }
    this->Pyramid->Contour(value, this->Scalars, locator, verts, lines, polys, this->PointData,
      outPd, this->CellData, i, outCd);
  }

  // contour each linear tetra separately
  this->Scalars->SetNumberOfTuples(4); // num of vertices
  for (i = 6; i < 10; i++)             // for each tetra
  {
    for (int j = 0; j < 4; j++) // for each point of tetra
    {
      this->Tetra->Points->SetPoint(j, this->Points->GetPoint(LinearPyramids[i][j]));
      this->Tetra->PointIds->SetId(j, LinearPyramids[i][j]);
      this->Scalars->SetTuple(j, this->CellScalars->GetTuple(LinearPyramids[i][j]));
    }
    this->Tetra->Contour(value, this->Scalars, locator, verts, lines, polys, this->PointData, outPd,
      this->CellData, i, outCd);
  }
  this->ResizeArrays(13);
}

//------------------------------------------------------------------------------
// Line-hex intersection. Intersection has to occur within [0,1] parametric
// coordinates and with specified tolerance.
//
int vtkQuadraticPyramid::IntersectWithLine(
  const double* p1, const double* p2, double tol, double& t, double* x, double* pcoords, int& subId)
{
  int intersection = 0;
  double tTemp;
  double pc[3], xTemp[3];
  int faceNum;
  int inter;

  t = VTK_DOUBLE_MAX;
  this->TriangleFace->GetPoints()->ShallowCopy(this->GetPoints());
  for (faceNum = 0; faceNum < 5; faceNum++)
  {
    // We have 8 nodes on rect face
    // and 6 on triangle faces
    if (faceNum > 0)
    {
      for (int i = 0; i < 6; i++)
      {
        this->TriangleFace->PointIds->SetId(i, this->PointIds->GetId(PyramidFaces[faceNum][i]));
      }
      inter = this->TriangleFace->IntersectWithLine(p1, p2, tol, tTemp, xTemp, pc, subId);
    }
    else
    {
      // No need to set the PointIds. They are set in the Face constructor
      // Points need to be set to correspond with the constructor's PointIds.
      for (int i = 0; i < 8; i++)
      {
        this->Face->Points->SetPoint(i, this->Points->GetPoint(PyramidFaces[faceNum][i]));
      }
      inter = this->Face->IntersectWithLine(p1, p2, tol, tTemp, xTemp, pc, subId);
    }
    if (inter)
    {
      intersection = 1;
      if (tTemp < t)
      {
        t = tTemp;
        x[0] = xTemp[0];
        x[1] = xTemp[1];
        x[2] = xTemp[2];
        switch (faceNum)
        {
          case 0:
            pcoords[0] = 0.0;
            pcoords[1] = pc[1];
            pcoords[2] = pc[0];
            break;

          case 1:
            pcoords[0] = 1.0;
            pcoords[1] = pc[0];
            pcoords[2] = pc[1];
            break;

          case 2:
            pcoords[0] = pc[0];
            pcoords[1] = 0.0;
            pcoords[2] = pc[1];
            break;

          case 3:
            pcoords[0] = pc[1];
            pcoords[1] = 1.0;
            pcoords[2] = pc[0];
            break;

          case 4:
            pcoords[0] = pc[1];
            pcoords[1] = pc[0];
            pcoords[2] = 0.0;
            break;

          case 5:
            pcoords[0] = pc[0];
            pcoords[1] = pc[1];
            pcoords[2] = 1.0;
            break;
        }
      }
    }
  }
  return intersection;
}

//------------------------------------------------------------------------------
int vtkQuadraticPyramid::Triangulate(int vtkNotUsed(index), vtkIdList* ptIds, vtkPoints* pts)
{
  // split into 14 tets
  pts->SetNumberOfPoints(14 * 4);
  ptIds->SetNumberOfIds(14 * 4);

  vtkIdType ids[14][4] = {
    { 7, 6, 2, 11 },
    { 1, 6, 5, 10 },
    { 8, 7, 3, 12 },
    { 5, 8, 0, 9 },
    { 7, 8, 5, 9 },
    { 6, 7, 5, 9 },
    { 6, 9, 5, 10 },
    { 9, 6, 7, 10 },
    { 10, 6, 7, 11 },
    { 9, 10, 7, 11 },
    { 10, 9, 4, 11 },
    { 9, 7, 8, 12 },
    { 9, 11, 7, 12 },
    { 11, 9, 4, 12 },
  };

  vtkIdType counter = 0;
  for (int i = 0; i < 14; i++)
  {
    for (int j = 0; j < 4; j++)
    {
      ptIds->SetId(counter, this->PointIds->GetId(ids[i][j]));
      pts->SetPoint(counter, this->Points->GetPoint(ids[i][j]));
      counter++;
    }
  }

  return 1;
}

//------------------------------------------------------------------------------
// Given parametric coordinates compute inverse Jacobian transformation
// matrix. Returns 9 elements of 3x3 inverse Jacobian plus interpolation
// function derivatives.
//
void vtkQuadraticPyramid::JacobianInverse(
  const double pcoords[3], double** inverse, double derivs[39])
{
  int i, j;
  double *m[3], m0[3], m1[3], m2[3];
  double x[3];

  // compute interpolation function derivatives
  vtkQuadraticPyramid::InterpolationDerivs(pcoords, derivs);

  // create Jacobian matrix
  m[0] = m0;
  m[1] = m1;
  m[2] = m2;
  for (i = 0; i < 3; i++) // initialize matrix
  {
    m0[i] = m1[i] = m2[i] = 0.0;
  }

  for (j = 0; j < 13; j++)
  {
    this->Points->GetPoint(j, x);
    for (i = 0; i < 3; i++)
    {
      m0[i] += x[i] * derivs[j];
      m1[i] += x[i] * derivs[13 + j];
      m2[i] += x[i] * derivs[26 + j];
    }
  }

  // now find the inverse
  if (vtkMath::InvertMatrix(m, inverse, 3) == 0)
  {
    vtkErrorMacro(<< "Jacobian inverse not found");
    return;
  }
}

//------------------------------------------------------------------------------
void vtkQuadraticPyramid::Derivatives(
  int vtkNotUsed(subId), const double pcoords[3], const double* values, int dim, double* derivs)
{
  double *jI[3], j0[3], j1[3], j2[3];
  double functionDerivs[3 * 13], sum[3];
  int i, j, k;

  // compute inverse Jacobian and interpolation function derivatives
  jI[0] = j0;
  jI[1] = j1;
  jI[2] = j2;
  this->JacobianInverse(pcoords, jI, functionDerivs);

  // now compute derivates of values provided
  for (k = 0; k < dim; k++) // loop over values per vertex
  {
    sum[0] = sum[1] = sum[2] = 0.0;
    for (i = 0; i < 13; i++) // loop over interp. function derivatives
    {
      sum[0] += functionDerivs[i] * values[dim * i + k];
      sum[1] += functionDerivs[13 + i] * values[dim * i + k];
      sum[2] += functionDerivs[26 + i] * values[dim * i + k];
    }
    for (j = 0; j < 3; j++) // loop over derivative directions
    {
      derivs[3 * k + j] = sum[0] * jI[j][0] + sum[1] * jI[j][1] + sum[2] * jI[j][2];
    }
  }
}

//------------------------------------------------------------------------------
// Clip this quadratic pyramid using scalar value provided. Like contouring,
// except that it cuts the pyramid to produce tetrahedra.
//
void vtkQuadraticPyramid::Clip(double value, vtkDataArray* cellScalars,
  vtkIncrementalPointLocator* locator, vtkCellArray* tets, vtkPointData* inPd, vtkPointData* outPd,
  vtkCellData* inCd, vtkIdType cellId, vtkCellData* outCd, int insideOut)
{
  int i;
  // create six linear pyramid + 4 tetra
  this->Subdivide(inPd, inCd, cellId, cellScalars);

  // contour each linear pyramid separately
  this->Scalars->SetNumberOfTuples(5); // num of vertices
  for (i = 0; i < 6; i++)              // for each subdivided pyramid
  {
    for (int j = 0; j < 5; j++) // for each of the five vertices of the pyramid
    {
      this->Pyramid->Points->SetPoint(j, this->Points->GetPoint(LinearPyramids[i][j]));
      this->Pyramid->PointIds->SetId(j, LinearPyramids[i][j]);
      this->Scalars->SetValue(j, this->CellScalars->GetValue(LinearPyramids[i][j]));
    }
    this->Pyramid->Clip(value, this->Scalars, locator, tets, this->PointData, outPd, this->CellData,
      i, outCd, insideOut);
  }

  this->Scalars->SetNumberOfTuples(4); // num of vertices
  for (i = 6; i < 10; i++)             // for each subdivided tetra
  {
    for (int j = 0; j < 4; j++) // for each of the four vertices of the tetra
    {
      this->Tetra->Points->SetPoint(j, this->Points->GetPoint(LinearPyramids[i][j]));
      this->Tetra->PointIds->SetId(j, LinearPyramids[i][j]);
      this->Scalars->SetValue(j, this->CellScalars->GetValue(LinearPyramids[i][j]));
    }
    this->Tetra->Clip(value, this->Scalars, locator, tets, this->PointData, outPd, this->CellData,
      i, outCd, insideOut);
  }
  this->ResizeArrays(13);
}

//------------------------------------------------------------------------------
// Compute interpolation functions for the fifteen nodes.
//
void vtkQuadraticPyramid::InterpolationFunctions(const double pcoords[3], double weights[13])
{
  // VTK needs parametric coordinates to be between (0,1). Isoparametric
  // shape functions are formulated between (-1,1). Here we do a
  // coordinate system conversion from (0,1) to (-1,1).
  const double r = 2.0 * (pcoords[0] - 0.5);
  const double s = 2.0 * (pcoords[1] - 0.5);
  const double t = 2.0 * (pcoords[2] - 0.5);
  const double r2 = r * r;
  const double s2 = s * s;
  const double t2 = t * t;

  weights[0] = -(1 - r) * (1 - s) * (1 - t) *
    (4 + 3 * r + 3 * s + 2 * r * s + 2 * t + r * t + s * t + 2 * r * s * t) / 16.0;
  weights[1] = -(1 + r) * (1 - s) * (1 - t) *
    (4 - 3 * r + 3 * s - 2 * r * s + 2 * t - r * t + s * t - 2 * r * s * t) / 16.0;
  weights[2] = -(1 + r) * (1 + s) * (1 - t) *
    (4 - 3 * r - 3 * s + 2 * r * s + 2 * t - r * t - s * t + 2 * r * s * t) / 16.0;
  weights[3] = -(1 - r) * (1 + s) * (1 - t) *
    (4 + 3 * r - 3 * s - 2 * r * s + 2 * t + r * t - s * t - 2 * r * s * t) / 16.0;

  weights[4] = t * (1 + t) / 2.0;

  weights[5] = (1 - r2) * (1 - s) * (1 - t) * (2 + s + s * t) / 8.0;
  weights[6] = (1 + r) * (1 - s2) * (1 - t) * (2 - r - r * t) / 8.0;
  weights[7] = (1 - r2) * (1 + s) * (1 - t) * (2 - s - s * t) / 8.0;
  weights[8] = (1 - r) * (1 - s2) * (1 - t) * (2 + r + r * t) / 8.0;

  weights[9] = (1 - r) * (1 - s) * (1 - t2) / 4.0;
  weights[10] = (1 + r) * (1 - s) * (1 - t2) / 4.0;
  weights[11] = (1 + r) * (1 + s) * (1 - t2) / 4.0;
  weights[12] = (1 - r) * (1 + s) * (1 - t2) / 4.0;
}

//------------------------------------------------------------------------------
// Derivatives in parametric space.
//
void vtkQuadraticPyramid::InterpolationDerivs(const double pcoords[3], double derivs[39])
{
  // VTK needs parametric coordinates to be between (0,1). Isoparametric
  // shape functions are formulated between (-1,1). Here we do a
  // coordinate system conversion from (0,1) to (-1,1).
  double r = 2 * pcoords[0] - 1;
  double s = 2 * pcoords[1] - 1;
  double t = 2 * pcoords[2] - 1;

  double rm = 1.0 - r;
  double rp = 1.0 + r;
  double sm = 1.0 - s;
  double sp = 1.0 + s;
  double tm = 1.0 - t;
  // double tp = 1.0 + t;
  double r2 = 1.0 - r * r;
  double t2 = 1.0 - t * t;

  // r-derivatives
  // corners
  derivs[0] = -0.125 * (sm * tm - 2.0 * r * sm * tm - s * sm * tm - t * sm * tm - 2.0 * sm * tm);
  derivs[1] = 0.125 * (sm * tm + 2.0 * r * sm * tm - s * sm * tm - t * sm * tm - 2.0 * sm * tm);
  derivs[2] = 0.125 * (sp * tm + 2.0 * r * sp * tm + s * sp * tm - t * sp * tm - 2.0 * sp * tm);
  derivs[3] = -0.125 * (sp * tm - 2.0 * r * sp * tm + s * sp * tm - t * sp * tm - 2.0 * sp * tm);
  derivs[4] = 0.0;

  // midsides of rectangles
  derivs[5] = -0.5 * r * sm * tm;
  derivs[6] = 0.25 * (tm - s * s * tm);
  derivs[7] = -0.5 * r * sp * tm;
  derivs[8] = -0.25 * (tm - s * s * tm);

  // midsides of triangles
  derivs[9] = -0.25 * (1 - s) * (1 - t * t);
  derivs[10] = 0.25 * (1 - s) * (1 - t * t);
  derivs[11] = 0.25 * (1 + s) * (1 - t * t);
  derivs[12] = -0.25 * (1 + s) * (1 - t * t);

  // s-derivatives
  // corners
  derivs[13] = -0.125 * (rm * tm - 2.0 * s * rm * tm - r * rm * tm - t * rm * tm - 2.0 * rm * tm);
  derivs[14] = -0.125 * (rp * tm - 2.0 * s * rp * tm + r * rp * tm - t * rp * tm - 2.0 * rp * tm);
  derivs[15] = 0.125 * (rp * tm + 2.0 * s * rp * tm + r * rp * tm - t * rp * tm - 2.0 * rp * tm);
  derivs[16] = 0.125 * (rm * tm + 2.0 * s * rm * tm - r * rm * tm - t * rm * tm - 2.0 * rm * tm);
  derivs[17] = 0.0;

  // midsides of rectangles
  derivs[18] = -0.25 * tm * r2;
  derivs[19] = -0.5 * tm * s * rp;
  derivs[20] = 0.25 * tm * r2;
  derivs[21] = -0.5 * tm * s * rm;

  // midsides of triangles
  derivs[22] = -0.25 * rm * t2;
  derivs[23] = -0.25 * rp * t2;
  derivs[24] = 0.25 * rp * t2;
  derivs[25] = 0.25 * rm * t2;

  // t-derivatives
  // corners
  derivs[26] = -0.125 * (rm * sm - 2.0 * t * rm * sm - r * rm * sm - s * rm * sm - 2.0 * rm * sm);
  derivs[27] = -0.125 * (rp * sm - 2.0 * t * rp * sm + r * rp * sm - s * rp * sm - 2.0 * rp * sm);
  derivs[28] = -0.125 * (rp * sp - 2.0 * t * rp * sp + r * rp * sp + s * rp * sp - 2.0 * rp * sp);
  derivs[29] = -0.125 * (rm * sp - 2.0 * t * rm * sp - r * rm * sp + s * rm * sp - 2.0 * rm * sp);
  derivs[30] = 0.5 + t;

  // midsides of rectangles
  derivs[31] = -0.25 * (sm - r * r * sm);
  derivs[32] = -0.25 * (rp - s * s * rp);
  derivs[33] = -0.25 * (sp - r * r * sp);
  derivs[34] = -0.25 * (rm - s * s * rm);

  // midsides of triangles
  derivs[35] = -0.5 * (1 - r) * (1 - s) * t;
  derivs[36] = -0.5 * (1 + r) * (1 - s) * t;
  derivs[37] = -0.5 * (1 + r) * (1 + s) * t;
  derivs[38] = -0.5 * (1 - r) * (1 + s) * t;

  // we compute derivatives in in [-1; 1] but we need them in [ 0; 1]
  for (int i = 0; i < 39; i++)
    derivs[i] *= 2;
}

static double vtkQPyramidCellPCoords[39] = {
  0.0, 0.0, 0.0, //
  1.0, 0.0, 0.0, //
  1.0, 1.0, 0.0, //
  0.0, 1.0, 0.0, //
  0.0, 0.0, 1.0, //
  0.5, 0.0, 0.0, //
  1.0, 0.5, 0.0, //
  0.5, 1.0, 0.0, //
  0.0, 0.5, 0.0, //
  0.0, 0.0, 0.5, //
  1.0, 0.0, 0.5, //
  1.0, 1.0, 0.5, //
  0.0, 1.0, 0.5  //
};

//------------------------------------------------------------------------------
double* vtkQuadraticPyramid::GetParametricCoords()
{
  return vtkQPyramidCellPCoords;
}

//------------------------------------------------------------------------------
void vtkQuadraticPyramid::PrintSelf(ostream& os, vtkIndent indent)
{
  this->Superclass::PrintSelf(os, indent);

  os << indent << "Edge:\n";
  this->Edge->PrintSelf(os, indent.GetNextIndent());
  os << indent << "TriangleFace:\n";
  this->TriangleFace->PrintSelf(os, indent.GetNextIndent());
  os << indent << "Face:\n";
  this->Face->PrintSelf(os, indent.GetNextIndent());
  os << indent << "Tetra:\n";
  this->Tetra->PrintSelf(os, indent.GetNextIndent());
  os << indent << "Pyramid:\n";
  this->Pyramid->PrintSelf(os, indent.GetNextIndent());
  os << indent << "PointData:\n";
  this->PointData->PrintSelf(os, indent.GetNextIndent());
  os << indent << "CellData:\n";
  this->CellData->PrintSelf(os, indent.GetNextIndent());
  os << indent << "Scalars:\n";
  this->Scalars->PrintSelf(os, indent.GetNextIndent());
}