Extras/CDTestFramework/Opcode/OPC_ArraySAP.cpp

   1 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
   2 /*
   3  *      OPCODE - Optimized Collision Detection
   4  *      Copyright (C) 2001 Pierre Terdiman
   5  *      Homepage: http://www.codercorner.com/Opcode.htm
   6  */
   7 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
   8
   9 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
  10 /**
  11  *      Contains an array-based version of the sweep-and-prune algorithm
  12  *      \file           OPC_ArraySAP.cpp
  13  *      \author         Pierre Terdiman
  14  *      \date           December, 2, 2007
  15  */
  16 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
  17
  18 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
  19 // Precompiled Header
  20 #include "StdAfx.h"
  21
  22 using namespace Opcode;
  23
  24 //#include "SAP_Utils.h"
  25
  26 #define INVALID_USER_ID 0xffff
  27
  28 inline_ void Sort(uword& id0, uword& id1)                                                                               { if(id0>id1)   TSwap(id0, id1);                                                }
  29 inline_ void Sort(uword& id0, uword& id1, const void*& obj0, const void*& obj1) { if(id0>id1)   { TSwap(id0, id1); TSwap(obj0, obj1);   }       }
  30
  31
  32         struct Opcode::IAABB : public Allocateable
  33         {
  34                 udword mMinX;
  35                 udword mMinY;
  36                 udword mMinZ;
  37                 udword mMaxX;
  38                 udword mMaxY;
  39                 udword mMaxZ;
  40
  41                 inline_ udword  GetMin(udword i)        const   {       return (&mMinX)[i];     }
  42                 inline_ udword  GetMax(udword i)        const   {       return (&mMaxX)[i];     }
  43         };
  44
  45
  46
  47 /*
  48         - already sorted for batch create?
  49         - better axis selection batch create
  50 */
  51
  52 //#define USE_WORDS             // Use words or dwords for box indices. Words save memory but seriously limit the max number of objects in the SAP.
  53 #define USE_PREFETCH
  54 #define USE_INTEGERS
  55 #define PAIR_USER_DATA
  56 #define USE_OVERLAP_TEST_ON_REMOVES     // "Useless" but faster overall because seriously reduces number of calls (from ~10000 to ~3 sometimes!)
  57 #define RELEASE_ON_RESET        // Release memory instead of just doing a reset
  58
  59 #include "OPC_ArraySAP.h"
  60
  61 //#include "SAP_PairManager.h"
  62 //#include "SAP_PairManager.cpp"
  63
  64
  65 inline_ udword Hash(uword id0, uword id1)                                                               { return Hash32Bits_1( udword(id0)|(udword(id1)<<16) );         }
  66 inline_ bool DifferentPair(const ASAP_Pair& p, uword id0, uword id1)    { return (id0!=p.id0) || (id1!=p.id1);                                          }
  67
  68 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
  69
  70 ASAP_PairManager::ASAP_PairManager() :
  71         mHashSize               (0),
  72         mMask                   (0),
  73         mHashTable              (null),
  74         mNext                   (null),
  75         mNbActivePairs  (0),
  76         mActivePairs    (null)
  77 {
  78 }
  79
  80 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
  81
  82 ASAP_PairManager::~ASAP_PairManager()
  83 {
  84         Purge();
  85 }
  86
  87 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
  88
  89 void ASAP_PairManager::Purge()
  90 {
  91         ICE_FREE(mNext);
  92         ICE_FREE(mActivePairs);
  93         ICE_FREE(mHashTable);
  94         mHashSize               = 0;
  95         mMask                   = 0;
  96         mNbActivePairs  = 0;
  97 }
  98
  99 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 100
 101 const ASAP_Pair* ASAP_PairManager::FindPair(uword id0, uword id1) const
 102 {
 103         if(!mHashTable) return null;    // Nothing has been allocated yet
 104
 105         // Order the ids
 106         Sort(id0, id1);
 107
 108         // Compute hash value for this pair
 109         udword HashValue = Hash(id0, id1) & mMask;
 110
 111         // Look for it in the table
 112         udword Offset = mHashTable[HashValue];
 113         while(Offset!=INVALID_ID && DifferentPair(mActivePairs[Offset], id0, id1))
 114         {
 115                 ASSERT(mActivePairs[Offset].id0!=INVALID_USER_ID);
 116                 Offset = mNext[Offset];         // Better to have a separate array for this
 117         }
 118         if(Offset==INVALID_ID)  return null;
 119         ASSERT(Offset<mNbActivePairs);
 120         // Match mActivePairs[Offset] => the pair is persistent
 121         return &mActivePairs[Offset];
 122 }
 123
 124 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 125
 126 // Internal version saving hash computation
 127 inline_ ASAP_Pair* ASAP_PairManager::FindPair(uword id0, uword id1, udword hash_value) const
 128 {
 129         if(!mHashTable) return null;    // Nothing has been allocated yet
 130
 131         // Look for it in the table
 132         udword Offset = mHashTable[hash_value];
 133         while(Offset!=INVALID_ID && DifferentPair(mActivePairs[Offset], id0, id1))
 134         {
 135                 ASSERT(mActivePairs[Offset].id0!=INVALID_USER_ID);
 136                 Offset = mNext[Offset];         // Better to have a separate array for this
 137         }
 138         if(Offset==INVALID_ID)  return null;
 139         ASSERT(Offset<mNbActivePairs);
 140         // Match mActivePairs[Offset] => the pair is persistent
 141         return &mActivePairs[Offset];
 142 }
 143
 144 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 145
 146 const ASAP_Pair* ASAP_PairManager::AddPair(uword id0, uword id1, const void* object0, const void* object1)
 147 {
 148         // Order the ids
 149         Sort(id0, id1, object0, object1);
 150
 151         udword HashValue = Hash(id0, id1) & mMask;
 152
 153         ASAP_Pair* P = FindPair(id0, id1, HashValue);
 154         if(P)
 155         {
 156                 return P;       // Persistent pair
 157         }
 158
 159         // This is a new pair
 160         if(mNbActivePairs >= mHashSize)
 161         {
 162                 // Get more entries
 163                 mHashSize = NextPowerOfTwo(mNbActivePairs+1);
 164                 mMask = mHashSize-1;
 165
 166                 ReallocPairs();
 167
 168                 // Recompute hash value with new hash size
 169                 HashValue = Hash(id0, id1) & mMask;
 170         }
 171
 172         ASAP_Pair* p = &mActivePairs[mNbActivePairs];
 173         p->id0          = id0;  // ### CMOVs would be nice here
 174         p->id1          = id1;
 175         p->object0      = object0;
 176         p->object1      = object1;
 177
 178         mNext[mNbActivePairs] = mHashTable[HashValue];
 179         mHashTable[HashValue] = mNbActivePairs++;
 180         return p;
 181 }
 182
 183 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 184
 185 void ASAP_PairManager::RemovePair(uword id0, uword id1, udword hash_value, udword pair_index)
 186 {
 187         // Walk the hash table to fix mNext
 188         udword Offset = mHashTable[hash_value];
 189         ASSERT(Offset!=INVALID_ID);
 190
 191         udword Previous=INVALID_ID;
 192         while(Offset!=pair_index)
 193         {
 194                 Previous = Offset;
 195                 Offset = mNext[Offset];
 196         }
 197
 198         // Let us go/jump us
 199         if(Previous!=INVALID_ID)
 200         {
 201                 ASSERT(mNext[Previous]==pair_index);
 202                 mNext[Previous] = mNext[pair_index];
 203         }
 204         // else we were the first
 205         else mHashTable[hash_value] = mNext[pair_index];
 206         // we're now free to reuse mNext[PairIndex] without breaking the list
 207
 208 #ifdef _DEBUG
 209         mNext[pair_index]=INVALID_ID;
 210 #endif
 211         // Invalidate entry
 212
 213         // Fill holes
 214         if(1)
 215         {
 216                 // 1) Remove last pair
 217                 const udword LastPairIndex = mNbActivePairs-1;
 218                 if(LastPairIndex==pair_index)
 219                 {
 220                         mNbActivePairs--;
 221                 }
 222                 else
 223                 {
 224                         const ASAP_Pair* Last = &mActivePairs[LastPairIndex];
 225                         const udword LastHashValue = Hash(Last->id0, Last->id1) & mMask;
 226
 227                         // Walk the hash table to fix mNext
 228                         udword Offset = mHashTable[LastHashValue];
 229                         ASSERT(Offset!=INVALID_ID);
 230
 231                         udword Previous=INVALID_ID;
 232                         while(Offset!=LastPairIndex)
 233                         {
 234                                 Previous = Offset;
 235                                 Offset = mNext[Offset];
 236                         }
 237
 238                         // Let us go/jump us
 239                         if(Previous!=INVALID_ID)
 240                         {
 241                                 ASSERT(mNext[Previous]==LastPairIndex);
 242                                 mNext[Previous] = mNext[LastPairIndex];
 243                         }
 244                         // else we were the first
 245                         else mHashTable[LastHashValue] = mNext[LastPairIndex];
 246                         // we're now free to reuse mNext[LastPairIndex] without breaking the list
 247
 248 #ifdef _DEBUG
 249                         mNext[LastPairIndex]=INVALID_ID;
 250 #endif
 251
 252                         // Don't invalidate entry since we're going to shrink the array
 253
 254                         // 2) Re-insert in free slot
 255                         mActivePairs[pair_index] = mActivePairs[LastPairIndex];
 256 #ifdef _DEBUG
 257                         ASSERT(mNext[pair_index]==INVALID_ID);
 258 #endif
 259                         mNext[pair_index] = mHashTable[LastHashValue];
 260                         mHashTable[LastHashValue] = pair_index;
 261
 262                         mNbActivePairs--;
 263                 }
 264         }
 265 }
 266
 267 bool ASAP_PairManager::RemovePair(uword id0, uword id1)
 268 {
 269         // Order the ids
 270         Sort(id0, id1);
 271
 272         const udword HashValue = Hash(id0, id1) & mMask;
 273         const ASAP_Pair* P = FindPair(id0, id1, HashValue);
 274         if(!P)  return false;
 275         ASSERT(P->id0==id0);
 276         ASSERT(P->id1==id1);
 277
 278         RemovePair(id0, id1, HashValue, GetPairIndex(P));
 279
 280         ShrinkMemory();
 281         return true;
 282 }
 283
 284 bool ASAP_PairManager::RemovePairs(const BitArray& array)
 285 {
 286         udword i=0;
 287         while(i<mNbActivePairs)
 288         {
 289                 const uword id0 = mActivePairs[i].id0;
 290                 const uword id1 = mActivePairs[i].id1;
 291                 if(array.IsSet(id0) || array.IsSet(id1))
 292                 {
 293                         const udword HashValue = Hash(id0, id1) & mMask;
 294                         RemovePair(id0, id1, HashValue, i);
 295                 }
 296                 else i++;
 297         }
 298         ShrinkMemory();
 299         return true;
 300 }
 301
 302 void ASAP_PairManager::ShrinkMemory()
 303 {
 304         // Check correct memory against actually used memory
 305         const udword CorrectHashSize = NextPowerOfTwo(mNbActivePairs);
 306         if(mHashSize==CorrectHashSize)  return;
 307
 308         // Reduce memory used
 309         mHashSize = CorrectHashSize;
 310         mMask = mHashSize-1;
 311
 312         ReallocPairs();
 313 }
 314
 315 void ASAP_PairManager::ReallocPairs()
 316 {
 317         ICE_FREE(mHashTable);
 318         mHashTable = (udword*)ICE_ALLOC(mHashSize*sizeof(udword));
 319         StoreDwords(mHashTable, mHashSize, INVALID_ID);
 320
 321         // Get some bytes for new entries
 322         ASAP_Pair* NewPairs     = (ASAP_Pair*)ICE_ALLOC(mHashSize * sizeof(ASAP_Pair)); ASSERT(NewPairs);
 323         udword* NewNext         = (udword*)ICE_ALLOC(mHashSize * sizeof(udword));               ASSERT(NewNext);
 324
 325         // Copy old data if needed
 326         if(mNbActivePairs)      CopyMemory(NewPairs, mActivePairs, mNbActivePairs*sizeof(ASAP_Pair));
 327         // ### check it's actually needed... probably only for pairs whose hash value was cut by the and
 328         // yeah, since Hash(id0, id1) is a constant
 329         // However it might not be needed to recompute them => only less efficient but still ok
 330         for(udword i=0;i<mNbActivePairs;i++)
 331         {
 332                 const udword HashValue = Hash(mActivePairs[i].id0, mActivePairs[i].id1) & mMask;        // New hash value with new mask
 333                 NewNext[i] = mHashTable[HashValue];
 334                 mHashTable[HashValue] = i;
 335         }
 336
 337         // Delete old data
 338         ICE_FREE(mNext);
 339         ICE_FREE(mActivePairs);
 340
 341         // Assign new pointer
 342         mActivePairs = NewPairs;
 343         mNext = NewNext;
 344 }
 345
 346
 347 #ifdef USE_WORDS
 348         typedef uword                   IndexType;
 349         #define INVALID_INDEX   0xffff
 350 #else
 351         typedef udword                  IndexType;
 352         #define INVALID_INDEX   0xffffffff
 353 #endif
 354
 355 #ifdef USE_INTEGERS
 356         typedef udword  ValType;
 357         typedef IAABB   SAP_AABB;
 358 #else
 359         typedef float   ValType;
 360         typedef AABB    SAP_AABB;
 361 #endif
 362
 363         struct Opcode::CreateData
 364         {
 365                 udword  mHandle;
 366                 AABB    mBox;
 367         };
 368
 369         class Opcode::ASAP_EndPoint : public Allocateable
 370         {
 371                 public:
 372                 inline_                                 ASAP_EndPoint()         {}
 373                 inline_                                 ~ASAP_EndPoint()        {}
 374
 375                                 ValType                 mValue;         // Min or Max value
 376                                 udword                  mData;          // Parent box | MinMax flag
 377                 public:
 378
 379                 inline_ bool                    IsSentinel()    const   { return (mData&~3)==0xfffffffc;        }
 380
 381                 inline_ void                    SetData(ValType v, udword owner_box_id, BOOL is_max)
 382                                                                 {
 383                                                                         mValue = v;
 384                                                                         mData = owner_box_id<<2;
 385                                                                         if(is_max)      mData |= 3;
 386                                                                 }
 387                 inline_ BOOL                    IsMax()         const   { return mData & 3;             }
 388                 inline_ udword                  GetOwner()      const   { return mData>>2;              }
 389         };
 390
 391         class Opcode::ASAP_Box : public Allocateable
 392         {
 393                 public:
 394                 inline_                                 ASAP_Box()      {}
 395                 inline_                                 ~ASAP_Box()     {}
 396
 397                                 IndexType               mMin[3];
 398                                 IndexType               mMax[3];
 399                                 void*                   mObject;
 400                                 udword                  mGUID;
 401
 402                 inline_ void                    SetInvalid()                    { mMin[0]=INVALID_INDEX;                        }
 403                 inline_ bool                    IsValid()               const   { return mMin[0]!=INVALID_INDEX;        }
 404
 405                 inline_ ValType                 GetMaxValue(udword i, const ASAP_EndPoint* base)        const
 406                                                                 {
 407                                                                         return base[mMax[i]].mValue;
 408                                                                 }
 409
 410                 inline_ ValType                 GetMinValue(udword i, const ASAP_EndPoint* base)        const
 411                                                                 {
 412                                                                         return base[mMin[i]].mValue;
 413                                                                 }
 414 #ifdef _DEBUG
 415                                 bool                    HasBeenInserted()       const
 416                                                                 {
 417                                                                         assert(mMin[0]!=INVALID_INDEX);
 418                                                                         assert(mMax[0]!=INVALID_INDEX);
 419                                                                         assert(mMin[1]!=INVALID_INDEX);
 420                                                                         assert(mMax[1]!=INVALID_INDEX);
 421                                                                         assert(mMin[2]!=INVALID_INDEX);
 422                                                                         assert(mMax[2]!=INVALID_INDEX);
 423                                                                         return true;
 424                                                                 }
 425 #endif
 426         };
 427
 428 inline_ BOOL Intersect1D_Min(const SAP_AABB& a, const ASAP_Box& b, const ASAP_EndPoint* const base, udword axis)
 429 {
 430         if(b.GetMaxValue(axis, base) < a.GetMin(axis))
 431                 return FALSE;
 432         return TRUE;
 433 }
 434
 435 inline_ BOOL Intersect2D(const ASAP_Box& c, const ASAP_Box& b, udword axis1, udword axis2)
 436 {
 437         if(             b.mMax[axis1] < c.mMin[axis1] || c.mMax[axis1] < b.mMin[axis1]
 438                 ||      b.mMax[axis2] < c.mMin[axis2] || c.mMax[axis2] < b.mMin[axis2]) return FALSE;
 439         return TRUE;
 440 }
 441
 442
 443 ArraySAP::ArraySAP()
 444 {
 445         mNbBoxes        = 0;
 446         mMaxNbBoxes     = 0;
 447         mBoxes          = null;
 448         mEndPoints[0] = mEndPoints[1] = mEndPoints[2] = null;
 449         mFirstFree      = INVALID_ID;
 450 }
 451
 452 ArraySAP::~ArraySAP()
 453 {
 454         mNbBoxes        = 0;
 455         mMaxNbBoxes     = 0;
 456         DELETEARRAY(mBoxes);
 457         for(udword i=0;i<3;i++)
 458         {
 459                 DELETEARRAY(mEndPoints[i]);
 460         }
 461 }
 462
 463 void ArraySAP::ResizeBoxArray()
 464 {
 465         const udword NewMaxBoxes = mMaxNbBoxes ? mMaxNbBoxes*2 : 64;
 466
 467         ASAP_Box* NewBoxes = ICE_NEW_TMP(ASAP_Box)[NewMaxBoxes];
 468         const udword NbSentinels=2;
 469         ASAP_EndPoint* NewEndPointsX = ICE_NEW_TMP(ASAP_EndPoint)[NewMaxBoxes*2+NbSentinels];
 470         ASAP_EndPoint* NewEndPointsY = ICE_NEW_TMP(ASAP_EndPoint)[NewMaxBoxes*2+NbSentinels];
 471         ASAP_EndPoint* NewEndPointsZ = ICE_NEW_TMP(ASAP_EndPoint)[NewMaxBoxes*2+NbSentinels];
 472
 473         if(mNbBoxes)
 474         {
 475                 CopyMemory(NewBoxes, mBoxes, sizeof(ASAP_Box)*mNbBoxes);
 476                 CopyMemory(NewEndPointsX, mEndPoints[0], sizeof(ASAP_EndPoint)*(mNbBoxes*2+NbSentinels));
 477                 CopyMemory(NewEndPointsY, mEndPoints[1], sizeof(ASAP_EndPoint)*(mNbBoxes*2+NbSentinels));
 478                 CopyMemory(NewEndPointsZ, mEndPoints[2], sizeof(ASAP_EndPoint)*(mNbBoxes*2+NbSentinels));
 479         }
 480         else
 481         {
 482                 // Initialize sentinels
 483 #ifdef USE_INTEGERS
 484                 const udword Min = EncodeFloat(MIN_FLOAT);
 485                 const udword Max = EncodeFloat(MAX_FLOAT);
 486 #else
 487                 const float Min = MIN_FLOAT;
 488                 const float Max = MAX_FLOAT;
 489 #endif
 490                 NewEndPointsX[0].SetData(Min, INVALID_INDEX, FALSE);
 491                 NewEndPointsX[1].SetData(Max, INVALID_INDEX, TRUE);
 492                 NewEndPointsY[0].SetData(Min, INVALID_INDEX, FALSE);
 493                 NewEndPointsY[1].SetData(Max, INVALID_INDEX, TRUE);
 494                 NewEndPointsZ[0].SetData(Min, INVALID_INDEX, FALSE);
 495                 NewEndPointsZ[1].SetData(Max, INVALID_INDEX, TRUE);
 496         }
 497         DELETEARRAY(mBoxes);
 498         DELETEARRAY(mEndPoints[2]);
 499         DELETEARRAY(mEndPoints[1]);
 500         DELETEARRAY(mEndPoints[0]);
 501         mBoxes = NewBoxes;
 502         mEndPoints[0] = NewEndPointsX;
 503         mEndPoints[1] = NewEndPointsY;
 504         mEndPoints[2] = NewEndPointsZ;
 505
 506         mMaxNbBoxes = NewMaxBoxes;
 507 }
 508
 509 inline_ BOOL Intersect(const IAABB& a, const IAABB& b, udword axis)
 510 {
 511         if(b.GetMax(axis) < a.GetMin(axis) || a.GetMax(axis) < b.GetMin(axis))  return FALSE;
 512         return TRUE;
 513 }
 514
 515 // ### TODO: the sorts here might be useless, as the values have been sorted already
 516 bool ArraySAP::CompleteBoxPruning2(udword nb, const IAABB* array, const Axes& axes, const CreateData* batched)
 517 {
 518         // Checkings
 519         if(!nb || !array)       return false;
 520
 521         // Catch axes
 522         const udword Axis0 = axes.mAxis0;
 523         const udword Axis1 = axes.mAxis1;
 524         const udword Axis2 = axes.mAxis2;
 525
 526         // Allocate some temporary data
 527         udword* PosList = (udword*)ICE_ALLOC_TMP(sizeof(udword)*(nb+1));
 528
 529         // 1) Build main list using the primary axis
 530         for(udword i=0;i<nb;i++)        PosList[i] = array[i].GetMin(Axis0);
 531 //PosList[nb++] = ConvertToSortable(MAX_FLOAT);
 532
 533         // 2) Sort the list
 534 //      static RadixSort r;
 535         RadixSort r;
 536         RadixSort* RS = &r;
 537
 538 //      const udword* Sorted = RS->Sort(PosList, nb, RADIX_SIGNED).GetRanks();
 539         const udword* Sorted = RS->Sort(PosList, nb, RADIX_UNSIGNED).GetRanks();
 540
 541         // 3) Prune the list
 542         const udword* const LastSorted = &Sorted[nb];
 543         const udword* RunningAddress = Sorted;
 544         udword Index0, Index1;
 545         while(RunningAddress<LastSorted && Sorted<LastSorted)
 546         {
 547                 Index0 = *Sorted++;
 548
 549                 while(RunningAddress<LastSorted && PosList[*RunningAddress++]<PosList[Index0]);
 550 //              while(PosList[*RunningAddress++]<PosList[Index0]);
 551
 552                 if(RunningAddress<LastSorted)
 553                 {
 554                         const udword* RunningAddress2 = RunningAddress;
 555
 556                         while(RunningAddress2<LastSorted && PosList[Index1 = *RunningAddress2++]<=array[Index0].GetMax(Axis0))
 557 //                      while(PosList[Index1 = *RunningAddress2++]<=(udword)ConvertToSortable(array[Index0].GetMax(Axis0)))
 558                         {
 559                                 if(Intersect(array[Index0], array[Index1], Axis1))
 560                                 {
 561                                         if(Intersect(array[Index0], array[Index1], Axis2))
 562                                         {
 563                                                 const ASAP_Box* Box0 = mBoxes + batched[Index0].mHandle;
 564                                                 const ASAP_Box* Box1 = mBoxes + batched[Index1].mHandle;
 565                                                 AddPair(Box0->mObject, Box1->mObject, Box0->mGUID, Box1->mGUID);
 566                                         }
 567                                 }
 568                         }
 569                 }
 570         }
 571
 572         ICE_FREE(PosList);
 573         return true;
 574 }
 575
 576 bool ArraySAP::BipartiteBoxPruning2(udword nb0, const IAABB* array0, udword nb1, const IAABB* array1, const Axes& axes, const CreateData* batched, const udword* box_indices)
 577 {
 578         // Checkings
 579         if(!nb0 || !array0 || !nb1 || !array1)  return false;
 580
 581         // Catch axes
 582         const udword Axis0 = axes.mAxis0;
 583         const udword Axis1 = axes.mAxis1;
 584         const udword Axis2 = axes.mAxis2;
 585
 586         // Allocate some temporary data
 587         udword* MinPosList0 = (udword*)ICE_ALLOC_TMP(sizeof(udword)*nb0);
 588         udword* MinPosList1 = (udword*)ICE_ALLOC_TMP(sizeof(udword)*nb1);
 589
 590         // 1) Build main lists using the primary axis
 591         for(udword i=0;i<nb0;i++)       MinPosList0[i] = array0[i].GetMin(Axis0);
 592         for(udword i=0;i<nb1;i++)       MinPosList1[i] = array1[i].GetMin(Axis0);
 593
 594         // 2) Sort the lists
 595 /*      static RadixSort r0;
 596         static RadixSort r1;
 597         RadixSort* RS0 = &r0;
 598         RadixSort* RS1 = &r1;*/
 599         RadixSort r0;
 600         RadixSort r1;
 601         RadixSort* RS0 = &r0;
 602         RadixSort* RS1 = &r1;
 603
 604         const udword* Sorted0 = RS0->Sort(MinPosList0, nb0, RADIX_UNSIGNED).GetRanks();
 605         const udword* Sorted1 = RS1->Sort(MinPosList1, nb1, RADIX_UNSIGNED).GetRanks();
 606
 607         // 3) Prune the lists
 608         udword Index0, Index1;
 609
 610         const udword* const LastSorted0 = &Sorted0[nb0];
 611         const udword* const LastSorted1 = &Sorted1[nb1];
 612         const udword* RunningAddress0 = Sorted0;
 613         const udword* RunningAddress1 = Sorted1;
 614
 615         while(RunningAddress1<LastSorted1 && Sorted0<LastSorted0)
 616         {
 617                 Index0 = *Sorted0++;
 618
 619                 while(RunningAddress1<LastSorted1 && MinPosList1[*RunningAddress1]<MinPosList0[Index0]) RunningAddress1++;
 620
 621                 const udword* RunningAddress2_1 = RunningAddress1;
 622
 623                 while(RunningAddress2_1<LastSorted1 && MinPosList1[Index1 = *RunningAddress2_1++]<=array0[Index0].GetMax(Axis0))
 624                 {
 625                         if(Intersect(array0[Index0], array1[Index1], Axis1))
 626                         {
 627                                 if(Intersect(array0[Index0], array1[Index1], Axis2))
 628                                 {
 629                                         const ASAP_Box* Box0 = mBoxes + batched[Index0].mHandle;
 630                                         const ASAP_Box* Box1 = mBoxes + box_indices[Index1];
 631                                         AddPair(Box0->mObject, Box1->mObject, Box0->mGUID, Box1->mGUID);
 632                                 }
 633                         }
 634                 }
 635         }
 636
 637         ////
 638
 639         while(RunningAddress0<LastSorted0 && Sorted1<LastSorted1)
 640         {
 641                 Index0 = *Sorted1++;
 642
 643                 while(RunningAddress0<LastSorted0 && MinPosList0[*RunningAddress0]<=MinPosList1[Index0])        RunningAddress0++;
 644
 645                 const udword* RunningAddress2_0 = RunningAddress0;
 646
 647                 while(RunningAddress2_0<LastSorted0 && MinPosList0[Index1 = *RunningAddress2_0++]<=array1[Index0].GetMax(Axis0))
 648                 {
 649                         if(Intersect(array0[Index1], array1[Index0], Axis1))
 650                         {
 651                                 if(Intersect(array0[Index1], array1[Index0], Axis2))
 652                                 {
 653                                         const ASAP_Box* Box0 = mBoxes + batched[Index1].mHandle;
 654                                         const ASAP_Box* Box1 = mBoxes + box_indices[Index0];
 655                                         AddPair(Box0->mObject, Box1->mObject, Box0->mGUID, Box1->mGUID);
 656                                 }
 657                         }
 658
 659                 }
 660         }
 661
 662         ICE_FREE(MinPosList0);
 663         ICE_FREE(MinPosList1);
 664         return true;
 665 }
 666
 667 udword ArraySAP::AddObject(void* object, uword guid, const AABB& box)
 668 {
 669         assert(!(size_t(object)&3));    // We will use the 2 LSBs
 670
 671 #ifdef _DEBUG
 672         int a = sizeof(ASAP_Box);               // 32
 673         int b = sizeof(ASAP_EndPoint);  // 8
 674 #endif
 675
 676         udword BoxIndex;
 677         if(mFirstFree!=INVALID_ID)
 678         {
 679                 BoxIndex = mFirstFree;
 680                 mFirstFree = mBoxes[BoxIndex].mGUID;
 681         }
 682         else
 683         {
 684                 if(mNbBoxes==mMaxNbBoxes)
 685                         ResizeBoxArray();
 686                 BoxIndex = mNbBoxes;
 687         }
 688
 689         ASAP_Box* Box = &mBoxes[BoxIndex];
 690         // Initialize box
 691         Box->mObject    = object;
 692         Box->mGUID              = guid;
 693         for(udword i=0;i<3;i++)
 694         {
 695                 Box->mMin[i] = INVALID_INDEX;
 696                 Box->mMax[i] = INVALID_INDEX;
 697         }
 698
 699         mNbBoxes++;
 700
 701         CreateData* CD = (CreateData*)mCreated.Reserve(sizeof(CreateData)/sizeof(udword));
 702         CD->mHandle = BoxIndex;
 703         CD->mBox = box;
 704
 705         return BoxIndex;
 706 }
 707
 708 void ArraySAP::InsertEndPoints(udword axis, const ASAP_EndPoint* end_points, udword nb_endpoints)
 709 {
 710         ASAP_EndPoint* const BaseEP = mEndPoints[axis];
 711
 712         const udword OldSize = mNbBoxes*2 - nb_endpoints;
 713         const udword NewSize = mNbBoxes*2;
 714
 715         BaseEP[NewSize + 1] = BaseEP[OldSize + 1];
 716
 717         sdword WriteIdx = NewSize;
 718         udword CurrInsIdx = 0;
 719
 720         const ASAP_EndPoint* First = &BaseEP[0];
 721         const ASAP_EndPoint* Current = &BaseEP[OldSize];
 722         while(Current>=First)
 723         {
 724                 const ASAP_EndPoint& Src = *Current;
 725                 const ASAP_EndPoint& Ins = end_points[CurrInsIdx];
 726
 727                 // We need to make sure we insert maxs before mins to handle exactly equal endpoints correctly
 728                 const bool ShouldInsert = Ins.IsMax() ? (Src.mValue <= Ins.mValue) : (Src.mValue < Ins.mValue);
 729
 730                 const ASAP_EndPoint& Moved = ShouldInsert ? Ins : Src;
 731                 BaseEP[WriteIdx] = Moved;
 732                 mBoxes[Moved.GetOwner()].mMin[axis + Moved.IsMax()] = WriteIdx--;
 733
 734                 if(ShouldInsert)
 735                 {
 736                         CurrInsIdx++;
 737                         if(CurrInsIdx >= nb_endpoints)
 738                                 break;//we just inserted the last endpoint
 739                 }
 740                 else
 741                 {
 742                         Current--;
 743                 }
 744         }
 745 }
 746
 747 void ArraySAP::BatchCreate()
 748 {
 749         udword NbBatched = mCreated.GetNbEntries();
 750         if(!NbBatched)  return; // Early-exit if no object has been created
 751         NbBatched /= sizeof(CreateData)/sizeof(udword);
 752         const CreateData* Batched = (const CreateData*)mCreated.GetEntries();
 753         mCreated.Reset();
 754
 755         {
 756         const udword NbEndPoints = NbBatched*2;
 757         ASAP_EndPoint* NewEPSorted = ICE_NEW_TMP(ASAP_EndPoint)[NbEndPoints];
 758         ASAP_EndPoint* Buffer = (ASAP_EndPoint*)ICE_ALLOC_TMP(sizeof(ASAP_EndPoint)*NbEndPoints);
 759         RadixSort RS;
 760
 761         for(udword Axis=0;Axis<3;Axis++)
 762         {
 763                 for(udword i=0;i<NbBatched;i++)
 764                 {
 765                         const udword BoxIndex = (udword)Batched[i].mHandle;
 766                         assert(mBoxes[BoxIndex].mMin[Axis]==INVALID_INDEX);
 767                         assert(mBoxes[BoxIndex].mMax[Axis]==INVALID_INDEX);
 768
 769                         const float MinValue = Batched[i].mBox.GetMin(Axis);
 770                         const float MaxValue = Batched[i].mBox.GetMax(Axis);
 771
 772                         NewEPSorted[i*2+0].SetData(EncodeFloat(MinValue), BoxIndex, FALSE);
 773                         NewEPSorted[i*2+1].SetData(EncodeFloat(MaxValue), BoxIndex, TRUE);
 774                 }
 775
 776                 // Sort endpoints backwards
 777                 {
 778                         udword* Keys = (udword*)Buffer;
 779                         for(udword i=0;i<NbEndPoints;i++)
 780                                 Keys[i] = NewEPSorted[i].mValue;
 781
 782                         const udword* Sorted = RS.Sort(Keys, NbEndPoints, RADIX_UNSIGNED).GetRanks();
 783
 784                         for(udword i=0;i<NbEndPoints;i++)
 785                                 Buffer[i] = NewEPSorted[Sorted[NbEndPoints-1-i]];
 786                 }
 787
 788                 InsertEndPoints(Axis, Buffer, NbEndPoints);
 789         }
 790
 791         ICE_FREE(Buffer);
 792         DELETEARRAY(NewEPSorted);
 793         }
 794
 795 #ifdef _DEBUG
 796         for(udword i=0;i<NbBatched;i++)
 797         {
 798                 udword BoxIndex = (udword)Batched[i].mHandle;
 799                 ASAP_Box* Box = mBoxes + BoxIndex;
 800                 assert(Box->HasBeenInserted());
 801         }
 802         for(udword i=0;i<mNbBoxes*2+1;i++)
 803         {
 804                 assert(mEndPoints[0][i].mValue <= mEndPoints[0][i+1].mValue);
 805                 assert(mEndPoints[1][i].mValue <= mEndPoints[1][i+1].mValue);
 806                 assert(mEndPoints[2][i].mValue <= mEndPoints[2][i+1].mValue);
 807         }
 808 #endif
 809
 810         if(1)
 811         {
 812                 BitArray BA(mMaxNbBoxes);
 813
 814                 // Using box-pruning on array indices....
 815                 IAABB* NewBoxes = ICE_NEW_TMP(IAABB)[NbBatched];
 816                 for(udword i=0;i<NbBatched;i++)
 817                 {
 818                         const ASAP_Box* Box = mBoxes + (udword)Batched[i].mHandle;
 819                         assert((udword)Batched[i].mHandle<mMaxNbBoxes);
 820                         BA.SetBit((udword)Batched[i].mHandle);
 821                         NewBoxes[i].mMinX = Box->mMin[0];
 822                         NewBoxes[i].mMaxX = Box->mMax[0];
 823                         NewBoxes[i].mMinY = Box->mMin[1];
 824                         NewBoxes[i].mMaxY = Box->mMax[1];
 825                         NewBoxes[i].mMinZ = Box->mMin[2];
 826                         NewBoxes[i].mMaxZ = Box->mMax[2];
 827                 }
 828
 829                 CompleteBoxPruning2(NbBatched, NewBoxes, Axes(AXES_XZY), Batched);
 830
 831                 // the old boxes are not the first ones in the array
 832
 833                 const udword NbOldBoxes = mNbBoxes - NbBatched;
 834                 if(NbOldBoxes)
 835                 {
 836                         IAABB* OldBoxes = ICE_NEW_TMP(IAABB)[NbOldBoxes];
 837                         udword* OldBoxesIndices = (udword*)ICE_ALLOC_TMP(sizeof(udword)*NbOldBoxes);
 838                         udword Offset=0;
 839                         udword i=0;
 840                         while(i<NbOldBoxes)
 841                         {
 842
 843
 844                                 if(!BA.IsSet(i))
 845                                 {
 846                                         const ASAP_Box* Box = mBoxes + i;
 847 //                                      if(Box->mObject)
 848                                         if(Box->IsValid())
 849                                         {
 850                                                 OldBoxesIndices[Offset] = i;
 851
 852                                                 OldBoxes[Offset].mMinX = Box->mMin[0];
 853                                                 OldBoxes[Offset].mMaxX = Box->mMax[0];
 854                                                 OldBoxes[Offset].mMinY = Box->mMin[1];
 855                                                 OldBoxes[Offset].mMaxY = Box->mMax[1];
 856                                                 OldBoxes[Offset].mMinZ = Box->mMin[2];
 857                                                 OldBoxes[Offset].mMaxZ = Box->mMax[2];
 858                                                 Offset++;
 859                                         }
 860                                 }
 861                                 i++;
 862                         }
 863 //                      assert(i==NbOldBoxes);
 864                         BipartiteBoxPruning2(NbBatched, NewBoxes, Offset, OldBoxes, Axes(AXES_XZY), Batched, OldBoxesIndices);
 865
 866                         ICE_FREE(OldBoxesIndices);
 867                         DELETEARRAY(OldBoxes);
 868                 }
 869                 DELETEARRAY(NewBoxes);
 870         }
 871 #ifdef RELEASE_ON_RESET
 872         mCreated.Empty();
 873 #endif
 874 }
 875
 876 void ArraySAP::BatchRemove()
 877 {
 878         udword NbRemoved = mRemoved.GetNbEntries();
 879         if(!NbRemoved)  return; // Early-exit if no object has been removed
 880         const udword* Removed = mRemoved.GetEntries();
 881         mRemoved.Reset();
 882
 883         for(udword Axis=0;Axis<3;Axis++)
 884         {
 885                 ASAP_EndPoint* const BaseEP = mEndPoints[Axis];
 886                 udword MinMinIndex = MAX_UDWORD;
 887                 for(udword i=0;i<NbRemoved;i++)
 888                 {
 889                         assert(Removed[i]<mMaxNbBoxes);
 890                         const ASAP_Box* RemovedObject = mBoxes + Removed[i];
 891                         const udword MinIndex = RemovedObject->mMin[Axis];
 892                         assert(MinIndex<mMaxNbBoxes*2+2);
 893                         const udword MaxIndex = RemovedObject->mMax[Axis];
 894                         assert(MaxIndex<mMaxNbBoxes*2+2);
 895                         assert(BaseEP[MinIndex].GetOwner()==Removed[i]);
 896                         assert(BaseEP[MaxIndex].GetOwner()==Removed[i]);
 897                         BaseEP[MinIndex].mData = 0xfffffffe;
 898                         BaseEP[MaxIndex].mData = 0xfffffffe;
 899                         if(MinIndex<MinMinIndex)        MinMinIndex = MinIndex;
 900                 }
 901
 902                 udword ReadIndex = MinMinIndex;
 903                 udword DestIndex = MinMinIndex;
 904                 const udword Limit = mNbBoxes*2+2;
 905                 while(ReadIndex!=Limit)
 906                 {
 907                         while(ReadIndex!=Limit && BaseEP[ReadIndex].mData == 0xfffffffe)
 908                         {
 909                                 ReadIndex++;
 910                         }
 911                         if(ReadIndex!=Limit)
 912                         {
 913                                 if(ReadIndex!=DestIndex)
 914                                 {
 915                                         BaseEP[DestIndex] = BaseEP[ReadIndex];
 916                                         assert(BaseEP[DestIndex].mData != 0xfffffffe);
 917
 918                                         if(!BaseEP[DestIndex].IsSentinel())
 919                                         {
 920                                                 udword BoxOwner = BaseEP[DestIndex].GetOwner();
 921                                                 assert(BoxOwner<mMaxNbBoxes);
 922                                                 ASAP_Box* Box = mBoxes + BoxOwner;
 923
 924                                                 Box->mMin[Axis + BaseEP[DestIndex].IsMax()] = DestIndex;
 925                                         }
 926                                 }
 927                                 DestIndex++;
 928                                 ReadIndex++;
 929                         }
 930                 }
 931         }
 932
 933         BitArray BA(65536);
 934         const udword Saved = NbRemoved;
 935         while(NbRemoved--)
 936         {
 937                 udword Index = *Removed++;
 938                 assert(Index<mMaxNbBoxes);
 939
 940                 ASAP_Box* Object = mBoxes + Index;
 941                 assert(Object->mGUID < 65536);
 942                 BA.SetBit(Object->mGUID);
 943
 944                 Object->mGUID = mFirstFree;
 945 //              Object->mObject = null; // ###########
 946                 Object->SetInvalid();
 947                 mFirstFree = Index;
 948         }
 949         mNbBoxes -= Saved;
 950         mPairs.RemovePairs(BA);
 951
 952 #ifdef RELEASE_ON_RESET
 953         mRemoved.Empty();
 954 #endif
 955 }
 956
 957 bool ArraySAP::RemoveObject(udword handle)
 958 {
 959         mRemoved.Add(handle);
 960         return true;
 961 }
 962
 963 #ifdef USE_INTEGERS
 964 bool ArraySAP::UpdateObject(udword handle, const AABB& box_)
 965 #else
 966 bool ArraySAP::UpdateObject(udword handle, const AABB& box)
 967 #endif
 968 {
 969         const ASAP_Box* Object = mBoxes + handle;
 970         assert(Object->HasBeenInserted());
 971         const void* UserObject = Object->mObject;
 972         const udword UserGUID = Object->mGUID;
 973
 974 #ifdef USE_INTEGERS
 975         IAABB box;
 976         box.mMinX = EncodeFloat(box_.GetMin(0));
 977         box.mMinY = EncodeFloat(box_.GetMin(1));
 978         box.mMinZ = EncodeFloat(box_.GetMin(2));
 979         box.mMaxX = EncodeFloat(box_.GetMax(0));
 980         box.mMaxY = EncodeFloat(box_.GetMax(1));
 981         box.mMaxZ = EncodeFloat(box_.GetMax(2));
 982 #endif
 983
 984         for(udword Axis=0;Axis<3;Axis++)
 985         {
 986                 const udword Axis1 = (1  << Axis) & 3;
 987                 const udword Axis2 = (1  << Axis1) & 3;
 988
 989                 ASAP_EndPoint* const BaseEP = mEndPoints[Axis];
 990
 991                 // Update min
 992                 {
 993                         ASAP_EndPoint* CurrentMin = BaseEP + Object->mMin[Axis];
 994                         ASSERT(!CurrentMin->IsMax());
 995
 996                         const ValType Limit = box.GetMin(Axis);
 997                         if(Limit < CurrentMin->mValue)
 998                         {
 999                                 CurrentMin->mValue = Limit;
1000
1001                                 // Min is moving left:
1002                                 ASAP_EndPoint Saved = *CurrentMin;
1003                                 udword EPIndex = (size_t(CurrentMin) - size_t(BaseEP))/sizeof(ASAP_EndPoint);
1004                                 const udword SavedIndex = EPIndex;
1005
1006                                 while((--CurrentMin)->mValue > Limit)
1007                                 {
1008 #ifdef USE_PREFETCH
1009                                         _prefetch(CurrentMin-1);
1010 #endif
1011                                         ASAP_Box* id1 = mBoxes + CurrentMin->GetOwner();
1012                                         const BOOL IsMax = CurrentMin->IsMax();
1013                                         if(IsMax)
1014                                         {
1015                                                 // Our min passed a max => start overlap
1016                                                 if(Object!=id1
1017                                                         && Intersect2D(*Object, *id1, Axis1, Axis2)
1018                                                         && Intersect1D_Min(box, *id1, BaseEP, Axis)
1019                                                         )
1020                                                         AddPair(UserObject, id1->mObject, UserGUID, id1->mGUID);
1021                                         }
1022
1023                                         id1->mMin[Axis + IsMax] = EPIndex--;
1024                                         *(CurrentMin+1) = *CurrentMin;
1025                                 }
1026
1027                                 if(SavedIndex!=EPIndex)
1028                                 {
1029                                         mBoxes[Saved.GetOwner()].mMin[Axis + Saved.IsMax()] = EPIndex;
1030                                         BaseEP[EPIndex] = Saved;
1031                                 }
1032                         }
1033                         else if(Limit > CurrentMin->mValue)
1034                         {
1035                                 CurrentMin->mValue = Limit;
1036
1037                                 // Min is moving right:
1038                                 ASAP_EndPoint Saved = *CurrentMin;
1039                                 udword EPIndex = (size_t(CurrentMin) - size_t(BaseEP))/sizeof(ASAP_EndPoint);
1040                                 const udword SavedIndex = EPIndex;
1041
1042                                 while((++CurrentMin)->mValue < Limit)
1043                                 {
1044 #ifdef USE_PREFETCH
1045                                         _prefetch(CurrentMin+1);
1046 #endif
1047                                         ASAP_Box* id1 = mBoxes + CurrentMin->GetOwner();
1048                                         const BOOL IsMax = CurrentMin->IsMax();
1049                                         if(IsMax)
1050                                         {
1051                                                 // Our min passed a max => stop overlap
1052                                                 if(Object!=id1
1053 #ifdef USE_OVERLAP_TEST_ON_REMOVES
1054                                                         && Intersect2D(*Object, *id1, Axis1, Axis2)
1055 #endif
1056                                                         )
1057                                                         RemovePair(UserObject, id1->mObject, UserGUID, id1->mGUID);
1058                                         }
1059
1060                                         id1->mMin[Axis + IsMax] = EPIndex++;
1061                                         *(CurrentMin-1) = *CurrentMin;
1062                                 }
1063
1064                                 if(SavedIndex!=EPIndex)
1065                                 {
1066                                         mBoxes[Saved.GetOwner()].mMin[Axis + Saved.IsMax()] = EPIndex;
1067                                         BaseEP[EPIndex] = Saved;
1068                                 }
1069                         }
1070                 }
1071
1072                 // Update max
1073                 {
1074                         ASAP_EndPoint* CurrentMax = BaseEP + Object->mMax[Axis];
1075                         ASSERT(CurrentMax->IsMax());
1076
1077                         const ValType Limit = box.GetMax(Axis);
1078                         if(Limit > CurrentMax->mValue)
1079                         {
1080                                 CurrentMax->mValue = Limit;
1081
1082                                 // Max is moving right:
1083                                 ASAP_EndPoint Saved = *CurrentMax;
1084                                 udword EPIndex = (size_t(CurrentMax) - size_t(BaseEP))/sizeof(ASAP_EndPoint);
1085                                 const udword SavedIndex = EPIndex;
1086
1087                                 while((++CurrentMax)->mValue < Limit)
1088                                 {
1089 #ifdef USE_PREFETCH
1090                                         _prefetch(CurrentMax+1);
1091 #endif
1092                                         ASAP_Box* id1 = mBoxes + CurrentMax->GetOwner();
1093                                         const BOOL IsMax = CurrentMax->IsMax();
1094                                         if(!IsMax)
1095                                         {
1096                                                 // Our max passed a min => start overlap
1097                                                 if(Object!=id1
1098                                                         && Intersect2D(*Object, *id1, Axis1, Axis2)
1099                                                         && Intersect1D_Min(box, *id1, BaseEP, Axis)
1100                                                         )
1101                                                         AddPair(UserObject, id1->mObject, UserGUID, id1->mGUID);
1102                                         }
1103
1104                                         id1->mMin[Axis + IsMax] = EPIndex++;
1105                                         *(CurrentMax-1) = *CurrentMax;
1106                                 }
1107
1108                                 if(SavedIndex!=EPIndex)
1109                                 {
1110                                         mBoxes[Saved.GetOwner()].mMin[Axis + Saved.IsMax()] = EPIndex;
1111                                         BaseEP[EPIndex] = Saved;
1112                                 }
1113                         }
1114                         else if(Limit < CurrentMax->mValue)
1115                         {
1116                                 CurrentMax->mValue = Limit;
1117
1118                                 // Max is moving left:
1119                                 ASAP_EndPoint Saved = *CurrentMax;
1120                                 udword EPIndex = (size_t(CurrentMax) - size_t(BaseEP))/sizeof(ASAP_EndPoint);
1121                                 const udword SavedIndex = EPIndex;
1122
1123                                 while((--CurrentMax)->mValue > Limit)
1124                                 {
1125 #ifdef USE_PREFETCH
1126                                         _prefetch(CurrentMax-1);
1127 #endif
1128                                         ASAP_Box* id1 = mBoxes + CurrentMax->GetOwner();
1129                                         const BOOL IsMax = CurrentMax->IsMax();
1130                                         if(!IsMax)
1131                                         {
1132                                                 // Our max passed a min => stop overlap
1133                                                 if(Object!=id1
1134 #ifdef USE_OVERLAP_TEST_ON_REMOVES
1135                                                         && Intersect2D(*Object, *id1, Axis1, Axis2)
1136 #endif
1137                                                         )
1138                                                         RemovePair(UserObject, id1->mObject, UserGUID, id1->mGUID);
1139                                         }
1140
1141                                         id1->mMin[Axis + IsMax] = EPIndex--;
1142                                         *(CurrentMax+1) = *CurrentMax;
1143                                 }
1144
1145                                 if(SavedIndex!=EPIndex)
1146                                 {
1147                                         mBoxes[Saved.GetOwner()].mMin[Axis + Saved.IsMax()] = EPIndex;
1148                                         BaseEP[EPIndex] = Saved;
1149                                 }
1150                         }
1151                 }
1152         }
1153         return true;
1154 }
1155
1156 udword ArraySAP::DumpPairs(SAP_CreatePair create_cb, SAP_DeletePair delete_cb, void* cb_user_data, ASAP_Pair** pairs)
1157 {
1158         BatchCreate();
1159
1160         const udword* Entries = mData.GetEntries();
1161         const udword* Last = Entries + mData.GetNbEntries();
1162         mData.Reset();
1163
1164         udword* ToRemove = (udword*)Entries;
1165         while(Entries!=Last)
1166         {
1167                 const udword ID = *Entries++;
1168                 ASAP_Pair* UP = mPairs.mActivePairs + ID;
1169
1170                 {
1171                         ASSERT(UP->IsInArray());
1172                         if(UP->IsRemoved())
1173                         {
1174                                 // No need to call "ClearInArray" in this case, since the pair will get removed anyway
1175
1176                                 // Remove
1177                                 if(delete_cb && !UP->IsNew())
1178                                 {
1179 #ifdef PAIR_USER_DATA
1180                                         (delete_cb)(UP->GetObject0(), UP->GetObject1(), cb_user_data, UP->userData);
1181 #else
1182                                         (delete_cb)(UP->GetObject0(), UP->GetObject1(), cb_user_data, null);
1183 #endif
1184                                 }
1185
1186                                 *ToRemove++ = udword(UP->id0)<<16|UP->id1;
1187                         }
1188                         else
1189                         {
1190                                 UP->ClearInArray();
1191                                 // Add => already there... Might want to create user data, though
1192                                 if(UP->IsNew())
1193                                 {
1194                                         if(create_cb)
1195                                         {
1196 #ifdef PAIR_USER_DATA
1197                                                 UP->userData = (create_cb)(UP->GetObject0(), UP->GetObject1(), cb_user_data);
1198 #else
1199                                                 (create_cb)(UP->GetObject0(), UP->GetObject1(), cb_user_data);
1200 #endif
1201                                         }
1202                                         UP->ClearNew();
1203                                 }
1204                         }
1205                 }
1206         }
1207
1208         // #### try batch removal here
1209         Entries = mData.GetEntries();
1210         while(Entries!=ToRemove)
1211         {
1212                 const udword ID = *Entries++;
1213                 const udword id0 = ID>>16;
1214                 const udword id1 = ID&0xffff;
1215                 bool Status = mPairs.RemovePair(id0, id1);
1216                 ASSERT(Status);
1217         }
1218
1219 #ifdef RELEASE_ON_RESET
1220         mData.Empty();
1221 #endif
1222
1223         BatchRemove();
1224
1225         if(pairs)       *pairs = mPairs.mActivePairs;
1226
1227         return mPairs.mNbActivePairs;
1228 }