// "buckytree" (c)2007 Bryan McNett. All Rights Reserved.
// bmcnett at gmail dot com

// sort an array of spheres in O(NlogN) time.
// determine which intersect a sphere in O(logN)? time,
// requiring no spatial index

// the database contains "minimum" values for each convex hull plane,
// and the search query contains "maximum" values.

// the search query is a convex hull. 
// the ABC of plane equation[i] goes in basis[i] 
// and the D goes in hull[i]

#include <fstream>
#include <algorithm>
#include <time.h>

#define AABB
#define EXPLICIT_SPATIAL_INDEX // not required, but vastly improves rejection rate
#define DIMENSIONS 3

#ifdef AABB
#define DIRECTIONS (DIMENSIONS*2) // AABB is easier to understand
#else
#define DIRECTIONS (DIMENSIONS+1) // simplex ( triangle, tetrahedron ) is more space-efficient
#endif

struct vector { float v[DIMENSIONS]; };
struct sphere { vector center; float radius; };
struct hull { float plane[DIRECTIONS]; }; // for 3D AABB, this is [ max(x), max(-x), max(y), max(-y), max(z), max(-z) ]

vector basis[DIRECTIONS] 
#if DIMENSIONS == 3
= { 
#if defined(AABB)
    {{  1,  0,  0 }}, {{ -1,  0,  0 }}, 
    {{  0,  1,  0 }}, {{  0, -1,  0 }}, 
    {{  0,  0,  1 }}, {{  0,  0, -1 }} 
#else
    {{  0.000,  0.000,  1.000 }},
    {{  0.943,  0.000, -0.333 }},
    {{ -0.471,  0.816, -0.333 }},
    {{ -0.471, -0.816, -0.333 }}
#endif // defined(AABB)
  }
#endif // DIMENSIONS == 3
;

float dot( const vector& a, const vector& b ) 
{
  float result = a.v[0] * b.v[0];
  for( int i=1; i<DIMENSIONS; ++i )
    result += a.v[i] * b.v[i];
  return result;
}

vector sub( const vector& a, const vector& b )
{
  vector result;
  for( int i=0; i<DIMENSIONS; ++i )
    result.v[i] = a.v[i] - b.v[i];
  return result;
}

bool intersects( const sphere& a, const sphere& b )
{
  vector delta = sub( a.center, b.center );
  float radius = a.radius + b.radius;
  return dot( delta, delta ) <= radius * radius; 
}

struct comparer
{
  int direction;
  comparer( int d ) : direction(d) {}
  float minimum( const sphere& a ) const
  {
    return dot( a.center, basis[direction] ) - a.radius;
  }
  bool operator()( const sphere& a, const sphere& b ) const
  {
    return minimum(a) < minimum(b);
  }
};

// sort an array of spheres in O(NlogN) time
//
void buckysort( sphere* begin, sphere* end, int direction )
{
  int spheres = end - begin;
  if( spheres < 2 ) 
    return;
  sphere* median = begin + spheres / 2;

  comparer predicate( direction );
  std::nth_element( begin, median, end, predicate );

  buckysort( begin, median, (direction+1)%DIRECTIONS );
  buckysort( median+1, end, (direction+1)%DIRECTIONS );  
}

sphere query_sphere;
long tests = 0;
long recursions = 0;

// search a sorted array of spheres in O(logN)? time
//
void buckysearch( sphere**& results, sphere* begin, sphere* end, hull* query, int direction )
{
  int spheres = end - begin;
  if( spheres < 1 )
    return;
  sphere* median = begin + spheres / 2;

  ++recursions;
    
  buckysearch( results, begin, median, query, (direction+1)%DIRECTIONS ); // can't reject near side of plane

  if( query->plane[direction] < dot( median->center, basis[direction] ) - median->radius )
    return; // reject median and far side of plane
    
#ifdef EXPLICIT_SPATIAL_INDEX // simulate the rejections made by an explicit spatial index
  int i;
  for( i=0; i<DIRECTIONS; ++i )
    if( query->plane[i] < dot( median->center, basis[i] ) - median->radius )
      break;
  if( i == DIRECTIONS )
#endif
 
  {
    ++tests;   
    if( intersects( query_sphere, *median ) )
      *results++ = median;
  }

  buckysearch( results, median+1, end, query, (direction+1)%DIRECTIONS );
}

// search an array of spheres for those that intersect a target sphere in O(logN)? time
//
void buckysearch( sphere**& results, sphere* begin, sphere* end, sphere* query )
{
  query_sphere = *query;
  hull query_maximum;
  for( int i=0; i<DIRECTIONS; ++i )
    query_maximum.plane[i] = dot( query->center, basis[i] ) + query->radius; 
  buckysearch( results, begin, end, &query_maximum, 0 );
}

#define SPHERES 50000
#define ITERATIONS 500000

int main( int argc, char* argv[] )
{
  std::ofstream out( "out.txt", std::ios_base::app );
  
  out << "bounding shape: " <<
#ifdef AABB
  "AABB"
#else
  "simplex"
#endif
  << std::endl;
  
  out << "spatial index: " <<
#ifdef EXPLICIT_SPATIAL_INDEX
  "explicit"
#else
  "implicit"
#endif
  << std::endl;
    
  sphere database[SPHERES];
  srand( 0xCAFEBABE );
  for( int i=0; i<SPHERES; ++i )
  {
    database[i].center.v[0] = rand() % 100;
    database[i].center.v[1] = rand() % 100;
    database[i].center.v[2] = rand() % 100;
    database[i].radius = rand() % 10;
  }
  sphere* results[SPHERES];
  for( int i=0; i<ITERATIONS; ++i )
  {
    sphere** end = results;
    buckysearch( end, database, database+SPHERES, &database[rand()%SPHERES] );
  } 
  double spheres = SPHERES;
  spheres *= ITERATIONS;
  out << "spheres: " << spheres << std::endl;
  out << "recursions: " << recursions << std::endl;
  out << "recursions / spheres: " << recursions / spheres << std::endl;
  out << "tests: " << tests << std::endl;
  out << "tests / spheres: " << tests / spheres << std::endl;
  out << std::endl;
}