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 #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 499
