btGImpactQuantizedBvh.cpp

Go to the documentation of this file.
00001 
00004 /*
00005 This source file is part of GIMPACT Library.
00006 
00007 For the latest info, see http://gimpact.sourceforge.net/
00008 
00009 Copyright (c) 2007 Francisco Leon Najera. C.C. 80087371.
00010 email: projectileman@yahoo.com
00011 
00012 
00013 This software is provided 'as-is', without any express or implied warranty.
00014 In no event will the authors be held liable for any damages arising from the use of this software.
00015 Permission is granted to anyone to use this software for any purpose,
00016 including commercial applications, and to alter it and redistribute it freely,
00017 subject to the following restrictions:
00018 
00019 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.
00020 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
00021 3. This notice may not be removed or altered from any source distribution.
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 //      float avgtime = g_q_count_traversing;
00060 //      g_q_count_traversing = 0;
00061 //      return avgtime;
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         //calc globa box
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         // average of centers
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         //sort leafNodes so all values larger then splitValue comes first, and smaller values start from 'splitIndex'.
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                         //swap
00151                         primitive_boxes.swap(i,splitIndex);
00152                         //swapLeafNodes(i,splitIndex);
00153                         splitIndex++;
00154                 }
00155         }
00156 
00157         //if the splitIndex causes unbalanced trees, fix this by using the center in between startIndex and endIndex
00158         //otherwise the tree-building might fail due to stack-overflows in certain cases.
00159         //unbalanced1 is unsafe: it can cause stack overflows
00160         //bool unbalanced1 = ((splitIndex==startIndex) || (splitIndex == (endIndex-1)));
00161 
00162         //unbalanced2 should work too: always use center (perfect balanced trees)
00163         //bool unbalanced2 = true;
00164 
00165         //this should be safe too:
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             //We have a leaf node
00191             setNodeBound(curIndex,primitive_boxes[startIndex].m_bound);
00192                 m_node_array[curIndex].setDataIndex(primitive_boxes[startIndex].m_data);
00193 
00194                 return;
00195         }
00196         //calculate Best Splitting Axis and where to split it. Sort the incoming 'leafNodes' array within range 'startIndex/endIndex'.
00197 
00198         //split axis
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//split axis
00204                         );
00205 
00206 
00207         //calc this node bounding box
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         //build left branch
00221         _build_sub_tree(primitive_boxes, startIndex, splitIndex );
00222 
00223 
00224         //build right branch
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         // initialize node count to 0
00238         m_num_nodes = 0;
00239         // allocate nodes
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                         //const GIM_BVH_TREE_NODE * nodepointer = get_node_pointer(nodecount);
00261                         //get left bound
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         //obtain primitive boxes
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         //quantize box
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                 //catch bugs in tree data
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                         //next subnode
00333                         curIndex++;
00334                 }
00335                 else
00336                 {
00337                         //skip node
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                 //catch bugs in tree data
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                         //next subnode
00373                         curIndex++;
00374                 }
00375                 else
00376                 {
00377                         //skip node
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 //      box1.appy_transform_trans_cache(trans_cache_1to0);
00398 //      return box0.has_collision(box1);
00399 
00400 }
00401 
00402 
00403 //stackless recursive collision routine
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;//avoid colliding internal nodes
00416 
00417         if(boxset0->isLeafNode(node0))
00418         {
00419                 if(boxset1->isLeafNode(node1))
00420                 {
00421                         // collision result
00422                         collision_pairs->push_pair(
00423                                 boxset0->getNodeData(node0),boxset1->getNodeData(node1));
00424                         return;
00425                 }
00426                 else
00427                 {
00428 
00429                         //collide left recursive
00430 
00431                         _find_quantized_collision_pairs_recursive(
00432                                                                 boxset0,boxset1,
00433                                                                 collision_pairs,trans_cache_1to0,
00434                                                                 node0,boxset1->getLeftNode(node1),false);
00435 
00436                         //collide right recursive
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                         //collide left recursive
00451                         _find_quantized_collision_pairs_recursive(
00452                                                                 boxset0,boxset1,
00453                                                                 collision_pairs,trans_cache_1to0,
00454                                                                 boxset0->getLeftNode(node0),node1,false);
00455 
00456 
00457                         //collide right recursive
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                         //collide left0 left1
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                         //collide left0 right1
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                         //collide right0 left1
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                         //collide right0 right1
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                 }// else if node1 is not a leaf
00500         }// else if node0 is not a leaf
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