forked from LeenkxTeam/LNXSDK
529 lines
13 KiB
C++
529 lines
13 KiB
C++
/*! \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
|
|
|
|
}
|
|
|
|
|