btGImpactBvh.cpp

Go to the documentation of this file.
00001 
00004 /*
00005 This source file is part of GIMPACT Library.
00006 
00007 For the latest info, see http://gimpact.sourceforge.net/
00008 
00009 Copyright (c) 2007 Francisco Leon Najera. C.C. 80087371.
00010 email: projectileman@yahoo.com
00011 
00012 
00013 This software is provided 'as-is', without any express or implied warranty.
00014 In no event will the authors be held liable for any damages arising from the use of this software.
00015 Permission is granted to anyone to use this software for any purpose,
00016 including commercial applications, and to alter it and redistribute it freely,
00017 subject to the following restrictions:
00018 
00019 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
00020 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
00021 3. This notice may not be removed or altered from any source distribution.
00022 */
00023 #include "btGImpactBvh.h"
00024 #include "LinearMath/btQuickprof.h"
00025 
00026 #ifdef TRI_COLLISION_PROFILING
00027 
00028 btClock g_tree_clock;
00029 
00030 float g_accum_tree_collision_time = 0;
00031 int g_count_traversing = 0;
00032 
00033 
00034 void bt_begin_gim02_tree_time()
00035 {
00036         g_tree_clock.reset();
00037 }
00038 
00039 void bt_end_gim02_tree_time()
00040 {
00041         g_accum_tree_collision_time += g_tree_clock.getTimeMicroseconds();
00042         g_count_traversing++;
00043 }
00044 
00046 float btGImpactBvh::getAverageTreeCollisionTime()
00047 {
00048         if(g_count_traversing == 0) return 0;
00049 
00050         float avgtime = g_accum_tree_collision_time;
00051         avgtime /= (float)g_count_traversing;
00052 
00053         g_accum_tree_collision_time = 0;
00054         g_count_traversing = 0;
00055         return avgtime;
00056 
00057 //      float avgtime = g_count_traversing;
00058 //      g_count_traversing = 0;
00059 //      return avgtime;
00060 
00061 }
00062 
00063 #endif //TRI_COLLISION_PROFILING
00064 
00066 
00067 int btBvhTree::_calc_splitting_axis(
00068         GIM_BVH_DATA_ARRAY & primitive_boxes, int startIndex,  int endIndex)
00069 {
00070 
00071         int i;
00072 
00073         btVector3 means(btScalar(0.),btScalar(0.),btScalar(0.));
00074         btVector3 variance(btScalar(0.),btScalar(0.),btScalar(0.));
00075         int numIndices = endIndex-startIndex;
00076 
00077         for (i=startIndex;i<endIndex;i++)
00078         {
00079                 btVector3 center = btScalar(0.5)*(primitive_boxes[i].m_bound.m_max +
00080                                          primitive_boxes[i].m_bound.m_min);
00081                 means+=center;
00082         }
00083         means *= (btScalar(1.)/(btScalar)numIndices);
00084 
00085         for (i=startIndex;i<endIndex;i++)
00086         {
00087                 btVector3 center = btScalar(0.5)*(primitive_boxes[i].m_bound.m_max +
00088                                          primitive_boxes[i].m_bound.m_min);
00089                 btVector3 diff2 = center-means;
00090                 diff2 = diff2 * diff2;
00091                 variance += diff2;
00092         }
00093         variance *= (btScalar(1.)/      ((btScalar)numIndices-1)        );
00094 
00095         return variance.maxAxis();
00096 }
00097 
00098 
00099 int btBvhTree::_sort_and_calc_splitting_index(
00100         GIM_BVH_DATA_ARRAY & primitive_boxes, int startIndex,
00101         int endIndex, int splitAxis)
00102 {
00103         int i;
00104         int splitIndex =startIndex;
00105         int numIndices = endIndex - startIndex;
00106 
00107         // average of centers
00108         btScalar splitValue = 0.0f;
00109 
00110         btVector3 means(btScalar(0.),btScalar(0.),btScalar(0.));
00111         for (i=startIndex;i<endIndex;i++)
00112         {
00113                 btVector3 center = btScalar(0.5)*(primitive_boxes[i].m_bound.m_max +
00114                                          primitive_boxes[i].m_bound.m_min);
00115                 means+=center;
00116         }
00117         means *= (btScalar(1.)/(btScalar)numIndices);
00118 
00119         splitValue = means[splitAxis];
00120 
00121 
00122         //sort leafNodes so all values larger then splitValue comes first, and smaller values start from 'splitIndex'.
00123         for (i=startIndex;i<endIndex;i++)
00124         {
00125                 btVector3 center = btScalar(0.5)*(primitive_boxes[i].m_bound.m_max +
00126                                          primitive_boxes[i].m_bound.m_min);
00127                 if (center[splitAxis] > splitValue)
00128                 {
00129                         //swap
00130                         primitive_boxes.swap(i,splitIndex);
00131                         //swapLeafNodes(i,splitIndex);
00132                         splitIndex++;
00133                 }
00134         }
00135 
00136         //if the splitIndex causes unbalanced trees, fix this by using the center in between startIndex and endIndex
00137         //otherwise the tree-building might fail due to stack-overflows in certain cases.
00138         //unbalanced1 is unsafe: it can cause stack overflows
00139         //bool unbalanced1 = ((splitIndex==startIndex) || (splitIndex == (endIndex-1)));
00140 
00141         //unbalanced2 should work too: always use center (perfect balanced trees)
00142         //bool unbalanced2 = true;
00143 
00144         //this should be safe too:
00145         int rangeBalancedIndices = numIndices/3;
00146         bool unbalanced = ((splitIndex<=(startIndex+rangeBalancedIndices)) || (splitIndex >=(endIndex-1-rangeBalancedIndices)));
00147 
00148         if (unbalanced)
00149         {
00150                 splitIndex = startIndex+ (numIndices>>1);
00151         }
00152 
00153         btAssert(!((splitIndex==startIndex) || (splitIndex == (endIndex))));
00154 
00155         return splitIndex;
00156 
00157 }
00158 
00159 
00160 void btBvhTree::_build_sub_tree(GIM_BVH_DATA_ARRAY & primitive_boxes, int startIndex,  int endIndex)
00161 {
00162         int curIndex = m_num_nodes;
00163         m_num_nodes++;
00164 
00165         btAssert((endIndex-startIndex)>0);
00166 
00167         if ((endIndex-startIndex)==1)
00168         {
00169             //We have a leaf node
00170             setNodeBound(curIndex,primitive_boxes[startIndex].m_bound);
00171                 m_node_array[curIndex].setDataIndex(primitive_boxes[startIndex].m_data);
00172 
00173                 return;
00174         }
00175         //calculate Best Splitting Axis and where to split it. Sort the incoming 'leafNodes' array within range 'startIndex/endIndex'.
00176 
00177         //split axis
00178         int splitIndex = _calc_splitting_axis(primitive_boxes,startIndex,endIndex);
00179 
00180         splitIndex = _sort_and_calc_splitting_index(
00181                         primitive_boxes,startIndex,endIndex,
00182                         splitIndex//split axis
00183                         );
00184 
00185 
00186         //calc this node bounding box
00187 
00188         btAABB node_bound;
00189         node_bound.invalidate();
00190 
00191         for (int i=startIndex;i<endIndex;i++)
00192         {
00193                 node_bound.merge(primitive_boxes[i].m_bound);
00194         }
00195 
00196         setNodeBound(curIndex,node_bound);
00197 
00198 
00199         //build left branch
00200         _build_sub_tree(primitive_boxes, startIndex, splitIndex );
00201 
00202 
00203         //build right branch
00204          _build_sub_tree(primitive_boxes, splitIndex ,endIndex);
00205 
00206         m_node_array[curIndex].setEscapeIndex(m_num_nodes - curIndex);
00207 
00208 
00209 }
00210 
00212 void btBvhTree::build_tree(
00213         GIM_BVH_DATA_ARRAY & primitive_boxes)
00214 {
00215         // initialize node count to 0
00216         m_num_nodes = 0;
00217         // allocate nodes
00218         m_node_array.resize(primitive_boxes.size()*2);
00219 
00220         _build_sub_tree(primitive_boxes, 0, primitive_boxes.size());
00221 }
00222 
00224 
00225 void btGImpactBvh::refit()
00226 {
00227         int nodecount = getNodeCount();
00228         while(nodecount--)
00229         {
00230                 if(isLeafNode(nodecount))
00231                 {
00232                         btAABB leafbox;
00233                         m_primitive_manager->get_primitive_box(getNodeData(nodecount),leafbox);
00234                         setNodeBound(nodecount,leafbox);
00235                 }
00236                 else
00237                 {
00238                         //const GIM_BVH_TREE_NODE * nodepointer = get_node_pointer(nodecount);
00239                         //get left bound
00240                         btAABB bound;
00241                         bound.invalidate();
00242 
00243                         btAABB temp_box;
00244 
00245                         int child_node = getLeftNode(nodecount);
00246                         if(child_node)
00247                         {
00248                                 getNodeBound(child_node,temp_box);
00249                                 bound.merge(temp_box);
00250                         }
00251 
00252                         child_node = getRightNode(nodecount);
00253                         if(child_node)
00254                         {
00255                                 getNodeBound(child_node,temp_box);
00256                                 bound.merge(temp_box);
00257                         }
00258 
00259                         setNodeBound(nodecount,bound);
00260                 }
00261         }
00262 }
00263 
00265 void btGImpactBvh::buildSet()
00266 {
00267         //obtain primitive boxes
00268         GIM_BVH_DATA_ARRAY primitive_boxes;
00269         primitive_boxes.resize(m_primitive_manager->get_primitive_count());
00270 
00271         for (int i = 0;i<primitive_boxes.size() ;i++ )
00272         {
00273                  m_primitive_manager->get_primitive_box(i,primitive_boxes[i].m_bound);
00274                  primitive_boxes[i].m_data = i;
00275         }
00276 
00277         m_box_tree.build_tree(primitive_boxes);
00278 }
00279 
00281 bool btGImpactBvh::boxQuery(const btAABB & box, btAlignedObjectArray<int> & collided_results) const
00282 {
00283         int curIndex = 0;
00284         int numNodes = getNodeCount();
00285 
00286         while (curIndex < numNodes)
00287         {
00288                 btAABB bound;
00289                 getNodeBound(curIndex,bound);
00290 
00291                 //catch bugs in tree data
00292 
00293                 bool aabbOverlap = bound.has_collision(box);
00294                 bool isleafnode = isLeafNode(curIndex);
00295 
00296                 if (isleafnode && aabbOverlap)
00297                 {
00298                         collided_results.push_back(getNodeData(curIndex));
00299                 }
00300 
00301                 if (aabbOverlap || isleafnode)
00302                 {
00303                         //next subnode
00304                         curIndex++;
00305                 }
00306                 else
00307                 {
00308                         //skip node
00309                         curIndex+= getEscapeNodeIndex(curIndex);
00310                 }
00311         }
00312         if(collided_results.size()>0) return true;
00313         return false;
00314 }
00315 
00316 
00317 
00319 bool btGImpactBvh::rayQuery(
00320         const btVector3 & ray_dir,const btVector3 & ray_origin ,
00321         btAlignedObjectArray<int> & collided_results) const
00322 {
00323         int curIndex = 0;
00324         int numNodes = getNodeCount();
00325 
00326         while (curIndex < numNodes)
00327         {
00328                 btAABB bound;
00329                 getNodeBound(curIndex,bound);
00330 
00331                 //catch bugs in tree data
00332 
00333                 bool aabbOverlap = bound.collide_ray(ray_origin,ray_dir);
00334                 bool isleafnode = isLeafNode(curIndex);
00335 
00336                 if (isleafnode && aabbOverlap)
00337                 {
00338                         collided_results.push_back(getNodeData( curIndex));
00339                 }
00340 
00341                 if (aabbOverlap || isleafnode)
00342                 {
00343                         //next subnode
00344                         curIndex++;
00345                 }
00346                 else
00347                 {
00348                         //skip node
00349                         curIndex+= getEscapeNodeIndex(curIndex);
00350                 }
00351         }
00352         if(collided_results.size()>0) return true;
00353         return false;
00354 }
00355 
00356 
00357 SIMD_FORCE_INLINE bool _node_collision(
00358         btGImpactBvh * boxset0, btGImpactBvh * boxset1,
00359         const BT_BOX_BOX_TRANSFORM_CACHE & trans_cache_1to0,
00360         int node0 ,int node1, bool complete_primitive_tests)
00361 {
00362         btAABB box0;
00363         boxset0->getNodeBound(node0,box0);
00364         btAABB box1;
00365         boxset1->getNodeBound(node1,box1);
00366 
00367         return box0.overlapping_trans_cache(box1,trans_cache_1to0,complete_primitive_tests );
00368 //      box1.appy_transform_trans_cache(trans_cache_1to0);
00369 //      return box0.has_collision(box1);
00370 
00371 }
00372 
00373 
00374 //stackless recursive collision routine
00375 static void _find_collision_pairs_recursive(
00376         btGImpactBvh * boxset0, btGImpactBvh * boxset1,
00377         btPairSet * collision_pairs,
00378         const BT_BOX_BOX_TRANSFORM_CACHE & trans_cache_1to0,
00379         int node0, int node1, bool complete_primitive_tests)
00380 {
00381 
00382 
00383 
00384         if( _node_collision(
00385                 boxset0,boxset1,trans_cache_1to0,
00386                 node0,node1,complete_primitive_tests) ==false) return;//avoid colliding internal nodes
00387 
00388         if(boxset0->isLeafNode(node0))
00389         {
00390                 if(boxset1->isLeafNode(node1))
00391                 {
00392                         // collision result
00393                         collision_pairs->push_pair(
00394                                 boxset0->getNodeData(node0),boxset1->getNodeData(node1));
00395                         return;
00396                 }
00397                 else
00398                 {
00399 
00400                         //collide left recursive
00401 
00402                         _find_collision_pairs_recursive(
00403                                                                 boxset0,boxset1,
00404                                                                 collision_pairs,trans_cache_1to0,
00405                                                                 node0,boxset1->getLeftNode(node1),false);
00406 
00407                         //collide right recursive
00408                         _find_collision_pairs_recursive(
00409                                                                 boxset0,boxset1,
00410                                                                 collision_pairs,trans_cache_1to0,
00411                                                                 node0,boxset1->getRightNode(node1),false);
00412 
00413 
00414                 }
00415         }
00416         else
00417         {
00418                 if(boxset1->isLeafNode(node1))
00419                 {
00420 
00421                         //collide left recursive
00422                         _find_collision_pairs_recursive(
00423                                                                 boxset0,boxset1,
00424                                                                 collision_pairs,trans_cache_1to0,
00425                                                                 boxset0->getLeftNode(node0),node1,false);
00426 
00427 
00428                         //collide right recursive
00429 
00430                         _find_collision_pairs_recursive(
00431                                                                 boxset0,boxset1,
00432                                                                 collision_pairs,trans_cache_1to0,
00433                                                                 boxset0->getRightNode(node0),node1,false);
00434 
00435 
00436                 }
00437                 else
00438                 {
00439                         //collide left0 left1
00440 
00441 
00442 
00443                         _find_collision_pairs_recursive(
00444                                 boxset0,boxset1,
00445                                 collision_pairs,trans_cache_1to0,
00446                                 boxset0->getLeftNode(node0),boxset1->getLeftNode(node1),false);
00447 
00448                         //collide left0 right1
00449 
00450                         _find_collision_pairs_recursive(
00451                                 boxset0,boxset1,
00452                                 collision_pairs,trans_cache_1to0,
00453                                 boxset0->getLeftNode(node0),boxset1->getRightNode(node1),false);
00454 
00455 
00456                         //collide right0 left1
00457 
00458                         _find_collision_pairs_recursive(
00459                                 boxset0,boxset1,
00460                                 collision_pairs,trans_cache_1to0,
00461                                 boxset0->getRightNode(node0),boxset1->getLeftNode(node1),false);
00462 
00463                         //collide right0 right1
00464 
00465                         _find_collision_pairs_recursive(
00466                                 boxset0,boxset1,
00467                                 collision_pairs,trans_cache_1to0,
00468                                 boxset0->getRightNode(node0),boxset1->getRightNode(node1),false);
00469 
00470                 }// else if node1 is not a leaf
00471         }// else if node0 is not a leaf
00472 }
00473 
00474 
00475 void btGImpactBvh::find_collision(btGImpactBvh * boxset0, const btTransform & trans0,
00476                 btGImpactBvh * boxset1, const btTransform & trans1,
00477                 btPairSet & collision_pairs)
00478 {
00479 
00480         if(boxset0->getNodeCount()==0 || boxset1->getNodeCount()==0 ) return;
00481 
00482         BT_BOX_BOX_TRANSFORM_CACHE trans_cache_1to0;
00483 
00484         trans_cache_1to0.calc_from_homogenic(trans0,trans1);
00485 
00486 #ifdef TRI_COLLISION_PROFILING
00487         bt_begin_gim02_tree_time();
00488 #endif //TRI_COLLISION_PROFILING
00489 
00490         _find_collision_pairs_recursive(
00491                 boxset0,boxset1,
00492                 &collision_pairs,trans_cache_1to0,0,0,true);
00493 #ifdef TRI_COLLISION_PROFILING
00494         bt_end_gim02_tree_time();
00495 #endif //TRI_COLLISION_PROFILING
00496 
00497 }
00498