#include <math.h>
#include "GR_Geometry.h"
#include "GR_Cell.h"
#include "GR_CellCV.h"
#include "GR_misc.h"
#include "GR_Face.h"
#include "GR_Vertex.h"
#include "GR_Vec.h"

const Vert* TetCell::pVVert(const int i) const
{
  assert(iFullCheck());
  assert2(i >= 0 && i <= 3, "Vertex index out of range");
  const Face* const pFBase = ppFFaces[0];
  if (i == 0) return (pFBase->pVVert(0));
  if (i == 3) return (pVVertOpposite(pFBase));
  // For verts 1 and 2, return value depends on face orientation.
  if (pFBase->pCCellRight() == static_cast<const Cell*>(this))
    return pFBase->pVVert(i);
  else
    return pFBase->pVVert(3-i);
}

void TetCell::vAllVertHandles(GRUMMP_Entity* aHandles[]) const
{
  assert(iFullCheck());
  const Face* const pFBase = ppFFaces[0];
  pFBase->vAllVertHandles(aHandles);
  // For verts 1 and 2, return value depends on face orientation, so
  // switch if needed.
  if (pFBase->pCCellRight() != static_cast<const Cell*>(this)) {
    GRUMMP_Entity* pvTmp = aHandles[1];
    aHandles[1] = aHandles[2];
    aHandles[2] = pvTmp;
  }
  aHandles[3] = pVVertOpposite(pFBase);
}

double TetCell::dSize() const
{
  assert(iFullCheck());
  const Vert *pV0, *pV1, *pV2, *pV3;
  const Face* const pFBase = ppFFaces[0];
  pV0 = pFBase->pVVert(0);
  if (pFBase->pCCellRight() == static_cast<const Cell*>(this)) {
    pV1 = pFBase->pVVert(1);
    pV2 = pFBase->pVVert(2);
  }
  else {
    pV1 = pFBase->pVVert(2);
    pV2 = pFBase->pVVert(1);
  }
  pV3 = pVVertOpposite(pFBase);

  double adVec1[] = { pV1->dX() - pV0->dX(),
		      pV1->dY() - pV0->dY(),
		      pV1->dZ() - pV0->dZ() };
  double adVec2[] = { pV2->dX() - pV0->dX(),
		      pV2->dY() - pV0->dY(),
		      pV2->dZ() - pV0->dZ() };
  double adVec3[] = { pV3->dX() - pV0->dX(),
		      pV3->dY() - pV0->dY(),
		      pV3->dZ() - pV0->dZ() };
  double adCross[3];
  vCROSS3D(adVec2, adVec3, adCross);
  return dDOT3D(adCross, adVec1) / 6.;
}

bool TetCell::qIsClosed() const
{
  // Check to be sure that the tet has only four vertices and that each
  // appears in exactly three faces.
  const Vert *apV[] = {pVInvalidVert, pVInvalidVert,
		       pVInvalidVert, pVInvalidVert};
  // If the cell is deleted, it doesn't matter whether it's closed or
  // not. 
  if (qDeleted()) return true;
  int iFaceCount[4];
  for (int iF = 0; iF < 4; iF++) {
    const Face *pF = pFFace(iF);
    assert(pF->qValid() && !pF->qDeleted());
    for (int iV = 0; iV < 3; iV++) {
      const Vert *pV = pF->pVVert(iV);
      assert(pV->qValid() && !pV->qDeleted());
      int ii;
      for (ii = 0; ii < 4; ii++) {
	if (apV[ii] == pVInvalidVert) {
	  apV[ii] = pV;
	  iFaceCount[ii] = 1;
	  break;
	}
	else if (apV[ii] == pV) {
	  iFaceCount[ii]++;
	  break;
	}
      }
      if (ii == 4)
	return (false);
    } // Done with this vertex
  } // Done with this face
  if (iFaceCount[0] != 3 || iFaceCount[1] != 3 ||
      iFaceCount[2] != 3 || iFaceCount[3] != 3)
    return (false);
  return (true);
}

void TetCell::vAllDihed(double adDihed[], int* const piNDihed) const
{
  *piNDihed = 6;
  const double * const adA = pVVert(0)->adCoords();
  const double * const adB = pVVert(1)->adCoords();
  const double * const adC = pVVert(2)->adCoords();
  const double * const adD = pVVert(3)->adCoords();

  double adNorm0[3], adNorm1[3], adNorm2[3], adNorm3[3];
  vUnitNormal(adC, adB, adD, adNorm0);
  vUnitNormal(adA, adC, adD, adNorm1);
  vUnitNormal(adB, adA, adD, adNorm2);
  vUnitNormal(adA, adB, adC, adNorm3);

  // The order in which the dihedrals are assigned matters for
  // computation of solid angles.  The way they're currently set up,
  // combining them as (0,1,2), (0,3,4), (1,3,5), (2,4,5) gives (in
  // order) solid angles at vertices A, B, C, D
  
  static const double dScale = 180/M_PI;
  double dDot = -dDOT3D(adNorm0, adNorm1);
  adDihed[5] = GR_acos(dDot) * dScale; // Edge CD

  dDot = -dDOT3D(adNorm0, adNorm2);
  adDihed[4] = GR_acos(dDot) * dScale; // Edge BD

  dDot = -dDOT3D(adNorm0, adNorm3);
  adDihed[3] = GR_acos(dDot) * dScale; // Edge BC

  dDot = -dDOT3D(adNorm1, adNorm2);
  adDihed[2] = GR_acos(dDot) * dScale; // Edge AD

  dDot = -dDOT3D(adNorm1, adNorm3);
  adDihed[1] = GR_acos(dDot) * dScale; // Edge AC

  dDot = -dDOT3D(adNorm2, adNorm3);
  adDihed[0] = GR_acos(dDot) * dScale; // Edge AB
}

void TetCell::vAllSolid(double adSolid[], int* const piNSolid) const
{
  *piNSolid = 4;
  double adDihed[6];
  int iNDihed;
  vAllDihed(adDihed, &iNDihed);

  adSolid[0] = adDihed[0] + adDihed[1] + adDihed[2] - 180.;
  adSolid[1] = adDihed[0] + adDihed[3] + adDihed[4] - 180.;
  adSolid[2] = adDihed[1] + adDihed[3] + adDihed[5] - 180.;
  adSolid[3] = adDihed[2] + adDihed[4] + adDihed[5] - 180.;
}
  
void TetCell::vCircumcenter(double adCircCent[]) const
{
  assert(qValid());
  double adB[3] = {pVVert(1)->dX() - pVVert(0)->dX(),
		   pVVert(1)->dY() - pVVert(0)->dY(),
		   pVVert(1)->dZ() - pVVert(0)->dZ()};
  double adC[3] = {pVVert(2)->dX() - pVVert(0)->dX(),
		   pVVert(2)->dY() - pVVert(0)->dY(),
		   pVVert(2)->dZ() - pVVert(0)->dZ()};
  double adD[3] = {pVVert(3)->dX() - pVVert(0)->dX(),
		   pVVert(3)->dY() - pVVert(0)->dY(),
		   pVVert(3)->dZ() - pVVert(0)->dZ()};


  double *adRow1 = adB;
  double *adRow2 = adC;
  double *adRow3 = adD;

  double dRHS1 = 0.5 * dDOT3D(adB, adB);
  double dRHS2 = 0.5 * dDOT3D(adC, adC);
  double dRHS3 = 0.5 * dDOT3D(adD, adD);

  vSolve3By3(adRow1, adRow2, adRow3, dRHS1, dRHS2, dRHS3, adCircCent);
#ifndef NDEBUG
  double adA[] = {0, 0, 0};
  double dComp = dDIST3D(adCircCent, adA);
  double dTest = dDIST3D(adCircCent, adB);
  assert(0 == iFuzzyComp(dComp, dTest));
  dTest = dDIST3D(adCircCent, adC);
  assert(0 == iFuzzyComp(dComp, dTest));
  dTest = dDIST3D(adCircCent, adD);
  assert(0 == iFuzzyComp(dComp, dTest));
#endif
  // The following assertion fails for flat tets.
  // assert(qCircumscribes(adRes));
  adCircCent[0] += pVVert(0)->dX();
  adCircCent[1] += pVVert(0)->dY();
  adCircCent[2] += pVVert(0)->dZ();
}

double TetCell::dCircumradius() const
{
  assert(qValid());
  if (iOrient3D(pVVert(0), pVVert(1), pVVert(2), pVVert(3)) == 0) {
    // Tet is totally flat.
    return LARGE_DBL;
  }
  else {
    double adTemp[3];
    vCircumcenter(adTemp);
    adTemp[0] -= pVVert(0)->dX();
    adTemp[1] -= pVVert(0)->dY();
    adTemp[2] -= pVVert(0)->dZ();
    return (dMAG3D(adTemp));
  }
}

bool TetCell::qCircumscribes(const double adPoint[]) const
{
  // Returns true if adPoint lies in or on the circumsphere of *this
  return (-1 != iInsphere(pVVert(0)->adCoords(), pVVert(1)->adCoords(),
			  pVVert(2)->adCoords(), pVVert(3)->adCoords(),
			  adPoint));
}

void TetCell::vContaincenter(double adResult[]) const
{
  assert(qValid());
  vCircumcenter(adResult);
  double adBary[4], REps = 1.e-10;
  int iTrips = 0;
  bool qReTry;
  do {
    qReTry = false;
    iTrips ++;
    assert(iTrips <= 2);
    vBarycentrics(adResult, adBary);
    int iLowBary = ((adBary[0] < REps ? 1 : 0) +
		    (adBary[1] < REps ? 2 : 0) +
		    (adBary[2] < REps ? 4 : 0) +
		    (adBary[3] < REps ? 8 : 0));
    if (iLowBary == 0) return;
  
    switch (iLowBary) {
    case 1: // Need containment center of face opposite vert 0
      {
	if (fabs(adBary[0]) < REps) break;
	// Project onto the plane of that face.
	const Face* pF = pFFaceOpposite(pVVert(0));
	pF->vProjOntoFace(adResult);
	// Re-cycle to make sure that the point is in or on the tet
	qReTry = true;
      }
      break;
    case 2: // Need containment center of face opposite vert 1
      {
	if (fabs(adBary[1]) < REps) break;
	// Project onto the plane of that face.
	const Face* pF = pFFaceOpposite(pVVert(1));
	pF->vProjOntoFace(adResult);
	// Re-cycle to make sure that the point is in or on the tet
	qReTry = true;
      }
      break;
    case 4: // Need containment center of face opposite vert 2
      {
	if (fabs(adBary[2]) < REps) break;
	// Project onto the plane of that face.
	const Face* pF = pFFaceOpposite(pVVert(2));
	pF->vProjOntoFace(adResult);
	// Re-cycle to make sure that the point is in or on the tet
	qReTry = true;
      }
      break;
    case 8: // Need containment center of face opposite vert 3
      {
	if (fabs(adBary[3]) < REps) break;
	// Project onto the plane of that face.
	const Face* pF = pFFaceOpposite(pVVert(3));
	pF->vProjOntoFace(adResult);
	// Re-cycle to make sure that the point is in or on the tet
	qReTry = true;
      }
      break;
    case 3: // Need midside of edge between verts 2 and 3
      adResult[0] = (pVVert(2)->dX() + pVVert(3)->dX()) * 0.5;
      adResult[1] = (pVVert(2)->dY() + pVVert(3)->dY()) * 0.5;
      adResult[2] = (pVVert(2)->dZ() + pVVert(3)->dZ()) * 0.5;
      break;
    case 5: // Need midside of edge between verts 1 and 3
      adResult[0] = (pVVert(1)->dX() + pVVert(3)->dX()) * 0.5;
      adResult[1] = (pVVert(1)->dY() + pVVert(3)->dY()) * 0.5;
      adResult[2] = (pVVert(1)->dZ() + pVVert(3)->dZ()) * 0.5;
      break;
    case 9: // Need midside of edge between verts 1 and 2
      adResult[0] = (pVVert(1)->dX() + pVVert(2)->dX()) * 0.5;
      adResult[1] = (pVVert(1)->dY() + pVVert(2)->dY()) * 0.5;
      adResult[2] = (pVVert(1)->dZ() + pVVert(2)->dZ()) * 0.5;
      break;
    case 6: // Need midside of edge between verts 0 and 3
      adResult[0] = (pVVert(0)->dX() + pVVert(3)->dX()) * 0.5;
      adResult[1] = (pVVert(0)->dY() + pVVert(3)->dY()) * 0.5;
      adResult[2] = (pVVert(0)->dZ() + pVVert(3)->dZ()) * 0.5;
      break;
    case 10: // Need midside of edge between verts 0 and 2
      adResult[0] = (pVVert(0)->dX() + pVVert(2)->dX()) * 0.5;
      adResult[1] = (pVVert(0)->dY() + pVVert(2)->dY()) * 0.5;
      adResult[2] = (pVVert(0)->dZ() + pVVert(2)->dZ()) * 0.5;
      break;
    case 12: // Need midside of edge between verts 0 and 1
      adResult[0] = (pVVert(0)->dX() + pVVert(1)->dX()) * 0.5;
      adResult[1] = (pVVert(0)->dY() + pVVert(1)->dY()) * 0.5;
      adResult[2] = (pVVert(0)->dZ() + pVVert(1)->dZ()) * 0.5;
      break;
    default:
      assert2(0, "Unexpected difficulty in finding containment center");
      break;
    }
  } while (qReTry);
}

double TetCell::dContainradius() const
{
  double adTemp[3];
  vContaincenter(adTemp);
  adTemp[0] -= pVVert(0)->dX();
  adTemp[1] -= pVVert(0)->dY();
  adTemp[2] -= pVVert(0)->dZ();
  return (dMAG3D(adTemp));
}

void TetCell::vBarycentrics(const double adPoint[], double adBary[]) const
{
  assert(qValid());

  const Vert* pVA = pVVert(0);
  const Vert* pVB = pVVert(1);
  const Vert* pVC = pVVert(2);
  const Vert* pVD = pVVert(3);
  
  // The old version of this code (commented out below) has a robustness
  // problem with small tets far from the origin.

  // Solve:
  // / 1  1  1  1  \ / b0 \   / 1 \ .
  // | xa xb xc xd | | b1 |   | x |
  // | ya yb yc yd | | b2 | = | y |
  // \ za zb zc zd / \ b3 /   \ z /

//   int i;
//   double a2dLHS[4][4], adRHS[4], a2dMat[4][4];
//   double dDet, dDet0, dDet1, dDet2, dDet3;

//   a2dLHS[0][0] = 1.;
//   a2dLHS[0][1] = pVA->dX();
//   a2dLHS[0][2] = pVA->dY();
//   a2dLHS[0][3] = pVA->dZ();
			       
//   a2dLHS[1][0] = 1.;	       
//   a2dLHS[1][1] = pVB->dX();
//   a2dLHS[1][2] = pVB->dY();
//   a2dLHS[1][3] = pVB->dZ();
			       
//   a2dLHS[2][0] = 1.;	       
//   a2dLHS[2][1] = pVC->dX();
//   a2dLHS[2][2] = pVC->dY();
//   a2dLHS[2][3] = pVC->dZ();
			       
//   a2dLHS[3][0] = 1.;	       
//   a2dLHS[3][1] = pVD->dX();
//   a2dLHS[3][2] = pVD->dY();
//   a2dLHS[3][3] = pVD->dZ();

//   adRHS[0] = 1.;
//   adRHS[1] = adPoint[0];
//   adRHS[2] = adPoint[1];
//   adRHS[3] = adPoint[2];

//   dDet = dDet4By4(a2dLHS);

//   for (i = 0; i <= 3; ++i) {
//     a2dMat[0][i] = adRHS    [i];
//     a2dMat[1][i] = a2dLHS[1][i];
//     a2dMat[2][i] = a2dLHS[2][i];
//     a2dMat[3][i] = a2dLHS[3][i];
//   }
//   dDet0 = dDet4By4(a2dMat);

//   for (i = 0; i <= 3; ++i) {
//     a2dMat[0][i] = a2dLHS[0][i];
//     a2dMat[1][i] = adRHS    [i];
//     a2dMat[2][i] = a2dLHS[2][i];
//     a2dMat[3][i] = a2dLHS[3][i];
//   }
//   dDet1 = dDet4By4(a2dMat);

//   for (i = 0; i <= 3; ++i) {
//     a2dMat[0][i] = a2dLHS[0][i];
//     a2dMat[1][i] = a2dLHS[1][i];
//     a2dMat[2][i] = adRHS    [i];
//     a2dMat[3][i] = a2dLHS[3][i];
//   }
//   dDet2 = dDet4By4(a2dMat);

//   for (i = 0; i <= 3; ++i) {
//     a2dMat[0][i] = a2dLHS[0][i];
//     a2dMat[1][i] = a2dLHS[1][i];
//     a2dMat[2][i] = a2dLHS[2][i];
//     a2dMat[3][i] = adRHS    [i];
//   }
//   dDet3 = dDet4By4(a2dMat);

//   assert(fabs(dDet) > 1.e-12);
//   adBary[0] = dDet0 / dDet;
//   adBary[1] = dDet1 / dDet;
//   adBary[2] = dDet2 / dDet;
//   adBary[3] = dDet3 / dDet;

  // New version:

  // Solve:
  // / 1    1     1     1   \ / b0 \   /   1  \ .
  // | 0  xb-xa xc-xa xd-xa | | b1 |   | x-xa |
  // | 0  yb-ya yc-ya yd-ya | | b2 | = | y-ya |
  // \ 0  zb-za zc-za zd-za / \ b3 /   \ z-za /
  //
  // by solving the 3x3 for b1, b2, b3 and subtracting to get b0.
  // The solution for b1, b2, b3 has been immunized against some forms
  // of insignificance by this linear combination.  Each row can be
  // normalized to make it so that small determinants for the 3x3 are
  // genuinely bad news.

  double a2dLHS[3][3];
  
  a2dLHS[0][0] = pVB->dX() - pVA->dX();
  a2dLHS[0][1] = pVB->dY() - pVA->dY();
  a2dLHS[0][2] = pVB->dZ() - pVA->dZ();
  
  a2dLHS[1][0] = pVC->dX() - pVA->dX();
  a2dLHS[1][1] = pVC->dY() - pVA->dY();
  a2dLHS[1][2] = pVC->dZ() - pVA->dZ();
  
  a2dLHS[2][0] = pVD->dX() - pVA->dX();
  a2dLHS[2][1] = pVD->dY() - pVA->dY();
  a2dLHS[2][2] = pVD->dZ() - pVA->dZ();
  
  double adRHS[] = {adPoint[0] - pVA->dX(),
		    adPoint[1] - pVA->dY(),
		    adPoint[2] - pVA->dZ()};
  
  double adNorm[] = {a2dLHS[0][0]*a2dLHS[0][0] +
		     a2dLHS[1][0]*a2dLHS[1][0] +
		     a2dLHS[2][0]*a2dLHS[2][0],
		     a2dLHS[0][1]*a2dLHS[0][1] +
		     a2dLHS[1][1]*a2dLHS[1][1] +
		     a2dLHS[2][1]*a2dLHS[2][1],
		     a2dLHS[0][2]*a2dLHS[0][2] +
		     a2dLHS[1][2]*a2dLHS[1][2] +
		     a2dLHS[2][2]*a2dLHS[2][2]};
  adNorm[0] = 1./sqrt(adNorm[0]);
  adNorm[1] = 1./sqrt(adNorm[1]);
  adNorm[2] = 1./sqrt(adNorm[2]);
  
  a2dLHS[0][0] *= adNorm[0];
  a2dLHS[1][0] *= adNorm[0];
  a2dLHS[2][0] *= adNorm[0];
  adRHS[0] *= adNorm[0];
  
  a2dLHS[0][1] *= adNorm[1];
  a2dLHS[1][1] *= adNorm[1];
  a2dLHS[2][1] *= adNorm[1];
  adRHS[1] *= adNorm[1];
  
  a2dLHS[0][2] *= adNorm[2];
  a2dLHS[1][2] *= adNorm[2];
  a2dLHS[2][2] *= adNorm[2];
  adRHS[2] *= adNorm[2];
  
  double dDet =
    a2dLHS[0][0]*(a2dLHS[1][1]*a2dLHS[2][2] - a2dLHS[1][2]*a2dLHS[2][1]) +
    a2dLHS[1][0]*(a2dLHS[2][1]*a2dLHS[0][2] - a2dLHS[0][1]*a2dLHS[2][2]) +
    a2dLHS[2][0]*(a2dLHS[0][1]*a2dLHS[1][2] - a2dLHS[0][2]*a2dLHS[1][1]);
    
  assert(fabs(dDet) > 1.e-12);

  double dDet1 = 
    adRHS    [0]*(a2dLHS[1][1]*a2dLHS[2][2] - a2dLHS[1][2]*a2dLHS[2][1]) +
    a2dLHS[1][0]*(a2dLHS[2][1]*adRHS    [2] - adRHS    [1]*a2dLHS[2][2]) +
    a2dLHS[2][0]*(adRHS    [1]*a2dLHS[1][2] - adRHS    [2]*a2dLHS[1][1]);

  double dDet2 =
    a2dLHS[0][0]*(adRHS    [1]*a2dLHS[2][2] - adRHS    [2]*a2dLHS[2][1]) +
    adRHS    [0]*(a2dLHS[2][1]*a2dLHS[0][2] - a2dLHS[0][1]*a2dLHS[2][2]) +
    a2dLHS[2][0]*(a2dLHS[0][1]*adRHS    [2] - a2dLHS[0][2]*adRHS    [1]);
    
  double dDet3 =
    a2dLHS[0][0]*(a2dLHS[1][1]*adRHS    [2] - a2dLHS[1][2]*adRHS    [1]) +
    a2dLHS[1][0]*(adRHS    [1]*a2dLHS[0][2] - a2dLHS[0][1]*adRHS    [2]) +
    adRHS    [0]*(a2dLHS[0][1]*a2dLHS[1][2] - a2dLHS[0][2]*a2dLHS[1][1]);
    
  adBary[3] = dDet3 / dDet;    
  adBary[2] = dDet2 / dDet;    
  adBary[1] = dDet1 / dDet;
  adBary[0] = 1. - adBary[1] - adBary[2] - adBary[3];
}

double TetCell::dShortestEdgeLength() const
{
  register double dDX, dDY, dDZ;
  register const Vert *pV0 = pVVert(0);
  register const Vert *pV1 = pVVert(1);
  register const Vert *pV2 = pVVert(2);
  register const Vert *pV3 = pVVert(3);

  dDX = pV0->dX() - pV1->dX();
  dDY = pV0->dY() - pV1->dY();
  dDZ = pV0->dZ() - pV1->dZ();
  double dLen01_Sq = dDX*dDX + dDY*dDY + dDZ*dDZ;

  dDX = pV0->dX() - pV2->dX();
  dDY = pV0->dY() - pV2->dY();
  dDZ = pV0->dZ() - pV2->dZ();
  double dLen02_Sq = dDX*dDX + dDY*dDY + dDZ*dDZ;
  
  dDX = pV0->dX() - pV3->dX();
  dDY = pV0->dY() - pV3->dY();
  dDZ = pV0->dZ() - pV3->dZ();
  double dLen03_Sq = dDX*dDX + dDY*dDY + dDZ*dDZ;

  dDX = pV1->dX() - pV2->dX();
  dDY = pV1->dY() - pV2->dY();
  dDZ = pV1->dZ() - pV2->dZ();
  double dLen12_Sq = dDX*dDX + dDY*dDY + dDZ*dDZ;

  dDX = pV1->dX() - pV3->dX();
  dDY = pV1->dY() - pV3->dY();
  dDZ = pV1->dZ() - pV3->dZ();
  double dLen13_Sq = dDX*dDX + dDY*dDY + dDZ*dDZ;

  dDX = pV2->dX() - pV3->dX();
  dDY = pV2->dY() - pV3->dY();
  dDZ = pV2->dZ() - pV3->dZ();
  double dLen23_Sq = dDX*dDX + dDY*dDY + dDZ*dDZ;

  double dShortest_Sq = min(min(min(dLen01_Sq, dLen02_Sq),
				min(dLen03_Sq, dLen12_Sq)),
			    min(dLen13_Sq, dLen23_Sq));
  return (sqrt(dShortest_Sq));
}

void TetCellCV::vCircumcenter(double adRes[]) const
{
  assert(qValid());
  const double * const adA = pVVert(0)->adCoords();
  const double * const adB = pVVert(1)->adCoords();
  const double * const adC = pVVert(2)->adCoords();
  const double * const adD = pVVert(3)->adCoords();

  double adRow1[] = {adA[0] - adB[0],
		     adA[1] - adB[1],
		     adA[2] - adB[2]};
  double adRow2[] = {adA[0] - adC[0],
		     adA[1] - adC[1],
		     adA[2] - adC[2]};
  double adRow3[] = {adA[0] - adD[0],
		     adA[1] - adD[1],
		     adA[2] - adD[2]};

  double dRHS1 = 0.5 * (dDOT3D(adA, adA) - dDOT3D(adB, adB));
  double dRHS2 = 0.5 * (dDOT3D(adA, adA) - dDOT3D(adC, adC));
  double dRHS3 = 0.5 * (dDOT3D(adA, adA) - dDOT3D(adD, adD));


  vSolve3By3(adRow1, adRow2, adRow3, dRHS1, dRHS2, dRHS3, adRes);
#ifndef NDEBUG
  double dComp = dDIST3D(adRes, adA);
  double dTest = dDIST3D(adRes, adB);
  assert(0 == iFuzzyComp(dComp, dTest));
  dTest = dDIST3D(adRes, adC);
  assert(0 == iFuzzyComp(dComp, dTest));
  dTest = dDIST3D(adRes, adD);
  assert(0 == iFuzzyComp(dComp, dTest));
#endif
  // The following assertion fails for flat tets.
  // assert(qCircumscribes(adRes));
}

void TetCellCV::vAllDihed(double adDihed[], int* const piNDihed) const
{
  *piNDihed = 6;
  const double * const adA = pVVert(0)->adCoords();
  const double * const adB = pVVert(1)->adCoords();
  const double * const adC = pVVert(2)->adCoords();
  const double * const adD = pVVert(3)->adCoords();

  double adNorm0[3], adNorm1[3], adNorm2[3], adNorm3[3];
  vUnitNormal(adC, adB, adD, adNorm0);
  vUnitNormal(adA, adC, adD, adNorm1);
  vUnitNormal(adB, adA, adD, adNorm2);
  vUnitNormal(adA, adB, adC, adNorm3);

  // The order in which the dihedrals are assigned matters for
  // computation of solid angles.  The way they're currently set up,
  // combining them as (0,1,2), (0,3,4), (1,3,5), (2,4,5) gives (in
  // order) solid angles at vertices A, B, C, D
  
  static const double dScale = 180/M_PI;
  double dDot = -dDOT3D(adNorm0, adNorm1);
  adDihed[5] = GR_acos(dDot) * dScale; // Edge CD

  dDot = -dDOT3D(adNorm0, adNorm2);
  adDihed[4] = GR_acos(dDot) * dScale; // Edge BD

  dDot = -dDOT3D(adNorm0, adNorm3);
  adDihed[3] = GR_acos(dDot) * dScale; // Edge BC

  dDot = -dDOT3D(adNorm1, adNorm2);
  adDihed[2] = GR_acos(dDot) * dScale; // Edge AD

  dDot = -dDOT3D(adNorm1, adNorm3);
  adDihed[1] = GR_acos(dDot) * dScale; // Edge AC

  dDot = -dDOT3D(adNorm2, adNorm3);
  adDihed[0] = GR_acos(dDot) * dScale; // Edge AB
}

void TetCellCV::vAllSolid(double adSolid[], int* const piNSolid) const
{
  *piNSolid = 4;
  double adDihed[6];
  int iNDihed;
  vAllDihed(adDihed, &iNDihed);

  adSolid[0] = adDihed[0] + adDihed[1] + adDihed[2] - 180.;
  adSolid[1] = adDihed[0] + adDihed[3] + adDihed[4] - 180.;
  adSolid[2] = adDihed[1] + adDihed[3] + adDihed[5] - 180.;
  adSolid[3] = adDihed[2] + adDihed[4] + adDihed[5] - 180.;
}
  
double TetCellCV::dSize() const
{
  assert(iFullCheck());
  const Vert *pV0 = pVVert(0);
  const Vert *pV1 = pVVert(1);
  const Vert *pV2 = pVVert(2);
  const Vert *pV3 = pVVert(3);

  double adVec1[] = { pV1->dX() - pV0->dX(),
		      pV1->dY() - pV0->dY(),
		      pV1->dZ() - pV0->dZ() };
  double adVec2[] = { pV2->dX() - pV0->dX(),
		      pV2->dY() - pV0->dY(),
		      pV2->dZ() - pV0->dZ() };
  double adVec3[] = { pV3->dX() - pV0->dX(),
		      pV3->dY() - pV0->dY(),
		      pV3->dZ() - pV0->dZ() };
  double adCross[3];
  vCROSS3D(adVec2, adVec3, adCross);
  return dDOT3D(adCross, adVec1) / 6.;
}

