vtkOBBTree.cxx 52.1 KB
Newer Older
Will Schroeder's avatar
Will Schroeder committed
1 2 3
/*=========================================================================

  Program:   Visualization Toolkit
Ken Martin's avatar
Ken Martin committed
4
  Module:    vtkOBBTree.cxx
Will Schroeder's avatar
Will Schroeder committed
5

6
  Copyright (c) Ken Martin, Will Schroeder, Bill Lorensen
7 8
  All rights reserved.
  See Copyright.txt or http://www.kitware.com/Copyright.htm for details.
Will Schroeder's avatar
Will Schroeder committed
9

10 11
     This software is distributed WITHOUT ANY WARRANTY; without even
     the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
12
     PURPOSE.  See the above copyright notice for more information.
Will Schroeder's avatar
Will Schroeder committed
13 14

=========================================================================*/
Ken Martin's avatar
Ken Martin committed
15
#include "vtkOBBTree.h"
16 17 18

#include "vtkCellArray.h"
#include "vtkGenericCell.h"
Ken Martin's avatar
Ken Martin committed
19
#include "vtkLine.h"
20
#include "vtkMath.h"
21
#include "vtkMatrix4x4.h"
22
#include "vtkObjectFactory.h"
23 24 25 26
#include "vtkPlane.h"
#include "vtkPolyData.h"
#include "vtkPolygon.h"
#include "vtkTriangle.h"
27
#include "vtkUnstructuredGrid.h"
28

29 30
#include <vector>

Brad King's avatar
Brad King committed
31
vtkStandardNewMacro(vtkOBBTree);
32

Will Schroeder's avatar
Will Schroeder committed
33
#define vtkCELLTRIANGLES(CELLPTIDS, TYPE, IDX, PTID0, PTID1, PTID2) \
34
        { switch( TYPE ) \
35
  { \
36 37 38 39 40 41 42 43 44 45 46 47 48 49
          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; \
50
  } }
Will Schroeder's avatar
Will Schroeder committed
51

Will Schroeder's avatar
Will Schroeder committed
52 53
vtkOBBNode::vtkOBBNode()
{
54 55 56
  this->Cells = nullptr;
  this->Parent = nullptr;
  this->Kids = nullptr;
Will Schroeder's avatar
Will Schroeder committed
57 58 59 60
}

vtkOBBNode::~vtkOBBNode()
{
61
  delete [] this->Kids;
62
  if (this->Cells)
63
  {
64
    this->Cells->Delete();
65
  }
Will Schroeder's avatar
Will Schroeder committed
66 67 68 69 70 71
}

// Construct with automatic computation of divisions, averaging
// 25 cells per octant.
vtkOBBTree::vtkOBBTree()
{
72
  this->DataSet = nullptr;
Will Schroeder's avatar
Will Schroeder committed
73 74 75 76
  this->Level = 4;
  this->MaxLevel = 12;
  this->Automatic = 1;
  this->Tolerance = 0.01;
77 78 79
  this->Tree = nullptr;
  this->PointsList = nullptr;
  this->InsertedPoints = nullptr;
80
  this->OBBCount = this->Level = 0;
Will Schroeder's avatar
Will Schroeder committed
81 82
}

Bill Lorensen's avatar
Bill Lorensen committed
83 84 85 86 87
vtkOBBTree::~vtkOBBTree()
{
  this->FreeSearchStructure();
}

Will Schroeder's avatar
Will Schroeder committed
88 89 90
void vtkOBBTree::FreeSearchStructure()
{
  if ( this->Tree )
91
  {
92
    this->DeleteTree(this->Tree);
Bill Lorensen's avatar
Bill Lorensen committed
93
    delete this->Tree;
94
    this->Tree = nullptr;
95
  }
Will Schroeder's avatar
Will Schroeder committed
96 97
}

98
void vtkOBBTree::DeleteTree(vtkOBBNode *OBBptr)
Will Schroeder's avatar
Will Schroeder committed
99
{
100
  if ( OBBptr->Kids != nullptr )
101
  {
102 103 104 105
    this->DeleteTree(OBBptr->Kids[0]);
    this->DeleteTree(OBBptr->Kids[1]);
    delete OBBptr->Kids[0];
    delete OBBptr->Kids[1];
106
  }
Will Schroeder's avatar
Will Schroeder committed
107 108 109 110 111
}

// 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.
112 113
void vtkOBBTree::ComputeOBB(vtkPoints *pts, double corner[3], double max[3],
                            double mid[3], double min[3], double size[3])
Will Schroeder's avatar
Will Schroeder committed
114
{
Amy Squillacote's avatar
Amy Squillacote committed
115 116
  int i;
  vtkIdType numPts, pointId;
117 118 119
  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;
Will Schroeder's avatar
Will Schroeder committed
120 121 122 123 124 125 126

  //
  // Compute mean
  //
  numPts = pts->GetNumberOfPoints();
  mean[0] = mean[1] = mean[2] = 0.0;
  for (pointId=0; pointId < numPts; pointId++ )
127
  {
128
    pts->GetPoint(pointId, x);
Bill Lorensen's avatar
Bill Lorensen committed
129
    for (i=0; i < 3; i++)
130
    {
Bill Lorensen's avatar
Bill Lorensen committed
131 132
      mean[i] += x[i];
    }
133
  }
Bill Lorensen's avatar
Bill Lorensen committed
134
  for (i=0; i < 3; i++)
135
  {
Bill Lorensen's avatar
Bill Lorensen committed
136
    mean[i] /= numPts;
137
  }
Will Schroeder's avatar
Will Schroeder committed
138 139 140 141

  //
  // Compute covariance matrix
  //
142
  a[0] = a0; a[1] = a1; a[2] = a2;
Bill Lorensen's avatar
Bill Lorensen committed
143
  for (i=0; i < 3; i++)
144
  {
Bill Lorensen's avatar
Bill Lorensen committed
145
    a0[i] = a1[i] = a2[i] = 0.0;
146
  }
Will Schroeder's avatar
Will Schroeder committed
147 148

  for (pointId=0; pointId < numPts; pointId++ )
149
  {
150
    pts->GetPoint(pointId, x);
Will Schroeder's avatar
Will Schroeder committed
151 152
    xp[0] = x[0] - mean[0]; xp[1] = x[1] - mean[1]; xp[2] = x[2] - mean[2];
    for (i=0; i < 3; i++)
153
    {
Will Schroeder's avatar
Will Schroeder committed
154 155 156
      a0[i] += xp[0] * xp[i];
      a1[i] += xp[1] * xp[i];
      a2[i] += xp[2] * xp[i];
157 158
    }
  }//for all points
Will Schroeder's avatar
Will Schroeder committed
159 160

  for (i=0; i < 3; i++)
161
  {
Will Schroeder's avatar
Will Schroeder committed
162 163 164
    a0[i] /= numPts;
    a1[i] /= numPts;
    a2[i] /= numPts;
165
  }
Will Schroeder's avatar
Will Schroeder committed
166 167

  //
168
  // Extract axes (i.e., eigenvectors) from covariance matrix.
Will Schroeder's avatar
Will Schroeder committed
169
  //
170
  v[0] = v0; v[1] = v1; v[2] = v2;
171
  vtkMath::Jacobi(a,size,v);
Will Schroeder's avatar
Will Schroeder committed
172 173 174 175 176
  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++)
177
  {
Will Schroeder's avatar
Will Schroeder committed
178 179 180
    a[0][i] = mean[i] + max[i];
    a[1][i] = mean[i] + mid[i];
    a[2][i] = mean[i] + min[i];
181
  }
Will Schroeder's avatar
Will Schroeder committed
182 183 184 185

  //
  // Create oriented bounding box by projecting points onto eigenvectors.
  //
186 187
  tMin[0] = tMin[1] = tMin[2] = VTK_DOUBLE_MAX;
  tMax[0] = tMax[1] = tMax[2] = -VTK_DOUBLE_MAX;
Will Schroeder's avatar
Will Schroeder committed
188 189

  for (pointId=0; pointId < numPts; pointId++ )
190
  {
191
    pts->GetPoint(pointId, x);
Will Schroeder's avatar
Will Schroeder committed
192
    for (i=0; i < 3; i++)
193
    {
194
      vtkLine::DistanceToLine(x, mean, a[i], t, closest);
Bill Lorensen's avatar
Bill Lorensen committed
195
      if ( t < tMin[i] )
196
      {
197
        tMin[i] = t;
198
      }
Bill Lorensen's avatar
Bill Lorensen committed
199
      if ( t > tMax[i] )
200
      {
201
        tMax[i] = t;
Will Schroeder's avatar
Will Schroeder committed
202
      }
203 204
    }
  }//for all points
Will Schroeder's avatar
Will Schroeder committed
205 206

  for (i=0; i < 3; i++)
207
  {
Will Schroeder's avatar
Will Schroeder committed
208 209 210 211 212
    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];
213
  }
214 215
}

216 217
// a method to compute the OBB of a dataset without having to go through the
// Execute method; It does set
218 219
void vtkOBBTree::ComputeOBB(vtkDataSet *input, double corner[3], double max[3],
                            double mid[3], double min[3], double size[3])
220
{
Amy Squillacote's avatar
Amy Squillacote committed
221
  vtkIdType numPts, numCells, i;
222 223 224 225 226
  vtkIdList *cellList;
  vtkDataSet *origDataSet;

  vtkDebugMacro(<<"Computing OBB");

227
  if ( input == nullptr || (numPts = input->GetNumberOfPoints()) < 1 ||
228
      (input->GetNumberOfCells()) < 1 )
229
  {
230 231
    vtkErrorMacro(<<"Can't compute OBB - no data available!");
    return;
232
  }
233
  numCells = input->GetNumberOfCells();
234 235 236 237 238 239 240 241 242 243

  // 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++)
244
  {
245
    this->InsertedPoints[i] = 0;
246
  }
247 248 249 250 251 252
  this->PointsList = vtkPoints::New();
  this->PointsList->Allocate(numPts);

  cellList = vtkIdList::New();
  cellList->Allocate(numCells);
  for (i=0; i < numCells; i++)
253
  {
254
    cellList->InsertId(i,i);
255
  }
256 257 258 259 260 261 262

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

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

265 266 267
// 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.
268 269
void vtkOBBTree::ComputeOBB(vtkIdList *cells, double corner[3], double max[3],
                            double mid[3], double min[3], double size[3])
Amy Squillacote's avatar
Amy Squillacote committed
270 271 272
{
  vtkIdType numCells, i, j, cellId, ptId, pId, qId, rId;
  int k, type;
273
  vtkIdType numPts = 0;
274
  vtkIdType *ptIds = nullptr;
275 276 277 278
  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];
279

280 281 282 283 284
  this->OBBCount++;
  this->PointsList->Reset();
  //
  // Compute mean & moments
  //
285

286 287 288 289 290
  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++ )
291
  {
292
    a0[i] = a1[i] = a2[i] = 0.0;
293
  }
294

295
  for ( i=0; i < numCells; i++ )
296
  {
297 298
    cellId = cells->GetId( i );
    type = this->DataSet->GetCellType( cellId );
299
    switch (this->DataSet->GetDataObjectType())
300
    {
301 302 303 304 305 306
      case VTK_POLY_DATA:
        ((vtkPolyData *)this->DataSet)->GetCellPoints( cellId, numPts, ptIds );
        break;
      case VTK_UNSTRUCTURED_GRID:
        ((vtkUnstructuredGrid *)this->DataSet)->GetCellPoints( cellId, numPts, ptIds );
        break;
307
      default:
308
        vtkErrorMacro( <<"DataSet " <<  this->DataSet->GetClassName() <<
309 310
                       " not supported." );
        break;
311
    }
312
    for ( j=0; j<numPts-2; j++ )
313
    {
Will Schroeder's avatar
Will Schroeder committed
314
      vtkCELLTRIANGLES( ptIds, type, j, pId, qId, rId );
315
      if ( pId < 0 )
316
      {
317
        continue;
318
      }
319 320 321
      this->DataSet->GetPoint(pId, p);
      this->DataSet->GetPoint(qId, q);
      this->DataSet->GetPoint(rId, r);
322 323 324
      // p, q, and r are the oriented triangle points.
      // Compute the components of the moment of inertia tensor.
      for ( k=0; k<3; k++ )
325
      {
326 327 328 329 330
        // 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;
331
      }
332 333 334
      vtkMath::Cross( dp0, dp1, xp );
      tri_mass = 0.5*vtkMath::Norm( xp );
      tot_mass += tri_mass;
335
      for ( k=0; k<3; k++ )
336
      {
Will Schroeder's avatar
Will Schroeder committed
337
        mean[k] += tri_mass*c[k];
338
      }
339

340 341 342 343
      // 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;
344

345 346 347 348
      // 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;
349
    } // end foreach triangle
Will Schroeder's avatar
Will Schroeder committed
350

351 352 353 354
    // While computing cell moments, gather all the cell's
    // point coordinates into a single list.
    //
    for ( j=0; j < numPts; j++ )
355
    {
356
      if ( this->InsertedPoints[ptIds[j]] != this->OBBCount )
357
      {
358 359
        this->InsertedPoints[ptIds[j]] = this->OBBCount;
        this->PointsList->InsertNextPoint(this->DataSet->GetPoint(ptIds[j]));
360 361 362
      }
    }//for all points of this cell
  } // end foreach cell
363

364

365 366
  // normalize data
  for ( i=0; i<3; i++ )
367
  {
368
    mean[i] = mean[i]/tot_mass;
369
  }
370

371 372 373 374
  // matrix is symmetric
  a1[0] = a0[1];
  a2[0] = a0[2];
  a2[1] = a1[2];
375

376 377
  // get covariance from moments
  for ( i=0; i<3; i++ )
378
  {
379
    for ( j=0; j<3; j++ )
380
    {
381
      a[i][j] = a[i][j]/tot_mass - mean[i]*mean[j];
Will Schroeder's avatar
Will Schroeder committed
382
    }
383
  }
384

385
    //
386
    // Extract axes (i.e., eigenvectors) from covariance matrix.
387
    //
388
    v[0] = v0; v[1] = v1; v[2] = v2;
389 390 391 392
    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];
393

394
    for (i=0; i < 3; i++)
395
    {
396 397 398
      a[0][i] = mean[i] + max[i];
      a[1][i] = mean[i] + mid[i];
      a[2][i] = mean[i] + min[i];
399
    }
400

401 402 403
    //
    // Create oriented bounding box by projecting points onto eigenvectors.
    //
404 405
    tMin[0] = tMin[1] = tMin[2] = VTK_DOUBLE_MAX;
    tMax[0] = tMax[1] = tMax[2] = -VTK_DOUBLE_MAX;
406

407 408
    numPts = this->PointsList->GetNumberOfPoints();
    for (ptId=0; ptId < numPts; ptId++ )
409
    {
410
      this->PointsList->GetPoint(ptId, p);
411
      for (i=0; i < 3; i++)
412
      {
413
        vtkLine::DistanceToLine(p, mean, a[i], t, closest);
414
        if ( t < tMin[i] )
415
        {
Will Schroeder's avatar
Will Schroeder committed
416
          tMin[i] = t;
417
        }
418
        if ( t > tMax[i] )
419
        {
Will Schroeder's avatar
Will Schroeder committed
420
          tMax[i] = t;
421
        }
422 423
      }
    }//for all points
424

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

429 430 431
      max[i] = (tMax[0] - tMin[0]) * max[i];
      mid[i] = (tMax[1] - tMin[1]) * mid[i];
      min[i] = (tMax[2] - tMin[2]) * min[i];
432
    }
Will Schroeder's avatar
Will Schroeder committed
433 434
}

435 436 437 438 439 440 441 442 443 444 445
// 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
446
int vtkOBBTreeLineIntersectsTriangle(const double p1[3], const double p2[3],
447
                                     double pt1[3], double pt2[3], double pt3[3],
448
                                     double tolerance, double point[3],
449
                                     double &t, int &sense)
450
{
451
  double normal[3];
452 453 454
  vtkTriangle::ComputeNormal(pt1, pt2, pt3, normal);

  // vector from p1 to p2
455
  double v12[3];
456 457 458 459 460
  v12[0] = p2[0] - p1[0];
  v12[1] = p2[1] - p1[1];
  v12[2] = p2[2] - p1[2];

  // vector from p1 to triangle
461
  double v1t[3];
462 463 464 465 466
  v1t[0] = pt1[0] - p1[0];
  v1t[1] = pt1[1] - p1[1];
  v1t[2] = pt1[2] - p1[2];

  // compute numerator/denominator of parametric distance
467 468
  double numerator = vtkMath::Dot(normal, v1t);
  double denominator = vtkMath::Dot(normal, v12);
469
  if (denominator == 0)
470
  {
471
    return 0;
472
  }
473 474

  // If denominator less than the tolerance, then the
475
  // line and plane are considered parallel.
476
  double fabsden = denominator;
477 478
  sense = -1;
  if (fabsden < 0.0)
479
  {
480 481
    sense = 1;
    fabsden = -fabsden;
482
  }
483
  if (fabsden > 1e-6 + tolerance)
484
  {
485
    // calculate the distance to the intersection along the line
486 487
    t = numerator/denominator;
    if (t < 0.0  ||  t > 1.0)
488
    {
489
      return 0;
490
    }
491

492 493 494 495
    // intersection point
    point[0] = p1[0] + t*v12[0];
    point[1] = p1[1] + t*v12[1];
    point[2] = p1[2] + t*v12[2];
496 497 498 499 500

    // 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])
501
    {
502
      xi = 1; yi = 2; zi = 0;
503
    }
504
    if (normal[xi]*normal[xi] < normal[2]*normal[2])
505
    {
506
      xi = 2; yi = 0; zi = 1;
507
    }
508 509

    // calculate vector from triangle corner to point
510 511
    double u0 = point[yi] - pt1[yi];
    double v0 = point[zi] - pt1[zi];
512
    // calculate edge vectors for triangle
513 514 515 516
    double u1 = pt2[yi] - pt1[yi];
    double v1 = pt2[zi] - pt1[zi];
    double u2 = pt3[yi] - pt1[yi];
    double v2 = pt3[zi] - pt1[zi];
517 518

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

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

    if (area < 0)
527
    {
528 529
      alpha = -alpha;
      beta = -beta;
530
      gamma = -gamma;
531
    }
532

533
    if (alpha > 0  &&  beta > 0  &&  gamma > 0)
534
    { // inside of polygon
535
      return 1;
536
    }
537
  }
538 539 540

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

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

550 551
  // make sure that order of points in each line segment is the
  // same for faces pointed in opposite directions
552
  double *tpoints[4];
553
  if (sense > 0)
554
  {
555 556 557 558
    tpoints[0] = pt1;
    tpoints[1] = pt2;
    tpoints[2] = pt3;
    tpoints[3] = pt1;
559
  }
560
  else
561
  {
562 563 564 565
    tpoints[0] = pt3;
    tpoints[1] = pt2;
    tpoints[2] = pt1;
    tpoints[3] = pt3;
566
  }
567

568 569
  double vec[3];
  double v;
570
  for (int i = 0; i < 3; i++)
571
  {
572 573
    pt1 = tpoints[i];
    pt2 = tpoints[i+1];
574

575
    if (vtkLine::Intersection(p1,p2, pt1,pt2, t,v) == 2)
576
    {
577 578 579
      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);
580

581
      if (vtkMath::Dot(vec,vec) < tolsquared)
582
      {
583
        return 1;
584 585
      }
    }
586
  }
587

588
  return 0;
589 590 591 592
}

// just check whether a point lies inside or outside the DataSet,
// assuming that the data is a closed vtkPolyData surface.
593
int vtkOBBTree::InsideOrOutside(const double point[3])
594 595
{
  // no points!
596 597 598 599
  // 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++)
600
  {
601 602 603 604 605 606 607
    vtkIdType numPts;
    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++)
608
    {
609 610 611
      vtkIdType pt1Id, pt2Id, pt3Id;
      vtkCELLTRIANGLES(ptIds, cellType, j, pt1Id, pt2Id, pt3Id);
      if (pt1Id < 0)
612
      { // cell wasn't a polygon, triangle, quad, or triangle strip
613
        continue;
614
      }
615
      // create a point that is guaranteed to be inside the cell
616
      double pt1[3], pt2[3], pt3[3];
617 618 619
      this->DataSet->GetPoint(pt1Id, pt1);
      this->DataSet->GetPoint(pt2Id, pt2);
      this->DataSet->GetPoint(pt3Id, pt3);
620

621
      double x[3];
622 623 624 625 626 627 628 629 630
      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
631
      double v12[3];
632 633 634 635 636 637
      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
638
      double normal[3];
639
      vtkTriangle::ComputeNormal(pt1, pt2, pt3, normal);
640
      double dotProd = vtkMath::Dot(normal, v12);
641
      if (dotProd < 0)
642
      {
643
        dotProd = -dotProd;
644
      }
645
      if (dotProd >= this->Tolerance + 1e-6)
646
      {
647
        return this->IntersectWithLine(point, x, nullptr, nullptr);
648
      }
649
      // otherwise go on to next triangle
650
    }
651
  }
652 653 654 655 656 657 658 659 660 661 662
  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.
663
int vtkOBBTree::IntersectWithLine(const double p1[3], const double p2[3],
664
                                  vtkPoints *points, vtkIdList *cellIds)
665
{
666
  if (this->DataSet == nullptr)
667
  {
668
    if (points)
669
    {
670
      points->SetNumberOfPoints(0);
671
    }
672
    if (cellIds)
673
    {
674 675
      cellIds->SetNumberOfIds(0);
    }
676 677
    return 0;
  }
678
  if (!this->DataSet->IsA("vtkPolyData"))
679
  {
680 681
    vtkErrorMacro("IntersectWithLine: this method requires a vtkPolyData");
    return 0;
682
  }
683 684 685 686 687 688 689

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

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

694 695
  double point[3];
  double distance = 0;
696
  int sense = 0;
697 698 699
  vtkIdType cellId;

  // compute line vector from p1 to p2
700
  double v12[3];
701 702 703 704 705 706 707 708 709 710
  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)
711
  { // simulate recursion without the overhead or limitations
712 713 714
    vtkOBBNode *node = OBBstack[--depth];

    // check for intersection with node
715
    if (this->LineIntersectsNode(node, p1, p2))
716
    {
717
      if (node->Kids == nullptr)
718
      { // then this is a leaf node...get Cells
719 720 721
        cells = node->Cells;
        vtkIdType numCells = cells->GetNumberOfIds();
        for (vtkIdType i = 0; i < numCells; i++)
722
        {
723 724 725 726 727 728 729 730 731
          // get the current cell
          cellId = cells->GetId(i);
          int cellType = this->DataSet->GetCellType(cellId);
          vtkIdType numPts;
          vtkIdType *ptIds;
          ((vtkPolyData *)this->DataSet)->GetCellPoints(cellId, numPts, ptIds);

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

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

746
            if (vtkOBBTreeLineIntersectsTriangle(p1, p2,
747
                                                 pt1, pt2, pt3,
748
                                                 this->Tolerance, point,
749
                                                 distance, sense) <= 0)
750
            { // no intersection with triangle
751
              continue;
752
            }
753

754 755 756
            // we made it! we have a hit!

            if (listSize >= listMaxSize)
757
            { // have to grow the distanceList
758
              listMaxSize *= 2;
759
              double *tmpDistanceList = new double[listMaxSize];
760 761 762
              vtkIdType *tmpCellList = new vtkIdType[listMaxSize];
              char *tmpSenseList = new char[listMaxSize];
              for (int k = 0; k < listSize; k++)
763
              {
764 765 766
                tmpDistanceList[k] = distanceList[k];
                tmpCellList[k] = cellList[k];
                tmpSenseList[k] = senseList[k];
767
              }
768 769 770 771 772 773
              delete [] distanceList;
              distanceList = tmpDistanceList;
              delete [] cellList;
              cellList = tmpCellList;
              delete [] senseList;
              senseList = tmpSenseList;
774
            }
775 776 777 778 779 780 781 782
            // 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)
783
            {
784 785 786 787
              break;
            }
          }
        }
788
      }
789
      else
790
      { // push kids onto stack
791 792 793
        OBBstack[depth] = node->Kids[0];
        OBBstack[depth+1] = node->Kids[1];
        depth += 2;
794
      }
795 796
    }
  } // end while
797

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

    if (points)
804
    {
805
      points->SetNumberOfPoints(listSize);
806
    }
807
    if (cellIds)
808
    {
809
      cellIds->SetNumberOfIds(0);
810
    }
811 812
    double ptol = this->Tolerance/sqrt(vtkMath::Dot(v12,v12));
    double lastDistance = 0.0;
813 814 815 816
    int lastSense = 0;
    int nPoints = 0;
    int listRemainder = listSize;
    while (listRemainder)
817
    {
818 819
      int minIdx = 0;
      for (int j = 1; j < listRemainder; j++)
820
      { // check for closest intersection of the correct sense
821 822
        if (senseList[j] != lastSense &&
            distanceList[j] < distanceList[minIdx])
823
        {
824 825
          minIdx = j;
        }
826
      }
827 828 829 830 831 832 833 834 835

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

836
      // only use point if it moves us forward,
837 838
      // or it moves us backward by less than tol
      if (distance > lastDistance - ptol  &&  sense != lastSense)
839
      {
840
        if (points)
841
        {
842 843 844 845
          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);
846
        }
847
        if (cellIds)
848
        {
849
          cellIds->InsertNextId(cellId);
850
        }
851 852 853 854
        nPoints++;

        // set return value according to sense of first intersection
        if (rval == 0)
855
        {
856
          rval = sense;
857
        }
858 859 860
        // remember the last point
        lastDistance = distance;
        lastSense = sense;
861
      }
862
    }
863 864
    // shrink points array if not all points were used
    if (nPoints < listSize)
865
    {
866
      if (points)
867
      {
868
        points->GetData()->Resize(nPoints);
869 870
      }
    }
871 872
    // done!
  }
873
  else
874
  {
875
    if (points)
876
    {
David Gobbi's avatar
David Gobbi committed
877
      points->SetNumberOfPoints(0);
878
    }
879
    if (cellIds)
880
    {
881 882
      cellIds->SetNumberOfIds(0);
    }
883
  }
884 885 886 887

  delete [] senseList;
  delete [] cellList;
  delete [] distanceList;
David Gobbi's avatar
David Gobbi committed
888
  delete [] OBBstack;
889 890 891 892
  // return 1 if p1 is inside, 0 is p1 is outside
  return rval;
}

893 894 895

// Return intersection point (if any) AND the cell which was intersected by
// finite line
896
int vtkOBBTree::IntersectWithLine(const double a0[3], const double a1[3], double tol,
897 898 899
                                       double& t, double x[3], double pcoords[3],
                                       int &subId, vtkIdType &cellId,
                                       vtkGenericCell *cell)
900
{
901
  double tBest = VTK_DOUBLE_MAX, xBest[3] = {0., 0., 0,}, pcoordsBest[3] = {0., 0., 0.};
Amy Squillacote's avatar
Amy Squillacote committed
902
  int subIdBest = -1;
903
  vtkIdType cellIdBest = -1;
904

905
  std::vector<vtkOBBNode*> OBBstack(this->GetLevel()+1);
906
  OBBstack[0] = this->Tree;
907
  int depth = 1;
908
  while( depth > 0 )
909
  { // simulate recursion without the overhead or limitations
910
    depth--;
911
    vtkOBBNode *node = OBBstack[depth];
912
    if ( this->LineIntersectsNode( node, a0, a1 ) )
913
    {
914
      if ( node->Kids == nullptr )
915
      { // then this is a leaf node...get Cells
916
        for ( int ii=0; ii<node->Cells->GetNumberOfIds(); ii++ )
917
        {
918
          vtkIdType thisId = node->Cells->GetId(ii);
919
          this->DataSet->GetCell( thisId, cell);
920 921
          if ( cell->IntersectWithLine( a0, a1, tol, t, x,
                                        pcoords, subId ) )
922
          { // line intersects cell, but is it the best one?
923
            if ( t < tBest )
924
            { // Yes, it's the best.
925 926 927 928 929 930 931 932 933
              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;
            }
          }
        }
934
      }
935
      else
936
      { // push kids onto stack
937 938 939 940
        OBBstack[depth] = node->Kids[0];
        OBBstack[depth+1] = node->Kids[1];
        depth += 2;
      }
941 942
    }
  } // end while
943

944 945 946 947 948 949 950 951 952
  // even though we may not have intersected with the line we at least give
  // the best intersection result. It's on the calling function to check
  // the return value in order to interpret the results.
  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;
David Gobbi's avatar
David Gobbi committed
953

954
  if ( cellIdBest < 0 )
955
  {
956
    // didn't find an intersection
957
    return 0;
958
  }
959
  return 1;
960 961 962 963
}

void vtkOBBNode::DebugPrintTree( int level, double *leaf_vol,
                                 int *minCells, int *maxCells )
964
{
965
  double xp[3], volume, c[3];
Amy Squillacote's avatar
Amy Squillacote committed
966 967
  int i;
  vtkIdType nCells;
968

969
  if ( this->Cells != nullptr )
970
  {
971
    nCells = this->Cells->GetNumberOfIds();
972
  }
973
  else
974
  {
975
    nCells = 0;
976
  }
977

978 979 980
  vtkMath::Cross( this->Axes[0], this->Axes[1], xp );
  volume = fabs( vtkMath::Dot( xp, this->Axes[2] ) );
  for ( i=0; i<3; i++ )
981
  {
982
    c[i] = this->Corner[i] + 0.5*this->Axes[0][i] + 0.5*this->Axes[1][i]
Will Schroeder's avatar
Will Schroeder committed
983
      + 0.5*this->Axes[2][i];
984
  }
985

986
  for ( i=0; i<level; i++ )
987
  {
988
    cout<<"  ";
989
  }
990
  cout <<level<<" # Cells: "<<nCells<<", Volume: "<<volume<<"\n";
991
  for ( i=0; i<level; i++ )
992
  {
993
    cout<<"  ";
994
  }
995
  cout << "    " << vtkMath::Norm( this->Axes[0] ) << " X " <<
996 997 998
                    vtkMath::Norm( this->Axes[1] ) << " X " <<
                    vtkMath::Norm( this->Axes[2] ) << "\n";
  for ( i=0; i<level; i++ )