aboutsummaryrefslogtreecommitdiffstatshomepage
path: root/libraries/ode-0.9/OPCODE/OPC_SweepAndPrune.cpp
diff options
context:
space:
mode:
authordan miller2007-10-19 05:15:33 +0000
committerdan miller2007-10-19 05:15:33 +0000
commit79eca25c945a535a7a0325999034bae17da92412 (patch)
tree40ff433d94859d629aac933d5ec73b382f62ba1a /libraries/ode-0.9/OPCODE/OPC_SweepAndPrune.cpp
parentadding ode source to /libraries (diff)
downloadopensim-SC-79eca25c945a535a7a0325999034bae17da92412.zip
opensim-SC-79eca25c945a535a7a0325999034bae17da92412.tar.gz
opensim-SC-79eca25c945a535a7a0325999034bae17da92412.tar.bz2
opensim-SC-79eca25c945a535a7a0325999034bae17da92412.tar.xz
resubmitting ode
Diffstat (limited to 'libraries/ode-0.9/OPCODE/OPC_SweepAndPrune.cpp')
-rw-r--r--libraries/ode-0.9/OPCODE/OPC_SweepAndPrune.cpp664
1 files changed, 664 insertions, 0 deletions
diff --git a/libraries/ode-0.9/OPCODE/OPC_SweepAndPrune.cpp b/libraries/ode-0.9/OPCODE/OPC_SweepAndPrune.cpp
new file mode 100644
index 0000000..7f1a8f3
--- /dev/null
+++ b/libraries/ode-0.9/OPCODE/OPC_SweepAndPrune.cpp
@@ -0,0 +1,664 @@
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}
generated by cgit v1.1 at 2024-12-28 07:46:40 +0000