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

  Program:   Visualization Toolkit
  Module:    vtkOBBTree.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 "vtkOBBTree.h"

#include "vtkCellArray.h"
#include "vtkGenericCell.h"
#include "vtkLine.h"
#include "vtkMath.h"
#include "vtkMatrix4x4.h"
#include "vtkObjectFactory.h"
#include "vtkPolyData.h"
#include "vtkPolygon.h"
#include "vtkTriangle.h"
#include "vtkUnstructuredGrid.h"

#include <vector>

//------------------------------------------------------------------------------
vtkStandardNewMacro(vtkOBBTree);

//------------------------------------------------------------------------------
#define vtkCELLTRIANGLES(CELLPTIDS, TYPE, IDX, PTID0, PTID1, PTID2)                                \
  {                                                                                                \
    switch (TYPE)                                                                                  \
    {                                                                                              \
      case VTK_TRIANGLE:                                                                           \
      case VTK_POLYGON:                                                                            \
      case VTK_QUAD:                                                                               \
        PTID0 = CELLPTIDS[0];                                                                      \
        PTID1 = CELLPTIDS[(IDX) + 1];                                                              \
        PTID2 = CELLPTIDS[(IDX) + 2];                                                              \
        break;                                                                                     \
      case VTK_TRIANGLE_STRIP:                                                                     \
        PTID0 = CELLPTIDS[IDX];                                                                    \
        PTID1 = CELLPTIDS[(IDX) + 1 + ((IDX)&1)];                                                  \
        PTID2 = CELLPTIDS[(IDX) + 2 - ((IDX)&1)];                                                  \
        break;                                                                                     \
      default:                                                                                     \
        PTID0 = PTID1 = PTID2 = -1;                                                                \
    }                                                                                              \
  }

//------------------------------------------------------------------------------
vtkOBBNode::vtkOBBNode()
{
  this->Cells = nullptr;
  this->Parent = nullptr;
  this->Kids = nullptr;
}

//------------------------------------------------------------------------------
vtkOBBNode::~vtkOBBNode()
{
  delete[] this->Kids;
  if (this->Cells)
  {
    this->Cells->Delete();
  }
}

//------------------------------------------------------------------------------
// Construct with automatic computation of divisions, averaging
// 25 cells per octant.
vtkOBBTree::vtkOBBTree()
{
  this->DataSet = nullptr;
  this->Level = 0;
  this->MaxLevel = 12;
  this->Tolerance = 0.01;
  this->Tree = nullptr;
  this->PointsList = nullptr;
  this->InsertedPoints = nullptr;
  this->OBBCount = 0;
}

//------------------------------------------------------------------------------
vtkOBBTree::~vtkOBBTree()
{
  this->FreeSearchStructure();
}

//------------------------------------------------------------------------------
void vtkOBBTree::FreeSearchStructure()
{
  if (this->Tree)
  {
    this->DeleteTree(this->Tree);
    delete this->Tree;
    this->Tree = nullptr;
  }
}

//------------------------------------------------------------------------------
void vtkOBBTree::DeleteTree(vtkOBBNode* OBBptr)
{
  if (OBBptr->Kids != nullptr)
  {
    this->DeleteTree(OBBptr->Kids[0]);
    this->DeleteTree(OBBptr->Kids[1]);
    delete OBBptr->Kids[0];
    delete OBBptr->Kids[1];
  }
}

//------------------------------------------------------------------------------
// Compute an OBB from the list of points given. Return the corner point
// and the three axes defining the orientation of the OBB. Also return
// a sorted list of relative "sizes" of axes for comparison purposes.
void vtkOBBTree::ComputeOBB(
  vtkPoints* pts, double corner[3], double max[3], double mid[3], double min[3], double size[3])
{
  int i;
  vtkIdType numPts, pointId;
  double x[3], mean[3], xp[3], *v[3], v0[3], v1[3], v2[3];
  double *a[3], a0[3], a1[3], a2[3];
  double tMin[3], tMax[3], closest[3], t;

  //
  // Compute mean
  //
  numPts = pts->GetNumberOfPoints();
  mean[0] = mean[1] = mean[2] = 0.0;
  for (pointId = 0; pointId < numPts; pointId++)
  {
    pts->GetPoint(pointId, x);
    for (i = 0; i < 3; i++)
    {
      mean[i] += x[i];
    }
  }
  for (i = 0; i < 3; i++)
  {
    mean[i] /= numPts;
  }

  //
  // Compute covariance matrix
  //
  a[0] = a0;
  a[1] = a1;
  a[2] = a2;
  for (i = 0; i < 3; i++)
  {
    a0[i] = a1[i] = a2[i] = 0.0;
  }

  for (pointId = 0; pointId < numPts; pointId++)
  {
    pts->GetPoint(pointId, x);
    xp[0] = x[0] - mean[0];
    xp[1] = x[1] - mean[1];
    xp[2] = x[2] - mean[2];
    for (i = 0; i < 3; i++)
    {
      a0[i] += xp[0] * xp[i];
      a1[i] += xp[1] * xp[i];
      a2[i] += xp[2] * xp[i];
    }
  } // for all points

  for (i = 0; i < 3; i++)
  {
    a0[i] /= numPts;
    a1[i] /= numPts;
    a2[i] /= numPts;
  }

  //
  // Extract axes (i.e., eigenvectors) from covariance matrix.
  //
  v[0] = v0;
  v[1] = v1;
  v[2] = v2;
  vtkMath::Jacobi(a, size, v);
  max[0] = v[0][0];
  max[1] = v[1][0];
  max[2] = v[2][0];
  mid[0] = v[0][1];
  mid[1] = v[1][1];
  mid[2] = v[2][1];
  min[0] = v[0][2];
  min[1] = v[1][2];
  min[2] = v[2][2];

  for (i = 0; i < 3; i++)
  {
    a[0][i] = mean[i] + max[i];
    a[1][i] = mean[i] + mid[i];
    a[2][i] = mean[i] + min[i];
  }

  //
  // Create oriented bounding box by projecting points onto eigenvectors.
  //
  tMin[0] = tMin[1] = tMin[2] = VTK_DOUBLE_MAX;
  tMax[0] = tMax[1] = tMax[2] = -VTK_DOUBLE_MAX;

  for (pointId = 0; pointId < numPts; pointId++)
  {
    pts->GetPoint(pointId, x);
    for (i = 0; i < 3; i++)
    {
      vtkLine::DistanceToLine(x, mean, a[i], t, closest);
      if (t < tMin[i])
      {
        tMin[i] = t;
      }
      if (t > tMax[i])
      {
        tMax[i] = t;
      }
    }
  } // for all points

  for (i = 0; i < 3; i++)
  {
    corner[i] = mean[i] + tMin[0] * max[i] + tMin[1] * mid[i] + tMin[2] * min[i];

    max[i] = (tMax[0] - tMin[0]) * max[i];
    mid[i] = (tMax[1] - tMin[1]) * mid[i];
    min[i] = (tMax[2] - tMin[2]) * min[i];
  }
}

//------------------------------------------------------------------------------
// a method to compute the OBB of a dataset without having to go through the
// Execute method; It does set
void vtkOBBTree::ComputeOBB(
  vtkDataSet* input, double corner[3], double max[3], double mid[3], double min[3], double size[3])
{
  vtkIdType numPts, numCells, i;
  vtkIdList* cellList;
  vtkDataSet* origDataSet;

  vtkDebugMacro(<< "Computing OBB");

  if (input == nullptr || (numPts = input->GetNumberOfPoints()) < 1 ||
    (input->GetNumberOfCells()) < 1)
  {
    vtkErrorMacro(<< "Can't compute OBB - no data available!");
    return;
  }
  numCells = input->GetNumberOfCells();

  // save previous value of DataSet and reset after calling ComputeOBB because
  // computeOBB used this->DataSet internally
  origDataSet = this->DataSet;
  this->DataSet = input;

  // these are other member variables that ComputeOBB requires
  this->OBBCount = 0;
  this->InsertedPoints = new int[numPts];
  for (i = 0; i < numPts; i++)
  {
    this->InsertedPoints[i] = 0;
  }
  this->PointsList = vtkPoints::New();
  this->PointsList->Allocate(numPts);

  cellList = vtkIdList::New();
  cellList->Allocate(numCells);
  for (i = 0; i < numCells; i++)
  {
    cellList->InsertId(i, i);
  }

  this->ComputeOBB(cellList, corner, max, mid, min, size);

  this->DataSet = origDataSet;
  delete[] this->InsertedPoints;
  this->PointsList->Delete();
  cellList->Delete();
}

//------------------------------------------------------------------------------
// Compute an OBB from the list of cells given. Return the corner point
// and the three axes defining the orientation of the OBB. Also return
// a sorted list of relative "sizes" of axes for comparison purposes.
void vtkOBBTree::ComputeOBB(
  vtkIdList* cells, double corner[3], double max[3], double mid[3], double min[3], double size[3])
{
  vtkIdType numCells, i, j, cellId, ptId, pId, qId, rId;
  int k, type;
  vtkIdType numPts = 0;
  const vtkIdType* ptIds = nullptr;
  double p[3], q[3], r[3], mean[3], xp[3], *v[3], v0[3], v1[3], v2[3];
  double *a[3], a0[3], a1[3], a2[3];
  double tMin[3], tMax[3], closest[3], t;
  double dp0[3], dp1[3], tri_mass, tot_mass, c[3];

  this->OBBCount++;
  this->PointsList->Reset();
  //
  // Compute mean & moments
  //

  numCells = cells->GetNumberOfIds();
  mean[0] = mean[1] = mean[2] = 0.0;
  tot_mass = 0.0;
  a[0] = a0;
  a[1] = a1;
  a[2] = a2;
  for (i = 0; i < 3; i++)
  {
    a0[i] = a1[i] = a2[i] = 0.0;
  }

  for (i = 0; i < numCells; i++)
  {
    cellId = cells->GetId(i);
    type = this->DataSet->GetCellType(cellId);
    switch (this->DataSet->GetDataObjectType())
    {
      case VTK_POLY_DATA:
        ((vtkPolyData*)this->DataSet)->GetCellPoints(cellId, numPts, ptIds);
        break;
      case VTK_UNSTRUCTURED_GRID:
        ((vtkUnstructuredGrid*)this->DataSet)->GetCellPoints(cellId, numPts, ptIds);
        break;
      default:
        vtkErrorMacro(<< "DataSet " << this->DataSet->GetClassName() << " not supported.");
        break;
    }
    for (j = 0; j < numPts - 2; j++)
    {
      vtkCELLTRIANGLES(ptIds, type, j, pId, qId, rId);
      if (pId < 0)
      {
        continue;
      }
      this->DataSet->GetPoint(pId, p);
      this->DataSet->GetPoint(qId, q);
      this->DataSet->GetPoint(rId, r);
      // p, q, and r are the oriented triangle points.
      // Compute the components of the moment of inertia tensor.
      for (k = 0; k < 3; k++)
      {
        // two edge vectors
        dp0[k] = q[k] - p[k];
        dp1[k] = r[k] - p[k];
        // centroid
        c[k] = (p[k] + q[k] + r[k]) / 3;
      }
      vtkMath::Cross(dp0, dp1, xp);
      tri_mass = 0.5 * vtkMath::Norm(xp);
      tot_mass += tri_mass;
      for (k = 0; k < 3; k++)
      {
        mean[k] += tri_mass * c[k];
      }

      // on-diagonal terms
      a0[0] += tri_mass * (9 * c[0] * c[0] + p[0] * p[0] + q[0] * q[0] + r[0] * r[0]) / 12;
      a1[1] += tri_mass * (9 * c[1] * c[1] + p[1] * p[1] + q[1] * q[1] + r[1] * r[1]) / 12;
      a2[2] += tri_mass * (9 * c[2] * c[2] + p[2] * p[2] + q[2] * q[2] + r[2] * r[2]) / 12;

      // off-diagonal terms
      a0[1] += tri_mass * (9 * c[0] * c[1] + p[0] * p[1] + q[0] * q[1] + r[0] * r[1]) / 12;
      a0[2] += tri_mass * (9 * c[0] * c[2] + p[0] * p[2] + q[0] * q[2] + r[0] * r[2]) / 12;
      a1[2] += tri_mass * (9 * c[1] * c[2] + p[1] * p[2] + q[1] * q[2] + r[1] * r[2]) / 12;
    } // end foreach triangle

    // While computing cell moments, gather all the cell's
    // point coordinates into a single list.
    //
    for (j = 0; j < numPts; j++)
    {
      if (this->InsertedPoints[ptIds[j]] != this->OBBCount)
      {
        this->InsertedPoints[ptIds[j]] = this->OBBCount;
        this->PointsList->InsertNextPoint(this->DataSet->GetPoint(ptIds[j]));
      }
    } // for all points of this cell
  }   // end foreach cell

  // normalize data
  for (i = 0; i < 3; i++)
  {
    mean[i] = mean[i] / tot_mass;
  }

  // matrix is symmetric
  a1[0] = a0[1];
  a2[0] = a0[2];
  a2[1] = a1[2];

  // get covariance from moments
  for (i = 0; i < 3; i++)
  {
    for (j = 0; j < 3; j++)
    {
      a[i][j] = a[i][j] / tot_mass - mean[i] * mean[j];
    }
  }

  //
  // Extract axes (i.e., eigenvectors) from covariance matrix.
  //
  v[0] = v0;
  v[1] = v1;
  v[2] = v2;
  vtkMath::Jacobi(a, size, v);
  max[0] = v[0][0];
  max[1] = v[1][0];
  max[2] = v[2][0];
  mid[0] = v[0][1];
  mid[1] = v[1][1];
  mid[2] = v[2][1];
  min[0] = v[0][2];
  min[1] = v[1][2];
  min[2] = v[2][2];

  for (i = 0; i < 3; i++)
  {
    a[0][i] = mean[i] + max[i];
    a[1][i] = mean[i] + mid[i];
    a[2][i] = mean[i] + min[i];
  }

  //
  // Create oriented bounding box by projecting points onto eigenvectors.
  //
  tMin[0] = tMin[1] = tMin[2] = VTK_DOUBLE_MAX;
  tMax[0] = tMax[1] = tMax[2] = -VTK_DOUBLE_MAX;

  numPts = this->PointsList->GetNumberOfPoints();
  for (ptId = 0; ptId < numPts; ptId++)
  {
    this->PointsList->GetPoint(ptId, p);
    for (i = 0; i < 3; i++)
    {
      vtkLine::DistanceToLine(p, mean, a[i], t, closest);
      if (t < tMin[i])
      {
        tMin[i] = t;
      }
      if (t > tMax[i])
      {
        tMax[i] = t;
      }
    }
  } // for all points

  for (i = 0; i < 3; i++)
  {
    corner[i] = mean[i] + tMin[0] * max[i] + tMin[1] * mid[i] + tMin[2] * min[i];

    max[i] = (tMax[0] - tMin[0]) * max[i];
    mid[i] = (tMax[1] - tMin[1]) * mid[i];
    min[i] = (tMax[2] - tMin[2]) * min[i];
  }
}

//------------------------------------------------------------------------------
// Efficient check for whether a line p1,p2 intersects with triangle
// pt1,pt2,pt3 to within specified tolerance.  This is included here
// because vtkTriangle doesn't have an equivalently efficient method.

// The intersection point is returned, along with the parametric
// coordinate t and the sense of the intersection (+1 if entering
// or -1 if exiting, according to normal of triangle)

// The function return value is 1 if an intersection was found.
static inline int vtkOBBTreeLineIntersectsTriangle(const double p1[3], const double p2[3],
  double pt1[3], double pt2[3], double pt3[3], double tolerance, double point[3], double& t,
  int& sense)
{
  double normal[3];
  vtkTriangle::ComputeNormal(pt1, pt2, pt3, normal);

  // vector from p1 to p2
  double v12[3];
  v12[0] = p2[0] - p1[0];
  v12[1] = p2[1] - p1[1];
  v12[2] = p2[2] - p1[2];

  // vector from p1 to triangle
  double v1t[3];
  v1t[0] = pt1[0] - p1[0];
  v1t[1] = pt1[1] - p1[1];
  v1t[2] = pt1[2] - p1[2];

  // compute numerator/denominator of parametric distance
  double numerator = vtkMath::Dot(normal, v1t);
  double denominator = vtkMath::Dot(normal, v12);
  if (denominator == 0)
  {
    return 0;
  }

  // If denominator less than the tolerance, then the
  // line and plane are considered parallel.
  double fabsden = denominator;
  sense = -1;
  if (fabsden < 0.0)
  {
    sense = 1;
    fabsden = -fabsden;
  }
  if (fabsden > 1e-6 + tolerance)
  {
    // calculate the distance to the intersection along the line
    t = numerator / denominator;
    if (t < 0.0 || t > 1.0)
    {
      return 0;
    }

    // intersection point
    point[0] = p1[0] + t * v12[0];
    point[1] = p1[1] + t * v12[1];
    point[2] = p1[2] + t * v12[2];

    // find axis permutation to allow us to do the rest of the
    // math in 2D (much more efficient than doing the math in 3D)
    int xi = 0, yi = 1, zi = 2;
    if (normal[0] * normal[0] < normal[1] * normal[1])
    {
      xi = 1;
      yi = 2;
      zi = 0;
    }
    if (normal[xi] * normal[xi] < normal[2] * normal[2])
    {
      xi = 2;
      yi = 0;
      zi = 1;
    }

    // calculate vector from triangle corner to point
    double u0 = point[yi] - pt1[yi];
    double v0 = point[zi] - pt1[zi];
    // calculate edge vectors for triangle
    double u1 = pt2[yi] - pt1[yi];
    double v1 = pt2[zi] - pt1[zi];
    double u2 = pt3[yi] - pt1[yi];
    double v2 = pt3[zi] - pt1[zi];

    // area of projected triangle (multiplied by 2) via cross product
    double area = (v2 * u1 - u2 * v1);

    // sub-areas that must sum to less than the total area
    double alpha = (v2 * u0 - u2 * v0);
    double beta = (v0 * u1 - u0 * v1);
    double gamma = area - alpha - beta;

    if (area < 0)
    {
      alpha = -alpha;
      beta = -beta;
      gamma = -gamma;
    }

    if (alpha > 0 && beta > 0 && gamma > 0)
    { // inside of polygon
      return 1;
    }
  }

  // if zero tolerance, nothing more that we can do!
  if (tolerance == 0)
  {
    return 0;
  }

  // check the edges of the triangle (because triangles share edges,
  // this check should be identical for adjacent triangles which is
  // a 'good thing'
  double tolsquared = tolerance * tolerance;

  // make sure that order of points in each line segment is the
  // same for faces pointed in opposite directions
  double* tpoints[4];
  if (sense > 0)
  {
    tpoints[0] = pt1;
    tpoints[1] = pt2;
    tpoints[2] = pt3;
    tpoints[3] = pt1;
  }
  else
  {
    tpoints[0] = pt3;
    tpoints[1] = pt2;
    tpoints[2] = pt1;
    tpoints[3] = pt3;
  }

  double vec[3];
  double v;
  for (int i = 0; i < 3; i++)
  {
    pt1 = tpoints[i];
    pt2 = tpoints[i + 1];

    if (vtkLine::Intersection(p1, p2, pt1, pt2, t, v) == vtkLine::Intersect)
    {
      vec[0] = (p1[0] + v12[0] * t) - (pt1[0] + (pt2[0] - pt1[0]) * v);
      vec[1] = (p1[1] + v12[1] * t) - (pt1[1] + (pt2[1] - pt1[1]) * v);
      vec[2] = (p1[2] + v12[2] * t) - (pt1[2] + (pt2[2] - pt1[2]) * v);

      if (vtkMath::Dot(vec, vec) < tolsquared)
      {
        return 1;
      }
    }
  }

  return 0;
}

//------------------------------------------------------------------------------
// just check whether a point lies inside or outside the DataSet,
// assuming that the data is a closed vtkPolyData surface.
int vtkOBBTree::InsideOrOutside(const double point[3])
{
  // no points!
  // shoot a ray that is guaranteed to hit one of the cells and use
  // that as our inside/outside check
  vtkIdType numCells = this->DataSet->GetNumberOfCells();
  for (vtkIdType i = 0; i < numCells; i++)
  {
    vtkIdType numPts;
    const vtkIdType* ptIds;
    int cellType = this->DataSet->GetCellType(i);
    ((vtkPolyData*)this->DataSet)->GetCellPoints(i, numPts, ptIds);

    // break the cell into triangles
    for (vtkIdType j = 0; j < numPts - 2; j++)
    {
      vtkIdType pt1Id, pt2Id, pt3Id;
      vtkCELLTRIANGLES(ptIds, cellType, j, pt1Id, pt2Id, pt3Id);
      if (pt1Id < 0)
      { // cell wasn't a polygon, triangle, quad, or triangle strip
        continue;
      }
      // create a point that is guaranteed to be inside the cell
      double pt1[3], pt2[3], pt3[3];
      this->DataSet->GetPoint(pt1Id, pt1);
      this->DataSet->GetPoint(pt2Id, pt2);
      this->DataSet->GetPoint(pt3Id, pt3);

      double x[3];
      x[0] = (pt1[0] + pt2[0] + pt3[0]) / 3;
      x[1] = (pt1[1] + pt2[1] + pt3[1]) / 3;
      x[2] = (pt1[2] + pt2[2] + pt3[2]) / 3;
      // make a line guaranteed to pass through the cell's first triangle
      x[0] += x[0] - point[0];
      x[1] += x[1] - point[1];
      x[2] += x[2] - point[2];

      // calculate vector
      double v12[3];
      v12[0] = x[0] - point[0];
      v12[1] = x[1] - point[1];
      v12[2] = x[2] - point[2];

      // get triangle normal, we need a triangle for whose face is
      // not parallel to the line
      double normal[3];
      vtkTriangle::ComputeNormal(pt1, pt2, pt3, normal);
      double dotProd = vtkMath::Dot(normal, v12);
      if (dotProd < 0)
      {
        dotProd = -dotProd;
      }
      if (dotProd >= this->Tolerance + 1e-6)
      {
        return this->IntersectWithLine(point, x, nullptr, nullptr);
      }
      // otherwise go on to next triangle
    }
  }
  return 0;
}

//------------------------------------------------------------------------------
// Take the passed line segment and intersect it with the OBB cells.
// This method assumes that the data set is a vtkPolyData that describes
// a closed surface, and the intersection points that are returned in
// 'points' alternate between entrance points and exit points.
// The return value of the function is 0 if no intersection was found,
// 1 if point 'p1' lies inside the polydata surface, or -1 if point 'p1'
// lies outside the polydata surface.
int vtkOBBTree::IntersectWithLine(
  const double p1[3], const double p2[3], vtkPoints* points, vtkIdList* cellIds)
{
  if (this->DataSet == nullptr)
  {
    if (points)
    {
      points->SetNumberOfPoints(0);
    }
    if (cellIds)
    {
      cellIds->SetNumberOfIds(0);
    }
    return 0;
  }
  if (!this->DataSet->IsA("vtkPolyData"))
  {
    vtkErrorMacro("IntersectWithLine: this method requires a vtkPolyData");
    return 0;
  }

  int rval = 0; // return value for function
  vtkIdList* cells;

  // temporary list used to sort intersections
  int listSize = 0;
  int listMaxSize = 10;
  double* distanceList = new double[listMaxSize];
  vtkIdType* cellList = new vtkIdType[listMaxSize];
  char* senseList = new char[listMaxSize];

  double point[3];
  double distance = 0;
  int sense = 0;
  vtkIdType cellId;

  // compute line vector from p1 to p2
  double v12[3];
  v12[0] = p2[0] - p1[0];
  v12[1] = p2[1] - p1[1];
  v12[2] = p2[2] - p1[2];

  vtkOBBNode** OBBstack = new vtkOBBNode*[this->GetLevel() + 1];
  OBBstack[0] = this->Tree;

  // depth counter for stack
  int depth = 1;
  while (depth > 0)
  { // simulate recursion without the overhead or limitations
    vtkOBBNode* node = OBBstack[--depth];

    // check for intersection with node
    if (this->LineIntersectsNode(node, p1, p2))
    {
      if (node->Kids == nullptr)
      { // then this is a leaf node...get Cells
        cells = node->Cells;
        vtkIdType numCells = cells->GetNumberOfIds();
        for (vtkIdType i = 0; i < numCells; i++)
        {
          // get the current cell
          cellId = cells->GetId(i);
          int cellType = this->DataSet->GetCellType(cellId);
          vtkIdType numPts;
          const vtkIdType* ptIds;
          ((vtkPolyData*)this->DataSet)->GetCellPoints(cellId, numPts, ptIds);

          // break the cell into triangles
          for (vtkIdType j = 0; j < numPts - 2; j++)
          {
            vtkIdType pt1Id, pt2Id, pt3Id;
            vtkCELLTRIANGLES(ptIds, cellType, j, pt1Id, pt2Id, pt3Id);
            if (pt1Id < 0)
            { // cell wasn't a polygon, triangle, quad, or triangle strip
              continue;
            }

            // get the points for this triangle
            double pt1[3], pt2[3], pt3[3];
            this->DataSet->GetPoint(pt1Id, pt1);
            this->DataSet->GetPoint(pt2Id, pt2);
            this->DataSet->GetPoint(pt3Id, pt3);

            if (vtkOBBTreeLineIntersectsTriangle(
                  p1, p2, pt1, pt2, pt3, this->Tolerance, point, distance, sense) <= 0)
            { // no intersection with triangle
              continue;
            }

            // we made it! we have a hit!

            if (listSize >= listMaxSize)
            { // have to grow the distanceList
              listMaxSize *= 2;
              double* tmpDistanceList = new double[listMaxSize];
              vtkIdType* tmpCellList = new vtkIdType[listMaxSize];
              char* tmpSenseList = new char[listMaxSize];
              for (int k = 0; k < listSize; k++)
              {
                tmpDistanceList[k] = distanceList[k];
                tmpCellList[k] = cellList[k];
                tmpSenseList[k] = senseList[k];
              }
              delete[] distanceList;
              distanceList = tmpDistanceList;
              delete[] cellList;
              cellList = tmpCellList;
              delete[] senseList;
              senseList = tmpSenseList;
            }
            // store in the distanceList
            distanceList[listSize] = distance;
            cellList[listSize] = cellId;
            senseList[listSize++] = sense;

            // if cell is planar (i.e. not a triangle strip) then proceed
            // immediately to the next cell, otherwise go to next triangle
            if (cellType != VTK_TRIANGLE_STRIP)
            {
              break;
            }
          }
        }
      }
      else
      { // push kids onto stack
        OBBstack[depth] = node->Kids[0];
        OBBstack[depth + 1] = node->Kids[1];
        depth += 2;
      }
    }
  } // end while

  if (listSize != 0)
  {
    // Look at the distanceList and return the intersection point
    // sorted according to their distance from p1.

    if (points)
    {
      points->SetNumberOfPoints(listSize);
    }
    if (cellIds)
    {
      cellIds->SetNumberOfIds(0);
    }
    double ptol = this->Tolerance / sqrt(vtkMath::Dot(v12, v12));
    double lastDistance = 0.0;
    int lastSense = 0;
    int nPoints = 0;
    int listRemainder = listSize;
    while (listRemainder)
    {
      int minIdx = 0;
      for (int j = 1; j < listRemainder; j++)
      { // check for closest intersection of the correct sense
        if (senseList[j] != lastSense && distanceList[j] < distanceList[minIdx])
        {
          minIdx = j;
        }
      }

      distance = distanceList[minIdx];
      cellId = cellList[minIdx];
      sense = senseList[minIdx];
      listRemainder--;
      distanceList[minIdx] = distanceList[listRemainder];
      cellList[minIdx] = cellList[listRemainder];
      senseList[minIdx] = senseList[listRemainder];

      // only use point if it moves us forward,
      // or it moves us backward by less than tol
      if (distance > lastDistance - ptol && sense != lastSense)
      {
        if (points)
        {
          point[0] = p1[0] + distance * v12[0];
          point[1] = p1[1] + distance * v12[1];
          point[2] = p1[2] + distance * v12[2];
          points->SetPoint(nPoints, point);
        }
        if (cellIds)
        {
          cellIds->InsertNextId(cellId);
        }
        nPoints++;

        // set return value according to sense of first intersection
        if (rval == 0)
        {
          rval = sense;
        }
        // remember the last point
        lastDistance = distance;
        lastSense = sense;
      }
    }
    // shrink points array if not all points were used
    if (nPoints < listSize)
    {
      if (points)
      {
        points->GetData()->Resize(nPoints);
      }
    }
    // done!
  }
  else
  {
    if (points)
    {
      points->SetNumberOfPoints(0);
    }
    if (cellIds)
    {
      cellIds->SetNumberOfIds(0);
    }
  }

  delete[] senseList;
  delete[] cellList;
  delete[] distanceList;
  delete[] OBBstack;
  // return 1 if p1 is inside, 0 is p1 is outside
  return rval;
}

//------------------------------------------------------------------------------
// Return intersection point (if any) AND the cell which was intersected by
// finite line
int vtkOBBTree::IntersectWithLine(const double a0[3], const double a1[3], double tol, double& t,
  double x[3], double pcoords[3], int& subId, vtkIdType& cellId, vtkGenericCell* cell)
{
  double tBest = VTK_DOUBLE_MAX, xBest[3] = {0., 0., 0,}, pcoordsBest[3] = {0., 0., 0.};
  int subIdBest = -1;
  vtkIdType cellIdBest = -1;

  std::vector<vtkOBBNode*> OBBstack(this->GetLevel() + 1);
  OBBstack[0] = this->Tree;
  int depth = 1;
  while (depth > 0)
  { // simulate recursion without the overhead or limitations
    depth--;
    vtkOBBNode* node = OBBstack[depth];
    if (this->LineIntersectsNode(node, a0, a1))
    {
      if (node->Kids == nullptr)
      { // then this is a leaf node...get Cells
        for (int ii = 0; ii < node->Cells->GetNumberOfIds(); ii++)
        {
          vtkIdType thisId = node->Cells->GetId(ii);
          this->DataSet->GetCell(thisId, cell);
          if (cell->IntersectWithLine(a0, a1, tol, t, x, pcoords, subId))
          { // line intersects cell, but is it the best one?
            if (t < tBest)
            { // Yes, it's the best.
              tBest = t;
              xBest[0] = x[0];
              xBest[1] = x[1];
              xBest[2] = x[2];
              pcoordsBest[0] = pcoords[0];
              pcoordsBest[1] = pcoords[1];
              pcoordsBest[2] = pcoords[2];
              subIdBest = subId;
              cellIdBest = thisId;
            }
          }
        }
      }
      else
      { // push kids onto stack
        OBBstack[depth] = node->Kids[0];
        OBBstack[depth + 1] = node->Kids[1];
        depth += 2;
      }
    }
  } // end while

  // If a cell has been intersected, recover the information and return.
  if (cellIdBest >= 0)
  {
    this->DataSet->GetCell(cellIdBest, cell);
    t = tBest;
    x[0] = xBest[0];
    x[1] = xBest[1];
    x[2] = xBest[2];
    pcoords[0] = pcoordsBest[0];
    pcoords[1] = pcoordsBest[1];
    pcoords[2] = pcoordsBest[2];
    subId = subIdBest;
    cellId = cellIdBest;
    return 1;
  }
  return 0;
}

//------------------------------------------------------------------------------
void vtkOBBNode::DebugPrintTree(int level, double* leaf_vol, int* minCells, int* maxCells)
{
  double xp[3], volume, c[3];
  int i;
  vtkIdType nCells;

  if (this->Cells != nullptr)
  {
    nCells = this->Cells->GetNumberOfIds();
  }
  else
  {
    nCells = 0;
  }

  vtkMath::Cross(this->Axes[0], this->Axes[1], xp);
  volume = fabs(vtkMath::Dot(xp, this->Axes[2]));
  for (i = 0; i < 3; i++)
  {
    c[i] =
      this->Corner[i] + 0.5 * this->Axes[0][i] + 0.5 * this->Axes[1][i] + 0.5 * this->Axes[2][i];
  }

  for (i = 0; i < level; i++)
  {
    cout << "  ";
  }
  cout << level << " # Cells: " << nCells << ", Volume: " << volume << "\n";
  for (i = 0; i < level; i++)
  {
    cout << "  ";
  }
  cout << "    " << vtkMath::Norm(this->Axes[0]) << " X " << vtkMath::Norm(this->Axes[1]) << " X "
       << vtkMath::Norm(this->Axes[2]) << "\n";
  for (i = 0; i < level; i++)
  {
    cout << "  ";
  }
  cout << "    Center: " << c[0] << " " << c[1] << " " << c[2] << "\n";
  if (nCells != 0)
  {
    *leaf_vol += volume;
    if (nCells < *minCells)
    {
      *minCells = nCells;
    }
    if (nCells > *maxCells)
    {
      *maxCells = nCells;
    }
  }
  if (this->Kids != nullptr)
  {
    this->Kids[0]->DebugPrintTree(level + 1, leaf_vol, minCells, maxCells);
    this->Kids[1]->DebugPrintTree(level + 1, leaf_vol, minCells, maxCells);
  }
}

//------------------------------------------------------------------------------
void vtkOBBTree::BuildLocator()
{
  // don't rebuild if build time is newer than modified and dataset modified time
  if (this->Tree && this->BuildTime > this->MTime && this->BuildTime > this->DataSet->GetMTime())
  {
    return;
  }
  // don't rebuild if UseExistingSearchStructure is ON and a search structure already exists
  if (this->Tree && this->UseExistingSearchStructure)
  {
    this->BuildTime.Modified();
    vtkDebugMacro(<< "BuildLocator exited - UseExistingSearchStructure");
    return;
  }
  this->BuildLocatorInternal();
}

//------------------------------------------------------------------------------
void vtkOBBTree::ForceBuildLocator()
{
  this->BuildLocatorInternal();
}

//------------------------------------------------------------------------------
void vtkOBBTree::BuildLocatorInternal()
{
  vtkIdType numPts, numCells, i;
  vtkIdList* cellList;

  vtkDebugMacro(<< "Building OBB tree");

  numPts = this->DataSet->GetNumberOfPoints();
  numCells = this->DataSet->GetNumberOfCells();
  if (this->DataSet == nullptr || numPts < 1 || numCells < 1)
  {
    vtkErrorMacro(<< "Can't build OBB tree - no data available!");
    return;
  }

  this->OBBCount = 0;
  this->InsertedPoints = new int[numPts];
  for (i = 0; i < numPts; i++)
  {
    this->InsertedPoints[i] = 0;
  }
  this->PointsList = vtkPoints::New();
  this->PointsList->Allocate(numPts);

  //
  // Begin recursively creating OBB's
  //
  cellList = vtkIdList::New();
  cellList->Allocate(numCells);
  for (i = 0; i < numCells; i++)
  {
    cellList->InsertId(i, i);
  }

  this->FreeSearchStructure();

  this->Tree = new vtkOBBNode;
  this->Level = 0;
  this->BuildTree(cellList, this->Tree, 0);

  vtkDebugMacro(<< "# Cells: " << numCells << ", Deepest tree level: " << this->Level
                << ", Created: " << this->OBBCount << " OBB nodes");
  if (this->GetDebug())
  { // print tree
    double volume = 0.0;
    int minCells = 65535, maxCells = 0;
    this->Tree->DebugPrintTree(0, &volume, &minCells, &maxCells);
    cout << "Total leafnode volume = " << volume << "\n";
    cout << "Min leaf cells: " << minCells << ", Max leaf cells: " << maxCells << "\n";
    cout.flush();
  }

  //
  // Clean up
  //
  delete[] this->InsertedPoints;
  this->PointsList->Delete();

  this->BuildTime.Modified();
}

//------------------------------------------------------------------------------
// NOTE: for better memory usage this recursive method
// frees its first argument
void vtkOBBTree::BuildTree(vtkIdList* cells, vtkOBBNode* OBBptr, int level)
{
  vtkIdType i, j, numCells = cells->GetNumberOfIds();
  vtkIdType cellId;
  vtkIdType ptId;
  vtkIdList* cellPts = vtkIdList::New();
  double size[3];

  if (level > this->Level)
  {
    this->Level = level;
  }
  //
  // Now compute the OBB
  //
  this->ComputeOBB(cells, OBBptr->Corner, OBBptr->Axes[0], OBBptr->Axes[1], OBBptr->Axes[2], size);

  //
  // Check whether to continue recursing; if so, create two children and
  // assign cells to appropriate child.
  //
  if (level < this->MaxLevel && numCells > this->NumberOfCellsPerNode)
  {
    vtkIdList* LHlist = vtkIdList::New();
    LHlist->Allocate(cells->GetNumberOfIds() / 2);
    vtkIdList* RHlist = vtkIdList::New();
    RHlist->Allocate(cells->GetNumberOfIds() / 2);
    double n[3], p[3], c[3], x[3], val, ratio, bestRatio;
    int negative, positive, splitAcceptable, splitPlane;
    int foundBestSplit, bestPlane = 0, numPts;
    int numInLHnode, numInRHnode;

    // loop over three split planes to find acceptable one
    for (i = 0; i < 3; i++) // compute split point
    {
      p[i] = OBBptr->Corner[i] + OBBptr->Axes[0][i] / 2.0 + OBBptr->Axes[1][i] / 2.0 +
        OBBptr->Axes[2][i] / 2.0;
    }

    bestRatio = 1.0; // worst case ratio
    foundBestSplit = 0;
    for (splitPlane = 0, splitAcceptable = 0; !splitAcceptable && splitPlane < 3;)
    {
      // compute split normal
      for (i = 0; i < 3; i++)
      {
        n[i] = OBBptr->Axes[splitPlane][i];
      }
      vtkMath::Normalize(n);

      // traverse cells, assigning to appropriate child list as necessary
      for (i = 0; i < numCells; i++)
      {
        cellId = cells->GetId(i);
        this->DataSet->GetCellPoints(cellId, cellPts);
        c[0] = c[1] = c[2] = 0.0;
        numPts = cellPts->GetNumberOfIds();
        for (negative = positive = j = 0; j < numPts; j++)
        {
          ptId = cellPts->GetId(j);
          this->DataSet->GetPoint(ptId, x);
          val = n[0] * (x[0] - p[0]) + n[1] * (x[1] - p[1]) + n[2] * (x[2] - p[2]);
          c[0] += x[0];
          c[1] += x[1];
          c[2] += x[2];
          if (val < 0.0)
          {
            negative = 1;
          }
          else
          {
            positive = 1;
          }
        }

        if (negative && positive)
        { // Use centroid to decide straddle cases
          c[0] /= numPts;
          c[1] /= numPts;
          c[2] /= numPts;
          if (n[0] * (c[0] - p[0]) + n[1] * (c[1] - p[1]) + n[2] * (c[2] - p[2]) < 0.0)
          {
            LHlist->InsertNextId(cellId);
          }
          else
          {
            RHlist->InsertNextId(cellId);
          }
        }
        else
        {
          if (negative)
          {
            LHlist->InsertNextId(cellId);
          }
          else
          {
            RHlist->InsertNextId(cellId);
          }
        }
      } // for all cells

      // evaluate this split
      numInLHnode = LHlist->GetNumberOfIds();
      numInRHnode = RHlist->GetNumberOfIds();
      ratio = fabs(((double)numInRHnode - numInLHnode) / numCells);

      // see whether we've found acceptable split plane
      if (ratio < 0.6 || foundBestSplit) // accept right off the bat
      {
        splitAcceptable = 1;
      }
      else
      { // not a great split try another
        LHlist->Reset();
        RHlist->Reset();
        if (ratio < bestRatio)
        {
          bestRatio = ratio;
          bestPlane = splitPlane;
        }
        if (++splitPlane == 3 && bestRatio < 0.95)
        { // at closing time, even the ugly ones look good
          splitPlane = bestPlane;
          foundBestSplit = 1;
        }
      } // try another split

    } // for each split

    if (splitAcceptable) // otherwise recursion terminates
    {
      vtkOBBNode* LHnode = new vtkOBBNode;
      vtkOBBNode* RHnode = new vtkOBBNode;
      OBBptr->Kids = new vtkOBBNode*[2];
      OBBptr->Kids[0] = LHnode;
      OBBptr->Kids[1] = RHnode;
      LHnode->Parent = OBBptr;
      RHnode->Parent = OBBptr;

      cells->Delete();
      cells = nullptr; // don't need to keep anymore
      this->BuildTree(LHlist, LHnode, level + 1);
      this->BuildTree(RHlist, RHnode, level + 1);
    }
    else
    {
      // free up local objects
      LHlist->Delete();
      RHlist->Delete();
    }
  } // if should build tree

  if (cells && this->RetainCellLists)
  {
    cells->Squeeze();
    OBBptr->Cells = cells;
  }
  else if (cells)
  {
    cells->Delete();
  }
  cellPts->Delete();
}

//------------------------------------------------------------------------------
// Create polygonal representation for OBB tree at specified level. If
// level < 0, then the leaf OBB nodes will be gathered. The aspect ratio (ar)
// and line diameter (d) are used to control the building of the
// representation. If a OBB node edge ratio's are greater than ar, then the
// dimension of the OBB is collapsed (OBB->plane->line). A "line" OBB will be
// represented either as two crossed polygons, or as a line, depending on
// the relative diameter of the OBB compared to the diameter (d).
void vtkOBBTree::GenerateRepresentation(int level, vtkPolyData* pd)
{
  this->BuildLocator();
  if (this->Tree == nullptr)
  {
    vtkErrorMacro(<< "Empty tree, most likely there are no cells in the input data set");
    return;
  }

  vtkPoints* pts;
  vtkCellArray* polys;

  pts = vtkPoints::New();
  pts->Allocate(5000);
  polys = vtkCellArray::New();
  polys->AllocateEstimate(5000, 1);
  this->GeneratePolygons(this->Tree, 0, level, pts, polys);

  pd->SetPoints(pts);
  pts->Delete();
  pd->SetPolys(polys);
  polys->Delete();
  pd->Squeeze();
}

//------------------------------------------------------------------------------
void vtkOBBTree::GeneratePolygons(
  vtkOBBNode* OBBptr, int level, int repLevel, vtkPoints* pts, vtkCellArray* polys)

{
  if (level == repLevel || (repLevel < 0 && OBBptr->Kids == nullptr))
  {
    double x[3];
    vtkIdType cubeIds[8];
    vtkIdType ptIds[4];

    x[0] = OBBptr->Corner[0];
    x[1] = OBBptr->Corner[1];
    x[2] = OBBptr->Corner[2];
    cubeIds[0] = pts->InsertNextPoint(x);

    x[0] = OBBptr->Corner[0] + OBBptr->Axes[0][0];
    x[1] = OBBptr->Corner[1] + OBBptr->Axes[0][1];
    x[2] = OBBptr->Corner[2] + OBBptr->Axes[0][2];
    cubeIds[1] = pts->InsertNextPoint(x);

    x[0] = OBBptr->Corner[0] + OBBptr->Axes[1][0];
    x[1] = OBBptr->Corner[1] + OBBptr->Axes[1][1];
    x[2] = OBBptr->Corner[2] + OBBptr->Axes[1][2];
    cubeIds[2] = pts->InsertNextPoint(x);

    x[0] = OBBptr->Corner[0] + OBBptr->Axes[0][0] + OBBptr->Axes[1][0];
    x[1] = OBBptr->Corner[1] + OBBptr->Axes[0][1] + OBBptr->Axes[1][1];
    x[2] = OBBptr->Corner[2] + OBBptr->Axes[0][2] + OBBptr->Axes[1][2];
    cubeIds[3] = pts->InsertNextPoint(x);

    x[0] = OBBptr->Corner[0] + OBBptr->Axes[2][0];
    x[1] = OBBptr->Corner[1] + OBBptr->Axes[2][1];
    x[2] = OBBptr->Corner[2] + OBBptr->Axes[2][2];
    cubeIds[4] = pts->InsertNextPoint(x);

    x[0] = OBBptr->Corner[0] + OBBptr->Axes[0][0] + OBBptr->Axes[2][0];
    x[1] = OBBptr->Corner[1] + OBBptr->Axes[0][1] + OBBptr->Axes[2][1];
    x[2] = OBBptr->Corner[2] + OBBptr->Axes[0][2] + OBBptr->Axes[2][2];
    cubeIds[5] = pts->InsertNextPoint(x);

    x[0] = OBBptr->Corner[0] + OBBptr->Axes[1][0] + OBBptr->Axes[2][0];
    x[1] = OBBptr->Corner[1] + OBBptr->Axes[1][1] + OBBptr->Axes[2][1];
    x[2] = OBBptr->Corner[2] + OBBptr->Axes[1][2] + OBBptr->Axes[2][2];
    cubeIds[6] = pts->InsertNextPoint(x);

    x[0] = OBBptr->Corner[0] + OBBptr->Axes[0][0] + OBBptr->Axes[1][0] + OBBptr->Axes[2][0];
    x[1] = OBBptr->Corner[1] + OBBptr->Axes[0][1] + OBBptr->Axes[1][1] + OBBptr->Axes[2][1];
    x[2] = OBBptr->Corner[2] + OBBptr->Axes[0][2] + OBBptr->Axes[1][2] + OBBptr->Axes[2][2];
    cubeIds[7] = pts->InsertNextPoint(x);

    ptIds[0] = cubeIds[0];
    ptIds[1] = cubeIds[2];
    ptIds[2] = cubeIds[3];
    ptIds[3] = cubeIds[1];
    polys->InsertNextCell(4, ptIds);

    ptIds[0] = cubeIds[0];
    ptIds[1] = cubeIds[1];
    ptIds[2] = cubeIds[5];
    ptIds[3] = cubeIds[4];
    polys->InsertNextCell(4, ptIds);

    ptIds[0] = cubeIds[0];
    ptIds[1] = cubeIds[4];
    ptIds[2] = cubeIds[6];
    ptIds[3] = cubeIds[2];
    polys->InsertNextCell(4, ptIds);

    ptIds[0] = cubeIds[1];
    ptIds[1] = cubeIds[3];
    ptIds[2] = cubeIds[7];
    ptIds[3] = cubeIds[5];
    polys->InsertNextCell(4, ptIds);

    ptIds[0] = cubeIds[4];
    ptIds[1] = cubeIds[5];
    ptIds[2] = cubeIds[7];
    ptIds[3] = cubeIds[6];
    polys->InsertNextCell(4, ptIds);

    ptIds[0] = cubeIds[2];
    ptIds[1] = cubeIds[6];
    ptIds[2] = cubeIds[7];
    ptIds[3] = cubeIds[3];
    polys->InsertNextCell(4, ptIds);
  }

  else if ((level < repLevel || repLevel < 0) && OBBptr->Kids != nullptr)
  {
    this->GeneratePolygons(OBBptr->Kids[0], level + 1, repLevel, pts, polys);
    this->GeneratePolygons(OBBptr->Kids[1], level + 1, repLevel, pts, polys);
  }
}

//------------------------------------------------------------------------------
int vtkOBBTree::DisjointOBBNodes(vtkOBBNode* nodeA, vtkOBBNode* nodeB, vtkMatrix4x4* XformBtoA)
{
  if (nodeA == nullptr || nodeB == nullptr)
  {
    return 5; // A and B are Disjoint.
  }

  vtkOBBNode nodeBxformed, *pB, *pA;
  double centerA[3], centerB[3], AtoB[3], in[4], out[4];
  double rangeAmin, rangeAmax, rangeBmin, rangeBmax, dotA, dotB, dotAB[3][3];
  double eps;
  int ii, jj, kk;

  eps = this->Tolerance;
  pA = nodeA;
  if (XformBtoA != nullptr)
  { // Here we assume that XformBtoA is an orthogonal matrix
    pB = &nodeBxformed;
    in[0] = nodeB->Corner[0];
    in[1] = nodeB->Corner[1];
    in[2] = nodeB->Corner[2];
    in[3] = 1.0;
    XformBtoA->MultiplyPoint(in, out);
    pB->Corner[0] = out[0] / out[3];
    pB->Corner[1] = out[1] / out[3];
    pB->Corner[2] = out[2] / out[3];
    // Clean this up when the bug in MultiplyVectors is fixed!
    for (ii = 0; ii < 3; ii++)
    {
      pB->Axes[0][ii] = nodeB->Corner[ii] + nodeB->Axes[0][ii];
      pB->Axes[1][ii] = nodeB->Corner[ii] + nodeB->Axes[1][ii];
      pB->Axes[2][ii] = nodeB->Corner[ii] + nodeB->Axes[2][ii];
    }
    for (ii = 0; ii < 3; ii++)
    {
      in[0] = pB->Axes[ii][0];
      in[1] = pB->Axes[ii][1];
      in[2] = pB->Axes[ii][2];
      in[3] = 1.0;
      XformBtoA->MultiplyPoint(in, out);
      pB->Axes[ii][0] = out[0] / out[3];
      pB->Axes[ii][1] = out[1] / out[3];
      pB->Axes[ii][2] = out[2] / out[3];
    }
    for (ii = 0; ii < 3; ii++)
    {
      pB->Axes[0][ii] = pB->Axes[0][ii] - pB->Corner[ii];
      pB->Axes[1][ii] = pB->Axes[1][ii] - pB->Corner[ii];
      pB->Axes[2][ii] = pB->Axes[2][ii] - pB->Corner[ii];
    }
  }
  else
  {
    pB = nodeB;
  }
  for (ii = 0; ii < 3; ii++)
  {
    centerA[ii] = pA->Corner[ii] + 0.5 * (pA->Axes[0][ii] + pA->Axes[1][ii] + pA->Axes[2][ii]);
    centerB[ii] = pB->Corner[ii] + 0.5 * (pB->Axes[0][ii] + pB->Axes[1][ii] + pB->Axes[2][ii]);
    AtoB[ii] = centerB[ii] - centerA[ii];
  }

  // Project maximal and minimal corners onto line between centers
  rangeAmin = rangeAmax = vtkMath::Dot(pA->Corner, AtoB);
  rangeBmin = rangeBmax = vtkMath::Dot(pB->Corner, AtoB);
  for (ii = 0; ii < 3; ii++)
  {
    // compute A range
    dotA = vtkMath::Dot(pA->Axes[ii], AtoB);
    if (dotA > 0)
    {
      rangeAmax += dotA;
    }
    else
    {
      rangeAmin += dotA;
    }

    // compute B range
    dotB = vtkMath::Dot(pB->Axes[ii], AtoB);
    if (dotB > 0)
    {
      rangeBmax += dotB;
    }
    else
    {
      rangeBmin += dotB;
    }
  }
  if ((rangeAmax + eps < rangeBmin) || (rangeBmax + eps < rangeAmin))
  {
    return (1); // A and B are Disjoint by the 1st test.
  }

  // now check for a separation plane parallel to the faces of B
  for (ii = 0; ii < 3; ii++)
  { // plane is normal to pB->Axes[ii]
    // computing B range is easy...
    rangeBmin = rangeBmax = vtkMath::Dot(pB->Corner, pB->Axes[ii]);
    rangeBmax += vtkMath::Dot(pB->Axes[ii], pB->Axes[ii]);

    // compute A range...
    rangeAmin = rangeAmax = vtkMath::Dot(pA->Corner, pB->Axes[ii]);
    for (jj = 0; jj < 3; jj++)
    {
      // (note: we are saving all 9 dotproducts for future use)
      dotA = dotAB[ii][jj] = vtkMath::Dot(pB->Axes[ii], pA->Axes[jj]);
      if (dotA > 0)
      {
        rangeAmax += dotA;
      }
      else
      {
        rangeAmin += dotA;
      }
    }
    if ((rangeAmax + eps < rangeBmin) || (rangeBmax + eps < rangeAmin))
    {
      return (2); // A and B are Disjoint by the 3rd test.
    }
  }

  // now check for a separation plane parallel to the faces of A
  for (ii = 0; ii < 3; ii++)
  { // plane is normal to pA->Axes[ii]
    // computing A range is easy...
    rangeAmin = rangeAmax = vtkMath::Dot(pA->Corner, pA->Axes[ii]);
    rangeAmax += vtkMath::Dot(pA->Axes[ii], pA->Axes[ii]);

    // compute B range...
    rangeBmin = rangeBmax = vtkMath::Dot(pB->Corner, pA->Axes[ii]);
    for (jj = 0; jj < 3; jj++)
    {
      // (note: we are using the 9 dotproducts computed earlier)
      dotB = dotAB[jj][ii];
      if (dotB > 0)
      {
        rangeBmax += dotB;
      }
      else
      {
        rangeBmin += dotB;
      }
    }
    if ((rangeAmax + eps < rangeBmin) || (rangeBmax + eps < rangeAmin))
    {
      return (3); // A and B are Disjoint by the 2nd test.
    }
  }

  // Bad luck: now we must look for a separation plane parallel
  // to one edge from A and one edge from B.
  for (ii = 0; ii < 3; ii++)
  {
    for (jj = 0; jj < 3; jj++)
    {
      // the plane is normal to pA->Axes[ii] X pB->Axes[jj]
      vtkMath::Cross(pA->Axes[ii], pB->Axes[jj], AtoB);
      rangeAmin = rangeAmax = vtkMath::Dot(pA->Corner, AtoB);
      rangeBmin = rangeBmax = vtkMath::Dot(pB->Corner, AtoB);
      for (kk = 0; kk < 3; kk++)
      {
        // compute A range
        dotA = vtkMath::Dot(pA->Axes[kk], AtoB);
        if (dotA > 0)
        {
          rangeAmax += dotA;
        }
        else
        {
          rangeAmin += dotA;
        }

        // compute B range
        dotB = vtkMath::Dot(pB->Axes[kk], AtoB);
        if (dotB > 0)
        {
          rangeBmax += dotB;
        }
        else
        {
          rangeBmin += dotB;
        }
      }
      if ((rangeAmax + eps < rangeBmin) || (rangeBmax + eps < rangeAmin))
      {
        return (4); // A and B are Disjoint by the 4th test.
      }
    }
  }
  // if we fall through to here, the OBB's overlap
  return (0);
}

//------------------------------------------------------------------------------
int vtkOBBTree::TriangleIntersectsNode(
  vtkOBBNode* nodeA, double p0[3], double p1[3], double p2[3], vtkMatrix4x4* XformBtoA)
{
  vtkOBBNode* pA;
  double p0Xformed[3], p1Xformed[3], p2Xformed[3];
  double *pB[3], in[4], out[4], v0[3], v1[3], AtoB[3], xprod[3];
  double rangeAmin, rangeAmax, rangeBmin, rangeBmax, dotA, dotB;
  double eps;
  int ii, jj, kk;

  eps = this->Tolerance;
  pA = nodeA;
  if (XformBtoA != nullptr)
  { // Here we assume that XformBtoA is an orthogonal matrix
    pB[0] = p0Xformed;
    pB[1] = p1Xformed;
    pB[2] = p2Xformed;
    for (ii = 0; ii < 3; ii++)
    {
      p0Xformed[ii] = p0[ii];
      p1Xformed[ii] = p1[ii];
      p2Xformed[ii] = p2[ii];
    }
    for (ii = 0; ii < 3; ii++)
    {
      in[0] = pB[ii][0];
      in[1] = pB[ii][1];
      in[2] = pB[ii][2];
      in[3] = 1.0;
      XformBtoA->MultiplyPoint(in, out);
      pB[ii][0] = out[0] / out[3];
      pB[ii][1] = out[1] / out[3];
      pB[ii][2] = out[2] / out[3];
    }
  }
  else
  {
    pB[0] = p0;
    pB[1] = p1;
    pB[2] = p2;
  }

  // now check for a separation plane parallel to the triangle
  for (ii = 0; ii < 3; ii++)
  { // plane is normal to the triangle
    v0[ii] = pB[1][ii] - pB[0][ii];
    v1[ii] = pB[2][ii] - pB[0][ii];
  }
  vtkMath::Cross(v0, v1, xprod);
  // computing B range is easy...
  rangeBmin = rangeBmax = vtkMath::Dot(pB[0], xprod);
  // compute A range...
  rangeAmin = rangeAmax = vtkMath::Dot(pA->Corner, xprod);
  for (jj = 0; jj < 3; jj++)
  {
    dotA = vtkMath::Dot(xprod, pA->Axes[jj]);
    if (dotA > 0)
    {
      rangeAmax += dotA;
    }
    else
    {
      rangeAmin += dotA;
    }
  }
  if ((rangeAmax + eps < rangeBmin) || (rangeBmax + eps < rangeAmin))
  {
    return (0); // A and B are Disjoint by the 1st test.
  }

  // now check for a separation plane parallel to the faces of A
  for (ii = 0; ii < 3; ii++)
  { // plane is normal to pA->Axes[ii]
    // computing A range is easy...
    rangeAmin = rangeAmax = vtkMath::Dot(pA->Corner, pA->Axes[ii]);
    rangeAmax += vtkMath::Dot(pA->Axes[ii], pA->Axes[ii]);

    // compute B range...
    rangeBmin = rangeBmax = vtkMath::Dot(pB[0], pA->Axes[ii]);
    dotB = vtkMath::Dot(pB[1], pA->Axes[ii]);
    if (dotB > rangeBmax)
    {
      rangeBmax = dotB;
    }
    else
    {
      rangeBmin = dotB;
    }

    dotB = vtkMath::Dot(pB[2], pA->Axes[ii]);
    if (dotB > rangeBmax)
    {
      rangeBmax = dotB;
    }
    else if (dotB < rangeBmin)
    {
      rangeBmin = dotB;
    }

    if ((rangeAmax + eps < rangeBmin) || (rangeBmax + eps < rangeAmin))
    {
      return (0); // A and B are Disjoint by the 2nd test.
    }
  }

  // Bad luck: now we must look for a separation plane parallel
  // to one edge from A and one edge from B.
  for (ii = 0; ii < 3; ii++)
  {
    for (jj = 0; jj < 3; jj++)
    {
      // the plane is normal to pA->Axes[ii] X (pB[jj+1]-pB[jj])
      v0[0] = pB[(jj + 1) % 3][0] - pB[jj][0];
      v0[1] = pB[(jj + 1) % 3][1] - pB[jj][1];
      v0[2] = pB[(jj + 1) % 3][2] - pB[jj][2];
      vtkMath::Cross(pA->Axes[ii], v0, AtoB);
      rangeAmin = rangeAmax = vtkMath::Dot(pA->Corner, AtoB);
      rangeBmin = rangeBmax = vtkMath::Dot(pB[jj], AtoB);
      for (kk = 0; kk < 3; kk++)
      {
        // compute A range
        dotA = vtkMath::Dot(pA->Axes[kk], AtoB);
        if (dotA > 0)
        {
          rangeAmax += dotA;
        }
        else
        {
          rangeAmin += dotA;
        }
      }
      // compute B range
      dotB = vtkMath::Dot(pB[(jj + 2) % 3], AtoB);
      if (dotB > rangeBmax)
      {
        rangeBmax = dotB;
      }
      else
      {
        rangeBmin = dotB;
      }

      if ((rangeAmax + eps < rangeBmin) || (rangeBmax + eps < rangeAmin))
      {
        return (0); // A and B are Disjoint by the 3rd test.
      }
    }
  }

  // if we fall through to here, the OBB overlaps the triangle.
  return (1);
}

//------------------------------------------------------------------------------
// check if a line intersects the node: the line doesn't have to actually
// pass all the way through the node, but at least some portion of the line
// must lie within the node.
int vtkOBBTree::LineIntersectsNode(vtkOBBNode* pA, const double b0[3], const double b1[3])
{
  double rangeAmin, rangeAmax, rangeBmin, rangeBmax, dotB;
  double eps;
  int ii;

  for (ii = 0; ii < 3; ii++)
  {
    // computing A range is easy...
    rangeAmin = vtkMath::Dot(pA->Corner, pA->Axes[ii]);
    rangeAmax = rangeAmin + vtkMath::Dot(pA->Axes[ii], pA->Axes[ii]);

    // compute B range...
    rangeBmin = vtkMath::Dot(b0, pA->Axes[ii]);
    rangeBmax = rangeBmin;
    dotB = vtkMath::Dot(b1, pA->Axes[ii]);
    if (dotB < rangeBmin)
    {
      rangeBmin = dotB;
    }
    else
    {
      rangeBmax = dotB;
    }

    eps = this->Tolerance;
    if (eps != 0)
    { // avoid sqrt call if tolerance check isn't being done
      eps *= sqrt(fabs(rangeAmax - rangeAmin));
    }

    if ((rangeAmax + eps < rangeBmin) || (rangeBmax + eps < rangeAmin))
    {
      return (0);
    }
  }

  // if we fall through to here, the OBB overlaps the line segment.
  return (1);
}

//------------------------------------------------------------------------------
// Intersect this OBBTree with OBBTreeB (as transformed) and
// call processing function for each intersecting leaf node pair.
// If the processing function returns a negative integer, terminate.
int vtkOBBTree::IntersectWithOBBTree(vtkOBBTree* OBBTreeB, vtkMatrix4x4* XformBtoA,
  int (*function)(vtkOBBNode* nodeA, vtkOBBNode* nodeB, vtkMatrix4x4* Xform, void* arg),
  void* data_arg)
{
  int maxdepth, mindepth, depth, returnValue = 0, count = 0, maxStackDepth;
  vtkOBBNode **OBBstackA, **OBBstackB, *nodeA, *nodeB;

  // Intersect OBBs and process intersecting leaf nodes.
  maxdepth = this->GetLevel();
  if ((mindepth = OBBTreeB->GetLevel()) > maxdepth)
  {
    mindepth = maxdepth;
    maxdepth = OBBTreeB->GetLevel();
  }
  // Compute maximum theoretical recursion depth.
  maxStackDepth = 3 * mindepth + 2 * (maxdepth - mindepth) + 1;

  OBBstackA = new vtkOBBNode*[maxStackDepth];
  OBBstackB = new vtkOBBNode*[maxStackDepth];
  OBBstackA[0] = this->Tree;
  OBBstackB[0] = OBBTreeB->Tree;
  depth = 1;
  while (depth > 0 && returnValue > -1)
  { // simulate recursion without overhead of real recursion.
    depth--;
    nodeA = OBBstackA[depth];
    nodeB = OBBstackB[depth];
    if (!this->DisjointOBBNodes(nodeA, nodeB, XformBtoA))
    {
      if (nodeA->Kids == nullptr)
      {
        if (nodeB->Kids == nullptr)
        { // then this is a pair of intersecting leaf nodes to process
          returnValue = (*function)(nodeA, nodeB, XformBtoA, data_arg);
          if (returnValue >= 0)
          {
            count += returnValue;
          }
          else
          {
            count = returnValue;
          }
        }
        else
        { // A is a leaf, but B goes deeper.
          OBBstackA[depth] = nodeA;
          OBBstackB[depth] = nodeB->Kids[0];
          OBBstackA[depth + 1] = nodeA;
          OBBstackB[depth + 1] = nodeB->Kids[1];
          depth += 2;
        }
      }
      else
      {
        if (nodeB->Kids == nullptr)
        { // B is a leaf, but A goes deeper.
          OBBstackB[depth] = nodeB;
          OBBstackA[depth] = nodeA->Kids[0];
          OBBstackB[depth + 1] = nodeB;
          OBBstackA[depth + 1] = nodeA->Kids[1];
          depth += 2;
        }
        else
        { // neither A nor B are leaves. Go to the next level.
          OBBstackA[depth] = nodeA->Kids[0];
          OBBstackB[depth] = nodeB->Kids[0];
          OBBstackA[depth + 1] = nodeA->Kids[1];
          OBBstackB[depth + 1] = nodeB->Kids[0];
          OBBstackA[depth + 2] = nodeA->Kids[0];
          OBBstackB[depth + 2] = nodeB->Kids[1];
          OBBstackA[depth + 3] = nodeA->Kids[1];
          OBBstackB[depth + 3] = nodeB->Kids[1];
          depth += 4;
        }
      }
    }
  }
  // cleanup
  delete[] OBBstackA;
  delete[] OBBstackB;

  return (count);
}

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

  if (this->Tree)
  {
    os << indent << "Tree " << this->Tree << "\n";
  }
  else
  {
    os << indent << "Tree: (null)\n";
  }
  if (this->PointsList)
  {
    os << indent << "PointsList " << this->PointsList << "\n";
  }
  else
  {
    os << indent << "PointsList: (null)\n";
  }
  if (this->InsertedPoints)
  {
    os << indent << "InsertedPoints " << this->InsertedPoints << "\n";
  }
  else
  {
    os << indent << "InsertedPoints: (null)\n";
  }

  os << indent << "OBBCount " << this->OBBCount << "\n";
}
