00001
00002
00003
00004
00005
00006
00008
00010
00016
00017
00019
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;
00051 SAP_EndPoint* Previous;
00052 SAP_EndPoint* Next;
00053 udword Data;
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
00064 if(Previous) Previous->Next = Next;
00065 if(Next) Next->Previous = Previous;
00066
00067
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
00081 if(Previous) Previous->Next = Next;
00082 if(Next) Next->Previous = Previous;
00083
00084
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
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
00187 FreeElem = mFirstFree;
00188 mFirstFree = mFirstFree->mNext;
00189 }
00190 else
00191 {
00192 if(mNbUsedElements==mNbElements)
00193 {
00194
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
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;
00236 mFirstFree = elem;
00237 }
00238
00239
00240 void SAP_PairData::AddPair(udword id1, udword id2)
00241 {
00242
00243 Sort(id1, id2);
00244
00245 ASSERT(id1<mNbObjects);
00246 if(id1>=mNbObjects) return;
00247
00248
00249 SAP_Element* Current = mArray[id1];
00250
00251 if(!Current)
00252 {
00253
00254 mArray[id1] = GetFreeElem(id2, null);
00255 }
00256 else if(Current->mID>id2)
00257 {
00258
00259 mArray[id1] = GetFreeElem(id2, mArray[id1]);
00260 }
00261 else
00262 {
00263
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;
00272
00273
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
00282 void SAP_PairData::RemovePair(udword id1, udword id2)
00283 {
00284
00285 Sort(id1, id2);
00286
00287
00288 if(id1>=mNbObjects) return;
00289
00290
00291 SAP_Element* Current = mArray[id1];
00292
00293
00294 if(!Current) return;
00295
00296
00297 if(Current->mID==id2)
00298 {
00299 mArray[id1] = Current->mNext;
00300 FreeElem(Current);
00301 }
00302 else
00303 {
00304
00305 while(Current->mNext)
00306 {
00307
00308 if(Current->mNext->mID > id2) return;
00309
00310
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
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
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
00415 mNbObjects = nb_objects;
00416
00417 mBoxes = new SAP_Box[nb_objects];
00418
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
00447
00448 CurrentEndPoint->SetData(BoxIndex, SortedIndex&1);
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
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
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
00538
00539
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
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
00564
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
00575 {
00576 CurrentMin->Value = Limit;
00577
00578
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
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
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
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
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
00636 {
00637 CurrentMax->Value = Limit;
00638
00639
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
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 }