from the start... checking in ode-0.9

1 files changed, 664 insertions, 0 deletions

diff --git a/libraries/ode-0.9/OPCODE/OPC_SweepAndPrune.cpp b/libraries/ode-0.9/OPCODE/OPC_SweepAndPrune.cpp
new file mode 100644
index 0000000..7f1a8f3
--- /dev/null
+++ b/libraries/ode-0.9/OPCODE/OPC_SweepAndPrune.cpp
@@ -0,0 +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, 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;
+}