aboutsummaryrefslogtreecommitdiffstatshomepage
path: root/libraries/ode-0.9/OPCODE/OPC_BoxPruning.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_BoxPruning.cpp
parentadding ode source to /libraries (diff)
downloadopensim-SC_OLD-79eca25c945a535a7a0325999034bae17da92412.zip
opensim-SC_OLD-79eca25c945a535a7a0325999034bae17da92412.tar.gz
opensim-SC_OLD-79eca25c945a535a7a0325999034bae17da92412.tar.bz2
opensim-SC_OLD-79eca25c945a535a7a0325999034bae17da92412.tar.xz
resubmitting ode
Diffstat (limited to 'libraries/ode-0.9/OPCODE/OPC_BoxPruning.cpp')
-rw-r--r--libraries/ode-0.9/OPCODE/OPC_BoxPruning.cpp367
1 files changed, 367 insertions, 0 deletions
diff --git a/libraries/ode-0.9/OPCODE/OPC_BoxPruning.cpp b/libraries/ode-0.9/OPCODE/OPC_BoxPruning.cpp
new file mode 100644
index 0000000..6906160
--- /dev/null
+++ b/libraries/ode-0.9/OPCODE/OPC_BoxPruning.cpp
@@ -0,0 +1,367 @@
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
35using 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
56static PRUNING_SORTER* gCompletePruningSorter = null;
57static PRUNING_SORTER* gBipartitePruningSorter0 = null;
58static PRUNING_SORTER* gBipartitePruningSorter1 = null;
59inline_ PRUNING_SORTER* GetCompletePruningSorter()
60{
61 if(!gCompletePruningSorter) gCompletePruningSorter = new PRUNING_SORTER;
62 return gCompletePruningSorter;
63}
64inline_ PRUNING_SORTER* GetBipartitePruningSorter0()
65{
66 if(!gBipartitePruningSorter0) gBipartitePruningSorter0 = new PRUNING_SORTER;
67 return gBipartitePruningSorter0;
68}
69inline_ PRUNING_SORTER* GetBipartitePruningSorter1()
70{
71 if(!gBipartitePruningSorter1) gBipartitePruningSorter1 = new PRUNING_SORTER;
72 return gBipartitePruningSorter1;
73}
74void 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///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
94bool 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///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
188bool 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);
205PosList[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///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
328bool 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///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
353bool 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}