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