00001
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
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
00058
00059
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
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
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
00130 primitive_boxes.swap(i,splitIndex);
00131
00132 splitIndex++;
00133 }
00134 }
00135
00136
00137
00138
00139
00140
00141
00142
00143
00144
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
00170 setNodeBound(curIndex,primitive_boxes[startIndex].m_bound);
00171 m_node_array[curIndex].setDataIndex(primitive_boxes[startIndex].m_data);
00172
00173 return;
00174 }
00175
00176
00177
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
00183 );
00184
00185
00186
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
00200 _build_sub_tree(primitive_boxes, startIndex, splitIndex );
00201
00202
00203
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
00216 m_num_nodes = 0;
00217
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
00239
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
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
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
00304 curIndex++;
00305 }
00306 else
00307 {
00308
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
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
00344 curIndex++;
00345 }
00346 else
00347 {
00348
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
00369
00370
00371 }
00372
00373
00374
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;
00387
00388 if(boxset0->isLeafNode(node0))
00389 {
00390 if(boxset1->isLeafNode(node1))
00391 {
00392
00393 collision_pairs->push_pair(
00394 boxset0->getNodeData(node0),boxset1->getNodeData(node1));
00395 return;
00396 }
00397 else
00398 {
00399
00400
00401
00402 _find_collision_pairs_recursive(
00403 boxset0,boxset1,
00404 collision_pairs,trans_cache_1to0,
00405 node0,boxset1->getLeftNode(node1),false);
00406
00407
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
00422 _find_collision_pairs_recursive(
00423 boxset0,boxset1,
00424 collision_pairs,trans_cache_1to0,
00425 boxset0->getLeftNode(node0),node1,false);
00426
00427
00428
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
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
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
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
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 }
00471 }
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