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

557
src/BulletSoftBody/btSoftBody.cpp
View File

@@ -15,13 +15,7 @@ subject to the following restrictions:
///btSoftBody implementation by Nathanael Presson
#include "btSoftBody.h"
#if 1
#include <stdio.h>
#define DOTRACE
#endif
#include <string.h>
#include "LinearMath/btQuickprof.h"
#include "BulletCollision/NarrowPhaseCollision/btGjkEpa2.h"
#include "BulletCollision/BroadphaseCollision/btBroadphaseInterface.h"
#include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h"
@@ -136,17 +130,6 @@ void btSoftBodyCollisionShape::processAllTriangles(btTriangleCallback* /*callbac
//
//
#ifdef DOTRACE
static inline void Trace(const btMatrix3x3& m,const char* name)
{
printf("%s[0]: %.2f,\t%.2f,\t%.2f\r\n",name,m[0].x(),m[0].y(),m[0].z());
printf("%s[1]: %.2f,\t%.2f,\t%.2f\r\n",name,m[1].x(),m[1].y(),m[1].z());
printf("%s[2]: %.2f,\t%.2f,\t%.2f\r\n",name,m[2].x(),m[2].y(),m[2].z());
printf("\r\n");
}
#else
static inline void Trace(const btMatrix3x3&,const char*) {}
#endif
//
template <typename T>
@@ -199,6 +182,10 @@ template <typename T>
static inline T Sign(const T& x)
{ return((T)(x<0?-1:+1)); }
//
template <typename T>
static inline bool SameSign(const T& x,const T& y)
{ return((x*y)>0); }
//
static inline btMatrix3x3 ScaleAlongAxis(const btVector3& a,btScalar s)
{
const btScalar xx=a.x()*a.x();
@@ -456,17 +443,6 @@ static void PointersToIndices(btSoftBody* psb)
psb->m_faces[i].m_leaf->data=*(void**)&i;
}
}
for(int i=0,ni=psb->m_tetras.size();i<ni;++i)
{
psb->m_tetras[i].m_n[0]=PTR2IDX(psb->m_tetras[i].m_n[0],base);
psb->m_tetras[i].m_n[1]=PTR2IDX(psb->m_tetras[i].m_n[1],base);
psb->m_tetras[i].m_n[2]=PTR2IDX(psb->m_tetras[i].m_n[2],base);
psb->m_tetras[i].m_n[3]=PTR2IDX(psb->m_tetras[i].m_n[3],base);
if(psb->m_tetras[i].m_leaf)
{
psb->m_tetras[i].m_leaf->data=*(void**)&i;
}
}
for(int i=0,ni=psb->m_anchors.size();i<ni;++i)
{
psb->m_anchors[i].m_node=PTR2IDX(psb->m_anchors[i].m_node,base);
@@ -509,17 +485,6 @@ static void IndicesToPointers(btSoftBody* psb,const int* map=0)
psb->m_faces[i].m_leaf->data=&psb->m_faces[i];
}
}
for(int i=0,ni=psb->m_tetras.size();i<ni;++i)
{
psb->m_tetras[i].m_n[0]=IDX2PTR(psb->m_tetras[i].m_n[0],base);
psb->m_tetras[i].m_n[1]=IDX2PTR(psb->m_tetras[i].m_n[1],base);
psb->m_tetras[i].m_n[2]=IDX2PTR(psb->m_tetras[i].m_n[2],base);
psb->m_tetras[i].m_n[3]=IDX2PTR(psb->m_tetras[i].m_n[3],base);
if(psb->m_tetras[i].m_leaf)
{
psb->m_tetras[i].m_leaf->data=&psb->m_tetras[i];
}
}
for(int i=0,ni=psb->m_anchors.size();i<ni;++i)
{
psb->m_anchors[i].m_node=IDX2PTR(psb->m_anchors[i].m_node,base);
@@ -605,11 +570,10 @@ static inline btScalar RayTriangle(const btVector3& org,
// Private implementation
//
struct RayCaster : public btDbvt::ICollide
struct RayCaster : btDbvt::ICollide
{
btVector3 o;
btVector3 d;
btVector3 nd;
btScalar mint;
btSoftBody::Face* face;
int tests;
@@ -617,18 +581,11 @@ struct RayCaster : public btDbvt::ICollide
{
o = org;
d = dir;
nd = dir.normalized();
mint = mxt;
face = 0;
tests = 0;
}
virtual void Process(const btDbvt::Node* /*a*/,const btDbvt::Node* /*b*/)
{
}
virtual void Process(const btDbvt::Node* leaf)
void Process(const btDbvt::Node* leaf)
{
btSoftBody::Face& f=*(btSoftBody::Face*)leaf->data;
const btScalar t=RayTriangle( o,d,
@@ -643,13 +600,6 @@ struct RayCaster : public btDbvt::ICollide
}
++tests;
}
virtual bool Descent(const btDbvt::Node* node)
{
const btVector3 ctr=node->volume.Center()-o;
const btScalar sqr=node->volume.Lengths().length2()/4;
const btScalar prj=dot(ctr,nd);
return((ctr-(nd*prj)).length2()<=sqr);
}
};
@@ -665,55 +615,22 @@ static int RaycastInternal(const btSoftBody* psb,
int cnt=0;
if(bcountonly||psb->m_fdbvt.empty())
{/* Full search */
if(!psb->m_faces.size())
{/* Tetras */
for(int i=0,ni=psb->m_tetras.size();i<ni;++i)
{
const btSoftBody::Tetra& t=psb->m_tetras[i];
const btSoftBody::Node* const* n=t.m_n;
const int f[]={ 0,1,2,
0,1,3,
1,2,3,
2,0,3};
for(int j=0;j<12;j+=3)
{
const btScalar t=RayTriangle( org,dir,
n[f[j+0]]->m_x,
n[f[j+1]]->m_x,
n[f[j+2]]->m_x,
for(int i=0,ni=psb->m_faces.size();i<ni;++i)
{
const btSoftBody::Face& f=psb->m_faces[i];
const btScalar t=RayTriangle( org,dir,
f.m_n[0]->m_x,
f.m_n[1]->m_x,
f.m_n[2]->m_x,
mint);
if(t>0)
{
++cnt;
if(!bcountonly)
{
feature=btSoftBody::eFeature::Tetra;
index=-1;
mint=t;
}
}
}
}
}
else
{/* Faces */
for(int i=0,ni=psb->m_faces.size();i<ni;++i)
if(t>0)
{
const btSoftBody::Face& f=psb->m_faces[i];
const btScalar t=RayTriangle( org,dir,
f.m_n[0]->m_x,
f.m_n[1]->m_x,
f.m_n[2]->m_x,
mint);
if(t>0)
++cnt;
if(!bcountonly)
{
++cnt;
if(!bcountonly)
{
feature=btSoftBody::eFeature::Face;
index=i;
mint=t;
}
feature=btSoftBody::eFeature::Face;
index=i;
mint=t;
}
}
}
@@ -721,7 +638,7 @@ static int RaycastInternal(const btSoftBody* psb,
else
{/* Use dbvt */
RayCaster collider(org,dir,mint);
psb->m_fdbvt.collideGeneric(&collider);
btDbvt::collideRAY(psb->m_fdbvt.m_root,org,dir,collider);
if(collider.face)
{
mint=collider.mint;
@@ -742,7 +659,6 @@ for(int i=0;i<psb->m_faces.size();++i)
btSoftBody::Face& f=psb->m_faces[i];
f.m_leaf=psb->m_fdbvt.insert(VolumeOf(f,0),&f);
}
psb->m_fdbvt.optimizeTopDown();
}
//
@@ -910,15 +826,6 @@ static void UpdateConstants(btSoftBody* psb)
btSoftBody::Face& f=psb->m_faces[i];
f.m_ra = AreaOf(f.m_n[0]->m_x,f.m_n[1]->m_x,f.m_n[2]->m_x);
}
/* Tetras */
for(int i=0,ni=psb->m_tetras.size();i<ni;++i)
{
btSoftBody::Tetra& t=psb->m_tetras[i];
btSoftBody::Material& m=*t.m_material;
btSoftBody::Node** n=t.m_n;
t.m_rv = VolumeOf(n[0]->m_x,n[1]->m_x,n[2]->m_x,n[3]->m_x);
t.m_c1 = (4*m.m_kVST)/(n[0]->m_im+n[1]->m_im+n[2]->m_im+n[3]->m_im);
}
/* Area's */
btAlignedObjectArray<int> counts;
counts.resize(psb->m_nodes.size(),0);
@@ -1081,9 +988,9 @@ static void ApplyForces(btSoftBody* psb,btScalar dt)
}
//
static void PSolve_Anchors(btSoftBody* psb)
static void PSolve_Anchors(btSoftBody* psb,btScalar kst)
{
const btScalar kAHR=psb->m_cfg.kAHR;
const btScalar kAHR=psb->m_cfg.kAHR*kst;
const btScalar dt=psb->m_sst.sdt;
for(int i=0,ni=psb->m_anchors.size();i<ni;++i)
{
@@ -1101,9 +1008,10 @@ static void PSolve_Anchors(btSoftBody* psb)
}
//
static void PSolve_RContacts(btSoftBody* psb)
static void PSolve_RContacts(btSoftBody* psb,btScalar kst)
{
const btScalar dt=psb->m_sst.sdt;
const btScalar mrg=psb->getCollisionShape()->getMargin();
for(int i=0,ni=psb->m_rcontacts.size();i<ni;++i)
{
const btSoftBody::RContact& c=psb->m_rcontacts[i];
@@ -1114,9 +1022,9 @@ static void PSolve_RContacts(btSoftBody* psb)
const btScalar dn=dot(vr,cti.m_normal);
if(dn<=SIMD_EPSILON)
{
const btScalar dp=dot(c.m_node->m_x,cti.m_normal)+cti.m_offset;
const btScalar dp=btMin(dot(c.m_node->m_x,cti.m_normal)+cti.m_offset,mrg);
const btVector3 fv=vr-cti.m_normal*dn;
const btVector3 impulse=c.m_c0*(vr-fv*c.m_c3+cti.m_normal*(dp*c.m_c4));
const btVector3 impulse=c.m_c0*((vr-fv*c.m_c3+cti.m_normal*(dp*c.m_c4))*kst);
c.m_node->m_x-=impulse*c.m_c2;
c.m_cti.m_body->applyImpulse(impulse,c.m_c1);
}
@@ -1124,7 +1032,7 @@ static void PSolve_RContacts(btSoftBody* psb)
}
//
static void PSolve_SContacts(btSoftBody* psb)
static void PSolve_SContacts(btSoftBody* psb,btScalar)
{
for(int i=0,ni=psb->m_scontacts.size();i<ni;++i)
{
@@ -1156,7 +1064,7 @@ for(int i=0,ni=psb->m_scontacts.size();i<ni;++i)
}
//
static void PSolve_Links(btSoftBody* psb)
static void PSolve_Links(btSoftBody* psb,btScalar kst)
{
for(int i=0,ni=psb->m_links.size();i<ni;++i)
{
@@ -1167,7 +1075,7 @@ static void PSolve_Links(btSoftBody* psb)
btSoftBody::Node& b=*l.m_n[1];
const btVector3 del=b.m_x-a.m_x;
const btScalar len=del.length2();
const btScalar k=((l.m_c1-len)/(l.m_c0*(l.m_c1+len)));
const btScalar k=((l.m_c1-len)/(l.m_c0*(l.m_c1+len)))*kst;
a.m_x-=del*(k*a.m_im);
b.m_x+=del*(k*b.m_im);
}
@@ -1175,72 +1083,30 @@ static void PSolve_Links(btSoftBody* psb)
}
//
static void PSolve_Tetras(btSoftBody* psb)
{
for(int i=0,ni=psb->m_tetras.size();i<ni;++i)
{
btSoftBody::Tetra& t=psb->m_tetras[i];
btSoftBody::Node** n=t.m_n;
const btVector3 g0=cross(n[3]->m_x-n[1]->m_x,n[2]->m_x-n[1]->m_x);
const btVector3 g1=cross(n[3]->m_x-n[2]->m_x,n[0]->m_x-n[2]->m_x);
const btVector3 g2=cross(n[3]->m_x-n[0]->m_x,n[1]->m_x-n[0]->m_x);
const btVector3 g3=cross(n[1]->m_x-n[0]->m_x,n[2]->m_x-n[0]->m_x);
const btScalar v1=VolumeOf(n[0]->m_x,n[1]->m_x,n[2]->m_x,n[3]->m_x);
const btScalar k=(t.m_rv-v1)/(g0.length2()+g1.length2()+g2.length2()+g3.length2());
const btScalar j=k*t.m_c1;
n[0]->m_x += g0*j*n[0]->m_im;
n[1]->m_x += g1*j*n[1]->m_im;
n[2]->m_x += g2*j*n[2]->m_im;
n[3]->m_x += g3*j*n[3]->m_im;
}
}
//
static void VSolve_Links(btSoftBody* psb)
static void VSolve_Links(btSoftBody* psb,btScalar kst)
{
for(int i=0,ni=psb->m_links.size();i<ni;++i)
{
btSoftBody::Link& l=psb->m_links[i];
btSoftBody::Node** n=l.m_n;
const btScalar j=-dot(l.m_c3,n[0]->m_v-n[1]->m_v)*l.m_c2;
const btScalar j=-dot(l.m_c3,n[0]->m_v-n[1]->m_v)*l.m_c2*kst;
n[0]->m_v+= l.m_c3*(j*n[0]->m_im);
n[1]->m_v-= l.m_c3*(j*n[1]->m_im);
}
}
//
static void VSolve_Tetras(btSoftBody* psb)
{
for(int i=0,ni=psb->m_tetras.size();i<ni;++i)
{
btSoftBody::Tetra& t=psb->m_tetras[i];
btSoftBody::Node** n=t.m_n;
const btScalar r= dot(t.m_c0[0],n[0]->m_v)+
dot(t.m_c0[1],n[1]->m_v)+
dot(t.m_c0[2],n[2]->m_v)+
dot(t.m_c0[3],n[3]->m_v);
const btScalar j=r*t.m_c2;
n[0]->m_v += t.m_c0[0]*(j*n[0]->m_im);
n[1]->m_v += t.m_c0[1]*(j*n[1]->m_im);
n[2]->m_v += t.m_c0[2]*(j*n[2]->m_im);
n[3]->m_v += t.m_c0[3]*(j*n[3]->m_im);
}
}
//
static void (* const VSolvers[])(btSoftBody*)= {
VSolve_Links,
VSolve_Tetras,
};
static void (* const VSolvers[])(btSoftBody*,btScalar)= {
VSolve_Links,
};
//
static void (* const PSolvers[])(btSoftBody*)= {
PSolve_Links,
PSolve_Tetras,
PSolve_Anchors,
PSolve_RContacts,
PSolve_SContacts,
};
static void (* const PSolvers[])(btSoftBody*,btScalar)= {
PSolve_Links,
PSolve_Anchors,
PSolve_RContacts,
PSolve_SContacts,
};
//
// btSoftBody
@@ -1262,7 +1128,7 @@ btSoftBody::btSoftBody(btSoftBody::btSoftBodyWorldInfo* worldInfo,int node_count
m_cfg.kDF = (btScalar)0.2;
m_cfg.kMT = 0;
m_cfg.kCHR = (btScalar)1.0;
m_cfg.kKHR = (btScalar)0.5;
m_cfg.kKHR = (btScalar)0.1;
m_cfg.kSHR = (btScalar)1.0;
m_cfg.kAHR = (btScalar)0.7;
m_cfg.maxvolume = (btScalar)1;
@@ -1308,7 +1174,6 @@ btSoftBody::btSoftBody(btSoftBody::btSoftBodyWorldInfo* worldInfo,int node_count
n.m_leaf = m_ndbvt.insert(btDbvt::Volume::FromCR(n.m_x,margin),&n);
n.m_material= pm;
}
m_ndbvt.optimizeTopDown();
UpdateBounds(this);
@@ -1319,7 +1184,7 @@ btSoftBody::~btSoftBody()
{
//for now, delete the internal shape
delete m_collisionShape;
for(int i=0;i<m_materials.size();++i) delete m_materials[i];
for(int i=0;i<m_materials.size();++i) btAlignedFree(m_materials[i]);
}
//
@@ -1368,7 +1233,7 @@ bool btSoftBody::checkFace(int node0,int node1,int node2) const
//
btSoftBody::Material* btSoftBody::appendMaterial()
{
Material* pm=new Material();
Material* pm=new(btAlignedAlloc(sizeof(Material),16)) Material();
if(m_materials.size()>0)
*pm=*m_materials[0];
else
@@ -1430,18 +1295,6 @@ appendNote(text,o,btVector4(w,w,w,0), feature->m_n[0],
feature->m_n[1],
feature->m_n[2]);
}
//
void btSoftBody::appendNote( const char* text,
const btVector3& o,
Tetra* feature)
{
static const btScalar w=1/(btScalar)4;
appendNote(text,o,btVector4(w,w,w,w), feature->m_n[0],
feature->m_n[1],
feature->m_n[2],
feature->m_n[3]);
}
//
void btSoftBody::appendNode( const btVector3& x,btScalar m)
@@ -1525,34 +1378,6 @@ void btSoftBody::appendFace(int node0,int node1,int node2,Material* mat)
m_bUpdateRtCst=true;
}
//
void btSoftBody::appendTetra(int model,Material* mat)
{
Tetra t;
if(model>=0)
t=m_tetras[model];
else
{ ZeroInitialize(t);t.m_material=mat?mat:m_materials[0]; }
m_tetras.push_back(t);
}
//
void btSoftBody::appendTetra(int node0,
int node1,
int node2,
int node3,
Material* mat)
{
appendTetra(-1,mat);
Tetra& t=m_tetras[m_tetras.size()-1];
t.m_n[0] = &m_nodes[node0];
t.m_n[1] = &m_nodes[node1];
t.m_n[2] = &m_nodes[node2];
t.m_n[3] = &m_nodes[node3];
t.m_rv = VolumeOf(t.m_n[0]->m_x,t.m_n[1]->m_x,t.m_n[2]->m_x,t.m_n[3]->m_x);
m_bUpdateRtCst=true;
}
//
void btSoftBody::appendAnchor(int node,btRigidBody* body)
{
@@ -1660,49 +1485,6 @@ void btSoftBody::setTotalDensity(btScalar density)
setTotalMass(getVolume()*density,true);
}
//
void btSoftBody::setVolumeMass(btScalar mass)
{
btAlignedObjectArray<btScalar> ranks;
ranks.resize(m_nodes.size(),0);
for(int i=0;i<m_nodes.size();++i)
{
m_nodes[i].m_im=0;
}
for(int i=0;i<m_tetras.size();++i)
{
const Tetra& t=m_tetras[i];
for(int j=0;j<4;++j)
{
t.m_n[j]->m_im+=btFabs(t.m_rv);
ranks[int(t.m_n[j]-&m_nodes[0])]+=1;
}
}
for(int i=0;i<m_nodes.size();++i)
{
if(m_nodes[i].m_im>0)
{
m_nodes[i].m_im=ranks[i]/m_nodes[i].m_im;
}
}
setTotalMass(mass,false);
}
//
void btSoftBody::setVolumeDensity(btScalar density)
{
btScalar volume=0;
for(int i=0;i<m_tetras.size();++i)
{
const Tetra& t=m_tetras[i];
for(int j=0;j<4;++j)
{
volume+=btFabs(t.m_rv);
}
}
setVolumeMass(volume*density/6);
}
//
void btSoftBody::transform(const btTransform& trs)
{
@@ -1715,7 +1497,6 @@ void btSoftBody::transform(const btTransform& trs)
n.m_n=trs.getBasis()*n.m_n;
m_ndbvt.update(n.m_leaf,btDbvt::Volume::FromCR(n.m_x,margin));
}
m_ndbvt.optimizeTopDown();
UpdateNormals(this);
UpdateBounds(this);
UpdateConstants(this);
@@ -1750,7 +1531,6 @@ void btSoftBody::scale(const btVector3& scl)
n.m_q*=scl;
m_ndbvt.update(n.m_leaf,btDbvt::Volume::FromCR(n.m_x,margin));
}
m_ndbvt.optimizeTopDown();
UpdateNormals(this);
UpdateBounds(this);
UpdateConstants(this);
@@ -1870,34 +1650,31 @@ int btSoftBody::generateBendingConstraints(int distance,Material* mat)
if(adj[IDX(i,j)]==(unsigned)distance)
{
appendLink(i,j,mat);
m_links[m_links.size()-1].m_bbending=1;
++nlinks;
}
}
}
delete[] adj;
delete[] adj;
return(nlinks);
}
return(0);
}
//
int btSoftBody::generateTetrahedralConstraints()
{
/// not implemented
return(0);
}
//
void btSoftBody::randomizeConstraints()
{
unsigned long seed=243703;
#define NEXTRAND (seed=(1664525L*seed+1013904223L)&0xffffffff)
for(int i=0,ni=m_links.size();i<ni;++i)
{
btSwap(m_links[i],m_links[std::rand()%ni]);
btSwap(m_links[i],m_links[NEXTRAND%ni]);
}
for(int i=0,ni=m_faces.size();i<ni;++i)
{
btSwap(m_faces[i],m_faces[std::rand()%ni]);
btSwap(m_faces[i],m_faces[NEXTRAND%ni]);
}
#undef NEXTRAND
}
//
@@ -1907,6 +1684,19 @@ const Node* nbase(&m_nodes[0]);
int ncount(m_nodes.size());
btSymMatrix<int> edges(ncount,-2);
int newnodes=0;
/* Filter out */
for(int i=0;i<m_links.size();++i)
{
Link& l=m_links[i];
if(l.m_bbending)
{
if(!SameSign(ifn->Eval(l.m_n[0]->m_x),ifn->Eval(l.m_n[1]->m_x)))
{
btSwap(m_links[i],m_links[m_links.size()-1]);
m_links.pop_back();--i;
}
}
}
/* Fill edges */
for(int i=0;i<m_links.size();++i)
{
@@ -1920,16 +1710,6 @@ for(int i=0;i<m_faces.size();++i)
edges(int(f.m_n[1]-nbase),int(f.m_n[2]-nbase))=-1;
edges(int(f.m_n[2]-nbase),int(f.m_n[0]-nbase))=-1;
}
for(int i=0;i<m_tetras.size();++i)
{
Tetra& t=m_tetras[i];
edges(int(t.m_n[0]-nbase),int(t.m_n[1]-nbase))=-1;
edges(int(t.m_n[1]-nbase),int(t.m_n[2]-nbase))=-1;
edges(int(t.m_n[2]-nbase),int(t.m_n[0]-nbase))=-1;
edges(int(t.m_n[0]-nbase),int(t.m_n[3]-nbase))=-1;
edges(int(t.m_n[1]-nbase),int(t.m_n[3]-nbase))=-1;
edges(int(t.m_n[2]-nbase),int(t.m_n[3]-nbase))=-1;
}
/* Intersect */
for(int i=0;i<ncount;++i)
{
@@ -2027,60 +1807,6 @@ for(int i=0;i<m_faces.size();++i)
}
}
}
/* Refine tetras */
static const int edg[]={0,1,1,2,2,0,0,3,1,3,2,3};
static const int apx[]={2,3,0,3,1,3,1,2,2,0,0,1};
for(int i=0;i<m_tetras.size();++i)
{
const Tetra& feat=m_tetras[i];
const int idx[]={ int(feat.m_n[0]-nbase),
int(feat.m_n[1]-nbase),
int(feat.m_n[2]-nbase),
int(feat.m_n[3]-nbase)};
for(int j=0;j<(sizeof(edg)/sizeof(edg[0]));j+=2)
{
const int ia=idx[edg[j]];
const int ib=idx[edg[j+1]];
if((ia<ncount)&&(ib<ncount))
{
const int ni=edges(ia,ib);
const int xa=idx[apx[j]];
const int xb=idx[apx[j+1]];
if(ni!=-1)
{
appendTetra(i);
Tetra* pft[]={ &m_tetras[i],
&m_tetras[m_tetras.size()-1]};
const btScalar sig=Sign(pft[0]->m_rv);
pft[0]->m_n[0]=&m_nodes[ia];
pft[0]->m_n[1]=&m_nodes[xb];
pft[0]->m_n[2]=&m_nodes[xa];
pft[0]->m_n[3]=&m_nodes[ni];
if(Sign(VolumeOf( pft[0]->m_n[0]->m_x,
pft[0]->m_n[1]->m_x,
pft[0]->m_n[2]->m_x,
pft[0]->m_n[3]->m_x))!=sig)
{
btSwap(pft[0]->m_n[0],pft[0]->m_n[1]);
}
pft[1]->m_n[0]=&m_nodes[ib];
pft[1]->m_n[1]=&m_nodes[xa];
pft[1]->m_n[2]=&m_nodes[xb];
pft[1]->m_n[3]=&m_nodes[ni];
if(Sign(VolumeOf( pft[1]->m_n[0]->m_x,
pft[1]->m_n[1]->m_x,
pft[1]->m_n[2]->m_x,
pft[1]->m_n[3]->m_x))!=sig)
{
btSwap(pft[1]->m_n[0],pft[1]->m_n[1]);
}
appendLink(ni,xa,pft[0]->m_material,true);
appendLink(ni,xb,pft[0]->m_material,true);
--i;break;
}
}
}
}
/* Cut */
if(cut)
{
@@ -2145,22 +1871,6 @@ if(cut)
}
}
}
/* Tetras */
for(int i=0,ni=m_tetras.size();i<ni;++i)
{
Node** n= m_tetras[i].m_n;
if( (ifn->Eval(n[0]->m_x)<accurary)&&
(ifn->Eval(n[1]->m_x)<accurary)&&
(ifn->Eval(n[2]->m_x)<accurary)&&
(ifn->Eval(n[3]->m_x)<accurary))
{
for(int j=0;j<4;++j)
{
int cn=cnodes[int(n[j]-nbase)];
if(cn) n[j]=&m_nodes[cn];
}
}
}
/* Clean orphans */
int nnodes=m_nodes.size();
btAlignedObjectArray<int> ranks;
@@ -2174,10 +1884,6 @@ if(cut)
{
for(int j=0;j<3;++j) ranks[int(m_faces[i].m_n[j]-nbase)]++;
}
for(int i=0,ni=m_tetras.size();i<ni;++i)
{
for(int j=0;j<4;++j) ranks[int(m_tetras[i].m_n[j]-nbase)]++;
}
for(int i=0;i<m_links.size();++i)
{
const int id[]={ int(m_links[i].m_n[0]-nbase),
@@ -2318,18 +2024,15 @@ switch(preset)
m_cfg.m_psequence.push_back(ePSolver::Anchors);
m_cfg.m_psequence.push_back(ePSolver::RContacts);
m_cfg.m_psequence.push_back(ePSolver::SContacts);
m_cfg.m_psequence.push_back(ePSolver::Volume);
m_cfg.m_psequence.push_back(ePSolver::Linear);
break;
case eSolverPresets::Velocities:
m_cfg.m_vsequence.push_back(eVSolver::Volume);
m_cfg.m_vsequence.push_back(eVSolver::Linear);
m_cfg.m_psequence.push_back(ePSolver::Anchors);
m_cfg.m_psequence.push_back(ePSolver::RContacts);
m_cfg.m_psequence.push_back(ePSolver::SContacts);
m_cfg.m_dsequence.push_back(ePSolver::Volume);
m_cfg.m_dsequence.push_back(ePSolver::Linear);
break;
}
@@ -2389,65 +2092,36 @@ void btSoftBody::predictMotion(btScalar dt)
}
/* Pose */
UpdatePose(this);
/* Clear contacts */
/* Match */
if(m_pose.m_bframe&&(m_cfg.kMT>0))
{
for(int i=0,ni=m_nodes.size();i<ni;++i)
{
Node& n=m_nodes[i];
if(n.m_im>0)
{
const btVector3 x=m_pose.m_rot*m_pose.m_pos[i]+m_pose.m_com;
n.m_x=Lerp(n.m_x,x,m_cfg.kMT);
}
}
}
/* Clear contacts */
m_rcontacts.resize(0);
m_scontacts.resize(0);
/* Optimize dbvt's */
m_ndbvt.optimizeIncremental(1);
m_fdbvt.optimizeIncremental(1);
}
//
void btSoftBody::solveConstraints()
{
/* Solve velocities */
if(m_cfg.viterations>0)
/* Prepare links */
for(int i=0,ni=m_links.size();i<ni;++i)
{
/* Prepare */
for(int i=0,ni=m_links.size();i<ni;++i)
{
Link& l=m_links[i];
l.m_c3 = l.m_n[1]->m_x-l.m_n[0]->m_x;
l.m_c2 = 1/(l.m_c3.length2()*l.m_c0);
}
for(int i=0,ni=m_tetras.size();i<ni;++i)
{
Tetra& t=m_tetras[i];
Node** n=t.m_n;
t.m_c0[0] = cross(n[3]->m_x-n[1]->m_x,n[2]->m_x-n[1]->m_x);
t.m_c0[1] = cross(n[3]->m_x-n[2]->m_x,n[0]->m_x-n[2]->m_x);
t.m_c0[2] = cross(n[3]->m_x-n[0]->m_x,n[1]->m_x-n[0]->m_x);
t.m_c0[3] = cross(n[1]->m_x-n[0]->m_x,n[2]->m_x-n[0]->m_x);
const btScalar den=t.m_c0[0].length2()+
t.m_c0[1].length2()+
t.m_c0[2].length2()+
t.m_c0[3].length2();
t.m_c2 = -t.m_c1/den;
}
/* Solve */
for(int isolve=0;isolve<m_cfg.viterations;++isolve)
{
for(int iseq=0;iseq<m_cfg.m_vsequence.size();++iseq)
{
VSolvers[m_cfg.m_vsequence[iseq]](this);
}
}
/* Update */
for(int i=0,ni=m_nodes.size();i<ni;++i)
{
Node& n=m_nodes[i];
n.m_x = n.m_q+n.m_v*m_sst.sdt;
}
}
/* Match */
if(m_pose.m_bframe&&(m_cfg.kMT>0))
{
for(int i=0,ni=m_nodes.size();i<ni;++i)
{
Node& n=m_nodes[i];
if(n.m_im>0)
{
const btVector3 x=m_pose.m_rot*m_pose.m_pos[i]+m_pose.m_com;
n.m_x=Lerp(n.m_x,x,m_cfg.kMT);
}
}
Link& l=m_links[i];
l.m_c3 = l.m_n[1]->m_x-l.m_n[0]->m_x;
l.m_c2 = 1/(l.m_c3.length2()*l.m_c0);
}
/* Prepare anchors */
for(int i=0,ni=m_anchors.size();i<ni;++i)
@@ -2463,6 +2137,24 @@ for(int i=0,ni=m_anchors.size();i<ni;++i)
a.m_c2 = m_sst.sdt*a.m_node->m_im;
a.m_body->activate();
}
/* Solve velocities */
if(m_cfg.viterations>0)
{
/* Solve */
for(int isolve=0;isolve<m_cfg.viterations;++isolve)
{
for(int iseq=0;iseq<m_cfg.m_vsequence.size();++iseq)
{
VSolvers[m_cfg.m_vsequence[iseq]](this,1);
}
}
/* Update */
for(int i=0,ni=m_nodes.size();i<ni;++i)
{
Node& n=m_nodes[i];
n.m_x = n.m_q+n.m_v*m_sst.sdt;
}
}
/* Solve positions */
if(m_cfg.piterations>0)
{
@@ -2470,7 +2162,7 @@ if(m_cfg.piterations>0)
{
for(int iseq=0;iseq<m_cfg.m_psequence.size();++iseq)
{
PSolvers[m_cfg.m_psequence[iseq]](this);
PSolvers[m_cfg.m_psequence[iseq]](this,1);
}
}
const btScalar vc=m_sst.isdt*(1-m_cfg.kDP);
@@ -2494,7 +2186,7 @@ if(m_cfg.diterations>0)
{
for(int iseq=0;iseq<m_cfg.m_dsequence.size();++iseq)
{
PSolvers[m_cfg.m_dsequence[iseq]](this);
PSolvers[m_cfg.m_dsequence[iseq]](this,1);
}
}
for(int i=0,ni=m_nodes.size();i<ni;++i)
@@ -2502,7 +2194,7 @@ if(m_cfg.diterations>0)
Node& n=m_nodes[i];
n.m_v += (n.m_x-n.m_q)*vcf;
}
}
}
}
//
@@ -2512,7 +2204,7 @@ for(int isolve=0;isolve<iterations;++isolve)
{
for(int iseq=0;iseq<m_cfg.m_psequence.size();++iseq)
{
PSolvers[m_cfg.m_psequence[iseq]](this);
PSolvers[m_cfg.m_psequence[iseq]](this,1);
}
}
}
@@ -2539,17 +2231,7 @@ switch(m_cfg.collisions&fCollision::RVSmask)
{
struct DoCollide : btDbvt::ICollide
{
virtual void Process(const btDbvt::Node* /*a*/,const btDbvt::Node* /*b*/)
{
}
virtual bool Descent(const btDbvt::Node*)
{
return false;
}
virtual void Process(const btDbvt::Node* leaf)
void Process(const btDbvt::Node* leaf)
{
Node* node=(Node*)leaf->data;
DoNode(*node);
@@ -2608,7 +2290,7 @@ switch(m_cfg.collisions&fCollision::RVSmask)
docollide.prb = prb;
docollide.dynmargin = basemargin+timemargin;
docollide.stamargin = basemargin;
m_ndbvt.collide(volume,&docollide);
btDbvt::collideTV(m_ndbvt.m_root,volume,docollide);
}
break;
}
@@ -2624,15 +2306,6 @@ switch(cf&fCollision::SVSmask)
{
struct DoCollide : btDbvt::ICollide
{
virtual bool Descent(const btDbvt::Node*)
{
return false;
}
virtual void Process(const btDbvt::Node* /*leaf*/)
{
}
void Process(const btDbvt::Node* lnode,
const btDbvt::Node* lface)
{
@@ -2683,14 +2356,16 @@ switch(cf&fCollision::SVSmask)
/* psb0 nodes vs psb1 faces */
docollide.psb[0]=this;
docollide.psb[1]=psb;
docollide.psb[0]->m_ndbvt.collide( &docollide.psb[1]->m_fdbvt,
&docollide);
btDbvt::collideTT( docollide.psb[0]->m_ndbvt.m_root,
docollide.psb[1]->m_fdbvt.m_root,
docollide);
/* psb1 nodes vs psb0 faces */
docollide.psb[0]=psb;
docollide.psb[1]=this;
docollide.psb[0]->m_ndbvt.collide( &docollide.psb[1]->m_fdbvt,
&docollide);
btDbvt::collideTT( docollide.psb[0]->m_ndbvt.m_root,
docollide.psb[1]->m_fdbvt.m_root,
docollide);
}
break;
}
}
}

71
src/BulletSoftBody/btSoftBody.h
View File

@@ -45,29 +45,31 @@ public:
V_TwoSided, ///Vertex normals are fliped to match velocity
V_OneSided, ///Vertex normals are taken as it is
F_TwoSided, ///Face normals are fliped to match velocity
F_OneSided ///Face normals are taken as it is
F_OneSided, ///Face normals are taken as it is
END
};};
///eVSolver : velocities solvers
struct eVSolver { enum _ {
Linear, ///Linear solver
Volume ///Volume solver
END
};};
///ePSolver : positions solvers
struct ePSolver { enum _ {
Linear, ///Linear solver
Volume, ///Volume solver
Anchors, ///Anchor solver
RContacts, ///Rigid contacts solver
SContacts ///Soft contacts solver
SContacts, ///Soft contacts solver
END
};};
///eSolverPresets
struct eSolverPresets { enum _ {
Positions,
Velocities,
Default = Positions
Default = Positions,
END
};};
///eFeature
@@ -76,7 +78,7 @@ public:
Node,
Link,
Face,
Tetra
END
};};
typedef btAlignedObjectArray<eVSolver::_> tVSolverArray;
@@ -94,14 +96,16 @@ public:
SVSmask = 0x00f0, ///Rigid versus soft mask
VF_SS = 0x0010, ///Vertex vs face soft vs soft handling
/* presets */
Default = SDF_RS
Default = SDF_RS,
END
};};
///fMaterial
struct fMaterial { enum _ {
DebugDraw = 0x0001, /// Enable debug draw
/* presets */
Default = DebugDraw
Default = DebugDraw,
END
};};
//
@@ -120,7 +124,6 @@ public:
/* ImplicitFn */
struct ImplicitFn
{
virtual ~ImplicitFn() {}
virtual btScalar Eval(const btVector3& x)=0;
};
@@ -163,16 +166,13 @@ public:
struct Element
{
void* m_tag; // User data
Element()
{
m_tag=0;
}
Element() : m_tag(0) {}
};
/* Material */
struct Material : Element
{
btScalar m_kLST; // Linear stiffness coefficient [0,1]
btScalar m_kAST; // Area stiffness coefficient [0,1]
btScalar m_kAST; // Area/Angular stiffness coefficient [0,1]
btScalar m_kVST; // Volume stiffness coefficient [0,1]
int m_flags; // Flags
};
@@ -199,7 +199,8 @@ public:
struct Link : Feature
{
Node* m_n[2]; // Node pointers
btScalar m_rl; // Rest length
btScalar m_rl; // Rest length
int m_bbending:1; // Bending link
btScalar m_c0; // (ima+imb)*kLST
btScalar m_c1; // rl^2
btScalar m_c2; // |gradient|^2/c0
@@ -213,16 +214,6 @@ public:
btScalar m_ra; // Rest area
btDbvt::Node* m_leaf; // Leaf data
};
/* Tetra */
struct Tetra : Feature
{
Node* m_n[4]; // Node pointers
btScalar m_rv; // Rest volume
btDbvt::Node* m_leaf; // Leaf data
btVector3 m_c0[4]; // gradients
btScalar m_c1; // (4*kVST)/(im0+im1+im2+im3)
btScalar m_c2; // m_c1/sum(|g0..3|^2)
};
/* RContact */
struct RContact
{
@@ -322,7 +313,6 @@ public:
typedef btAlignedObjectArray<btDbvt::Node*> tLeafArray;
typedef btAlignedObjectArray<Link> tLinkArray;
typedef btAlignedObjectArray<Face> tFaceArray;
typedef btAlignedObjectArray<Tetra> tTetraArray;
typedef btAlignedObjectArray<Anchor> tAnchorArray;
typedef btAlignedObjectArray<RContact> tRContactArray;
typedef btAlignedObjectArray<SContact> tSContactArray;
@@ -342,7 +332,6 @@ public:
tNodeArray m_nodes; // Nodes
tLinkArray m_links; // Links
tFaceArray m_faces; // Faces
tTetraArray m_tetras; // Tetras
tAnchorArray m_anchors; // Anchors
tRContactArray m_rcontacts; // Rigid contacts
tSContactArray m_scontacts; // Soft contacts
@@ -365,12 +354,12 @@ public:
/* Check for existing link */
bool checkLink( int node0,
int node1) const;
bool checkLink( const btSoftBody::Node* node0,
const btSoftBody::Node* node1) const;
bool checkLink( const Node* node0,
const Node* node1) const;
/* Check for existring face */
bool checkFace( int node0,
int node1,
int node2) const;
int node1,
int node2) const;
/* Append material */
Material* appendMaterial();
/* Append note */
@@ -390,9 +379,6 @@ public:
void appendNote( const char* text,
const btVector3& o,
Face* feature);
void appendNote( const char* text,
const btVector3& o,
Tetra* feature);
/* Append node */
void appendNode( const btVector3& x,btScalar m);
/* Append link */
@@ -401,8 +387,8 @@ public:
int node1,
Material* mat=0,
bool bcheckexist=false);
void appendLink( btSoftBody::Node* node0,
btSoftBody::Node* node1,
void appendLink( Node* node0,
Node* node1,
Material* mat=0,
bool bcheckexist=false);
/* Append face */
@@ -411,13 +397,6 @@ public:
int node1,
int node2,
Material* mat=0);
/* Append tetrahedron */
void appendTetra(int model=-1,Material* mat=0);
void appendTetra(int node0,
int node1,
int node2,
int node3,
Material* mat=0);
/* Append anchor */
void appendAnchor( int node,
btRigidBody* body);
@@ -443,10 +422,6 @@ public:
bool fromfaces=false);
/* Set total density */
void setTotalDensity(btScalar density);
/* Set volume mass (using tetrahedrons) */
void setVolumeMass( btScalar mass);
/* Set volume density (using tetrahedrons) */
void setVolumeDensity( btScalar density);
/* Transform */
void transform( const btTransform& trs);
/* Translate */
@@ -463,8 +438,6 @@ public:
/* Generate bending constraints based on distance in the adjency graph */
int generateBendingConstraints( int distance,
Material* mat=0);
/* Generate tetrahedral constraints */
int generateTetrahedralConstraints();
/* Randomize constraints to reduce solver bias */
void randomizeConstraints();
/* Refine */

378
src/BulletSoftBody/btSoftBodyHelpers.cpp
View File

@@ -15,7 +15,6 @@ subject to the following restrictions:
///btSoftBodyHelpers.cpp by Nathanael Presson
#include "btSoftBody.h"
#include "BulletCollision/BroadphaseCollision/btDbvt.h"
#include <stdio.h>
#include <string.h>
#include "btSoftBodyHelpers.h"
@@ -23,97 +22,98 @@ subject to the following restrictions:
//
static void drawVertex( btIDebugDraw* idraw,
const btVector3& x,btScalar s,const btVector3& c)
{
idraw->drawLine(x-btVector3(s,0,0),x+btVector3(s,0,0),c);
idraw->drawLine(x-btVector3(0,s,0),x+btVector3(0,s,0),c);
idraw->drawLine(x-btVector3(0,0,s),x+btVector3(0,0,s),c);
}
const btVector3& x,btScalar s,const btVector3& c)
{
idraw->drawLine(x-btVector3(s,0,0),x+btVector3(s,0,0),c);
idraw->drawLine(x-btVector3(0,s,0),x+btVector3(0,s,0),c);
idraw->drawLine(x-btVector3(0,0,s),x+btVector3(0,0,s),c);
}
//
static void drawBox( btIDebugDraw* idraw,
const btVector3& mins,
const btVector3& maxs,
const btVector3& color)
const btVector3& mins,
const btVector3& maxs,
const btVector3& color)
{
const btVector3 c[]={ btVector3(mins.x(),mins.y(),mins.z()),
btVector3(maxs.x(),mins.y(),mins.z()),
btVector3(maxs.x(),maxs.y(),mins.z()),
btVector3(mins.x(),maxs.y(),mins.z()),
btVector3(mins.x(),mins.y(),maxs.z()),
btVector3(maxs.x(),mins.y(),maxs.z()),
btVector3(maxs.x(),maxs.y(),maxs.z()),
btVector3(mins.x(),maxs.y(),maxs.z())};
idraw->drawLine(c[0],c[1],color);idraw->drawLine(c[1],c[2],color);
idraw->drawLine(c[2],c[3],color);idraw->drawLine(c[3],c[0],color);
idraw->drawLine(c[4],c[5],color);idraw->drawLine(c[5],c[6],color);
idraw->drawLine(c[6],c[7],color);idraw->drawLine(c[7],c[4],color);
idraw->drawLine(c[0],c[4],color);idraw->drawLine(c[1],c[5],color);
idraw->drawLine(c[2],c[6],color);idraw->drawLine(c[3],c[7],color);
const btVector3 c[]={ btVector3(mins.x(),mins.y(),mins.z()),
btVector3(maxs.x(),mins.y(),mins.z()),
btVector3(maxs.x(),maxs.y(),mins.z()),
btVector3(mins.x(),maxs.y(),mins.z()),
btVector3(mins.x(),mins.y(),maxs.z()),
btVector3(maxs.x(),mins.y(),maxs.z()),
btVector3(maxs.x(),maxs.y(),maxs.z()),
btVector3(mins.x(),maxs.y(),maxs.z())};
idraw->drawLine(c[0],c[1],color);idraw->drawLine(c[1],c[2],color);
idraw->drawLine(c[2],c[3],color);idraw->drawLine(c[3],c[0],color);
idraw->drawLine(c[4],c[5],color);idraw->drawLine(c[5],c[6],color);
idraw->drawLine(c[6],c[7],color);idraw->drawLine(c[7],c[4],color);
idraw->drawLine(c[0],c[4],color);idraw->drawLine(c[1],c[5],color);
idraw->drawLine(c[2],c[6],color);idraw->drawLine(c[3],c[7],color);
}
//
static void drawTree( btIDebugDraw* idraw,
const btDbvt::Node* node,
int depth,
const btVector3& ncolor,
const btVector3& lcolor,
int mindepth,
int maxdepth)
const btDbvt::Node* node,
int depth,
const btVector3& ncolor,
const btVector3& lcolor,
int mindepth,
int maxdepth)
{
if(node)
if(node)
{
if(node->isinternal()&&((depth<maxdepth)||(maxdepth<0)))
if(node->isinternal()&&((depth<maxdepth)||(maxdepth<0)))
{
drawTree(idraw,node->childs[0],depth+1,ncolor,lcolor,mindepth,maxdepth);
drawTree(idraw,node->childs[1],depth+1,ncolor,lcolor,mindepth,maxdepth);
drawTree(idraw,node->childs[0],depth+1,ncolor,lcolor,mindepth,maxdepth);
drawTree(idraw,node->childs[1],depth+1,ncolor,lcolor,mindepth,maxdepth);
}
if(depth>=mindepth)
if(depth>=mindepth)
{
const btScalar scl=(btScalar)(node->isinternal()?1:1);
const btVector3 mi=node->volume.Center()-node->volume.Extent()*scl;
const btVector3 mx=node->volume.Center()+node->volume.Extent()*scl;
drawBox(idraw,mi,mx,node->isleaf()?lcolor:ncolor);
const btScalar scl=(btScalar)(node->isinternal()?1:1);
const btVector3 mi=node->volume.Center()-node->volume.Extent()*scl;
const btVector3 mx=node->volume.Center()+node->volume.Extent()*scl;
drawBox(idraw,mi,mx,node->isleaf()?lcolor:ncolor);
}
}
}
//
//
static inline btScalar tetravolume(const btVector3& x0,
const btVector3& x1,
const btVector3& x2,
const btVector3& x3)
{
const btVector3 a=x1-x0;
const btVector3 b=x2-x0;
const btVector3 c=x3-x0;
return(dot(a,cross(b,c)));
//
static inline btScalar tetravolume(const btVector3& x0,
const btVector3& x1,
const btVector3& x2,
const btVector3& x3)
{
const btVector3 a=x1-x0;
const btVector3 b=x2-x0;
const btVector3 c=x3-x0;
return(dot(a,cross(b,c)));
}
//
/*
#if 0
static btVector3 stresscolor(btScalar stress)
{
{
static const btVector3 spectrum[]= { btVector3(1,0,1),
btVector3(0,0,1),
btVector3(0,1,1),
btVector3(0,1,0),
btVector3(1,1,0),
btVector3(1,0,0),
btVector3(1,0,0)};
btVector3(0,0,1),
btVector3(0,1,1),
btVector3(0,1,0),
btVector3(1,1,0),
btVector3(1,0,0),
btVector3(1,0,0)};
static const int ncolors=sizeof(spectrum)/sizeof(spectrum[0])-1;
static const btScalar one=1;
stress=btMax<btScalar>(0,btMin<btScalar>(1,stress))*ncolors;
const int sel=(int)stress;
const btScalar frc=stress-sel;
return(spectrum[sel]+(spectrum[sel+1]-spectrum[sel])*frc);
}
*/
}
#endif
//
void btSoftBodyHelpers::Draw( btSoftBody* psb,
btIDebugDraw* idraw,
int drawflags)
btIDebugDraw* idraw,
int drawflags)
{
const btScalar scl=(btScalar)0.1;
const btScalar nscl=scl*5;
@@ -209,32 +209,14 @@ void btSoftBodyHelpers::Draw( btSoftBody* psb,
idraw->drawLine((x[1]-c)*scl+c,(x[2]-c)*scl+c,col);
idraw->drawLine((x[2]-c)*scl+c,(x[0]-c)*scl+c,col);*/
idraw->drawTriangle((x[0]-c)*scl+c,
(x[1]-c)*scl+c,
(x[2]-c)*scl+c,
col,alp);
/*idraw->drawTriangle((x[0]-c)*scl+c,
(x[1]-c)*scl+c,
(x[2]-c)*scl+c,
col,alp);
/*idraw->drawTriangle((x[0]-c)*scl+c,
(x[1]-c)*scl+c,
(x[2]-c)*scl+c,
f.m_n[0]->m_n,f.m_n[1]->m_n,f.m_n[2]->m_n,
col,alp);*/
}
}
/* Tetras */
if(0!=(drawflags&fDrawFlags::Tetras))
{
const btScalar scl=(btScalar)0.8;
const btScalar alp=(btScalar)1;
const btVector3 col((btScalar)0.7,(btScalar)0.7,(btScalar)0.7);
for(int i=0;i<psb->m_tetras.size();++i)
{
const btSoftBody::Tetra& t=psb->m_tetras[i];
if(0==(t.m_material->m_flags&btSoftBody::fMaterial::DebugDraw)) continue;
const btVector3 x[]={t.m_n[0]->m_x,t.m_n[1]->m_x,t.m_n[2]->m_x,t.m_n[3]->m_x};
const btVector3 c=(x[0]+x[1]+x[2]+x[3])/4;
idraw->drawTriangle((x[0]-c)*scl+c,(x[1]-c)*scl+c,(x[2]-c)*scl+c,col,alp);
idraw->drawTriangle((x[0]-c)*scl+c,(x[1]-c)*scl+c,(x[3]-c)*scl+c,col,alp);
idraw->drawTriangle((x[1]-c)*scl+c,(x[2]-c)*scl+c,(x[3]-c)*scl+c,col,alp);
idraw->drawTriangle((x[2]-c)*scl+c,(x[0]-c)*scl+c,(x[3]-c)*scl+c,col,alp);
f.m_n[0]->m_n,f.m_n[1]->m_n,f.m_n[2]->m_n,
col,alp);*/
}
}
/* Notes */
@@ -245,21 +227,20 @@ void btSoftBodyHelpers::Draw( btSoftBody* psb,
const btSoftBody::Note& n=psb->m_notes[i];
btVector3 p=n.m_offset;
for(int j=0;j<n.m_rank;++j)
{
{
p+=n.m_nodes[j]->m_x*n.m_coords[j];
}
}
idraw->draw3dText(p,n.m_text);
}
}
}
#if 0
//
void btSoftBodyHelpers::DrawInfos( btSoftBody* psb,
btIDebugDraw* idraw,
bool masses,
bool areas,
bool /*stress*/)
btIDebugDraw* idraw,
bool masses,
bool areas,
bool /*stress*/)
{
for(int i=0;i<psb->m_nodes.size();++i)
{
@@ -279,29 +260,28 @@ void btSoftBodyHelpers::DrawInfos( btSoftBody* psb,
if(text[0]) idraw->draw3dText(n.m_x,text);
}
}
#endif
//
void btSoftBodyHelpers::DrawNodeTree( btSoftBody* psb,
btIDebugDraw* idraw,
int mindepth,
int maxdepth)
btIDebugDraw* idraw,
int mindepth,
int maxdepth)
{
drawTree(idraw,psb->m_ndbvt.m_root,0,btVector3(1,0,1),btVector3(1,1,1),mindepth,maxdepth);
drawTree(idraw,psb->m_ndbvt.m_root,0,btVector3(1,0,1),btVector3(1,1,1),mindepth,maxdepth);
}
//
void btSoftBodyHelpers::DrawFaceTree( btSoftBody* psb,
btIDebugDraw* idraw,
int mindepth,
int maxdepth)
btIDebugDraw* idraw,
int mindepth,
int maxdepth)
{
drawTree(idraw,psb->m_fdbvt.m_root,0,btVector3(0,1,0),btVector3(1,0,0),mindepth,maxdepth);
drawTree(idraw,psb->m_fdbvt.m_root,0,btVector3(0,1,0),btVector3(1,0,0),mindepth,maxdepth);
}
//
void btSoftBodyHelpers::DrawFrame( btSoftBody* psb,
btIDebugDraw* idraw)
btIDebugDraw* idraw)
{
if(psb->m_pose.m_bframe)
{
@@ -325,9 +305,9 @@ void btSoftBodyHelpers::DrawFrame( btSoftBody* psb,
//
btSoftBody* btSoftBodyHelpers::CreateRope( btSoftBody::btSoftBodyWorldInfo& worldInfo, const btVector3& from,
const btVector3& to,
int res,
int fixeds)
const btVector3& to,
int res,
int fixeds)
{
/* Create nodes */
const int r=res+2;
@@ -355,13 +335,13 @@ btSoftBody* btSoftBodyHelpers::CreateRope( btSoftBody::btSoftBodyWorldInfo& wor
//
btSoftBody* btSoftBodyHelpers::CreatePatch(btSoftBody::btSoftBodyWorldInfo& worldInfo,const btVector3& corner00,
const btVector3& corner10,
const btVector3& corner01,
const btVector3& corner11,
int resx,
int resy,
int fixeds,
bool gendiags)
const btVector3& corner10,
const btVector3& corner01,
const btVector3& corner11,
int resx,
int resy,
int fixeds,
bool gendiags)
{
#define IDX(_x_,_y_) ((_y_)*rx+(_x_))
/* Create nodes */
@@ -430,8 +410,8 @@ btSoftBody* btSoftBodyHelpers::CreatePatch(btSoftBody::btSoftBodyWorldInfo& wor
//
btSoftBody* btSoftBodyHelpers::CreateEllipsoid(btSoftBody::btSoftBodyWorldInfo& worldInfo,const btVector3& center,
const btVector3& radius,
int res)
const btVector3& radius,
int res)
{
struct Hammersley
{
@@ -462,8 +442,8 @@ btSoftBody* btSoftBodyHelpers::CreateEllipsoid(btSoftBody::btSoftBodyWorldInfo&
//
btSoftBody* btSoftBodyHelpers::CreateFromTriMesh(btSoftBody::btSoftBodyWorldInfo& worldInfo,const btScalar* vertices,
const int* triangles,
int ntriangles)
const int* triangles,
int ntriangles)
{
int maxidx=0;
for(int i=0,ni=ntriangles*3;i<ni;++i)
@@ -502,20 +482,20 @@ btSoftBody* btSoftBodyHelpers::CreateFromTriMesh(btSoftBody::btSoftBodyWorldInf
//
btSoftBody* btSoftBodyHelpers::CreateFromConvexHull(btSoftBody::btSoftBodyWorldInfo& worldInfo, const btVector3* vertices,
int nvertices)
int nvertices)
{
HullDesc hdsc(QF_TRIANGLES,static_cast<unsigned int>(nvertices),vertices);
HullDesc hdsc(QF_TRIANGLES,nvertices,vertices);
HullResult hres;
HullLibrary hlib;/*??*/
hdsc.mMaxVertices=static_cast<unsigned int>(nvertices);
hdsc.mMaxVertices=nvertices;
hlib.CreateConvexHull(hdsc,hres);
btSoftBody* psb=new btSoftBody(&worldInfo,(int)hres.mNumOutputVertices,
&hres.m_OutputVertices[0],0);
for(int i=0;i<(int)hres.mNumFaces;++i)
{
const int idx[]={ static_cast<int>(hres.m_Indices[i*3+0]),
static_cast<int>(hres.m_Indices[i*3+1]),
static_cast<int>(hres.m_Indices[i*3+2])};
const int idx[]={ hres.m_Indices[i*3+0],
hres.m_Indices[i*3+1],
hres.m_Indices[i*3+2]};
if(idx[0]<idx[1]) psb->appendLink( idx[0],idx[1]);
if(idx[1]<idx[2]) psb->appendLink( idx[1],idx[2]);
if(idx[2]<idx[0]) psb->appendLink( idx[2],idx[0]);
@@ -524,156 +504,4 @@ btSoftBody* btSoftBodyHelpers::CreateFromConvexHull(btSoftBody::btSoftBodyWorld
hlib.ReleaseResult(hres);
psb->randomizeConstraints();
return(psb);
}
#if BT_SOFTBODY_USE_STL
#include <string>
#include <sstream>
#include <fstream>
#include <iostream>
#include <stdio.h>
//
void btSoftBodyHelpers::ExportAsSMeshFile(btSoftBody* psb,
const char* filename)
{
std::ofstream output(filename);
output << psb->m_nodes.size() << " " << 3 << " " << 0 << " " << 0 << "\n";
for(int i=0;i<psb->m_nodes.size();++i)
{
const btSoftBody::Node& n=psb->m_nodes[i];
output << i << " "
<< n.m_x.x() << " "
<< n.m_x.y() << " "
<< n.m_x.z() << "\n";
}
output << psb->m_faces.size() << " " << 1 << "\n";
for(int i=0;i<psb->m_faces.size();++i)
{
const btSoftBody::Node* b=&psb->m_nodes[0];
const btSoftBody::Face& f=psb->m_faces[i];
output << 3 << " "
<< int(f.m_n[0]-b) << " "
<< int(f.m_n[1]-b) << " "
<< int(f.m_n[2]-b) << " "
<< 1 << "\n";
}
output << 0 << "\n";
output << 0 << "\n";
output.close();
}
/* Create from TetGen .ele, .face, .node files */
btSoftBody* btSoftBodyHelpers::CreateFromTetGenFile(btSoftBody::btSoftBodyWorldInfo& worldInfo,
const char* ele,
const char* face,
const char* node,
bool bfacelinks,
bool btetralinks,
bool bfacesfromtetras)
{
std::ifstream efile(ele?ele:"");
std::ifstream ffile(face?face:"");
std::ifstream nfile(node);
std::string edata;
std::string fdata;
std::string ndata;
if(efile.good()) while(!efile.eof()) edata+=efile.get();
if(ffile.good()) while(!ffile.eof()) fdata+=ffile.get();
if(nfile.good()) while(!nfile.eof()) ndata+=nfile.get();
efile.close();
ffile.close();
nfile.close();
return(CreateFromTetGenData(worldInfo,edata.c_str(),fdata.c_str(),ndata.c_str(),
bfacelinks,btetralinks,bfacesfromtetras));
}
/* Create from TetGen .ele, .face, .node data */
btSoftBody* btSoftBodyHelpers::CreateFromTetGenData(btSoftBody::btSoftBodyWorldInfo& worldInfo,
const char* ele,
const char* face,
const char* node,
bool bfacelinks,
bool btetralinks,
bool bfacesfromtetras)
{
std::istringstream se(ele?ele:"");
std::istringstream sf(face?face:"");
std::istringstream sn(node?node:"");
btAlignedObjectArray<btVector3> pos;
int nnode=0;
int ndims=0;
int nattrb=0;
int hasbounds=0;
sn>>nnode;sn>>ndims;sn>>nattrb;sn>>hasbounds;
pos.resize(nnode);
for(int i=0;i<pos.size();++i)
{
int index=0;
int bound=0;
btScalar x,y,z,a;
sn>>index;
sn>>x;sn>>y;sn>>z;
for(int j=0;j<nattrb;++j) sn>>a;
if(hasbounds) sn>>bound;
pos[index].setX(x);
pos[index].setY(y);
pos[index].setZ(z);
}
btSoftBody* psb=new btSoftBody(&worldInfo,nnode,&pos[0],0);
if(face&&face[0])
{
int nface=0;
sf>>nface;sf>>hasbounds;
for(int i=0;i<nface;++i)
{
int index=0;
int bound=0;
int ni[3];
sf>>index;
sf>>ni[0];sf>>ni[1];sf>>ni[2];
sf>>bound;
psb->appendFace(ni[0],ni[1],ni[2]);
if(btetralinks)
{
psb->appendLink(ni[0],ni[1],0,true);
psb->appendLink(ni[1],ni[2],0,true);
psb->appendLink(ni[2],ni[0],0,true);
}
}
}
if(ele&&ele[0])
{
int ntetra=0;
int ncorner=0;
int neattrb=0;
se>>ntetra;se>>ncorner;se>>neattrb;
for(int i=0;i<ntetra;++i)
{
int index=0;
int ni[4],a;
se>>index;
se>>ni[0];se>>ni[1];se>>ni[2];se>>ni[3];
for(int j=0;j<neattrb;++j) se>>a;
psb->appendTetra(ni[0],ni[1],ni[2],ni[3]);
if(btetralinks)
{
psb->appendLink(ni[0],ni[1],0,true);
psb->appendLink(ni[1],ni[2],0,true);
psb->appendLink(ni[2],ni[0],0,true);
psb->appendLink(ni[0],ni[3],0,true);
psb->appendLink(ni[1],ni[3],0,true);
psb->appendLink(ni[2],ni[3],0,true);
}
}
}
printf("Nodes: %u\r\n",psb->m_nodes.size());
printf("Links: %u\r\n",psb->m_links.size());
printf("Faces: %u\r\n",psb->m_faces.size());
printf("Tetras: %u\r\n",psb->m_tetras.size());
return(psb);
}
#endif
}

28
src/BulletSoftBody/btSoftBodyHelpers.h
View File

@@ -18,9 +18,6 @@ subject to the following restrictions:
#include "btSoftBody.h"
//Can't enable this, Bullet doesn't use STL
//#define BT_SOFTBODY_USE_STL 1
//
// Helpers
//
@@ -37,7 +34,7 @@ struct fDrawFlags { enum _ {
Notes = 0x0080,
/* presets */
Std = Links+Faces+Tetras+Anchors+Notes,
StdTetra = Std-Faces+Tetras
StdTetra = Std-Faces+Tetras,
};};
struct btSoftBodyHelpers
@@ -46,14 +43,12 @@ struct btSoftBodyHelpers
static void Draw( btSoftBody* psb,
btIDebugDraw* idraw,
int drawflags=fDrawFlags::Std);
#if 0
/* Draw body infos */
static void DrawInfos( btSoftBody* psb,
btIDebugDraw* idraw,
bool masses,
bool areas,
bool stress);
#endif
/* Draw node tree */
static void DrawNodeTree( btSoftBody* psb,
btIDebugDraw* idraw,
@@ -97,27 +92,6 @@ struct btSoftBodyHelpers
static btSoftBody* CreateFromConvexHull( btSoftBody::btSoftBodyWorldInfo& worldInfo,
const btVector3* vertices,
int nvertices);
#if BT_SOFTBODY_USE_STL
/* Export TetGen compatible .smesh file */
static void ExportAsSMeshFile( btSoftBody* psb,
const char* filename);
/* Create from TetGen .ele, .face, .node files */
static btSoftBody* CreateFromTetGenFile( btSoftBody::btSoftBodyWorldInfo& worldInfo,
const char* ele,
const char* face,
const char* node,
bool bfacelinks,
bool btetralinks,
bool bfacesfromtetras);
/* Create from TetGen .ele, .face, .node data */
static btSoftBody* CreateFromTetGenData( btSoftBody::btSoftBodyWorldInfo& worldInfo,
const char* ele,
const char* face,
const char* node,
bool bfacelinks,
bool btetralinks,
bool bfacesfromtetras);
#endif
};
#endif //SOFT_BODY_HELPERS_H