diff options
Diffstat (limited to 'contrib/Opcode/OPC_SweepAndPrune.cpp')
| -rw-r--r-- | contrib/Opcode/OPC_SweepAndPrune.cpp | 664 |
1 files changed, 0 insertions, 664 deletions
diff --git a/contrib/Opcode/OPC_SweepAndPrune.cpp b/contrib/Opcode/OPC_SweepAndPrune.cpp deleted file mode 100644 index e56e7ab..0000000 --- a/contrib/Opcode/OPC_SweepAndPrune.cpp +++ /dev/null @@ -1,664 +0,0 @@ -/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// -/* - * 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, udword delta) -{ - if(element) element = (SAP_Element*)(udword(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 - { - udword Delta = udword(NewElems) - udword(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; -} |