///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/*
 *	OPCODE - Optimized Collision Detection
 *	Copyright (C) 2001 Pierre Terdiman
 *	Homepage: http://www.codercorner.com/Opcode.htm
 */
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
 *	Contains code for a versatile AABB tree.
 *	\file		OPC_AABBTree.cpp
 *	\author		Pierre Terdiman
 *	\date		March, 20, 2001
 */
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
 *	Contains a generic AABB tree node.
 *
 *	\class		AABBTreeNode
 *	\author		Pierre Terdiman
 *	\version	1.0
 *	\date		March, 20, 2001
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
 *	Contains a generic AABB tree.
 *
 *	\class		AABBTree
 *	\author		Pierre Terdiman
 *	\version	1.0
 *	\date		March, 20, 2001
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// Precompiled Header
#include "Stdafx.h"

using namespace Opcode;

///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
 *	Constructor.
 */
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
AABBTreeNode::AABBTreeNode() : mP(null), mN(null), mNodePrimitives(null), mNbPrimitives(0)
{
}

///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
 *	Destructor.
 */
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
AABBTreeNode::~AABBTreeNode()
{
	DELETESINGLE(mP);
	DELETESINGLE(mN);
	mNodePrimitives	= null;	// This was just a shortcut to the global list => no release
	mNbPrimitives	= 0;
}

///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
 *	A method to split the node along a given axis.
 *	The list of indices is reorganized according to the split values.
 *	\param		axis		[in] splitting axis index
 *	\param		builder		[in] the tree builder
 *	\return		the number of primitives assigned to the first child
 */
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
udword AABBTreeNode::Split(udword axis, AABBTreeBuilder* builder)
{
	// Middle of the axis = our split value
	float SplitValue = mBV.GetCenter(axis);

	udword NbPos = 0;
	// Loop through all node-related primitives. Their indices range from mNodePrimitives[0] to mNodePrimitives[mNbPrimitives-1].
	// Those indices map the global list in the tree builder.
	for(udword i=0;i<mNbPrimitives;i++)
	{
		// Get index in global list
		udword Index = mNodePrimitives[i];

		// Test against the splitting value. The primitive value is tested against the enclosing-box center.
		// [We only need an approximate partition of the enclosing box here.]
		float PrimitiveValue = builder->GetSplittingValue(Index, axis);

		// Reorganize the list of indices in this order: positive - negative.
		if(PrimitiveValue > SplitValue)
		{
			// Swap entries
			udword Tmp = mNodePrimitives[i];
			mNodePrimitives[i] = mNodePrimitives[NbPos];
			mNodePrimitives[NbPos] = Tmp;
			// Count primitives assigned to positive space
			NbPos++;
		}
	}
	return NbPos;
}

///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
 *	A method to subdivide the node.
 *	
 *	          N
 *	        /   \
 *	      /       \
 *	   N/2         N/2
 *	  /   \       /   \
 *	N/4   N/4   N/4   N/4
 *	(etc)
 *
 *	A well-balanced tree should have a O(log n) depth.
 *	A degenerate tree would have a O(n) depth.
 *	Note a perfectly-balanced tree is not well-suited to collision detection anyway.
 *
 *	\param		builder		[in] the tree builder
 *	\return		true if success
 */
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
bool AABBTreeNode::Subdivide(AABBTreeBuilder* builder)
{
	// Checkings
	if(!builder)	return false;

	// Stop subdividing if we reach a leaf node
	if(mNbPrimitives==1)	return true;

	// Check the user-defined limit
	if(mNbPrimitives<=builder->mLimit)	return true;

	bool ValidSplit = true;	// Optimism...
	udword NbPos;
	if(builder->mRules&SPLIT_LARGESTAXIS)
	{
		// Find the largest axis to split along
		Point Extents;	mBV.GetExtents(Extents);	// Box extents
		udword Axis	= Extents.LargestAxis();		// Index of largest axis

		// Split along the axis
		NbPos = Split(Axis, builder);

		// Check split validity
		if(!NbPos || NbPos==mNbPrimitives)	ValidSplit = false;
	}
	else if(builder->mRules&SPLIT_SPLATTERPOINTS)
	{
		// Compute the means
		Point Means(0.0f, 0.0f, 0.0f);
		udword i;
		for(i=0;i<mNbPrimitives;i++)
		{
			udword Index = mNodePrimitives[i];
			Means.x+=builder->GetSplittingValue(Index, 0);
			Means.y+=builder->GetSplittingValue(Index, 1);
			Means.z+=builder->GetSplittingValue(Index, 2);
		}
		Means/=float(mNbPrimitives);

		// Compute variances
		Point Vars(0.0f, 0.0f, 0.0f);
		for(i=0;i<mNbPrimitives;i++)
		{
			udword Index = mNodePrimitives[i];
			float Cx = builder->GetSplittingValue(Index, 0);
			float Cy = builder->GetSplittingValue(Index, 1);
			float Cz = builder->GetSplittingValue(Index, 2);
			Vars.x += (Cx - Means.x)*(Cx - Means.x);
			Vars.y += (Cy - Means.y)*(Cy - Means.y);
			Vars.z += (Cz - Means.z)*(Cz - Means.z);
		}
		Vars/=float(mNbPrimitives-1);

		// Choose axis with greatest variance
		udword Axis = Vars.LargestAxis();

		// Split along the axis
		NbPos = Split(Axis, builder);

		// Check split validity
		if(!NbPos || NbPos==mNbPrimitives)	ValidSplit = false;
	}
	else if(builder->mRules&SPLIT_BALANCED)
	{
		// Test 3 axis, take the best
		float Results[3];
		NbPos = Split(0, builder);	Results[0] = float(NbPos)/float(mNbPrimitives);
		NbPos = Split(1, builder);	Results[1] = float(NbPos)/float(mNbPrimitives);
		NbPos = Split(2, builder);	Results[2] = float(NbPos)/float(mNbPrimitives);
		Results[0]-=0.5f;	Results[0]*=Results[0];
		Results[1]-=0.5f;	Results[1]*=Results[1];
		Results[2]-=0.5f;	Results[2]*=Results[2];
		udword Min=0;
		if(Results[1]<Results[Min])	Min = 1;
		if(Results[2]<Results[Min])	Min = 2;
		
		// Split along the axis
		NbPos = Split(Min, builder);

		// Check split validity
		if(!NbPos || NbPos==mNbPrimitives)	ValidSplit = false;
	}
	else if(builder->mRules&SPLIT_BESTAXIS)
	{
		// Test largest, then middle, then smallest axis...

		// Sort axis
		Point Extents;	mBV.GetExtents(Extents);	// Box extents
		udword SortedAxis[] = { 0, 1, 2 };
		float* Keys = (float*)&Extents.x;
		for(udword j=0;j<3;j++)
		{
			for(udword i=0;i<2;i++)
			{
				if(Keys[SortedAxis[i]]<Keys[SortedAxis[i+1]])
				{
					udword Tmp = SortedAxis[i];
					SortedAxis[i] = SortedAxis[i+1];
					SortedAxis[i+1] = Tmp;
				}
			}
		}

		// Find the largest axis to split along
		udword CurAxis = 0;
		ValidSplit = false;
		while(!ValidSplit && CurAxis!=3)
		{
			NbPos = Split(SortedAxis[CurAxis], builder);
			// Check the subdivision has been successful
			if(!NbPos || NbPos==mNbPrimitives)	CurAxis++;
			else								ValidSplit = true;
		}
	}
	else if(builder->mRules&SPLIT_FIFTY)
	{
		// Don't even bother splitting (mainly a performance test)
		ValidSplit = false;
	}
	else return false;

	// Check the subdivision has been successful
	if(!ValidSplit)
	{
		// Here, all boxes lie in the same sub-space. Two strategies:
		// - if the tree *must* be complete, make an arbitrary 50-50 split
		// - else stop subdividing
		if(builder->mRules&SPLIT_COMPLETE)	NbPos = mNbPrimitives>>1;
		else								return true;
	}

	// Now create children and assign their pointers.
	mP = new AABBTreeNode;	CHECKALLOC(mP);
	mN = new AABBTreeNode;	CHECKALLOC(mN);

	// Update stats
	builder->IncreaseCount(2);

	// Assign children
	mP->mNodePrimitives	= &mNodePrimitives[0];
	mP->mNbPrimitives	= NbPos;
	mN->mNodePrimitives	= &mNodePrimitives[NbPos];
	mN->mNbPrimitives	= mNbPrimitives - NbPos;

	return true;
}

///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
 *	Recursive hierarchy building in a top-down fashion.
 *	\param		builder		[in] the tree builder
 */
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
void AABBTreeNode::_BuildHierarchy(AABBTreeBuilder* builder)
{
	// 1) Compute the global box for current node
	builder->ComputeGlobalBox(mNodePrimitives, mNbPrimitives, mBV);

	// 2) Subdivide
	Subdivide(builder);

	// 3) Recurse
	if(mP)	mP->_BuildHierarchy(builder);
	if(mN)	mN->_BuildHierarchy(builder);
}

///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
 *	Constructor.
 */
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
AABBTree::AABBTree() : mIndices(null), mTotalNbNodes(0)
{
}

///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
 *	Destructor.
 */
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
AABBTree::~AABBTree()
{
	DELETEARRAY(mIndices);
}

///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
 *	A method to build a generic AABB tree from a tree builder.
 *	\param		builder		[in] the tree builder
 *	\return		true if success
 */
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
bool AABBTree::Build(AABBTreeBuilder* builder)
{
	// Checkings
	if(!builder || !builder->mNbPrimitives)	return false;

	// Init count
	builder->SetCount(1);

	// Initialize indices. This list will be modified during build.
	DELETEARRAY(mIndices);
	mIndices	= new udword[builder->mNbPrimitives];
	CHECKALLOC(mIndices);
	for(udword i=0;i<builder->mNbPrimitives;i++)	mIndices[i] = i;

	// Setup initial box
	mNodePrimitives	= mIndices;
	mNbPrimitives	= builder->mNbPrimitives;

	// Build the hierarchy
	_BuildHierarchy(builder);

	// Get back total number of nodes
	mTotalNbNodes	= builder->GetCount();
	return true;
}

///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
 *	A method to compute the depth of the tree.
 *	A well-balanced tree should have a log(n) depth. A degenerate tree O(n) depth.
 *	\return		depth of the tree
 */
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
udword AABBTree::ComputeDepth() const
{
	udword Depth = 0;
	udword Current = 0;

	struct Local
	{
		static void _UpdateDepth(const AABBTreeNode* curnode, udword& depth, udword& current)
		{
			// Checkings
			if(!curnode)	return;
			// Entering a new node => increase depth
			current++;
			// Keep track of max depth
			if(current>depth)	depth = current;
			// Recurse
			if(curnode->GetPos())	{ _UpdateDepth(curnode->GetPos(), depth, current);	current--;	}
			if(curnode->GetNeg())	{ _UpdateDepth(curnode->GetNeg(), depth, current);	current--;	}
		}
	};
	Local::_UpdateDepth(this, Depth, Current);
	return Depth;
}

///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
 *	A method to compute the number of bytes used by the tree.
 *	\return		number of bytes used
 */
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
udword AABBTree::GetUsedBytes() const
{
	udword TotalSize = mTotalNbNodes*GetNodeSize();
	if(mIndices)	TotalSize+=mNbPrimitives*sizeof(udword);
	return TotalSize;
}
// END-OF-FILE