opc_treecollider.cpp

opcode是功能强大

///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/*
 *	OPCODE - Optimized Collision Detection
 *	Copyright (C) 2001 Pierre Terdiman
 *	Homepage: http://www.codercorner.com/Opcode.htm
 *
 *  OPCODE modifications for scaled model support (and other things)
 *  Copyright (C) 2004 Gilvan Maia (gilvan 'at' vdl.ufc.br)
 *	Check http://www.vdl.ufc.br/gilvan/coll/opcode/index.htm for updates.
 *
 */
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
 *	Contains code for a tree collider.
 *	\file		OPC_TreeCollider.cpp
 *	\author		Pierre Terdiman
 *	\date		March, 20, 2001
 */
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
 *	Contains an AABB tree collider.
 *	This class performs a collision test between two AABB trees.
 *  This class had changed a bit since jan/2005 in order to support scaled models.
 *
 *	\class		AABBTreeCollider
 *	\author		Pierre Terdiman
 *	\version	1.3
 *	\date		March, 20, 2001
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

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

using namespace Opcode;
using namespace IceMaths;

#include "Opcode/OPC_BoxBoxOverlap.h"
#include "Opcode/OPC_TriBoxOverlap.h"

// The tri-tri overlap
#include "Opcode/OPC_TriTriOverlap.h"  // Standard OPCODE's tri-tri overlap routine (by Pierre)
// #include "OPC_TriTriOverlapGilvan.h" // An optional tri-tri overlap routine based on SAT - Separating Axis Theorem (by Gilvan)



///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
 *	Constructor.
 */
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
AABBTreeCollider::AABBTreeCollider() :
	mNbBVBVTests		(0),
	mNbPrimPrimTests	(0),
	mNbBVPrimTests		(0),
	mScale0				(1.0,1.0,1.0),
	mScale1				(1.0,1.0,1.0),
	mFullBoxBoxTest		(true),
	mFullPrimBoxTest	(true),
	mIMesh0				(null),
	mIMesh1				(null)
{
}

///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
 *	Destructor.
 */
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
AABBTreeCollider::~AABBTreeCollider()
{
}

///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
 *	Validates current settings. You should call this method after all the settings and callbacks have been defined.
 *	\return		null if everything is ok, else a string describing the problem
 */
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
const char* AABBTreeCollider::ValidateSettings()
{
	if(TemporalCoherenceEnabled() && !FirstContactEnabled())	return "Temporal coherence only works with ""First contact"" mode!";
	return null;
}

///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
 *	Generic collision query for generic OPCODE models. After the call, access the results with:
 *	- GetContactStatus()
 *	- GetNbPairs()
 *	- GetPairs()
 *
 *	\param		cache			[in] collision cache for model pointers and a colliding pair of primitives
 *	\param		world0			[in] world matrix for first object
 *	\param		world1			[in] world matrix for second object
 *	\return		true if success 
 */
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
bool AABBTreeCollider::Collide(BVTCache& cache, const IceMaths::Matrix4x4* world0, const IceMaths::Matrix4x4* world1)
{
	// Checkings: olny works for corresponding models(leaf style and quantization )
	if(!cache.Model0 || !cache.Model1)								return false;
	if(cache.Model0->HasLeafNodes()!=cache.Model1->HasLeafNodes())	return false;
	if(cache.Model0->IsQuantized()!=cache.Model1->IsQuantized())	return false;

	/*
	
	  Rules:
		- perform hull test
		- when hulls collide, disable hull test
		- if meshes overlap, reset countdown
		- if countdown reaches 0, enable hull test

	*/

#ifdef __MESHMERIZER_H__
	// Handle hulls
	if(cache.HullTest)
	{
		if(cache.Model0->GetHull() && cache.Model1->GetHull())
		{
			struct Local
			{
				static IceMaths::Point* SVCallback(const IceMaths::Point& sv, udword& previndex, udword user_data)
				{
					CollisionHull* Hull = (CollisionHull*)user_data;
					previndex = Hull->ComputeSupportingVertex(sv, previndex);
					return (IceMaths::Point*)&Hull->GetVerts()[previndex];
				}
			};

			bool Collide;

			if(0)
			{
				static GJKEngine GJK;
				static bool GJKInitDone=false;
				if(!GJKInitDone)
				{
					GJK.Enable(GJK_BACKUP_PROCEDURE);
					GJK.Enable(GJK_DEGENERATE);
					GJK.Enable(GJK_HILLCLIMBING);
					GJKInitDone = true;
				}
				GJK.SetCallbackObj0(Local::SVCallback);
				GJK.SetCallbackObj1(Local::SVCallback);
				GJK.SetUserData0(udword(cache.Model0->GetHull()));
				GJK.SetUserData1(udword(cache.Model1->GetHull()));
				Collide = GJK.Collide(*world0, *world1, &cache.SepVector);
			}
			else
			{
				static SVEngine SVE;
				SVE.SetCallbackObj0(Local::SVCallback);
				SVE.SetCallbackObj1(Local::SVCallback);
				SVE.SetUserData0(udword(cache.Model0->GetHull()));
				SVE.SetUserData1(udword(cache.Model1->GetHull()));
				Collide = SVE.Collide(*world0, *world1, &cache.SepVector);
			}

			if(!Collide)
			{
				// Reset stats & contact status
				mFlags &= ~OPC_CONTACT;
				mNbBVBVTests		= 0;
				mNbPrimPrimTests	= 0;
				mNbBVPrimTests		= 0;
				mPairs.Reset();
				return true;
			}
		}
	}

	// Here, hulls collide
	cache.HullTest = false;
#endif // __MESHMERIZER_H__

	// Checkings: was this modified by someone? Why?
	// mFlags &= ~OPC_CONTACT;
	if(!Setup(cache.Model0->GetMeshInterface(), cache.Model1->GetMeshInterface()))	return false;

	// Simple double-dispatch
	bool Status;
	if(!cache.Model0->HasLeafNodes())
	{
		if(cache.Model0->IsQuantized())
		{
			const AABBQuantizedNoLeafTree* T0 = (const AABBQuantizedNoLeafTree*)cache.Model0->GetTree();
			const AABBQuantizedNoLeafTree* T1 = (const AABBQuantizedNoLeafTree*)cache.Model1->GetTree();
			Status = Collide(T0, T1, world0, world1, &cache);
		}
		else
		{
			const AABBNoLeafTree* T0 = (const AABBNoLeafTree*)cache.Model0->GetTree();
			const AABBNoLeafTree* T1 = (const AABBNoLeafTree*)cache.Model1->GetTree();
			Status = Collide(T0, T1, world0, world1, &cache);
		}
	}
	else
	{
		if(cache.Model0->IsQuantized())
		{
			const AABBQuantizedTree* T0 = (const AABBQuantizedTree*)cache.Model0->GetTree();
			const AABBQuantizedTree* T1 = (const AABBQuantizedTree*)cache.Model1->GetTree();
			Status = Collide(T0, T1, world0, world1, &cache);
		}
		else
		{
			const AABBCollisionTree* T0 = (const AABBCollisionTree*)cache.Model0->GetTree();
			const AABBCollisionTree* T1 = (const AABBCollisionTree*)cache.Model1->GetTree();
			Status = Collide(T0, T1, world0, world1, &cache);
		}
	}

#ifdef __MESHMERIZER_H__
	if(Status)
	{
		// Reset counter as long as overlap occurs
		if(GetContactStatus())	cache.ResetCountDown();

		// Enable hull test again when counter reaches zero
		cache.CountDown--;
		if(!cache.CountDown)
		{
			cache.ResetCountDown();
			cache.HullTest = true;
		}
	}
#endif
	return Status;
}

///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
 *	Initializes a collision query :
 *	- reset stats & contact status
 *	- setup matrices
 *
 *	\param		world0			[in] world matrix for first object
 *	\param		world1			[in] world matrix for second object
 */
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
void AABBTreeCollider::InitQuery(const IceMaths::Matrix4x4* world0, const IceMaths::Matrix4x4* world1)
{
	// Reset stats & contact status
	Collider::InitQuery();
	mNbBVBVTests		= 0;
	mNbPrimPrimTests	= 0;
	mNbBVPrimTests		= 0;
	mPairs.Reset();

	// Setup matrices
	IceMaths::Matrix4x4 InvWorld0, InvWorld1;
	IceMaths::Matrix4x4 WorldM0, WorldM1; // normalized (rotation & translation parts)

	if(world0)
	{		
		NormalizePRSMatrix( WorldM0, mScale0,*world0);
		
		InvertPRMatrix(InvWorld0, WorldM0);
	}
	else
	{
		mScale0.Set(1.0,1.0,1.0);		
		InvWorld0.Identity();
	}

	if(world1)
	{
		NormalizePRSMatrix( WorldM1, mScale1,*world1);		
		
		InvertPRMatrix(InvWorld1, WorldM1);
	}
	else
	{
		mScale1.Set(1.0,1.0,1.0);		
		InvWorld1.Identity();
	}

	IceMaths::Matrix4x4 World0to1 = world0 ? (WorldM0 * InvWorld1) : InvWorld1;
	IceMaths::Matrix4x4 World1to0 = world1 ? (WorldM1 * InvWorld0) : InvWorld0;
	// scale & rotation only
	mSR0to1 = world0 ? (*world0 * InvWorld1) : InvWorld1;
	mSR1to0 = world1 ? (*world1 * InvWorld0) : InvWorld0;
	// rotation & translation only
	mR0to1 = World0to1;		World0to1.GetTrans(mT0to1);
	mR1to0 = World1to0;		World1to0.GetTrans(mT1to0);

	// Precompute absolute 1-to-0 rotation matrix
	for(udword i=0;i<3;i++)
	{
		for(udword j=0;j<3;j++)
		{
			// Epsilon value prevents floating-point inaccuracies (strategy borrowed from RAPID)
			mAR.m[i][j] = 1e-6f + fabsf(mR1to0.m[i][j]);
		}
	}
}

///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
 *	Takes advantage of temporal coherence.
 *	\param		cache	[in] cache for a pair of previously colliding primitives
 *	\return		true if we can return immediately
 *	\warning	only works for "First Contact" mode
 */
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
bool AABBTreeCollider::CheckTemporalCoherence(Pair* cache)
{
	// Checkings
	if(!cache)	return false;

	// Test previously colliding primitives first
	if(TemporalCoherenceEnabled() && FirstContactEnabled())
	{
		PrimTest(cache->id0, cache->id1);
		if(GetContactStatus())	return true;
	}
	return false;
}