initial upgrade to GIMPACT 0.3, thanks to Francisco Leon for the update. GIMPACT demo/build system will be fixed today.
This commit is contained in:
ejcoumans
180
Extras/GIMPACT/src/Bullet/btContactProcessing.cpp
Normal file
180
Extras/GIMPACT/src/Bullet/btContactProcessing.cpp
Normal file
@@ -0,0 +1,180 @@
|
||||
|
||||
/*
|
||||
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 "GIMPACT/Bullet/btContactProcessing.h"
|
||||
|
||||
#define MAX_COINCIDENT 8
|
||||
|
||||
struct CONTACT_KEY_TOKEN
|
||||
{
|
||||
unsigned int m_key;
|
||||
int m_value;
|
||||
CONTACT_KEY_TOKEN()
|
||||
{
|
||||
}
|
||||
|
||||
CONTACT_KEY_TOKEN(unsigned int key,int token)
|
||||
{
|
||||
m_key = key;
|
||||
m_value = token;
|
||||
}
|
||||
|
||||
CONTACT_KEY_TOKEN(const CONTACT_KEY_TOKEN& rtoken)
|
||||
{
|
||||
m_key = rtoken.m_key;
|
||||
m_value = rtoken.m_value;
|
||||
}
|
||||
|
||||
inline bool operator <(const CONTACT_KEY_TOKEN& other) const
|
||||
{
|
||||
return (m_key < other.m_key);
|
||||
}
|
||||
|
||||
inline bool operator >(const CONTACT_KEY_TOKEN& other) const
|
||||
{
|
||||
return (m_key > other.m_key);
|
||||
}
|
||||
|
||||
};
|
||||
|
||||
class CONTACT_KEY_TOKEN_COMP
|
||||
{
|
||||
public:
|
||||
|
||||
bool operator() ( const CONTACT_KEY_TOKEN& a, const CONTACT_KEY_TOKEN& b )
|
||||
{
|
||||
return ( a < b );
|
||||
}
|
||||
};
|
||||
|
||||
|
||||
void btContactArray::merge_contacts(
|
||||
const btContactArray & contacts, bool normal_contact_average)
|
||||
{
|
||||
clear();
|
||||
|
||||
if(contacts.size()==0) return;
|
||||
|
||||
|
||||
if(contacts.size()==1)
|
||||
{
|
||||
push_back(contacts[0]);
|
||||
return;
|
||||
}
|
||||
|
||||
btAlignedObjectArray<CONTACT_KEY_TOKEN> keycontacts;
|
||||
|
||||
keycontacts.reserve(contacts.size());
|
||||
|
||||
//fill key contacts
|
||||
|
||||
for (int i = 0;i<contacts.size() ;i++ )
|
||||
{
|
||||
keycontacts.push_back(CONTACT_KEY_TOKEN(contacts[i].calc_key_contact(),i));
|
||||
}
|
||||
|
||||
//sort keys
|
||||
keycontacts.heapSort(CONTACT_KEY_TOKEN_COMP());
|
||||
|
||||
// Merge contacts
|
||||
int coincident_count=0;
|
||||
btVector3 coincident_normals[MAX_COINCIDENT];
|
||||
|
||||
unsigned int last_key = keycontacts[0].m_key;
|
||||
unsigned int key = 0;
|
||||
|
||||
push_back(contacts[keycontacts[0].m_value]);
|
||||
|
||||
BT_CONTACT * pcontact = &(*this)[0];
|
||||
|
||||
for(int i=1;i<keycontacts.size();i++)
|
||||
{
|
||||
key = keycontacts[i].m_key;
|
||||
const BT_CONTACT * scontact = &contacts[keycontacts[i].m_value];
|
||||
|
||||
if(last_key == key)//same points
|
||||
{
|
||||
//merge contact
|
||||
if(pcontact->m_depth - CONTACT_DIFF_EPSILON > scontact->m_depth)//)
|
||||
{
|
||||
*pcontact = *scontact;
|
||||
coincident_count = 0;
|
||||
}
|
||||
else if(normal_contact_average)
|
||||
{
|
||||
if(btFabs(pcontact->m_depth - scontact->m_depth)<CONTACT_DIFF_EPSILON)
|
||||
{
|
||||
if(coincident_count<MAX_COINCIDENT)
|
||||
{
|
||||
coincident_normals[coincident_count] = scontact->m_normal;
|
||||
coincident_count++;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
else
|
||||
{//add new contact
|
||||
|
||||
if(normal_contact_average && coincident_count>0)
|
||||
{
|
||||
pcontact->interpolate_normals(coincident_normals,coincident_count);
|
||||
coincident_count = 0;
|
||||
}
|
||||
|
||||
push_back(*scontact);
|
||||
pcontact = &(*this)[this->size()-1];
|
||||
}
|
||||
last_key = key;
|
||||
}
|
||||
}
|
||||
|
||||
void btContactArray::merge_contacts_unique(const btContactArray & contacts)
|
||||
{
|
||||
clear();
|
||||
|
||||
if(contacts.size()==0) return;
|
||||
|
||||
if(contacts.size()==1)
|
||||
{
|
||||
push_back(contacts[0]);
|
||||
return;
|
||||
}
|
||||
|
||||
BT_CONTACT average_contact = contacts[0];
|
||||
|
||||
for (int i=1;i<contacts.size() ;i++ )
|
||||
{
|
||||
average_contact.m_point += contacts[i].m_point;
|
||||
average_contact.m_normal += contacts[i].m_normal * contacts[i].m_depth;
|
||||
}
|
||||
|
||||
//divide
|
||||
btScalar divide_average = 1.0f/((btScalar)contacts.size());
|
||||
|
||||
average_contact.m_point *= divide_average;
|
||||
|
||||
average_contact.m_normal *= divide_average;
|
||||
|
||||
average_contact.m_depth = average_contact.m_normal.length();
|
||||
|
||||
average_contact.m_normal /= average_contact.m_depth;
|
||||
|
||||
}
|
||||
|
||||
493
Extras/GIMPACT/src/Bullet/btGImpactBvh.cpp
Normal file
493
Extras/GIMPACT/src/Bullet/btGImpactBvh.cpp
Normal file
@@ -0,0 +1,493 @@
|
||||
/*! \file gim_box_set.h
|
||||
\author Francisco Le<4C>n N<>jera
|
||||
*/
|
||||
/*
|
||||
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 "GIMPACT/Bullet/btGImpactBvh.h"
|
||||
#include "LinearMath/btQuickprof.h"
|
||||
|
||||
btClock g_tree_clock;
|
||||
|
||||
|
||||
float g_accum_tree_collision_time = 0;
|
||||
int g_count_traversing = 0;
|
||||
|
||||
|
||||
void bt_begin_gim02_tree_time()
|
||||
{
|
||||
g_tree_clock.reset();
|
||||
}
|
||||
|
||||
void bt_end_gim02_tree_time()
|
||||
{
|
||||
g_accum_tree_collision_time += g_tree_clock.getTimeMicroseconds();
|
||||
g_count_traversing++;
|
||||
}
|
||||
|
||||
//! Gets the average time in miliseconds of tree collisions
|
||||
float btGImpactBvh::getAverageTreeCollisionTime()
|
||||
{
|
||||
if(g_count_traversing == 0) return 0;
|
||||
|
||||
float avgtime = g_accum_tree_collision_time;
|
||||
avgtime /= (float)g_count_traversing;
|
||||
|
||||
g_accum_tree_collision_time = 0;
|
||||
g_count_traversing = 0;
|
||||
return avgtime;
|
||||
|
||||
// float avgtime = g_count_traversing;
|
||||
// g_count_traversing = 0;
|
||||
// return avgtime;
|
||||
|
||||
}
|
||||
|
||||
|
||||
/////////////////////// btBvhTree /////////////////////////////////
|
||||
|
||||
int btBvhTree::_calc_splitting_axis(
|
||||
BT_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 btBvhTree::_sort_and_calc_splitting_index(
|
||||
BT_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);
|
||||
}
|
||||
|
||||
bool unbal = (splitIndex==startIndex) || (splitIndex == (endIndex));
|
||||
btAssert(!unbal);
|
||||
|
||||
return splitIndex;
|
||||
|
||||
}
|
||||
|
||||
|
||||
void btBvhTree::_build_sub_tree(BT_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 btBvhTree::build_tree(
|
||||
BT_BVH_DATA_ARRAY & 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 btGImpactBvh
|
||||
|
||||
void btGImpactBvh::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 BT_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 btGImpactBvh::buildSet()
|
||||
{
|
||||
//obtain primitive boxes
|
||||
BT_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 btGImpactBvh::boxQuery(const btAABB & box, 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.has_collision(box);
|
||||
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 btGImpactBvh::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 _node_collision(
|
||||
btGImpactBvh * boxset0, btGImpactBvh * 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_collision_pairs_recursive(
|
||||
btGImpactBvh * boxset0, btGImpactBvh * boxset1,
|
||||
btPairSet * collision_pairs,
|
||||
const BT_BOX_BOX_TRANSFORM_CACHE & trans_cache_1to0,
|
||||
int node0, int node1, bool complete_primitive_tests)
|
||||
{
|
||||
|
||||
|
||||
|
||||
if( _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_collision_pairs_recursive(
|
||||
boxset0,boxset1,
|
||||
collision_pairs,trans_cache_1to0,
|
||||
node0,boxset1->getLeftNode(node1),false);
|
||||
|
||||
//collide right recursive
|
||||
_find_collision_pairs_recursive(
|
||||
boxset0,boxset1,
|
||||
collision_pairs,trans_cache_1to0,
|
||||
node0,boxset1->getRightNode(node1),false);
|
||||
|
||||
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
if(boxset1->isLeafNode(node1))
|
||||
{
|
||||
|
||||
//collide left recursive
|
||||
_find_collision_pairs_recursive(
|
||||
boxset0,boxset1,
|
||||
collision_pairs,trans_cache_1to0,
|
||||
boxset0->getLeftNode(node0),node1,false);
|
||||
|
||||
|
||||
//collide right recursive
|
||||
|
||||
_find_collision_pairs_recursive(
|
||||
boxset0,boxset1,
|
||||
collision_pairs,trans_cache_1to0,
|
||||
boxset0->getRightNode(node0),node1,false);
|
||||
|
||||
|
||||
}
|
||||
else
|
||||
{
|
||||
//collide left0 left1
|
||||
|
||||
|
||||
|
||||
_find_collision_pairs_recursive(
|
||||
boxset0,boxset1,
|
||||
collision_pairs,trans_cache_1to0,
|
||||
boxset0->getLeftNode(node0),boxset1->getLeftNode(node1),false);
|
||||
|
||||
//collide left0 right1
|
||||
|
||||
_find_collision_pairs_recursive(
|
||||
boxset0,boxset1,
|
||||
collision_pairs,trans_cache_1to0,
|
||||
boxset0->getLeftNode(node0),boxset1->getRightNode(node1),false);
|
||||
|
||||
|
||||
//collide right0 left1
|
||||
|
||||
_find_collision_pairs_recursive(
|
||||
boxset0,boxset1,
|
||||
collision_pairs,trans_cache_1to0,
|
||||
boxset0->getRightNode(node0),boxset1->getLeftNode(node1),false);
|
||||
|
||||
//collide right0 right1
|
||||
|
||||
_find_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 btGImpactBvh::find_collision(btGImpactBvh * boxset0, const btTransform & trans0,
|
||||
btGImpactBvh * 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);
|
||||
|
||||
bt_begin_gim02_tree_time();
|
||||
|
||||
_find_collision_pairs_recursive(
|
||||
boxset0,boxset1,
|
||||
&collision_pairs,trans_cache_1to0,0,0,true);
|
||||
|
||||
bt_end_gim02_tree_time();
|
||||
}
|
||||
|
||||
File diff suppressed because it is too large
Load Diff
523
Extras/GIMPACT/src/Bullet/btGImpactQuantizedBvh.cpp
Normal file
523
Extras/GIMPACT/src/Bullet/btGImpactQuantizedBvh.cpp
Normal file
@@ -0,0 +1,523 @@
|
||||
/*! \file gim_box_set.h
|
||||
\author Francisco Le<4C>n N<>jera
|
||||
*/
|
||||
/*
|
||||
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 "GIMPACT/Bullet/btGImpactQuantizedBvh.h"
|
||||
#include "LinearMath/btQuickprof.h"
|
||||
|
||||
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;
|
||||
|
||||
}
|
||||
|
||||
|
||||
/////////////////////// btQuantizedBvhTree /////////////////////////////////
|
||||
|
||||
void btQuantizedBvhTree::calc_quantization(
|
||||
BT_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(
|
||||
BT_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(
|
||||
BT_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);
|
||||
}
|
||||
|
||||
bool unbal = (splitIndex==startIndex) || (splitIndex == (endIndex));
|
||||
btAssert(!unbal);
|
||||
|
||||
return splitIndex;
|
||||
|
||||
}
|
||||
|
||||
|
||||
void btQuantizedBvhTree::_build_sub_tree(BT_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(
|
||||
BT_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 BT_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
|
||||
BT_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(
|
||||
btGImpactQuantizedBvh * boxset0, 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(
|
||||
btGImpactQuantizedBvh * boxset0, 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(btGImpactQuantizedBvh * boxset0, const btTransform & trans0,
|
||||
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);
|
||||
|
||||
bt_begin_gim02_q_tree_time();
|
||||
|
||||
_find_quantized_collision_pairs_recursive(
|
||||
boxset0,boxset1,
|
||||
&collision_pairs,trans_cache_1to0,0,0,true);
|
||||
|
||||
bt_end_gim02_q_tree_time();
|
||||
}
|
||||
|
||||
|
||||
@@ -1,30 +1,21 @@
|
||||
/*
|
||||
-----------------------------------------------------------------------------
|
||||
This source file is part of GIMPACT Library.
|
||||
|
||||
For the latest info, see http://gimpact.sourceforge.net/
|
||||
|
||||
Copyright (c) 2006 Francisco Leon Najera. C.C. 80087371.
|
||||
Copyright (c) 2007 Francisco Leon Najera. C.C. 80087371.
|
||||
email: projectileman@yahoo.com
|
||||
|
||||
This library is free software; you can redistribute it and/or
|
||||
modify it under the terms of EITHER:
|
||||
(1) The GNU Lesser General Public License as published by the Free
|
||||
Software Foundation; either version 2.1 of the License, or (at
|
||||
your option) any later version. The text of the GNU Lesser
|
||||
General Public License is included with this library in the
|
||||
file GIMPACT-LICENSE-LGPL.TXT.
|
||||
(2) The BSD-style license that is included with this library in
|
||||
the file GIMPACT-LICENSE-BSD.TXT.
|
||||
(3) The zlib/libpng license that is included with this library in
|
||||
the file GIMPACT-LICENSE-ZLIB.TXT.
|
||||
|
||||
This library is distributed in the hope that it will be useful,
|
||||
but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files
|
||||
GIMPACT-LICENSE-LGPL.TXT, GIMPACT-LICENSE-ZLIB.TXT and GIMPACT-LICENSE-BSD.TXT for more details.
|
||||
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.
|
||||
*/
|
||||
|
||||
|
||||
@@ -36,18 +27,26 @@ email: projectileman@yahoo.com
|
||||
|
||||
void btGImpactCompoundShape::calculateLocalInertia(btScalar mass,btVector3& inertia)
|
||||
{
|
||||
|
||||
lockChildShapes();
|
||||
#ifdef CALC_EXACT_INERTIA
|
||||
inertia.setValue(0.f,0.f,0.f);
|
||||
|
||||
GUINT i = this->getNumChildShapes();
|
||||
GREAL shapemass = mass/btScalar(i);
|
||||
int i = this->getNumChildShapes();
|
||||
btScalar shapemass = mass/btScalar(i);
|
||||
|
||||
while(i--)
|
||||
{
|
||||
btVector3 temp_inertia;
|
||||
m_childShapes[i]->calculateLocalInertia(shapemass,temp_inertia);
|
||||
inertia = gim_inertia_add_transformed( inertia,temp_inertia,m_childTransforms[i]);
|
||||
if(childrenHasTransform())
|
||||
{
|
||||
inertia = gim_inertia_add_transformed( inertia,temp_inertia,m_childTransforms[i]);
|
||||
}
|
||||
else
|
||||
{
|
||||
inertia = gim_inertia_add_transformed( inertia,temp_inertia,btTransform::getIdentity());
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
#else
|
||||
@@ -65,20 +64,21 @@ void btGImpactCompoundShape::calculateLocalInertia(btScalar mass,btVector3& iner
|
||||
inertia = scaledmass * (btVector3(y2+z2,x2+z2,x2+y2));
|
||||
|
||||
#endif
|
||||
unlockChildShapes();
|
||||
}
|
||||
|
||||
|
||||
|
||||
void btGImpactMeshShapePart::calculateLocalInertia(btScalar mass,btVector3& inertia)
|
||||
{
|
||||
lock();
|
||||
lockChildShapes();
|
||||
|
||||
|
||||
#ifdef CALC_EXACT_INERTIA
|
||||
inertia.setValue(0.f,0.f,0.f);
|
||||
|
||||
GUINT i = this->getVertexCount();
|
||||
GREAL pointmass = mass/btScalar(i);
|
||||
int i = this->getVertexCount();
|
||||
btScalar pointmass = mass/btScalar(i);
|
||||
|
||||
while(i--)
|
||||
{
|
||||
@@ -104,7 +104,7 @@ void btGImpactMeshShapePart::calculateLocalInertia(btScalar mass,btVector3& iner
|
||||
|
||||
#endif
|
||||
|
||||
unlock();
|
||||
unlockChildShapes();
|
||||
}
|
||||
|
||||
void btGImpactMeshShape::calculateLocalInertia(btScalar mass,btVector3& inertia)
|
||||
@@ -113,8 +113,8 @@ void btGImpactMeshShape::calculateLocalInertia(btScalar mass,btVector3& inertia)
|
||||
#ifdef CALC_EXACT_INERTIA
|
||||
inertia.setValue(0.f,0.f,0.f);
|
||||
|
||||
GUINT i = this->getMeshPartCount();
|
||||
GREAL partmass = mass/btScalar(i);
|
||||
int i = this->getMeshPartCount();
|
||||
btScalar partmass = mass/btScalar(i);
|
||||
|
||||
while(i--)
|
||||
{
|
||||
@@ -140,53 +140,42 @@ void btGImpactMeshShape::calculateLocalInertia(btScalar mass,btVector3& inertia)
|
||||
#endif
|
||||
}
|
||||
|
||||
|
||||
|
||||
void btGImpactCompoundShape::rayTest(const btVector3& rayFrom, const btVector3& rayTo, btCollisionWorld::RayResultCallback& resultCallback) const
|
||||
{
|
||||
|
||||
}
|
||||
|
||||
void btGImpactMeshShape::rayTest(const btVector3& rayFrom, const btVector3& rayTo, btCollisionWorld::RayResultCallback& resultCallback) const
|
||||
{
|
||||
}
|
||||
|
||||
void btGImpactMeshShapePart::rayTest(const btVector3& rayFrom, const btVector3& rayTo, btCollisionWorld::RayResultCallback& resultCallback) const
|
||||
{
|
||||
}
|
||||
|
||||
|
||||
void btGImpactMeshShapePart::processAllTriangles(btTriangleCallback* callback,const btVector3& aabbMin,const btVector3& aabbMax) const
|
||||
{
|
||||
lock();
|
||||
GIM_AABB box;
|
||||
lockChildShapes();
|
||||
btAABB box;
|
||||
box.m_min = aabbMin;
|
||||
box.m_max = aabbMax;
|
||||
|
||||
gim_array<GUINT> collided;
|
||||
btAlignedObjectArray<int> collided;
|
||||
m_box_set.boxQuery(box,collided);
|
||||
|
||||
if(collided.size()==0)
|
||||
{
|
||||
unlock();
|
||||
unlockChildShapes();
|
||||
return;
|
||||
}
|
||||
|
||||
int part = (int)getPart();
|
||||
GIM_TRIANGLE triangle;
|
||||
GUINT i = collided.size();
|
||||
btPrimitiveTriangle triangle;
|
||||
int i = collided.size();
|
||||
while(i--)
|
||||
{
|
||||
this->getTriangle(collided[i],triangle);
|
||||
this->getPrimitiveTriangle(collided[i],triangle);
|
||||
callback->processTriangle(triangle.m_vertices,part,collided[i]);
|
||||
}
|
||||
unlock();
|
||||
unlockChildShapes();
|
||||
|
||||
}
|
||||
|
||||
void btGImpactMeshShape::processAllTriangles(btTriangleCallback* callback,const btVector3& aabbMin,const btVector3& aabbMax) const
|
||||
{
|
||||
GUINT i = m_mesh_parts.size();
|
||||
int i = m_mesh_parts.size();
|
||||
while(i--)
|
||||
{
|
||||
m_mesh_parts[i]->processAllTriangles(callback,aabbMin,aabbMax);
|
||||
|
||||
216
Extras/GIMPACT/src/Bullet/btTriangleShapeEx.cpp
Normal file
216
Extras/GIMPACT/src/Bullet/btTriangleShapeEx.cpp
Normal file
@@ -0,0 +1,216 @@
|
||||
/*! \file btGImpactTriangleShape.h
|
||||
\author Francisco Le<4C>n N<>jera
|
||||
*/
|
||||
/*
|
||||
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 "GIMPACT/Bullet/btTriangleShapeEx.h"
|
||||
|
||||
|
||||
|
||||
void BT_TRIANGLE_CONTACT::merge_points(const btVector4 & plane,
|
||||
btScalar margin, const btVector3 * points, int point_count)
|
||||
{
|
||||
m_point_count = 0;
|
||||
m_penetration_depth= -1000.0f;
|
||||
|
||||
int point_indices[MAX_TRI_CLIPPING];
|
||||
|
||||
for (int _k=0;_k<point_count;_k++)
|
||||
{
|
||||
btScalar _dist = - bt_distance_point_plane(plane,points[_k]) + margin;
|
||||
|
||||
if (_dist>=0.0f)
|
||||
{
|
||||
if (_dist>m_penetration_depth)
|
||||
{
|
||||
m_penetration_depth = _dist;
|
||||
point_indices[0] = _k;
|
||||
m_point_count=1;
|
||||
}
|
||||
else if ((_dist+SIMD_EPSILON)>=m_penetration_depth)
|
||||
{
|
||||
point_indices[m_point_count] = _k;
|
||||
m_point_count++;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
for (int _k=0;_k<m_point_count;_k++)
|
||||
{
|
||||
m_points[_k] = points[point_indices[_k]];
|
||||
}
|
||||
}
|
||||
|
||||
///class btPrimitiveTriangle
|
||||
bool btPrimitiveTriangle::overlap_test_conservative(const btPrimitiveTriangle& other)
|
||||
{
|
||||
btScalar total_margin = m_margin + other.m_margin;
|
||||
// classify points on other triangle
|
||||
btScalar dis0 = bt_distance_point_plane(m_plane,other.m_vertices[0]) - total_margin;
|
||||
|
||||
btScalar dis1 = bt_distance_point_plane(m_plane,other.m_vertices[1]) - total_margin;
|
||||
|
||||
btScalar dis2 = bt_distance_point_plane(m_plane,other.m_vertices[2]) - total_margin;
|
||||
|
||||
if (dis0>0.0f&&dis1>0.0f&&dis2>0.0f) return false;
|
||||
|
||||
// classify points on this triangle
|
||||
dis0 = bt_distance_point_plane(other.m_plane,m_vertices[0]) - total_margin;
|
||||
|
||||
dis1 = bt_distance_point_plane(other.m_plane,m_vertices[1]) - total_margin;
|
||||
|
||||
dis2 = bt_distance_point_plane(other.m_plane,m_vertices[2]) - total_margin;
|
||||
|
||||
if (dis0>0.0f&&dis1>0.0f&&dis2>0.0f) return false;
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
int btPrimitiveTriangle::clip_triangle(btPrimitiveTriangle & other, btVector3 * clipped_points )
|
||||
{
|
||||
// edge 0
|
||||
|
||||
btVector3 temp_points[MAX_TRI_CLIPPING];
|
||||
|
||||
|
||||
btVector4 edgeplane;
|
||||
|
||||
get_edge_plane(0,edgeplane);
|
||||
|
||||
|
||||
int clipped_count = bt_plane_clip_triangle(
|
||||
edgeplane,other.m_vertices[0],other.m_vertices[1],other.m_vertices[2],temp_points);
|
||||
|
||||
if (clipped_count == 0) return 0;
|
||||
|
||||
btVector3 temp_points1[MAX_TRI_CLIPPING];
|
||||
|
||||
|
||||
// edge 1
|
||||
get_edge_plane(1,edgeplane);
|
||||
|
||||
|
||||
clipped_count = bt_plane_clip_polygon(edgeplane,temp_points,clipped_count,temp_points1);
|
||||
|
||||
if (clipped_count == 0) return 0;
|
||||
|
||||
// edge 2
|
||||
get_edge_plane(2,edgeplane);
|
||||
|
||||
clipped_count = bt_plane_clip_polygon(
|
||||
edgeplane,temp_points1,clipped_count,clipped_points);
|
||||
|
||||
return clipped_count;
|
||||
}
|
||||
|
||||
bool btPrimitiveTriangle::find_triangle_collision_clip_method(btPrimitiveTriangle & other, BT_TRIANGLE_CONTACT & contacts)
|
||||
{
|
||||
btScalar margin = m_margin + other.m_margin;
|
||||
|
||||
btVector3 clipped_points[MAX_TRI_CLIPPING];
|
||||
int clipped_count;
|
||||
//create planes
|
||||
// plane v vs U points
|
||||
|
||||
BT_TRIANGLE_CONTACT contacts1;
|
||||
|
||||
contacts1.m_separating_normal = m_plane;
|
||||
|
||||
|
||||
clipped_count = clip_triangle(other,clipped_points);
|
||||
|
||||
if (clipped_count == 0 )
|
||||
{
|
||||
return false;//Reject
|
||||
}
|
||||
|
||||
//find most deep interval face1
|
||||
contacts1.merge_points(contacts1.m_separating_normal,margin,clipped_points,clipped_count);
|
||||
if (contacts1.m_point_count == 0) return false; // too far
|
||||
//Normal pointing to this triangle
|
||||
contacts1.m_separating_normal *= -1.f;
|
||||
|
||||
|
||||
//Clip tri1 by tri2 edges
|
||||
BT_TRIANGLE_CONTACT contacts2;
|
||||
contacts2.m_separating_normal = other.m_plane;
|
||||
|
||||
clipped_count = other.clip_triangle(*this,clipped_points);
|
||||
|
||||
if (clipped_count == 0 )
|
||||
{
|
||||
return false;//Reject
|
||||
}
|
||||
|
||||
//find most deep interval face1
|
||||
contacts2.merge_points(contacts2.m_separating_normal,margin,clipped_points,clipped_count);
|
||||
if (contacts2.m_point_count == 0) return false; // too far
|
||||
|
||||
|
||||
|
||||
|
||||
////check most dir for contacts
|
||||
if (contacts2.m_penetration_depth<contacts1.m_penetration_depth)
|
||||
{
|
||||
contacts.copy_from(contacts2);
|
||||
}
|
||||
else
|
||||
{
|
||||
contacts.copy_from(contacts1);
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
|
||||
|
||||
///class btTriangleShapeEx: public btTriangleShape
|
||||
|
||||
bool btTriangleShapeEx::overlap_test_conservative(const btTriangleShapeEx& other)
|
||||
{
|
||||
btScalar total_margin = getMargin() + other.getMargin();
|
||||
|
||||
btVector4 plane0;
|
||||
buildTriPlane(plane0);
|
||||
btVector4 plane1;
|
||||
other.buildTriPlane(plane1);
|
||||
|
||||
// classify points on other triangle
|
||||
btScalar dis0 = bt_distance_point_plane(plane0,other.m_vertices1[0]) - total_margin;
|
||||
|
||||
btScalar dis1 = bt_distance_point_plane(plane0,other.m_vertices1[1]) - total_margin;
|
||||
|
||||
btScalar dis2 = bt_distance_point_plane(plane0,other.m_vertices1[2]) - total_margin;
|
||||
|
||||
if (dis0>0.0f&&dis1>0.0f&&dis2>0.0f) return false;
|
||||
|
||||
// classify points on this triangle
|
||||
dis0 = bt_distance_point_plane(plane1,m_vertices1[0]) - total_margin;
|
||||
|
||||
dis1 = bt_distance_point_plane(plane1,m_vertices1[1]) - total_margin;
|
||||
|
||||
dis2 = bt_distance_point_plane(plane1,m_vertices1[2]) - total_margin;
|
||||
|
||||
if (dis0>0.0f&&dis1>0.0f&&dis2>0.0f) return false;
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
|
||||
@@ -48,10 +48,10 @@ void gim_contact_array::merge_contacts(
|
||||
|
||||
//fill key contacts
|
||||
|
||||
for (GUINT i = 0;i<contacts.size() ;i++ )
|
||||
for (GUINT i = 0;i<contacts.size() ;i++ )
|
||||
{
|
||||
keycontacts[i].m_key = contacts[i].calc_key_contact();
|
||||
keycontacts[i].m_value = i;
|
||||
keycontacts[i].m_value = i;
|
||||
}
|
||||
|
||||
//sort keys
|
||||
@@ -121,10 +121,10 @@ void gim_contact_array::merge_contacts_unique(const gim_contact_array & contacts
|
||||
|
||||
GIM_CONTACT average_contact = contacts.back();
|
||||
|
||||
for (GUINT i=1;i<contacts.size() ;i++ )
|
||||
for (GUINT i=1;i<contacts.size() ;i++ )
|
||||
{
|
||||
average_contact.m_point += contacts[i].m_point;
|
||||
average_contact.m_normal += contacts[i].m_normal * contacts[i].m_depth;
|
||||
average_contact.m_normal += contacts[i].m_normal * contacts[i].m_depth;
|
||||
}
|
||||
|
||||
//divide
|
||||
|
||||
Reference in New Issue
Block a user