OPC_SweepAndPrune.cpp
Go to the documentation of this file.
00001 
00002 /*
00003  *      OPCODE - Optimized Collision Detection
00004  *      Copyright (C) 2001 Pierre Terdiman
00005  *      Homepage: http://www.codercorner.com/Opcode.htm
00006  */
00008 
00010 
00016 
00017 
00019 // Precompiled Header
00020 #include "Stdafx.h"
00021 
00022 using namespace Opcode;
00023 
00024 inline_ void Sort(udword& id0, udword& id1)
00025 {
00026         if(id0>id1)     Swap(id0, id1);
00027 }
00028 
00029         class Opcode::SAP_Element
00030         {
00031                 public:
00032                 inline_                                 SAP_Element()                                                                                                           {}
00033                 inline_                                 SAP_Element(udword id, SAP_Element* next) : mID(id), mNext(next)        {}
00034                 inline_                                 ~SAP_Element()                                                                                                          {}
00035 
00036                                 udword                  mID;
00037                                 SAP_Element*    mNext;
00038         };
00039 
00040         class Opcode::SAP_Box
00041         {
00042                 public:
00043                                 SAP_EndPoint*   Min[3];
00044                                 SAP_EndPoint*   Max[3];
00045         };
00046 
00047         class Opcode::SAP_EndPoint
00048         {
00049                 public:
00050                                 float                   Value;          // Min or Max value
00051                                 SAP_EndPoint*   Previous;       // Previous EndPoint whose Value is smaller than ours (or null)
00052                                 SAP_EndPoint*   Next;           // Next EndPoint whose Value is greater than ours (or null)
00053                                 udword                  Data;           // Parent box ID *2 | MinMax flag
00054 
00055                 inline_ void                    SetData(udword box_id, BOOL is_max)                     { Data = (box_id<<1)|is_max;    }
00056                 inline_ BOOL                    IsMax()                                                         const   { return Data & 1;                              }
00057                 inline_ udword                  GetBoxID()                                                      const   { return Data>>1;                               }
00058 
00059                 inline_ void InsertAfter(SAP_EndPoint* element)
00060                 {
00061                         if(this!=element && this!=element->Next)
00062                         {
00063                                 // Remove
00064                                 if(Previous)    Previous->Next = Next;
00065                                 if(Next)                Next->Previous = Previous;
00066 
00067                                 // Insert
00068                                 Next = element->Next;
00069                                 if(Next)        Next->Previous = this;
00070 
00071                                 element->Next = this;
00072                                 Previous = element;
00073                         }
00074                 }
00075 
00076                 inline_ void InsertBefore(SAP_EndPoint* element)
00077                 {
00078                         if(this!=element && this!=element->Previous)
00079                         {
00080                                 // Remove
00081                                 if(Previous)    Previous->Next = Next;
00082                                 if(Next)                Next->Previous = Previous;
00083 
00084                                 // Insert
00085                                 Previous = element->Previous;
00086                                 element->Previous = this;
00087 
00088                                 Next = element;
00089                                 if(Previous)    Previous->Next = this;
00090                         }
00091                 }
00092         };
00093 
00094 
00095 
00096 
00097 
00098 
00099 
00100 
00101 
00102 
00104 
00107 
00108 SAP_PairData::SAP_PairData() :
00109         mNbElements             (0),
00110         mNbUsedElements (0),
00111         mElementPool    (null),
00112         mFirstFree              (null),
00113         mNbObjects              (0),
00114         mArray                  (null)
00115 {
00116 }
00117 
00119 
00122 
00123 SAP_PairData::~SAP_PairData()
00124 {
00125         Release();
00126 }
00127 
00128 void SAP_PairData::Release()
00129 {
00130         mNbElements             = 0;
00131         mNbUsedElements = 0;
00132         mNbObjects              = 0;
00133         DELETEARRAY(mElementPool);
00134         DELETEARRAY(mArray);
00135 }
00136 
00138 
00143 
00144 bool SAP_PairData::Init(udword nb_objects)
00145 {
00146         // Make sure everything has been released
00147         Release();
00148         if(!nb_objects) return false;
00149 
00150         mArray = new SAP_Element*[nb_objects];
00151         CHECKALLOC(mArray);
00152         ZeroMemory(mArray, nb_objects*sizeof(SAP_Element*));
00153         mNbObjects = nb_objects;
00154 
00155         return true;
00156 }
00157 
00159 
00164 
00165 inline_ void Remap(SAP_Element*& element, udword delta)
00166 {
00167         if(element)     element = (SAP_Element*)(udword(element) + delta);
00168 }
00169 
00171 
00178 
00179 SAP_Element* SAP_PairData::GetFreeElem(udword id, SAP_Element* next, udword* remap)
00180 {
00181         if(remap)       *remap = 0;
00182 
00183         SAP_Element* FreeElem;
00184         if(mFirstFree)
00185         {
00186                 // Recycle
00187                 FreeElem = mFirstFree;
00188                 mFirstFree = mFirstFree->mNext; // First free = next free (or null)
00189         }
00190         else
00191         {
00192                 if(mNbUsedElements==mNbElements)
00193                 {
00194                         // Resize
00195                         mNbElements = mNbElements ? (mNbElements<<1) : 2;
00196 
00197                         SAP_Element* NewElems = new SAP_Element[mNbElements];
00198 
00199                         if(mNbUsedElements)     CopyMemory(NewElems, mElementPool, mNbUsedElements*sizeof(SAP_Element));
00200 
00201                         // Remap everything
00202                         {
00203                                 udword Delta = udword(NewElems) - udword(mElementPool);
00204 
00205                                 for(udword i=0;i<mNbUsedElements;i++)   Remap(NewElems[i].mNext, Delta);
00206                                 for(udword i=0;i<mNbObjects;i++)                Remap(mArray[i], Delta);
00207 
00208                                 Remap(mFirstFree, Delta);
00209                                 Remap(next, Delta);
00210 
00211                                 if(remap)       *remap = Delta;
00212                         }
00213 
00214                         DELETEARRAY(mElementPool);
00215                         mElementPool = NewElems;
00216                 }
00217 
00218                 FreeElem = &mElementPool[mNbUsedElements++];
00219         }
00220 
00221         FreeElem->mID = id;
00222         FreeElem->mNext = next;
00223 
00224         return FreeElem;
00225 }
00226 
00228 
00232 
00233 inline_ void SAP_PairData::FreeElem(SAP_Element* elem)
00234 {
00235         elem->mNext = mFirstFree;       // Next free
00236         mFirstFree = elem;
00237 }
00238 
00239 // Add a pair to the set.
00240 void SAP_PairData::AddPair(udword id1, udword id2)
00241 {
00242         // Order the ids
00243         Sort(id1, id2);
00244 
00245         ASSERT(id1<mNbObjects);
00246         if(id1>=mNbObjects)     return;
00247 
00248         // Select the right list from "mArray".
00249         SAP_Element* Current = mArray[id1];
00250 
00251         if(!Current)
00252         {
00253                 // Empty slot => create new element
00254                 mArray[id1] = GetFreeElem(id2, null);
00255         }
00256         else if(Current->mID>id2)
00257         {
00258                 // The list is not empty but all elements are greater than id2 => insert id2 in the front.
00259                 mArray[id1]     = GetFreeElem(id2, mArray[id1]);
00260         }
00261         else
00262         {
00263                 // Else find the correct location in the sorted list (ascending order) and insert id2 there.
00264                 while(Current->mNext)
00265                 {
00266                         if(Current->mNext->mID > id2)   break;
00267 
00268                         Current = Current->mNext;
00269                 }
00270 
00271                 if(Current->mID==id2)   return; // The pair already exists
00272                 
00273 //              Current->mNext = GetFreeElem(id2, Current->mNext);
00274                 udword Delta;
00275                 SAP_Element* E = GetFreeElem(id2, Current->mNext, &Delta);
00276                 if(Delta)       Remap(Current, Delta);
00277                 Current->mNext = E;
00278         }
00279 }
00280 
00281 // Delete a pair from the set.
00282 void SAP_PairData::RemovePair(udword id1, udword id2)
00283 {
00284         // Order the ids.
00285         Sort(id1, id2);
00286 
00287         // Exit if the pair doesn't exist in the set
00288         if(id1>=mNbObjects)     return;
00289 
00290         // Otherwise, select the correct list.
00291         SAP_Element* Current = mArray[id1];
00292 
00293         // If this list is empty, the pair doesn't exist.
00294         if(!Current) return;
00295 
00296         // Otherwise, if id2 is the first element, delete it.
00297         if(Current->mID==id2)
00298         {
00299                 mArray[id1] = Current->mNext;
00300                 FreeElem(Current);
00301         }
00302         else
00303         {
00304                 // If id2 is not the first element, start traversing the sorted list.
00305                 while(Current->mNext)
00306                 {
00307                         // If we have moved too far away without hitting id2, then the pair doesn't exist
00308                         if(Current->mNext->mID > id2)   return;
00309 
00310                         // Otherwise, delete id2.
00311                         if(Current->mNext->mID == id2)
00312                         {
00313                                 SAP_Element* Temp = Current->mNext;
00314                                 Current->mNext = Temp->mNext;
00315                                 FreeElem(Temp);
00316                                 return;
00317                         }
00318                         Current = Current->mNext;
00319                 }
00320         }
00321 }
00322 
00323 void SAP_PairData::DumpPairs(Pairs& pairs) const
00324 {
00325         // ### Ugly and slow
00326         for(udword i=0;i<mNbObjects;i++)
00327         {
00328                 SAP_Element* Current = mArray[i];
00329                 while(Current)
00330                 {
00331                         ASSERT(Current->mID<mNbObjects);
00332 
00333                         pairs.AddPair(i, Current->mID);
00334                         Current = Current->mNext;
00335                 }
00336         }
00337 }
00338 
00339 void SAP_PairData::DumpPairs(PairCallback callback, void* user_data) const
00340 {
00341         if(!callback)   return;
00342 
00343         // ### Ugly and slow
00344         for(udword i=0;i<mNbObjects;i++)
00345         {
00346                 SAP_Element* Current = mArray[i];
00347                 while(Current)
00348                 {
00349                         ASSERT(Current->mID<mNbObjects);
00350 
00351                         if(!(callback)(i, Current->mID, user_data))     return;
00352                         Current = Current->mNext;
00353                 }
00354         }
00355 }
00356 
00357 
00358 
00359 
00360 
00361 
00362 
00363 
00364 
00365 
00366 
00367 
00368 
00369 
00370 
00371 
00372 
00373 
00374 
00375 
00376 
00377 
00378 
00379 
00380 
00381 
00382 
00383 
00385 
00388 
00389 SweepAndPrune::SweepAndPrune()
00390 {
00391 }
00392 
00394 
00397 
00398 SweepAndPrune::~SweepAndPrune()
00399 {
00400 }
00401 
00402 void SweepAndPrune::GetPairs(Pairs& pairs) const
00403 {
00404         mPairs.DumpPairs(pairs);
00405 }
00406 
00407 void SweepAndPrune::GetPairs(PairCallback callback, void* user_data) const
00408 {
00409         mPairs.DumpPairs(callback, user_data);
00410 }
00411 
00412 bool SweepAndPrune::Init(udword nb_objects, const AABB** boxes)
00413 {
00414         // 1) Create sorted lists
00415         mNbObjects = nb_objects;
00416 
00417         mBoxes = new SAP_Box[nb_objects];
00418 //      for(udword i=0;i<nb_objects;i++)        mBoxes[i].Box = *boxes[i];
00419 
00420         float* Data = new float[nb_objects*2];
00421 
00422         for(udword Axis=0;Axis<3;Axis++)
00423         {
00424                 mList[Axis] = new SAP_EndPoint[nb_objects*2];
00425 
00426                 for(udword i=0;i<nb_objects;i++)
00427                 {
00428                         Data[i*2+0] = boxes[i]->GetMin(Axis);
00429                         Data[i*2+1] = boxes[i]->GetMax(Axis);
00430                 }
00431                 RadixSort RS;
00432                 const udword* Sorted = RS.Sort(Data, nb_objects*2).GetRanks();
00433 
00434                 SAP_EndPoint* PreviousEndPoint = null;
00435 
00436                 for(udword i=0;i<nb_objects*2;i++)
00437                 {
00438                         udword SortedIndex      = *Sorted++;
00439                         float SortedCoord       = Data[SortedIndex];
00440                         udword BoxIndex         = SortedIndex>>1;
00441 
00442                         ASSERT(BoxIndex<nb_objects);
00443 
00444                         SAP_EndPoint* CurrentEndPoint = &mList[Axis][SortedIndex];
00445                         CurrentEndPoint->Value          = SortedCoord;
00446 //                      CurrentEndPoint->IsMax          = SortedIndex&1;                // ### could be implicit ?
00447 //                      CurrentEndPoint->ID                     = BoxIndex;                             // ### could be implicit ?
00448                         CurrentEndPoint->SetData(BoxIndex, SortedIndex&1);      // ### could be implicit ?
00449                         CurrentEndPoint->Previous       = PreviousEndPoint;
00450                         CurrentEndPoint->Next           = null;
00451                         if(PreviousEndPoint)    PreviousEndPoint->Next = CurrentEndPoint;
00452 
00453                         if(CurrentEndPoint->IsMax())    mBoxes[BoxIndex].Max[Axis] = CurrentEndPoint;
00454                         else                                                    mBoxes[BoxIndex].Min[Axis] = CurrentEndPoint;
00455 
00456                         PreviousEndPoint = CurrentEndPoint;
00457                 }
00458         }
00459 
00460         DELETEARRAY(Data);
00461 
00462         CheckListsIntegrity();
00463 
00464         // 2) Quickly find starting pairs
00465 
00466         mPairs.Init(nb_objects);
00467 
00468         {
00469                 Pairs P;
00470                 CompleteBoxPruning(nb_objects, boxes, P, Axes(AXES_XZY));
00471                 for(udword i=0;i<P.GetNbPairs();i++)
00472                 {
00473                         const Pair* PP = P.GetPair(i);
00474 
00475                         udword id0 = PP->id0;
00476                         udword id1 = PP->id1;
00477 
00478                         if(id0!=id1 && boxes[id0]->Intersect(*boxes[id1]))
00479                         {
00480                                 mPairs.AddPair(id0, id1);
00481                         }
00482                         else ASSERT(0);
00483                 }
00484         }
00485 
00486         return true;
00487 }
00488 
00489 bool SweepAndPrune::CheckListsIntegrity()
00490 {
00491         for(udword Axis=0;Axis<3;Axis++)
00492         {
00493                 // Find list head
00494                 SAP_EndPoint* Current = mList[Axis];
00495                 while(Current->Previous)        Current = Current->Previous;
00496 
00497                 udword Nb = 0;
00498 
00499                 SAP_EndPoint* Previous = null;
00500                 while(Current)
00501                 {
00502                         Nb++;
00503 
00504                         if(Previous)
00505                         {
00506                                 ASSERT(Previous->Value <= Current->Value);
00507                                 if(Previous->Value > Current->Value)    return false;
00508                         }
00509 
00510                         ASSERT(Current->Previous==Previous);
00511                         if(Current->Previous!=Previous) return false;
00512 
00513                         Previous = Current;
00514                         Current = Current->Next;
00515                 }
00516 
00517                 ASSERT(Nb==mNbObjects*2);
00518         }
00519         return true;
00520 }
00521 
00522 inline_ BOOL Intersect(const AABB& a, const SAP_Box& b)
00523 {
00524         if(b.Max[0]->Value < a.GetMin(0) || a.GetMax(0) < b.Min[0]->Value
00525         || b.Max[1]->Value < a.GetMin(1) || a.GetMax(1) < b.Min[1]->Value
00526         || b.Max[2]->Value < a.GetMin(2) || a.GetMax(2) < b.Min[2]->Value)      return FALSE;
00527 
00528         return TRUE;
00529 }
00530 
00531 
00532 
00533 bool SweepAndPrune::UpdateObject(udword i, const AABB& box)
00534 {
00535         for(udword Axis=0;Axis<3;Axis++)
00536         {
00537 //              udword Base = (udword)&mList[Axis][0];
00538 
00539                 // Update min
00540                 {
00541                         SAP_EndPoint* const CurrentMin = mBoxes[i].Min[Axis];
00542                         ASSERT(!CurrentMin->IsMax());
00543 
00544                         const float Limit = box.GetMin(Axis);
00545                         if(Limit == CurrentMin->Value)
00546                         {
00547                         }
00548                         else if(Limit < CurrentMin->Value)
00549                         {
00550                                 CurrentMin->Value = Limit;
00551 
00552                                 // Min is moving left:
00553                                 SAP_EndPoint* NewPos = CurrentMin;
00554                                 ASSERT(NewPos);
00555 
00556                                 SAP_EndPoint* tmp;
00557                                 while((tmp = NewPos->Previous) && tmp->Value > Limit)
00558                                 {
00559                                         NewPos = tmp;
00560 
00561                                         if(NewPos->IsMax())
00562                                         {
00563                                                 // Our min passed a max => start overlap
00564                                                 //udword SortedIndex = (udword(CurrentMin) - Base)/sizeof(NS_EndPoint);
00565                                                 const udword id0 = CurrentMin->GetBoxID();
00566                                                 const udword id1 = NewPos->GetBoxID();
00567 
00568                                                 if(id0!=id1 && Intersect(box, mBoxes[id1]))     mPairs.AddPair(id0, id1);
00569                                         }
00570                                 }
00571 
00572                                 CurrentMin->InsertBefore(NewPos);
00573                         }
00574                         else// if(Limit > CurrentMin->Value)
00575                         {
00576                                 CurrentMin->Value = Limit;
00577 
00578                                 // Min is moving right:
00579                                 SAP_EndPoint* NewPos = CurrentMin;
00580                                 ASSERT(NewPos);
00581 
00582                                 SAP_EndPoint* tmp;
00583                                 while((tmp = NewPos->Next) && tmp->Value < Limit)
00584                                 {
00585                                         NewPos = tmp;
00586 
00587                                         if(NewPos->IsMax())
00588                                         {
00589                                                 // Our min passed a max => stop overlap
00590                                                 const udword id0 = CurrentMin->GetBoxID();
00591                                                 const udword id1 = NewPos->GetBoxID();
00592 
00593                                                 if(id0!=id1)    mPairs.RemovePair(id0, id1);
00594                                         }
00595                                 }
00596 
00597                                 CurrentMin->InsertAfter(NewPos);
00598                         }
00599                 }
00600 
00601                 // Update max
00602                 {
00603                         SAP_EndPoint* const CurrentMax = mBoxes[i].Max[Axis];
00604                         ASSERT(CurrentMax->IsMax());
00605 
00606                         const float Limit = box.GetMax(Axis);
00607                         if(Limit == CurrentMax->Value)
00608                         {
00609                         }
00610                         else if(Limit > CurrentMax->Value)
00611                         {
00612                                 CurrentMax->Value = Limit;
00613 
00614                                 // Max is moving right:
00615                                 SAP_EndPoint* NewPos = CurrentMax;
00616                                 ASSERT(NewPos);
00617 
00618                                 SAP_EndPoint* tmp;
00619                                 while((tmp = NewPos->Next) && tmp->Value < Limit)
00620                                 {
00621                                         NewPos = tmp;
00622 
00623                                         if(!NewPos->IsMax())
00624                                         {
00625                                                 // Our max passed a min => start overlap
00626                                                 const udword id0 = CurrentMax->GetBoxID();
00627                                                 const udword id1 = NewPos->GetBoxID();
00628 
00629                                                 if(id0!=id1 && Intersect(box, mBoxes[id1]))     mPairs.AddPair(id0, id1);
00630                                         }
00631                                 }
00632 
00633                                 CurrentMax->InsertAfter(NewPos);
00634                         }
00635                         else// if(Limit < CurrentMax->Value)
00636                         {
00637                                 CurrentMax->Value = Limit;
00638 
00639                                 // Max is moving left:
00640                                 SAP_EndPoint* NewPos = CurrentMax;
00641                                 ASSERT(NewPos);
00642 
00643                                 SAP_EndPoint* tmp;
00644                                 while((tmp = NewPos->Previous) && tmp->Value > Limit)
00645                                 {
00646                                         NewPos = tmp;
00647 
00648                                         if(!NewPos->IsMax())
00649                                         {
00650                                                 // Our max passed a min => stop overlap
00651                                                 const udword id0 = CurrentMax->GetBoxID();
00652                                                 const udword id1 = NewPos->GetBoxID();
00653 
00654                                                 if(id0!=id1)    mPairs.RemovePair(id0, id1);
00655                                         }
00656                                 }
00657 
00658                                 CurrentMax->InsertBefore(NewPos);
00659                         }
00660                 }
00661         }
00662 
00663         return true;
00664 }


openhrp3
Author(s): AIST, General Robotix Inc., Nakamura Lab of Dept. of Mechano Informatics at University of Tokyo
autogenerated on Sun Apr 2 2017 03:43:56