Go to the documentation of this file.
26 if(id0>id1)
Swap(id0, id1);
61 if(
this!=element &&
this!=element->
Next)
78 if(
this!=element &&
this!=element->
Previous)
148 if(!nb_objects)
return false;
153 mNbObjects = nb_objects;
181 if(remap) *remap = 0;
187 FreeElem = mFirstFree;
188 mFirstFree = mFirstFree->
mNext;
192 if(mNbUsedElements==mNbElements)
195 mNbElements = mNbElements ? (mNbElements<<1) : 2;
208 Remap(mFirstFree, Delta);
211 if(remap) *remap = Delta;
215 mElementPool = NewElems;
218 FreeElem = &mElementPool[mNbUsedElements++];
222 FreeElem->
mNext = next;
235 elem->
mNext = mFirstFree;
246 if(id1>=mNbObjects)
return;
254 mArray[id1] = GetFreeElem(id2,
null);
256 else if(Current->
mID>id2)
259 mArray[id1] = GetFreeElem(id2, mArray[id1]);
264 while(Current->
mNext)
266 if(Current->
mNext->
mID > id2)
break;
268 Current = Current->
mNext;
271 if(Current->
mID==id2)
return;
276 if(Delta)
Remap(Current, Delta);
288 if(id1>=mNbObjects)
return;
297 if(Current->
mID==id2)
299 mArray[id1] = Current->
mNext;
305 while(Current->
mNext)
308 if(Current->
mNext->
mID > id2)
return;
318 Current = Current->
mNext;
334 Current = Current->
mNext;
341 if(!callback)
return;
351 if(!(callback)(
i, Current->
mID, user_data))
return;
352 Current = Current->
mNext;
404 mPairs.DumpPairs(pairs);
409 mPairs.DumpPairs(callback, user_data);
415 mNbObjects = nb_objects;
417 mBoxes =
new SAP_Box[nb_objects];
420 float* Data =
new float[nb_objects*2];
422 for(
udword Axis=0;Axis<3;Axis++)
428 Data[
i*2+0] = boxes[
i]->
GetMin(Axis);
429 Data[
i*2+1] = boxes[
i]->
GetMax(Axis);
438 udword SortedIndex = *Sorted++;
439 float SortedCoord = Data[SortedIndex];
440 udword BoxIndex = SortedIndex>>1;
442 ASSERT(BoxIndex<nb_objects);
444 SAP_EndPoint* CurrentEndPoint = &mList[Axis][SortedIndex];
445 CurrentEndPoint->
Value = SortedCoord;
448 CurrentEndPoint->
SetData(BoxIndex, SortedIndex&1);
449 CurrentEndPoint->
Previous = PreviousEndPoint;
450 CurrentEndPoint->
Next =
null;
451 if(PreviousEndPoint) PreviousEndPoint->
Next = CurrentEndPoint;
453 if(CurrentEndPoint->
IsMax()) mBoxes[BoxIndex].Max[Axis] = CurrentEndPoint;
454 else mBoxes[BoxIndex].Min[Axis] = CurrentEndPoint;
456 PreviousEndPoint = CurrentEndPoint;
462 CheckListsIntegrity();
466 mPairs.Init(nb_objects);
478 if(id0!=id1 && boxes[id0]->
Intersect(*boxes[id1]))
480 mPairs.AddPair(id0, id1);
491 for(
udword Axis=0;Axis<3;Axis++)
507 if(Previous->
Value > Current->
Value)
return false;
511 if(Current->
Previous!=Previous)
return false;
514 Current = Current->
Next;
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;
535 for(
udword Axis=0;Axis<3;Axis++)
544 const float Limit =
box.GetMin(Axis);
545 if(Limit == CurrentMin->
Value)
548 else if(Limit < CurrentMin->Value)
550 CurrentMin->
Value = Limit;
568 if(id0!=id1 &&
Intersect(
box, mBoxes[id1])) mPairs.AddPair(id0, id1);
576 CurrentMin->
Value = Limit;
583 while((tmp = NewPos->
Next) && tmp->
Value < Limit)
593 if(id0!=id1) mPairs.RemovePair(id0, id1);
606 const float Limit =
box.GetMax(Axis);
607 if(Limit == CurrentMax->
Value)
610 else if(Limit > CurrentMax->
Value)
612 CurrentMax->
Value = Limit;
619 while((tmp = NewPos->
Next) && tmp->
Value < Limit)
629 if(id0!=id1 &&
Intersect(
box, mBoxes[id1])) mPairs.AddPair(id0, id1);
637 CurrentMax->
Value = Limit;
654 if(id0!=id1) mPairs.RemovePair(id0, id1);
A generic couple structure.
inline_ udword GetBoxID() const
inline_ void InsertAfter(SAP_EndPoint *element)
inline_ void ZeroMemory(void *addr, udword size)
udword id0
First index of the pair.
inline_ void Remap(SAP_Element *&element, udword delta)
unsigned int udword
sizeof(udword) must be 4
int BOOL
Another boolean type.
void RemovePair(udword id1, udword id2)
udword id1
Second index of the pair.
bool CheckListsIntegrity()
const inline_ udword * GetRanks() const
Access to results. mRanks is a list of indices in sorted order, i.e. in the order you may further pro...
bool Init(udword nb_objects, const AABB **boxes)
void AddPair(udword id1, udword id2)
FUNCTION OPCODE_API bool CompleteBoxPruning(udword nb, const AABB **array, Pairs &pairs, const Axes &axes)
inline_ void CopyMemory(void *dest, const void *src, udword size)
bool Init(udword nb_objects)
inline_ void Sort(udword &id0, udword &id1)
inline_ void AddPair(const Pair &p)
inline_ BOOL IsMax() const
inline_ BOOL Intersect(const AABB &a, const SAP_Box &b)
inline_ udword GetNbPairs() const
void GetPairs(Pairs &pairs) const
inline_ void InsertBefore(SAP_EndPoint *element)
bool UpdateObject(udword i, const AABB &box)
BOOL(* PairCallback)(udword id0, udword id1, void *user_data)
inline_ void Swap(udword &x, udword &y)
void DumpPairs(Pairs &pairs) const
SAP_Element * GetFreeElem(udword id, SAP_Element *next, udword *remap=null)
inline_ void FreeElem(SAP_Element *elem)
inline_ void GetMin(Point &min) const
Get min point of the box.
#define DELETEARRAY(x)
Deletes an array.
const inline_ Pair * GetPair(udword i) const
RadixSort & Sort(const udword *input, udword nb, RadixHint hint=RADIX_SIGNED)
inline_ SAP_Element(udword id, SAP_Element *next)
inline_ void GetMax(Point &max) const
Get max point of the box.
#define null
our own NULL pointer
inline_ void SetData(udword box_id, BOOL is_max)
openhrp3
Author(s): AIST, General Robotix Inc., Nakamura Lab of Dept. of Mechano Informatics at University of Tokyo
autogenerated on Wed Sep 7 2022 02:51:04