// "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]

#define DIMENSIONS 3
#define AABB

#ifdef AABB
#define DIRECTIONS (DIMENSIONS*2) // AABB is easier to understand
#else
#define DIRECTIONS (DIMENSIONS+1) // simplex ( triangle, tetrahedron ) is more 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;
}

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 );  
}

// 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;

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

  int i;
  for( i=0; i<DIRECTIONS; ++i )
  {
    float query_maximum = query->plane[i];
    float median_minimum = dot( median->center, basis[i] ) - median->radius;
    if( median_minimum > query_maximum ) 
    {
      if( i == direction ) 
        return; // reject median and far side of plane
      break; // reject median
    }
  }
  if( i == DIRECTIONS )
    *results++ = median; // may want to do a real sphere-sphere check just before this

  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 )
{
  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 );
}