path: root/libraries/ode-0.9/OPCODE/OPC_SweepAndPrune.cpp

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

///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
 *	Contains an implementation of the sweep-and-prune algorithm (moved from Z-Collide)
 *	\file		OPC_SweepAndPrune.cpp
 *	\author		Pierre Terdiman
 *	\date		January, 29, 2000
 */
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

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

using namespace Opcode;

inline_ void Sort(udword& id0, udword& id1)
{
	if(id0>id1)	Swap(id0, id1);
}

	class Opcode::SAP_Element
	{
		public:
		inline_					SAP_Element()														{}
		inline_					SAP_Element(udword id, SAP_Element* next) : mID(id), mNext(next)	{}
		inline_					~SAP_Element()														{}

				udword			mID;
				SAP_Element*	mNext;
	};

	class Opcode::SAP_Box
	{
		public:
				SAP_EndPoint*	Min[3];
				SAP_EndPoint*	Max[3];
	};

	class Opcode::SAP_EndPoint
	{
		public:
				float			Value;		// Min or Max value
				SAP_EndPoint*	Previous;	// Previous EndPoint whose Value is smaller than ours (or null)
				SAP_EndPoint*	Next;		// Next EndPoint whose Value is greater than ours (or null)
				udword			Data;		// Parent box ID *2 | MinMax flag

		inline_	void			SetData(udword box_id, BOOL is_max)			{ Data = (box_id<<1)|is_max;	}
		inline_	BOOL			IsMax()								const	{ return Data & 1;				}
		inline_	udword			GetBoxID()							const	{ return Data>>1;				}

		inline_	void InsertAfter(SAP_EndPoint* element)
		{
			if(this!=element && this!=element->Next)
			{
				// Remove
				if(Previous)	Previous->Next = Next;
				if(Next)		Next->Previous = Previous;

				// Insert
				Next = element->Next;
				if(Next)	Next->Previous = this;

				element->Next = this;
				Previous = element;
			}
		}

		inline_	void InsertBefore(SAP_EndPoint* element)
		{
			if(this!=element && this!=element->Previous)
			{
				// Remove
				if(Previous)	Previous->Next = Next;
				if(Next)		Next->Previous = Previous;

				// Insert
				Previous = element->Previous;
				element->Previous = this;

				Next = element;
				if(Previous)	Previous->Next = this;
			}
		}
	};










///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
 *	Constructor.
 */
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
SAP_PairData::SAP_PairData() :
	mNbElements		(0),
	mNbUsedElements	(0),
	mElementPool	(null),
	mFirstFree		(null),
	mNbObjects		(0),
	mArray			(null)
{
}

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

void SAP_PairData::Release()
{
	mNbElements		= 0;
	mNbUsedElements	= 0;
	mNbObjects		= 0;
	DELETEARRAY(mElementPool);
	DELETEARRAY(mArray);
}

///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
 *	Initializes.
 *	\param		nb_objects	[in] 
 *	\return		true if success
 */
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
bool SAP_PairData::Init(udword nb_objects)
{
	// Make sure everything has been released
	Release();
	if(!nb_objects)	return false;

	mArray = new SAP_Element*[nb_objects];
	CHECKALLOC(mArray);
	ZeroMemory(mArray, nb_objects*sizeof(SAP_Element*));
	mNbObjects = nb_objects;

	return true;
}

///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
 *	Remaps a pointer when pool gets resized.
 *	\param		element	[in/out] remapped element
 *	\param		delta	[in] offset in bytes
 */
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
inline_ void Remap(SAP_Element*& element, size_t delta)
{
	if(element)	element = (SAP_Element*)(size_t(element) + delta);
}

///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
 *	Gets a free element in the pool.
 *	\param		id		[in] element id
 *	\param		next	[in] next element
 *	\param		remap	[out] possible remapping offset
 *	\return		the new element
 */
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
SAP_Element* SAP_PairData::GetFreeElem(udword id, SAP_Element* next, udword* remap)
{
	if(remap)	*remap = 0;

	SAP_Element* FreeElem;
	if(mFirstFree)
	{
		// Recycle
		FreeElem = mFirstFree;
		mFirstFree = mFirstFree->mNext;	// First free = next free (or null)
	}
	else
	{
		if(mNbUsedElements==mNbElements)
		{
			// Resize
			mNbElements = mNbElements ? (mNbElements<<1) : 2;

			SAP_Element* NewElems = new SAP_Element[mNbElements];

			if(mNbUsedElements)	CopyMemory(NewElems, mElementPool, mNbUsedElements*sizeof(SAP_Element));

			// Remap everything
			{
				size_t Delta = size_t(NewElems) - size_t(mElementPool);

				for(udword i=0;i<mNbUsedElements;i++)	Remap(NewElems[i].mNext, Delta);
				for(udword i=0;i<mNbObjects;i++)		Remap(mArray[i], Delta);

				Remap(mFirstFree, Delta);
				Remap(next, Delta);

				if(remap)	*remap = Delta;
			}

			DELETEARRAY(mElementPool);
			mElementPool = NewElems;
		}

		FreeElem = &mElementPool[mNbUsedElements++];
	}

	FreeElem->mID = id;
	FreeElem->mNext = next;

	return FreeElem;
}

///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
 *	Frees an element of the pool.
 *	\param		elem	[in] element to free/recycle
 */
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
inline_ void SAP_PairData::FreeElem(SAP_Element* elem)
{
	elem->mNext = mFirstFree;	// Next free
	mFirstFree = elem;
}

// Add a pair to the set.
void SAP_PairData::AddPair(udword id1, udword id2)
{
	// Order the ids
	Sort(id1, id2);

	ASSERT(id1<mNbObjects);
	if(id1>=mNbObjects)	return;

	// Select the right list from "mArray".
	SAP_Element* Current = mArray[id1];

	if(!Current)
	{
		// Empty slot => create new element
		mArray[id1] = GetFreeElem(id2, null);
	}
	else if(Current->mID>id2)
	{
		// The list is not empty but all elements are greater than id2 => insert id2 in the front.
		mArray[id1]	= GetFreeElem(id2, mArray[id1]);
	}
	else
	{
		// Else find the correct location in the sorted list (ascending order) and insert id2 there.
		while(Current->mNext)
		{
			if(Current->mNext->mID > id2)	break;

			Current = Current->mNext;
		}

		if(Current->mID==id2)	return;	// The pair already exists
		
//		Current->mNext = GetFreeElem(id2, Current->mNext);
		udword Delta;
		SAP_Element* E = GetFreeElem(id2, Current->mNext, &Delta);
		if(Delta)	Remap(Current, Delta);
		Current->mNext = E;
	}
}

// Delete a pair from the set.
void SAP_PairData::RemovePair(udword id1, udword id2)
{
	// Order the ids.
	Sort(id1, id2);

	// Exit if the pair doesn't exist in the set
	if(id1>=mNbObjects)	return;

	// Otherwise, select the correct list.
	SAP_Element* Current = mArray[id1];

	// If this list is empty, the pair doesn't exist.
	if(!Current) return;

	// Otherwise, if id2 is the first element, delete it.
	if(Current->mID==id2)
	{
		mArray[id1] = Current->mNext;
		FreeElem(Current);
	}
	else
	{
		// If id2 is not the first element, start traversing the sorted list.
		while(Current->mNext)
		{
			// If we have moved too far away without hitting id2, then the pair doesn't exist
			if(Current->mNext->mID > id2)	return;

			// Otherwise, delete id2.
			if(Current->mNext->mID == id2)
			{
				SAP_Element* Temp = Current->mNext;
				Current->mNext = Temp->mNext;
				FreeElem(Temp);
				return;
			}
			Current = Current->mNext;
		}
	}
}

void SAP_PairData::DumpPairs(Pairs& pairs) const
{
	// ### Ugly and slow
	for(udword i=0;i<mNbObjects;i++)
	{
		SAP_Element* Current = mArray[i];
		while(Current)
		{
			ASSERT(Current->mID<mNbObjects);

			pairs.AddPair(i, Current->mID);
			Current = Current->mNext;
		}
	}
}

void SAP_PairData::DumpPairs(PairCallback callback, void* user_data) const
{
	if(!callback)	return;

	// ### Ugly and slow
	for(udword i=0;i<mNbObjects;i++)
	{
		SAP_Element* Current = mArray[i];
		while(Current)
		{
			ASSERT(Current->mID<mNbObjects);

			if(!(callback)(i, Current->mID, user_data))	return;
			Current = Current->mNext;
		}
	}
}




























///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
 *	Constructor.
 */
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
SweepAndPrune::SweepAndPrune()
{
}

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

void SweepAndPrune::GetPairs(Pairs& pairs) const
{
	mPairs.DumpPairs(pairs);
}

void SweepAndPrune::GetPairs(PairCallback callback, void* user_data) const
{
	mPairs.DumpPairs(callback, user_data);
}

bool SweepAndPrune::Init(udword nb_objects, const AABB** boxes)
{
	// 1) Create sorted lists
	mNbObjects = nb_objects;

	mBoxes = new SAP_Box[nb_objects];
//	for(udword i=0;i<nb_objects;i++)	mBoxes[i].Box = *boxes[i];

	float* Data = new float[nb_objects*2];

	for(udword Axis=0;Axis<3;Axis++)
	{
		mList[Axis] = new SAP_EndPoint[nb_objects*2];

		for(udword i=0;i<nb_objects;i++)
		{
			Data[i*2+0] = boxes[i]->GetMin(Axis);
			Data[i*2+1] = boxes[i]->GetMax(Axis);
		}
		RadixSort RS;
		const udword* Sorted = RS.Sort(Data, nb_objects*2).GetRanks();

		SAP_EndPoint* PreviousEndPoint = null;

		for(udword i=0;i<nb_objects*2;i++)
		{
			udword SortedIndex	= *Sorted++;
			float SortedCoord	= Data[SortedIndex];
			udword BoxIndex		= SortedIndex>>1;

			ASSERT(BoxIndex<nb_objects);

			SAP_EndPoint* CurrentEndPoint = &mList[Axis][SortedIndex];
			CurrentEndPoint->Value		= SortedCoord;
//			CurrentEndPoint->IsMax		= SortedIndex&1;		// ### could be implicit ?
//			CurrentEndPoint->ID			= BoxIndex;				// ### could be implicit ?
			CurrentEndPoint->SetData(BoxIndex, SortedIndex&1);	// ### could be implicit ?
			CurrentEndPoint->Previous	= PreviousEndPoint;
			CurrentEndPoint->Next		= null;
			if(PreviousEndPoint)	PreviousEndPoint->Next = CurrentEndPoint;

			if(CurrentEndPoint->IsMax())	mBoxes[BoxIndex].Max[Axis] = CurrentEndPoint;
			else							mBoxes[BoxIndex].Min[Axis] = CurrentEndPoint;

			PreviousEndPoint = CurrentEndPoint;
		}
	}

	DELETEARRAY(Data);

	CheckListsIntegrity();

	// 2) Quickly find starting pairs

	mPairs.Init(nb_objects);

	{
		Pairs P;
		CompleteBoxPruning(nb_objects, boxes, P, Axes(AXES_XZY));
		for(udword i=0;i<P.GetNbPairs();i++)
		{
			const Pair* PP = P.GetPair(i);

			udword id0 = PP->id0;
			udword id1 = PP->id1;

			if(id0!=id1 && boxes[id0]->Intersect(*boxes[id1]))
			{
				mPairs.AddPair(id0, id1);
			}
			else ASSERT(0);
		}
	}

	return true;
}

bool SweepAndPrune::CheckListsIntegrity()
{
	for(udword Axis=0;Axis<3;Axis++)
	{
		// Find list head
		SAP_EndPoint* Current = mList[Axis];
		while(Current->Previous)	Current = Current->Previous;

		udword Nb = 0;

		SAP_EndPoint* Previous = null;
		while(Current)
		{
			Nb++;

			if(Previous)
			{
				ASSERT(Previous->Value <= Current->Value);
				if(Previous->Value > Current->Value)	return false;
			}

			ASSERT(Current->Previous==Previous);
			if(Current->Previous!=Previous)	return false;

			Previous = Current;
			Current = Current->Next;
		}

		ASSERT(Nb==mNbObjects*2);
	}
	return true;
}

inline_ BOOL Intersect(const AABB& a, const SAP_Box& b)
{
	if(b.Max[0]->Value < a.GetMin(0) || a.GetMax(0) < b.Min[0]->Value
	|| b.Max[1]->Value < a.GetMin(1) || a.GetMax(1) < b.Min[1]->Value
	|| b.Max[2]->Value < a.GetMin(2) || a.GetMax(2) < b.Min[2]->Value)	return FALSE;

	return TRUE;
}



bool SweepAndPrune::UpdateObject(udword i, const AABB& box)
{
	for(udword Axis=0;Axis<3;Axis++)
	{
//		udword Base = (udword)&mList[Axis][0];

		// Update min
		{
			SAP_EndPoint* const CurrentMin = mBoxes[i].Min[Axis];
			ASSERT(!CurrentMin->IsMax());

			const float Limit = box.GetMin(Axis);
			if(Limit == CurrentMin->Value)
			{
			}
			else if(Limit < CurrentMin->Value)
			{
				CurrentMin->Value = Limit;

				// Min is moving left:
				SAP_EndPoint* NewPos = CurrentMin;
				ASSERT(NewPos);

				SAP_EndPoint* tmp;
				while((tmp = NewPos->Previous) && tmp->Value > Limit)
				{
					NewPos = tmp;

					if(NewPos->IsMax())
					{
						// Our min passed a max => start overlap
						//udword SortedIndex = (udword(CurrentMin) - Base)/sizeof(NS_EndPoint);
						const udword id0 = CurrentMin->GetBoxID();
						const udword id1 = NewPos->GetBoxID();

						if(id0!=id1 && Intersect(box, mBoxes[id1]))	mPairs.AddPair(id0, id1);
					}
				}

				CurrentMin->InsertBefore(NewPos);
			}
			else// if(Limit > CurrentMin->Value)
			{
				CurrentMin->Value = Limit;

				// Min is moving right:
				SAP_EndPoint* NewPos = CurrentMin;
				ASSERT(NewPos);

				SAP_EndPoint* tmp;
				while((tmp = NewPos->Next) && tmp->Value < Limit)
				{
					NewPos = tmp;

					if(NewPos->IsMax())
					{
						// Our min passed a max => stop overlap
						const udword id0 = CurrentMin->GetBoxID();
						const udword id1 = NewPos->GetBoxID();

						if(id0!=id1)	mPairs.RemovePair(id0, id1);
					}
				}

				CurrentMin->InsertAfter(NewPos);
			}
		}

		// Update max
		{
			SAP_EndPoint* const CurrentMax = mBoxes[i].Max[Axis];
			ASSERT(CurrentMax->IsMax());

			const float Limit = box.GetMax(Axis);
			if(Limit == CurrentMax->Value)
			{
			}
			else if(Limit > CurrentMax->Value)
			{
				CurrentMax->Value = Limit;

				// Max is moving right:
				SAP_EndPoint* NewPos = CurrentMax;
				ASSERT(NewPos);

				SAP_EndPoint* tmp;
				while((tmp = NewPos->Next) && tmp->Value < Limit)
				{
					NewPos = tmp;

					if(!NewPos->IsMax())
					{
						// Our max passed a min => start overlap
						const udword id0 = CurrentMax->GetBoxID();
						const udword id1 = NewPos->GetBoxID();

						if(id0!=id1 && Intersect(box, mBoxes[id1]))	mPairs.AddPair(id0, id1);
					}
				}

				CurrentMax->InsertAfter(NewPos);
			}
			else// if(Limit < CurrentMax->Value)
			{
				CurrentMax->Value = Limit;

				// Max is moving left:
				SAP_EndPoint* NewPos = CurrentMax;
				ASSERT(NewPos);

				SAP_EndPoint* tmp;
				while((tmp = NewPos->Previous) && tmp->Value > Limit)
				{
					NewPos = tmp;

					if(!NewPos->IsMax())
					{
						// Our max passed a min => stop overlap
						const udword id0 = CurrentMax->GetBoxID();
						const udword id1 = NewPos->GetBoxID();

						if(id0!=id1)	mPairs.RemovePair(id0, id1);
					}
				}

				CurrentMax->InsertBefore(NewPos);
			}
		}
	}

	return true;
}