#include <stdlib.h>

#include "vtkPoints.h"
#include "vtkPolyData.h"
#include "vtkKdTree.h"

int main( int argc, char * argv[] )
{

  vtkIdType num_points = 1000;
  vtkIdType num_test_points = 100;

  vtkIdType idA;
  vtkIdType closest_id;
  vtkIdType point;
  vtkIdType test_point;

  double pointA[3];
  double pointB[3];

  vtkPoints * A = vtkPoints::New();
  A->SetDataTypeToDouble();
  A->SetNumberOfPoints( num_points );
  for ( point = 0; point < num_points; ++point )
    {
    pointA[0] = ((double) rand()) / RAND_MAX;
    pointA[1] = ((double) rand()) / RAND_MAX;
    pointA[2] = ((double) rand()) / RAND_MAX;
    A->SetPoint( point, pointA );
    }
 
  vtkKdTree * kd = vtkKdTree::New();
  kd->BuildLocatorFromPoints( A );

  for ( test_point = 0; test_point < num_test_points; ++test_point )
    {
    double min_dist2 = 10.0;
    pointB[0] = ((double) rand()) / RAND_MAX;
    pointB[1] = ((double) rand()) / RAND_MAX;
    pointB[2] = ((double) rand()) / RAND_MAX;
    for ( point = 0; point < num_points; ++point )
      {
      double dist2;
      double dx, dy, dz;
      A->GetPoint( point, pointA );
      dx = pointA[0] - pointB[0];
      dy = pointA[1] - pointB[1];
      dz = pointA[2] - pointB[2];
      dist2 = dx * dx + dy * dy + dz * dz;
      if ( dist2 < min_dist2 )
        {
        closest_id = point;
        min_dist2 = dist2;
        }
      }
    double ld2;
    idA = kd->FindClosestPoint( pointB, ld2 );
    if ( (idA != closest_id ) && ( ld2 > min_dist2 ))
      std::cout << idA << " " << closest_id  << " " 
        << ld2 << " " << min_dist2 << std::endl;
    }

  return EXIT_SUCCESS;
}