diff options
Diffstat (limited to 'Opcode/OPC_SweepAndPrune.cpp')
| -rw-r--r-- | Opcode/OPC_SweepAndPrune.cpp | 1328 |
1 files changed, 664 insertions, 664 deletions
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;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;
-}
+/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +/* + * 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; +} |