Changeset View
Changeset View
Standalone View
Standalone View
extern/bullet2/src/BulletCollision/Gimpact/btGImpactQuantizedBvh.cpp
| Show All 21 Lines | |||||
| */ | */ | ||||
| #include "btGImpactQuantizedBvh.h" | #include "btGImpactQuantizedBvh.h" | ||||
| #include "LinearMath/btQuickprof.h" | #include "LinearMath/btQuickprof.h" | ||||
| #ifdef TRI_COLLISION_PROFILING | #ifdef TRI_COLLISION_PROFILING | ||||
| btClock g_q_tree_clock; | btClock g_q_tree_clock; | ||||
| float g_q_accum_tree_collision_time = 0; | float g_q_accum_tree_collision_time = 0; | ||||
| int g_q_count_traversing = 0; | int g_q_count_traversing = 0; | ||||
| void bt_begin_gim02_q_tree_time() | void bt_begin_gim02_q_tree_time() | ||||
| { | { | ||||
| g_q_tree_clock.reset(); | g_q_tree_clock.reset(); | ||||
| } | } | ||||
| void bt_end_gim02_q_tree_time() | void bt_end_gim02_q_tree_time() | ||||
| { | { | ||||
| g_q_accum_tree_collision_time += g_q_tree_clock.getTimeMicroseconds(); | g_q_accum_tree_collision_time += g_q_tree_clock.getTimeMicroseconds(); | ||||
| g_q_count_traversing++; | g_q_count_traversing++; | ||||
| } | } | ||||
| //! Gets the average time in miliseconds of tree collisions | //! Gets the average time in miliseconds of tree collisions | ||||
| float btGImpactQuantizedBvh::getAverageTreeCollisionTime() | 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; | float avgtime = g_q_accum_tree_collision_time; | ||||
| avgtime /= (float)g_q_count_traversing; | avgtime /= (float)g_q_count_traversing; | ||||
| g_q_accum_tree_collision_time = 0; | g_q_accum_tree_collision_time = 0; | ||||
| g_q_count_traversing = 0; | g_q_count_traversing = 0; | ||||
| return avgtime; | return avgtime; | ||||
| // float avgtime = g_q_count_traversing; | // float avgtime = g_q_count_traversing; | ||||
| // g_q_count_traversing = 0; | // g_q_count_traversing = 0; | ||||
| // return avgtime; | // return avgtime; | ||||
| } | } | ||||
| #endif //TRI_COLLISION_PROFILING | #endif //TRI_COLLISION_PROFILING | ||||
| /////////////////////// btQuantizedBvhTree ///////////////////////////////// | /////////////////////// btQuantizedBvhTree ///////////////////////////////// | ||||
| void btQuantizedBvhTree::calc_quantization( | void btQuantizedBvhTree::calc_quantization( | ||||
| GIM_BVH_DATA_ARRAY & primitive_boxes, btScalar boundMargin) | GIM_BVH_DATA_ARRAY& primitive_boxes, btScalar boundMargin) | ||||
| { | { | ||||
| //calc globa box | //calc globa box | ||||
| btAABB global_bound; | btAABB global_bound; | ||||
| global_bound.invalidate(); | 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); | global_bound.merge(primitive_boxes[i].m_bound); | ||||
| } | } | ||||
| bt_calc_quantization_parameters( | 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( | 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; | int i; | ||||
| btVector3 means(btScalar(0.),btScalar(0.),btScalar(0.)); | btVector3 means(btScalar(0.), btScalar(0.), btScalar(0.)); | ||||
| btVector3 variance(btScalar(0.),btScalar(0.),btScalar(0.)); | btVector3 variance(btScalar(0.), btScalar(0.), btScalar(0.)); | ||||
| int numIndices = endIndex-startIndex; | 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 + | btVector3 center = btScalar(0.5) * (primitive_boxes[i].m_bound.m_max + | ||||
| primitive_boxes[i].m_bound.m_min); | primitive_boxes[i].m_bound.m_min); | ||||
| means+=center; | 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 + | btVector3 center = btScalar(0.5) * (primitive_boxes[i].m_bound.m_max + | ||||
| primitive_boxes[i].m_bound.m_min); | primitive_boxes[i].m_bound.m_min); | ||||
| btVector3 diff2 = center-means; | btVector3 diff2 = center - means; | ||||
| diff2 = diff2 * diff2; | diff2 = diff2 * diff2; | ||||
| variance += diff2; | variance += diff2; | ||||
| } | } | ||||
| variance *= (btScalar(1.)/ ((btScalar)numIndices-1) ); | variance *= (btScalar(1.) / ((btScalar)numIndices - 1)); | ||||
| return variance.maxAxis(); | return variance.maxAxis(); | ||||
| } | } | ||||
| int btQuantizedBvhTree::_sort_and_calc_splitting_index( | 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 endIndex, int splitAxis) | ||||
| { | { | ||||
| int i; | int i; | ||||
| int splitIndex =startIndex; | int splitIndex = startIndex; | ||||
| int numIndices = endIndex - startIndex; | int numIndices = endIndex - startIndex; | ||||
| // average of centers | // average of centers | ||||
| btScalar splitValue = 0.0f; | btScalar splitValue = 0.0f; | ||||
| btVector3 means(btScalar(0.),btScalar(0.),btScalar(0.)); | btVector3 means(btScalar(0.), btScalar(0.), btScalar(0.)); | ||||
| for (i=startIndex;i<endIndex;i++) | for (i = startIndex; i < endIndex; i++) | ||||
| { | { | ||||
| btVector3 center = btScalar(0.5)*(primitive_boxes[i].m_bound.m_max + | btVector3 center = btScalar(0.5) * (primitive_boxes[i].m_bound.m_max + | ||||
| primitive_boxes[i].m_bound.m_min); | primitive_boxes[i].m_bound.m_min); | ||||
| means+=center; | means += center; | ||||
| } | } | ||||
| means *= (btScalar(1.)/(btScalar)numIndices); | means *= (btScalar(1.) / (btScalar)numIndices); | ||||
| splitValue = means[splitAxis]; | splitValue = means[splitAxis]; | ||||
| //sort leafNodes so all values larger then splitValue comes first, and smaller values start from 'splitIndex'. | //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 + | btVector3 center = btScalar(0.5) * (primitive_boxes[i].m_bound.m_max + | ||||
| primitive_boxes[i].m_bound.m_min); | primitive_boxes[i].m_bound.m_min); | ||||
| if (center[splitAxis] > splitValue) | if (center[splitAxis] > splitValue) | ||||
| { | { | ||||
| //swap | //swap | ||||
| primitive_boxes.swap(i,splitIndex); | primitive_boxes.swap(i, splitIndex); | ||||
| //swapLeafNodes(i,splitIndex); | //swapLeafNodes(i,splitIndex); | ||||
| splitIndex++; | splitIndex++; | ||||
| } | } | ||||
| } | } | ||||
| //if the splitIndex causes unbalanced trees, fix this by using the center in between startIndex and endIndex | //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. | //otherwise the tree-building might fail due to stack-overflows in certain cases. | ||||
| //unbalanced1 is unsafe: it can cause stack overflows | //unbalanced1 is unsafe: it can cause stack overflows | ||||
| //bool unbalanced1 = ((splitIndex==startIndex) || (splitIndex == (endIndex-1))); | //bool unbalanced1 = ((splitIndex==startIndex) || (splitIndex == (endIndex-1))); | ||||
| //unbalanced2 should work too: always use center (perfect balanced trees) | //unbalanced2 should work too: always use center (perfect balanced trees) | ||||
| //bool unbalanced2 = true; | //bool unbalanced2 = true; | ||||
| //this should be safe too: | //this should be safe too: | ||||
| int rangeBalancedIndices = numIndices/3; | int rangeBalancedIndices = numIndices / 3; | ||||
| bool unbalanced = ((splitIndex<=(startIndex+rangeBalancedIndices)) || (splitIndex >=(endIndex-1-rangeBalancedIndices))); | bool unbalanced = ((splitIndex <= (startIndex + rangeBalancedIndices)) || (splitIndex >= (endIndex - 1 - rangeBalancedIndices))); | ||||
| if (unbalanced) | if (unbalanced) | ||||
| { | { | ||||
| splitIndex = startIndex+ (numIndices>>1); | splitIndex = startIndex + (numIndices >> 1); | ||||
| } | } | ||||
| btAssert(!((splitIndex==startIndex) || (splitIndex == (endIndex)))); | btAssert(!((splitIndex == startIndex) || (splitIndex == (endIndex)))); | ||||
| return splitIndex; | 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; | int curIndex = m_num_nodes; | ||||
| 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 | //We have a leaf node | ||||
| setNodeBound(curIndex,primitive_boxes[startIndex].m_bound); | setNodeBound(curIndex, primitive_boxes[startIndex].m_bound); | ||||
| m_node_array[curIndex].setDataIndex(primitive_boxes[startIndex].m_data); | m_node_array[curIndex].setDataIndex(primitive_boxes[startIndex].m_data); | ||||
| return; | return; | ||||
| } | } | ||||
| //calculate Best Splitting Axis and where to split it. Sort the incoming 'leafNodes' array within range 'startIndex/endIndex'. | //calculate Best Splitting Axis and where to split it. Sort the incoming 'leafNodes' array within range 'startIndex/endIndex'. | ||||
| //split axis | //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( | splitIndex = _sort_and_calc_splitting_index( | ||||
| primitive_boxes,startIndex,endIndex, | primitive_boxes, startIndex, endIndex, | ||||
| splitIndex//split axis | splitIndex //split axis | ||||
| ); | ); | ||||
| //calc this node bounding box | //calc this node bounding box | ||||
| btAABB node_bound; | btAABB node_bound; | ||||
| node_bound.invalidate(); | 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); | node_bound.merge(primitive_boxes[i].m_bound); | ||||
| } | } | ||||
| setNodeBound(curIndex,node_bound); | setNodeBound(curIndex, node_bound); | ||||
| //build left branch | //build left branch | ||||
| _build_sub_tree(primitive_boxes, startIndex, splitIndex ); | _build_sub_tree(primitive_boxes, startIndex, splitIndex); | ||||
| //build right branch | //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); | m_node_array[curIndex].setEscapeIndex(m_num_nodes - curIndex); | ||||
| } | } | ||||
| //! stackless build tree | //! stackless build tree | ||||
| void btQuantizedBvhTree::build_tree( | void btQuantizedBvhTree::build_tree( | ||||
| GIM_BVH_DATA_ARRAY & primitive_boxes) | GIM_BVH_DATA_ARRAY& primitive_boxes) | ||||
| { | { | ||||
| calc_quantization(primitive_boxes); | calc_quantization(primitive_boxes); | ||||
| // initialize node count to 0 | // initialize node count to 0 | ||||
| m_num_nodes = 0; | m_num_nodes = 0; | ||||
| // allocate nodes | // 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()); | _build_sub_tree(primitive_boxes, 0, primitive_boxes.size()); | ||||
| } | } | ||||
| ////////////////////////////////////class btGImpactQuantizedBvh | ////////////////////////////////////class btGImpactQuantizedBvh | ||||
| void btGImpactQuantizedBvh::refit() | void btGImpactQuantizedBvh::refit() | ||||
| { | { | ||||
| int nodecount = getNodeCount(); | int nodecount = getNodeCount(); | ||||
| while(nodecount--) | while (nodecount--) | ||||
| { | { | ||||
| if(isLeafNode(nodecount)) | if (isLeafNode(nodecount)) | ||||
| { | { | ||||
| btAABB leafbox; | btAABB leafbox; | ||||
| m_primitive_manager->get_primitive_box(getNodeData(nodecount),leafbox); | m_primitive_manager->get_primitive_box(getNodeData(nodecount), leafbox); | ||||
| setNodeBound(nodecount,leafbox); | setNodeBound(nodecount, leafbox); | ||||
| } | } | ||||
| else | else | ||||
| { | { | ||||
| //const GIM_BVH_TREE_NODE * nodepointer = get_node_pointer(nodecount); | //const GIM_BVH_TREE_NODE * nodepointer = get_node_pointer(nodecount); | ||||
| //get left bound | //get left bound | ||||
| btAABB bound; | btAABB bound; | ||||
| bound.invalidate(); | bound.invalidate(); | ||||
| btAABB temp_box; | btAABB temp_box; | ||||
| int child_node = getLeftNode(nodecount); | 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); | bound.merge(temp_box); | ||||
| } | } | ||||
| child_node = getRightNode(nodecount); | child_node = getRightNode(nodecount); | ||||
| if(child_node) | if (child_node) | ||||
| { | { | ||||
| getNodeBound(child_node,temp_box); | getNodeBound(child_node, temp_box); | ||||
| bound.merge(temp_box); | bound.merge(temp_box); | ||||
| } | } | ||||
| setNodeBound(nodecount,bound); | setNodeBound(nodecount, bound); | ||||
| } | } | ||||
| } | } | ||||
| } | } | ||||
| //! this rebuild the entire set | //! this rebuild the entire set | ||||
| void btGImpactQuantizedBvh::buildSet() | void btGImpactQuantizedBvh::buildSet() | ||||
| { | { | ||||
| //obtain primitive boxes | //obtain primitive boxes | ||||
| GIM_BVH_DATA_ARRAY primitive_boxes; | GIM_BVH_DATA_ARRAY primitive_boxes; | ||||
| primitive_boxes.resize(m_primitive_manager->get_primitive_count()); | 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); | m_primitive_manager->get_primitive_box(i, primitive_boxes[i].m_bound); | ||||
| primitive_boxes[i].m_data = i; | primitive_boxes[i].m_data = i; | ||||
| } | } | ||||
| m_box_tree.build_tree(primitive_boxes); | m_box_tree.build_tree(primitive_boxes); | ||||
| } | } | ||||
| //! returns the indices of the primitives in the m_primitive_manager | //! 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 curIndex = 0; | ||||
| int numNodes = getNodeCount(); | int numNodes = getNodeCount(); | ||||
| //quantize box | //quantize box | ||||
| unsigned short quantizedMin[3]; | unsigned short quantizedMin[3]; | ||||
| unsigned short quantizedMax[3]; | unsigned short quantizedMax[3]; | ||||
| m_box_tree.quantizePoint(quantizedMin,box.m_min); | m_box_tree.quantizePoint(quantizedMin, box.m_min); | ||||
| m_box_tree.quantizePoint(quantizedMax,box.m_max); | m_box_tree.quantizePoint(quantizedMax, box.m_max); | ||||
| while (curIndex < numNodes) | while (curIndex < numNodes) | ||||
| { | { | ||||
| //catch bugs in tree data | //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); | bool isleafnode = isLeafNode(curIndex); | ||||
| if (isleafnode && aabbOverlap) | if (isleafnode && aabbOverlap) | ||||
| { | { | ||||
| collided_results.push_back(getNodeData(curIndex)); | collided_results.push_back(getNodeData(curIndex)); | ||||
| } | } | ||||
| if (aabbOverlap || isleafnode) | if (aabbOverlap || isleafnode) | ||||
| { | { | ||||
| //next subnode | //next subnode | ||||
| curIndex++; | curIndex++; | ||||
| } | } | ||||
| else | else | ||||
| { | { | ||||
| //skip node | //skip node | ||||
| curIndex+= getEscapeNodeIndex(curIndex); | curIndex += getEscapeNodeIndex(curIndex); | ||||
| } | } | ||||
| } | } | ||||
| if(collided_results.size()>0) return true; | if (collided_results.size() > 0) return true; | ||||
| return false; | return false; | ||||
| } | } | ||||
| //! returns the indices of the primitives in the m_primitive_manager | //! returns the indices of the primitives in the m_primitive_manager | ||||
| bool btGImpactQuantizedBvh::rayQuery( | bool btGImpactQuantizedBvh::rayQuery( | ||||
| const btVector3 & ray_dir,const btVector3 & ray_origin , | const btVector3& ray_dir, const btVector3& ray_origin, | ||||
| btAlignedObjectArray<int> & collided_results) const | btAlignedObjectArray<int>& collided_results) const | ||||
| { | { | ||||
| int curIndex = 0; | int curIndex = 0; | ||||
| int numNodes = getNodeCount(); | int numNodes = getNodeCount(); | ||||
| while (curIndex < numNodes) | while (curIndex < numNodes) | ||||
| { | { | ||||
| btAABB bound; | btAABB bound; | ||||
| getNodeBound(curIndex,bound); | getNodeBound(curIndex, bound); | ||||
| //catch bugs in tree data | //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); | bool isleafnode = isLeafNode(curIndex); | ||||
| if (isleafnode && aabbOverlap) | if (isleafnode && aabbOverlap) | ||||
| { | { | ||||
| collided_results.push_back(getNodeData( curIndex)); | collided_results.push_back(getNodeData(curIndex)); | ||||
| } | } | ||||
| if (aabbOverlap || isleafnode) | if (aabbOverlap || isleafnode) | ||||
| { | { | ||||
| //next subnode | //next subnode | ||||
| curIndex++; | curIndex++; | ||||
| } | } | ||||
| else | else | ||||
| { | { | ||||
| //skip node | //skip node | ||||
| curIndex+= getEscapeNodeIndex(curIndex); | curIndex += getEscapeNodeIndex(curIndex); | ||||
| } | } | ||||
| } | } | ||||
| if(collided_results.size()>0) return true; | if (collided_results.size() > 0) return true; | ||||
| return false; | return false; | ||||
| } | } | ||||
| SIMD_FORCE_INLINE bool _quantized_node_collision( | SIMD_FORCE_INLINE bool _quantized_node_collision( | ||||
| const btGImpactQuantizedBvh * boxset0, const btGImpactQuantizedBvh * boxset1, | const btGImpactQuantizedBvh* boxset0, const btGImpactQuantizedBvh* boxset1, | ||||
| const BT_BOX_BOX_TRANSFORM_CACHE & trans_cache_1to0, | const BT_BOX_BOX_TRANSFORM_CACHE& trans_cache_1to0, | ||||
| int node0 ,int node1, bool complete_primitive_tests) | int node0, int node1, bool complete_primitive_tests) | ||||
| { | { | ||||
| btAABB box0; | btAABB box0; | ||||
| boxset0->getNodeBound(node0,box0); | boxset0->getNodeBound(node0, box0); | ||||
| btAABB box1; | btAABB box1; | ||||
| boxset1->getNodeBound(node1,box1); | boxset1->getNodeBound(node1, box1); | ||||
| return box0.overlapping_trans_cache(box1,trans_cache_1to0,complete_primitive_tests ); | return box0.overlapping_trans_cache(box1, trans_cache_1to0, complete_primitive_tests); | ||||
| // box1.appy_transform_trans_cache(trans_cache_1to0); | // box1.appy_transform_trans_cache(trans_cache_1to0); | ||||
| // return box0.has_collision(box1); | // return box0.has_collision(box1); | ||||
| } | } | ||||
| //stackless recursive collision routine | //stackless recursive collision routine | ||||
| static void _find_quantized_collision_pairs_recursive( | static void _find_quantized_collision_pairs_recursive( | ||||
| const btGImpactQuantizedBvh * boxset0, const btGImpactQuantizedBvh * boxset1, | const btGImpactQuantizedBvh* boxset0, const btGImpactQuantizedBvh* boxset1, | ||||
| btPairSet * collision_pairs, | btPairSet* collision_pairs, | ||||
| const BT_BOX_BOX_TRANSFORM_CACHE & trans_cache_1to0, | const BT_BOX_BOX_TRANSFORM_CACHE& trans_cache_1to0, | ||||
| int node0, int node1, bool complete_primitive_tests) | int node0, int node1, bool complete_primitive_tests) | ||||
| { | { | ||||
| if( _quantized_node_collision( | if (_quantized_node_collision( | ||||
| boxset0,boxset1,trans_cache_1to0, | boxset0, boxset1, trans_cache_1to0, | ||||
| node0,node1,complete_primitive_tests) ==false) return;//avoid colliding internal nodes | 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 result | ||||
| collision_pairs->push_pair( | collision_pairs->push_pair( | ||||
| boxset0->getNodeData(node0),boxset1->getNodeData(node1)); | boxset0->getNodeData(node0), boxset1->getNodeData(node1)); | ||||
| return; | return; | ||||
| } | } | ||||
| else | else | ||||
| { | { | ||||
| //collide left recursive | //collide left recursive | ||||
| _find_quantized_collision_pairs_recursive( | _find_quantized_collision_pairs_recursive( | ||||
| boxset0,boxset1, | boxset0, boxset1, | ||||
| collision_pairs,trans_cache_1to0, | collision_pairs, trans_cache_1to0, | ||||
| node0,boxset1->getLeftNode(node1),false); | node0, boxset1->getLeftNode(node1), false); | ||||
| //collide right recursive | //collide right recursive | ||||
| _find_quantized_collision_pairs_recursive( | _find_quantized_collision_pairs_recursive( | ||||
| boxset0,boxset1, | boxset0, boxset1, | ||||
| collision_pairs,trans_cache_1to0, | collision_pairs, trans_cache_1to0, | ||||
| node0,boxset1->getRightNode(node1),false); | node0, boxset1->getRightNode(node1), false); | ||||
| } | } | ||||
| } | } | ||||
| else | else | ||||
| { | { | ||||
| if(boxset1->isLeafNode(node1)) | if (boxset1->isLeafNode(node1)) | ||||
| { | { | ||||
| //collide left recursive | //collide left recursive | ||||
| _find_quantized_collision_pairs_recursive( | _find_quantized_collision_pairs_recursive( | ||||
| boxset0,boxset1, | boxset0, boxset1, | ||||
| collision_pairs,trans_cache_1to0, | collision_pairs, trans_cache_1to0, | ||||
| boxset0->getLeftNode(node0),node1,false); | boxset0->getLeftNode(node0), node1, false); | ||||
| //collide right recursive | //collide right recursive | ||||
| _find_quantized_collision_pairs_recursive( | _find_quantized_collision_pairs_recursive( | ||||
| boxset0,boxset1, | boxset0, boxset1, | ||||
| collision_pairs,trans_cache_1to0, | collision_pairs, trans_cache_1to0, | ||||
| boxset0->getRightNode(node0),node1,false); | boxset0->getRightNode(node0), node1, false); | ||||
| } | } | ||||
| else | else | ||||
| { | { | ||||
| //collide left0 left1 | //collide left0 left1 | ||||
| _find_quantized_collision_pairs_recursive( | _find_quantized_collision_pairs_recursive( | ||||
| boxset0,boxset1, | boxset0, boxset1, | ||||
| collision_pairs,trans_cache_1to0, | collision_pairs, trans_cache_1to0, | ||||
| boxset0->getLeftNode(node0),boxset1->getLeftNode(node1),false); | boxset0->getLeftNode(node0), boxset1->getLeftNode(node1), false); | ||||
| //collide left0 right1 | //collide left0 right1 | ||||
| _find_quantized_collision_pairs_recursive( | _find_quantized_collision_pairs_recursive( | ||||
| boxset0,boxset1, | boxset0, boxset1, | ||||
| collision_pairs,trans_cache_1to0, | collision_pairs, trans_cache_1to0, | ||||
| boxset0->getLeftNode(node0),boxset1->getRightNode(node1),false); | boxset0->getLeftNode(node0), boxset1->getRightNode(node1), false); | ||||
| //collide right0 left1 | //collide right0 left1 | ||||
| _find_quantized_collision_pairs_recursive( | _find_quantized_collision_pairs_recursive( | ||||
| boxset0,boxset1, | boxset0, boxset1, | ||||
| collision_pairs,trans_cache_1to0, | collision_pairs, trans_cache_1to0, | ||||
| boxset0->getRightNode(node0),boxset1->getLeftNode(node1),false); | boxset0->getRightNode(node0), boxset1->getLeftNode(node1), false); | ||||
| //collide right0 right1 | //collide right0 right1 | ||||
| _find_quantized_collision_pairs_recursive( | _find_quantized_collision_pairs_recursive( | ||||
| boxset0,boxset1, | boxset0, boxset1, | ||||
| collision_pairs,trans_cache_1to0, | collision_pairs, trans_cache_1to0, | ||||
| boxset0->getRightNode(node0),boxset1->getRightNode(node1),false); | boxset0->getRightNode(node0), boxset1->getRightNode(node1), false); | ||||
| }// else if node1 is not a leaf | } // else if node1 is not a leaf | ||||
| }// else if node0 is not a leaf | } // else if node0 is not a leaf | ||||
| } | } | ||||
| void btGImpactQuantizedBvh::find_collision(const btGImpactQuantizedBvh * boxset0, const btTransform & trans0, | void btGImpactQuantizedBvh::find_collision(const btGImpactQuantizedBvh* boxset0, const btTransform& trans0, | ||||
| const btGImpactQuantizedBvh * boxset1, const btTransform & trans1, | const btGImpactQuantizedBvh* boxset1, const btTransform& trans1, | ||||
| btPairSet & collision_pairs) | 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; | 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 | #ifdef TRI_COLLISION_PROFILING | ||||
| bt_begin_gim02_q_tree_time(); | bt_begin_gim02_q_tree_time(); | ||||
| #endif //TRI_COLLISION_PROFILING | #endif //TRI_COLLISION_PROFILING | ||||
| _find_quantized_collision_pairs_recursive( | _find_quantized_collision_pairs_recursive( | ||||
| boxset0,boxset1, | boxset0, boxset1, | ||||
| &collision_pairs,trans_cache_1to0,0,0,true); | &collision_pairs, trans_cache_1to0, 0, 0, true); | ||||
| #ifdef TRI_COLLISION_PROFILING | #ifdef TRI_COLLISION_PROFILING | ||||
| bt_end_gim02_q_tree_time(); | bt_end_gim02_q_tree_time(); | ||||
| #endif //TRI_COLLISION_PROFILING | #endif //TRI_COLLISION_PROFILING | ||||
| } | } | ||||