diff options
Diffstat (limited to 'thirdparty/bullet/src/BulletCollision/Gimpact/btGImpactQuantizedBvh.cpp')
| -rw-r--r-- | thirdparty/bullet/src/BulletCollision/Gimpact/btGImpactQuantizedBvh.cpp | 528 |
1 files changed, 0 insertions, 528 deletions
diff --git a/thirdparty/bullet/src/BulletCollision/Gimpact/btGImpactQuantizedBvh.cpp b/thirdparty/bullet/src/BulletCollision/Gimpact/btGImpactQuantizedBvh.cpp deleted file mode 100644 index 4528758c37..0000000000 --- a/thirdparty/bullet/src/BulletCollision/Gimpact/btGImpactQuantizedBvh.cpp +++ /dev/null @@ -1,528 +0,0 @@ -/*! \file gim_box_set.h -\author Francisco Leon Najera -*/ -/* -This source file is part of GIMPACT Library. - -For the latest info, see http://gimpact.sourceforge.net/ - -Copyright (c) 2007 Francisco Leon Najera. C.C. 80087371. -email: projectileman@yahoo.com - - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -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. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -#include "btGImpactQuantizedBvh.h" -#include "LinearMath/btQuickprof.h" - -#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(); -} - -void bt_end_gim02_q_tree_time() -{ - g_q_accum_tree_collision_time += g_q_tree_clock.getTimeMicroseconds(); - g_q_count_traversing++; -} - - -//! Gets the average time in miliseconds of tree collisions -float btGImpactQuantizedBvh::getAverageTreeCollisionTime() -{ - if(g_q_count_traversing == 0) return 0; - - float avgtime = g_q_accum_tree_collision_time; - avgtime /= (float)g_q_count_traversing; - - g_q_accum_tree_collision_time = 0; - g_q_count_traversing = 0; - return avgtime; - -// float avgtime = g_q_count_traversing; -// g_q_count_traversing = 0; -// return avgtime; - -} - -#endif //TRI_COLLISION_PROFILING - -/////////////////////// btQuantizedBvhTree ///////////////////////////////// - -void btQuantizedBvhTree::calc_quantization( - 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++ ) - { - 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); - -} - - - -int btQuantizedBvhTree::_calc_splitting_axis( - 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; - - 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; - } - means *= (btScalar(1.)/(btScalar)numIndices); - - 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; - diff2 = diff2 * diff2; - variance += diff2; - } - variance *= (btScalar(1.)/ ((btScalar)numIndices-1) ); - - return variance.maxAxis(); -} - - -int btQuantizedBvhTree::_sort_and_calc_splitting_index( - GIM_BVH_DATA_ARRAY & primitive_boxes, int startIndex, - int endIndex, int splitAxis) -{ - int i; - 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 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); - - 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++) - { - 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); - //swapLeafNodes(i,splitIndex); - splitIndex++; - } - } - - //if the splitIndex causes unbalanced trees, fix this by using the center in between startIndex and endIndex - //otherwise the tree-building might fail due to stack-overflows in certain cases. - //unbalanced1 is unsafe: it can cause stack overflows - //bool unbalanced1 = ((splitIndex==startIndex) || (splitIndex == (endIndex-1))); - - //unbalanced2 should work too: always use center (perfect balanced trees) - //bool unbalanced2 = true; - - //this should be safe too: - int rangeBalancedIndices = numIndices/3; - bool unbalanced = ((splitIndex<=(startIndex+rangeBalancedIndices)) || (splitIndex >=(endIndex-1-rangeBalancedIndices))); - - if (unbalanced) - { - splitIndex = startIndex+ (numIndices>>1); - } - - btAssert(!((splitIndex==startIndex) || (splitIndex == (endIndex)))); - - return splitIndex; - -} - - -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); - - if ((endIndex-startIndex)==1) - { - //We have a leaf node - setNodeBound(curIndex,primitive_boxes[startIndex].m_bound); - m_node_array[curIndex].setDataIndex(primitive_boxes[startIndex].m_data); - - return; - } - //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); - - splitIndex = _sort_and_calc_splitting_index( - 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++) - { - node_bound.merge(primitive_boxes[i].m_bound); - } - - setNodeBound(curIndex,node_bound); - - - //build left branch - _build_sub_tree(primitive_boxes, startIndex, splitIndex ); - - - //build right branch - _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) -{ - calc_quantization(primitive_boxes); - // initialize node count to 0 - m_num_nodes = 0; - // allocate nodes - m_node_array.resize(primitive_boxes.size()*2); - - _build_sub_tree(primitive_boxes, 0, primitive_boxes.size()); -} - -////////////////////////////////////class btGImpactQuantizedBvh - -void btGImpactQuantizedBvh::refit() -{ - int nodecount = getNodeCount(); - while(nodecount--) - { - if(isLeafNode(nodecount)) - { - btAABB leafbox; - m_primitive_manager->get_primitive_box(getNodeData(nodecount),leafbox); - setNodeBound(nodecount,leafbox); - } - else - { - //const GIM_BVH_TREE_NODE * nodepointer = get_node_pointer(nodecount); - //get left bound - btAABB bound; - bound.invalidate(); - - btAABB temp_box; - - int child_node = getLeftNode(nodecount); - if(child_node) - { - getNodeBound(child_node,temp_box); - bound.merge(temp_box); - } - - child_node = getRightNode(nodecount); - if(child_node) - { - getNodeBound(child_node,temp_box); - bound.merge(temp_box); - } - - setNodeBound(nodecount,bound); - } - } -} - -//! this rebuild the entire set -void btGImpactQuantizedBvh::buildSet() -{ - //obtain primitive boxes - GIM_BVH_DATA_ARRAY primitive_boxes; - primitive_boxes.resize(m_primitive_manager->get_primitive_count()); - - 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_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 -{ - int curIndex = 0; - int numNodes = getNodeCount(); - - //quantize box - - unsigned short quantizedMin[3]; - unsigned short quantizedMax[3]; - - 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 isleafnode = isLeafNode(curIndex); - - if (isleafnode && aabbOverlap) - { - collided_results.push_back(getNodeData(curIndex)); - } - - if (aabbOverlap || isleafnode) - { - //next subnode - curIndex++; - } - else - { - //skip node - curIndex+= getEscapeNodeIndex(curIndex); - } - } - 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 -{ - int curIndex = 0; - int numNodes = getNodeCount(); - - while (curIndex < numNodes) - { - btAABB bound; - getNodeBound(curIndex,bound); - - //catch bugs in tree data - - bool aabbOverlap = bound.collide_ray(ray_origin,ray_dir); - bool isleafnode = isLeafNode(curIndex); - - if (isleafnode && aabbOverlap) - { - collided_results.push_back(getNodeData( curIndex)); - } - - if (aabbOverlap || isleafnode) - { - //next subnode - curIndex++; - } - else - { - //skip node - curIndex+= getEscapeNodeIndex(curIndex); - } - } - 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) -{ - btAABB 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); - -} - - -//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, - 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(boxset0->isLeafNode(node0)) - { - if(boxset1->isLeafNode(node1)) - { - // collision result - collision_pairs->push_pair( - 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); - - //collide right recursive - _find_quantized_collision_pairs_recursive( - boxset0,boxset1, - collision_pairs,trans_cache_1to0, - node0,boxset1->getRightNode(node1),false); - - - } - } - else - { - if(boxset1->isLeafNode(node1)) - { - - //collide left recursive - _find_quantized_collision_pairs_recursive( - 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); - - - } - else - { - //collide left0 left1 - - - - _find_quantized_collision_pairs_recursive( - 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); - - - //collide right0 left1 - - _find_quantized_collision_pairs_recursive( - 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); - - }// 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) -{ - - if(boxset0->getNodeCount()==0 || boxset1->getNodeCount()==0 ) return; - - BT_BOX_BOX_TRANSFORM_CACHE trans_cache_1to0; - - trans_cache_1to0.calc_from_homogenic(trans0,trans1); - -#ifdef TRI_COLLISION_PROFILING - bt_begin_gim02_q_tree_time(); -#endif //TRI_COLLISION_PROFILING - - _find_quantized_collision_pairs_recursive( - boxset0,boxset1, - &collision_pairs,trans_cache_1to0,0,0,true); -#ifdef TRI_COLLISION_PROFILING - bt_end_gim02_q_tree_time(); -#endif //TRI_COLLISION_PROFILING - -} - - |