Planet
navi homePPSaboutscreenshotsdownloaddevelopmentforum

source: code/branches/ode/ode-0.9/OPCODE/OPC_SweepAndPrune.cpp @ 237

Last change on this file since 237 was 216, checked in by mathiask, 18 years ago

[Physik] add ode-0.9
File size: 16.9 KB
RevLine 
[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
22using namespace Opcode;
23
24inline_ 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///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
108SAP_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///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
123SAP_PairData::~SAP_PairData()
124{
125        Release();
126}
127
128void 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///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
144bool 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///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
165inline_ 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///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
179SAP_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///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
233inline_ 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.
240void 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.
282void 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
323void 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
339void 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///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
389SweepAndPrune::SweepAndPrune()
390{
391}
392
393///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
394/**
395 *      Destructor.
396 */
397///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
398SweepAndPrune::~SweepAndPrune()
399{
400}
401
402void SweepAndPrune::GetPairs(Pairs& pairs) const
403{
404        mPairs.DumpPairs(pairs);
405}
406
407void SweepAndPrune::GetPairs(PairCallback callback, void* user_data) const
408{
409        mPairs.DumpPairs(callback, user_data);
410}
411
412bool 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
489bool 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
522inline_ 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
533bool 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}
Note: See TracBrowser for help on using the repository browser.