Bart/bullet3
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

Softbody improvements, thanks to Nathanael

Browse Source
This commit is contained in:
erwin.coumans
2008-05-17 12:39:16 +00:00
parent 649709dc2d
commit d9e7058ff2
12 changed files with 2098 additions and 1828 deletions
Download Patch File Download Diff File Expand all files Collapse all files

523
src/BulletCollision/BroadphaseCollision/btDbvt.cpp
View File

@@ -17,235 +17,242 @@ subject to the following restrictions:
#include "btDbvt.h"
//
typedef btAlignedObjectArray<btDbvt::Node*> tNodeArray;
typedef btAlignedObjectArray<btDbvt::Node*> tNodeArray;
typedef btAlignedObjectArray<const btDbvt::Node*> tConstNodeArray;
//
struct btDbvtNodeEnumerator : btDbvt::ICollide
{
tConstNodeArray nodes;
void Process(const btDbvt::Node* n) { nodes.push_back(n); }
};
//
static inline int indexof(const btDbvt::Node* node)
{
return(node->parent->childs[1]==node);
return(node->parent->childs[1]==node);
}
//
static inline btDbvt::Volume merge( const btDbvt::Volume& a,
const btDbvt::Volume& b)
const btDbvt::Volume& b)
{
btDbvt::Volume res;
Merge(a,b,res);
return(res);
btDbvt::Volume res;
Merge(a,b,res);
return(res);
}
// volume+edge lengths
static inline btScalar size(const btDbvt::Volume& a)
{
const btVector3 edges=a.Lengths();
return( edges.x()*edges.y()*edges.z()+
const btVector3 edges=a.Lengths();
return( edges.x()*edges.y()*edges.z()+
edges.x()+edges.y()+edges.z());
}
//
static inline void deletenode( btDbvt* pdbvt,
btDbvt::Node* node)
btDbvt::Node* node)
{
delete pdbvt->m_free;
pdbvt->m_free=node;
btAlignedFree(pdbvt->m_free);
pdbvt->m_free=node;
}
//
static inline void recursedeletenode( btDbvt* pdbvt,
btDbvt::Node* node)
btDbvt::Node* node)
{
if(!node->isleaf())
if(!node->isleaf())
{
recursedeletenode(pdbvt,node->childs[0]);
recursedeletenode(pdbvt,node->childs[1]);
recursedeletenode(pdbvt,node->childs[0]);
recursedeletenode(pdbvt,node->childs[1]);
}
if(node==pdbvt->m_root)
pdbvt->m_root=0;
deletenode(pdbvt,node);
if(node==pdbvt->m_root) pdbvt->m_root=0;
deletenode(pdbvt,node);
}
//
static inline btDbvt::Node* createnode( btDbvt* pdbvt,
btDbvt::Node* parent,
const btDbvt::Volume& volume,
void* data)
btDbvt::Node* parent,
const btDbvt::Volume& volume,
void* data)
{
btDbvt::Node* node;
if(pdbvt->m_free)
btDbvt::Node* node;
if(pdbvt->m_free)
{ node=pdbvt->m_free;pdbvt->m_free=0; }
else
{ node=new btDbvt::Node(); }
node->parent = parent;
node->volume = volume;
node->data = data;
node->childs[1] = 0;
return(node);
{ node=new(btAlignedAlloc(sizeof(btDbvt::Node),16)) btDbvt::Node(); }
node->parent = parent;
node->volume = volume;
node->data = data;
node->childs[1] = 0;
return(node);
}
//
static inline void insertleaf( btDbvt* pdbvt,
btDbvt::Node* root,
btDbvt::Node* leaf)
btDbvt::Node* root,
btDbvt::Node* leaf)
{
if(!pdbvt->m_root)
if(!pdbvt->m_root)
{
pdbvt->m_root = leaf;
leaf->parent = 0;
pdbvt->m_root = leaf;
leaf->parent = 0;
}
else
{
if(!root->isleaf())
if(!root->isleaf())
{
do {
if( Proximity(root->childs[0]->volume,leaf->volume)<
Proximity(root->childs[1]->volume,leaf->volume))
root=root->childs[0];
do {
if( Proximity(root->childs[0]->volume,leaf->volume)<
Proximity(root->childs[1]->volume,leaf->volume))
root=root->childs[0];
else
root=root->childs[1];
root=root->childs[1];
} while(!root->isleaf());
}
btDbvt::Node* prev=root->parent;
btDbvt::Node* node=createnode(pdbvt,prev,merge(leaf->volume,root->volume),0);
if(prev)
btDbvt::Node* prev=root->parent;
btDbvt::Node* node=createnode(pdbvt,prev,merge(leaf->volume,root->volume),0);
if(prev)
{
prev->childs[indexof(root)] = node;
node->childs[0] = root;root->parent=node;
node->childs[1] = leaf;leaf->parent=node;
do {
if(prev->volume.Contain(node->volume))
break;
prev->childs[indexof(root)] = node;
node->childs[0] = root;root->parent=node;
node->childs[1] = leaf;leaf->parent=node;
do {
if(!prev->volume.Contain(node->volume))
Merge(prev->childs[0]->volume,prev->childs[1]->volume,prev->volume);
else
Merge(prev->childs[0]->volume,prev->childs[1]->volume,prev->volume);
node=prev;
break;
node=prev;
} while(0!=(prev=node->parent));
}
else
{
node->childs[0] = root;root->parent=node;
node->childs[1] = leaf;leaf->parent=node;
pdbvt->m_root = node;
node->childs[0] = root;root->parent=node;
node->childs[1] = leaf;leaf->parent=node;
pdbvt->m_root = node;
}
}
}
//
static inline btDbvt::Node* removeleaf( btDbvt* pdbvt,
btDbvt::Node* leaf)
btDbvt::Node* leaf)
{
if(leaf==pdbvt->m_root)
if(leaf==pdbvt->m_root)
{
pdbvt->m_root=0;
return(0);
pdbvt->m_root=0;
return(0);
}
else
{
btDbvt::Node* parent=leaf->parent;
btDbvt::Node* prev=parent->parent;
btDbvt::Node* sibling=parent->childs[1-indexof(leaf)];
if(prev)
btDbvt::Node* parent=leaf->parent;
btDbvt::Node* prev=parent->parent;
btDbvt::Node* sibling=parent->childs[1-indexof(leaf)];
if(prev)
{
prev->childs[indexof(parent)]=sibling;
sibling->parent=prev;
deletenode(pdbvt,parent);
while(prev)
prev->childs[indexof(parent)]=sibling;
sibling->parent=prev;
deletenode(pdbvt,parent);
while(prev)
{
const btDbvt::Volume pb=prev->volume;
Merge(prev->childs[0]->volume,prev->childs[1]->volume,prev->volume);
if(NotEqual(pb,prev->volume))
const btDbvt::Volume pb=prev->volume;
Merge(prev->childs[0]->volume,prev->childs[1]->volume,prev->volume);
if(NotEqual(pb,prev->volume))
{
sibling = prev;
prev = prev->parent;
sibling = prev;
prev = prev->parent;
} else break;
}
return(prev?prev:pdbvt->m_root);
return(prev?prev:pdbvt->m_root);
}
else
{
pdbvt->m_root=sibling;
sibling->parent=0;
deletenode(pdbvt,parent);
return(pdbvt->m_root);
pdbvt->m_root=sibling;
sibling->parent=0;
deletenode(pdbvt,parent);
return(pdbvt->m_root);
}
}
}
//
static void fetchleafs( btDbvt* pdbvt,
btDbvt::Node* root,
tNodeArray& leafs,
int depth=-1)
btDbvt::Node* root,
tNodeArray& leafs,
int depth=-1)
{
if(root->isinternal()&&depth)
if(root->isinternal()&&depth)
{
fetchleafs(pdbvt,root->childs[0],leafs,depth-1);
fetchleafs(pdbvt,root->childs[1],leafs,depth-1);
deletenode(pdbvt,root);
fetchleafs(pdbvt,root->childs[0],leafs,depth-1);
fetchleafs(pdbvt,root->childs[1],leafs,depth-1);
deletenode(pdbvt,root);
}
else
{
leafs.push_back(root);
leafs.push_back(root);
}
}
//
static void split( const tNodeArray& leafs,
tNodeArray& left,
tNodeArray& right,
const btVector3& org,
const btVector3& axis)
tNodeArray& left,
tNodeArray& right,
const btVector3& org,
const btVector3& axis)
{
left.resize(0);
right.resize(0);
for(int i=0,ni=leafs.size();i<ni;++i)
left.resize(0);
right.resize(0);
for(int i=0,ni=leafs.size();i<ni;++i)
{
if(dot(axis,leafs[i]->volume.Center()-org)<0)
left.push_back(leafs[i]);
if(dot(axis,leafs[i]->volume.Center()-org)<0)
left.push_back(leafs[i]);
else
right.push_back(leafs[i]);
right.push_back(leafs[i]);
}
}
//
static btDbvt::Volume bounds( const tNodeArray& leafs)
{
btDbvt::Volume volume=leafs[0]->volume;
for(int i=1,ni=leafs.size();i<ni;++i)
btDbvt::Volume volume=leafs[0]->volume;
for(int i=1,ni=leafs.size();i<ni;++i)
{
volume=merge(volume,leafs[i]->volume);
volume=merge(volume,leafs[i]->volume);
}
return(volume);
return(volume);
}
//
static void bottomup( btDbvt* pdbvt,
tNodeArray& leafs)
tNodeArray& leafs)
{
while(leafs.size()>1)
while(leafs.size()>1)
{
btScalar minsize=SIMD_INFINITY;
int minidx[2]={-1,-1};
for(int i=0;i<leafs.size();++i)
btScalar minsize=SIMD_INFINITY;
int minidx[2]={-1,-1};
for(int i=0;i<leafs.size();++i)
{
for(int j=i+1;j<leafs.size();++j)
for(int j=i+1;j<leafs.size();++j)
{
const btScalar sz=size(merge(leafs[i]->volume,leafs[j]->volume));
if(sz<minsize)
const btScalar sz=size(merge(leafs[i]->volume,leafs[j]->volume));
if(sz<minsize)
{
minsize = sz;
minidx[0] = i;
minidx[1] = j;
minsize = sz;
minidx[0] = i;
minidx[1] = j;
}
}
}
btDbvt::Node* n[] = {leafs[minidx[0]],leafs[minidx[1]]};
btDbvt::Node* p = createnode(pdbvt,0,merge(n[0]->volume,n[1]->volume),0);
p->childs[0] = n[0];
p->childs[1] = n[1];
n[0]->parent = p;
n[1]->parent = p;
leafs[minidx[0]] = p;
leafs.swap(minidx[1],leafs.size()-1);
leafs.pop_back();
btDbvt::Node* n[] = {leafs[minidx[0]],leafs[minidx[1]]};
btDbvt::Node* p = createnode(pdbvt,0,merge(n[0]->volume,n[1]->volume),0);
p->childs[0] = n[0];
p->childs[1] = n[1];
n[0]->parent = p;
n[1]->parent = p;
leafs[minidx[0]] = p;
leafs.swap(minidx[1],leafs.size()-1);
leafs.pop_back();
}
}
@@ -254,90 +261,90 @@ static btDbvt::Node* topdown(btDbvt* pdbvt,
tNodeArray& leafs,
int bu_treshold)
{
static const btVector3 axis[]={btVector3(1,0,0),
btVector3(0,1,0),
btVector3(0,0,1)};
if(leafs.size()>1)
static const btVector3 axis[]={btVector3(1,0,0),
btVector3(0,1,0),
btVector3(0,0,1)};
if(leafs.size()>1)
{
if(leafs.size()>bu_treshold)
if(leafs.size()>bu_treshold)
{
const btDbvt::Volume vol=bounds(leafs);
const btVector3 org=vol.Center();
tNodeArray sets[2];
int bestaxis=-1;
int bestmidp=leafs.size();
int splitcount[3][2]={0,0,0,0,0,0};
for(int i=0;i<leafs.size();++i)
const btDbvt::Volume vol=bounds(leafs);
const btVector3 org=vol.Center();
tNodeArray sets[2];
int bestaxis=-1;
int bestmidp=leafs.size();
int splitcount[3][2]={0,0,0,0,0,0};
for(int i=0;i<leafs.size();++i)
{
const btVector3 x=leafs[i]->volume.Center()-org;
for(int j=0;j<3;++j)
const btVector3 x=leafs[i]->volume.Center()-org;
for(int j=0;j<3;++j)
{
++splitcount[j][dot(x,axis[j])>0?1:0];
++splitcount[j][dot(x,axis[j])>0?1:0];
}
}
for(int i=0;i<3;++i)
for(int i=0;i<3;++i)
{
if((splitcount[i][0]>0)&&(splitcount[i][1]>0))
if((splitcount[i][0]>0)&&(splitcount[i][1]>0))
{
const int midp=static_cast<int>(btFabs(static_cast<btScalar>(splitcount[i][0]-splitcount[i][1])));
if(midp<bestmidp)
const int midp=abs(splitcount[i][0]-splitcount[i][1]);
if(midp<bestmidp)
{
bestaxis=i;
bestmidp=midp;
bestaxis=i;
bestmidp=midp;
}
}
}
if(bestaxis>=0)
if(bestaxis>=0)
{
sets[0].reserve(splitcount[bestaxis][0]);
sets[1].reserve(splitcount[bestaxis][1]);
split(leafs,sets[0],sets[1],org,axis[bestaxis]);
sets[0].reserve(splitcount[bestaxis][0]);
sets[1].reserve(splitcount[bestaxis][1]);
split(leafs,sets[0],sets[1],org,axis[bestaxis]);
}
else
{
sets[0].reserve(leafs.size()/2+1);
sets[1].reserve(leafs.size()/2);
for(int i=0,ni=leafs.size();i<ni;++i)
sets[0].reserve(leafs.size()/2+1);
sets[1].reserve(leafs.size()/2);
for(int i=0,ni=leafs.size();i<ni;++i)
{
sets[i&1].push_back(leafs[i]);
sets[i&1].push_back(leafs[i]);
}
}
btDbvt::Node* node=createnode(pdbvt,0,vol,0);
node->childs[0]=topdown(pdbvt,sets[0],bu_treshold);
node->childs[1]=topdown(pdbvt,sets[1],bu_treshold);
node->childs[0]->parent=node;
node->childs[1]->parent=node;
return(node);
btDbvt::Node* node=createnode(pdbvt,0,vol,0);
node->childs[0]=topdown(pdbvt,sets[0],bu_treshold);
node->childs[1]=topdown(pdbvt,sets[1],bu_treshold);
node->childs[0]->parent=node;
node->childs[1]->parent=node;
return(node);
}
else
{
bottomup(pdbvt,leafs);
return(leafs[0]);
bottomup(pdbvt,leafs);
return(leafs[0]);
}
}
return(leafs[0]);
return(leafs[0]);
}
//
static inline btDbvt::Node* refit( btDbvt* pdbvt,
btDbvt::Node* node)
btDbvt::Node* node)
{
btDbvt::Node* parent=node->parent;
if(parent)
btDbvt::Node* parent=node->parent;
if(parent)
{
const int idx=indexof(node);
tNodeArray leafs;
leafs.reserve(64);
fetchleafs(pdbvt,node,leafs,3);
if(leafs.size()>=2)
const int idx=indexof(node);
tNodeArray leafs;
leafs.reserve(64);
fetchleafs(pdbvt,node,leafs,3);
if(leafs.size()>=2)
{
bottomup(pdbvt,leafs);
node=leafs[0];
node->parent=parent;
parent->childs[idx]=node;
bottomup(pdbvt,leafs);
node=leafs[0];
node->parent=parent;
parent->childs[idx]=node;
}
}
return(node);
return(node);
}
//
@@ -345,150 +352,174 @@ static inline btDbvt::Node* refit( btDbvt* pdbvt,
//
//
btDbvt::btDbvt()
btDbvt::btDbvt()
{
m_root = 0;
m_free = 0;
m_lkhd = 2;
m_leafs = 0;
m_root = 0;
m_free = 0;
m_lkhd = -1;
m_leafs = 0;
m_opath = 0;
}
//
btDbvt::~btDbvt()
btDbvt::~btDbvt()
{
clear();
clear();
}
//
void btDbvt::clear()
{
if(m_root) recursedeletenode(this,m_root);
delete m_free;
m_free=0;
if(m_root) recursedeletenode(this,m_root);
btAlignedFree(m_free);
m_free=0;
}
//
void btDbvt::optimizeBottomUp()
{
if(m_root)
if(m_root)
{
tNodeArray leafs;
leafs.reserve(m_leafs);
fetchleafs(this,m_root,leafs);
bottomup(this,leafs);
m_root=leafs[0];
tNodeArray leafs;
leafs.reserve(m_leafs);
fetchleafs(this,m_root,leafs);
bottomup(this,leafs);
m_root=leafs[0];
}
}
//
void btDbvt::optimizeTopDown(int bu_treshold)
{
if(m_root)
if(m_root)
{
tNodeArray leafs;
leafs.reserve(m_leafs);
fetchleafs(this,m_root,leafs);
m_root=topdown(this,leafs,bu_treshold);
tNodeArray leafs;
leafs.reserve(m_leafs);
fetchleafs(this,m_root,leafs);
m_root=topdown(this,leafs,bu_treshold);
}
}
//
void btDbvt::optimizeIncremental(int passes)
{
if(m_root&&(passes>0))
{
do {
Node* node=m_root;
unsigned bit=0;
while(node->isinternal())
{
node=node->childs[(m_opath>>bit)&1];
bit=(bit+1)&(sizeof(unsigned)*8-1);
}
update(node);
++m_opath;
} while(--passes);
}
}
//
btDbvt::Node* btDbvt::insert(const Volume& volume,void* data)
{
Node* leaf=createnode(this,0,volume,data);
insertleaf(this,m_root,leaf);
++m_leafs;
return(leaf);
Node* leaf=createnode(this,0,volume,data);
insertleaf(this,m_root,leaf);
++m_leafs;
return(leaf);
}
//
void btDbvt::update(Node* leaf,int lookahead)
{
Node* root=removeleaf(this,leaf);
if(root)
Node* root=removeleaf(this,leaf);
if(root)
{
for(int i=0;(i<lookahead)&&root->parent;++i)
if(lookahead>=0)
{
for(int i=0;(i<lookahead)&&root->parent;++i)
{
root=root->parent;
}
}
} else root=m_root;
}
insertleaf(this,root,leaf);
insertleaf(this,root,leaf);
}
//
void btDbvt::update(Node* leaf,const Volume& volume)
{
Node* root=removeleaf(this,leaf);
if(root)
Node* root=removeleaf(this,leaf);
if(root)
{
for(int i=0;(i<m_lkhd)&&root->parent;++i)
if(m_lkhd>=0)
{
for(int i=0;(i<m_lkhd)&&root->parent;++i)
{
root=root->parent;
}
}
} else root=m_root;
}
leaf->volume=volume;
insertleaf(this,root,leaf);
leaf->volume=volume;
insertleaf(this,root,leaf);
}
//
bool btDbvt::update(Node* leaf,Volume volume,const btVector3& velocity,btScalar margin)
{
if(leaf->volume.Contain(volume)) return(false);
volume.Expand(btVector3(margin,margin,margin));
volume.SignedExpand(velocity);
update(leaf,volume);
return(true);
if(leaf->volume.Contain(volume)) return(false);
volume.Expand(btVector3(margin,margin,margin));
volume.SignedExpand(velocity);
update(leaf,volume);
return(true);
}
//
bool btDbvt::update(Node* leaf,Volume volume,const btVector3& velocity)
{
if(leaf->volume.Contain(volume)) return(false);
volume.SignedExpand(velocity);
update(leaf,volume);
return(true);
if(leaf->volume.Contain(volume)) return(false);
volume.SignedExpand(velocity);
update(leaf,volume);
return(true);
}
//
bool btDbvt::update(Node* leaf,Volume volume,btScalar margin)
{
if(leaf->volume.Contain(volume)) return(false);
volume.Expand(btVector3(margin,margin,margin));
update(leaf,volume);
return(true);
if(leaf->volume.Contain(volume)) return(false);
volume.Expand(btVector3(margin,margin,margin));
update(leaf,volume);
return(true);
}
//
void btDbvt::remove(Node* leaf)
{
removeleaf(this,leaf);
deletenode(this,leaf);
--m_leafs;
removeleaf(this,leaf);
deletenode(this,leaf);
--m_leafs;
}
//
void btDbvt::collide(btDbvt* tree,
ICollide* icollide) const
void btDbvt::write(IWriter* iwriter) const
{
collideGeneric(tree,icollide);
}
//
void btDbvt::collide(btDbvt::Node* node,
ICollide* icollide) const
{
collideGeneric(node,icollide);
}
//
void btDbvt::collide(const Volume& volume,
ICollide* icollide) const
{
collideGeneric(volume,icollide);
}
//
void btDbvt::collide(ICollide* icollide) const
{
collideGeneric(icollide);
btDbvtNodeEnumerator nodes;
nodes.nodes.reserve(m_leafs*2);
enumNodes(m_root,nodes);
iwriter->Prepare(m_root,nodes.nodes.size());
for(int i=0;i<nodes.nodes.size();++i)
{
const Node* n=nodes.nodes[i];
int p=-1;
if(n->parent) p=nodes.nodes.findLinearSearch(n->parent);
if(n->isinternal())
{
const int c0=nodes.nodes.findLinearSearch(n->childs[0]);
const int c1=nodes.nodes.findLinearSearch(n->childs[1]);
iwriter->WriteNode(n,i,p,c0,c1);
}
else
{
iwriter->WriteLeaf(n,i,p);
}
}
}

768
src/BulletCollision/BroadphaseCollision/btDbvt.h
View File

@@ -20,6 +20,26 @@ subject to the following restrictions:
#include "LinearMath/btAlignedObjectArray.h"
#include "LinearMath/btVector3.h"
//
// Compile time configuration
//
#ifdef WIN32
#define DBVT_USE_TEMPLATE
#endif
#ifdef DBVT_USE_TEMPLATE
#define DBVT_VIRTUAL
#define DBVT_PREFIX template <typename T>
#define DBVT_IPOLICY T& policy
#define DBVT_CHECKTYPE static const ICollide& typechecker=*(T*)0;
#else
#define DBVT_VIRTUAL virtual
#define DBVT_PREFIX
#define DBVT_IPOLICY ICollide& policy
#define DBVT_CHECKTYPE
#endif
//
// Defaults volumes
//
@@ -27,122 +47,165 @@ subject to the following restrictions:
/* btDbvtAabbMm */
struct btDbvtAabbMm
{
inline btVector3 Center() const { return((mi+mx)/2); }
inline btVector3 Extent() const { return((mx-mi)/2); }
inline const btVector3& Mins() const { return(mi); }
inline const btVector3& Maxs() const { return(mx); }
inline btVector3 Lengths() const { return(mx-mi); }
static inline btDbvtAabbMm FromCE(const btVector3& c,const btVector3& e);
static inline btDbvtAabbMm FromCR(const btVector3& c,btScalar r);
static inline btDbvtAabbMm FromMM(const btVector3& mi,const btVector3& mx);
static inline btDbvtAabbMm FromPoints(const btVector3* pts,int n);
static inline btDbvtAabbMm FromPoints(const btVector3** ppts,int n);
inline void Expand(const btVector3 e);
inline void SignedExpand(const btVector3 e);
inline bool Contain(const btDbvtAabbMm& a) const;
inline friend bool Intersect( const btDbvtAabbMm& a,
const btDbvtAabbMm& b);
inline friend bool Intersect( const btDbvtAabbMm& a,
const btVector3& b);
inline friend btScalar Proximity( const btDbvtAabbMm& a,
const btDbvtAabbMm& b);
inline friend void Merge( const btDbvtAabbMm& a,
const btDbvtAabbMm& b,
btDbvtAabbMm& r);
inline friend bool NotEqual( const btDbvtAabbMm& a,
const btDbvtAabbMm& b);
inline btVector3 Center() const { return((mi+mx)/2); }
inline btVector3 Extent() const { return((mx-mi)/2); }
inline const btVector3& Mins() const { return(mi); }
inline const btVector3& Maxs() const { return(mx); }
inline btVector3 Lengths() const { return(mx-mi); }
static inline btDbvtAabbMm FromCE(const btVector3& c,const btVector3& e);
static inline btDbvtAabbMm FromCR(const btVector3& c,btScalar r);
static inline btDbvtAabbMm FromMM(const btVector3& mi,const btVector3& mx);
static inline btDbvtAabbMm FromPoints(const btVector3* pts,int n);
static inline btDbvtAabbMm FromPoints(const btVector3** ppts,int n);
inline void Expand(const btVector3 e);
inline void SignedExpand(const btVector3 e);
inline bool Contain(const btDbvtAabbMm& a) const;
inline int Classify(const btVector3& n,btScalar o,int s) const;
inline btScalar ProjectMinimum(const btVector3& v,unsigned signs) const;
inline friend bool Intersect( const btDbvtAabbMm& a,
const btDbvtAabbMm& b);
inline friend bool Intersect( const btDbvtAabbMm& a,
const btVector3& b);
inline friend bool Intersect( const btDbvtAabbMm& a,
const btVector3& org,
const btVector3& invdir,
const unsigned* signs);
inline friend btScalar Proximity( const btDbvtAabbMm& a,
const btDbvtAabbMm& b);
inline friend void Merge( const btDbvtAabbMm& a,
const btDbvtAabbMm& b,
btDbvtAabbMm& r);
inline friend bool NotEqual( const btDbvtAabbMm& a,
const btDbvtAabbMm& b);
private:
btVector3 mi,mx;
btVector3 mi,mx;
};
//
// Dynamic bounding volume tree
//
struct btDbvt
{
{
// Types
typedef btDbvtAabbMm Volume;
/* Node */
struct Node
{
{
Volume volume;
Node* parent;
bool isleaf() const { return(childs[1]==0); }
bool isinternal() const { return(!isleaf()); }
union {
Node* childs[2];
void* data;
Node* childs[2];
void* data;
};
};
};
/* Stack element */
struct sStkElm
{
struct sStkNN
{
const Node* a;
const Node* b;
sStkElm(const Node* na,const Node* nb) : a(na),b(nb) {}
};
// Interfaces
sStkNN(const Node* na,const Node* nb) : a(na),b(nb) {}
};
struct sStkNP
{
const Node* node;
unsigned mask;
sStkNP(const Node* n,unsigned m) : node(n),mask(m) {}
};
struct sStkNPS
{
const Node* node;
unsigned mask;
btScalar value;
sStkNPS(const Node* n,unsigned m,btScalar v) : node(n),mask(m),value(v) {}
};
// Policies/Interfaces
/* ICollide */
struct ICollide
{
virtual ~ICollide()
{
DBVT_VIRTUAL void Process(const Node*,const Node*) {}
DBVT_VIRTUAL void Process(const Node*) {}
DBVT_VIRTUAL bool Descent(const Node*) { return(true); }
DBVT_VIRTUAL bool AllLeafs(const Node*) { return(true); }
};
/* IWriter */
struct IWriter
{
}
virtual void Process(const Node*,const Node*)=0;
virtual void Process(const Node*)=0;
virtual bool Descent(const Node*)=0;
};
virtual void Prepare(const Node* root,int numnodes)=0;
virtual void WriteNode(const Node*,int index,int parent,int child0,int child1)=0;
virtual void WriteLeaf(const Node*,int index,int parent)=0;
};
// Constants
enum {
TREETREE_STACKSIZE = 128,
VOLUMETREE_STACKSIZE = 64
};
SIMPLE_STACKSIZE = 64,
DOUBLE_STACKSIZE = SIMPLE_STACKSIZE*2,
};
// Fields
Node* m_root;
Node* m_free;
int m_lkhd;
int m_leafs;
unsigned m_opath;
// Methods
btDbvt();
~btDbvt();
btDbvt();
~btDbvt();
void clear();
bool empty() const { return(0==m_root); }
void optimizeBottomUp();
void optimizeTopDown(int bu_treshold=128);
void optimizeIncremental(int passes);
Node* insert(const Volume& box,void* data);
void update(Node* leaf,int lookahead=1);
void update(Node* leaf,int lookahead=-1);
void update(Node* leaf,const Volume& volume);
bool update(Node* leaf,Volume volume,const btVector3& velocity,btScalar margin);
bool update(Node* leaf,Volume volume,const btVector3& velocity);
bool update(Node* leaf,Volume volume,btScalar margin);
void remove(Node* leaf);
void collide(btDbvt* tree,
ICollide* icollide) const;
void collide(btDbvt::Node* node,
ICollide* icollide) const;
void collide(const Volume& volume,
ICollide* icollide) const;
void collide(const btVector3& org,
const btVector3& dir,
ICollide* icollide) const;
void collide(ICollide* icollide) const;
// Generics : T must implement ICollide
void collideGeneric( btDbvt* tree,ICollide* policy) const;
void collideGeneric( btDbvt::Node* node,ICollide* policy) const;
void collideGeneric(const Volume& volume,ICollide* policy) const;
void collideGeneric(ICollide* policy) const;
void write(IWriter* iwriter) const;
// DBVT_IPOLICY must support ICollide policy/interface
DBVT_PREFIX
static void enumNodes( const Node* root,
DBVT_IPOLICY);
DBVT_PREFIX
static void enumLeafs( const Node* root,
DBVT_IPOLICY);
DBVT_PREFIX
static void collideTT( const Node* root0,
const Node* root1,
DBVT_IPOLICY);
DBVT_PREFIX
static void collideTV( const Node* root,
const Volume& volume,
DBVT_IPOLICY);
DBVT_PREFIX
static void collideRAY( const Node* root,
const btVector3& origin,
const btVector3& direction,
DBVT_IPOLICY);
DBVT_PREFIX
static void collideKDOP(const Node* root,
const btVector3* normals,
const btScalar* offsets,
int count,
DBVT_IPOLICY);
DBVT_PREFIX
static void collideOCL( const Node* root,
const btVector3* normals,
const btScalar* offsets,
const btVector3& sortaxis,
int count,
DBVT_IPOLICY);
DBVT_PREFIX
static void collideTU( const Node* root,
DBVT_IPOLICY);
//
private:
private:
btDbvt(const btDbvt&) {}
};
};
//
// Inline's
@@ -151,69 +214,69 @@ private:
//
inline btDbvtAabbMm btDbvtAabbMm::FromCE(const btVector3& c,const btVector3& e)
{
btDbvtAabbMm box;
box.mi=c-e;box.mx=c+e;
return(box);
btDbvtAabbMm box;
box.mi=c-e;box.mx=c+e;
return(box);
}
//
inline btDbvtAabbMm btDbvtAabbMm::FromCR(const btVector3& c,btScalar r)
{
return(FromCE(c,btVector3(r,r,r)));
return(FromCE(c,btVector3(r,r,r)));
}
//
inline btDbvtAabbMm btDbvtAabbMm::FromMM(const btVector3& mi,const btVector3& mx)
{
btDbvtAabbMm box;
box.mi=mi;box.mx=mx;
return(box);
btDbvtAabbMm box;
box.mi=mi;box.mx=mx;
return(box);
}
//
inline btDbvtAabbMm btDbvtAabbMm::FromPoints(const btVector3* pts,int n)
{
btDbvtAabbMm box;
box.mi=box.mx=pts[0];
for(int i=1;i<n;++i)
btDbvtAabbMm box;
box.mi=box.mx=pts[0];
for(int i=1;i<n;++i)
{
box.mi.setMin(pts[i]);
box.mx.setMax(pts[i]);
box.mi.setMin(pts[i]);
box.mx.setMax(pts[i]);
}
return(box);
return(box);
}
//
inline btDbvtAabbMm btDbvtAabbMm::FromPoints(const btVector3** ppts,int n)
{
btDbvtAabbMm box;
box.mi=box.mx=*ppts[0];
for(int i=1;i<n;++i)
btDbvtAabbMm box;
box.mi=box.mx=*ppts[0];
for(int i=1;i<n;++i)
{
box.mi.setMin(*ppts[i]);
box.mx.setMax(*ppts[i]);
box.mi.setMin(*ppts[i]);
box.mx.setMax(*ppts[i]);
}
return(box);
return(box);
}
//
inline void btDbvtAabbMm::Expand(const btVector3 e)
{
mi-=e;mx+=e;
mi-=e;mx+=e;
}
//
inline void btDbvtAabbMm::SignedExpand(const btVector3 e)
{
if(e.x()>0) mx.setX(mx.x()+e.x()); else mi.setX(mi.x()+e.x());
if(e.y()>0) mx.setY(mx.y()+e.y()); else mi.setY(mi.y()+e.y());
if(e.z()>0) mx.setZ(mx.z()+e.z()); else mi.setZ(mi.z()+e.z());
if(e.x()>0) mx.setX(mx.x()+e.x()); else mi.setX(mi.x()+e.x());
if(e.y()>0) mx.setY(mx.y()+e.y()); else mi.setY(mi.y()+e.y());
if(e.z()>0) mx.setZ(mx.z()+e.z()); else mi.setZ(mi.z()+e.z());
}
//
inline bool btDbvtAabbMm::Contain(const btDbvtAabbMm& a) const
{
return( (mi.x()<=a.mi.x())&&
return( (mi.x()<=a.mi.x())&&
(mi.y()<=a.mi.y())&&
(mi.z()<=a.mi.z())&&
(mx.x()>=a.mx.x())&&
@@ -221,24 +284,62 @@ inline bool btDbvtAabbMm::Contain(const btDbvtAabbMm& a) const
(mx.z()>=a.mx.z()));
}
//
inline int btDbvtAabbMm::Classify(const btVector3& n,btScalar o,int s) const
{
btVector3 pi,px;
switch(s)
{
case (0+0+0): px=btVector3(mi.x(),mi.y(),mi.z());
pi=btVector3(mx.x(),mx.y(),mx.z());break;
case (1+0+0): px=btVector3(mx.x(),mi.y(),mi.z());
pi=btVector3(mi.x(),mx.y(),mx.z());break;
case (0+2+0): px=btVector3(mi.x(),mx.y(),mi.z());
pi=btVector3(mx.x(),mi.y(),mx.z());break;
case (1+2+0): px=btVector3(mx.x(),mx.y(),mi.z());
pi=btVector3(mi.x(),mi.y(),mx.z());break;
case (0+0+4): px=btVector3(mi.x(),mi.y(),mx.z());
pi=btVector3(mx.x(),mx.y(),mi.z());break;
case (1+0+4): px=btVector3(mx.x(),mi.y(),mx.z());
pi=btVector3(mi.x(),mx.y(),mi.z());break;
case (0+2+4): px=btVector3(mi.x(),mx.y(),mx.z());
pi=btVector3(mx.x(),mi.y(),mi.z());break;
case (1+2+4): px=btVector3(mx.x(),mx.y(),mx.z());
pi=btVector3(mi.x(),mi.y(),mi.z());break;
}
if((dot(n,px)+o)<0) return(-1);
if((dot(n,pi)+o)>=0) return(+1);
return(0);
}
//
inline btScalar btDbvtAabbMm::ProjectMinimum(const btVector3& v,unsigned signs) const
{
const btVector3* b[]={&mx,&mi};
const btVector3 p( b[(signs>>0)&1]->x(),
b[(signs>>1)&1]->y(),
b[(signs>>2)&1]->z());
return(dot(p,v));
}
//
inline bool Intersect( const btDbvtAabbMm& a,
const btDbvtAabbMm& b)
const btDbvtAabbMm& b)
{
#if 0
const btScalar mi[]={ b.mx.x()-a.mi.x(),
b.mx.y()-a.mi.y(),
b.mx.z()-a.mi.z()};
const unsigned* imi=(const unsigned*)mi;
if((imi[0]|imi[1]|imi[2])&0x80000000) return(false);
const btScalar mx[]={ a.mx.x()-b.mi.x(),
a.mx.y()-b.mi.y(),
a.mx.z()-b.mi.z()};
const unsigned* imx=(const unsigned*)mx;
if((imx[0]|imx[1]|imx[2])&0x80000000) return(false);
return(true);
const btScalar mi[]={ b.mx.x()-a.mi.x(),
b.mx.y()-a.mi.y(),
b.mx.z()-a.mi.z()};
const unsigned* imi=(const unsigned*)mi;
if((imi[0]|imi[1]|imi[2])&0x80000000) return(false);
const btScalar mx[]={ a.mx.x()-b.mi.x(),
a.mx.y()-b.mi.y(),
a.mx.z()-b.mi.z()};
const unsigned* imx=(const unsigned*)mx;
if((imx[0]|imx[1]|imx[2])&0x80000000) return(false);
return(true);
#else
return( (a.mi.x()<=b.mx.x())&&
return( (a.mi.x()<=b.mx.x())&&
(a.mi.y()<=b.mx.y())&&
(a.mi.z()<=b.mx.z())&&
(a.mx.x()>=b.mi.x())&&
@@ -249,9 +350,9 @@ inline bool Intersect( const btDbvtAabbMm& a,
//
inline bool Intersect( const btDbvtAabbMm& a,
const btVector3& b)
const btVector3& b)
{
return( (b.x()>=a.mi.x())&&
return( (b.x()>=a.mi.x())&&
(b.y()>=a.mi.y())&&
(b.z()>=a.mi.z())&&
(b.x()<=a.mx.x())&&
@@ -260,28 +361,50 @@ inline bool Intersect( const btDbvtAabbMm& a,
}
//
inline btScalar Proximity( const btDbvtAabbMm& a,
const btDbvtAabbMm& b)
inline bool Intersect( const btDbvtAabbMm& a,
const btVector3& org,
const btVector3& invdir,
const unsigned* signs)
{
const btVector3 d=(a.mi+a.mx)-(b.mi+b.mx);
return(btFabs(d.x())+btFabs(d.y())+btFabs(d.z()));
const btVector3* bounds[2]={&a.mi,&a.mx};
btScalar txmin=(bounds[ signs[0]]->x()-org[0])*invdir[0];
btScalar txmax=(bounds[1-signs[0]]->x()-org[0])*invdir[0];
const btScalar tymin=(bounds[ signs[1]]->y()-org[1])*invdir[1];
const btScalar tymax=(bounds[1-signs[1]]->y()-org[1])*invdir[1];
if((txmin>tymax)||(tymin>txmax)) return(false);
if(tymin>txmin) txmin=tymin;
if(tymax<txmax) txmax=tymax;
const btScalar tzmin=(bounds[ signs[2]]->z()-org[2])*invdir[2];
const btScalar tzmax=(bounds[1-signs[2]]->z()-org[2])*invdir[2];
if((txmin>tzmax)||(tzmin>txmax)) return(false);
if(tzmin>txmin) txmin=tzmin;
if(tzmax<txmax) txmax=tzmax;
return(txmax>0);
}
//
inline btScalar Proximity( const btDbvtAabbMm& a,
const btDbvtAabbMm& b)
{
const btVector3 d=(a.mi+a.mx)-(b.mi+b.mx);
return(btFabs(d.x())+btFabs(d.y())+btFabs(d.z()));
}
//
inline void Merge( const btDbvtAabbMm& a,
const btDbvtAabbMm& b,
btDbvtAabbMm& r)
const btDbvtAabbMm& b,
btDbvtAabbMm& r)
{
r=a;
r.mi.setMin(b.mi);
r.mx.setMax(b.mx);
r=a;
r.mi.setMin(b.mi);
r.mx.setMax(b.mx);
}
//
inline bool NotEqual( const btDbvtAabbMm& a,
const btDbvtAabbMm& b)
const btDbvtAabbMm& b)
{
return( (a.mi.x()!=b.mi.x())||
return( (a.mi.x()!=b.mi.x())||
(a.mi.y()!=b.mi.y())||
(a.mi.z()!=b.mi.z())||
(a.mx.x()!=b.mx.x())||
@@ -290,117 +413,330 @@ inline bool NotEqual( const btDbvtAabbMm& a,
}
//
// Generic's
// Inline's
//
//
inline void btDbvt::collideGeneric( btDbvt::Node* node,ICollide* policy) const
DBVT_PREFIX
inline void btDbvt::enumNodes( const Node* root,
DBVT_IPOLICY)
{
if(m_root&&node)
DBVT_CHECKTYPE
policy.Process(root);
if(root->isinternal())
{
btAlignedObjectArray<sStkElm> stack;
stack.reserve(TREETREE_STACKSIZE);
stack.push_back(sStkElm(m_root,node));
do {
sStkElm p=stack[stack.size()-1];
stack.pop_back();
if(p.a==p.b)
{
if(p.a->isinternal())
{
stack.push_back(sStkElm(p.a->childs[0],p.a->childs[0]));
stack.push_back(sStkElm(p.a->childs[1],p.a->childs[1]));
stack.push_back(sStkElm(p.a->childs[0],p.a->childs[1]));
}
}
else if(Intersect(p.a->volume,p.b->volume))
{
if(p.a->isinternal())
{
if(p.b->isinternal())
{
stack.push_back(sStkElm(p.a->childs[0],p.b->childs[0]));
stack.push_back(sStkElm(p.a->childs[1],p.b->childs[0]));
stack.push_back(sStkElm(p.a->childs[0],p.b->childs[1]));
stack.push_back(sStkElm(p.a->childs[1],p.b->childs[1]));
}
else
{
stack.push_back(sStkElm(p.a->childs[0],p.b));
stack.push_back(sStkElm(p.a->childs[1],p.b));
}
}
else
{
if(p.b->isinternal())
{
stack.push_back(sStkElm(p.a,p.b->childs[0]));
stack.push_back(sStkElm(p.a,p.b->childs[1]));
}
else
{
policy->Process(p.a,p.b);
}
}
}
} while(stack.size()>0);
enumNodes(root->childs[0],policy);
enumNodes(root->childs[1],policy);
}
}
//
inline void btDbvt::collideGeneric( btDbvt* tree,ICollide* policy) const
DBVT_PREFIX
inline void btDbvt::enumLeafs( const Node* root,
DBVT_IPOLICY)
{
collideGeneric(tree->m_root,policy);
}
//
inline void btDbvt::collideGeneric(const Volume& volume,ICollide* policy) const
{
if(m_root)
DBVT_CHECKTYPE
if(root->isinternal())
{
btAlignedObjectArray<const Node*> stack;
stack.reserve(VOLUMETREE_STACKSIZE);
stack.push_back(m_root);
do {
const Node* n=stack[stack.size()-1];
stack.pop_back();
if(Intersect(n->volume,volume))
{
if(n->isinternal())
{
stack.push_back(n->childs[0]);
stack.push_back(n->childs[1]);
}
else
{
policy->Process(n);
}
}
} while(stack.size()>0);
enumLeafs(root->childs[0],policy);
enumLeafs(root->childs[1],policy);
}
else
{
policy.Process(root);
}
}
//
inline void btDbvt::collideGeneric(ICollide* policy) const
DBVT_PREFIX
inline void btDbvt::collideTT( const Node* root0,
const Node* root1,
DBVT_IPOLICY)
{
if(m_root)
DBVT_CHECKTYPE
if(root0&&root1)
{
btAlignedObjectArray<sStkNN> stack;
stack.reserve(DOUBLE_STACKSIZE);
stack.push_back(sStkNN(root0,root1));
do {
sStkNN p=stack[stack.size()-1];
stack.pop_back();
if(p.a==p.b)
{
if(p.a->isinternal())
{
stack.push_back(sStkNN(p.a->childs[0],p.a->childs[0]));
stack.push_back(sStkNN(p.a->childs[1],p.a->childs[1]));
stack.push_back(sStkNN(p.a->childs[0],p.a->childs[1]));
}
}
else if(Intersect(p.a->volume,p.b->volume))
{
if(p.a->isinternal())
{
if(p.b->isinternal())
{
stack.push_back(sStkNN(p.a->childs[0],p.b->childs[0]));
stack.push_back(sStkNN(p.a->childs[1],p.b->childs[0]));
stack.push_back(sStkNN(p.a->childs[0],p.b->childs[1]));
stack.push_back(sStkNN(p.a->childs[1],p.b->childs[1]));
}
else
{
stack.push_back(sStkNN(p.a->childs[0],p.b));
stack.push_back(sStkNN(p.a->childs[1],p.b));
}
}
else
{
if(p.b->isinternal())
{
stack.push_back(sStkNN(p.a,p.b->childs[0]));
stack.push_back(sStkNN(p.a,p.b->childs[1]));
}
else
{
policy.Process(p.a,p.b);
}
}
}
} while(stack.size()>0);
}
}
//
DBVT_PREFIX
inline void btDbvt::collideTV( const Node* root,
const Volume& volume,
DBVT_IPOLICY)
{
DBVT_CHECKTYPE
if(root)
{
btAlignedObjectArray<const Node*> stack;
stack.reserve(SIMPLE_STACKSIZE);
stack.push_back(root);
do {
const Node* n=stack[stack.size()-1];
stack.pop_back();
if(Intersect(n->volume,volume))
{
if(n->isinternal())
{
stack.push_back(n->childs[0]);
stack.push_back(n->childs[1]);
}
else
{
policy.Process(n);
}
}
} while(stack.size()>0);
}
}
//
DBVT_PREFIX
inline void btDbvt::collideRAY( const Node* root,
const btVector3& origin,
const btVector3& direction,
DBVT_IPOLICY)
{
DBVT_CHECKTYPE
if(root)
{
btAlignedObjectArray<const Node*> stack;
stack.reserve(VOLUMETREE_STACKSIZE);
stack.push_back(m_root);
do {
const Node* n=stack[stack.size()-1];
stack.pop_back();
if(policy->Descent(n))
const btVector3 normal=direction.normalized();
const btVector3 invdir( 1/normal.x(),
1/normal.y(),
1/normal.z());
const unsigned signs[]={ direction.x()<0,
direction.y()<0,
direction.z()<0};
btAlignedObjectArray<const Node*> stack;
stack.reserve(SIMPLE_STACKSIZE);
stack.push_back(root);
do {
const Node* node=stack[stack.size()-1];
stack.pop_back();
if(Intersect(node->volume,origin,invdir,signs))
{
if(n->isinternal())
if(node->isinternal())
{
stack.push_back(node->childs[0]);
stack.push_back(node->childs[1]);
}
else
{
policy.Process(node);
}
}
} while(stack.size());
}
}
//
DBVT_PREFIX
inline void btDbvt::collideKDOP(const Node* root,
const btVector3* normals,
const btScalar* offsets,
int count,
DBVT_IPOLICY)
{
DBVT_CHECKTYPE
if(root)
{
const int inside=(1<<count)-1;
btAlignedObjectArray<sStkNP> stack;
int signs[sizeof(unsigned)*8];
btAssert(count<(sizeof(signs)/sizeof(signs[0])));
for(int i=0;i<count;++i)
{
signs[i]= ((normals[i].x()>=0)?1:0)+
((normals[i].y()>=0)?2:0)+
((normals[i].z()>=0)?4:0);
}
stack.reserve(SIMPLE_STACKSIZE);
stack.push_back(sStkNP(root,0));
do {
sStkNP se=stack[stack.size()-1];
bool out=false;
stack.pop_back();
for(int i=0,j=1;(!out)&&(i<count);++i,j<<=1)
{
if(0==(se.mask&j))
{
const int side=se.node->volume.Classify(normals[i],offsets[i],signs[i]);
switch(side)
{
case -1: out=true;break;
case +1: se.mask|=j;break;
}
}
}
if(!out)
{
if((se.mask!=inside)&&(se.node->isinternal()))
{
stack.push_back(sStkNP(se.node->childs[0],se.mask));
stack.push_back(sStkNP(se.node->childs[1],se.mask));
}
else
{
if(policy.AllLeafs(se.node)) enumLeafs(se.node,policy);
}
}
} while(stack.size());
}
}
//
DBVT_PREFIX
inline void btDbvt::collideOCL( const Node* root,
const btVector3* normals,
const btScalar* offsets,
const btVector3& sortaxis,
int count,
DBVT_IPOLICY)
{
DBVT_CHECKTYPE
if(root)
{
const unsigned srtsgns=(sortaxis[0]>=0?1:0)+
(sortaxis[1]>=0?2:0)+
(sortaxis[2]>=0?4:0);
const int inside=(1<<count)-1;
btAlignedObjectArray<sStkNPS> stack;
int signs[sizeof(unsigned)*8];
btAssert(count<(sizeof(signs)/sizeof(signs[0])));
for(int i=0;i<count;++i)
{
signs[i]= ((normals[i].x()>=0)?1:0)+
((normals[i].y()>=0)?2:0)+
((normals[i].z()>=0)?4:0);
}
stack.reserve(SIMPLE_STACKSIZE);
stack.push_back(sStkNPS(root,0,root->volume.ProjectMinimum(sortaxis,srtsgns)));
do {
sStkNPS se=stack[stack.size()-1];
stack.pop_back();
if(se.mask!=inside)
{
bool out=false;
for(int i=0,j=1;(!out)&&(i<count);++i,j<<=1)
{
if(0==(se.mask&j))
{
const int side=se.node->volume.Classify(normals[i],offsets[i],signs[i]);
switch(side)
{
case -1: out=true;break;
case +1: se.mask|=j;break;
}
}
}
if(out) continue;
}
if(policy.Descent(se.node))
{
if(se.node->isinternal())
{
for(int i=0;i<2;++i)
{
const Node* n=se.node->childs[i];
int j=stack.size();
sStkNPS ne(n,se.mask,n->volume.ProjectMinimum(sortaxis,srtsgns));
stack.push_back(ne);
while((j>0)&&(ne.value>stack[j-1].value))
{
btSwap(stack[j],stack[j-1]);--j;
}
}
}
else
{
policy.Process(se.node);
}
}
} while(stack.size());
}
}
//
DBVT_PREFIX
inline void btDbvt::collideTU( const Node* root,
DBVT_IPOLICY)
{
DBVT_CHECKTYPE
if(root)
{
btAlignedObjectArray<const Node*> stack;
stack.reserve(SIMPLE_STACKSIZE);
stack.push_back(root);
do {
const Node* n=stack[stack.size()-1];
stack.pop_back();
if(policy.Descent(n))
{
if(n->isinternal())
{ stack.push_back(n->childs[0]);stack.push_back(n->childs[1]); }
else
{ policy->Process(n); }
{ policy.Process(n); }
}
} while(stack.size()>0);
}
}
//
// PP Cleanup
//
#ifdef DBVT_USE_TEMPLATE
#undef DBVT_USE_TEMPLATE
#endif
#undef DBVT_VIRTUAL
#undef DBVT_PREFIX
#undef DBVT_IPOLICY
#undef DBVT_CHECKTYPE
#endif

481
src/BulletCollision/BroadphaseCollision/btDbvtBroadphase.cpp
View File

@@ -1,19 +1,3 @@
/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2003-2007 Erwin Coumans http://continuousphysics.com/Bullet/
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.
*/
///btDbvtBroadphase implementation by Nathanael Presson
#include "btDbvtBroadphase.h"
//
@@ -23,21 +7,21 @@ subject to the following restrictions:
#if DBVT_BP_PROFILE
#include <stdio.h>
struct ProfileScope
{
ProfileScope(btClock& clock,unsigned long& value)
{
ProfileScope(btClock& clock,unsigned long& value)
{
m_clock=&clock;
m_value=&value;
m_base=clock.getTimeMicroseconds();
}
}
~ProfileScope()
{
{
(*m_value)+=m_clock->getTimeMicroseconds()-m_base;
}
}
btClock* m_clock;
unsigned long* m_value;
unsigned long m_base;
};
};
#define SPC(_value_) ProfileScope spc_scope(m_clock,_value_)
#else
#define SPC(_value_)
@@ -47,82 +31,84 @@ struct ProfileScope
// Helpers
//
//
static inline int hash(unsigned int i,unsigned int j)
{
int key=static_cast<int>(((unsigned int)i)|(((unsigned int)j)<<16));
key+=~(key<<15);
key^= (key>>10);
key+= (key<<3);
key^= (key>>6);
key+=~(key<<11);
key^= (key>>16);
return(key);
}
//
template <typename T>
static inline void listappend(T* item,T*& list)
{
item->links[0]=0;
item->links[1]=list;
if(list) list->links[0]=item;
list=item;
item->links[0]=0;
item->links[1]=list;
if(list) list->links[0]=item;
list=item;
}
//
template <typename T>
static inline void listremove(T* item,T*& list)
{
if(item->links[0]) item->links[0]->links[1]=item->links[1]; else list=item->links[1];
if(item->links[1]) item->links[1]->links[0]=item->links[0];
if(item->links[0]) item->links[0]->links[1]=item->links[1]; else list=item->links[1];
if(item->links[1]) item->links[1]->links[0]=item->links[0];
}
//
template <typename T>
static inline int listcount(T* root)
{
int n=0;
while(root) { ++n;root=root->links[1]; }
return(n);
int n=0;
while(root) { ++n;root=root->links[1]; }
return(n);
}
//
template <typename T>
static inline void clear(T& value)
{
static const T zerodummy;
value=zerodummy;
static const struct ZeroDummy : T {} zerodummy;
value=zerodummy;
}
//
// Collider
// Colliders
//
struct btDbvtBroadphaseCollider : btDbvt::ICollide
/* Leaf collider */
struct btDbvtLeafCollider : btDbvt::ICollide
{
btDbvtBroadphase* pbp;
int pid;
btDbvtBroadphaseCollider(btDbvtBroadphase* p,int id) : pbp(p),pid(id) {}
virtual void Process(const btDbvt::Node* /*na*/)
btDbvtBroadphase* pbp;
btDbvtProxy* ppx;
btDbvtLeafCollider(btDbvtBroadphase* p,btDbvtProxy* px) : pbp(p),ppx(px) {}
void Process(const btDbvt::Node* na)
{
}
virtual bool Descent(const btDbvt::Node*)
{
return false;
}
virtual void Process(const btDbvt::Node* na,const btDbvt::Node* nb)
{
btDbvtProxy* pa=(btDbvtProxy*)na->data;
btDbvtProxy* pb=(btDbvtProxy*)nb->data;
#if DBVT_BP_DISCRETPAIRS
if(Intersect(pa->aabb,pb->aabb))
#endif
const btDbvt::Node* nb=ppx->leaf;
if(nb!=na)
{
btBroadphasePair* pp=pbp->m_paircache->addOverlappingPair(pa,pb);
if(pp)
pp->m_userInfo=*(void**)&pid;
btDbvtProxy* pa=(btDbvtProxy*)na->data;
btDbvtProxy* pb=(btDbvtProxy*)nb->data;
#if DBVT_BP_DISCRETPAIRS
if(Intersect(pa->aabb,pb->aabb))
#endif
{
if(pa>pb) btSwap(pa,pb);
pbp->m_paircache->addOverlappingPair(pa,pb);
}
}
}
};
/* Tree collider */
struct btDbvtTreeCollider : btDbvt::ICollide
{
btDbvtBroadphase* pbp;
btDbvtTreeCollider(btDbvtBroadphase* p) : pbp(p) {}
void Process(const btDbvt::Node* na,const btDbvt::Node* nb)
{
btDbvtProxy* pa=(btDbvtProxy*)na->data;
btDbvtProxy* pb=(btDbvtProxy*)nb->data;
#if DBVT_BP_DISCRETPAIRS
if(Intersect(pa->aabb,pb->aabb))
#endif
{
if(pa>pb) btSwap(pa,pb);
pbp->m_paircache->addOverlappingPair(pa,pb);
}
}
};
@@ -132,112 +118,125 @@ struct btDbvtBroadphaseCollider : btDbvt::ICollide
//
//
btDbvtBroadphase::btDbvtBroadphase()
btDbvtBroadphase::btDbvtBroadphase(btOverlappingPairCache* paircache)
{
m_invalidPair = 0;
m_fcursor = 0;
m_dcursor = 0;
m_stageCurrent = 0;
m_fupdates = 1;
m_dupdates = 0;
//m_paircache = new btSortedOverlappingPairCache();
m_paircache = new btHashedOverlappingPairCache();
m_gid = 0;
m_pid = 0;
for(int i=0;i<=STAGECOUNT;++i)
m_releasepaircache = (paircache!=0)?false:true;
m_stageCurrent = 0;
m_fupdates = 1;
m_dupdates = 1;
m_paircache = paircache?
paircache :
new(btAlignedAlloc(sizeof(btHashedOverlappingPairCache),16)) btHashedOverlappingPairCache();
m_gid = 0;
m_pid = 0;
for(int i=0;i<=STAGECOUNT;++i)
{
m_stageRoots[i]=0;
m_stageRoots[i]=0;
}
#if DBVT_BP_PROFILE
clear(m_profiling);
clear(m_profiling);
#endif
}
//
btDbvtBroadphase::~btDbvtBroadphase()
{
delete m_paircache;
if(m_releasepaircache) btAlignedFree(m_paircache);
}
//
btBroadphaseProxy* btDbvtBroadphase::createProxy( const btVector3& aabbMin,
const btVector3& aabbMax,
int /*shapeType*/,
void* userPtr,
short int collisionFilterGroup,
short int collisionFilterMask,
btDispatcher* /*dispatcher*/,
void* /*multiSapProxy*/)
const btVector3& aabbMax,
int /*shapeType*/,
void* userPtr,
short int collisionFilterGroup,
short int collisionFilterMask,
btDispatcher* /*dispatcher*/,
void* /*multiSapProxy*/)
{
btDbvtProxy* proxy=new btDbvtProxy(userPtr,collisionFilterGroup,collisionFilterMask);
proxy->aabb = btDbvtAabbMm::FromMM(aabbMin,aabbMax);
proxy->leaf = m_sets[0].insert(proxy->aabb,proxy);
proxy->stage = m_stageCurrent;
proxy->m_uniqueId = ++m_gid;
listappend(proxy,m_stageRoots[m_stageCurrent]);
return(proxy);
btDbvtProxy* proxy=new(btAlignedAlloc(sizeof(btDbvtProxy),16)) btDbvtProxy( userPtr,
collisionFilterGroup,
collisionFilterMask);
proxy->aabb = btDbvtAabbMm::FromMM(aabbMin,aabbMax);
proxy->leaf = m_sets[0].insert(proxy->aabb,proxy);
proxy->stage = m_stageCurrent;
proxy->m_uniqueId = ++m_gid;
listappend(proxy,m_stageRoots[m_stageCurrent]);
return(proxy);
}
//
void btDbvtBroadphase::destroyProxy( btBroadphaseProxy* absproxy,
btDispatcher* dispatcher)
btDispatcher* dispatcher)
{
btDbvtProxy* proxy=(btDbvtProxy*)absproxy;
if(proxy->stage==STAGECOUNT)
m_sets[1].remove(proxy->leaf);
btDbvtProxy* proxy=(btDbvtProxy*)absproxy;
if(proxy->stage==STAGECOUNT)
m_sets[1].remove(proxy->leaf);
else
m_sets[0].remove(proxy->leaf);
listremove(proxy,m_stageRoots[proxy->stage]);
m_paircache->removeOverlappingPairsContainingProxy(proxy,dispatcher);
delete proxy;
m_sets[0].remove(proxy->leaf);
listremove(proxy,m_stageRoots[proxy->stage]);
m_paircache->removeOverlappingPairsContainingProxy(proxy,dispatcher);
btAlignedFree(proxy);
}
//
void btDbvtBroadphase::setAabb( btBroadphaseProxy* absproxy,
const btVector3& aabbMin,
const btVector3& aabbMax,
btDispatcher* /*dispatcher*/)
const btVector3& aabbMin,
const btVector3& aabbMax,
btDispatcher* /*dispatcher*/)
{
btDbvtProxy* proxy=(btDbvtProxy*)absproxy;
btDbvtAabbMm aabb=btDbvtAabbMm::FromMM(aabbMin,aabbMax);
if(proxy->stage==STAGECOUNT)
btDbvtProxy* proxy=(btDbvtProxy*)absproxy;
btDbvtAabbMm aabb=btDbvtAabbMm::FromMM(aabbMin,aabbMax);
if(proxy->stage==STAGECOUNT)
{/* fixed -> dynamic set */
m_sets[1].remove(proxy->leaf);
proxy->leaf=m_sets[0].insert(aabb,proxy);
m_fcursor=0;
m_sets[1].remove(proxy->leaf);
proxy->leaf=m_sets[0].insert(aabb,proxy);
}
else
{/* dynamic set */
if(Intersect(proxy->leaf->volume,aabb))
{/* Moving */
const btVector3 delta=(aabbMin+aabbMax)/2-proxy->aabb.Center();
m_sets[0].update(proxy->leaf,aabb,delta*PREDICTED_FRAMES,DBVT_BP_MARGIN);
#ifdef DBVT_BP_MARGIN
m_sets[0].update(proxy->leaf,aabb,delta*PREDICTED_FRAMES,DBVT_BP_MARGIN);
#else
m_sets[0].update(proxy->leaf,aabb,delta*PREDICTED_FRAMES);
#endif
}
else
{/* Teleporting */
m_sets[0].update(proxy->leaf,aabb);
}
}
listremove(proxy,m_stageRoots[proxy->stage]);
proxy->aabb = aabb;
proxy->stage = m_stageCurrent;
listappend(proxy,m_stageRoots[m_stageCurrent]);
listremove(proxy,m_stageRoots[proxy->stage]);
proxy->aabb = aabb;
proxy->stage = m_stageCurrent;
listappend(proxy,m_stageRoots[m_stageCurrent]);
}
//
void btDbvtBroadphase::calculateOverlappingPairs(btDispatcher* dispatcher)
{
collide(dispatcher);
collide(dispatcher);
#if DBVT_BP_PROFILE
if(0==(m_pid%DBVT_BP_PROFILING_RATE))
if(0==(m_pid%DBVT_BP_PROFILING_RATE))
{
printf("fixed(%u) dynamics(%u) pairs(%u)\r\n",m_sets[1].m_leafs,m_sets[0].m_leafs,m_paircache->getNumOverlappingPairs());
unsigned int total=m_profiling.m_total;
if(total<=0) total=1;
printf("ddcollide: %u%% (%uus)\r\n",(50+m_profiling.m_ddcollide*100)/total,m_profiling.m_ddcollide/DBVT_BP_PROFILING_RATE);
printf("fdcollide: %u%% (%uus)\r\n",(50+m_profiling.m_fdcollide*100)/total,m_profiling.m_fdcollide/DBVT_BP_PROFILING_RATE);
printf("cleanup: %u%% (%uus)\r\n",(50+m_profiling.m_cleanup*100)/total,m_profiling.m_cleanup/DBVT_BP_PROFILING_RATE);
printf("total: %uus\r\n",total/DBVT_BP_PROFILING_RATE);
const unsigned long sum=m_profiling.m_ddcollide+
m_profiling.m_fdcollide+
m_profiling.m_cleanup;
printf("leaked: %u%% (%uus)\r\n",100-((50+sum*100)/total),(total-sum)/DBVT_BP_PROFILING_RATE);
clear(m_profiling);
m_clock.reset();
printf("fixed(%u) dynamics(%u) pairs(%u)\r\n",m_sets[1].m_leafs,m_sets[0].m_leafs,m_paircache->getNumOverlappingPairs());
printf("mode: %s\r\n",m_mode==MODE_FULL?"full":"incremental");
printf("cleanup: %s\r\n",m_cleanupmode==CLEANUP_FULL?"full":"incremental");
unsigned int total=m_profiling.m_total;
if(total<=0) total=1;
printf("ddcollide: %u%% (%uus)\r\n",(50+m_profiling.m_ddcollide*100)/total,m_profiling.m_ddcollide/DBVT_BP_PROFILING_RATE);
printf("fdcollide: %u%% (%uus)\r\n",(50+m_profiling.m_fdcollide*100)/total,m_profiling.m_fdcollide/DBVT_BP_PROFILING_RATE);
printf("cleanup: %u%% (%uus)\r\n",(50+m_profiling.m_cleanup*100)/total,m_profiling.m_cleanup/DBVT_BP_PROFILING_RATE);
printf("total: %uus\r\n",total/DBVT_BP_PROFILING_RATE);
const unsigned long sum=m_profiling.m_ddcollide+
m_profiling.m_fdcollide+
m_profiling.m_cleanup;
printf("leaked: %u%% (%uus)\r\n",100-((50+sum*100)/total),(total-sum)/DBVT_BP_PROFILING_RATE);
printf("job counts: %u%%\r\n",(m_profiling.m_jobcount*100)/((m_sets[0].m_leafs+m_sets[1].m_leafs)*DBVT_BP_PROFILING_RATE));
clear(m_profiling);
m_clock.reset();
}
#endif
}
@@ -245,194 +244,88 @@ void btDbvtBroadphase::calculateOverlappingPairs(btDispatcher* dispatcher)
//
void btDbvtBroadphase::collide(btDispatcher* dispatcher)
{
SPC(m_profiling.m_total);
/* refine dynamic */
if(m_stageRoots[m_stageCurrent]&&(m_dupdates>0))
SPC(m_profiling.m_total);
/* optimize */
m_sets[0].optimizeIncremental(1+(m_sets[0].m_leafs*m_dupdates)/100);
m_sets[1].optimizeIncremental(1+(m_sets[1].m_leafs*m_fupdates)/100);
/* dynamic -> fixed set */
m_stageCurrent=(m_stageCurrent+1)%STAGECOUNT;
btDbvtProxy* current=m_stageRoots[m_stageCurrent];
if(current)
{
const int count=1+(m_sets[0].m_leafs*m_dupdates)/100;
for(int i=0;i<count;++i)
{
if(!m_dcursor)
m_dcursor=m_stageRoots[m_stageCurrent];
m_sets[0].update(m_dcursor->leaf);
m_dcursor=m_dcursor->links[1];
}
}
/* dynamic -> fixed set */
m_stageCurrent=(m_stageCurrent+1)%STAGECOUNT;
btDbvtProxy* current=m_stageRoots[m_stageCurrent];
if(current)
{
btDbvtBroadphaseCollider collider(this,m_pid);
do {
btDbvtProxy* next=current->links[1];
if(m_dcursor==current) m_dcursor=0;
listremove(current,m_stageRoots[current->stage]);
listappend(current,m_stageRoots[STAGECOUNT]);
m_sets[1].collideGeneric(current->leaf,&collider);
m_sets[0].remove(current->leaf);
current->leaf = m_sets[1].insert(current->aabb,current);
current->stage = STAGECOUNT;
current = next;
btDbvtTreeCollider collider(this);
do {
btDbvtProxy* next=current->links[1];
listremove(current,m_stageRoots[current->stage]);
listappend(current,m_stageRoots[STAGECOUNT]);
btDbvt::collideTT(m_sets[1].m_root,current->leaf,collider);
m_sets[0].remove(current->leaf);
current->leaf = m_sets[1].insert(current->aabb,current);
current->stage = STAGECOUNT;
current = next;
} while(current);
}
/* refine fixed */
if(m_stageRoots[STAGECOUNT]&&(m_fupdates>0))
/* collide dynamics */
{
const int count=1+(m_sets[1].m_leafs*m_fupdates)/100;
for(int i=0;i<count;++i)
btDbvtTreeCollider collider(this);
{
if(!m_fcursor) m_fcursor=m_stageRoots[STAGECOUNT];
m_sets[1].update(m_fcursor->leaf);
m_fcursor=m_fcursor->links[1];
}
}
/* collide dynamics */
btDbvtBroadphaseCollider collider(this,m_pid);
{
SPC(m_profiling.m_fdcollide);
m_sets[0].collideGeneric(&m_sets[1],&collider);
}
{
btDbvt::collideTT(m_sets[0].m_root,m_sets[1].m_root,collider);
}
{
SPC(m_profiling.m_ddcollide);
m_sets[0].collideGeneric(&m_sets[0],&collider);
btDbvt::collideTT(m_sets[0].m_root,m_sets[0].m_root,collider);
}
}
/* clean up */
///m_paircache->processAllOverlappingPairs(0,dispatcher);
/* clean up */
{
SPC(m_profiling.m_cleanup);
if (!m_paircache->hasDeferredRemoval())
SPC(m_profiling.m_cleanup);
btBroadphasePairArray& pairs=m_paircache->getOverlappingPairArray();
if(pairs.size()>0)
{
btBroadphasePairArray& pairs=m_paircache->getOverlappingPairArray();
for(int i=0,ni=pairs.size();i<ni;++i)
for(int i=0,ni=pairs.size();i<ni;++i)
{
btBroadphasePair& p=pairs[i];
if(m_pid!=(*(int*)&p.m_userInfo))
btBroadphasePair& p=pairs[i];
btDbvtProxy* pa=(btDbvtProxy*)p.m_pProxy0;
btDbvtProxy* pb=(btDbvtProxy*)p.m_pProxy1;
if(!Intersect(pa->aabb,pb->aabb))
{
btDbvtProxy* pa=(btDbvtProxy*)p.m_pProxy0;
btDbvtProxy* pb=(btDbvtProxy*)p.m_pProxy1;
if(!Intersect(pa->aabb,pb->aabb))
{
m_paircache->removeOverlappingPair(pa,pb,dispatcher);
--ni;--i;
}
if(pa>pb) btSwap(pa,pb);
m_paircache->removeOverlappingPair(pa,pb,dispatcher);
--ni;--i;
}
}
} else
{
if ( m_paircache->hasDeferredRemoval())
{
btBroadphasePairArray& overlappingPairArray = m_paircache->getOverlappingPairArray();
//perform a sort, to find duplicates and to sort 'invalid' pairs to the end
overlappingPairArray.quickSort(btBroadphasePairSortPredicate());
overlappingPairArray.resize(overlappingPairArray.size() - m_invalidPair);
m_invalidPair = 0;
btBroadphasePair previousPair;
previousPair.m_pProxy0 = 0;
previousPair.m_pProxy1 = 0;
previousPair.m_algorithm = 0;
int i;
for (i=0;i<overlappingPairArray.size();i++)
{
btBroadphasePair& pair = overlappingPairArray[i];
bool isDuplicate = (pair == previousPair);
previousPair = pair;
bool needsRemoval = false;
if (!isDuplicate)
{
btDbvtProxy* pa=(btDbvtProxy*)pair.m_pProxy0;
btDbvtProxy* pb=(btDbvtProxy*)pair.m_pProxy1;
bool hasOverlap = Intersect(pa->aabb,pb->aabb);
if (hasOverlap)
{
needsRemoval = false;//callback->processOverlap(pair);
} else
{
needsRemoval = true;
}
} else
{
//remove duplicate
needsRemoval = true;
//should have no algorithm
btAssert(!pair.m_algorithm);
}
if (needsRemoval)
{
m_paircache->cleanOverlappingPair(pair,dispatcher);
// m_overlappingPairArray.swap(i,m_overlappingPairArray.size()-1);
// m_overlappingPairArray.pop_back();
pair.m_pProxy0 = 0;
pair.m_pProxy1 = 0;
m_invalidPair++;
}
}
///if you don't like to skip the invalid pairs in the array, execute following code:
#define CLEAN_INVALID_PAIRS 1
#ifdef CLEAN_INVALID_PAIRS
//perform a sort, to sort 'invalid' pairs to the end
overlappingPairArray.quickSort(btBroadphasePairSortPredicate());
overlappingPairArray.resize(overlappingPairArray.size() - m_invalidPair);
m_invalidPair = 0;
#endif//CLEAN_INVALID_PAIRS
}
}
}
++m_pid;
++m_pid;
}
//
btOverlappingPairCache* btDbvtBroadphase::getOverlappingPairCache()
{
return(m_paircache);
return(m_paircache);
}
//
const btOverlappingPairCache* btDbvtBroadphase::getOverlappingPairCache() const
{
return(m_paircache);
return(m_paircache);
}
//
void btDbvtBroadphase::getBroadphaseAabb(btVector3& aabbMin,btVector3& aabbMax) const
{
btDbvtAabbMm bounds;
if(!m_sets[0].empty())
if(!m_sets[1].empty()) Merge( m_sets[0].m_root->volume,
m_sets[1].m_root->volume,bounds);
else
bounds=m_sets[0].m_root->volume;
else if(!m_sets[1].empty()) bounds=m_sets[1].m_root->volume;
else
bounds=btDbvtAabbMm::FromCR(btVector3(0,0,0),0);
aabbMin=bounds.Mins();
aabbMax=bounds.Maxs();
btDbvtAabbMm bounds;
if(!m_sets[0].empty())
if(!m_sets[1].empty()) Merge( m_sets[0].m_root->volume,
m_sets[1].m_root->volume,bounds);
else
bounds=m_sets[0].m_root->volume;
else if(!m_sets[1].empty()) bounds=m_sets[1].m_root->volume;
else
bounds=btDbvtAabbMm::FromCR(btVector3(0,0,0),0);
aabbMin=bounds.Mins();
aabbMax=bounds.Maxs();
}
//
@@ -441,4 +334,4 @@ void btDbvtBroadphase::printStats()
#if DBVT_BP_PROFILE
#undef SPC
#endif
#endif

100
src/BulletCollision/BroadphaseCollision/btDbvtBroadphase.h
View File

@@ -13,31 +13,23 @@ subject to the following restrictions:
3. This notice may not be removed or altered from any source distribution.
*/
///btDbvtBroadphase implementation by Nathanael Presson
#ifndef BT_DBVT_BROADPHASE_H
#define BT_DBVT_BROADPHASE_H
#include "btDbvt.h"
#include "LinearMath/btPoint3.h"
#include "LinearMath/btVector3.h"
#include "BulletCollision/BroadphaseCollision/btDbvt.h"
#include "BulletCollision/BroadphaseCollision/btOverlappingPairCache.h"
#include "BulletCollision/BroadphaseCollision/btBroadphaseInterface.h"
#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h"
#include "BulletCollision/BroadphaseCollision/btOverlappingPairCallback.h"
#include "BulletCollision/BroadphaseCollision/btOverlappingPairCallback.h"
//
// Compile time config
//
//#define DBVT_BP_PROFILE 1
#define DBVT_BP_DISCRETPAIRS 0
#define DBVT_BP_PROFILE 0
#define DBVT_BP_DISCRETPAIRS 1
#define DBVT_BP_MARGIN (btScalar)0.05
#if DBVT_BP_PROFILE
#define DBVT_BP_PROFILING_RATE 50
#include "LinearMath/btQuickprof.h"
#define DBVT_BP_PROFILING_RATE 256
#include "LinearMath/btQuickprof.h"
#endif
//
@@ -45,66 +37,66 @@ subject to the following restrictions:
//
struct btDbvtProxy : btBroadphaseProxy
{
/* Fields */
btDbvtAabbMm aabb;
btDbvt::Node* leaf;
btDbvtProxy* links[2];
int stage;
/* ctor */
btDbvtProxy(void* userPtr,short int collisionFilterGroup, short int collisionFilterMask) :
/* Fields */
btDbvtAabbMm aabb;
btDbvt::Node* leaf;
btDbvtProxy* links[2];
int stage;
/* ctor */
btDbvtProxy(void* userPtr,short int collisionFilterGroup, short int collisionFilterMask) :
btBroadphaseProxy(userPtr,collisionFilterGroup,collisionFilterMask)
{
links[0]=links[1]=0;
links[0]=links[1]=0;
}
};
typedef btAlignedObjectArray<btDbvtProxy*> btDbvtProxyArray;
//
// btDbvtBroadphase
//
struct btDbvtBroadphase : btBroadphaseInterface
{
/* Config */
enum {
/* Config */
enum {
DYNAMIC_SET = 0, /* Dynamic set index */
FIXED_SET = 1, /* Fixed set index */
STAGECOUNT = 2, /* Number of stages */
PREDICTED_FRAMES = 2 /* Frames prediction */
};
/* Fields */
btDbvt m_sets[2]; // Dbvt sets
btDbvtProxy* m_stageRoots[STAGECOUNT+1]; // Stages list
int m_stageCurrent; // Current stage
btOverlappingPairCache* m_paircache; // Pair cache
btDbvtProxy* m_fcursor; // Current fixed cursor
btDbvtProxy* m_dcursor; // Current dynamic cursor
int m_fupdates; // % of fixed updates per frame
int m_dupdates; // % of dynamic updates per frame
int m_pid; // Parse id
int m_gid; // Gen id
int m_invalidPair;
PREDICTED_FRAMES = 2 /* Frames prediction */
};
/* Fields */
btDbvt m_sets[2]; // Dbvt sets
btDbvtProxy* m_stageRoots[STAGECOUNT+1]; // Stages list
btOverlappingPairCache* m_paircache; // Pair cache
int m_stageCurrent; // Current stage
int m_fupdates; // % of fixed updates per frame
int m_dupdates; // % of dynamic updates per frame
int m_pid; // Parse id
int m_gid; // Gen id
bool m_releasepaircache; // Release pair cache on delete
#if DBVT_BP_PROFILE
btClock m_clock;
struct {
btClock m_clock;
struct {
unsigned long m_total;
unsigned long m_ddcollide;
unsigned long m_fdcollide;
unsigned long m_cleanup;
} m_profiling;
unsigned long m_jobcount;
} m_profiling;
#endif
/* Methods */
btDbvtBroadphase();
~btDbvtBroadphase();
void collide(btDispatcher* dispatcher);
/* btBroadphaseInterface Implementation */
btBroadphaseProxy* createProxy(const btVector3& aabbMin,const btVector3& aabbMax,int shapeType,void* userPtr,short int collisionFilterGroup,short int collisionFilterMask,btDispatcher* dispatcher,void* multiSapProxy);
void destroyProxy(btBroadphaseProxy* proxy,btDispatcher* dispatcher);
void setAabb(btBroadphaseProxy* proxy,const btVector3& aabbMin,const btVector3& aabbMax,btDispatcher* dispatcher);
void calculateOverlappingPairs(btDispatcher* dispatcher);
btOverlappingPairCache* getOverlappingPairCache();
const btOverlappingPairCache* getOverlappingPairCache() const;
void getBroadphaseAabb(btVector3& aabbMin,btVector3& aabbMax) const;
void printStats();
/* Methods */
btDbvtBroadphase(btOverlappingPairCache* paircache=0);
~btDbvtBroadphase();
void collide(btDispatcher* dispatcher);
/* btBroadphaseInterface Implementation */
btBroadphaseProxy* createProxy(const btVector3& aabbMin,const btVector3& aabbMax,int shapeType,void* userPtr,short int collisionFilterGroup,short int collisionFilterMask,btDispatcher* dispatcher,void* multiSapProxy);
void destroyProxy(btBroadphaseProxy* proxy,btDispatcher* dispatcher);
void setAabb(btBroadphaseProxy* proxy,const btVector3& aabbMin,const btVector3& aabbMax,btDispatcher* dispatcher);
void calculateOverlappingPairs(btDispatcher* dispatcher);
btOverlappingPairCache* getOverlappingPairCache();
const btOverlappingPairCache* getOverlappingPairCache() const;
void getBroadphaseAabb(btVector3& aabbMin,btVector3& aabbMax) const;
void printStats();
};
#endif