OPC_SweepAndPrune.cpp
Go to the documentation of this file.
1 /*
3  * OPCODE - Optimized Collision Detection
4  * Copyright (C) 2001 Pierre Terdiman
5  * Homepage: http://www.codercorner.com/Opcode.htm
6  */
8 
10 
16 
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 
30  {
31  public:
33  inline_ SAP_Element(udword id, SAP_Element* next) : mID(id), mNext(next) {}
35 
38  };
39 
41  {
42  public:
43  SAP_EndPoint* Min[3];
44  SAP_EndPoint* Max[3];
45  };
46 
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 
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 
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 
104 
109  mNbElements (0),
110  mNbUsedElements (0),
111  mElementPool (null),
112  mFirstFree (null),
113  mNbObjects (0),
114  mArray (null)
115 {
116 }
117 
119 
124 {
125  Release();
126 }
127 
129 {
130  mNbElements = 0;
131  mNbUsedElements = 0;
132  mNbObjects = 0;
133  DELETEARRAY(mElementPool);
134  DELETEARRAY(mArray);
135 }
136 
138 
143 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 
159 
164 inline_ void Remap(SAP_Element*& element, udword delta)
166 {
167  if(element) element = (SAP_Element*)(udword(element) + delta);
168 }
169 
171 
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  udword Delta = udword(NewElems) - udword(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 
228 
234 {
235  elem->mNext = mFirstFree; // Next free
236  mFirstFree = elem;
237 }
238 
239 // Add a pair to the set.
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.
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 
385 
390 {
391 }
392 
394 
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 
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 
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 }
inline_ void FreeElem(SAP_Element *elem)
inline_ void GetMin(Point &min) const
Get min point of the box.
Definition: OPC_IceHook.h:344
#define FALSE
Definition: OPC_IceHook.h:9
inline_ void SetData(udword box_id, BOOL is_max)
#define null
our own NULL pointer
Definition: IceTypes.h:57
inline_ udword GetBoxID() const
A generic couple structure.
Definition: OPC_IceHook.h:17
SAP_Element * GetFreeElem(udword id, SAP_Element *next, udword *remap=null)
inline_ void AddPair(const Pair &p)
Definition: OPC_IceHook.h:42
void RemovePair(udword id1, udword id2)
#define TRUE
Definition: OPC_IceHook.h:13
#define DELETEARRAY(x)
Deletes an array.
#define inline_
inline_ void InsertAfter(SAP_EndPoint *element)
bool Init(udword nb_objects, const AABB **boxes)
BOOL(* PairCallback)(udword id0, udword id1, void *user_data)
png_uint_32 i
Definition: png.h:2735
void AddPair(udword id1, udword id2)
long b
Definition: jpegint.h:371
udword id1
Second index of the pair.
Definition: OPC_IceHook.h:23
inline_ BOOL IsMax() const
inline_ SAP_Element(udword id, SAP_Element *next)
int BOOL
Another boolean type.
Definition: IceTypes.h:102
unsigned int udword
sizeof(udword) must be 4
Definition: IceTypes.h:65
FUNCTION OPCODE_API bool CompleteBoxPruning(udword nb, const AABB **array, Pairs &pairs, const Axes &axes)
SAP_EndPoint * Min[3]
#define CHECKALLOC(x)
inline_ void InsertBefore(SAP_EndPoint *element)
udword id0
First index of the pair.
Definition: OPC_IceHook.h:22
void DumpPairs(Pairs &pairs) const
inline_ void GetMax(Point &max) const
Get max point of the box.
Definition: OPC_IceHook.h:346
RadixSort & Sort(const udword *input, udword nb, RadixHint hint=RADIX_SIGNED)
inline_ const udword * GetRanks() const
Access to results. mRanks is a list of indices in sorted order, i.e. in the order you may further pro...
Definition: OPC_IceHook.h:38
void GetPairs(Pairs &pairs) const
inline_ void Remap(SAP_Element *&element, udword delta)
#define ASSERT(exp)
Definition: OPC_IceHook.h:24
id
inline_ void CopyMemory(void *dest, const void *src, udword size)
inline_ void ZeroMemory(void *addr, udword size)
inline_ BOOL Intersect(const AABB &a, const SAP_Box &b)
SAP_EndPoint * Max[3]
inline_ void Sort(udword &id0, udword &id1)
inline_ udword GetNbPairs() const
Definition: OPC_IceHook.h:33
inline_ const Pair * GetPair(udword i) const
Definition: OPC_IceHook.h:35
bool UpdateObject(udword i, const AABB &box)
bool Init(udword nb_objects)
Definition: jquant2.c:258
*inline_ void Swap(udword &x, udword &y)
Definition: IceUtils.h:99


openhrp3
Author(s): AIST, General Robotix Inc., Nakamura Lab of Dept. of Mechano Informatics at University of Tokyo
autogenerated on Thu Sep 8 2022 02:24:04