00001
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024 #include "btGImpactQuantizedBvh.h"
00025 #include "LinearMath/btQuickprof.h"
00026
00027 #ifdef TRI_COLLISION_PROFILING
00028 btClock g_q_tree_clock;
00029
00030
00031 float g_q_accum_tree_collision_time = 0;
00032 int g_q_count_traversing = 0;
00033
00034
00035 void bt_begin_gim02_q_tree_time()
00036 {
00037 g_q_tree_clock.reset();
00038 }
00039
00040 void bt_end_gim02_q_tree_time()
00041 {
00042 g_q_accum_tree_collision_time += g_q_tree_clock.getTimeMicroseconds();
00043 g_q_count_traversing++;
00044 }
00045
00046
00048 float btGImpactQuantizedBvh::getAverageTreeCollisionTime()
00049 {
00050 if(g_q_count_traversing == 0) return 0;
00051
00052 float avgtime = g_q_accum_tree_collision_time;
00053 avgtime /= (float)g_q_count_traversing;
00054
00055 g_q_accum_tree_collision_time = 0;
00056 g_q_count_traversing = 0;
00057 return avgtime;
00058
00059
00060
00061
00062
00063 }
00064
00065 #endif //TRI_COLLISION_PROFILING
00066
00068
00069 void btQuantizedBvhTree::calc_quantization(
00070 GIM_BVH_DATA_ARRAY & primitive_boxes, btScalar boundMargin)
00071 {
00072
00073 btAABB global_bound;
00074 global_bound.invalidate();
00075
00076 for (int i=0;i<primitive_boxes.size() ;i++ )
00077 {
00078 global_bound.merge(primitive_boxes[i].m_bound);
00079 }
00080
00081 bt_calc_quantization_parameters(
00082 m_global_bound.m_min,m_global_bound.m_max,m_bvhQuantization,global_bound.m_min,global_bound.m_max,boundMargin);
00083
00084 }
00085
00086
00087
00088 int btQuantizedBvhTree::_calc_splitting_axis(
00089 GIM_BVH_DATA_ARRAY & primitive_boxes, int startIndex, int endIndex)
00090 {
00091
00092 int i;
00093
00094 btVector3 means(btScalar(0.),btScalar(0.),btScalar(0.));
00095 btVector3 variance(btScalar(0.),btScalar(0.),btScalar(0.));
00096 int numIndices = endIndex-startIndex;
00097
00098 for (i=startIndex;i<endIndex;i++)
00099 {
00100 btVector3 center = btScalar(0.5)*(primitive_boxes[i].m_bound.m_max +
00101 primitive_boxes[i].m_bound.m_min);
00102 means+=center;
00103 }
00104 means *= (btScalar(1.)/(btScalar)numIndices);
00105
00106 for (i=startIndex;i<endIndex;i++)
00107 {
00108 btVector3 center = btScalar(0.5)*(primitive_boxes[i].m_bound.m_max +
00109 primitive_boxes[i].m_bound.m_min);
00110 btVector3 diff2 = center-means;
00111 diff2 = diff2 * diff2;
00112 variance += diff2;
00113 }
00114 variance *= (btScalar(1.)/ ((btScalar)numIndices-1) );
00115
00116 return variance.maxAxis();
00117 }
00118
00119
00120 int btQuantizedBvhTree::_sort_and_calc_splitting_index(
00121 GIM_BVH_DATA_ARRAY & primitive_boxes, int startIndex,
00122 int endIndex, int splitAxis)
00123 {
00124 int i;
00125 int splitIndex =startIndex;
00126 int numIndices = endIndex - startIndex;
00127
00128
00129 btScalar splitValue = 0.0f;
00130
00131 btVector3 means(btScalar(0.),btScalar(0.),btScalar(0.));
00132 for (i=startIndex;i<endIndex;i++)
00133 {
00134 btVector3 center = btScalar(0.5)*(primitive_boxes[i].m_bound.m_max +
00135 primitive_boxes[i].m_bound.m_min);
00136 means+=center;
00137 }
00138 means *= (btScalar(1.)/(btScalar)numIndices);
00139
00140 splitValue = means[splitAxis];
00141
00142
00143
00144 for (i=startIndex;i<endIndex;i++)
00145 {
00146 btVector3 center = btScalar(0.5)*(primitive_boxes[i].m_bound.m_max +
00147 primitive_boxes[i].m_bound.m_min);
00148 if (center[splitAxis] > splitValue)
00149 {
00150
00151 primitive_boxes.swap(i,splitIndex);
00152
00153 splitIndex++;
00154 }
00155 }
00156
00157
00158
00159
00160
00161
00162
00163
00164
00165
00166 int rangeBalancedIndices = numIndices/3;
00167 bool unbalanced = ((splitIndex<=(startIndex+rangeBalancedIndices)) || (splitIndex >=(endIndex-1-rangeBalancedIndices)));
00168
00169 if (unbalanced)
00170 {
00171 splitIndex = startIndex+ (numIndices>>1);
00172 }
00173
00174 btAssert(!((splitIndex==startIndex) || (splitIndex == (endIndex))));
00175
00176 return splitIndex;
00177
00178 }
00179
00180
00181 void btQuantizedBvhTree::_build_sub_tree(GIM_BVH_DATA_ARRAY & primitive_boxes, int startIndex, int endIndex)
00182 {
00183 int curIndex = m_num_nodes;
00184 m_num_nodes++;
00185
00186 btAssert((endIndex-startIndex)>0);
00187
00188 if ((endIndex-startIndex)==1)
00189 {
00190
00191 setNodeBound(curIndex,primitive_boxes[startIndex].m_bound);
00192 m_node_array[curIndex].setDataIndex(primitive_boxes[startIndex].m_data);
00193
00194 return;
00195 }
00196
00197
00198
00199 int splitIndex = _calc_splitting_axis(primitive_boxes,startIndex,endIndex);
00200
00201 splitIndex = _sort_and_calc_splitting_index(
00202 primitive_boxes,startIndex,endIndex,
00203 splitIndex
00204 );
00205
00206
00207
00208
00209 btAABB node_bound;
00210 node_bound.invalidate();
00211
00212 for (int i=startIndex;i<endIndex;i++)
00213 {
00214 node_bound.merge(primitive_boxes[i].m_bound);
00215 }
00216
00217 setNodeBound(curIndex,node_bound);
00218
00219
00220
00221 _build_sub_tree(primitive_boxes, startIndex, splitIndex );
00222
00223
00224
00225 _build_sub_tree(primitive_boxes, splitIndex ,endIndex);
00226
00227 m_node_array[curIndex].setEscapeIndex(m_num_nodes - curIndex);
00228
00229
00230 }
00231
00233 void btQuantizedBvhTree::build_tree(
00234 GIM_BVH_DATA_ARRAY & primitive_boxes)
00235 {
00236 calc_quantization(primitive_boxes);
00237
00238 m_num_nodes = 0;
00239
00240 m_node_array.resize(primitive_boxes.size()*2);
00241
00242 _build_sub_tree(primitive_boxes, 0, primitive_boxes.size());
00243 }
00244
00246
00247 void btGImpactQuantizedBvh::refit()
00248 {
00249 int nodecount = getNodeCount();
00250 while(nodecount--)
00251 {
00252 if(isLeafNode(nodecount))
00253 {
00254 btAABB leafbox;
00255 m_primitive_manager->get_primitive_box(getNodeData(nodecount),leafbox);
00256 setNodeBound(nodecount,leafbox);
00257 }
00258 else
00259 {
00260
00261
00262 btAABB bound;
00263 bound.invalidate();
00264
00265 btAABB temp_box;
00266
00267 int child_node = getLeftNode(nodecount);
00268 if(child_node)
00269 {
00270 getNodeBound(child_node,temp_box);
00271 bound.merge(temp_box);
00272 }
00273
00274 child_node = getRightNode(nodecount);
00275 if(child_node)
00276 {
00277 getNodeBound(child_node,temp_box);
00278 bound.merge(temp_box);
00279 }
00280
00281 setNodeBound(nodecount,bound);
00282 }
00283 }
00284 }
00285
00287 void btGImpactQuantizedBvh::buildSet()
00288 {
00289
00290 GIM_BVH_DATA_ARRAY primitive_boxes;
00291 primitive_boxes.resize(m_primitive_manager->get_primitive_count());
00292
00293 for (int i = 0;i<primitive_boxes.size() ;i++ )
00294 {
00295 m_primitive_manager->get_primitive_box(i,primitive_boxes[i].m_bound);
00296 primitive_boxes[i].m_data = i;
00297 }
00298
00299 m_box_tree.build_tree(primitive_boxes);
00300 }
00301
00303 bool btGImpactQuantizedBvh::boxQuery(const btAABB & box, btAlignedObjectArray<int> & collided_results) const
00304 {
00305 int curIndex = 0;
00306 int numNodes = getNodeCount();
00307
00308
00309
00310 unsigned short quantizedMin[3];
00311 unsigned short quantizedMax[3];
00312
00313 m_box_tree.quantizePoint(quantizedMin,box.m_min);
00314 m_box_tree.quantizePoint(quantizedMax,box.m_max);
00315
00316
00317 while (curIndex < numNodes)
00318 {
00319
00320
00321
00322 bool aabbOverlap = m_box_tree.testQuantizedBoxOverlapp(curIndex, quantizedMin,quantizedMax);
00323 bool isleafnode = isLeafNode(curIndex);
00324
00325 if (isleafnode && aabbOverlap)
00326 {
00327 collided_results.push_back(getNodeData(curIndex));
00328 }
00329
00330 if (aabbOverlap || isleafnode)
00331 {
00332
00333 curIndex++;
00334 }
00335 else
00336 {
00337
00338 curIndex+= getEscapeNodeIndex(curIndex);
00339 }
00340 }
00341 if(collided_results.size()>0) return true;
00342 return false;
00343 }
00344
00345
00346
00348 bool btGImpactQuantizedBvh::rayQuery(
00349 const btVector3 & ray_dir,const btVector3 & ray_origin ,
00350 btAlignedObjectArray<int> & collided_results) const
00351 {
00352 int curIndex = 0;
00353 int numNodes = getNodeCount();
00354
00355 while (curIndex < numNodes)
00356 {
00357 btAABB bound;
00358 getNodeBound(curIndex,bound);
00359
00360
00361
00362 bool aabbOverlap = bound.collide_ray(ray_origin,ray_dir);
00363 bool isleafnode = isLeafNode(curIndex);
00364
00365 if (isleafnode && aabbOverlap)
00366 {
00367 collided_results.push_back(getNodeData( curIndex));
00368 }
00369
00370 if (aabbOverlap || isleafnode)
00371 {
00372
00373 curIndex++;
00374 }
00375 else
00376 {
00377
00378 curIndex+= getEscapeNodeIndex(curIndex);
00379 }
00380 }
00381 if(collided_results.size()>0) return true;
00382 return false;
00383 }
00384
00385
00386 SIMD_FORCE_INLINE bool _quantized_node_collision(
00387 const btGImpactQuantizedBvh * boxset0, const btGImpactQuantizedBvh * boxset1,
00388 const BT_BOX_BOX_TRANSFORM_CACHE & trans_cache_1to0,
00389 int node0 ,int node1, bool complete_primitive_tests)
00390 {
00391 btAABB box0;
00392 boxset0->getNodeBound(node0,box0);
00393 btAABB box1;
00394 boxset1->getNodeBound(node1,box1);
00395
00396 return box0.overlapping_trans_cache(box1,trans_cache_1to0,complete_primitive_tests );
00397
00398
00399
00400 }
00401
00402
00403
00404 static void _find_quantized_collision_pairs_recursive(
00405 const btGImpactQuantizedBvh * boxset0, const btGImpactQuantizedBvh * boxset1,
00406 btPairSet * collision_pairs,
00407 const BT_BOX_BOX_TRANSFORM_CACHE & trans_cache_1to0,
00408 int node0, int node1, bool complete_primitive_tests)
00409 {
00410
00411
00412
00413 if( _quantized_node_collision(
00414 boxset0,boxset1,trans_cache_1to0,
00415 node0,node1,complete_primitive_tests) ==false) return;
00416
00417 if(boxset0->isLeafNode(node0))
00418 {
00419 if(boxset1->isLeafNode(node1))
00420 {
00421
00422 collision_pairs->push_pair(
00423 boxset0->getNodeData(node0),boxset1->getNodeData(node1));
00424 return;
00425 }
00426 else
00427 {
00428
00429
00430
00431 _find_quantized_collision_pairs_recursive(
00432 boxset0,boxset1,
00433 collision_pairs,trans_cache_1to0,
00434 node0,boxset1->getLeftNode(node1),false);
00435
00436
00437 _find_quantized_collision_pairs_recursive(
00438 boxset0,boxset1,
00439 collision_pairs,trans_cache_1to0,
00440 node0,boxset1->getRightNode(node1),false);
00441
00442
00443 }
00444 }
00445 else
00446 {
00447 if(boxset1->isLeafNode(node1))
00448 {
00449
00450
00451 _find_quantized_collision_pairs_recursive(
00452 boxset0,boxset1,
00453 collision_pairs,trans_cache_1to0,
00454 boxset0->getLeftNode(node0),node1,false);
00455
00456
00457
00458
00459 _find_quantized_collision_pairs_recursive(
00460 boxset0,boxset1,
00461 collision_pairs,trans_cache_1to0,
00462 boxset0->getRightNode(node0),node1,false);
00463
00464
00465 }
00466 else
00467 {
00468
00469
00470
00471
00472 _find_quantized_collision_pairs_recursive(
00473 boxset0,boxset1,
00474 collision_pairs,trans_cache_1to0,
00475 boxset0->getLeftNode(node0),boxset1->getLeftNode(node1),false);
00476
00477
00478
00479 _find_quantized_collision_pairs_recursive(
00480 boxset0,boxset1,
00481 collision_pairs,trans_cache_1to0,
00482 boxset0->getLeftNode(node0),boxset1->getRightNode(node1),false);
00483
00484
00485
00486
00487 _find_quantized_collision_pairs_recursive(
00488 boxset0,boxset1,
00489 collision_pairs,trans_cache_1to0,
00490 boxset0->getRightNode(node0),boxset1->getLeftNode(node1),false);
00491
00492
00493
00494 _find_quantized_collision_pairs_recursive(
00495 boxset0,boxset1,
00496 collision_pairs,trans_cache_1to0,
00497 boxset0->getRightNode(node0),boxset1->getRightNode(node1),false);
00498
00499 }
00500 }
00501 }
00502
00503
00504 void btGImpactQuantizedBvh::find_collision(const btGImpactQuantizedBvh * boxset0, const btTransform & trans0,
00505 const btGImpactQuantizedBvh * boxset1, const btTransform & trans1,
00506 btPairSet & collision_pairs)
00507 {
00508
00509 if(boxset0->getNodeCount()==0 || boxset1->getNodeCount()==0 ) return;
00510
00511 BT_BOX_BOX_TRANSFORM_CACHE trans_cache_1to0;
00512
00513 trans_cache_1to0.calc_from_homogenic(trans0,trans1);
00514
00515 #ifdef TRI_COLLISION_PROFILING
00516 bt_begin_gim02_q_tree_time();
00517 #endif //TRI_COLLISION_PROFILING
00518
00519 _find_quantized_collision_pairs_recursive(
00520 boxset0,boxset1,
00521 &collision_pairs,trans_cache_1to0,0,0,true);
00522 #ifdef TRI_COLLISION_PROFILING
00523 bt_end_gim02_q_tree_time();
00524 #endif //TRI_COLLISION_PROFILING
00525
00526 }
00527
00528