From c01469dddabe404506ef3a64542e8423f9e11f2c Mon Sep 17 00:00:00 2001 From: Aki Date: Sun, 30 Jan 2022 17:44:05 +0100 Subject: Converted Opcode and Ice into unix newlines format --- Opcode/OPC_SweepAndPrune.cpp | 1328 +++++++++++++++++++++--------------------- 1 file changed, 664 insertions(+), 664 deletions(-) (limited to 'Opcode/OPC_SweepAndPrune.cpp') diff --git a/Opcode/OPC_SweepAndPrune.cpp b/Opcode/OPC_SweepAndPrune.cpp index 2e60ca8..e56e7ab 100644 --- a/Opcode/OPC_SweepAndPrune.cpp +++ b/Opcode/OPC_SweepAndPrune.cpp @@ -1,664 +1,664 @@ -/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// -/* - * 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;imID = 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) 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;imIDmID); - Current = Current->mNext; - } - } -} - -void SAP_PairData::DumpPairs(PairCallback callback, void* user_data) const -{ - if(!callback) return; - - // ### Ugly and slow - for(udword i=0;imIDmID, 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;iGetMin(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>1; - - ASSERT(BoxIndexValue = 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;iid0; - 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; -} +/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +/* + * 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;imID = 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) 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;imIDmID); + Current = Current->mNext; + } + } +} + +void SAP_PairData::DumpPairs(PairCallback callback, void* user_data) const +{ + if(!callback) return; + + // ### Ugly and slow + for(udword i=0;imIDmID, 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;iGetMin(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>1; + + ASSERT(BoxIndexValue = 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;iid0; + 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; +} -- cgit v1.1