OPC_BoxPruning.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 
18 /*
20  You could use a complex sweep-and-prune as implemented in I-Collide.
21  You could use a complex hashing scheme as implemented in V-Clip or recently in ODE it seems.
22  You could use a "Recursive Dimensional Clustering" algorithm as implemented in GPG2.
23 
24  Or you could use this.
25  Faster ? I don't know. Probably not. It would be a shame. But who knows ?
26  Easier ? Definitely. Enjoy the sheer simplicity.
28 */
29 
30 
32 // Precompiled Header
33 #include "Stdafx.h"
34 
35 using namespace Opcode;
36 
37  inline_ void FindRunningIndex(udword& index, float* array, udword* sorted, int last, float max)
38  {
39  int First=index;
40  while(First<=last)
41  {
42  index = (First+last)>>1;
43 
44  if(max>array[sorted[index]]) First = index+1;
45  else last = index-1;
46  }
47  }
48 // ### could be log(n) !
49 // and maybe use cmp integers
50 
51 // InsertionSort has better coherence, RadixSort is better for one-shot queries.
52 #define PRUNING_SORTER RadixSort
53 //#define PRUNING_SORTER InsertionSort
54 
55 // Static for coherence
60 {
63 }
65 {
68 }
70 {
73 }
75 {
79 }
80 
81 
83 
93 bool Opcode::BipartiteBoxPruning(udword nb0, const AABB** array0, udword nb1, const AABB** array1, Pairs& pairs, const Axes& axes)
95 {
96  // Checkings
97  if(!nb0 || !array0 || !nb1 || !array1) return false;
98 
99  // Catch axes
100  udword Axis0 = axes.mAxis0;
101  udword Axis1 = axes.mAxis1;
102  udword Axis2 = axes.mAxis2;
103 
104  // Allocate some temporary data
105  float* MinPosList0 = new float[nb0];
106  float* MinPosList1 = new float[nb1];
107 
108  // 1) Build main lists using the primary axis
109  for(udword i=0;i<nb0;i++) MinPosList0[i] = array0[i]->GetMin(Axis0);
110  for(udword i=0;i<nb1;i++) MinPosList1[i] = array1[i]->GetMin(Axis0);
111 
112  // 2) Sort the lists
115  const udword* Sorted0 = RS0->Sort(MinPosList0, nb0).GetRanks();
116  const udword* Sorted1 = RS1->Sort(MinPosList1, nb1).GetRanks();
117 
118  // 3) Prune the lists
119  udword Index0, Index1;
120 
121  const udword* const LastSorted0 = &Sorted0[nb0];
122  const udword* const LastSorted1 = &Sorted1[nb1];
123  const udword* RunningAddress0 = Sorted0;
124  const udword* RunningAddress1 = Sorted1;
125 
126  while(RunningAddress1<LastSorted1 && Sorted0<LastSorted0)
127  {
128  Index0 = *Sorted0++;
129 
130  while(RunningAddress1<LastSorted1 && MinPosList1[*RunningAddress1]<MinPosList0[Index0]) RunningAddress1++;
131 
132  const udword* RunningAddress2_1 = RunningAddress1;
133 
134  while(RunningAddress2_1<LastSorted1 && MinPosList1[Index1 = *RunningAddress2_1++]<=array0[Index0]->GetMax(Axis0))
135  {
136  if(array0[Index0]->Intersect(*array1[Index1], Axis1))
137  {
138  if(array0[Index0]->Intersect(*array1[Index1], Axis2))
139  {
140  pairs.AddPair(Index0, Index1);
141  }
142  }
143  }
144  }
145 
147 
148  while(RunningAddress0<LastSorted0 && Sorted1<LastSorted1)
149  {
150  Index0 = *Sorted1++;
151 
152  while(RunningAddress0<LastSorted0 && MinPosList0[*RunningAddress0]<=MinPosList1[Index0]) RunningAddress0++;
153 
154  const udword* RunningAddress2_0 = RunningAddress0;
155 
156  while(RunningAddress2_0<LastSorted0 && MinPosList0[Index1 = *RunningAddress2_0++]<=array1[Index0]->GetMax(Axis0))
157  {
158  if(array0[Index1]->Intersect(*array1[Index0], Axis1))
159  {
160  if(array0[Index1]->Intersect(*array1[Index0], Axis2))
161  {
162  pairs.AddPair(Index1, Index0);
163  }
164  }
165 
166  }
167  }
168 
169  DELETEARRAY(MinPosList1);
170  DELETEARRAY(MinPosList0);
171 
172  return true;
173 }
174 
175 #define ORIGINAL_VERSION
176 //#define JOAKIM
177 
179 
187 bool Opcode::CompleteBoxPruning(udword nb, const AABB** array, Pairs& pairs, const Axes& axes)
189 {
190  // Checkings
191  if(!nb || !array) return false;
192 
193  // Catch axes
194  udword Axis0 = axes.mAxis0;
195  udword Axis1 = axes.mAxis1;
196  udword Axis2 = axes.mAxis2;
197 
198 #ifdef ORIGINAL_VERSION
199  // Allocate some temporary data
200 // float* PosList = new float[nb];
201  float* PosList = new float[nb+1];
202 
203  // 1) Build main list using the primary axis
204  for(udword i=0;i<nb;i++) PosList[i] = array[i]->GetMin(Axis0);
205 PosList[nb++] = MAX_FLOAT;
206 
207  // 2) Sort the list
209  const udword* Sorted = RS->Sort(PosList, nb).GetRanks();
210 
211  // 3) Prune the list
212  const udword* const LastSorted = &Sorted[nb];
213  const udword* RunningAddress = Sorted;
214  udword Index0, Index1;
215  while(RunningAddress<LastSorted && Sorted<LastSorted)
216  {
217  Index0 = *Sorted++;
218 
219 // while(RunningAddress<LastSorted && PosList[*RunningAddress++]<PosList[Index0]);
220  while(PosList[*RunningAddress++]<PosList[Index0]);
221 
222  if(RunningAddress<LastSorted)
223  {
224  const udword* RunningAddress2 = RunningAddress;
225 
226 // while(RunningAddress2<LastSorted && PosList[Index1 = *RunningAddress2++]<=array[Index0]->GetMax(Axis0))
227  while(PosList[Index1 = *RunningAddress2++]<=array[Index0]->GetMax(Axis0))
228  {
229 // if(Index0!=Index1)
230 // {
231  if(array[Index0]->Intersect(*array[Index1], Axis1))
232  {
233  if(array[Index0]->Intersect(*array[Index1], Axis2))
234  {
235  pairs.AddPair(Index0, Index1);
236  }
237  }
238 // }
239  }
240  }
241  }
242 
243  DELETEARRAY(PosList);
244 #endif
245 
246 #ifdef JOAKIM
247  // Allocate some temporary data
248 // float* PosList = new float[nb];
249  float* MinList = new float[nb+1];
250 
251  // 1) Build main list using the primary axis
252  for(udword i=0;i<nb;i++) MinList[i] = array[i]->GetMin(Axis0);
253  MinList[nb] = MAX_FLOAT;
254 
255  // 2) Sort the list
257  udword* Sorted = RS->Sort(MinList, nb+1).GetRanks();
258 
259  // 3) Prune the list
260 // const udword* const LastSorted = &Sorted[nb];
261 // const udword* const LastSorted = &Sorted[nb-1];
262  const udword* RunningAddress = Sorted;
263  udword Index0, Index1;
264 
265 // while(RunningAddress<LastSorted && Sorted<LastSorted)
266 // while(RunningAddress<LastSorted)
267  while(RunningAddress<&Sorted[nb])
268 // while(Sorted<LastSorted)
269  {
270 // Index0 = *Sorted++;
271  Index0 = *RunningAddress++;
272 
273 // while(RunningAddress<LastSorted && PosList[*RunningAddress++]<PosList[Index0]);
274 // while(PosList[*RunningAddress++]<PosList[Index0]);
275 //RunningAddress = Sorted;
276 // if(RunningAddress<LastSorted)
277  {
278  const udword* RunningAddress2 = RunningAddress;
279 
280 // while(RunningAddress2<LastSorted && PosList[Index1 = *RunningAddress2++]<=array[Index0]->GetMax(Axis0))
281 
282 // float CurrentMin = array[Index0]->GetMin(Axis0);
283  float CurrentMax = array[Index0]->GetMax(Axis0);
284 
285  while(MinList[Index1 = *RunningAddress2] <= CurrentMax)
286 // while(PosList[Index1 = *RunningAddress] <= CurrentMax)
287  {
288 // if(Index0!=Index1)
289 // {
290  if(array[Index0]->Intersect(*array[Index1], Axis1))
291  {
292  if(array[Index0]->Intersect(*array[Index1], Axis2))
293  {
294  pairs.AddPair(Index0, Index1);
295  }
296  }
297 // }
298 
299  RunningAddress2++;
300 // RunningAddress++;
301  }
302  }
303  }
304 
305  DELETEARRAY(MinList);
306 #endif
307 
308  return true;
309 }
310 
312 // Brute-force versions are kept:
313 // - to check the optimized versions return the correct list of intersections
314 // - to check the speed of the optimized code against the brute-force one
316 
318 
327 bool Opcode::BruteForceBipartiteBoxTest(udword nb0, const AABB** array0, udword nb1, const AABB** array1, Pairs& pairs)
329 {
330  // Checkings
331  if(!nb0 || !array0 || !nb1 || !array1) return false;
332 
333  // Brute-force nb0*nb1 overlap tests
334  for(udword i=0;i<nb0;i++)
335  {
336  for(udword j=0;j<nb1;j++)
337  {
338  if(array0[i]->Intersect(*array1[j])) pairs.AddPair(i, j);
339  }
340  }
341  return true;
342 }
343 
345 
352 bool Opcode::BruteForceCompleteBoxTest(udword nb, const AABB** array, Pairs& pairs)
354 {
355  // Checkings
356  if(!nb || !array) return false;
357 
358  // Brute-force n(n-1)/2 overlap tests
359  for(udword i=0;i<nb;i++)
360  {
361  for(udword j=i+1;j<nb;j++)
362  {
363  if(array[i]->Intersect(*array[j])) pairs.AddPair(i, j);
364  }
365  }
366  return true;
367 }
PRUNING_SORTER
#define PRUNING_SORTER
Definition: OPC_BoxPruning.cpp:52
i
png_uint_32 i
Definition: png.h:2732
gBipartitePruningSorter0
static PRUNING_SORTER * gBipartitePruningSorter0
Definition: OPC_BoxPruning.cpp:57
gBipartitePruningSorter1
static PRUNING_SORTER * gBipartitePruningSorter1
Definition: OPC_BoxPruning.cpp:58
DELETESINGLE
#define DELETESINGLE(x)
Deletes an instance of a class.
Definition: IceMemoryMacros.h:95
udword
unsigned int udword
sizeof(udword) must be 4
Definition: IceTypes.h:65
gCompletePruningSorter
static PRUNING_SORTER * gCompletePruningSorter
Definition: OPC_BoxPruning.cpp:56
hrp::max
static int max(int a, int b)
Definition: MatrixSolvers.cpp:54
CompleteBoxPruning
FUNCTION OPCODE_API bool CompleteBoxPruning(udword nb, const AABB **array, Pairs &pairs, const Axes &axes)
GetBipartitePruningSorter0
inline_ PRUNING_SORTER * GetBipartitePruningSorter0()
Definition: OPC_BoxPruning.cpp:64
Stdafx.h
FindRunningIndex
inline_ void FindRunningIndex(udword &index, float *array, udword *sorted, int last, float max)
Definition: OPC_BoxPruning.cpp:37
Pairs::AddPair
inline_ void AddPair(const Pair &p)
Definition: IcePairs.h:41
Axes::mAxis2
udword mAxis2
Definition: IceAxes.h:51
BruteForceBipartiteBoxTest
FUNCTION OPCODE_API bool BruteForceBipartiteBoxTest(udword nb0, const AABB **array0, udword nb1, const AABB **array1, Pairs &pairs)
Axes::mAxis1
udword mAxis1
Definition: IceAxes.h:50
Axes
Definition: IceAxes.h:37
BruteForceCompleteBoxTest
FUNCTION OPCODE_API bool BruteForceCompleteBoxTest(udword nb, const AABB **array, Pairs &pairs)
Intersect
inline_ BOOL Intersect(const AABB &a, const SAP_Box &b)
Definition: OPC_SweepAndPrune.cpp:522
ReleasePruningSorters
void ReleasePruningSorters()
Definition: OPC_BoxPruning.cpp:74
GetBipartitePruningSorter1
inline_ PRUNING_SORTER * GetBipartitePruningSorter1()
Definition: OPC_BoxPruning.cpp:69
BipartiteBoxPruning
FUNCTION OPCODE_API bool BipartiteBoxPruning(udword nb0, const AABB **array0, udword nb1, const AABB **array1, Pairs &pairs, const Axes &axes)
GetCompletePruningSorter
inline_ PRUNING_SORTER * GetCompletePruningSorter()
Definition: OPC_BoxPruning.cpp:59
MAX_FLOAT
#define MAX_FLOAT
max possible float value
Definition: IceTypes.h:130
Axes::mAxis0
udword mAxis0
Definition: IceAxes.h:49
Pairs
Definition: IcePairs.h:25
DELETEARRAY
#define DELETEARRAY(x)
Deletes an array.
Definition: IceMemoryMacros.h:96
AABB::GetMax
inline_ void GetMax(Point &max) const
Get max point of the box.
Definition: IceAABB.h:345
inline_
#define inline_
Definition: IcePreprocessor.h:103
AABB
Definition: IceAABB.h:267
Opcode
Definition: CollisionPairInserterBase.h:18


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:03