src/BulletCollision/Gimpact/btGImpactBvh.cpp

   1 /*! \file gim_box_set.h
   2 \author Francisco Leon Najera
   3 */
   4 /*
   5 This source file is part of GIMPACT Library.
   6
   7 For the latest info, see http://gimpact.sourceforge.net/
   8
   9 Copyright (c) 2007 Francisco Leon Najera. C.C. 80087371.
  10 email: projectileman@yahoo.com
  11
  12
  13 This software is provided 'as-is', without any express or implied warranty.
  14 In no event will the authors be held liable for any damages arising from the use of this software.
  15 Permission is granted to anyone to use this software for any purpose,
  16 including commercial applications, and to alter it and redistribute it freely,
  17 subject to the following restrictions:
  18
  19 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.
  20 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
  21 3. This notice may not be removed or altered from any source distribution.
  22 */
  23 #include "btGImpactBvh.h"
  24 #include "LinearMath/btQuickprof.h"
  25
  26 #ifdef TRI_COLLISION_PROFILING
  27
  28 btClock g_tree_clock;
  29
  30 float g_accum_tree_collision_time = 0;
  31 int g_count_traversing = 0;
  32
  33
  34 void bt_begin_gim02_tree_time()
  35 {
  36         g_tree_clock.reset();
  37 }
  38
  39 void bt_end_gim02_tree_time()
  40 {
  41         g_accum_tree_collision_time += g_tree_clock.getTimeMicroseconds();
  42         g_count_traversing++;
  43 }
  44
  45 //! Gets the average time in miliseconds of tree collisions
  46 float btGImpactBvh::getAverageTreeCollisionTime()
  47 {
  48         if(g_count_traversing == 0) return 0;
  49
  50         float avgtime = g_accum_tree_collision_time;
  51         avgtime /= (float)g_count_traversing;
  52
  53         g_accum_tree_collision_time = 0;
  54         g_count_traversing = 0;
  55         return avgtime;
  56
  57 //      float avgtime = g_count_traversing;
  58 //      g_count_traversing = 0;
  59 //      return avgtime;
  60
  61 }
  62
  63 #endif //TRI_COLLISION_PROFILING
  64
  65 /////////////////////// btBvhTree /////////////////////////////////
  66
  67 int btBvhTree::_calc_splitting_axis(
  68         GIM_BVH_DATA_ARRAY & primitive_boxes, int startIndex,  int endIndex)
  69 {
  70
  71         int i;
  72
  73         btVector3 means(btScalar(0.),btScalar(0.),btScalar(0.));
  74         btVector3 variance(btScalar(0.),btScalar(0.),btScalar(0.));
  75         int numIndices = endIndex-startIndex;
  76
  77         for (i=startIndex;i<endIndex;i++)
  78         {
  79                 btVector3 center = btScalar(0.5)*(primitive_boxes[i].m_bound.m_max +
  80                                          primitive_boxes[i].m_bound.m_min);
  81                 means+=center;
  82         }
  83         means *= (btScalar(1.)/(btScalar)numIndices);
  84
  85         for (i=startIndex;i<endIndex;i++)
  86         {
  87                 btVector3 center = btScalar(0.5)*(primitive_boxes[i].m_bound.m_max +
  88                                          primitive_boxes[i].m_bound.m_min);
  89                 btVector3 diff2 = center-means;
  90                 diff2 = diff2 * diff2;
  91                 variance += diff2;
  92         }
  93         variance *= (btScalar(1.)/      ((btScalar)numIndices-1)        );
  94
  95         return variance.maxAxis();
  96 }
  97
  98
  99 int btBvhTree::_sort_and_calc_splitting_index(
 100         GIM_BVH_DATA_ARRAY & primitive_boxes, int startIndex,
 101         int endIndex, int splitAxis)
 102 {
 103         int i;
 104         int splitIndex =startIndex;
 105         int numIndices = endIndex - startIndex;
 106
 107         // average of centers
 108         btScalar splitValue = 0.0f;
 109
 110         btVector3 means(btScalar(0.),btScalar(0.),btScalar(0.));
 111         for (i=startIndex;i<endIndex;i++)
 112         {
 113                 btVector3 center = btScalar(0.5)*(primitive_boxes[i].m_bound.m_max +
 114                                          primitive_boxes[i].m_bound.m_min);
 115                 means+=center;
 116         }
 117         means *= (btScalar(1.)/(btScalar)numIndices);
 118
 119         splitValue = means[splitAxis];
 120
 121
 122         //sort leafNodes so all values larger then splitValue comes first, and smaller values start from 'splitIndex'.
 123         for (i=startIndex;i<endIndex;i++)
 124         {
 125                 btVector3 center = btScalar(0.5)*(primitive_boxes[i].m_bound.m_max +
 126                                          primitive_boxes[i].m_bound.m_min);
 127                 if (center[splitAxis] > splitValue)
 128                 {
 129                         //swap
 130                         primitive_boxes.swap(i,splitIndex);
 131                         //swapLeafNodes(i,splitIndex);
 132                         splitIndex++;
 133                 }
 134         }
 135
 136         //if the splitIndex causes unbalanced trees, fix this by using the center in between startIndex and endIndex
 137         //otherwise the tree-building might fail due to stack-overflows in certain cases.
 138         //unbalanced1 is unsafe: it can cause stack overflows
 139         //bool unbalanced1 = ((splitIndex==startIndex) || (splitIndex == (endIndex-1)));
 140
 141         //unbalanced2 should work too: always use center (perfect balanced trees)
 142         //bool unbalanced2 = true;
 143
 144         //this should be safe too:
 145         int rangeBalancedIndices = numIndices/3;
 146         bool unbalanced = ((splitIndex<=(startIndex+rangeBalancedIndices)) || (splitIndex >=(endIndex-1-rangeBalancedIndices)));
 147
 148         if (unbalanced)
 149         {
 150                 splitIndex = startIndex+ (numIndices>>1);
 151         }
 152
 153         btAssert(!((splitIndex==startIndex) || (splitIndex == (endIndex))));
 154
 155         return splitIndex;
 156
 157 }
 158
 159
 160 void btBvhTree::_build_sub_tree(GIM_BVH_DATA_ARRAY & primitive_boxes, int startIndex,  int endIndex)
 161 {
 162         int curIndex = m_num_nodes;
 163         m_num_nodes++;
 164
 165         btAssert((endIndex-startIndex)>0);
 166
 167         if ((endIndex-startIndex)==1)
 168         {
 169             //We have a leaf node
 170             setNodeBound(curIndex,primitive_boxes[startIndex].m_bound);
 171                 m_node_array[curIndex].setDataIndex(primitive_boxes[startIndex].m_data);
 172
 173                 return;
 174         }
 175         //calculate Best Splitting Axis and where to split it. Sort the incoming 'leafNodes' array within range 'startIndex/endIndex'.
 176
 177         //split axis
 178         int splitIndex = _calc_splitting_axis(primitive_boxes,startIndex,endIndex);
 179
 180         splitIndex = _sort_and_calc_splitting_index(
 181                         primitive_boxes,startIndex,endIndex,
 182                         splitIndex//split axis
 183                         );
 184
 185
 186         //calc this node bounding box
 187
 188         btAABB node_bound;
 189         node_bound.invalidate();
 190
 191         for (int i=startIndex;i<endIndex;i++)
 192         {
 193                 node_bound.merge(primitive_boxes[i].m_bound);
 194         }
 195
 196         setNodeBound(curIndex,node_bound);
 197
 198
 199         //build left branch
 200         _build_sub_tree(primitive_boxes, startIndex, splitIndex );
 201
 202
 203         //build right branch
 204          _build_sub_tree(primitive_boxes, splitIndex ,endIndex);
 205
 206         m_node_array[curIndex].setEscapeIndex(m_num_nodes - curIndex);
 207
 208
 209 }
 210
 211 //! stackless build tree
 212 void btBvhTree::build_tree(
 213         GIM_BVH_DATA_ARRAY & primitive_boxes)
 214 {
 215         // initialize node count to 0
 216         m_num_nodes = 0;
 217         // allocate nodes
 218         m_node_array.resize(primitive_boxes.size()*2);
 219
 220         _build_sub_tree(primitive_boxes, 0, primitive_boxes.size());
 221 }
 222
 223 ////////////////////////////////////class btGImpactBvh
 224
 225 void btGImpactBvh::refit()
 226 {
 227         int nodecount = getNodeCount();
 228         while(nodecount--)
 229         {
 230                 if(isLeafNode(nodecount))
 231                 {
 232                         btAABB leafbox;
 233                         m_primitive_manager->get_primitive_box(getNodeData(nodecount),leafbox);
 234                         setNodeBound(nodecount,leafbox);
 235                 }
 236                 else
 237                 {
 238                         //const GIM_BVH_TREE_NODE * nodepointer = get_node_pointer(nodecount);
 239                         //get left bound
 240                         btAABB bound;
 241                         bound.invalidate();
 242
 243                         btAABB temp_box;
 244
 245                         int child_node = getLeftNode(nodecount);
 246                         if(child_node)
 247                         {
 248                                 getNodeBound(child_node,temp_box);
 249                                 bound.merge(temp_box);
 250                         }
 251
 252                         child_node = getRightNode(nodecount);
 253                         if(child_node)
 254                         {
 255                                 getNodeBound(child_node,temp_box);
 256                                 bound.merge(temp_box);
 257                         }
 258
 259                         setNodeBound(nodecount,bound);
 260                 }
 261         }
 262 }
 263
 264 //! this rebuild the entire set
 265 void btGImpactBvh::buildSet()
 266 {
 267         //obtain primitive boxes
 268         GIM_BVH_DATA_ARRAY primitive_boxes;
 269         primitive_boxes.resize(m_primitive_manager->get_primitive_count());
 270
 271         for (int i = 0;i<primitive_boxes.size() ;i++ )
 272         {
 273                  m_primitive_manager->get_primitive_box(i,primitive_boxes[i].m_bound);
 274                  primitive_boxes[i].m_data = i;
 275         }
 276
 277         m_box_tree.build_tree(primitive_boxes);
 278 }
 279
 280 //! returns the indices of the primitives in the m_primitive_manager
 281 bool btGImpactBvh::boxQuery(const btAABB & box, btAlignedObjectArray<int> & collided_results) const
 282 {
 283         int curIndex = 0;
 284         int numNodes = getNodeCount();
 285
 286         while (curIndex < numNodes)
 287         {
 288                 btAABB bound;
 289                 getNodeBound(curIndex,bound);
 290
 291                 //catch bugs in tree data
 292
 293                 bool aabbOverlap = bound.has_collision(box);
 294                 bool isleafnode = isLeafNode(curIndex);
 295
 296                 if (isleafnode && aabbOverlap)
 297                 {
 298                         collided_results.push_back(getNodeData(curIndex));
 299                 }
 300
 301                 if (aabbOverlap || isleafnode)
 302                 {
 303                         //next subnode
 304                         curIndex++;
 305                 }
 306                 else
 307                 {
 308                         //skip node
 309                         curIndex+= getEscapeNodeIndex(curIndex);
 310                 }
 311         }
 312         if(collided_results.size()>0) return true;
 313         return false;
 314 }
 315
 316
 317
 318 //! returns the indices of the primitives in the m_primitive_manager
 319 bool btGImpactBvh::rayQuery(
 320         const btVector3 & ray_dir,const btVector3 & ray_origin ,
 321         btAlignedObjectArray<int> & collided_results) const
 322 {
 323         int curIndex = 0;
 324         int numNodes = getNodeCount();
 325
 326         while (curIndex < numNodes)
 327         {
 328                 btAABB bound;
 329                 getNodeBound(curIndex,bound);
 330
 331                 //catch bugs in tree data
 332
 333                 bool aabbOverlap = bound.collide_ray(ray_origin,ray_dir);
 334                 bool isleafnode = isLeafNode(curIndex);
 335
 336                 if (isleafnode && aabbOverlap)
 337                 {
 338                         collided_results.push_back(getNodeData( curIndex));
 339                 }
 340
 341                 if (aabbOverlap || isleafnode)
 342                 {
 343                         //next subnode
 344                         curIndex++;
 345                 }
 346                 else
 347                 {
 348                         //skip node
 349                         curIndex+= getEscapeNodeIndex(curIndex);
 350                 }
 351         }
 352         if(collided_results.size()>0) return true;
 353         return false;
 354 }
 355
 356
 357 SIMD_FORCE_INLINE bool _node_collision(
 358         btGImpactBvh * boxset0, btGImpactBvh * boxset1,
 359         const BT_BOX_BOX_TRANSFORM_CACHE & trans_cache_1to0,
 360         int node0 ,int node1, bool complete_primitive_tests)
 361 {
 362         btAABB box0;
 363         boxset0->getNodeBound(node0,box0);
 364         btAABB box1;
 365         boxset1->getNodeBound(node1,box1);
 366
 367         return box0.overlapping_trans_cache(box1,trans_cache_1to0,complete_primitive_tests );
 368 //      box1.appy_transform_trans_cache(trans_cache_1to0);
 369 //      return box0.has_collision(box1);
 370
 371 }
 372
 373
 374 //stackless recursive collision routine
 375 static void _find_collision_pairs_recursive(
 376         btGImpactBvh * boxset0, btGImpactBvh * boxset1,
 377         btPairSet * collision_pairs,
 378         const BT_BOX_BOX_TRANSFORM_CACHE & trans_cache_1to0,
 379         int node0, int node1, bool complete_primitive_tests)
 380 {
 381
 382
 383
 384         if( _node_collision(
 385                 boxset0,boxset1,trans_cache_1to0,
 386                 node0,node1,complete_primitive_tests) ==false) return;//avoid colliding internal nodes
 387
 388         if(boxset0->isLeafNode(node0))
 389         {
 390                 if(boxset1->isLeafNode(node1))
 391                 {
 392                         // collision result
 393                         collision_pairs->push_pair(
 394                                 boxset0->getNodeData(node0),boxset1->getNodeData(node1));
 395                         return;
 396                 }
 397                 else
 398                 {
 399
 400                         //collide left recursive
 401
 402                         _find_collision_pairs_recursive(
 403                                                                 boxset0,boxset1,
 404                                                                 collision_pairs,trans_cache_1to0,
 405                                                                 node0,boxset1->getLeftNode(node1),false);
 406
 407                         //collide right recursive
 408                         _find_collision_pairs_recursive(
 409                                                                 boxset0,boxset1,
 410                                                                 collision_pairs,trans_cache_1to0,
 411                                                                 node0,boxset1->getRightNode(node1),false);
 412
 413
 414                 }
 415         }
 416         else
 417         {
 418                 if(boxset1->isLeafNode(node1))
 419                 {
 420
 421                         //collide left recursive
 422                         _find_collision_pairs_recursive(
 423                                                                 boxset0,boxset1,
 424                                                                 collision_pairs,trans_cache_1to0,
 425                                                                 boxset0->getLeftNode(node0),node1,false);
 426
 427
 428                         //collide right recursive
 429
 430                         _find_collision_pairs_recursive(
 431                                                                 boxset0,boxset1,
 432                                                                 collision_pairs,trans_cache_1to0,
 433                                                                 boxset0->getRightNode(node0),node1,false);
 434
 435
 436                 }
 437                 else
 438                 {
 439                         //collide left0 left1
 440
 441
 442
 443                         _find_collision_pairs_recursive(
 444                                 boxset0,boxset1,
 445                                 collision_pairs,trans_cache_1to0,
 446                                 boxset0->getLeftNode(node0),boxset1->getLeftNode(node1),false);
 447
 448                         //collide left0 right1
 449
 450                         _find_collision_pairs_recursive(
 451                                 boxset0,boxset1,
 452                                 collision_pairs,trans_cache_1to0,
 453                                 boxset0->getLeftNode(node0),boxset1->getRightNode(node1),false);
 454
 455
 456                         //collide right0 left1
 457
 458                         _find_collision_pairs_recursive(
 459                                 boxset0,boxset1,
 460                                 collision_pairs,trans_cache_1to0,
 461                                 boxset0->getRightNode(node0),boxset1->getLeftNode(node1),false);
 462
 463                         //collide right0 right1
 464
 465                         _find_collision_pairs_recursive(
 466                                 boxset0,boxset1,
 467                                 collision_pairs,trans_cache_1to0,
 468                                 boxset0->getRightNode(node0),boxset1->getRightNode(node1),false);
 469
 470                 }// else if node1 is not a leaf
 471         }// else if node0 is not a leaf
 472 }
 473
 474
 475 void btGImpactBvh::find_collision(btGImpactBvh * boxset0, const btTransform & trans0,
 476                 btGImpactBvh * boxset1, const btTransform & trans1,
 477                 btPairSet & collision_pairs)
 478 {
 479
 480         if(boxset0->getNodeCount()==0 || boxset1->getNodeCount()==0 ) return;
 481
 482         BT_BOX_BOX_TRANSFORM_CACHE trans_cache_1to0;
 483
 484         trans_cache_1to0.calc_from_homogenic(trans0,trans1);
 485
 486 #ifdef TRI_COLLISION_PROFILING
 487         bt_begin_gim02_tree_time();
 488 #endif //TRI_COLLISION_PROFILING
 489
 490         _find_collision_pairs_recursive(
 491                 boxset0,boxset1,
 492                 &collision_pairs,trans_cache_1to0,0,0,true);
 493 #ifdef TRI_COLLISION_PROFILING
 494         bt_end_gim02_tree_time();
 495 #endif //TRI_COLLISION_PROFILING
 496
 497 }
 498