OPC_SweepAndPrune.cpp

Go to the documentation of this file.

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



// 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?1:0);  }
        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;
            }
        }
    };











SAP_PairData::SAP_PairData() :
    mNbElements     (0),
    mNbUsedElements (0),
    mElementPool    (null),
    mFirstFree      (null),
    mNbObjects      (0),
    mArray          (null)
{
}


SAP_PairData::~SAP_PairData()
{
    Release();
}

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


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;
}


inline_ void Remap(SAP_Element*& element, udword delta)
{
    if(element) element = (SAP_Element*)(uintptr_t(element) + delta);
}


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 = uintptr_t(NewElems) - uintptr_t(mElementPool);

                udword i;
                for(i=0;i<mNbUsedElements;i++)  Remap(NewElems[i].mNext, Delta);
                for(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;
}


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);

    OPASSERT(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)
        {
            OPASSERT(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)
        {
            OPASSERT(Current->mID<mNbObjects);

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





























SweepAndPrune::SweepAndPrune()
{
}


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];

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

            OPASSERT(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 OPASSERT(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)
            {
                OPASSERT(Previous->Value <= Current->Value);
                if(Previous->Value > Current->Value)    return false;
            }

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

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

        OPASSERT(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];
            OPASSERT(!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;
                OPASSERT(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;
                OPASSERT(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];
            OPASSERT(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;
                OPASSERT(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;
                OPASSERT(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;
}