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 code for box pruning.
12 * \file IceBoxPruning.cpp
13 * \author Pierre Terdiman
14 * \date January, 29, 2000
15 */
16 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
17
18 /*
19 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
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.
27 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
28 */
29
30
31 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
32 // Precompiled Header
33 #include "Stdafx.h"
34
35 using namespace Opcode;
36
FindRunningIndex(udword & index,float * array,udword * sorted,int last,float max)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
56 static PRUNING_SORTER* gCompletePruningSorter = null;
57 static PRUNING_SORTER* gBipartitePruningSorter0 = null;
58 static PRUNING_SORTER* gBipartitePruningSorter1 = null;
GetCompletePruningSorter()59 inline_ PRUNING_SORTER* GetCompletePruningSorter()
60 {
61 if(!gCompletePruningSorter) gCompletePruningSorter = new PRUNING_SORTER;
62 return gCompletePruningSorter;
63 }
GetBipartitePruningSorter0()64 inline_ PRUNING_SORTER* GetBipartitePruningSorter0()
65 {
66 if(!gBipartitePruningSorter0) gBipartitePruningSorter0 = new PRUNING_SORTER;
67 return gBipartitePruningSorter0;
68 }
GetBipartitePruningSorter1()69 inline_ PRUNING_SORTER* GetBipartitePruningSorter1()
70 {
71 if(!gBipartitePruningSorter1) gBipartitePruningSorter1 = new PRUNING_SORTER;
72 return gBipartitePruningSorter1;
73 }
ReleasePruningSorters()74 void ReleasePruningSorters()
75 {
76 DELETESINGLE(gBipartitePruningSorter1);
77 DELETESINGLE(gBipartitePruningSorter0);
78 DELETESINGLE(gCompletePruningSorter);
79 }
80
81
82 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
83 /**
84 * Bipartite box pruning. Returns a list of overlapping pairs of boxes, each box of the pair belongs to a different set.
85 * \param nb0 [in] number of boxes in the first set
86 * \param array0 [in] array of boxes for the first set
87 * \param nb1 [in] number of boxes in the second set
88 * \param array1 [in] array of boxes for the second set
89 * \param pairs [out] array of overlapping pairs
90 * \param axes [in] projection order (0,2,1 is often best)
91 * \return true if success.
92 */
93 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
BipartiteBoxPruning(udword nb0,const AABB ** array0,udword nb1,const AABB ** array1,Pairs & pairs,const Axes & axes)94 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
113 PRUNING_SORTER* RS0 = GetBipartitePruningSorter0();
114 PRUNING_SORTER* RS1 = GetBipartitePruningSorter1();
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
146 ////
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
178 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
179 /**
180 * Complete box pruning. Returns a list of overlapping pairs of boxes, each box of the pair belongs to the same set.
181 * \param nb [in] number of boxes
182 * \param array [in] array of boxes
183 * \param pairs [out] array of overlapping pairs
184 * \param axes [in] projection order (0,2,1 is often best)
185 * \return true if success.
186 */
187 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
CompleteBoxPruning(udword nb,const AABB ** array,Pairs & pairs,const Axes & axes)188 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
208 PRUNING_SORTER* RS = GetCompletePruningSorter();
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
256 PRUNING_SORTER* RS = GetCompletePruningSorter();
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
311 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
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
315 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
316
317 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
318 /**
319 * Brute-force bipartite box pruning. Returns a list of overlapping pairs of boxes, each box of the pair belongs to a different set.
320 * \param nb0 [in] number of boxes in the first set
321 * \param array0 [in] array of boxes for the first set
322 * \param nb1 [in] number of boxes in the second set
323 * \param array1 [in] array of boxes for the second set
324 * \param pairs [out] array of overlapping pairs
325 * \return true if success.
326 */
327 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
BruteForceBipartiteBoxTest(udword nb0,const AABB ** array0,udword nb1,const AABB ** array1,Pairs & pairs)328 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
344 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
345 /**
346 * Complete box pruning. Returns a list of overlapping pairs of boxes, each box of the pair belongs to the same set.
347 * \param nb [in] number of boxes
348 * \param array [in] array of boxes
349 * \param pairs [out] array of overlapping pairs
350 * \return true if success.
351 */
352 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
BruteForceCompleteBoxTest(udword nb,const AABB ** array,Pairs & pairs)353 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 }
368