diff options
Diffstat (limited to 'thirdparty/bullet/BulletCollision/Gimpact/btGImpactQuantizedBvh.cpp')
| -rw-r--r-- | thirdparty/bullet/BulletCollision/Gimpact/btGImpactQuantizedBvh.cpp | 304 |
1 files changed, 131 insertions, 173 deletions
diff --git a/thirdparty/bullet/BulletCollision/Gimpact/btGImpactQuantizedBvh.cpp b/thirdparty/bullet/BulletCollision/Gimpact/btGImpactQuantizedBvh.cpp index 4528758c37..b81fc97044 100644 --- a/thirdparty/bullet/BulletCollision/Gimpact/btGImpactQuantizedBvh.cpp +++ b/thirdparty/bullet/BulletCollision/Gimpact/btGImpactQuantizedBvh.cpp @@ -27,11 +27,9 @@ subject to the following restrictions: #ifdef TRI_COLLISION_PROFILING btClock g_q_tree_clock; - float g_q_accum_tree_collision_time = 0; int g_q_count_traversing = 0; - void bt_begin_gim02_q_tree_time() { g_q_tree_clock.reset(); @@ -43,11 +41,10 @@ void bt_end_gim02_q_tree_time() g_q_count_traversing++; } - //! Gets the average time in miliseconds of tree collisions float btGImpactQuantizedBvh::getAverageTreeCollisionTime() { - if(g_q_count_traversing == 0) return 0; + if (g_q_count_traversing == 0) return 0; float avgtime = g_q_accum_tree_collision_time; avgtime /= (float)g_q_count_traversing; @@ -56,99 +53,92 @@ float btGImpactQuantizedBvh::getAverageTreeCollisionTime() g_q_count_traversing = 0; return avgtime; -// float avgtime = g_q_count_traversing; -// g_q_count_traversing = 0; -// return avgtime; - + // float avgtime = g_q_count_traversing; + // g_q_count_traversing = 0; + // return avgtime; } -#endif //TRI_COLLISION_PROFILING +#endif //TRI_COLLISION_PROFILING /////////////////////// btQuantizedBvhTree ///////////////////////////////// void btQuantizedBvhTree::calc_quantization( - GIM_BVH_DATA_ARRAY & primitive_boxes, btScalar boundMargin) + GIM_BVH_DATA_ARRAY& primitive_boxes, btScalar boundMargin) { //calc globa box btAABB global_bound; global_bound.invalidate(); - for (int i=0;i<primitive_boxes.size() ;i++ ) + for (int i = 0; i < primitive_boxes.size(); i++) { global_bound.merge(primitive_boxes[i].m_bound); } bt_calc_quantization_parameters( - m_global_bound.m_min,m_global_bound.m_max,m_bvhQuantization,global_bound.m_min,global_bound.m_max,boundMargin); - + m_global_bound.m_min, m_global_bound.m_max, m_bvhQuantization, global_bound.m_min, global_bound.m_max, boundMargin); } - - int btQuantizedBvhTree::_calc_splitting_axis( - GIM_BVH_DATA_ARRAY & primitive_boxes, int startIndex, int endIndex) + GIM_BVH_DATA_ARRAY& primitive_boxes, int startIndex, int endIndex) { - int i; - btVector3 means(btScalar(0.),btScalar(0.),btScalar(0.)); - btVector3 variance(btScalar(0.),btScalar(0.),btScalar(0.)); - int numIndices = endIndex-startIndex; + btVector3 means(btScalar(0.), btScalar(0.), btScalar(0.)); + btVector3 variance(btScalar(0.), btScalar(0.), btScalar(0.)); + int numIndices = endIndex - startIndex; - for (i=startIndex;i<endIndex;i++) + for (i = startIndex; i < endIndex; i++) { - btVector3 center = btScalar(0.5)*(primitive_boxes[i].m_bound.m_max + - primitive_boxes[i].m_bound.m_min); - means+=center; + btVector3 center = btScalar(0.5) * (primitive_boxes[i].m_bound.m_max + + primitive_boxes[i].m_bound.m_min); + means += center; } - means *= (btScalar(1.)/(btScalar)numIndices); + means *= (btScalar(1.) / (btScalar)numIndices); - for (i=startIndex;i<endIndex;i++) + for (i = startIndex; i < endIndex; i++) { - btVector3 center = btScalar(0.5)*(primitive_boxes[i].m_bound.m_max + - primitive_boxes[i].m_bound.m_min); - btVector3 diff2 = center-means; + btVector3 center = btScalar(0.5) * (primitive_boxes[i].m_bound.m_max + + primitive_boxes[i].m_bound.m_min); + btVector3 diff2 = center - means; diff2 = diff2 * diff2; variance += diff2; } - variance *= (btScalar(1.)/ ((btScalar)numIndices-1) ); + variance *= (btScalar(1.) / ((btScalar)numIndices - 1)); return variance.maxAxis(); } - int btQuantizedBvhTree::_sort_and_calc_splitting_index( - GIM_BVH_DATA_ARRAY & primitive_boxes, int startIndex, + GIM_BVH_DATA_ARRAY& primitive_boxes, int startIndex, int endIndex, int splitAxis) { int i; - int splitIndex =startIndex; + int splitIndex = startIndex; int numIndices = endIndex - startIndex; // average of centers btScalar splitValue = 0.0f; - btVector3 means(btScalar(0.),btScalar(0.),btScalar(0.)); - for (i=startIndex;i<endIndex;i++) + btVector3 means(btScalar(0.), btScalar(0.), btScalar(0.)); + for (i = startIndex; i < endIndex; i++) { - btVector3 center = btScalar(0.5)*(primitive_boxes[i].m_bound.m_max + - primitive_boxes[i].m_bound.m_min); - means+=center; + btVector3 center = btScalar(0.5) * (primitive_boxes[i].m_bound.m_max + + primitive_boxes[i].m_bound.m_min); + means += center; } - means *= (btScalar(1.)/(btScalar)numIndices); + means *= (btScalar(1.) / (btScalar)numIndices); splitValue = means[splitAxis]; - //sort leafNodes so all values larger then splitValue comes first, and smaller values start from 'splitIndex'. - for (i=startIndex;i<endIndex;i++) + for (i = startIndex; i < endIndex; i++) { - btVector3 center = btScalar(0.5)*(primitive_boxes[i].m_bound.m_max + - primitive_boxes[i].m_bound.m_min); + btVector3 center = btScalar(0.5) * (primitive_boxes[i].m_bound.m_max + + primitive_boxes[i].m_bound.m_min); if (center[splitAxis] > splitValue) { //swap - primitive_boxes.swap(i,splitIndex); + primitive_boxes.swap(i, splitIndex); //swapLeafNodes(i,splitIndex); splitIndex++; } @@ -163,32 +153,30 @@ int btQuantizedBvhTree::_sort_and_calc_splitting_index( //bool unbalanced2 = true; //this should be safe too: - int rangeBalancedIndices = numIndices/3; - bool unbalanced = ((splitIndex<=(startIndex+rangeBalancedIndices)) || (splitIndex >=(endIndex-1-rangeBalancedIndices))); + int rangeBalancedIndices = numIndices / 3; + bool unbalanced = ((splitIndex <= (startIndex + rangeBalancedIndices)) || (splitIndex >= (endIndex - 1 - rangeBalancedIndices))); if (unbalanced) { - splitIndex = startIndex+ (numIndices>>1); + splitIndex = startIndex + (numIndices >> 1); } - btAssert(!((splitIndex==startIndex) || (splitIndex == (endIndex)))); + btAssert(!((splitIndex == startIndex) || (splitIndex == (endIndex)))); return splitIndex; - } - -void btQuantizedBvhTree::_build_sub_tree(GIM_BVH_DATA_ARRAY & primitive_boxes, int startIndex, int endIndex) +void btQuantizedBvhTree::_build_sub_tree(GIM_BVH_DATA_ARRAY& primitive_boxes, int startIndex, int endIndex) { int curIndex = m_num_nodes; m_num_nodes++; - btAssert((endIndex-startIndex)>0); + btAssert((endIndex - startIndex) > 0); - if ((endIndex-startIndex)==1) + if ((endIndex - startIndex) == 1) { - //We have a leaf node - setNodeBound(curIndex,primitive_boxes[startIndex].m_bound); + //We have a leaf node + setNodeBound(curIndex, primitive_boxes[startIndex].m_bound); m_node_array[curIndex].setDataIndex(primitive_boxes[startIndex].m_data); return; @@ -196,48 +184,43 @@ void btQuantizedBvhTree::_build_sub_tree(GIM_BVH_DATA_ARRAY & primitive_boxes, i //calculate Best Splitting Axis and where to split it. Sort the incoming 'leafNodes' array within range 'startIndex/endIndex'. //split axis - int splitIndex = _calc_splitting_axis(primitive_boxes,startIndex,endIndex); + int splitIndex = _calc_splitting_axis(primitive_boxes, startIndex, endIndex); splitIndex = _sort_and_calc_splitting_index( - primitive_boxes,startIndex,endIndex, - splitIndex//split axis - ); - + primitive_boxes, startIndex, endIndex, + splitIndex //split axis + ); //calc this node bounding box btAABB node_bound; node_bound.invalidate(); - for (int i=startIndex;i<endIndex;i++) + for (int i = startIndex; i < endIndex; i++) { node_bound.merge(primitive_boxes[i].m_bound); } - setNodeBound(curIndex,node_bound); - + setNodeBound(curIndex, node_bound); //build left branch - _build_sub_tree(primitive_boxes, startIndex, splitIndex ); - + _build_sub_tree(primitive_boxes, startIndex, splitIndex); //build right branch - _build_sub_tree(primitive_boxes, splitIndex ,endIndex); + _build_sub_tree(primitive_boxes, splitIndex, endIndex); m_node_array[curIndex].setEscapeIndex(m_num_nodes - curIndex); - - } //! stackless build tree void btQuantizedBvhTree::build_tree( - GIM_BVH_DATA_ARRAY & primitive_boxes) + GIM_BVH_DATA_ARRAY& primitive_boxes) { calc_quantization(primitive_boxes); // initialize node count to 0 m_num_nodes = 0; // allocate nodes - m_node_array.resize(primitive_boxes.size()*2); + m_node_array.resize(primitive_boxes.size() * 2); _build_sub_tree(primitive_boxes, 0, primitive_boxes.size()); } @@ -247,13 +230,13 @@ void btQuantizedBvhTree::build_tree( void btGImpactQuantizedBvh::refit() { int nodecount = getNodeCount(); - while(nodecount--) + while (nodecount--) { - if(isLeafNode(nodecount)) + if (isLeafNode(nodecount)) { btAABB leafbox; - m_primitive_manager->get_primitive_box(getNodeData(nodecount),leafbox); - setNodeBound(nodecount,leafbox); + m_primitive_manager->get_primitive_box(getNodeData(nodecount), leafbox); + setNodeBound(nodecount, leafbox); } else { @@ -265,20 +248,20 @@ void btGImpactQuantizedBvh::refit() btAABB temp_box; int child_node = getLeftNode(nodecount); - if(child_node) + if (child_node) { - getNodeBound(child_node,temp_box); + getNodeBound(child_node, temp_box); bound.merge(temp_box); } child_node = getRightNode(nodecount); - if(child_node) + if (child_node) { - getNodeBound(child_node,temp_box); + getNodeBound(child_node, temp_box); bound.merge(temp_box); } - setNodeBound(nodecount,bound); + setNodeBound(nodecount, bound); } } } @@ -290,17 +273,17 @@ void btGImpactQuantizedBvh::buildSet() GIM_BVH_DATA_ARRAY primitive_boxes; primitive_boxes.resize(m_primitive_manager->get_primitive_count()); - for (int i = 0;i<primitive_boxes.size() ;i++ ) + for (int i = 0; i < primitive_boxes.size(); i++) { - m_primitive_manager->get_primitive_box(i,primitive_boxes[i].m_bound); - primitive_boxes[i].m_data = i; + m_primitive_manager->get_primitive_box(i, primitive_boxes[i].m_bound); + primitive_boxes[i].m_data = i; } m_box_tree.build_tree(primitive_boxes); } //! returns the indices of the primitives in the m_primitive_manager -bool btGImpactQuantizedBvh::boxQuery(const btAABB & box, btAlignedObjectArray<int> & collided_results) const +bool btGImpactQuantizedBvh::boxQuery(const btAABB& box, btAlignedObjectArray<int>& collided_results) const { int curIndex = 0; int numNodes = getNodeCount(); @@ -310,16 +293,14 @@ bool btGImpactQuantizedBvh::boxQuery(const btAABB & box, btAlignedObjectArray<in unsigned short quantizedMin[3]; unsigned short quantizedMax[3]; - m_box_tree.quantizePoint(quantizedMin,box.m_min); - m_box_tree.quantizePoint(quantizedMax,box.m_max); - + m_box_tree.quantizePoint(quantizedMin, box.m_min); + m_box_tree.quantizePoint(quantizedMax, box.m_max); while (curIndex < numNodes) { - //catch bugs in tree data - bool aabbOverlap = m_box_tree.testQuantizedBoxOverlapp(curIndex, quantizedMin,quantizedMax); + bool aabbOverlap = m_box_tree.testQuantizedBoxOverlapp(curIndex, quantizedMin, quantizedMax); bool isleafnode = isLeafNode(curIndex); if (isleafnode && aabbOverlap) @@ -335,19 +316,17 @@ bool btGImpactQuantizedBvh::boxQuery(const btAABB & box, btAlignedObjectArray<in else { //skip node - curIndex+= getEscapeNodeIndex(curIndex); + curIndex += getEscapeNodeIndex(curIndex); } } - if(collided_results.size()>0) return true; + if (collided_results.size() > 0) return true; return false; } - - //! returns the indices of the primitives in the m_primitive_manager bool btGImpactQuantizedBvh::rayQuery( - const btVector3 & ray_dir,const btVector3 & ray_origin , - btAlignedObjectArray<int> & collided_results) const + const btVector3& ray_dir, const btVector3& ray_origin, + btAlignedObjectArray<int>& collided_results) const { int curIndex = 0; int numNodes = getNodeCount(); @@ -355,16 +334,16 @@ bool btGImpactQuantizedBvh::rayQuery( while (curIndex < numNodes) { btAABB bound; - getNodeBound(curIndex,bound); + getNodeBound(curIndex, bound); //catch bugs in tree data - bool aabbOverlap = bound.collide_ray(ray_origin,ray_dir); + bool aabbOverlap = bound.collide_ray(ray_origin, ray_dir); bool isleafnode = isLeafNode(curIndex); if (isleafnode && aabbOverlap) { - collided_results.push_back(getNodeData( curIndex)); + collided_results.push_back(getNodeData(curIndex)); } if (aabbOverlap || isleafnode) @@ -375,154 +354,133 @@ bool btGImpactQuantizedBvh::rayQuery( else { //skip node - curIndex+= getEscapeNodeIndex(curIndex); + curIndex += getEscapeNodeIndex(curIndex); } } - if(collided_results.size()>0) return true; + if (collided_results.size() > 0) return true; return false; } - SIMD_FORCE_INLINE bool _quantized_node_collision( - const btGImpactQuantizedBvh * boxset0, const btGImpactQuantizedBvh * boxset1, - const BT_BOX_BOX_TRANSFORM_CACHE & trans_cache_1to0, - int node0 ,int node1, bool complete_primitive_tests) + const btGImpactQuantizedBvh* boxset0, const btGImpactQuantizedBvh* boxset1, + const BT_BOX_BOX_TRANSFORM_CACHE& trans_cache_1to0, + int node0, int node1, bool complete_primitive_tests) { btAABB box0; - boxset0->getNodeBound(node0,box0); + boxset0->getNodeBound(node0, box0); btAABB box1; - boxset1->getNodeBound(node1,box1); - - return box0.overlapping_trans_cache(box1,trans_cache_1to0,complete_primitive_tests ); -// box1.appy_transform_trans_cache(trans_cache_1to0); -// return box0.has_collision(box1); + boxset1->getNodeBound(node1, box1); + return box0.overlapping_trans_cache(box1, trans_cache_1to0, complete_primitive_tests); + // box1.appy_transform_trans_cache(trans_cache_1to0); + // return box0.has_collision(box1); } - //stackless recursive collision routine static void _find_quantized_collision_pairs_recursive( - const btGImpactQuantizedBvh * boxset0, const btGImpactQuantizedBvh * boxset1, - btPairSet * collision_pairs, - const BT_BOX_BOX_TRANSFORM_CACHE & trans_cache_1to0, + const btGImpactQuantizedBvh* boxset0, const btGImpactQuantizedBvh* boxset1, + btPairSet* collision_pairs, + const BT_BOX_BOX_TRANSFORM_CACHE& trans_cache_1to0, int node0, int node1, bool complete_primitive_tests) { + if (_quantized_node_collision( + boxset0, boxset1, trans_cache_1to0, + node0, node1, complete_primitive_tests) == false) return; //avoid colliding internal nodes - - - if( _quantized_node_collision( - boxset0,boxset1,trans_cache_1to0, - node0,node1,complete_primitive_tests) ==false) return;//avoid colliding internal nodes - - if(boxset0->isLeafNode(node0)) + if (boxset0->isLeafNode(node0)) { - if(boxset1->isLeafNode(node1)) + if (boxset1->isLeafNode(node1)) { // collision result collision_pairs->push_pair( - boxset0->getNodeData(node0),boxset1->getNodeData(node1)); + boxset0->getNodeData(node0), boxset1->getNodeData(node1)); return; } else { - //collide left recursive _find_quantized_collision_pairs_recursive( - boxset0,boxset1, - collision_pairs,trans_cache_1to0, - node0,boxset1->getLeftNode(node1),false); + boxset0, boxset1, + collision_pairs, trans_cache_1to0, + node0, boxset1->getLeftNode(node1), false); //collide right recursive _find_quantized_collision_pairs_recursive( - boxset0,boxset1, - collision_pairs,trans_cache_1to0, - node0,boxset1->getRightNode(node1),false); - - + boxset0, boxset1, + collision_pairs, trans_cache_1to0, + node0, boxset1->getRightNode(node1), false); } } else { - if(boxset1->isLeafNode(node1)) + if (boxset1->isLeafNode(node1)) { - //collide left recursive _find_quantized_collision_pairs_recursive( - boxset0,boxset1, - collision_pairs,trans_cache_1to0, - boxset0->getLeftNode(node0),node1,false); - + boxset0, boxset1, + collision_pairs, trans_cache_1to0, + boxset0->getLeftNode(node0), node1, false); //collide right recursive _find_quantized_collision_pairs_recursive( - boxset0,boxset1, - collision_pairs,trans_cache_1to0, - boxset0->getRightNode(node0),node1,false); - - + boxset0, boxset1, + collision_pairs, trans_cache_1to0, + boxset0->getRightNode(node0), node1, false); } else { //collide left0 left1 - - _find_quantized_collision_pairs_recursive( - boxset0,boxset1, - collision_pairs,trans_cache_1to0, - boxset0->getLeftNode(node0),boxset1->getLeftNode(node1),false); + boxset0, boxset1, + collision_pairs, trans_cache_1to0, + boxset0->getLeftNode(node0), boxset1->getLeftNode(node1), false); //collide left0 right1 _find_quantized_collision_pairs_recursive( - boxset0,boxset1, - collision_pairs,trans_cache_1to0, - boxset0->getLeftNode(node0),boxset1->getRightNode(node1),false); - + boxset0, boxset1, + collision_pairs, trans_cache_1to0, + boxset0->getLeftNode(node0), boxset1->getRightNode(node1), false); //collide right0 left1 _find_quantized_collision_pairs_recursive( - boxset0,boxset1, - collision_pairs,trans_cache_1to0, - boxset0->getRightNode(node0),boxset1->getLeftNode(node1),false); + boxset0, boxset1, + collision_pairs, trans_cache_1to0, + boxset0->getRightNode(node0), boxset1->getLeftNode(node1), false); //collide right0 right1 _find_quantized_collision_pairs_recursive( - boxset0,boxset1, - collision_pairs,trans_cache_1to0, - boxset0->getRightNode(node0),boxset1->getRightNode(node1),false); + boxset0, boxset1, + collision_pairs, trans_cache_1to0, + boxset0->getRightNode(node0), boxset1->getRightNode(node1), false); - }// else if node1 is not a leaf - }// else if node0 is not a leaf + } // else if node1 is not a leaf + } // else if node0 is not a leaf } - -void btGImpactQuantizedBvh::find_collision(const btGImpactQuantizedBvh * boxset0, const btTransform & trans0, - const btGImpactQuantizedBvh * boxset1, const btTransform & trans1, - btPairSet & collision_pairs) +void btGImpactQuantizedBvh::find_collision(const btGImpactQuantizedBvh* boxset0, const btTransform& trans0, + const btGImpactQuantizedBvh* boxset1, const btTransform& trans1, + btPairSet& collision_pairs) { - - if(boxset0->getNodeCount()==0 || boxset1->getNodeCount()==0 ) return; + if (boxset0->getNodeCount() == 0 || boxset1->getNodeCount() == 0) return; BT_BOX_BOX_TRANSFORM_CACHE trans_cache_1to0; - trans_cache_1to0.calc_from_homogenic(trans0,trans1); + trans_cache_1to0.calc_from_homogenic(trans0, trans1); #ifdef TRI_COLLISION_PROFILING bt_begin_gim02_q_tree_time(); -#endif //TRI_COLLISION_PROFILING +#endif //TRI_COLLISION_PROFILING _find_quantized_collision_pairs_recursive( - boxset0,boxset1, - &collision_pairs,trans_cache_1to0,0,0,true); + boxset0, boxset1, + &collision_pairs, trans_cache_1to0, 0, 0, true); #ifdef TRI_COLLISION_PROFILING bt_end_gim02_q_tree_time(); -#endif //TRI_COLLISION_PROFILING - +#endif //TRI_COLLISION_PROFILING } - - |