515 lines
17 KiB
C++
515 lines
17 KiB
C++
|
|
/*
|
|
-----------------------------------------------------------------------------
|
|
This source file is part of GIMPACT Library.
|
|
|
|
For the latest info, see http://gimpact.sourceforge.net/
|
|
|
|
Copyright (c) 2006 Francisco Leon. C.C. 80087371.
|
|
email: projectileman@yahoo.com
|
|
|
|
This library is free software; you can redistribute it and/or
|
|
modify it under the terms of EITHER:
|
|
(1) The GNU Lesser General Public License as published by the Free
|
|
Software Foundation; either version 2.1 of the License, or (at
|
|
your option) any later version. The text of the GNU Lesser
|
|
General Public License is included with this library in the
|
|
file GIMPACT-LICENSE-LGPL.TXT.
|
|
(2) The BSD-style license that is included with this library in
|
|
the file GIMPACT-LICENSE-BSD.TXT.
|
|
|
|
This library is distributed in the hope that it will be useful,
|
|
but WITHOUT ANY WARRANTY; without even the implied warranty of
|
|
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files
|
|
GIMPACT-LICENSE-LGPL.TXT and GIMPACT-LICENSE-BSD.TXT for more details.
|
|
|
|
-----------------------------------------------------------------------------
|
|
*/
|
|
|
|
|
|
#include "GIMPACT/gim_boxpruning.h"
|
|
|
|
|
|
|
|
//! Allocate memory for all aabb set.
|
|
void gim_aabbset_alloc(GIM_AABB_SET * aabbset, GUINT count)
|
|
{
|
|
aabbset->m_count = count;
|
|
aabbset->m_boxes = (aabb3f *)gim_alloc(sizeof(aabb3f)*count);
|
|
|
|
if(count<GIM_MIN_SORTED_BIPARTITE_PRUNING_BOXES)
|
|
{
|
|
aabbset->m_maxcoords = 0;
|
|
aabbset->m_sorted_mincoords = 0;
|
|
}
|
|
else
|
|
{
|
|
aabbset->m_maxcoords = (GUINT *)gim_alloc(sizeof(GUINT)*aabbset->m_count );
|
|
aabbset->m_sorted_mincoords = (GIM_RSORT_TOKEN *)gim_alloc(sizeof(GIM_RSORT_TOKEN)*aabbset->m_count);
|
|
}
|
|
aabbset->m_shared = 0;
|
|
INVALIDATE_AABB(aabbset->m_global_bound);
|
|
}
|
|
|
|
//! Destroys the aabb set.
|
|
void gim_aabbset_destroy(GIM_AABB_SET * aabbset)
|
|
{
|
|
aabbset->m_count = 0;
|
|
if(aabbset->m_shared==0)
|
|
{
|
|
gim_free(aabbset->m_boxes,0);
|
|
gim_free(aabbset->m_maxcoords,0);
|
|
gim_free(aabbset->m_sorted_mincoords,0);
|
|
}
|
|
aabbset->m_boxes = 0;
|
|
aabbset->m_sorted_mincoords = 0;
|
|
aabbset->m_maxcoords = 0;
|
|
}
|
|
|
|
void gim_aabbset_calc_global_bound(GIM_AABB_SET * aabbset)
|
|
{
|
|
aabb3f * paabb = aabbset->m_boxes;
|
|
aabb3f * globalbox = &aabbset->m_global_bound;
|
|
AABB_COPY((*globalbox),(*paabb));
|
|
|
|
GUINT count = aabbset->m_count-1;
|
|
paabb++;
|
|
while(count)
|
|
{
|
|
MERGEBOXES(*globalbox,*paabb)
|
|
paabb++;
|
|
count--;
|
|
}
|
|
}
|
|
|
|
|
|
//! Sorts the boxes for box prunning.
|
|
/*!
|
|
1) find the integer representation of the aabb coords
|
|
2) Sorts the min coords
|
|
3) Calcs the global bound
|
|
\pre aabbset must be allocated. And the boxes must be already set.
|
|
\param aabbset
|
|
\param calc_global_bound If 1 , calcs the global bound
|
|
\post If aabbset->m_sorted_mincoords == 0, then it allocs the sorted coordinates
|
|
*/
|
|
void gim_aabbset_sort(GIM_AABB_SET * aabbset, char calc_global_bound)
|
|
{
|
|
if(aabbset->m_sorted_mincoords == 0)
|
|
{//allocate
|
|
aabbset->m_maxcoords = (GUINT *)gim_alloc(sizeof(GUINT)*aabbset->m_count );
|
|
aabbset->m_sorted_mincoords = (GIM_RSORT_TOKEN *)gim_alloc(sizeof(GIM_RSORT_TOKEN)*aabbset->m_count);
|
|
}
|
|
|
|
GUINT i, count = aabbset->m_count;
|
|
aabb3f * paabb = aabbset->m_boxes;
|
|
GUINT * maxcoords = aabbset->m_maxcoords;
|
|
GIM_RSORT_TOKEN * sorted_tokens = aabbset->m_sorted_mincoords;
|
|
|
|
if(count<860)//Calibrated on a Pentium IV
|
|
{
|
|
//Sort by quick sort
|
|
//Calculate keys
|
|
for(i=0;i<count;i++)
|
|
{
|
|
GIM_CONVERT_VEC3F_GUINT_XZ_UPPER(paabb[i].maxX,paabb[i].maxZ,maxcoords[i]);
|
|
GIM_CONVERT_VEC3F_GUINT_XZ(paabb[i].minX,paabb[i].minZ,sorted_tokens[i].m_key);
|
|
sorted_tokens[i].m_value = i;
|
|
}
|
|
GIM_QUICK_SORT_ARRAY(GIM_RSORT_TOKEN , sorted_tokens, count, RSORT_TOKEN_COMPARATOR,GIM_DEF_EXCHANGE_MACRO);
|
|
}
|
|
else
|
|
{
|
|
//Sort by radix sort
|
|
GIM_RSORT_TOKEN * unsorted = (GIM_RSORT_TOKEN *)gim_alloc(sizeof(GIM_RSORT_TOKEN )*count);
|
|
//Calculate keys
|
|
for(i=0;i<count;i++)
|
|
{
|
|
GIM_CONVERT_VEC3F_GUINT_XZ_UPPER(paabb[i].maxX,paabb[i].maxZ,maxcoords[i]);
|
|
GIM_CONVERT_VEC3F_GUINT_XZ(paabb[i].minX,paabb[i].minZ,unsorted[i].m_key);
|
|
unsorted[i].m_value = i;
|
|
}
|
|
GIM_RADIX_SORT_RTOKENS(unsorted,sorted_tokens,count);
|
|
gim_free(unsorted,0);
|
|
}
|
|
|
|
if(calc_global_bound) gim_aabbset_calc_global_bound(aabbset);
|
|
}
|
|
|
|
//utility macros
|
|
|
|
/*#define PUSH_PAIR(i,j,pairset)\
|
|
{\
|
|
GIM_PAIR _pair={i,j};\
|
|
GIM_DYNARRAY_PUSH_ITEM(GIM_PAIR,pairset,_pair);\
|
|
}*/
|
|
|
|
#define PUSH_PAIR(i,j,pairset)\
|
|
{\
|
|
GIM_DYNARRAY_PUSH_EMPTY(GIM_PAIR,pairset);\
|
|
GIM_PAIR * _pair = GIM_DYNARRAY_POINTER(GIM_PAIR,pairset) + (pairset).m_size - 1;\
|
|
_pair->m_index1 = i;\
|
|
_pair->m_index2 = j;\
|
|
}
|
|
|
|
#define PUSH_PAIR_INV(i,j,pairset)\
|
|
{\
|
|
GIM_DYNARRAY_PUSH_EMPTY(GIM_PAIR,pairset);\
|
|
GIM_PAIR * _pair = GIM_DYNARRAY_POINTER(GIM_PAIR,pairset) + (pairset).m_size - 1;\
|
|
_pair->m_index1 = j;\
|
|
_pair->m_index2 = i;\
|
|
}
|
|
|
|
#define FIND_OVERLAPPING_FOWARD(\
|
|
curr_index,\
|
|
test_count,\
|
|
test_aabb,\
|
|
max_coord_uint,\
|
|
sorted_tokens,\
|
|
aabbarray,\
|
|
pairset,\
|
|
push_pair_macro)\
|
|
{\
|
|
GUINT _i = test_count;\
|
|
char _intersected;\
|
|
GIM_RSORT_TOKEN * _psorted_tokens = sorted_tokens;\
|
|
while(max_coord_uint >= _psorted_tokens->m_key && _i>0)\
|
|
{\
|
|
AABBCOLLISION(_intersected,test_aabb,aabbarray[_psorted_tokens->m_value]);\
|
|
if(_intersected)\
|
|
{\
|
|
push_pair_macro(curr_index, _psorted_tokens->m_value,pairset);\
|
|
}\
|
|
_psorted_tokens++;\
|
|
_i--;\
|
|
}\
|
|
}
|
|
|
|
//! log(N) Complete box pruning. Returns a list of overlapping pairs of boxes, each box of the pair belongs to the same set.
|
|
/*!
|
|
\pre aabbset must be allocated and sorted, the boxes must be already set.
|
|
\param aabbset Must be sorted. Global bound isn't required
|
|
\param collision_pairs Array of GIM_PAIR elements. Must be initialized before (Reserve size ~ 100)
|
|
*/
|
|
void gim_aabbset_self_intersections_sorted(GIM_AABB_SET * aabbset, GDYNAMIC_ARRAY * collision_pairs)
|
|
{
|
|
collision_pairs->m_size = 0;
|
|
GUINT count = aabbset->m_count;
|
|
aabb3f * paabb = aabbset->m_boxes;
|
|
GUINT * maxcoords = aabbset->m_maxcoords;
|
|
GIM_RSORT_TOKEN * sorted_tokens = aabbset->m_sorted_mincoords;
|
|
aabb3f test_aabb;
|
|
while(count>1)
|
|
{
|
|
///current cache variables
|
|
GUINT curr_index = sorted_tokens->m_value;
|
|
GUINT max_coord_uint = maxcoords[curr_index];
|
|
AABB_COPY(test_aabb,paabb[curr_index]);
|
|
|
|
///next pairs
|
|
sorted_tokens++;
|
|
count--;
|
|
FIND_OVERLAPPING_FOWARD( curr_index, count, test_aabb, max_coord_uint, sorted_tokens , paabb, (*collision_pairs),PUSH_PAIR);
|
|
}
|
|
}
|
|
|
|
//! NxN Complete box pruning. Returns a list of overlapping pairs of boxes, each box of the pair belongs to the same set.
|
|
/*!
|
|
\pre aabbset must be allocated, the boxes must be already set.
|
|
\param aabbset Global bound isn't required. Doen't need to be sorted.
|
|
\param collision_pairs Array of GIM_PAIR elements. Must be initialized before (Reserve size ~ 100)
|
|
*/
|
|
void gim_aabbset_self_intersections_brute_force(GIM_AABB_SET * aabbset, GDYNAMIC_ARRAY * collision_pairs)
|
|
{
|
|
collision_pairs->m_size = 0;
|
|
GUINT i,j;
|
|
GUINT count = aabbset->m_count;
|
|
aabb3f * paabb = aabbset->m_boxes;
|
|
char intersected;
|
|
for (i=0;i< count-1 ;i++ )
|
|
{
|
|
for (j=i+1;j<count ;j++ )
|
|
{
|
|
AABBCOLLISION(intersected,paabb[i],paabb[j]);
|
|
if(intersected)
|
|
{
|
|
PUSH_PAIR(i,j,(*collision_pairs));
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
//! log(N) Bipartite box pruning. Returns a list of overlapping pairs of boxes, each box of the pair belongs to a different set.
|
|
/*!
|
|
\pre aabbset1 and aabbset2 must be allocated and sorted, the boxes must be already set.
|
|
\param aabbset1 Must be sorted, Global bound is required.
|
|
\param aabbset2 Must be sorted, Global bound is required.
|
|
\param collision_pairs Array of GIM_PAIR elements. Must be initialized before (Reserve size ~ 100)
|
|
*/
|
|
void gim_aabbset_bipartite_intersections_sorted(GIM_AABB_SET * aabbset1, GIM_AABB_SET * aabbset2, GDYNAMIC_ARRAY * collision_pairs)
|
|
{
|
|
char intersected;
|
|
collision_pairs->m_size = 0;
|
|
|
|
AABBCOLLISION(intersected,aabbset1->m_global_bound,aabbset2->m_global_bound);
|
|
if(intersected == 0) return;
|
|
|
|
GUINT count1 = aabbset1->m_count;
|
|
aabb3f * paabb1 = aabbset1->m_boxes;
|
|
GUINT * maxcoords1 = aabbset1->m_maxcoords;
|
|
GIM_RSORT_TOKEN * sorted_tokens1 = aabbset1->m_sorted_mincoords;
|
|
|
|
GUINT count2 = aabbset2->m_count;
|
|
aabb3f * paabb2 = aabbset2->m_boxes;
|
|
GUINT * maxcoords2 = aabbset2->m_maxcoords;
|
|
GIM_RSORT_TOKEN * sorted_tokens2 = aabbset2->m_sorted_mincoords;
|
|
|
|
GUINT curr_index;
|
|
|
|
GUINT max_coord_uint;
|
|
aabb3f test_aabb;
|
|
|
|
//Classify boxes
|
|
//Find Set intersection
|
|
aabb3f int_abbb;
|
|
BOXINTERSECTION(aabbset1->m_global_bound,aabbset2->m_global_bound, int_abbb);
|
|
|
|
//Clasify set 1
|
|
GIM_RSORT_TOKEN * classified_tokens1 = (GIM_RSORT_TOKEN *) gim_alloc(sizeof(GIM_RSORT_TOKEN)*count1);
|
|
GUINT i,classified_count1 = 0,classified_count2 = 0;
|
|
|
|
|
|
for (i=0;i<count1;i++ )
|
|
{
|
|
curr_index = sorted_tokens1[i].m_value;
|
|
AABBCOLLISION(intersected,paabb1[curr_index],int_abbb);
|
|
if(intersected)
|
|
{
|
|
classified_tokens1[classified_count1] = sorted_tokens1[i];
|
|
classified_count1++;
|
|
}
|
|
}
|
|
|
|
if(classified_count1==0)
|
|
{
|
|
gim_free(classified_tokens1 ,0);
|
|
return; // no pairs
|
|
}
|
|
|
|
//Clasify set 2
|
|
GIM_RSORT_TOKEN * classified_tokens2 = (GIM_RSORT_TOKEN *) gim_alloc(sizeof(GIM_RSORT_TOKEN)*count2);
|
|
|
|
for (i=0;i<count2;i++ )
|
|
{
|
|
curr_index = sorted_tokens2[i].m_value;
|
|
AABBCOLLISION(intersected,paabb2[curr_index],int_abbb);
|
|
if(intersected)
|
|
{
|
|
classified_tokens2[classified_count2] = sorted_tokens2[i];
|
|
classified_count2++;
|
|
}
|
|
}
|
|
|
|
if(classified_count2==0)
|
|
{
|
|
gim_free(classified_tokens1 ,0);
|
|
gim_free(classified_tokens2 ,0);
|
|
return; // no pairs
|
|
}
|
|
|
|
sorted_tokens1 = classified_tokens1;
|
|
sorted_tokens2 = classified_tokens2;
|
|
|
|
while(classified_count1>0&&classified_count2>0)
|
|
{
|
|
if(sorted_tokens1->m_key <= sorted_tokens2->m_key)
|
|
{
|
|
///current cache variables
|
|
curr_index = sorted_tokens1->m_value;
|
|
max_coord_uint = maxcoords1[curr_index];
|
|
AABB_COPY(test_aabb,paabb1[curr_index]);
|
|
///next pairs
|
|
sorted_tokens1++;
|
|
classified_count1--;
|
|
FIND_OVERLAPPING_FOWARD( curr_index, classified_count2, test_aabb, max_coord_uint, sorted_tokens2 , paabb2, (*collision_pairs), PUSH_PAIR);
|
|
}
|
|
else ///Switch test
|
|
{
|
|
///current cache variables
|
|
curr_index = sorted_tokens2->m_value;
|
|
max_coord_uint = maxcoords2[curr_index];
|
|
AABB_COPY(test_aabb,paabb2[curr_index]);
|
|
///next pairs
|
|
sorted_tokens2++;
|
|
classified_count2--;
|
|
FIND_OVERLAPPING_FOWARD( curr_index, classified_count1, test_aabb, max_coord_uint, sorted_tokens1 , paabb1, (*collision_pairs), PUSH_PAIR_INV );
|
|
}
|
|
}
|
|
gim_free(classified_tokens1 ,0);
|
|
gim_free(classified_tokens2 ,0);
|
|
}
|
|
|
|
//! NxM Bipartite box pruning. Returns a list of overlapping pairs of boxes, each box of the pair belongs to a different set.
|
|
/*!
|
|
\pre aabbset1 and aabbset2 must be allocated and sorted, the boxes must be already set.
|
|
\param aabbset1 Must be sorted, Global bound is required.
|
|
\param aabbset2 Must be sorted, Global bound is required.
|
|
\param collision_pairs Array of GIM_PAIR elements. Must be initialized before (Reserve size ~ 100)
|
|
*/
|
|
void gim_aabbset_bipartite_intersections_brute_force(GIM_AABB_SET * aabbset1,GIM_AABB_SET * aabbset2, GDYNAMIC_ARRAY * collision_pairs)
|
|
{
|
|
char intersected;
|
|
collision_pairs->m_size = 0;
|
|
AABBCOLLISION(intersected,aabbset1->m_global_bound,aabbset2->m_global_bound);
|
|
if(intersected == 0) return;
|
|
|
|
aabb3f int_abbb;
|
|
//Find Set intersection
|
|
BOXINTERSECTION(aabbset1->m_global_bound,aabbset2->m_global_bound, int_abbb);
|
|
//Clasify set 1
|
|
GUINT i,j;
|
|
GUINT classified_count = 0;
|
|
|
|
GUINT count = aabbset1->m_count;
|
|
aabb3f * paabb1 = aabbset1->m_boxes;
|
|
aabb3f * paabb2 = aabbset2->m_boxes;
|
|
|
|
GUINT * classified = (GUINT *) gim_alloc(sizeof(GUINT)*count);
|
|
|
|
for (i=0;i<count;i++ )
|
|
{
|
|
AABBCOLLISION(intersected,paabb1[i],int_abbb);
|
|
if(intersected)
|
|
{
|
|
classified[classified_count] = i;
|
|
classified_count++;
|
|
}
|
|
}
|
|
|
|
if(classified_count==0) return; // no pairs
|
|
|
|
//intesect set2
|
|
count = aabbset2->m_count;
|
|
for (i=0;i<count;i++)
|
|
{
|
|
AABBCOLLISION(intersected,paabb2[i],int_abbb);
|
|
if(intersected)
|
|
{
|
|
for (j=0;j<classified_count;j++)
|
|
{
|
|
AABBCOLLISION(intersected,paabb2[i],paabb1[classified[j]]);
|
|
if(intersected)
|
|
{
|
|
PUSH_PAIR(classified[j],i,(*collision_pairs));
|
|
}
|
|
}
|
|
}
|
|
}
|
|
gim_free(classified,0);
|
|
}
|
|
|
|
|
|
//! Initalizes the set. Sort Boxes if needed.
|
|
/*!
|
|
\pre aabbset must be allocated. And the boxes must be already set.
|
|
\post If the set has less of GIM_MIN_SORTED_BIPARTITE_PRUNING_BOXES boxes, only calcs the global box,
|
|
else it Sorts the entire set( Only applicable for large sets)
|
|
*/
|
|
void gim_aabbset_update(GIM_AABB_SET * aabbset)
|
|
{
|
|
if(aabbset->m_count < GIM_MIN_SORTED_BIPARTITE_PRUNING_BOXES)
|
|
{//Brute force approach
|
|
gim_aabbset_calc_global_bound(aabbset);
|
|
}
|
|
else
|
|
{//Sorted force approach
|
|
gim_aabbset_sort(aabbset,1);
|
|
}
|
|
}
|
|
|
|
//! Complete box pruning. Returns a list of overlapping pairs of boxes, each box of the pair belongs to the same set.
|
|
/*!
|
|
This function sorts the set and then it calls to gim_aabbset_self_intersections_brute_force or gim_aabbset_self_intersections_sorted.
|
|
|
|
\param aabbset Set of boxes. Sorting isn't required.
|
|
\param collision_pairs Array of GIM_PAIR elements. Must be initialized before (Reserve size ~ 100)
|
|
\pre aabbset must be allocated and initialized.
|
|
\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.
|
|
*/
|
|
void gim_aabbset_self_intersections(GIM_AABB_SET * aabbset, GDYNAMIC_ARRAY * collision_pairs)
|
|
{
|
|
if(aabbset->m_count < GIM_MIN_SORTED_PRUNING_BOXES)
|
|
{//Brute force approach
|
|
gim_aabbset_self_intersections_brute_force(aabbset,collision_pairs);
|
|
}
|
|
else
|
|
{//Sorted force approach
|
|
gim_aabbset_sort(aabbset,0);
|
|
gim_aabbset_self_intersections_sorted(aabbset,collision_pairs);
|
|
}
|
|
}
|
|
|
|
//! Collides two sets. Returns a list of overlapping pairs of boxes, each box of the pair belongs to a different set.
|
|
/*!
|
|
\pre aabbset1 and aabbset2 must be allocated and updated. See .
|
|
\param aabbset1 Must be sorted, Global bound is required.
|
|
\param aabbset2 Must be sorted, Global bound is required.
|
|
\param collision_pairs Array of GIM_PAIR elements. Must be initialized before (Reserve size ~ 100)
|
|
*/
|
|
void gim_aabbset_bipartite_intersections(GIM_AABB_SET * aabbset1, GIM_AABB_SET * aabbset2, GDYNAMIC_ARRAY * collision_pairs)
|
|
{
|
|
if(aabbset1->m_sorted_mincoords == 0||aabbset2->m_sorted_mincoords == 0)
|
|
{//Brute force approach
|
|
gim_aabbset_bipartite_intersections_brute_force(aabbset1,aabbset2,collision_pairs);
|
|
}
|
|
else
|
|
{//Sorted force approach
|
|
gim_aabbset_bipartite_intersections_sorted(aabbset1,aabbset2,collision_pairs);
|
|
}
|
|
}
|
|
|
|
void gim_aabbset_box_collision(aabb3f *test_aabb, GIM_AABB_SET * aabbset, GDYNAMIC_ARRAY * collided)
|
|
{
|
|
collided->m_size = 0;
|
|
char intersected;
|
|
AABBCOLLISION(intersected,aabbset->m_global_bound,(*test_aabb));
|
|
if(intersected == 0) return;
|
|
|
|
GUINT i;
|
|
GUINT count = aabbset->m_count;
|
|
aabb3f * paabb = aabbset->m_boxes;
|
|
aabb3f _testaabb;
|
|
AABB_COPY(_testaabb,*test_aabb);
|
|
|
|
for (i=0;i< count;i++ )
|
|
{
|
|
AABBCOLLISION(intersected,paabb[i],_testaabb);
|
|
if(intersected)
|
|
{
|
|
GIM_DYNARRAY_PUSH_ITEM(GUINT,(*collided),i);
|
|
}
|
|
}
|
|
}
|
|
|
|
void gim_aabbset_ray_collision(vec3f vorigin,vec3f vdir, GREAL tmax, GIM_AABB_SET * aabbset, GDYNAMIC_ARRAY * collided)
|
|
{
|
|
collided->m_size = 0;
|
|
char intersected;
|
|
GREAL tparam = 0;
|
|
BOX_INTERSECTS_RAY(aabbset->m_global_bound, vorigin, vdir, tparam, tmax,intersected);
|
|
if(intersected==0) return;
|
|
|
|
GUINT i;
|
|
GUINT count = aabbset->m_count;
|
|
aabb3f * paabb = aabbset->m_boxes;
|
|
|
|
for (i=0;i< count;i++ )
|
|
{
|
|
BOX_INTERSECTS_RAY(paabb[i], vorigin, vdir, tparam, tmax,intersected);
|
|
if(intersected)
|
|
{
|
|
GIM_DYNARRAY_PUSH_ITEM(GUINT,(*collided),i);
|
|
}
|
|
}
|
|
}
|