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 (at) 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 529
