OpenSimMirror/libraries/ode-0.9/OPCODE/OPC_HybridModel.cpp

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

///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
 *	Contains code for hybrid models.
 *	\file		OPC_HybridModel.cpp
 *	\author		Pierre Terdiman
 *	\date		May, 18, 2003
 */
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
 *	An hybrid collision model.
 *	
 *	The problem :
 *	
 *	Opcode really shines for mesh-mesh collision, especially when meshes are deeply overlapping
 *	(it typically outperforms RAPID in those cases).
 *	
 *	Unfortunately this is not the typical scenario in games.
 *	
 *	For close-proximity cases, especially for volume-mesh queries, it's relatively easy to run faster
 *	than Opcode, that suffers from a relatively high setup time.
 *	
 *	In particular, Opcode's "vanilla" trees in those cases -can- run faster. They can also use -less-
 *	memory than the optimized ones, when you let the system stop at ~10 triangles / leaf for example
 *	(i.e. when you don't use "complete" trees). However, those trees tend to fragment memory quite a
 *	lot, increasing cache misses : since they're not "complete", we can't predict the final number of
 *	nodes and we have to allocate nodes on-the-fly. For the same reasons we can't use Opcode's "optimized"
 *	trees here, since they rely on a known layout to perform the "optimization".
 *	
 *	Hybrid trees :
 *	
 *	Hybrid trees try to combine best of both worlds :
 *	
 *	- they use a maximum limit of 16 triangles/leaf. "16" is used so that we'll be able to save the
 *	number of triangles using 4 bits only.
 *	
 *	- they're still "complete" trees thanks to a two-passes building phase. First we create a "vanilla"
 *	AABB-tree with Opcode, limited to 16 triangles/leaf. Then we create a *second* vanilla tree, this
 *	time using the leaves of the first one. The trick is : this second tree is now "complete"... so we
 *	can further transform it into an Opcode's optimized tree.
 *	
 *	- then we run the collision queries on that standard Opcode tree. The only difference is that leaf
 *	nodes contain indices to leaf nodes of another tree. Also, we have to skip all primitive tests in
 *	Opcode optimized trees, since our leaves don't contain triangles anymore.
 *	
 *	- finally, for each collided leaf, we simply loop through 16 triangles max, and collide them with
 *	the bounding volume used in the query (we only support volume-vs-mesh queries here, not mesh-vs-mesh)
 *	
 *	All of that is wrapped in this "hybrid model" that contains the minimal data required for this to work.
 *	It's a mix between old "vanilla" trees, and old "optimized" trees.
 *
 *	Extra advantages:
 *
 *	- If we use them for dynamic models, we're left with a very small number of leaf nodes to refit. It
 *	might be a bit faster since we have less nodes to write back.
 *
 *	- In rigid body simulation, using temporal coherence and sleeping objects greatly reduce the actual
 *	influence of one tree over another (i.e. the speed difference is often invisible). So memory is really
 *	the key element to consider, and in this regard hybrid trees are just better.
 *	
 *	Information to take home:
 *	- they use less ram
 *	- they're not slower (they're faster or slower depending on cases, overall there's no significant
 *	difference *as long as objects don't interpenetrate too much* - in which case Opcode's optimized trees
 *	are still notably faster)
 *
 *	\class		HybridModel
 *	\author		Pierre Terdiman
 *	\version	1.3
 *	\date		May, 18, 2003
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

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

using namespace Opcode;

///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
 *	Constructor.
 */
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
HybridModel::HybridModel() :
	mNbLeaves		(0),
	mNbPrimitives	(0),
	mTriangles		(null),
	mIndices		(null)
{
}

///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
 *	Destructor.
 */
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
HybridModel::~HybridModel()
{
	Release();
}

///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
 *	Releases everything.
 */
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
void HybridModel::Release()
{
	ReleaseBase();
	DELETEARRAY(mIndices);
	DELETEARRAY(mTriangles);
	mNbLeaves		= 0;
	mNbPrimitives	= 0;
}

	struct Internal
	{
		Internal()
		{
			mNbLeaves	= 0;
			mLeaves		= null;
			mTriangles	= null;
			mBase		= null;
		}
		~Internal()
		{
			DELETEARRAY(mLeaves);
		}

		udword			mNbLeaves;
		AABB*			mLeaves;
		LeafTriangles*	mTriangles;
		const udword*	mBase;
	};

///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
 *	Builds a collision model.
 *	\param		create		[in] model creation structure
 *	\return		true if success
 */
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
bool HybridModel::Build(const OPCODECREATE& create)
{
	// 1) Checkings
	if(!create.mIMesh || !create.mIMesh->IsValid())	return false;

	// Look for degenerate faces.
	udword NbDegenerate = create.mIMesh->CheckTopology();
	if(NbDegenerate)	Log("OPCODE WARNING: found %d degenerate faces in model! Collision might report wrong results!\n", NbDegenerate);
	// We continue nonetheless.... 

	Release();	// Make sure previous tree has been discarded

	// 1-1) Setup mesh interface automatically
	SetMeshInterface(create.mIMesh);

	bool Status = false;
	AABBTree* LeafTree = null;
	Internal Data;

	// 2) Build a generic AABB Tree.
	mSource = new AABBTree;
	CHECKALLOC(mSource);

	// 2-1) Setup a builder. Our primitives here are triangles from input mesh,
	// so we use an AABBTreeOfTrianglesBuilder.....
	{
		AABBTreeOfTrianglesBuilder TB;
		TB.mIMesh			= create.mIMesh;
		TB.mNbPrimitives	= create.mIMesh->GetNbTriangles();
		TB.mSettings		= create.mSettings;
		TB.mSettings.mLimit	= 16;	// ### Hardcoded, but maybe we could let the user choose 8 / 16 / 32 ...
		if(!mSource->Build(&TB))	goto FreeAndExit;
	}

	// 2-2) Here's the trick : create *another* AABB tree using the leaves of the first one (which are boxes, this time)
	struct Local
	{
		// A callback to count leaf nodes
		static bool CountLeaves(const AABBTreeNode* current, udword depth, void* user_data)
		{
			if(current->IsLeaf())
			{
				Internal* Data = (Internal*)user_data;
				Data->mNbLeaves++;
			}
			return true;
		}

		// A callback to setup leaf nodes in our internal structures
		static bool SetupLeafData(const AABBTreeNode* current, udword depth, void* user_data)
		{
			if(current->IsLeaf())
			{
				Internal* Data = (Internal*)user_data;

				// Get current leaf's box
				Data->mLeaves[Data->mNbLeaves] = *current->GetAABB();

				// Setup leaf data
				udword Index = udword((size_t(current->GetPrimitives()) - size_t(Data->mBase)) / sizeof(udword));
				Data->mTriangles[Data->mNbLeaves].SetData(current->GetNbPrimitives(), Index);

				Data->mNbLeaves++;
			}
			return true;
		}
	};

	// Walk the tree & count number of leaves
	Data.mNbLeaves = 0;
	mSource->Walk(Local::CountLeaves, &Data);
	mNbLeaves = Data.mNbLeaves;	// Keep track of it

	// Special case for 1-leaf meshes
	if(mNbLeaves==1)
	{
		mModelCode |= OPC_SINGLE_NODE;
		Status = true;
		goto FreeAndExit;
	}

	// Allocate our structures
	Data.mLeaves = new AABB[Data.mNbLeaves];		CHECKALLOC(Data.mLeaves);
	mTriangles = new LeafTriangles[Data.mNbLeaves];	CHECKALLOC(mTriangles);

	// Walk the tree again & setup leaf data
	Data.mTriangles	= mTriangles;
	Data.mBase		= mSource->GetIndices();
	Data.mNbLeaves	= 0;	// Reset for incoming walk
	mSource->Walk(Local::SetupLeafData, &Data);

	// Handle source indices
	{
		bool MustKeepIndices = true;
		if(create.mCanRemap)
		{
			// We try to get rid of source indices (saving more ram!) by reorganizing triangle arrays...
			// Remap can fail when we use callbacks => keep track of indices in that case (it still
			// works, only using more memory)
			if(create.mIMesh->RemapClient(mSource->GetNbPrimitives(), mSource->GetIndices()))
			{
				MustKeepIndices = false;
			}
		}

		if(MustKeepIndices)
		{
			// Keep track of source indices (from vanilla tree)
			mNbPrimitives = mSource->GetNbPrimitives();
			mIndices = new udword[mNbPrimitives];
			CopyMemory(mIndices, mSource->GetIndices(), mNbPrimitives*sizeof(udword));
		}
	}

	// Now, create our optimized tree using previous leaf nodes
	LeafTree = new AABBTree;
	CHECKALLOC(LeafTree);
	{
		AABBTreeOfAABBsBuilder TB;	// Now using boxes !
		TB.mSettings		= create.mSettings;
		TB.mSettings.mLimit	= 1;	// We now want a complete tree so that we can "optimize" it
		TB.mNbPrimitives	= Data.mNbLeaves;
		TB.mAABBArray		= Data.mLeaves;
		if(!LeafTree->Build(&TB))	goto FreeAndExit;
	}

	// 3) Create an optimized tree according to user-settings
	if(!CreateTree(create.mNoLeaf, create.mQuantized))	goto FreeAndExit;

	// 3-2) Create optimized tree
	if(!mTree->Build(LeafTree))	goto FreeAndExit;

	// Finally ok...
	Status = true;

FreeAndExit:	// Allow me this one...
	DELETESINGLE(LeafTree);

	// 3-3) Delete generic tree if needed
	if(!create.mKeepOriginal)	DELETESINGLE(mSource);

	return Status;
}

///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
 *	Gets the number of bytes used by the tree.
 *	\return		amount of bytes used
 */
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
udword HybridModel::GetUsedBytes() const
{
	udword UsedBytes = 0;
	if(mTree)		UsedBytes += mTree->GetUsedBytes();
	if(mIndices)	UsedBytes += mNbPrimitives * sizeof(udword);	// mIndices
	if(mTriangles)	UsedBytes += mNbLeaves * sizeof(LeafTriangles);	// mTriangles
	return UsedBytes;
}

inline_ void ComputeMinMax(Point& min, Point& max, const VertexPointers& vp)
{
	// Compute triangle's AABB = a leaf box
#ifdef OPC_USE_FCOMI	// a 15% speedup on my machine, not much
	min.x = FCMin3(vp.Vertex[0]->x, vp.Vertex[1]->x, vp.Vertex[2]->x);
	max.x = FCMax3(vp.Vertex[0]->x, vp.Vertex[1]->x, vp.Vertex[2]->x);

	min.y = FCMin3(vp.Vertex[0]->y, vp.Vertex[1]->y, vp.Vertex[2]->y);
	max.y = FCMax3(vp.Vertex[0]->y, vp.Vertex[1]->y, vp.Vertex[2]->y);

	min.z = FCMin3(vp.Vertex[0]->z, vp.Vertex[1]->z, vp.Vertex[2]->z);
	max.z = FCMax3(vp.Vertex[0]->z, vp.Vertex[1]->z, vp.Vertex[2]->z);
#else
	min = *vp.Vertex[0];
	max = *vp.Vertex[0];
	min.Min(*vp.Vertex[1]);
	max.Max(*vp.Vertex[1]);
	min.Min(*vp.Vertex[2]);
	max.Max(*vp.Vertex[2]);
#endif
}

///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
 *	Refits the collision model. This can be used to handle dynamic meshes. Usage is:
 *	1. modify your mesh vertices (keep the topology constant!)
 *	2. refit the tree (call this method)
 *	\return		true if success
 */
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
bool HybridModel::Refit()
{
	if(!mIMesh)	return false;
	if(!mTree)	return false;

	if(IsQuantized())	return false;
	if(HasLeafNodes())	return false;

	const LeafTriangles* LT = GetLeafTriangles();
	const udword* Indices = GetIndices();

	// Bottom-up update
	VertexPointers VP;
	Point Min,Max;
	Point Min_,Max_;
	udword Index = mTree->GetNbNodes();
	AABBNoLeafNode* Nodes = (AABBNoLeafNode*)((AABBNoLeafTree*)mTree)->GetNodes();
	while(Index--)
	{
		AABBNoLeafNode& Current = Nodes[Index];

		if(Current.HasPosLeaf())
		{
			const LeafTriangles& CurrentLeaf = LT[Current.GetPosPrimitive()];

			Min.SetPlusInfinity();
			Max.SetMinusInfinity();

			Point TmpMin, TmpMax;

			// Each leaf box has a set of triangles
			udword NbTris = CurrentLeaf.GetNbTriangles();
			if(Indices)
			{
				const udword* T = &Indices[CurrentLeaf.GetTriangleIndex()];

				// Loop through triangles and test each of them
				while(NbTris--)
				{
					mIMesh->GetTriangle(VP, *T++);
					ComputeMinMax(TmpMin, TmpMax, VP);
					Min.Min(TmpMin);
					Max.Max(TmpMax);
				}
			}
			else
			{
				udword BaseIndex = CurrentLeaf.GetTriangleIndex();

				// Loop through triangles and test each of them
				while(NbTris--)
				{
					mIMesh->GetTriangle(VP, BaseIndex++);
					ComputeMinMax(TmpMin, TmpMax, VP);
					Min.Min(TmpMin);
					Max.Max(TmpMax);
				}
			}
		}
		else
		{
			const CollisionAABB& CurrentBox = Current.GetPos()->mAABB;
			CurrentBox.GetMin(Min);
			CurrentBox.GetMax(Max);
		}

		if(Current.HasNegLeaf())
		{
			const LeafTriangles& CurrentLeaf = LT[Current.GetNegPrimitive()];

			Min_.SetPlusInfinity();
			Max_.SetMinusInfinity();

			Point TmpMin, TmpMax;

			// Each leaf box has a set of triangles
			udword NbTris = CurrentLeaf.GetNbTriangles();
			if(Indices)
			{
				const udword* T = &Indices[CurrentLeaf.GetTriangleIndex()];

				// Loop through triangles and test each of them
				while(NbTris--)
				{
					mIMesh->GetTriangle(VP, *T++);
					ComputeMinMax(TmpMin, TmpMax, VP);
					Min_.Min(TmpMin);
					Max_.Max(TmpMax);
				}
			}
			else
			{
				udword BaseIndex = CurrentLeaf.GetTriangleIndex();

				// Loop through triangles and test each of them
				while(NbTris--)
				{
					mIMesh->GetTriangle(VP, BaseIndex++);
					ComputeMinMax(TmpMin, TmpMax, VP);
					Min_.Min(TmpMin);
					Max_.Max(TmpMax);
				}
			}
		}
		else
		{
			const CollisionAABB& CurrentBox = Current.GetNeg()->mAABB;
			CurrentBox.GetMin(Min_);
			CurrentBox.GetMax(Max_);
		}
#ifdef OPC_USE_FCOMI
		Min.x = FCMin2(Min.x, Min_.x);
		Max.x = FCMax2(Max.x, Max_.x);
		Min.y = FCMin2(Min.y, Min_.y);
		Max.y = FCMax2(Max.y, Max_.y);
		Min.z = FCMin2(Min.z, Min_.z);
		Max.z = FCMax2(Max.z, Max_.z);
#else
		Min.Min(Min_);
		Max.Max(Max_);
#endif
		Current.mAABB.SetMinMax(Min, Max);
	}
	return true;
}