[216] | 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 implementation of the sweep-and-prune algorithm (moved from Z-Collide) |
---|
| 12 | * \file OPC_SweepAndPrune.cpp |
---|
| 13 | * \author Pierre Terdiman |
---|
| 14 | * \date January, 29, 2000 |
---|
| 15 | */ |
---|
| 16 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// |
---|
| 17 | |
---|
| 18 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// |
---|
| 19 | // Precompiled Header |
---|
| 20 | #include "Stdafx.h" |
---|
| 21 | |
---|
| 22 | using namespace Opcode; |
---|
| 23 | |
---|
| 24 | inline_ void Sort(udword& id0, udword& id1) |
---|
| 25 | { |
---|
| 26 | if(id0>id1) Swap(id0, id1); |
---|
| 27 | } |
---|
| 28 | |
---|
| 29 | class Opcode::SAP_Element |
---|
| 30 | { |
---|
| 31 | public: |
---|
| 32 | inline_ SAP_Element() {} |
---|
| 33 | inline_ SAP_Element(udword id, SAP_Element* next) : mID(id), mNext(next) {} |
---|
| 34 | inline_ ~SAP_Element() {} |
---|
| 35 | |
---|
| 36 | udword mID; |
---|
| 37 | SAP_Element* mNext; |
---|
| 38 | }; |
---|
| 39 | |
---|
| 40 | class Opcode::SAP_Box |
---|
| 41 | { |
---|
| 42 | public: |
---|
| 43 | SAP_EndPoint* Min[3]; |
---|
| 44 | SAP_EndPoint* Max[3]; |
---|
| 45 | }; |
---|
| 46 | |
---|
| 47 | class Opcode::SAP_EndPoint |
---|
| 48 | { |
---|
| 49 | public: |
---|
| 50 | float Value; // Min or Max value |
---|
| 51 | SAP_EndPoint* Previous; // Previous EndPoint whose Value is smaller than ours (or null) |
---|
| 52 | SAP_EndPoint* Next; // Next EndPoint whose Value is greater than ours (or null) |
---|
| 53 | udword Data; // Parent box ID *2 | MinMax flag |
---|
| 54 | |
---|
| 55 | inline_ void SetData(udword box_id, BOOL is_max) { Data = (box_id<<1)|is_max; } |
---|
| 56 | inline_ BOOL IsMax() const { return Data & 1; } |
---|
| 57 | inline_ udword GetBoxID() const { return Data>>1; } |
---|
| 58 | |
---|
| 59 | inline_ void InsertAfter(SAP_EndPoint* element) |
---|
| 60 | { |
---|
| 61 | if(this!=element && this!=element->Next) |
---|
| 62 | { |
---|
| 63 | // Remove |
---|
| 64 | if(Previous) Previous->Next = Next; |
---|
| 65 | if(Next) Next->Previous = Previous; |
---|
| 66 | |
---|
| 67 | // Insert |
---|
| 68 | Next = element->Next; |
---|
| 69 | if(Next) Next->Previous = this; |
---|
| 70 | |
---|
| 71 | element->Next = this; |
---|
| 72 | Previous = element; |
---|
| 73 | } |
---|
| 74 | } |
---|
| 75 | |
---|
| 76 | inline_ void InsertBefore(SAP_EndPoint* element) |
---|
| 77 | { |
---|
| 78 | if(this!=element && this!=element->Previous) |
---|
| 79 | { |
---|
| 80 | // Remove |
---|
| 81 | if(Previous) Previous->Next = Next; |
---|
| 82 | if(Next) Next->Previous = Previous; |
---|
| 83 | |
---|
| 84 | // Insert |
---|
| 85 | Previous = element->Previous; |
---|
| 86 | element->Previous = this; |
---|
| 87 | |
---|
| 88 | Next = element; |
---|
| 89 | if(Previous) Previous->Next = this; |
---|
| 90 | } |
---|
| 91 | } |
---|
| 92 | }; |
---|
| 93 | |
---|
| 94 | |
---|
| 95 | |
---|
| 96 | |
---|
| 97 | |
---|
| 98 | |
---|
| 99 | |
---|
| 100 | |
---|
| 101 | |
---|
| 102 | |
---|
| 103 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// |
---|
| 104 | /** |
---|
| 105 | * Constructor. |
---|
| 106 | */ |
---|
| 107 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// |
---|
| 108 | SAP_PairData::SAP_PairData() : |
---|
| 109 | mNbElements (0), |
---|
| 110 | mNbUsedElements (0), |
---|
| 111 | mElementPool (null), |
---|
| 112 | mFirstFree (null), |
---|
| 113 | mNbObjects (0), |
---|
| 114 | mArray (null) |
---|
| 115 | { |
---|
| 116 | } |
---|
| 117 | |
---|
| 118 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// |
---|
| 119 | /** |
---|
| 120 | * Destructor. |
---|
| 121 | */ |
---|
| 122 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// |
---|
| 123 | SAP_PairData::~SAP_PairData() |
---|
| 124 | { |
---|
| 125 | Release(); |
---|
| 126 | } |
---|
| 127 | |
---|
| 128 | void SAP_PairData::Release() |
---|
| 129 | { |
---|
| 130 | mNbElements = 0; |
---|
| 131 | mNbUsedElements = 0; |
---|
| 132 | mNbObjects = 0; |
---|
| 133 | DELETEARRAY(mElementPool); |
---|
| 134 | DELETEARRAY(mArray); |
---|
| 135 | } |
---|
| 136 | |
---|
| 137 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// |
---|
| 138 | /** |
---|
| 139 | * Initializes. |
---|
| 140 | * \param nb_objects [in] |
---|
| 141 | * \return true if success |
---|
| 142 | */ |
---|
| 143 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// |
---|
| 144 | bool SAP_PairData::Init(udword nb_objects) |
---|
| 145 | { |
---|
| 146 | // Make sure everything has been released |
---|
| 147 | Release(); |
---|
| 148 | if(!nb_objects) return false; |
---|
| 149 | |
---|
| 150 | mArray = new SAP_Element*[nb_objects]; |
---|
| 151 | CHECKALLOC(mArray); |
---|
| 152 | ZeroMemory(mArray, nb_objects*sizeof(SAP_Element*)); |
---|
| 153 | mNbObjects = nb_objects; |
---|
| 154 | |
---|
| 155 | return true; |
---|
| 156 | } |
---|
| 157 | |
---|
| 158 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// |
---|
| 159 | /** |
---|
| 160 | * Remaps a pointer when pool gets resized. |
---|
| 161 | * \param element [in/out] remapped element |
---|
| 162 | * \param delta [in] offset in bytes |
---|
| 163 | */ |
---|
| 164 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// |
---|
| 165 | inline_ void Remap(SAP_Element*& element, size_t delta) |
---|
| 166 | { |
---|
| 167 | if(element) element = (SAP_Element*)(size_t(element) + delta); |
---|
| 168 | } |
---|
| 169 | |
---|
| 170 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// |
---|
| 171 | /** |
---|
| 172 | * Gets a free element in the pool. |
---|
| 173 | * \param id [in] element id |
---|
| 174 | * \param next [in] next element |
---|
| 175 | * \param remap [out] possible remapping offset |
---|
| 176 | * \return the new element |
---|
| 177 | */ |
---|
| 178 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// |
---|
| 179 | SAP_Element* SAP_PairData::GetFreeElem(udword id, SAP_Element* next, udword* remap) |
---|
| 180 | { |
---|
| 181 | if(remap) *remap = 0; |
---|
| 182 | |
---|
| 183 | SAP_Element* FreeElem; |
---|
| 184 | if(mFirstFree) |
---|
| 185 | { |
---|
| 186 | // Recycle |
---|
| 187 | FreeElem = mFirstFree; |
---|
| 188 | mFirstFree = mFirstFree->mNext; // First free = next free (or null) |
---|
| 189 | } |
---|
| 190 | else |
---|
| 191 | { |
---|
| 192 | if(mNbUsedElements==mNbElements) |
---|
| 193 | { |
---|
| 194 | // Resize |
---|
| 195 | mNbElements = mNbElements ? (mNbElements<<1) : 2; |
---|
| 196 | |
---|
| 197 | SAP_Element* NewElems = new SAP_Element[mNbElements]; |
---|
| 198 | |
---|
| 199 | if(mNbUsedElements) CopyMemory(NewElems, mElementPool, mNbUsedElements*sizeof(SAP_Element)); |
---|
| 200 | |
---|
| 201 | // Remap everything |
---|
| 202 | { |
---|
| 203 | size_t Delta = size_t(NewElems) - size_t(mElementPool); |
---|
| 204 | |
---|
| 205 | for(udword i=0;i<mNbUsedElements;i++) Remap(NewElems[i].mNext, Delta); |
---|
| 206 | for(udword i=0;i<mNbObjects;i++) Remap(mArray[i], Delta); |
---|
| 207 | |
---|
| 208 | Remap(mFirstFree, Delta); |
---|
| 209 | Remap(next, Delta); |
---|
| 210 | |
---|
| 211 | if(remap) *remap = Delta; |
---|
| 212 | } |
---|
| 213 | |
---|
| 214 | DELETEARRAY(mElementPool); |
---|
| 215 | mElementPool = NewElems; |
---|
| 216 | } |
---|
| 217 | |
---|
| 218 | FreeElem = &mElementPool[mNbUsedElements++]; |
---|
| 219 | } |
---|
| 220 | |
---|
| 221 | FreeElem->mID = id; |
---|
| 222 | FreeElem->mNext = next; |
---|
| 223 | |
---|
| 224 | return FreeElem; |
---|
| 225 | } |
---|
| 226 | |
---|
| 227 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// |
---|
| 228 | /** |
---|
| 229 | * Frees an element of the pool. |
---|
| 230 | * \param elem [in] element to free/recycle |
---|
| 231 | */ |
---|
| 232 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// |
---|
| 233 | inline_ void SAP_PairData::FreeElem(SAP_Element* elem) |
---|
| 234 | { |
---|
| 235 | elem->mNext = mFirstFree; // Next free |
---|
| 236 | mFirstFree = elem; |
---|
| 237 | } |
---|
| 238 | |
---|
| 239 | // Add a pair to the set. |
---|
| 240 | void SAP_PairData::AddPair(udword id1, udword id2) |
---|
| 241 | { |
---|
| 242 | // Order the ids |
---|
| 243 | Sort(id1, id2); |
---|
| 244 | |
---|
| 245 | ASSERT(id1<mNbObjects); |
---|
| 246 | if(id1>=mNbObjects) return; |
---|
| 247 | |
---|
| 248 | // Select the right list from "mArray". |
---|
| 249 | SAP_Element* Current = mArray[id1]; |
---|
| 250 | |
---|
| 251 | if(!Current) |
---|
| 252 | { |
---|
| 253 | // Empty slot => create new element |
---|
| 254 | mArray[id1] = GetFreeElem(id2, null); |
---|
| 255 | } |
---|
| 256 | else if(Current->mID>id2) |
---|
| 257 | { |
---|
| 258 | // The list is not empty but all elements are greater than id2 => insert id2 in the front. |
---|
| 259 | mArray[id1] = GetFreeElem(id2, mArray[id1]); |
---|
| 260 | } |
---|
| 261 | else |
---|
| 262 | { |
---|
| 263 | // Else find the correct location in the sorted list (ascending order) and insert id2 there. |
---|
| 264 | while(Current->mNext) |
---|
| 265 | { |
---|
| 266 | if(Current->mNext->mID > id2) break; |
---|
| 267 | |
---|
| 268 | Current = Current->mNext; |
---|
| 269 | } |
---|
| 270 | |
---|
| 271 | if(Current->mID==id2) return; // The pair already exists |
---|
| 272 | |
---|
| 273 | // Current->mNext = GetFreeElem(id2, Current->mNext); |
---|
| 274 | udword Delta; |
---|
| 275 | SAP_Element* E = GetFreeElem(id2, Current->mNext, &Delta); |
---|
| 276 | if(Delta) Remap(Current, Delta); |
---|
| 277 | Current->mNext = E; |
---|
| 278 | } |
---|
| 279 | } |
---|
| 280 | |
---|
| 281 | // Delete a pair from the set. |
---|
| 282 | void SAP_PairData::RemovePair(udword id1, udword id2) |
---|
| 283 | { |
---|
| 284 | // Order the ids. |
---|
| 285 | Sort(id1, id2); |
---|
| 286 | |
---|
| 287 | // Exit if the pair doesn't exist in the set |
---|
| 288 | if(id1>=mNbObjects) return; |
---|
| 289 | |
---|
| 290 | // Otherwise, select the correct list. |
---|
| 291 | SAP_Element* Current = mArray[id1]; |
---|
| 292 | |
---|
| 293 | // If this list is empty, the pair doesn't exist. |
---|
| 294 | if(!Current) return; |
---|
| 295 | |
---|
| 296 | // Otherwise, if id2 is the first element, delete it. |
---|
| 297 | if(Current->mID==id2) |
---|
| 298 | { |
---|
| 299 | mArray[id1] = Current->mNext; |
---|
| 300 | FreeElem(Current); |
---|
| 301 | } |
---|
| 302 | else |
---|
| 303 | { |
---|
| 304 | // If id2 is not the first element, start traversing the sorted list. |
---|
| 305 | while(Current->mNext) |
---|
| 306 | { |
---|
| 307 | // If we have moved too far away without hitting id2, then the pair doesn't exist |
---|
| 308 | if(Current->mNext->mID > id2) return; |
---|
| 309 | |
---|
| 310 | // Otherwise, delete id2. |
---|
| 311 | if(Current->mNext->mID == id2) |
---|
| 312 | { |
---|
| 313 | SAP_Element* Temp = Current->mNext; |
---|
| 314 | Current->mNext = Temp->mNext; |
---|
| 315 | FreeElem(Temp); |
---|
| 316 | return; |
---|
| 317 | } |
---|
| 318 | Current = Current->mNext; |
---|
| 319 | } |
---|
| 320 | } |
---|
| 321 | } |
---|
| 322 | |
---|
| 323 | void SAP_PairData::DumpPairs(Pairs& pairs) const |
---|
| 324 | { |
---|
| 325 | // ### Ugly and slow |
---|
| 326 | for(udword i=0;i<mNbObjects;i++) |
---|
| 327 | { |
---|
| 328 | SAP_Element* Current = mArray[i]; |
---|
| 329 | while(Current) |
---|
| 330 | { |
---|
| 331 | ASSERT(Current->mID<mNbObjects); |
---|
| 332 | |
---|
| 333 | pairs.AddPair(i, Current->mID); |
---|
| 334 | Current = Current->mNext; |
---|
| 335 | } |
---|
| 336 | } |
---|
| 337 | } |
---|
| 338 | |
---|
| 339 | void SAP_PairData::DumpPairs(PairCallback callback, void* user_data) const |
---|
| 340 | { |
---|
| 341 | if(!callback) return; |
---|
| 342 | |
---|
| 343 | // ### Ugly and slow |
---|
| 344 | for(udword i=0;i<mNbObjects;i++) |
---|
| 345 | { |
---|
| 346 | SAP_Element* Current = mArray[i]; |
---|
| 347 | while(Current) |
---|
| 348 | { |
---|
| 349 | ASSERT(Current->mID<mNbObjects); |
---|
| 350 | |
---|
| 351 | if(!(callback)(i, Current->mID, user_data)) return; |
---|
| 352 | Current = Current->mNext; |
---|
| 353 | } |
---|
| 354 | } |
---|
| 355 | } |
---|
| 356 | |
---|
| 357 | |
---|
| 358 | |
---|
| 359 | |
---|
| 360 | |
---|
| 361 | |
---|
| 362 | |
---|
| 363 | |
---|
| 364 | |
---|
| 365 | |
---|
| 366 | |
---|
| 367 | |
---|
| 368 | |
---|
| 369 | |
---|
| 370 | |
---|
| 371 | |
---|
| 372 | |
---|
| 373 | |
---|
| 374 | |
---|
| 375 | |
---|
| 376 | |
---|
| 377 | |
---|
| 378 | |
---|
| 379 | |
---|
| 380 | |
---|
| 381 | |
---|
| 382 | |
---|
| 383 | |
---|
| 384 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// |
---|
| 385 | /** |
---|
| 386 | * Constructor. |
---|
| 387 | */ |
---|
| 388 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// |
---|
| 389 | SweepAndPrune::SweepAndPrune() |
---|
| 390 | { |
---|
| 391 | } |
---|
| 392 | |
---|
| 393 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// |
---|
| 394 | /** |
---|
| 395 | * Destructor. |
---|
| 396 | */ |
---|
| 397 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// |
---|
| 398 | SweepAndPrune::~SweepAndPrune() |
---|
| 399 | { |
---|
| 400 | } |
---|
| 401 | |
---|
| 402 | void SweepAndPrune::GetPairs(Pairs& pairs) const |
---|
| 403 | { |
---|
| 404 | mPairs.DumpPairs(pairs); |
---|
| 405 | } |
---|
| 406 | |
---|
| 407 | void SweepAndPrune::GetPairs(PairCallback callback, void* user_data) const |
---|
| 408 | { |
---|
| 409 | mPairs.DumpPairs(callback, user_data); |
---|
| 410 | } |
---|
| 411 | |
---|
| 412 | bool SweepAndPrune::Init(udword nb_objects, const AABB** boxes) |
---|
| 413 | { |
---|
| 414 | // 1) Create sorted lists |
---|
| 415 | mNbObjects = nb_objects; |
---|
| 416 | |
---|
| 417 | mBoxes = new SAP_Box[nb_objects]; |
---|
| 418 | // for(udword i=0;i<nb_objects;i++) mBoxes[i].Box = *boxes[i]; |
---|
| 419 | |
---|
| 420 | float* Data = new float[nb_objects*2]; |
---|
| 421 | |
---|
| 422 | for(udword Axis=0;Axis<3;Axis++) |
---|
| 423 | { |
---|
| 424 | mList[Axis] = new SAP_EndPoint[nb_objects*2]; |
---|
| 425 | |
---|
| 426 | for(udword i=0;i<nb_objects;i++) |
---|
| 427 | { |
---|
| 428 | Data[i*2+0] = boxes[i]->GetMin(Axis); |
---|
| 429 | Data[i*2+1] = boxes[i]->GetMax(Axis); |
---|
| 430 | } |
---|
| 431 | RadixSort RS; |
---|
| 432 | const udword* Sorted = RS.Sort(Data, nb_objects*2).GetRanks(); |
---|
| 433 | |
---|
| 434 | SAP_EndPoint* PreviousEndPoint = null; |
---|
| 435 | |
---|
| 436 | for(udword i=0;i<nb_objects*2;i++) |
---|
| 437 | { |
---|
| 438 | udword SortedIndex = *Sorted++; |
---|
| 439 | float SortedCoord = Data[SortedIndex]; |
---|
| 440 | udword BoxIndex = SortedIndex>>1; |
---|
| 441 | |
---|
| 442 | ASSERT(BoxIndex<nb_objects); |
---|
| 443 | |
---|
| 444 | SAP_EndPoint* CurrentEndPoint = &mList[Axis][SortedIndex]; |
---|
| 445 | CurrentEndPoint->Value = SortedCoord; |
---|
| 446 | // CurrentEndPoint->IsMax = SortedIndex&1; // ### could be implicit ? |
---|
| 447 | // CurrentEndPoint->ID = BoxIndex; // ### could be implicit ? |
---|
| 448 | CurrentEndPoint->SetData(BoxIndex, SortedIndex&1); // ### could be implicit ? |
---|
| 449 | CurrentEndPoint->Previous = PreviousEndPoint; |
---|
| 450 | CurrentEndPoint->Next = null; |
---|
| 451 | if(PreviousEndPoint) PreviousEndPoint->Next = CurrentEndPoint; |
---|
| 452 | |
---|
| 453 | if(CurrentEndPoint->IsMax()) mBoxes[BoxIndex].Max[Axis] = CurrentEndPoint; |
---|
| 454 | else mBoxes[BoxIndex].Min[Axis] = CurrentEndPoint; |
---|
| 455 | |
---|
| 456 | PreviousEndPoint = CurrentEndPoint; |
---|
| 457 | } |
---|
| 458 | } |
---|
| 459 | |
---|
| 460 | DELETEARRAY(Data); |
---|
| 461 | |
---|
| 462 | CheckListsIntegrity(); |
---|
| 463 | |
---|
| 464 | // 2) Quickly find starting pairs |
---|
| 465 | |
---|
| 466 | mPairs.Init(nb_objects); |
---|
| 467 | |
---|
| 468 | { |
---|
| 469 | Pairs P; |
---|
| 470 | CompleteBoxPruning(nb_objects, boxes, P, Axes(AXES_XZY)); |
---|
| 471 | for(udword i=0;i<P.GetNbPairs();i++) |
---|
| 472 | { |
---|
| 473 | const Pair* PP = P.GetPair(i); |
---|
| 474 | |
---|
| 475 | udword id0 = PP->id0; |
---|
| 476 | udword id1 = PP->id1; |
---|
| 477 | |
---|
| 478 | if(id0!=id1 && boxes[id0]->Intersect(*boxes[id1])) |
---|
| 479 | { |
---|
| 480 | mPairs.AddPair(id0, id1); |
---|
| 481 | } |
---|
| 482 | else ASSERT(0); |
---|
| 483 | } |
---|
| 484 | } |
---|
| 485 | |
---|
| 486 | return true; |
---|
| 487 | } |
---|
| 488 | |
---|
| 489 | bool SweepAndPrune::CheckListsIntegrity() |
---|
| 490 | { |
---|
| 491 | for(udword Axis=0;Axis<3;Axis++) |
---|
| 492 | { |
---|
| 493 | // Find list head |
---|
| 494 | SAP_EndPoint* Current = mList[Axis]; |
---|
| 495 | while(Current->Previous) Current = Current->Previous; |
---|
| 496 | |
---|
| 497 | udword Nb = 0; |
---|
| 498 | |
---|
| 499 | SAP_EndPoint* Previous = null; |
---|
| 500 | while(Current) |
---|
| 501 | { |
---|
| 502 | Nb++; |
---|
| 503 | |
---|
| 504 | if(Previous) |
---|
| 505 | { |
---|
| 506 | ASSERT(Previous->Value <= Current->Value); |
---|
| 507 | if(Previous->Value > Current->Value) return false; |
---|
| 508 | } |
---|
| 509 | |
---|
| 510 | ASSERT(Current->Previous==Previous); |
---|
| 511 | if(Current->Previous!=Previous) return false; |
---|
| 512 | |
---|
| 513 | Previous = Current; |
---|
| 514 | Current = Current->Next; |
---|
| 515 | } |
---|
| 516 | |
---|
| 517 | ASSERT(Nb==mNbObjects*2); |
---|
| 518 | } |
---|
| 519 | return true; |
---|
| 520 | } |
---|
| 521 | |
---|
| 522 | inline_ BOOL Intersect(const AABB& a, const SAP_Box& b) |
---|
| 523 | { |
---|
| 524 | if(b.Max[0]->Value < a.GetMin(0) || a.GetMax(0) < b.Min[0]->Value |
---|
| 525 | || b.Max[1]->Value < a.GetMin(1) || a.GetMax(1) < b.Min[1]->Value |
---|
| 526 | || b.Max[2]->Value < a.GetMin(2) || a.GetMax(2) < b.Min[2]->Value) return FALSE; |
---|
| 527 | |
---|
| 528 | return TRUE; |
---|
| 529 | } |
---|
| 530 | |
---|
| 531 | |
---|
| 532 | |
---|
| 533 | bool SweepAndPrune::UpdateObject(udword i, const AABB& box) |
---|
| 534 | { |
---|
| 535 | for(udword Axis=0;Axis<3;Axis++) |
---|
| 536 | { |
---|
| 537 | // udword Base = (udword)&mList[Axis][0]; |
---|
| 538 | |
---|
| 539 | // Update min |
---|
| 540 | { |
---|
| 541 | SAP_EndPoint* const CurrentMin = mBoxes[i].Min[Axis]; |
---|
| 542 | ASSERT(!CurrentMin->IsMax()); |
---|
| 543 | |
---|
| 544 | const float Limit = box.GetMin(Axis); |
---|
| 545 | if(Limit == CurrentMin->Value) |
---|
| 546 | { |
---|
| 547 | } |
---|
| 548 | else if(Limit < CurrentMin->Value) |
---|
| 549 | { |
---|
| 550 | CurrentMin->Value = Limit; |
---|
| 551 | |
---|
| 552 | // Min is moving left: |
---|
| 553 | SAP_EndPoint* NewPos = CurrentMin; |
---|
| 554 | ASSERT(NewPos); |
---|
| 555 | |
---|
| 556 | SAP_EndPoint* tmp; |
---|
| 557 | while((tmp = NewPos->Previous) && tmp->Value > Limit) |
---|
| 558 | { |
---|
| 559 | NewPos = tmp; |
---|
| 560 | |
---|
| 561 | if(NewPos->IsMax()) |
---|
| 562 | { |
---|
| 563 | // Our min passed a max => start overlap |
---|
| 564 | //udword SortedIndex = (udword(CurrentMin) - Base)/sizeof(NS_EndPoint); |
---|
| 565 | const udword id0 = CurrentMin->GetBoxID(); |
---|
| 566 | const udword id1 = NewPos->GetBoxID(); |
---|
| 567 | |
---|
| 568 | if(id0!=id1 && Intersect(box, mBoxes[id1])) mPairs.AddPair(id0, id1); |
---|
| 569 | } |
---|
| 570 | } |
---|
| 571 | |
---|
| 572 | CurrentMin->InsertBefore(NewPos); |
---|
| 573 | } |
---|
| 574 | else// if(Limit > CurrentMin->Value) |
---|
| 575 | { |
---|
| 576 | CurrentMin->Value = Limit; |
---|
| 577 | |
---|
| 578 | // Min is moving right: |
---|
| 579 | SAP_EndPoint* NewPos = CurrentMin; |
---|
| 580 | ASSERT(NewPos); |
---|
| 581 | |
---|
| 582 | SAP_EndPoint* tmp; |
---|
| 583 | while((tmp = NewPos->Next) && tmp->Value < Limit) |
---|
| 584 | { |
---|
| 585 | NewPos = tmp; |
---|
| 586 | |
---|
| 587 | if(NewPos->IsMax()) |
---|
| 588 | { |
---|
| 589 | // Our min passed a max => stop overlap |
---|
| 590 | const udword id0 = CurrentMin->GetBoxID(); |
---|
| 591 | const udword id1 = NewPos->GetBoxID(); |
---|
| 592 | |
---|
| 593 | if(id0!=id1) mPairs.RemovePair(id0, id1); |
---|
| 594 | } |
---|
| 595 | } |
---|
| 596 | |
---|
| 597 | CurrentMin->InsertAfter(NewPos); |
---|
| 598 | } |
---|
| 599 | } |
---|
| 600 | |
---|
| 601 | // Update max |
---|
| 602 | { |
---|
| 603 | SAP_EndPoint* const CurrentMax = mBoxes[i].Max[Axis]; |
---|
| 604 | ASSERT(CurrentMax->IsMax()); |
---|
| 605 | |
---|
| 606 | const float Limit = box.GetMax(Axis); |
---|
| 607 | if(Limit == CurrentMax->Value) |
---|
| 608 | { |
---|
| 609 | } |
---|
| 610 | else if(Limit > CurrentMax->Value) |
---|
| 611 | { |
---|
| 612 | CurrentMax->Value = Limit; |
---|
| 613 | |
---|
| 614 | // Max is moving right: |
---|
| 615 | SAP_EndPoint* NewPos = CurrentMax; |
---|
| 616 | ASSERT(NewPos); |
---|
| 617 | |
---|
| 618 | SAP_EndPoint* tmp; |
---|
| 619 | while((tmp = NewPos->Next) && tmp->Value < Limit) |
---|
| 620 | { |
---|
| 621 | NewPos = tmp; |
---|
| 622 | |
---|
| 623 | if(!NewPos->IsMax()) |
---|
| 624 | { |
---|
| 625 | // Our max passed a min => start overlap |
---|
| 626 | const udword id0 = CurrentMax->GetBoxID(); |
---|
| 627 | const udword id1 = NewPos->GetBoxID(); |
---|
| 628 | |
---|
| 629 | if(id0!=id1 && Intersect(box, mBoxes[id1])) mPairs.AddPair(id0, id1); |
---|
| 630 | } |
---|
| 631 | } |
---|
| 632 | |
---|
| 633 | CurrentMax->InsertAfter(NewPos); |
---|
| 634 | } |
---|
| 635 | else// if(Limit < CurrentMax->Value) |
---|
| 636 | { |
---|
| 637 | CurrentMax->Value = Limit; |
---|
| 638 | |
---|
| 639 | // Max is moving left: |
---|
| 640 | SAP_EndPoint* NewPos = CurrentMax; |
---|
| 641 | ASSERT(NewPos); |
---|
| 642 | |
---|
| 643 | SAP_EndPoint* tmp; |
---|
| 644 | while((tmp = NewPos->Previous) && tmp->Value > Limit) |
---|
| 645 | { |
---|
| 646 | NewPos = tmp; |
---|
| 647 | |
---|
| 648 | if(!NewPos->IsMax()) |
---|
| 649 | { |
---|
| 650 | // Our max passed a min => stop overlap |
---|
| 651 | const udword id0 = CurrentMax->GetBoxID(); |
---|
| 652 | const udword id1 = NewPos->GetBoxID(); |
---|
| 653 | |
---|
| 654 | if(id0!=id1) mPairs.RemovePair(id0, id1); |
---|
| 655 | } |
---|
| 656 | } |
---|
| 657 | |
---|
| 658 | CurrentMax->InsertBefore(NewPos); |
---|
| 659 | } |
---|
| 660 | } |
---|
| 661 | } |
---|
| 662 | |
---|
| 663 | return true; |
---|
| 664 | } |
---|