namespace AlistairMilne_ElectronicArtsMontreal_submission2

{

/*

Partition and subdivide into the list the same way a qsort would, but

switch axis each time we descend a level to build a kind of axis aligned

BSP. When any given node in the tree is below a certain maximum size,

then stop recursing there and just brute-force those points. Unlike

qsort a third step is necessary after recursing into the left and right

branches of a node; we also then have to then repartition the node's

list and get the points which lie in the overlapping region across the

partition and recurse into that list too.

*/

__forceinline float compVal(const PointC& point, int treeDepth)

{

return ((float*)(&point))[(treeDepth) % 3];

}

__forceinline void SWAP(PointC& p1, PointC& p2)

{

PointC temp = p1;

p1 = p2;

p2 = temp;

}

__forceinline int Partition(PointC* pBuffer, int leftIdx, int rightIdx, int treeDepth)

{

int pivotIdx = (leftIdx + rightIdx) >> 1;

SWAP(pBuffer[pivotIdx], pBuffer[leftIdx+1]);

if (compVal(pBuffer[leftIdx+1], treeDepth) > compVal(pBuffer[rightIdx], treeDepth))

{

SWAP(pBuffer[leftIdx+1], pBuffer[rightIdx]);

}

if (compVal(pBuffer[leftIdx], treeDepth) > compVal(pBuffer[rightIdx], treeDepth))

{

SWAP(pBuffer[leftIdx], pBuffer[rightIdx]);

}

if (compVal(pBuffer[leftIdx+1], treeDepth) > compVal(pBuffer[leftIdx], treeDepth))

{

SWAP(pBuffer[leftIdx+1], pBuffer[leftIdx]);

}

int i = leftIdx+1;

int j = rightIdx;

float pivotValue = compVal(pBuffer[leftIdx], treeDepth);

for (;;)

{

do ++i; while (compVal(pBuffer[i], treeDepth) < pivotValue);

do --j; while (compVal(pBuffer[j], treeDepth) > pivotValue);

if (j < i)

break;

SWAP(pBuffer[i], pBuffer[j]);

}

SWAP(pBuffer[leftIdx], pBuffer[j]);

return j;

}

__forceinline int Repartition(PointC* pBuffer, int leftIdx, int rightIdx,

int treeDepth, float leftExtent, float rightExtent)

{

int pivotIdx = leftIdx;

for (int i=leftIdx; i<=rightIdx; ++i)

{

float iVal = compVal(pBuffer[i], treeDepth);

if ((iVal > leftExtent) && (iVal < rightExtent))

{

SWAP(pBuffer[pivotIdx], pBuffer[i]);

++pivotIdx;

}

}

return pivotIdx;

}

float GetMinDist(PointC* pBuffer, int leftIdx, int rightIdx, int treeDepth)

{

static float bestDist;

// reset

if (treeDepth == 0)

bestDist = sqrtf(FLT_MAX);

const int MAX_LEAF_SIZE = 12;

if (rightIdx - leftIdx < MAX_LEAF_SIZE)

{

// brute force when we get down to a small enough number of points

PointC* p1 = pBuffer + leftIdx;

PointC* pEnd = pBuffer + rightIdx + 1;

float bestSqDist = bestDist*bestDist;

for (; p1 < pEnd; ++p1)

{

register float p1x = p1->x;

register float p1y = p1->y;

register float p1z = p1->z;

for (PointC *p2 = p1+1; p2 < pEnd; ++p2)

{

float sub = p1x - p2->x;

float currSqDist = sub*sub;

sub = p1y - p2->y;

currSqDist = currSqDist + sub*sub;

sub = p1z - p2->z;

currSqDist = currSqDist + sub*sub;

if (currSqDist < bestSqDist)

bestSqDist = currSqDist;

}

}

if (bestSqDist < bestDist*bestDist)

bestDist = sqrtf(bestSqDist);

return bestDist;

}

else

{

// partition, recurse left, recurse right (just like qsort)

int pivotIdx = Partition(pBuffer, leftIdx, rightIdx, treeDepth);

float pivotValue = compVal(pBuffer[pivotIdx], treeDepth);

// each time we descend left, output the details from the stack

GetMinDist(pBuffer, leftIdx, pivotIdx-1, treeDepth+1);

GetMinDist(pBuffer, pivotIdx, rightIdx, treeDepth+1);

// repartition, then recurse into the overlap between left & right

pivotIdx = Repartition(pBuffer, leftIdx, rightIdx, treeDepth,

pivotValue-bestDist, pivotValue+bestDist);

if (pivotIdx-leftIdx > 1)

{

GetMinDist(pBuffer, leftIdx, pivotIdx-1, treeDepth+1);

}

return bestDist;

}

}

float GetMinDist(CloudC* pCloud)

{

return GetMinDist( pCloud->GetPointBuffer(), 0, pCloud->GetNumPoints() - 1, 0);

}

}