mirror of
https://github.com/bulletphysics/bullet3
synced 2025-01-10 17:30:12 +00:00
upgrade version to 2.69
moved btDbvt/btDbvtBroadphase to BulletCollision/BroadphaseCollision applied code-layout to btSoftBodyHelpers.*
This commit is contained in:
erwin.coumans
parent
3268cab6d7
commit
6989ea8908
5
VERSION
5
VERSION
@ -1,3 +1,2 @@
|
||||
Bullet Collision Detection and Physics Library version 2.68
|
||||
http://bullet.sourceforge.net
|
||||
|
||||
Bullet Collision Detection and Physics Library version 2.69
|
||||
http://bullet.googlecode.com
|
||||
|
||||
494
src/BulletCollision/BroadphaseCollision/btDbvt.cpp
Normal file
494
src/BulletCollision/BroadphaseCollision/btDbvt.cpp
Normal file
@ -0,0 +1,494 @@
|
||||
/*
|
||||
Bullet Continuous Collision Detection and Physics Library
|
||||
Copyright (c) 2003-2006 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.
|
||||
*/
|
||||
///btDbvt implementation by Nathanael Presson
|
||||
|
||||
#include "btDbvt.h"
|
||||
|
||||
//
|
||||
typedef btAlignedObjectArray<btDbvt::Node*> tNodeArray;
|
||||
|
||||
//
|
||||
static inline int indexof(const btDbvt::Node* node)
|
||||
{
|
||||
return(node->parent->childs[1]==node);
|
||||
}
|
||||
|
||||
//
|
||||
static inline btDbvt::Volume merge( const btDbvt::Volume& a,
|
||||
const btDbvt::Volume& b)
|
||||
{
|
||||
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()+
|
||||
edges.x()+edges.y()+edges.z());
|
||||
}
|
||||
|
||||
//
|
||||
static inline void deletenode( btDbvt* pdbvt,
|
||||
btDbvt::Node* node)
|
||||
{
|
||||
delete pdbvt->m_free;
|
||||
pdbvt->m_free=node;
|
||||
}
|
||||
|
||||
//
|
||||
static inline void recursedeletenode( btDbvt* pdbvt,
|
||||
btDbvt::Node* node)
|
||||
{
|
||||
if(!node->isleaf())
|
||||
{
|
||||
recursedeletenode(pdbvt,node->childs[0]);
|
||||
recursedeletenode(pdbvt,node->childs[1]);
|
||||
}
|
||||
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* 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);
|
||||
}
|
||||
|
||||
//
|
||||
static inline void insertleaf( btDbvt* pdbvt,
|
||||
btDbvt::Node* root,
|
||||
btDbvt::Node* leaf)
|
||||
{
|
||||
if(!pdbvt->m_root)
|
||||
{
|
||||
pdbvt->m_root = leaf;
|
||||
leaf->parent = 0;
|
||||
}
|
||||
else
|
||||
{
|
||||
if(!root->isleaf())
|
||||
{
|
||||
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];
|
||||
} while(!root->isleaf());
|
||||
}
|
||||
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;
|
||||
else
|
||||
Merge(prev->childs[0]->volume,prev->childs[1]->volume,prev->volume);
|
||||
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;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
//
|
||||
static inline btDbvt::Node* removeleaf( btDbvt* pdbvt,
|
||||
btDbvt::Node* leaf)
|
||||
{
|
||||
if(leaf==pdbvt->m_root)
|
||||
{
|
||||
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)
|
||||
{
|
||||
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))
|
||||
{
|
||||
sibling = prev;
|
||||
prev = prev->parent;
|
||||
} else break;
|
||||
}
|
||||
return(prev?prev:pdbvt->m_root);
|
||||
}
|
||||
else
|
||||
{
|
||||
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)
|
||||
{
|
||||
if(root->isinternal()&&depth)
|
||||
{
|
||||
fetchleafs(pdbvt,root->childs[0],leafs,depth-1);
|
||||
fetchleafs(pdbvt,root->childs[1],leafs,depth-1);
|
||||
deletenode(pdbvt,root);
|
||||
}
|
||||
else
|
||||
{
|
||||
leafs.push_back(root);
|
||||
}
|
||||
}
|
||||
|
||||
//
|
||||
static void split( const tNodeArray& leafs,
|
||||
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)
|
||||
{
|
||||
if(dot(axis,leafs[i]->volume.Center()-org)<0)
|
||||
left.push_back(leafs[i]);
|
||||
else
|
||||
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)
|
||||
{
|
||||
volume=merge(volume,leafs[i]->volume);
|
||||
}
|
||||
return(volume);
|
||||
}
|
||||
|
||||
//
|
||||
static void bottomup( btDbvt* pdbvt,
|
||||
tNodeArray& leafs)
|
||||
{
|
||||
while(leafs.size()>1)
|
||||
{
|
||||
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)
|
||||
{
|
||||
const btScalar sz=size(merge(leafs[i]->volume,leafs[j]->volume));
|
||||
if(sz<minsize)
|
||||
{
|
||||
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();
|
||||
}
|
||||
}
|
||||
|
||||
//
|
||||
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)
|
||||
{
|
||||
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 btVector3 x=leafs[i]->volume.Center()-org;
|
||||
for(int j=0;j<3;++j)
|
||||
{
|
||||
++splitcount[j][dot(x,axis[j])>0?1:0];
|
||||
}
|
||||
}
|
||||
for(int i=0;i<3;++i)
|
||||
{
|
||||
if((splitcount[i][0]>0)&&(splitcount[i][1]>0))
|
||||
{
|
||||
const int midp=abs(splitcount[i][0]-splitcount[i][1]);
|
||||
if(midp<bestmidp)
|
||||
{
|
||||
bestaxis=i;
|
||||
bestmidp=midp;
|
||||
}
|
||||
}
|
||||
}
|
||||
if(bestaxis>=0)
|
||||
{
|
||||
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[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);
|
||||
}
|
||||
else
|
||||
{
|
||||
bottomup(pdbvt,leafs);
|
||||
return(leafs[0]);
|
||||
}
|
||||
}
|
||||
return(leafs[0]);
|
||||
}
|
||||
|
||||
//
|
||||
static inline btDbvt::Node* refit( btDbvt* pdbvt,
|
||||
btDbvt::Node* node)
|
||||
{
|
||||
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)
|
||||
{
|
||||
bottomup(pdbvt,leafs);
|
||||
node=leafs[0];
|
||||
node->parent=parent;
|
||||
parent->childs[idx]=node;
|
||||
}
|
||||
}
|
||||
return(node);
|
||||
}
|
||||
|
||||
//
|
||||
// Api
|
||||
//
|
||||
|
||||
//
|
||||
btDbvt::btDbvt()
|
||||
{
|
||||
m_root = 0;
|
||||
m_free = 0;
|
||||
m_lkhd = 2;
|
||||
m_leafs = 0;
|
||||
}
|
||||
|
||||
//
|
||||
btDbvt::~btDbvt()
|
||||
{
|
||||
clear();
|
||||
}
|
||||
|
||||
//
|
||||
void btDbvt::clear()
|
||||
{
|
||||
if(m_root) recursedeletenode(this,m_root);
|
||||
delete m_free;
|
||||
m_free=0;
|
||||
}
|
||||
|
||||
//
|
||||
void btDbvt::optimizeBottomUp()
|
||||
{
|
||||
if(m_root)
|
||||
{
|
||||
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)
|
||||
{
|
||||
tNodeArray leafs;
|
||||
leafs.reserve(m_leafs);
|
||||
fetchleafs(this,m_root,leafs);
|
||||
m_root=topdown(this,leafs,bu_treshold);
|
||||
}
|
||||
}
|
||||
|
||||
//
|
||||
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);
|
||||
}
|
||||
|
||||
//
|
||||
void btDbvt::update(Node* leaf,int lookahead)
|
||||
{
|
||||
Node* root=removeleaf(this,leaf);
|
||||
if(root)
|
||||
{
|
||||
for(int i=0;(i<lookahead)&&root->parent;++i)
|
||||
{
|
||||
root=root->parent;
|
||||
}
|
||||
}
|
||||
insertleaf(this,root,leaf);
|
||||
}
|
||||
|
||||
//
|
||||
void btDbvt::update(Node* leaf,const Volume& volume)
|
||||
{
|
||||
Node* root=removeleaf(this,leaf);
|
||||
if(root)
|
||||
{
|
||||
for(int i=0;(i<m_lkhd)&&root->parent;++i)
|
||||
{
|
||||
root=root->parent;
|
||||
}
|
||||
}
|
||||
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);
|
||||
}
|
||||
|
||||
//
|
||||
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);
|
||||
}
|
||||
|
||||
//
|
||||
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);
|
||||
}
|
||||
|
||||
//
|
||||
void btDbvt::remove(Node* leaf)
|
||||
{
|
||||
removeleaf(this,leaf);
|
||||
deletenode(this,leaf);
|
||||
--m_leafs;
|
||||
}
|
||||
|
||||
//
|
||||
void btDbvt::collide(btDbvt* tree,
|
||||
ICollide* icollide) const
|
||||
{
|
||||
collideGeneric(tree,GCollide(icollide));
|
||||
}
|
||||
|
||||
//
|
||||
void btDbvt::collide(btDbvt::Node* node,
|
||||
ICollide* icollide) const
|
||||
{
|
||||
collideGeneric(node,GCollide(icollide));
|
||||
}
|
||||
|
||||
//
|
||||
void btDbvt::collide(const Volume& volume,
|
||||
ICollide* icollide) const
|
||||
{
|
||||
collideGeneric(volume,GCollide(icollide));
|
||||
}
|
||||
|
||||
//
|
||||
void btDbvt::collide(ICollide* icollide) const
|
||||
{
|
||||
collideGeneric(GCollide(icollide));
|
||||
}
|
||||
@ -27,94 +27,94 @@ 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 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);
|
||||
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
|
||||
{
|
||||
{
|
||||
const Node* a;
|
||||
const Node* b;
|
||||
sStkElm(const Node* na,const Node* nb) : a(na),b(nb) {}
|
||||
};
|
||||
|
||||
};
|
||||
|
||||
// Interfaces
|
||||
|
||||
|
||||
/* ICollide */
|
||||
struct ICollide
|
||||
{
|
||||
{
|
||||
virtual void Process(const Node*,const Node*) {}
|
||||
virtual void Process(const Node*) {}
|
||||
virtual bool Descent(const Node*) { return(false); }
|
||||
};
|
||||
};
|
||||
/* GCollide */
|
||||
struct GCollide
|
||||
{
|
||||
{
|
||||
ICollide* icollide;
|
||||
GCollide(ICollide* ic) : icollide(ic) {}
|
||||
GCollide(ICollide* ic) : icollide(ic) {}
|
||||
void Process(const Node* a,const Node* b) { icollide->Process(a,b); }
|
||||
void Process(const Node* a) { icollide->Process(a); }
|
||||
bool Descent(const Node* a) { return(icollide->Descent(a)); }
|
||||
};
|
||||
|
||||
};
|
||||
|
||||
// Constants
|
||||
enum {
|
||||
TREETREE_STACKSIZE = 128,
|
||||
VOLUMETREE_STACKSIZE = 64,
|
||||
};
|
||||
|
||||
TREETREE_STACKSIZE = 128,
|
||||
VOLUMETREE_STACKSIZE = 64,
|
||||
};
|
||||
|
||||
// Fields
|
||||
Node* m_root;
|
||||
Node* m_free;
|
||||
int m_lkhd;
|
||||
int m_leafs;
|
||||
// Methods
|
||||
btDbvt();
|
||||
~btDbvt();
|
||||
btDbvt();
|
||||
~btDbvt();
|
||||
void clear();
|
||||
bool empty() const { return(0==m_root); }
|
||||
void optimizeBottomUp();
|
||||
@ -127,14 +127,14 @@ struct btDbvt
|
||||
bool update(Node* leaf,Volume volume,btScalar margin);
|
||||
void remove(Node* leaf);
|
||||
void collide(btDbvt* tree,
|
||||
ICollide* icollide) const;
|
||||
ICollide* icollide) const;
|
||||
void collide(btDbvt::Node* node,
|
||||
ICollide* icollide) const;
|
||||
ICollide* icollide) const;
|
||||
void collide(const Volume& volume,
|
||||
ICollide* icollide) const;
|
||||
ICollide* icollide) const;
|
||||
void collide(const btVector3& org,
|
||||
const btVector3& dir,
|
||||
ICollide* icollide) const;
|
||||
const btVector3& dir,
|
||||
ICollide* icollide) const;
|
||||
void collide(ICollide* icollide) const;
|
||||
// Generics : T must implement ICollide
|
||||
template <typename T>
|
||||
@ -146,9 +146,9 @@ struct btDbvt
|
||||
template <typename T>
|
||||
void collideGeneric(T& policy) const;
|
||||
//
|
||||
private:
|
||||
private:
|
||||
btDbvt(const btDbvt&) {}
|
||||
};
|
||||
};
|
||||
|
||||
//
|
||||
// Inline's
|
||||
@ -157,94 +157,94 @@ struct btDbvt
|
||||
//
|
||||
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())&&
|
||||
(mx.y()>=a.mx.y())&&
|
||||
(mx.z()>=a.mx.z()));
|
||||
}
|
||||
|
||||
|
||||
//
|
||||
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())&&
|
||||
@ -255,39 +255,39 @@ return( (a.mi.x()<=b.mx.x())&&
|
||||
|
||||
//
|
||||
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())&&
|
||||
(b.y()<=a.mx.y())&&
|
||||
(b.z()<=a.mx.z()));
|
||||
}
|
||||
|
||||
|
||||
//
|
||||
inline btScalar Proximity( const btDbvtAabbMm& a,
|
||||
const btDbvtAabbMm& b)
|
||||
const btDbvtAabbMm& b)
|
||||
{
|
||||
const btVector3 d=(a.mi+a.mx)-(b.mi+b.mx);
|
||||
return(btFabs(d.x())+btFabs(d.y())+btFabs(d.z()));
|
||||
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())||
|
||||
@ -303,50 +303,50 @@ return( (a.mi.x()!=b.mi.x())||
|
||||
template <typename T>
|
||||
inline void btDbvt::collideGeneric( btDbvt::Node* node,T& policy) const
|
||||
{
|
||||
if(m_root&&node)
|
||||
if(m_root&&node)
|
||||
{
|
||||
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)
|
||||
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())
|
||||
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]));
|
||||
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))
|
||||
else if(Intersect(p.a->volume,p.b->volume))
|
||||
{
|
||||
if(p.a->isinternal())
|
||||
if(p.a->isinternal())
|
||||
{
|
||||
if(p.b->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]));
|
||||
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));
|
||||
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())
|
||||
if(p.b->isinternal())
|
||||
{
|
||||
stack.push_back(sStkElm(p.a,p.b->childs[0]));
|
||||
stack.push_back(sStkElm(p.a,p.b->childs[1]));
|
||||
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);
|
||||
policy.Process(p.a,p.b);
|
||||
}
|
||||
}
|
||||
}
|
||||
@ -358,31 +358,31 @@ if(m_root&&node)
|
||||
template <typename T>
|
||||
inline void btDbvt::collideGeneric( btDbvt* tree,T& policy) const
|
||||
{
|
||||
collideGeneric<T>(tree->m_root,policy);
|
||||
collideGeneric<T>(tree->m_root,policy);
|
||||
}
|
||||
|
||||
//
|
||||
template <typename T>
|
||||
inline void btDbvt::collideGeneric(const Volume& volume,T& policy) const
|
||||
{
|
||||
if(m_root)
|
||||
if(m_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(Intersect(n->volume,volume))
|
||||
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())
|
||||
if(n->isinternal())
|
||||
{
|
||||
stack.push_back(n->childs[0]);
|
||||
stack.push_back(n->childs[1]);
|
||||
stack.push_back(n->childs[0]);
|
||||
stack.push_back(n->childs[1]);
|
||||
}
|
||||
else
|
||||
{
|
||||
policy.Process(n);
|
||||
policy.Process(n);
|
||||
}
|
||||
}
|
||||
} while(stack.size()>0);
|
||||
@ -393,17 +393,17 @@ if(m_root)
|
||||
template <typename T>
|
||||
inline void btDbvt::collideGeneric(T& policy) const
|
||||
{
|
||||
if(m_root)
|
||||
if(m_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))
|
||||
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))
|
||||
{
|
||||
if(n->isinternal())
|
||||
if(n->isinternal())
|
||||
{ stack.push_back(n->childs[0]);stack.push_back(n->childs[1]); }
|
||||
else
|
||||
{ policy.Process(n); }
|
||||
@ -56,6 +56,7 @@ struct btDispatcherInfo
|
||||
class btIDebugDraw* m_debugDraw;
|
||||
bool m_enableSatConvex;
|
||||
bool m_enableSPU;
|
||||
bool m_useEpa;
|
||||
btStackAlloc* m_stackAllocator;
|
||||
|
||||
};
|
||||
|
||||
@ -1,493 +0,0 @@
|
||||
/*
|
||||
Bullet Continuous Collision Detection and Physics Library
|
||||
Copyright (c) 2003-2006 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.
|
||||
*/
|
||||
///btDbvt implementation by Nathanael Presson
|
||||
|
||||
#include "btDbvt.h"
|
||||
|
||||
//
|
||||
typedef btAlignedObjectArray<btDbvt::Node*> tNodeArray;
|
||||
|
||||
//
|
||||
static inline int indexof(const btDbvt::Node* node)
|
||||
{
|
||||
return(node->parent->childs[1]==node);
|
||||
}
|
||||
|
||||
//
|
||||
static inline btDbvt::Volume merge( const btDbvt::Volume& a,
|
||||
const btDbvt::Volume& b)
|
||||
{
|
||||
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()+
|
||||
edges.x()+edges.y()+edges.z());
|
||||
}
|
||||
|
||||
//
|
||||
static inline void deletenode( btDbvt* pdbvt,
|
||||
btDbvt::Node* node)
|
||||
{
|
||||
delete pdbvt->m_free;
|
||||
pdbvt->m_free=node;
|
||||
}
|
||||
|
||||
//
|
||||
static inline void recursedeletenode( btDbvt* pdbvt,
|
||||
btDbvt::Node* node)
|
||||
{
|
||||
if(!node->isleaf())
|
||||
{
|
||||
recursedeletenode(pdbvt,node->childs[0]);
|
||||
recursedeletenode(pdbvt,node->childs[1]);
|
||||
}
|
||||
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* 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);
|
||||
}
|
||||
|
||||
//
|
||||
static inline void insertleaf( btDbvt* pdbvt,
|
||||
btDbvt::Node* root,
|
||||
btDbvt::Node* leaf)
|
||||
{
|
||||
if(!pdbvt->m_root)
|
||||
{
|
||||
pdbvt->m_root = leaf;
|
||||
leaf->parent = 0;
|
||||
}
|
||||
else
|
||||
{
|
||||
if(!root->isleaf())
|
||||
{
|
||||
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];
|
||||
} while(!root->isleaf());
|
||||
}
|
||||
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;
|
||||
else
|
||||
Merge(prev->childs[0]->volume,prev->childs[1]->volume,prev->volume);
|
||||
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;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
//
|
||||
static inline btDbvt::Node* removeleaf( btDbvt* pdbvt,
|
||||
btDbvt::Node* leaf)
|
||||
{
|
||||
if(leaf==pdbvt->m_root)
|
||||
{
|
||||
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)
|
||||
{
|
||||
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))
|
||||
{
|
||||
sibling = prev;
|
||||
prev = prev->parent;
|
||||
} else break;
|
||||
}
|
||||
return(prev?prev:pdbvt->m_root);
|
||||
}
|
||||
else
|
||||
{
|
||||
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)
|
||||
{
|
||||
if(root->isinternal()&&depth)
|
||||
{
|
||||
fetchleafs(pdbvt,root->childs[0],leafs,depth-1);
|
||||
fetchleafs(pdbvt,root->childs[1],leafs,depth-1);
|
||||
deletenode(pdbvt,root);
|
||||
}
|
||||
else
|
||||
{
|
||||
leafs.push_back(root);
|
||||
}
|
||||
}
|
||||
|
||||
//
|
||||
static void split( const tNodeArray& leafs,
|
||||
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)
|
||||
{
|
||||
if(dot(axis,leafs[i]->volume.Center()-org)<0)
|
||||
left.push_back(leafs[i]);
|
||||
else
|
||||
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)
|
||||
{
|
||||
volume=merge(volume,leafs[i]->volume);
|
||||
}
|
||||
return(volume);
|
||||
}
|
||||
|
||||
//
|
||||
static void bottomup( btDbvt* pdbvt,
|
||||
tNodeArray& leafs)
|
||||
{
|
||||
while(leafs.size()>1)
|
||||
{
|
||||
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)
|
||||
{
|
||||
const btScalar sz=size(merge(leafs[i]->volume,leafs[j]->volume));
|
||||
if(sz<minsize)
|
||||
{
|
||||
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();
|
||||
}
|
||||
}
|
||||
|
||||
//
|
||||
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)
|
||||
{
|
||||
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 btVector3 x=leafs[i]->volume.Center()-org;
|
||||
for(int j=0;j<3;++j)
|
||||
{
|
||||
++splitcount[j][dot(x,axis[j])>0?1:0];
|
||||
}
|
||||
}
|
||||
for(int i=0;i<3;++i)
|
||||
{
|
||||
if((splitcount[i][0]>0)&&(splitcount[i][1]>0))
|
||||
{
|
||||
const int midp=abs(splitcount[i][0]-splitcount[i][1]);
|
||||
if(midp<bestmidp)
|
||||
{
|
||||
bestaxis=i;
|
||||
bestmidp=midp;
|
||||
}
|
||||
}
|
||||
}
|
||||
if(bestaxis>=0)
|
||||
{
|
||||
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[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);
|
||||
}
|
||||
else
|
||||
{
|
||||
bottomup(pdbvt,leafs);
|
||||
return(leafs[0]);
|
||||
}
|
||||
}
|
||||
return(leafs[0]);
|
||||
}
|
||||
|
||||
//
|
||||
static inline btDbvt::Node* refit( btDbvt* pdbvt,
|
||||
btDbvt::Node* node)
|
||||
{
|
||||
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)
|
||||
{
|
||||
bottomup(pdbvt,leafs);
|
||||
node=leafs[0];
|
||||
node->parent=parent;
|
||||
parent->childs[idx]=node;
|
||||
}
|
||||
}
|
||||
return(node);
|
||||
}
|
||||
|
||||
//
|
||||
// Api
|
||||
//
|
||||
|
||||
//
|
||||
btDbvt::btDbvt()
|
||||
{
|
||||
m_root = 0;
|
||||
m_free = 0;
|
||||
m_lkhd = 2;
|
||||
m_leafs = 0;
|
||||
}
|
||||
|
||||
//
|
||||
btDbvt::~btDbvt()
|
||||
{
|
||||
clear();
|
||||
}
|
||||
|
||||
//
|
||||
void btDbvt::clear()
|
||||
{
|
||||
if(m_root) recursedeletenode(this,m_root);
|
||||
delete m_free;
|
||||
m_free=0;
|
||||
}
|
||||
|
||||
//
|
||||
void btDbvt::optimizeBottomUp()
|
||||
{
|
||||
if(m_root)
|
||||
{
|
||||
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)
|
||||
{
|
||||
tNodeArray leafs;
|
||||
leafs.reserve(m_leafs);
|
||||
fetchleafs(this,m_root,leafs);
|
||||
m_root=topdown(this,leafs,bu_treshold);
|
||||
}
|
||||
}
|
||||
|
||||
//
|
||||
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);
|
||||
}
|
||||
|
||||
//
|
||||
void btDbvt::update(Node* leaf,int lookahead)
|
||||
{
|
||||
Node* root=removeleaf(this,leaf);
|
||||
if(root)
|
||||
{
|
||||
for(int i=0;(i<lookahead)&&root->parent;++i)
|
||||
{
|
||||
root=root->parent;
|
||||
}
|
||||
}
|
||||
insertleaf(this,root,leaf);
|
||||
}
|
||||
|
||||
//
|
||||
void btDbvt::update(Node* leaf,const Volume& volume)
|
||||
{
|
||||
Node* root=removeleaf(this,leaf);
|
||||
if(root)
|
||||
{
|
||||
for(int i=0;(i<m_lkhd)&&root->parent;++i)
|
||||
{
|
||||
root=root->parent;
|
||||
}
|
||||
}
|
||||
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);
|
||||
}
|
||||
|
||||
//
|
||||
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);
|
||||
}
|
||||
|
||||
//
|
||||
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);
|
||||
}
|
||||
|
||||
//
|
||||
void btDbvt::remove(Node* leaf)
|
||||
{
|
||||
removeleaf(this,leaf);
|
||||
deletenode(this,leaf);
|
||||
--m_leafs;
|
||||
}
|
||||
|
||||
//
|
||||
void btDbvt::collide(btDbvt* tree,
|
||||
ICollide* icollide) const
|
||||
{
|
||||
collideGeneric(tree,GCollide(icollide));
|
||||
}
|
||||
|
||||
//
|
||||
void btDbvt::collide(btDbvt::Node* node,
|
||||
ICollide* icollide) const
|
||||
{
|
||||
collideGeneric(node,GCollide(icollide));
|
||||
}
|
||||
|
||||
//
|
||||
void btDbvt::collide(const Volume& volume,
|
||||
ICollide* icollide) const
|
||||
{
|
||||
collideGeneric(volume,GCollide(icollide));
|
||||
}
|
||||
|
||||
//
|
||||
void btDbvt::collide(ICollide* icollide) const
|
||||
{
|
||||
collideGeneric(GCollide(icollide));
|
||||
}
|
||||
@ -26,7 +26,7 @@ subject to the following restrictions:
|
||||
#include "BulletCollision/CollisionShapes/btConcaveShape.h"
|
||||
#include "BulletCollision/CollisionDispatch/btCollisionCreateFunc.h"
|
||||
#include "btSparseSDF.h"
|
||||
#include "btDbvt.h"
|
||||
#include "BulletCollision/BroadphaseCollision/btDbvt.h"
|
||||
|
||||
class btBroadphaseInterface;
|
||||
class btCollisionDispatcher;
|
||||
|
||||
@ -15,7 +15,7 @@ subject to the following restrictions:
|
||||
///btSoftBodyHelpers.cpp by Nathanael Presson
|
||||
|
||||
#include "btSoftBody.h"
|
||||
#include "btDbvt.h"
|
||||
#include "BulletCollision/BroadphaseCollision/btDbvt.h"
|
||||
#include <stdio.h>
|
||||
#include <string.h>
|
||||
#include "btSoftBodyHelpers.h"
|
||||
@ -23,57 +23,57 @@ 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);
|
||||
}
|
||||
}
|
||||
}
|
||||
@ -93,26 +93,26 @@ static inline btScalar tetravolume(const btVector3& x0,
|
||||
|
||||
//
|
||||
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);
|
||||
}
|
||||
}
|
||||
|
||||
//
|
||||
void btSoftBodyHelpers::Draw( btSoftBody* psb,
|
||||
btIDebugDraw* idraw,
|
||||
int drawflags)
|
||||
btIDebugDraw* idraw,
|
||||
int drawflags)
|
||||
{
|
||||
const btScalar scl=(btScalar)0.1;
|
||||
const btScalar nscl=scl*5;
|
||||
@ -208,14 +208,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,
|
||||
f.m_n[0]->m_n,f.m_n[1]->m_n,f.m_n[2]->m_n,
|
||||
col,alp);*/
|
||||
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 */
|
||||
@ -244,9 +244,9 @@ 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);
|
||||
}
|
||||
}
|
||||
@ -254,10 +254,10 @@ void btSoftBodyHelpers::Draw( btSoftBody* psb,
|
||||
|
||||
//
|
||||
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)
|
||||
{
|
||||
@ -280,25 +280,25 @@ void btSoftBodyHelpers::DrawInfos( btSoftBody* psb,
|
||||
|
||||
//
|
||||
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)
|
||||
{
|
||||
@ -322,9 +322,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;
|
||||
@ -352,13 +352,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 */
|
||||
@ -427,8 +427,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
|
||||
{
|
||||
@ -459,8 +459,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)
|
||||
@ -499,7 +499,7 @@ btSoftBody* btSoftBodyHelpers::CreateFromTriMesh(btSoftBody::btSoftBodyWorldInf
|
||||
|
||||
//
|
||||
btSoftBody* btSoftBodyHelpers::CreateFromConvexHull(btSoftBody::btSoftBodyWorldInfo& worldInfo, const btVector3* vertices,
|
||||
int nvertices)
|
||||
int nvertices)
|
||||
{
|
||||
HullDesc hdsc(QF_TRIANGLES,nvertices,vertices);
|
||||
HullResult hres;
|
||||
@ -533,32 +533,32 @@ btSoftBody* btSoftBodyHelpers::CreateFromConvexHull(btSoftBody::btSoftBodyWorld
|
||||
|
||||
//
|
||||
void btSoftBodyHelpers::ExportAsSMeshFile(btSoftBody* psb,
|
||||
const char* filename)
|
||||
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)
|
||||
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 << " "
|
||||
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)
|
||||
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 << " "
|
||||
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();
|
||||
output << 0 << "\n";
|
||||
output << 0 << "\n";
|
||||
output.close();
|
||||
}
|
||||
|
||||
/* Create from TetGen .ele, .face, .node files */
|
||||
@ -570,20 +570,20 @@ btSoftBody* btSoftBodyHelpers::CreateFromTetGenFile(btSoftBody::btSoftBodyWorldI
|
||||
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));
|
||||
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 */
|
||||
@ -595,81 +595,81 @@ btSoftBody* btSoftBodyHelpers::CreateFromTetGenData(btSoftBody::btSoftBodyWorldI
|
||||
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)
|
||||
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);
|
||||
}
|
||||
}
|
||||
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);
|
||||
}
|
||||
if(ele&&ele[0])
|
||||
btSoftBody* psb=new btSoftBody(&worldInfo,nnode,&pos[0],0);
|
||||
if(face&&face[0])
|
||||
{
|
||||
int ntetra=0;
|
||||
int ncorner=0;
|
||||
int neattrb=0;
|
||||
se>>ntetra;se>>ncorner;se>>neattrb;
|
||||
for(int i=0;i<ntetra;++i)
|
||||
int nface=0;
|
||||
sf>>nface;sf>>hasbounds;
|
||||
for(int i=0;i<nface;++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)
|
||||
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);
|
||||
psb->appendLink(ni[0],ni[3],0,true);
|
||||
psb->appendLink(ni[1],ni[3],0,true);
|
||||
psb->appendLink(ni[2],ni[3],0,true);
|
||||
psb->appendLink(ni[0],ni[1],0,true);
|
||||
psb->appendLink(ni[1],ni[2],0,true);
|
||||
psb->appendLink(ni[2],ni[0],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);
|
||||
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
|
||||
|
||||
@ -23,7 +23,7 @@ subject to the following restrictions:
|
||||
#include <cfloat>
|
||||
#include <float.h>
|
||||
|
||||
#define BT_BULLET_VERSION 268
|
||||
#define BT_BULLET_VERSION 269
|
||||
|
||||
inline int btGetVersion()
|
||||
{
|
||||
|
||||
Loading…
Reference in New Issue
Block a user