aboutsummaryrefslogtreecommitdiffstatshomepage
path: root/libraries/ode-0.9/GIMPACT/src/gim_boxpruning.cpp
diff options
context:
space:
mode:
Diffstat (limited to 'libraries/ode-0.9/GIMPACT/src/gim_boxpruning.cpp')
-rw-r--r--libraries/ode-0.9/GIMPACT/src/gim_boxpruning.cpp514
1 files changed, 514 insertions, 0 deletions
diff --git a/libraries/ode-0.9/GIMPACT/src/gim_boxpruning.cpp b/libraries/ode-0.9/GIMPACT/src/gim_boxpruning.cpp
new file mode 100644
index 0000000..91e1d37
--- /dev/null
+++ b/libraries/ode-0.9/GIMPACT/src/gim_boxpruning.cpp
@@ -0,0 +1,514 @@
1
2/*
3-----------------------------------------------------------------------------
4This source file is part of GIMPACT Library.
5
6For the latest info, see http://gimpact.sourceforge.net/
7
8Copyright (c) 2006 Francisco Leon. C.C. 80087371.
9email: projectileman@yahoo.com
10
11 This library is free software; you can redistribute it and/or
12 modify it under the terms of EITHER:
13 (1) The GNU Lesser General Public License as published by the Free
14 Software Foundation; either version 2.1 of the License, or (at
15 your option) any later version. The text of the GNU Lesser
16 General Public License is included with this library in the
17 file GIMPACT-LICENSE-LGPL.TXT.
18 (2) The BSD-style license that is included with this library in
19 the file GIMPACT-LICENSE-BSD.TXT.
20
21 This library is distributed in the hope that it will be useful,
22 but WITHOUT ANY WARRANTY; without even the implied warranty of
23 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files
24 GIMPACT-LICENSE-LGPL.TXT and GIMPACT-LICENSE-BSD.TXT for more details.
25
26-----------------------------------------------------------------------------
27*/
28
29
30#include "GIMPACT/gim_boxpruning.h"
31
32
33
34//! Allocate memory for all aabb set.
35void gim_aabbset_alloc(GIM_AABB_SET * aabbset, GUINT count)
36{
37 aabbset->m_count = count;
38 aabbset->m_boxes = (aabb3f *)gim_alloc(sizeof(aabb3f)*count);
39
40 if(count<GIM_MIN_SORTED_BIPARTITE_PRUNING_BOXES)
41 {
42 aabbset->m_maxcoords = 0;
43 aabbset->m_sorted_mincoords = 0;
44 }
45 else
46 {
47 aabbset->m_maxcoords = (GUINT *)gim_alloc(sizeof(GUINT)*aabbset->m_count );
48 aabbset->m_sorted_mincoords = (GIM_RSORT_TOKEN *)gim_alloc(sizeof(GIM_RSORT_TOKEN)*aabbset->m_count);
49 }
50 aabbset->m_shared = 0;
51 INVALIDATE_AABB(aabbset->m_global_bound);
52}
53
54//! Destroys the aabb set.
55void gim_aabbset_destroy(GIM_AABB_SET * aabbset)
56{
57 aabbset->m_count = 0;
58 if(aabbset->m_shared==0)
59 {
60 gim_free(aabbset->m_boxes,0);
61 gim_free(aabbset->m_maxcoords,0);
62 gim_free(aabbset->m_sorted_mincoords,0);
63 }
64 aabbset->m_boxes = 0;
65 aabbset->m_sorted_mincoords = 0;
66 aabbset->m_maxcoords = 0;
67}
68
69void gim_aabbset_calc_global_bound(GIM_AABB_SET * aabbset)
70{
71 aabb3f * paabb = aabbset->m_boxes;
72 aabb3f * globalbox = &aabbset->m_global_bound;
73 AABB_COPY((*globalbox),(*paabb));
74
75 GUINT count = aabbset->m_count-1;
76 paabb++;
77 while(count)
78 {
79 MERGEBOXES(*globalbox,*paabb)
80 paabb++;
81 count--;
82 }
83}
84
85
86//! Sorts the boxes for box prunning.
87/*!
881) find the integer representation of the aabb coords
892) Sorts the min coords
903) Calcs the global bound
91\pre aabbset must be allocated. And the boxes must be already set.
92\param aabbset
93\param calc_global_bound If 1 , calcs the global bound
94\post If aabbset->m_sorted_mincoords == 0, then it allocs the sorted coordinates
95*/
96void gim_aabbset_sort(GIM_AABB_SET * aabbset, char calc_global_bound)
97{
98 if(aabbset->m_sorted_mincoords == 0)
99 {//allocate
100 aabbset->m_maxcoords = (GUINT *)gim_alloc(sizeof(GUINT)*aabbset->m_count );
101 aabbset->m_sorted_mincoords = (GIM_RSORT_TOKEN *)gim_alloc(sizeof(GIM_RSORT_TOKEN)*aabbset->m_count);
102 }
103
104 GUINT i, count = aabbset->m_count;
105 aabb3f * paabb = aabbset->m_boxes;
106 GUINT * maxcoords = aabbset->m_maxcoords;
107 GIM_RSORT_TOKEN * sorted_tokens = aabbset->m_sorted_mincoords;
108
109 if(count<860)//Calibrated on a Pentium IV
110 {
111 //Sort by quick sort
112 //Calculate keys
113 for(i=0;i<count;i++)
114 {
115 GIM_CONVERT_VEC3F_GUINT_XZ_UPPER(paabb[i].maxX,paabb[i].maxZ,maxcoords[i]);
116 GIM_CONVERT_VEC3F_GUINT_XZ(paabb[i].minX,paabb[i].minZ,sorted_tokens[i].m_key);
117 sorted_tokens[i].m_value = i;
118 }
119 GIM_QUICK_SORT_ARRAY(GIM_RSORT_TOKEN , sorted_tokens, count, RSORT_TOKEN_COMPARATOR,GIM_DEF_EXCHANGE_MACRO);
120 }
121 else
122 {
123 //Sort by radix sort
124 GIM_RSORT_TOKEN * unsorted = (GIM_RSORT_TOKEN *)gim_alloc(sizeof(GIM_RSORT_TOKEN )*count);
125 //Calculate keys
126 for(i=0;i<count;i++)
127 {
128 GIM_CONVERT_VEC3F_GUINT_XZ_UPPER(paabb[i].maxX,paabb[i].maxZ,maxcoords[i]);
129 GIM_CONVERT_VEC3F_GUINT_XZ(paabb[i].minX,paabb[i].minZ,unsorted[i].m_key);
130 unsorted[i].m_value = i;
131 }
132 GIM_RADIX_SORT_RTOKENS(unsorted,sorted_tokens,count);
133 gim_free(unsorted,0);
134 }
135
136 if(calc_global_bound) gim_aabbset_calc_global_bound(aabbset);
137}
138
139//utility macros
140
141/*#define PUSH_PAIR(i,j,pairset)\
142{\
143 GIM_PAIR _pair={i,j};\
144 GIM_DYNARRAY_PUSH_ITEM(GIM_PAIR,pairset,_pair);\
145}*/
146
147#define PUSH_PAIR(i,j,pairset)\
148{\
149 GIM_DYNARRAY_PUSH_EMPTY(GIM_PAIR,pairset);\
150 GIM_PAIR * _pair = GIM_DYNARRAY_POINTER(GIM_PAIR,pairset) + (pairset).m_size - 1;\
151 _pair->m_index1 = i;\
152 _pair->m_index2 = j;\
153}
154
155#define PUSH_PAIR_INV(i,j,pairset)\
156{\
157 GIM_DYNARRAY_PUSH_EMPTY(GIM_PAIR,pairset);\
158 GIM_PAIR * _pair = GIM_DYNARRAY_POINTER(GIM_PAIR,pairset) + (pairset).m_size - 1;\
159 _pair->m_index1 = j;\
160 _pair->m_index2 = i;\
161}
162
163#define FIND_OVERLAPPING_FOWARD(\
164 curr_index,\
165 test_count,\
166 test_aabb,\
167 max_coord_uint,\
168 sorted_tokens,\
169 aabbarray,\
170 pairset,\
171 push_pair_macro)\
172{\
173 GUINT _i = test_count;\
174 char _intersected;\
175 GIM_RSORT_TOKEN * _psorted_tokens = sorted_tokens;\
176 while(max_coord_uint >= _psorted_tokens->m_key && _i>0)\
177 {\
178 AABBCOLLISION(_intersected,test_aabb,aabbarray[_psorted_tokens->m_value]);\
179 if(_intersected)\
180 {\
181 push_pair_macro(curr_index, _psorted_tokens->m_value,pairset);\
182 }\
183 _psorted_tokens++;\
184 _i--;\
185 }\
186}
187
188//! log(N) Complete box pruning. Returns a list of overlapping pairs of boxes, each box of the pair belongs to the same set.
189/*!
190\pre aabbset must be allocated and sorted, the boxes must be already set.
191\param aabbset Must be sorted. Global bound isn't required
192\param collision_pairs Array of GIM_PAIR elements. Must be initialized before (Reserve size ~ 100)
193*/
194void gim_aabbset_self_intersections_sorted(GIM_AABB_SET * aabbset, GDYNAMIC_ARRAY * collision_pairs)
195{
196 collision_pairs->m_size = 0;
197 GUINT count = aabbset->m_count;
198 aabb3f * paabb = aabbset->m_boxes;
199 GUINT * maxcoords = aabbset->m_maxcoords;
200 GIM_RSORT_TOKEN * sorted_tokens = aabbset->m_sorted_mincoords;
201 aabb3f test_aabb;
202 while(count>1)
203 {
204 ///current cache variables
205 GUINT curr_index = sorted_tokens->m_value;
206 GUINT max_coord_uint = maxcoords[curr_index];
207 AABB_COPY(test_aabb,paabb[curr_index]);
208
209 ///next pairs
210 sorted_tokens++;
211 count--;
212 FIND_OVERLAPPING_FOWARD( curr_index, count, test_aabb, max_coord_uint, sorted_tokens , paabb, (*collision_pairs),PUSH_PAIR);
213 }
214}
215
216//! NxN Complete box pruning. Returns a list of overlapping pairs of boxes, each box of the pair belongs to the same set.
217/*!
218\pre aabbset must be allocated, the boxes must be already set.
219\param aabbset Global bound isn't required. Doen't need to be sorted.
220\param collision_pairs Array of GIM_PAIR elements. Must be initialized before (Reserve size ~ 100)
221*/
222void gim_aabbset_self_intersections_brute_force(GIM_AABB_SET * aabbset, GDYNAMIC_ARRAY * collision_pairs)
223{
224 collision_pairs->m_size = 0;
225 GUINT i,j;
226 GUINT count = aabbset->m_count;
227 aabb3f * paabb = aabbset->m_boxes;
228 char intersected;
229 for (i=0;i< count-1 ;i++ )
230 {
231 for (j=i+1;j<count ;j++ )
232 {
233 AABBCOLLISION(intersected,paabb[i],paabb[j]);
234 if(intersected)
235 {
236 PUSH_PAIR(i,j,(*collision_pairs));
237 }
238 }
239 }
240}
241
242//! log(N) Bipartite box pruning. Returns a list of overlapping pairs of boxes, each box of the pair belongs to a different set.
243/*!
244\pre aabbset1 and aabbset2 must be allocated and sorted, the boxes must be already set.
245\param aabbset1 Must be sorted, Global bound is required.
246\param aabbset2 Must be sorted, Global bound is required.
247\param collision_pairs Array of GIM_PAIR elements. Must be initialized before (Reserve size ~ 100)
248*/
249void gim_aabbset_bipartite_intersections_sorted(GIM_AABB_SET * aabbset1, GIM_AABB_SET * aabbset2, GDYNAMIC_ARRAY * collision_pairs)
250{
251 char intersected;
252 collision_pairs->m_size = 0;
253
254 AABBCOLLISION(intersected,aabbset1->m_global_bound,aabbset2->m_global_bound);
255 if(intersected == 0) return;
256
257 GUINT count1 = aabbset1->m_count;
258 aabb3f * paabb1 = aabbset1->m_boxes;
259 GUINT * maxcoords1 = aabbset1->m_maxcoords;
260 GIM_RSORT_TOKEN * sorted_tokens1 = aabbset1->m_sorted_mincoords;
261
262 GUINT count2 = aabbset2->m_count;
263 aabb3f * paabb2 = aabbset2->m_boxes;
264 GUINT * maxcoords2 = aabbset2->m_maxcoords;
265 GIM_RSORT_TOKEN * sorted_tokens2 = aabbset2->m_sorted_mincoords;
266
267 GUINT curr_index;
268
269 GUINT max_coord_uint;
270 aabb3f test_aabb;
271
272 //Classify boxes
273 //Find Set intersection
274 aabb3f int_abbb;
275 BOXINTERSECTION(aabbset1->m_global_bound,aabbset2->m_global_bound, int_abbb);
276
277 //Clasify set 1
278 GIM_RSORT_TOKEN * classified_tokens1 = (GIM_RSORT_TOKEN *) gim_alloc(sizeof(GIM_RSORT_TOKEN)*count1);
279 GUINT i,classified_count1 = 0,classified_count2 = 0;
280
281
282 for (i=0;i<count1;i++ )
283 {
284 curr_index = sorted_tokens1[i].m_value;
285 AABBCOLLISION(intersected,paabb1[curr_index],int_abbb);
286 if(intersected)
287 {
288 classified_tokens1[classified_count1] = sorted_tokens1[i];
289 classified_count1++;
290 }
291 }
292
293 if(classified_count1==0)
294 {
295 gim_free(classified_tokens1 ,0);
296 return; // no pairs
297 }
298
299 //Clasify set 2
300 GIM_RSORT_TOKEN * classified_tokens2 = (GIM_RSORT_TOKEN *) gim_alloc(sizeof(GIM_RSORT_TOKEN)*count2);
301
302 for (i=0;i<count2;i++ )
303 {
304 curr_index = sorted_tokens2[i].m_value;
305 AABBCOLLISION(intersected,paabb2[curr_index],int_abbb);
306 if(intersected)
307 {
308 classified_tokens2[classified_count2] = sorted_tokens2[i];
309 classified_count2++;
310 }
311 }
312
313 if(classified_count2==0)
314 {
315 gim_free(classified_tokens1 ,0);
316 gim_free(classified_tokens2 ,0);
317 return; // no pairs
318 }
319
320 sorted_tokens1 = classified_tokens1;
321 sorted_tokens2 = classified_tokens2;
322
323 while(classified_count1>0&&classified_count2>0)
324 {
325 if(sorted_tokens1->m_key <= sorted_tokens2->m_key)
326 {
327 ///current cache variables
328 curr_index = sorted_tokens1->m_value;
329 max_coord_uint = maxcoords1[curr_index];
330 AABB_COPY(test_aabb,paabb1[curr_index]);
331 ///next pairs
332 sorted_tokens1++;
333 classified_count1--;
334 FIND_OVERLAPPING_FOWARD( curr_index, classified_count2, test_aabb, max_coord_uint, sorted_tokens2 , paabb2, (*collision_pairs), PUSH_PAIR);
335 }
336 else ///Switch test
337 {
338 ///current cache variables
339 curr_index = sorted_tokens2->m_value;
340 max_coord_uint = maxcoords2[curr_index];
341 AABB_COPY(test_aabb,paabb2[curr_index]);
342 ///next pairs
343 sorted_tokens2++;
344 classified_count2--;
345 FIND_OVERLAPPING_FOWARD( curr_index, classified_count1, test_aabb, max_coord_uint, sorted_tokens1 , paabb1, (*collision_pairs), PUSH_PAIR_INV );
346 }
347 }
348 gim_free(classified_tokens1 ,0);
349 gim_free(classified_tokens2 ,0);
350}
351
352//! NxM Bipartite box pruning. Returns a list of overlapping pairs of boxes, each box of the pair belongs to a different set.
353/*!
354\pre aabbset1 and aabbset2 must be allocated and sorted, the boxes must be already set.
355\param aabbset1 Must be sorted, Global bound is required.
356\param aabbset2 Must be sorted, Global bound is required.
357\param collision_pairs Array of GIM_PAIR elements. Must be initialized before (Reserve size ~ 100)
358*/
359void gim_aabbset_bipartite_intersections_brute_force(GIM_AABB_SET * aabbset1,GIM_AABB_SET * aabbset2, GDYNAMIC_ARRAY * collision_pairs)
360{
361 char intersected;
362 collision_pairs->m_size = 0;
363 AABBCOLLISION(intersected,aabbset1->m_global_bound,aabbset2->m_global_bound);
364 if(intersected == 0) return;
365
366 aabb3f int_abbb;
367 //Find Set intersection
368 BOXINTERSECTION(aabbset1->m_global_bound,aabbset2->m_global_bound, int_abbb);
369 //Clasify set 1
370 GUINT i,j;
371 GUINT classified_count = 0;
372
373 GUINT count = aabbset1->m_count;
374 aabb3f * paabb1 = aabbset1->m_boxes;
375 aabb3f * paabb2 = aabbset2->m_boxes;
376
377 GUINT * classified = (GUINT *) gim_alloc(sizeof(GUINT)*count);
378
379 for (i=0;i<count;i++ )
380 {
381 AABBCOLLISION(intersected,paabb1[i],int_abbb);
382 if(intersected)
383 {
384 classified[classified_count] = i;
385 classified_count++;
386 }
387 }
388
389 if(classified_count==0) return; // no pairs
390
391 //intesect set2
392 count = aabbset2->m_count;
393 for (i=0;i<count;i++)
394 {
395 AABBCOLLISION(intersected,paabb2[i],int_abbb);
396 if(intersected)
397 {
398 for (j=0;j<classified_count;j++)
399 {
400 AABBCOLLISION(intersected,paabb2[i],paabb1[classified[j]]);
401 if(intersected)
402 {
403 PUSH_PAIR(classified[j],i,(*collision_pairs));
404 }
405 }
406 }
407 }
408 gim_free(classified,0);
409}
410
411
412//! Initalizes the set. Sort Boxes if needed.
413/*!
414\pre aabbset must be allocated. And the boxes must be already set.
415\post If the set has less of GIM_MIN_SORTED_BIPARTITE_PRUNING_BOXES boxes, only calcs the global box,
416 else it Sorts the entire set( Only applicable for large sets)
417*/
418void gim_aabbset_update(GIM_AABB_SET * aabbset)
419{
420 if(aabbset->m_count < GIM_MIN_SORTED_BIPARTITE_PRUNING_BOXES)
421 {//Brute force approach
422 gim_aabbset_calc_global_bound(aabbset);
423 }
424 else
425 {//Sorted force approach
426 gim_aabbset_sort(aabbset,1);
427 }
428}
429
430//! Complete box pruning. Returns a list of overlapping pairs of boxes, each box of the pair belongs to the same set.
431/*!
432This function sorts the set and then it calls to gim_aabbset_self_intersections_brute_force or gim_aabbset_self_intersections_sorted.
433
434\param aabbset Set of boxes. Sorting isn't required.
435\param collision_pairs Array of GIM_PAIR elements. Must be initialized before (Reserve size ~ 100)
436\pre aabbset must be allocated and initialized.
437\post If aabbset->m_count >= GIM_MIN_SORTED_PRUNING_BOXES, then it calls to gim_aabbset_sort and then to gim_aabbset_self_intersections_sorted.
438*/
439void gim_aabbset_self_intersections(GIM_AABB_SET * aabbset, GDYNAMIC_ARRAY * collision_pairs)
440{
441 if(aabbset->m_count < GIM_MIN_SORTED_PRUNING_BOXES)
442 {//Brute force approach
443 gim_aabbset_self_intersections_brute_force(aabbset,collision_pairs);
444 }
445 else
446 {//Sorted force approach
447 gim_aabbset_sort(aabbset,0);
448 gim_aabbset_self_intersections_sorted(aabbset,collision_pairs);
449 }
450}
451
452//! Collides two sets. Returns a list of overlapping pairs of boxes, each box of the pair belongs to a different set.
453/*!
454\pre aabbset1 and aabbset2 must be allocated and updated. See .
455\param aabbset1 Must be sorted, Global bound is required.
456\param aabbset2 Must be sorted, Global bound is required.
457\param collision_pairs Array of GIM_PAIR elements. Must be initialized before (Reserve size ~ 100)
458*/
459void gim_aabbset_bipartite_intersections(GIM_AABB_SET * aabbset1, GIM_AABB_SET * aabbset2, GDYNAMIC_ARRAY * collision_pairs)
460{
461 if(aabbset1->m_sorted_mincoords == 0||aabbset2->m_sorted_mincoords == 0)
462 {//Brute force approach
463 gim_aabbset_bipartite_intersections_brute_force(aabbset1,aabbset2,collision_pairs);
464 }
465 else
466 {//Sorted force approach
467 gim_aabbset_bipartite_intersections_sorted(aabbset1,aabbset2,collision_pairs);
468 }
469}
470
471void gim_aabbset_box_collision(aabb3f *test_aabb, GIM_AABB_SET * aabbset, GDYNAMIC_ARRAY * collided)
472{
473 collided->m_size = 0;
474 char intersected;
475 AABBCOLLISION(intersected,aabbset->m_global_bound,(*test_aabb));
476 if(intersected == 0) return;
477
478 GUINT i;
479 GUINT count = aabbset->m_count;
480 aabb3f * paabb = aabbset->m_boxes;
481 aabb3f _testaabb;
482 AABB_COPY(_testaabb,*test_aabb);
483
484 for (i=0;i< count;i++ )
485 {
486 AABBCOLLISION(intersected,paabb[i],_testaabb);
487 if(intersected)
488 {
489 GIM_DYNARRAY_PUSH_ITEM(GUINT,(*collided),i);
490 }
491 }
492}
493
494void gim_aabbset_ray_collision(vec3f vorigin,vec3f vdir, GREAL tmax, GIM_AABB_SET * aabbset, GDYNAMIC_ARRAY * collided)
495{
496 collided->m_size = 0;
497 char intersected;
498 GREAL tparam = 0;
499 BOX_INTERSECTS_RAY(aabbset->m_global_bound, vorigin, vdir, tparam, tmax,intersected);
500 if(intersected==0) return;
501
502 GUINT i;
503 GUINT count = aabbset->m_count;
504 aabb3f * paabb = aabbset->m_boxes;
505
506 for (i=0;i< count;i++ )
507 {
508 BOX_INTERSECTS_RAY(paabb[i], vorigin, vdir, tparam, tmax,intersected);
509 if(intersected)
510 {
511 GIM_DYNARRAY_PUSH_ITEM(GUINT,(*collided),i);
512 }
513 }
514}
generated by cgit v1.1 at 2024-12-28 08:37:35 +0000