#include "GR_VertTree.h"
#include "GR_Vertex.h"

int VertTree::iWhichOctant(const double adTest[], const double adMean[]) const
{
  if (iDim == 2) {
    return ( (adTest[XDIR] >= adMean[XDIR] ? eEast : eWest) +
	     (adTest[YDIR] >= adMean[YDIR] ? eNorth : eSouth) ) ;
  }
  else {
    return ( (adTest[XDIR] >= adMean[XDIR] ? eEast : eWest) +
	     (adTest[YDIR] >= adMean[YDIR] ? eNorth : eSouth) +
	     (adTest[ZDIR] >= adMean[ZDIR] ? eUpper : eLower) ) ;
  }
}

void VertTree::vChildOctantBounds(const int iOct,
				  double adNewMin[], double adNewMax[]) const
  // For a given child of an octant, find the spatial bounds of that child
{
  if (iDim == 2) {
    double adMean[3];
    adMean[XDIR] = 0.5 * (adMin[XDIR] + adMax[XDIR]);
    adMean[YDIR] = 0.5 * (adMin[YDIR] + adMax[YDIR]);
    switch (iOct) {
    case eNorthEast:
      adNewMin[XDIR] = adMean[XDIR];       adNewMax[XDIR] = adMax[XDIR];
      adNewMin[YDIR] = adMean[YDIR];       adNewMax[YDIR] = adMax[YDIR];
      break;
    case eSouthEast:
      adNewMin[XDIR] = adMean[XDIR];       adNewMax[XDIR] = adMax[XDIR];
      adNewMin[YDIR] = adMin[YDIR];        adNewMax[YDIR] = adMean[YDIR];
    break;
    case eNorthWest:
      adNewMin[XDIR] = adMin[XDIR];        adNewMax[XDIR] = adMean[XDIR];
      adNewMin[YDIR] = adMean[YDIR];       adNewMax[YDIR] = adMax[YDIR];
      break;
    case eSouthWest:
      adNewMin[XDIR] = adMin[XDIR];        adNewMax[XDIR] = adMean[XDIR];
      adNewMin[YDIR] = adMin[YDIR];        adNewMax[YDIR] = adMean[YDIR];
      break;
    default:
      assert2(0, "Invalid octant requested");
    } // end switch on which "octant"
  } // Done with 2D
  else {
    double adMean[3];
    adMean[XDIR] = 0.5 * (adMin[XDIR] + adMax[XDIR]);
    adMean[YDIR] = 0.5 * (adMin[YDIR] + adMax[YDIR]);
    adMean[ZDIR] = 0.5 * (adMin[ZDIR] + adMax[ZDIR]);
    switch (iOct) {
    case eUpperNorthEast:
      adNewMin[XDIR] = adMean[XDIR];       adNewMax[XDIR] = adMax[XDIR];
      adNewMin[YDIR] = adMean[YDIR];       adNewMax[YDIR] = adMax[YDIR];
      adNewMin[ZDIR] = adMean[ZDIR];       adNewMax[ZDIR] = adMax[ZDIR];
      break;
    case eLowerNorthEast:
      adNewMin[XDIR] = adMean[XDIR];       adNewMax[XDIR] = adMax[XDIR];
      adNewMin[YDIR] = adMean[YDIR];       adNewMax[YDIR] = adMax[YDIR];
      adNewMin[ZDIR] = adMin[ZDIR];        adNewMax[ZDIR] = adMean[ZDIR];
      break;
    case eUpperSouthEast:
      adNewMin[XDIR] = adMean[XDIR];       adNewMax[XDIR] = adMax[XDIR];
      adNewMin[YDIR] = adMin[YDIR];        adNewMax[YDIR] = adMean[YDIR];
      adNewMin[ZDIR] = adMean[ZDIR];       adNewMax[ZDIR] = adMax[ZDIR];
    break;
    case eLowerSouthEast:
      adNewMin[XDIR] = adMean[XDIR];       adNewMax[XDIR] = adMax[XDIR];
      adNewMin[YDIR] = adMin[YDIR];        adNewMax[YDIR] = adMean[YDIR];
      adNewMin[ZDIR] = adMin[ZDIR];        adNewMax[ZDIR] = adMean[ZDIR];
      break;
    case eUpperNorthWest:
      adNewMin[XDIR] = adMin[XDIR];        adNewMax[XDIR] = adMean[XDIR];
      adNewMin[YDIR] = adMean[YDIR];       adNewMax[YDIR] = adMax[YDIR];
      adNewMin[ZDIR] = adMean[ZDIR];       adNewMax[ZDIR] = adMax[ZDIR];
      break;
    case eLowerNorthWest:
      adNewMin[XDIR] = adMin[XDIR];        adNewMax[XDIR] = adMean[XDIR];
      adNewMin[YDIR] = adMean[YDIR];       adNewMax[YDIR] = adMax[YDIR];
      adNewMin[ZDIR] = adMin[ZDIR];        adNewMax[ZDIR] = adMean[ZDIR];
      break;
    case eUpperSouthWest:
      adNewMin[XDIR] = adMin[XDIR];        adNewMax[XDIR] = adMean[XDIR];
      adNewMin[YDIR] = adMin[YDIR];        adNewMax[YDIR] = adMean[YDIR];
      adNewMin[ZDIR] = adMean[ZDIR];       adNewMax[ZDIR] = adMax[ZDIR];
      break;
    case eLowerSouthWest:
      adNewMin[XDIR] = adMin[XDIR];        adNewMax[XDIR] = adMean[XDIR];
      adNewMin[YDIR] = adMin[YDIR];        adNewMax[YDIR] = adMean[YDIR];
      adNewMin[ZDIR] = adMin[ZDIR];        adNewMax[ZDIR] = adMean[ZDIR];
      break;
    default:
      assert2(0, "Invalid octant requested");
    } // end switch on which octant
  } // Done with 3D
}


VertTree::VertTree(const int iNV, Vert* const apVIn[], const int aiVIn[],
		   const double adMinIn[], const double adMaxIn[]) :
  iNVerts(iNV), iDim(apVIn[0]->iSpaceDimen()),
  iNumOct((iDim == 2) ? 4 : 8)
  // Construct an octree recursively from a vertex list
{
  adMin[0] = adMinIn[0];
  adMin[1] = adMinIn[1];
  adMax[0] = adMaxIn[0];
  adMax[1] = adMaxIn[1];
  if (iDim == 3) {
    // Conditional to avoid the remote possibility of a seg fault.
    adMax[2] = adMaxIn[2];
    adMin[2] = adMinIn[2];
  }

  // If the list is really small, stick it all in this node
  if (iNV <= iMaxVertsPerNode) {
    iNVerts = iNV;
    int i;
    for (i = 0; i < iNV; i++) {
      apV[i] = apVIn[i];
      aiV[i] = aiVIn[i];
    }
    for (i = 0; i < iNumOct; i++)
      apChild[i] = 0;
  }
  // Otherwise, split the list into eight and set them up separately
  else {
    iNVerts = 0;
    double adMean[] = {0.5 * (adMin[XDIR] + adMax[XDIR]),
		       0.5 * (adMin[YDIR] + adMax[YDIR]),
		       0.5 * (adMin[ZDIR] + adMax[ZDIR])};

    int i;
    int   **aaiV = new int*[iNumOct];
    Vert ***aapV = new Vert**[iNumOct];
    int   *aiLen = new int[iNumOct];
    for (i = 0; i < iNumOct; i++) {
      aaiV[i] = new int[iNV];
      aapV[i] = new Vert*[iNV];
      aiLen[i] = 0;
    }
    for (i = iNV-1; i >= 0; i--) {
      int iOct = iWhichOctant(apVIn[i]->adCoords(), adMean);
      aapV[iOct][aiLen[iOct]] = apVIn[i];
      aaiV[iOct][aiLen[iOct]] = aiVIn[i];
      aiLen[iOct]++;
    } // end of vertex categorization
    for (i = 0; i < iNumOct; i++) {
      if (aiLen[i] > 0) {
	double adNewMin[3], adNewMax[3];
	vChildOctantBounds(i, adNewMin, adNewMax);
	apChild[i] = new VertTree(aiLen[i], aapV[i], aaiV[i],
				  adNewMin, adNewMax);
      } // Add octants which have entries
      else apChild[i] = 0; // Take care of octants without any entries
    } // loop over possible octants
    for (i = 0; i < iNumOct; i++) {
      delete [] aaiV[i];
      delete [] aapV[i];
    }
    delete [] aaiV;
    delete [] aapV;
    delete [] aiLen;
  } // Too many entries for a single octant
} // VertTree constructor from list of pointers to vertex


VertTree::VertTree(const VAVertex& VA) :
  iNVerts(VA.iSize()), iDim(VA[0].iSpaceDimen()),
  iNumOct((iDim == 2) ? 4 : 8)
  // Construction of an octree decomposition of the points in a mesh.
{
  // Find dXMin, dYMin, dZMin, dXMax, dYMax, dZMax
  int i;
  adMin[XDIR] = LARGE_DBL;
  adMin[YDIR] = LARGE_DBL;
  adMin[ZDIR] = LARGE_DBL;

  adMax[XDIR] = - LARGE_DBL;
  adMax[YDIR] = - LARGE_DBL;
  adMax[ZDIR] = - LARGE_DBL;

  for (i = VA.iSize() - 1; i >= 0; i--) {
    Vert *pV = VA.pTEntry(i);
    const double * adLoc = pV->adCoords();
    for (int ii = 0; ii < iDim; ii++) {
      if (adLoc[ii] < adMin[ii]) adMin[ii] = adLoc[ii];
      if (adLoc[ii] > adMax[ii]) adMax[ii] = adLoc[ii];
    }
  }
  // If the list is really small, stick it all in this node
  if (VA.iSize() <= iMaxVertsPerNode) {
    iNVerts = VA.iSize();
    for (i = VA.iSize()-1; i >= 0; i--) {
      apV[i] = VA.pTEntry(i);
      aiV[i] = i;
    }
    for (i = 0; i < iNumOct; i++)
      apChild[i] = 0;
  }
  // Otherwise, split the list into eight and set them up separately
  else {
    iNVerts = 0;
    double adMean[] = {0.5 * (adMin[XDIR] + adMax[XDIR]),
		       0.5 * (adMin[YDIR] + adMax[YDIR]),
		       0.5 * (adMin[ZDIR] + adMax[ZDIR])};

    int   **aaiV = new int*[iNumOct];
    Vert ***aapV = new Vert**[iNumOct];
    int   *aiLen = new int[iNumOct];
    for (i = 0; i < iNumOct; i++) {
      aaiV[i] = new int[VA.iSize()];
      aapV[i] = new Vert*[VA.iSize()];
      aiLen[i] = 0;
    }
    for (i = VA.iSize()-1; i >= 0; i--) {
      int iOct = iWhichOctant(VA.pTEntry(i)->adCoords(), adMean);
      aapV[iOct][aiLen[iOct]] = VA.pTEntry(i);
      aaiV[iOct][aiLen[iOct]] = i;
      aiLen[iOct]++;
    } // end of vertex categorization
    for (i = 0; i < iNumOct; i++) {
      if (aiLen[i] > 0) {
	double adNewMin[3], adNewMax[3];
	vChildOctantBounds(i, adNewMin, adNewMax);
	apChild[i] = new VertTree(aiLen[i], aapV[i], aaiV[i],
				  adNewMin, adNewMax);
      } // Add octants which have entries
      else apChild[i] = 0; // Take care of octants without any entries
    } // loop over possible octants
    // Clean up the mess
    for (i = 0; i < iNumOct; i++) {
      delete [] aaiV[i];
      delete [] aapV[i];
    }
    delete [] aaiV;
    delete [] aapV;
    delete [] aiLen;
  } // Too many entries for a single octant
}

VertTree::~VertTree()
     // Recursively delete all the children
{
  if (iNVerts == 0) {
    for (int i = 0; i < iNumOct; i++)
      if (apChild[i] != 0) delete apChild[i];
  }
}

void VertTree::vAddPoint(const int iV, Vert* const pV)
{
  assert(iNVerts >= 0 && iNVerts <= iMaxVertsPerNode);
  double adMean[3];
  adMean[XDIR] = 0.5 * (adMin[XDIR] + adMax[XDIR]);
  adMean[YDIR] = 0.5 * (adMin[YDIR] + adMax[YDIR]);
  adMean[ZDIR] = 0.5 * (adMin[ZDIR] + adMax[ZDIR]);
  switch (iNVerts) {
  case 0: // Insert into a child
    {
      int iOct = iWhichOctant(pV->adCoords(), adMean);
      if (apChild[iOct] == 0) {
	double adNewMin[3], adNewMax[3];
	vChildOctantBounds(iOct, adNewMin, adNewMax);
	apChild[iOct] = new VertTree(1, &pV, &iV, adNewMin, adNewMax);
      }
      else
	apChild[iOct]->vAddPoint(iV, pV);
      break;
    }
  case iMaxVertsPerNode: // Split the data for this node and add a level
    // to the tree locally
    {
      int   **aaiV = new int*[iNumOct];
      Vert ***aapV = new Vert**[iNumOct];
      int   *aiLen = new int[iNumOct];
      int i;
      for (i = 0; i < iNumOct; i++) {
	aaiV[i] = new int[iNVerts];
	aapV[i] = new Vert*[iNVerts];
	aiLen[i] = 0;
      }
      int iOct = iWhichOctant(pV->adCoords(), adMean);
      aapV[iOct][aiLen[iOct]] = pV;
      aaiV[iOct][aiLen[iOct]] = iV;
      aiLen[iOct]++;

      for (i = iNVerts-1; i >= 0; i--) {
	iOct = iWhichOctant(apV[i]->adCoords(), adMean);
	aapV[iOct][aiLen[iOct]] = apV[i];
	aaiV[iOct][aiLen[iOct]] = aiV[i];
	aiLen[iOct]++;
      } // end of vertex categorization
      for (i = 0; i < iNumOct; i++) {
	if (aiLen[i] > 0) {
	  double adNewMin[3], adNewMax[3];
	  vChildOctantBounds(i, adNewMin, adNewMax);
	  apChild[i] = new VertTree(aiLen[i], aapV[i], aaiV[i],
				    adNewMin, adNewMax);
	} // Add octants which have entries
	else apChild[i] = 0; // Take care of octants without any entries
      } // loop over possible octants
      iNVerts = 0;
      for (i = 0; i < iNumOct; i++) {
	delete [] aaiV[i];
	delete [] aapV[i];
      }
      delete [] aaiV;
      delete [] aapV;
      delete [] aiLen;
      break;
    } // Too many entries for a single octant
  default: // Just add another vert to the octant
    apV[iNVerts] = pV;
    aiV[iNVerts] = iV;
    iNVerts++;
    break;
  }
}

std::vector<int> VertTree::veciRangeQuery(const double adLo[],
					  const double adHi[]) const
{
  std::vector<int> veciResult;

  if (iDim == 2) {
    if (iNVerts == 0) { // Not a leaf; search recursively
      double dXMean = 0.5 * (adMin[XDIR] + adMax[XDIR]);
      double dYMean = 0.5 * (adMin[YDIR] + adMax[YDIR]);

      bool qCheckQuadrant[] = {true, true, true, true};
      // Eliminate leaves that don't overlap the bounding box.
      if (adLo[XDIR] > dXMean) {
	qCheckQuadrant[eLowerSouthWest] = false;
	qCheckQuadrant[eLowerNorthWest] = false;
      }
      else if (adHi[XDIR] < dXMean) {
	qCheckQuadrant[eLowerSouthEast] = false;
	qCheckQuadrant[eLowerNorthEast] = false;
      }
      if (adLo[YDIR] > dYMean) {
	qCheckQuadrant[eLowerSouthWest] = false;
	qCheckQuadrant[eLowerSouthEast] = false;
      }
      else if (adHi[YDIR] < dYMean) {
	qCheckQuadrant[eLowerNorthWest] = false;
	qCheckQuadrant[eLowerNorthEast] = false;
      }
      // Now check recursively in all existing octants that overlap the
      // bounding box.
      for (int i = 0; i < iNumOct; i++) {
	if (qCheckQuadrant[i] && apChild[i]) {
	  std::vector<int> vecTmp(apChild[i]->veciRangeQuery(adLo, adHi));
	  // The following is equivalent to using push_back for every element.
	  veciResult.insert(veciResult.end(), vecTmp.begin(), vecTmp.end());
	}
      }
    } // End of case with sub-octants
    else { // Add verts from this level
      for (int i = 0; i < iNVerts; i++) {
	double dX = apV[i]->dX();
	double dY = apV[i]->dY();
	if (dX >= adLo[XDIR] && dX <= adHi[XDIR] &&
	    dY >= adLo[YDIR] && dY <= adHi[YDIR])
	  veciResult.push_back(aiV[i]);
      }
    }
  }
  else {
    // iDim == 3
    if (iNVerts == 0) { // Not a leaf; search recursively
      double dXMean = 0.5 * (adMin[XDIR] + adMax[XDIR]);
      double dYMean = 0.5 * (adMin[YDIR] + adMax[YDIR]);
      double dZMean = 0.5 * (adMin[ZDIR] + adMax[ZDIR]);

      bool qCheckOctant[] = {true, true, true, true, true, true, true, true};
      // Eliminate leaves that don't overlap the bounding box.
      if (adLo[XDIR] > dXMean) {
	qCheckOctant[eLowerSouthWest] = false;
	qCheckOctant[eLowerNorthWest] = false;
	qCheckOctant[eUpperSouthWest] = false;
	qCheckOctant[eUpperNorthWest] = false;
      }
      else if (adHi[XDIR] < dXMean) {
	qCheckOctant[eLowerSouthEast] = false;
	qCheckOctant[eLowerNorthEast] = false;
	qCheckOctant[eUpperSouthEast] = false;
	qCheckOctant[eUpperNorthEast] = false;
      }
      if (adLo[YDIR] > dYMean) {
	qCheckOctant[eLowerSouthWest] = false;
	qCheckOctant[eLowerSouthEast] = false;
	qCheckOctant[eUpperSouthWest] = false;
	qCheckOctant[eUpperSouthEast] = false;
      }
      else if (adHi[YDIR] < dYMean) {
	qCheckOctant[eLowerNorthWest] = false;
	qCheckOctant[eLowerNorthEast] = false;
	qCheckOctant[eUpperNorthWest] = false;
	qCheckOctant[eUpperNorthEast] = false;
      }
      if (adLo[ZDIR] > dZMean) {
	qCheckOctant[eLowerSouthWest] = false;
	qCheckOctant[eLowerSouthEast] = false;
	qCheckOctant[eLowerNorthWest] = false;
	qCheckOctant[eLowerNorthEast] = false;
      }
      else if (adHi[ZDIR] < dZMean) {
	qCheckOctant[eUpperSouthWest] = false;
	qCheckOctant[eUpperSouthEast] = false;
	qCheckOctant[eUpperNorthWest] = false;
	qCheckOctant[eUpperNorthEast] = false;
      }
      // Now check recursively in all existing octants that overlap the
      // bounding box.
      for (int i = 0; i < iNumOct; i++) {
	if (qCheckOctant[i] && apChild[i]) {
	  std::vector<int> vecTmp(apChild[i]->veciRangeQuery(adLo, adHi));
	  // The following is equivalent to using push_back for every element.
	  veciResult.insert(veciResult.end(), vecTmp.begin(), vecTmp.end());
	}
      }
    } // End of case with sub-octants
    else { // Add verts from this level
      for (int i = 0; i < iNVerts; i++) {
	double dX = apV[i]->dX();
	double dY = apV[i]->dY();
	double dZ = apV[i]->dZ();
	if (dX >= adLo[XDIR] && dX <= adHi[XDIR] &&
	    dY >= adLo[YDIR] && dY <= adHi[YDIR] &&
	    dZ >= adLo[ZDIR] && dZ <= adHi[ZDIR])
	  veciResult.push_back(aiV[i]);
      }
    }
  }
  return(veciResult);
}

std::vector<Vert*> VertTree::vecpVRangeQuery(const double adLo[],
					     const double adHi[]) const
{
  std::vector<Vert*> vecpVResult;

  if (iDim == 2) {
    if (iNVerts == 0) { // Not a leaf; search recursively
      double dXMean = 0.5 * (adMin[XDIR] + adMax[XDIR]);
      double dYMean = 0.5 * (adMin[YDIR] + adMax[YDIR]);

      bool qCheckQuadrant[] = {true, true, true, true};
      // Eliminate leaves that don't overlap the bounding box.
      if (adLo[XDIR] > dXMean) {
	qCheckQuadrant[eLowerSouthWest] = false;
	qCheckQuadrant[eLowerNorthWest] = false;
      }
      else if (adHi[XDIR] < dXMean) {
	qCheckQuadrant[eLowerSouthEast] = false;
	qCheckQuadrant[eLowerNorthEast] = false;
      }
      if (adLo[YDIR] > dYMean) {
	qCheckQuadrant[eLowerSouthWest] = false;
	qCheckQuadrant[eLowerSouthEast] = false;
      }
      else if (adHi[YDIR] < dYMean) {
	qCheckQuadrant[eLowerNorthWest] = false;
	qCheckQuadrant[eLowerNorthEast] = false;
      }
      // Now check recursively in all existing octants that overlap the
      // bounding box.
      for (int i = 0; i < iNumOct; i++) {
	if (qCheckQuadrant[i] && apChild[i]) {
	  std::vector<Vert*> vecpVTmp(apChild[i]->vecpVRangeQuery(adLo, adHi));
	  vecpVResult.insert(vecpVResult.end(),
			     vecpVTmp.begin(), vecpVTmp.end());
	}
      }
    } // End of case with sub-octants
    else { // Add verts from this level
      for (int i = 0; i < iNVerts; i++) {
	double dX = apV[i]->dX();
	double dY = apV[i]->dY();
	if (dX >= adLo[XDIR] && dX <= adHi[XDIR] &&
	    dY >= adLo[YDIR] && dY <= adHi[YDIR])
	  vecpVResult.push_back(apV[i]);
      }
    }
  }
  else {
    // iDim == 3
    if (iNVerts == 0) { // Not a leaf; search recursively
      double dXMean = 0.5 * (adMin[XDIR] + adMax[XDIR]);
      double dYMean = 0.5 * (adMin[YDIR] + adMax[YDIR]);
      double dZMean = 0.5 * (adMin[ZDIR] + adMax[ZDIR]);

      bool qCheckOctant[] = {true, true, true, true, true, true, true, true};
      // Eliminate leaves that don't overlap the bounding box.
      if (adLo[XDIR] > dXMean) {
	qCheckOctant[eLowerSouthWest] = false;
	qCheckOctant[eLowerNorthWest] = false;
	qCheckOctant[eUpperSouthWest] = false;
	qCheckOctant[eUpperNorthWest] = false;
      }
      else if (adHi[XDIR] < dXMean) {
	qCheckOctant[eLowerSouthEast] = false;
	qCheckOctant[eLowerNorthEast] = false;
	qCheckOctant[eUpperSouthEast] = false;
	qCheckOctant[eUpperNorthEast] = false;
      }
      if (adLo[YDIR] > dYMean) {
	qCheckOctant[eLowerSouthWest] = false;
	qCheckOctant[eLowerSouthEast] = false;
	qCheckOctant[eUpperSouthWest] = false;
	qCheckOctant[eUpperSouthEast] = false;
      }
      else if (adHi[YDIR] < dYMean) {
	qCheckOctant[eLowerNorthWest] = false;
	qCheckOctant[eLowerNorthEast] = false;
	qCheckOctant[eUpperNorthWest] = false;
	qCheckOctant[eUpperNorthEast] = false;
      }
      if (adLo[ZDIR] > dZMean) {
	qCheckOctant[eLowerSouthWest] = false;
	qCheckOctant[eLowerSouthEast] = false;
	qCheckOctant[eLowerNorthWest] = false;
	qCheckOctant[eLowerNorthEast] = false;
      }
      else if (adHi[ZDIR] < dZMean) {
	qCheckOctant[eUpperSouthWest] = false;
	qCheckOctant[eUpperSouthEast] = false;
	qCheckOctant[eUpperNorthWest] = false;
	qCheckOctant[eUpperNorthEast] = false;
      }
      // Now check recursively in all existing octants that overlap the
      // bounding box.
      for (int i = 0; i < iNumOct; i++) {
	if (qCheckOctant[i] && apChild[i]) {
	  std::vector<Vert*> vecpVTmp(apChild[i]->vecpVRangeQuery(adLo, adHi));
	  vecpVResult.insert(vecpVResult.end(),
			     vecpVTmp.begin(), vecpVTmp.end());
	}
      }
    } // End of case with sub-octants
    else { // Add verts from this level
      for (int i = 0; i < iNVerts; i++) {
	double dX = apV[i]->dX();
	double dY = apV[i]->dY();
	double dZ = apV[i]->dZ();
	if (dX >= adLo[XDIR] && dX <= adHi[XDIR] &&
	    dY >= adLo[YDIR] && dY <= adHi[YDIR] &&
	    dZ >= adLo[ZDIR] && dZ <= adHi[ZDIR])
	  vecpVResult.push_back(apV[i]);
      }
    }
  }
  return(vecpVResult);
}

std::vector<int> VertTree::veciBallQuery(const double adCenter[],
					   const double dRad) const
{
  double adBallMin[3], adBallMax[3];
  adBallMin[XDIR] = adCenter[XDIR] - dRad;
  adBallMin[YDIR] = adCenter[YDIR] - dRad;
  adBallMax[XDIR] = adCenter[XDIR] + dRad;
  adBallMax[YDIR] = adCenter[YDIR] + dRad;
  if (iDim == 3) {
    adBallMin[ZDIR] = adCenter[ZDIR] - dRad;
    adBallMax[ZDIR] = adCenter[ZDIR] + dRad;
  }
  std::vector<Vert*> vecpVResult = vecpVRangeQuery(adBallMin, adBallMax);
  std::vector<int> veciTmp = veciRangeQuery(adBallMin, adBallMax),
    veciResult;
  for (int i = vecpVResult.size() - 1; i >= 0 ; i--) {
    const double *adLoc = vecpVResult[i]->adCoords();

    double dDist;
    if (iDim == 2) {
      dDist = dDIST2D(adLoc, adCenter);
    }
    else {
      assert(iDim == 3);
      dDist = dDIST3D(adLoc, adCenter);
    }
    if (iFuzzyComp(dDist, dRad) != 1)
      veciResult.push_back(veciTmp[i]);
  }
  return veciResult;
}

std::vector<int> VertTree::veciHalfSpaceQuery(const double adNorm[],
					      const double dDist,
					      const double adSamplePt[]) const
{
  std::vector<int> veciResult;

  if (iDim == 2) {
    if (iNVerts == 0) { // Not a leaf; search recursively
      bool qIsCut = false;
      int iSideRef;
      // First find the side of the line for the southwest corner of
      // the bounding box for this node.
      double dDot = adMin[XDIR]*adNorm[XDIR] + adMin[YDIR]*adNorm[YDIR];
      iSideRef = iFuzzyComp(dDot, dDist);

      // Now check the northeast corner.
      dDot = adMax[XDIR]*adNorm[XDIR] + adMax[YDIR]*adNorm[YDIR];
      qIsCut = (iSideRef * iFuzzyComp(dDot, dDist) != 1);

      if (!qIsCut) {
	// Check the northwest corner.
	dDot = adMin[XDIR]*adNorm[XDIR] + adMax[YDIR]*adNorm[YDIR];
	qIsCut = (iSideRef * iFuzzyComp(dDot, dDist) != 1);

	if (!qIsCut) {
	  // Check the southeast corner.
	  dDot = adMax[XDIR]*adNorm[XDIR] + adMin[YDIR]*adNorm[YDIR];
	  qIsCut = (iSideRef * iFuzzyComp(dDot, dDist) != 1);
	}
      }

      // If the line passes through this box, recursively check all its
      // children.
      if (qIsCut) {
	for (int i = 0; i < iNumOct; i++) {
	  if (apChild[i]) {
	    std::vector<int> veciTmp(apChild[i]->
				     veciHalfSpaceQuery(adNorm, dDist,
							adSamplePt));
	    veciResult.insert(veciResult.end(),
			      veciTmp.begin(), veciTmp.end());
	  }
	}
      }
      else {
	// Also, check whether maybe this entire box should be in!
	dDot = dDOT2D(adNorm, adSamplePt);
	if (iSideRef * iFuzzyComp(dDot, dDist) == 1) {
	  // Yep, include all the verts in all the children of this box.
	  return veciAllChildren();
	}
      }
    } // End of case with sub-quadrants
    else { // Add verts from this level
      double dDot = dDOT2D(adNorm, adSamplePt);
      int iResultToKeep = iFuzzyComp(dDot, dDist);
      assert(iResultToKeep != 0);

      for (int i = 0; i < iNVerts; i++) {
	double dDot2 = adNorm[XDIR]*apV[i]->dX() + adNorm[YDIR]*apV[i]->dY();
	if (iFuzzyComp(dDot2, dDist) == iResultToKeep)
	  veciResult.push_back(aiV[i]);
      }
    }
  }
  else {
    // iDim == 3
    if (iNVerts == 0) { // Not a leaf; search recursively
      bool qIsCut = false;
      int iSideRef;
      // The order in which checks are done is meant to reduce the
      // number of dot products required.

      // First find the side of the line for the lower southwest corner of
      // the bounding box for this node.
      double dDot = adMin[XDIR]*adNorm[XDIR] + adMin[YDIR]*adNorm[YDIR]
	+ adMin[ZDIR]*adNorm[ZDIR];
      iSideRef = iFuzzyComp(dDot, dDist);

      // Now check the upper northeast corner.
      dDot = adMax[XDIR]*adNorm[XDIR] + adMax[YDIR]*adNorm[YDIR]
	+ adMax[ZDIR]*adNorm[ZDIR];
      qIsCut = (iSideRef * iFuzzyComp(dDot, dDist) != 1);

      if (!qIsCut) {
	// Check the lower southeast corner.
	dDot = adMax[XDIR]*adNorm[XDIR] + adMin[YDIR]*adNorm[YDIR]
	  + adMin[ZDIR]*adNorm[ZDIR];
	qIsCut = (iSideRef * iFuzzyComp(dDot, dDist) != 1);

	if (!qIsCut) {
	  // Check the upper southwest corner.
	  dDot = adMin[XDIR]*adNorm[XDIR] + adMin[YDIR]*adNorm[YDIR]
	    + adMax[ZDIR]*adNorm[ZDIR];
	  qIsCut = (iSideRef * iFuzzyComp(dDot, dDist) != 1);

	  if (!qIsCut) {
	    // Check the lower northwest corner.
	    dDot = adMin[XDIR]*adNorm[XDIR] + adMax[YDIR]*adNorm[YDIR]
	      + adMin[ZDIR]*adNorm[ZDIR];
	    qIsCut = (iSideRef * iFuzzyComp(dDot, dDist) != 1);

	    if (!qIsCut) {
	      // Check the upper southeast corner.
	      dDot = adMax[XDIR]*adNorm[XDIR] + adMin[YDIR]*adNorm[YDIR]
		+ adMax[ZDIR]*adNorm[ZDIR];
	      qIsCut = (iSideRef * iFuzzyComp(dDot, dDist) != 1);

	      if (!qIsCut) {
		// Check the upper northwest corner.
		dDot = adMin[XDIR]*adNorm[XDIR] + adMax[YDIR]*adNorm[YDIR]
		  + adMax[ZDIR]*adNorm[ZDIR];
		qIsCut = (iSideRef * iFuzzyComp(dDot, dDist) != 1);

		if (!qIsCut) {
		  // Check the lower northeast corner.
		  dDot = adMax[XDIR]*adNorm[XDIR] + adMax[YDIR]*adNorm[YDIR]
		    + adMin[ZDIR]*adNorm[ZDIR];
		  qIsCut = (iSideRef * iFuzzyComp(dDot, dDist) != 1);
		} // low NE
	      } // up NW
	    } // up SE
	  } // low NW
	} // up SW
      } // low SE

      // If the line passes through this box, recursively check all its
      // children.
      if (qIsCut) {
	for (int i = 0; i < iNumOct; i++) {
	  if (apChild[i]) {
	    std::vector<int> veciTmp(apChild[i]->
				     veciHalfSpaceQuery(adNorm, dDist,
							adSamplePt));
	    veciResult.insert(veciResult.end(),
			      veciTmp.begin(), veciTmp.end());
	  }
	}
      }
      else {
	// Also, check whether maybe this entire box should be in!
	dDot = dDOT3D(adNorm, adSamplePt);
	if (iSideRef * iFuzzyComp(dDot, dDist) == 1) {
	  // Yep, include all the verts in all the children of this box.
	  return veciAllChildren();
	}
      }
    } // End of case with sub-octants
    else { // Add verts from this level
      double dDot = dDOT3D(adNorm, adSamplePt);
      int iResultToKeep = iFuzzyComp(dDot, dDist);
      assert(iResultToKeep != 0);

      for (int i = 0; i < iNVerts; i++) {
	double dDot2 = adNorm[XDIR]*apV[i]->dX() + adNorm[YDIR]*apV[i]->dY()
	  + adNorm[ZDIR]*apV[i]->dZ();
	if (iFuzzyComp(dDot2, dDist) == iResultToKeep)
	  veciResult.push_back(aiV[i]);
      }
    }
  }
  return(veciResult);
}

std::vector<int> VertTree::veciAllChildren() const
{
  std::vector<int> veciResult;
  if (iNVerts == 0) {
    for (int i = 0; i < iNumOct; i++) {
      if (apChild[i]) {
	std::vector<int> veciTmp(apChild[i]->veciAllChildren());
	veciResult.insert(veciResult.end(), veciTmp.begin(), veciTmp.end());
      }
    }
  }
  else {
    for (int i = 0; i < iNVerts; i++) {
      veciResult.push_back(aiV[i]);
    }
  }
  return veciResult;
}
