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

312
Demos/SoftDemo/SoftDemo.cpp
View File

@@ -99,13 +99,32 @@ void SoftDemo::clientMoveAndDisplay()
float dt = getDeltaTimeMicroseconds() * 0.000001f; float dt = 1.0/60.;
// printf("dt = %f: ",dt);
if (m_dynamicsWorld) if (m_dynamicsWorld)
{ {
if(m_drag)
{
const int x=m_lastmousepos[0];
const int y=m_lastmousepos[1];
const btVector3 rayFrom=m_cameraPosition;
const btVector3 rayTo=getRayTo(x,y);
const btVector3 rayDir=(rayTo-rayFrom).normalized();
const btVector3 N=(m_cameraTargetPosition-m_cameraPosition).normalized();
const btScalar O=dot(m_impact,N);
const btScalar den=dot(N,rayDir);
if((den*den)>0)
{
const btScalar num=O-dot(N,rayFrom);
const btScalar hit=num/den;
if((hit>0)&&(hit<1500))
{
m_goal=rayFrom+rayDir*hit;
}
}
m_node->m_v+=(m_goal-m_node->m_x)/dt;
}
#define FIXED_STEP #define FIXED_STEP
#ifdef FIXED_STEP #ifdef FIXED_STEP
m_dynamicsWorld->stepSimulation(dt=1.0f/60.f,0); m_dynamicsWorld->stepSimulation(dt=1.0f/60.f,0);
@@ -137,6 +156,10 @@ void SoftDemo::clientMoveAndDisplay()
#endif #endif
if(m_drag)
{
m_node->m_v*=0;
}
m_softBodyWorldInfo.m_sparsesdf.GarbageCollect(); m_softBodyWorldInfo.m_sparsesdf.GarbageCollect();
@@ -670,6 +693,28 @@ static void Init_Sticks(SoftDemo* pdemo)
Ctor_BigBall(pdemo); Ctor_BigBall(pdemo);
} }
//
// Bending
//
static void Init_Bending(SoftDemo* pdemo)
{
//TRACEDEMO
const btScalar s=4;
const btVector3 x[]={ btVector3(-s,0,-s),
btVector3(+s,0,-s),
btVector3(+s,0,+s),
btVector3(-s,0,+s)};
const btScalar m[]={ 0,0,0,1};
btSoftBody* psb=new btSoftBody(&pdemo->m_softBodyWorldInfo,4,x,m);
psb->appendLink(0,1);
psb->appendLink(1,2);
psb->appendLink(2,3);
psb->appendLink(3,0);
psb->appendLink(0,2);
pdemo->getSoftDynamicsWorld()->addSoftBody(psb);
}
// //
// 100kg cloth locked at corners, 10 falling 10kg rb's. // 100kg cloth locked at corners, 10 falling 10kg rb's.
// //
@@ -715,6 +760,7 @@ static void Init_Bunny(SoftDemo* pdemo)
psb->scale(btVector3(6,6,6)); psb->scale(btVector3(6,6,6));
psb->setTotalMass(100,true); psb->setTotalMass(100,true);
pdemo->getSoftDynamicsWorld()->addSoftBody(psb); pdemo->getSoftDynamicsWorld()->addSoftBody(psb);
pdemo->m_cutting=true;
} }
@@ -756,6 +802,7 @@ static void Init_Torus(SoftDemo* pdemo)
psb->scale(btVector3(2,2,2)); psb->scale(btVector3(2,2,2));
psb->setTotalMass(50,true); psb->setTotalMass(50,true);
pdemo->getSoftDynamicsWorld()->addSoftBody(psb); pdemo->getSoftDynamicsWorld()->addSoftBody(psb);
pdemo->m_cutting=true;
} }
@@ -798,155 +845,6 @@ static void Init_Cutting1(SoftDemo* pdemo)
pdemo->m_cutting=true; pdemo->m_cutting=true;
} }
#ifdef BT_SOFTBODY_USE_STL
//
// TetraBunny
//
static void Init_TetraBunny(SoftDemo* pdemo)
{
btSoftBody* psb=btSoftBodyHelpers::CreateFromTetGenData(pdemo->m_softBodyWorldInfo,
TetraBunny::getElements(),
0,
TetraBunny::getNodes(),
false,true,true);
pdemo->getSoftDynamicsWorld()->addSoftBody(psb);
psb->rotate(btQuaternion(SIMD_PI/2,0,0));
psb->setVolumeMass(150);
psb->m_cfg.piterations=2;
pdemo->m_cutting=true;
}
//
// TetraCube
//
static void Init_TetraCube(SoftDemo* pdemo)
{
btSoftBody* psb=btSoftBodyHelpers::CreateFromTetGenData(pdemo->m_softBodyWorldInfo,
TetraCube::getElements(),
0,
TetraCube::getNodes(),
false,true,true);
pdemo->getSoftDynamicsWorld()->addSoftBody(psb);
psb->scale(btVector3(4,4,4));
psb->translate(btVector3(0,5,0));
psb->setVolumeMass(300);
psb->setMass(0,0);
/*psb->setMass(10,0);
psb->setMass(20,0);*/
psb->m_cfg.piterations=1;
//psb->m_materials[0]->m_kLST=0.05;
pdemo->m_cutting=true;
}
#endif //BT_SOFTBODY_USE_STL
//
// Tetra
//
static void Init_Tetra(SoftDemo* pdemo)
{
//TRACEDEMO
#if 0
{
btVector3 pts[]={ btVector3(-1,-1,-1),
btVector3(+1,-1,-1),
btVector3(+1,+1,-1),
btVector3(-1,+1,-1),
btVector3(-1,-1,+1),
btVector3(+1,-1,+1),
btVector3(+1,+1,+1),
btVector3(-1,+1,+1)};
btSoftBody* psb=btSoftBodyHelpers::CreateFromConvexHull(pdemo->m_softBodyWorldInfo,pts,8);
btSoftBodyHelpers::ExportAsSMeshFile(psb,"C:/HomeH/Oss/tetgen/Release/cube.smesh");
delete psb;
/*btSoftBody* psb=btSoftBodyHelpers::CreateFromTriMesh( pdemo->m_softBodyWorldInfo,gVerticesBunny,
&gIndicesBunny[0][0],
BUNNY_NUM_TRIANGLES);
psb->scale(btVector3(6,6,6));
btSoftBodyHelpers::ExportAsSMeshFile(psb,"C:/HomeH/Oss/tetgen/Release/bunny.smesh");
delete psb;*/
}
btSoftBody* psb=btSoftBodyHelpers::CreateFromTetGenFile(pdemo->m_softBodyWorldInfo,
/*"C:/HomeH/Oss/tetgen/Release/bunny.1.ele",
"C:/HomeH/Oss/tetgen/Release/bunny.1.face",
"C:/HomeH/Oss/tetgen/Release/bunny.1.node",*/
"C:/HomeH/Oss/tetgen/Release/cube.1.ele",
0/*"C:/HomeH/Oss/tetgen/Release/cube.1.face"*/,
"C:/HomeH/Oss/tetgen/Release/cube.1.node",
true,true);
pdemo->getSoftDynamicsWorld()->addSoftBody(psb);
psb->scale(btVector3(4,4,4));
/*psb->rotate(btQuaternion(SIMD_PI/4,SIMD_PI/4,0));
psb->translate(btVector3(0,10,0));*/
psb->setVolumeMass(30);
psb->m_cfg.piterations=1;
psb->m_cfg.kKHR=1;
//psb->addVelocity(btVector3(0,50,0));
//psb->m_tetras.clear();
ImplicitSphere fnc;
fnc.center = btVector3(4,4,4);
fnc.radius = 4;
psb->refine(&fnc,0.001,true);
//psb->m_tetras.clear();
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());
#else
#if 1
const btScalar s=4;
const int r=32;
const btVector3 p[]={ btVector3(+s,0,-s),
btVector3(-s,0,-s),
btVector3(+s,0,+s),
btVector3(-s,0,+s)};
btSoftBody* psb=btSoftBodyHelpers::CreatePatch(pdemo->m_softBodyWorldInfo,p[0],p[1],p[2],p[3],r,r,1+2+4+8,true);
pdemo->getSoftDynamicsWorld()->addSoftBody(psb);
psb->m_cfg.piterations=2;
/*ImplicitSphere fnc;
fnc.center = btVector3(0,0,0);
fnc.radius = 1.5;
psb->refine(&fnc,0.001,true);*/
//psb->m_faces.clear();
/*fnc.center = btVector3(4,0,4);
fnc.radius = 2;
psb->refine(&fnc,0.001,true);*/
#else
const btScalar s=4;
const btVector3 p[]={ btVector3(+s,-s,0),
btVector3(-s,0,0),
btVector3(0,0,+s),
btVector3(0,+s,0)};
btSoftBody* psb=new btSoftBody(&pdemo->m_softBodyWorldInfo,4,p,0);
psb->appendTetra(0,1,2,3);
psb->appendLink(0,1,1,btSoftBody::eLType::Structural);
psb->appendLink(1,2,1,btSoftBody::eLType::Structural);
psb->appendLink(2,0,1,btSoftBody::eLType::Structural);
psb->appendLink(0,3,1,btSoftBody::eLType::Structural);
psb->appendLink(1,3,1,btSoftBody::eLType::Structural);
psb->appendLink(2,3,1,btSoftBody::eLType::Structural);
pdemo->getSoftDynamicsWorld()->addSoftBody(psb);
psb->setSolver(btSoftBody::eSolverPresets::Velocities);
psb->m_cfg.viterations=1;
psb->m_cfg.diterations=1;
psb->m_cfg.kDF=0;
psb->m_cfg.kLST=0.000001;
//psb1->m_cfg.diterations=1;
/*btSoftBody* psb0=btSoftBodyHelpers::CreateRope( pdemo->m_softBodyWorldInfo,
btVector3(0,0,0),btVector3(5,0,0),16,1);
pdemo->getSoftDynamicsWorld()->addSoftBody(psb0);
btSoftBody* psb1=btSoftBodyHelpers::CreateRope( pdemo->m_softBodyWorldInfo,
btVector3(0,0,2),btVector3(5,0,2),16,1);
psb1->m_cfg.viterations=1;
psb1->m_cfg.diterations=1;
psb1->setSolver(btSoftBody::eSolverPresets::Velocities);
pdemo->getSoftDynamicsWorld()->addSoftBody(psb1);*/
#endif
#endif
pdemo->toggleIdle();
}
unsigned current_demo=0; unsigned current_demo=0;
void SoftDemo::clientResetScene() void SoftDemo::clientResetScene()
@@ -977,11 +875,6 @@ void SoftDemo::clientResetScene()
void (*demofncs[])(SoftDemo*)= void (*demofncs[])(SoftDemo*)=
{ {
Init_Cloth, Init_Cloth,
Init_Cutting1,
#ifdef BT_SOFTBODY_USE_STL
Init_TetraBunny,
Init_TetraCube,
#endif //BT_SOFTBODY_USE_STL
Init_Pressure, Init_Pressure,
Init_Volume, Init_Volume,
Init_Ropes, Init_Ropes,
@@ -998,6 +891,7 @@ void SoftDemo::clientResetScene()
Init_TorusMatch, Init_TorusMatch,
Init_Bunny, Init_Bunny,
Init_BunnyMatch, Init_BunnyMatch,
Init_Cutting1,
}; };
current_demo=current_demo%(sizeof(demofncs)/sizeof(demofncs[0])); current_demo=current_demo%(sizeof(demofncs)/sizeof(demofncs[0]));
@@ -1012,6 +906,7 @@ void SoftDemo::clientResetScene()
m_autocam = false; m_autocam = false;
m_raycast = false; m_raycast = false;
m_cutting = false; m_cutting = false;
m_results.time = SIMD_INFINITY;
demofncs[current_demo](this); demofncs[current_demo](this);
} }
@@ -1157,53 +1052,106 @@ void SoftDemo::keyboardCallback(unsigned char key, int x, int y)
} }
} }
//
void SoftDemo::mouseMotionFunc(int x,int y)
{
if(m_node&&(m_results.time<SIMD_INFINITY))
{
if(!m_drag)
{
#define SQ(_x_) (_x_)*(_x_)
if((SQ(x-m_lastmousepos[0])+SQ(y-m_lastmousepos[1]))>6)
{
m_drag=true;
}
#undef SQ
}
if(m_drag)
{
m_lastmousepos[0] = x;
m_lastmousepos[1] = y;
}
}
else
{
DemoApplication::mouseMotionFunc(x,y);
}
}
// //
void SoftDemo::mouseFunc(int button, int state, int x, int y) void SoftDemo::mouseFunc(int button, int state, int x, int y)
{ {
if(m_cutting&&(state==0)&&(button==0)) if(button==0)
{
switch(state)
{
case 0:
{
m_results.time=SIMD_INFINITY;
DemoApplication::mouseFunc(button,state,x,y);
if(!m_pickConstraint)
{ {
const btVector3 rayFrom=m_cameraPosition; const btVector3 rayFrom=m_cameraPosition;
const btVector3 rayTo=getRayTo(x,y); const btVector3 rayTo=getRayTo(x,y);
const btVector3 rayDir=(rayTo-rayFrom).normalized(); const btVector3 rayDir=(rayTo-rayFrom).normalized();
btSoftBodyArray& sbs=getSoftDynamicsWorld()->getSoftBodyArray(); btSoftBodyArray& sbs=getSoftDynamicsWorld()->getSoftBodyArray();
btSoftBody::sRayCast results;
results.time=SIMD_INFINITY;
for(int ib=0;ib<sbs.size();++ib) for(int ib=0;ib<sbs.size();++ib)
{ {
btSoftBody* psb=sbs[ib]; btSoftBody* psb=sbs[ib];
btSoftBody::sRayCast res; btSoftBody::sRayCast res;
if(psb->rayCast(rayFrom,rayDir,res,results.time)) if(psb->rayCast(rayFrom,rayDir,res,m_results.time))
{ {
results=res; m_results=res;
} }
} }
if(results.time<SIMD_INFINITY) if(m_results.time<SIMD_INFINITY)
{ {
#if 0 m_impact = rayFrom+rayDir*m_results.time;
const btVector3 x=rayFrom+rayDir*results.time; m_drag = false;
const btSoftBody::Face& f=results.body->m_faces[results.index]; m_lastmousepos[0] = x;
btScalar bestarea=SIMD_INFINITY; m_lastmousepos[1] = y;
const btSoftBody::Node* n[2]={0,0}; m_node = 0;
for(int i=2,j=0;j<3;i=j++) switch(m_results.feature)
{ {
const btScalar a2=cross(f.m_n[i]->m_x-x,f.m_n[j]->m_x-x).length2(); case btSoftBody::eFeature::Face:
if(a2<bestarea)
{ {
bestarea=a2; btSoftBody::Face& f=m_results.body->m_faces[m_results.index];
n[0]=f.m_n[i]; m_node=f.m_n[0];
n[1]=f.m_n[j]; for(int i=1;i<3;++i)
{
if( (m_node->m_x-m_impact).length2()>
(f.m_n[i]->m_x-m_impact).length2())
{
m_node=f.m_n[i];
} }
} }
results.body->cutLink(n[0],n[1],0.5); }
#endif break;
ImplicitSphere isphere(rayFrom+rayDir*results.time,1); }
printf("Mass before: %f\r\n",results.body->getTotalMass()); if(m_node) m_goal=m_node->m_x;
results.body->refine(&isphere,0.0001,true);
printf("Mass after: %f\r\n",results.body->getTotalMass());
return; return;
} }
} }
DemoApplication::mouseFunc(button,state,x,y); }
break;
case 1:
if((!m_drag)&&m_cutting&&(m_results.time<SIMD_INFINITY))
{
ImplicitSphere isphere(m_impact,1);
printf("Mass before: %f\r\n",m_results.body->getTotalMass());
m_results.body->refine(&isphere,0.0001,true);
printf("Mass after: %f\r\n",m_results.body->getTotalMass());
}
m_results.time=SIMD_INFINITY;
m_drag=false;
DemoApplication::mouseFunc(button,state,x,y);
break;
}
}
else
{
DemoApplication::mouseFunc(button,state,x,y);
}
} }

11
Demos/SoftDemo/SoftDemo.h
View File

@@ -57,6 +57,12 @@ public:
bool m_raycast; bool m_raycast;
btScalar m_animtime; btScalar m_animtime;
btClock m_clock; btClock m_clock;
int m_lastmousepos[2];
btVector3 m_impact;
btSoftBody::sRayCast m_results;
btSoftBody::Node* m_node;
btVector3 m_goal;
bool m_drag;
//keep the collision shapes, for deletion/cleanup //keep the collision shapes, for deletion/cleanup
@@ -80,6 +86,10 @@ public:
void exitPhysics(); void exitPhysics();
SoftDemo() : m_drag(false)
{
}
virtual ~SoftDemo() virtual ~SoftDemo()
{ {
exitPhysics(); exitPhysics();
@@ -118,6 +128,7 @@ public:
void renderme(); void renderme();
void keyboardCallback(unsigned char key, int x, int y); void keyboardCallback(unsigned char key, int x, int y);
void mouseFunc(int button, int state, int x, int y); void mouseFunc(int button, int state, int x, int y);
void mouseMotionFunc(int x,int y);
}; };

632
Extras/CDTestFramework/BulletSAPCompleteBoxPruningTest.cpp
View File

@@ -22,10 +22,319 @@ subject to the following restrictions:
#include "RenderingHelpers.h" #include "RenderingHelpers.h"
#include "GLFontRenderer.h" #include "GLFontRenderer.h"
#include "btBulletCollisionCommon.h" #include "btBulletCollisionCommon.h"
#include "BulletSoftBody/btDbvtBroadphase.h" #include "BulletCollision/BroadphaseCollision/btDbvtBroadphase.h"
#include "Camera.h"
int numParts =2; int numParts =2;
bool enableCulling = true;
bool cullFarPlane = false;
bool showCulling = false;
bool enableOcclusion = false;
bool showOcclusion = true;
int visiblecount = 0;
struct OcclusionBuffer
{
struct WriteOCL
{
static inline bool Process(btScalar& q,btScalar v) { if(q<v) q=v;return(false); }
};
struct QueryOCL
{
static inline bool Process(btScalar& q,btScalar v) { return(q<=v); }
};
bool initialized;
btAlignedObjectArray<btScalar> buffer;
int sizes[2];
btScalar scales[2];
btScalar offsets[2];
btScalar wtrs[16];
btVector3 eye;
btVector3 neardist;
btScalar ocarea;
btScalar qrarea;
GLuint texture;
OcclusionBuffer()
{
initialized=false;
neardist=btVector3(2,2,2);
ocarea=(btScalar)0;
qrarea=(btScalar)0;
}
void setup(int w,int h)
{
initialized=true;
sizes[0]=w;
sizes[1]=h;
scales[0]=w/2;
scales[1]=h/2;
offsets[0]=scales[0]+0.5;
offsets[1]=scales[1]+0.5;
glGenTextures(1,&texture);
clear();
}
void clear()
{
buffer.resize(0);
buffer.resize(sizes[0]*sizes[1],0);
}
void initialize()
{
if(!initialized)
{
setup(128,128);
}
GLint v[4];
GLdouble m[16],p[16];
glGetIntegerv(GL_VIEWPORT,v);
glGetDoublev(GL_MODELVIEW_MATRIX,m);
glGetDoublev(GL_PROJECTION_MATRIX,p);
for(int i=0;i<16;++i) wtrs[i]=p[i];
clear();
}
void drawBuffer( btScalar l,btScalar t,
btScalar r,btScalar b)
{
btAlignedObjectArray<GLubyte> data;
data.resize(buffer.size());
for(int i=0;i<data.size();++i)
{
data[i]=int(32/buffer[i])%255;
}
glBindTexture(GL_TEXTURE_2D,texture);
glDisable(GL_BLEND);
glEnable(GL_TEXTURE_2D);
glTexParameteri(GL_TEXTURE_2D,GL_TEXTURE_WRAP_S,GL_REPEAT);
glTexParameteri(GL_TEXTURE_2D,GL_TEXTURE_WRAP_T,GL_REPEAT);
glTexParameteri(GL_TEXTURE_2D,GL_TEXTURE_MAG_FILTER,GL_NEAREST);
glTexParameteri(GL_TEXTURE_2D,GL_TEXTURE_MIN_FILTER,GL_NEAREST);
glTexImage2D(GL_TEXTURE_2D,0,GL_LUMINANCE,sizes[0],sizes[1],0,GL_LUMINANCE,GL_UNSIGNED_BYTE,&data[0]);
glBegin(GL_QUADS);
glColor4ub(255,255,255,255);
glTexCoord2f(0,0);glVertex2f(l,t);
glTexCoord2f(1,0);glVertex2f(r,t);
glTexCoord2f(1,1);glVertex2f(r,b);
glTexCoord2f(0,1);glVertex2f(l,b);
glEnd();
glDisable(GL_TEXTURE_2D);
}
btVector4 transform(const btVector3& x) const
{
btVector4 t;
t[0] = x[0]*wtrs[0]+x[1]*wtrs[4]+x[2]*wtrs[8]+wtrs[12];
t[1] = x[0]*wtrs[1]+x[1]*wtrs[5]+x[2]*wtrs[9]+wtrs[13];
t[2] = x[0]*wtrs[2]+x[1]*wtrs[6]+x[2]*wtrs[10]+wtrs[14];
t[3] = x[0]*wtrs[3]+x[1]*wtrs[7]+x[2]*wtrs[11]+wtrs[15];
return(t);
}
static bool project(btVector4* p,int n)
{
for(int i=0;i<n;++i)
{
const btScalar iw=1/p[i][3];
p[i][2]=1/p[i][3];
p[i][0]*=p[i][2];
p[i][1]*=p[i][2];
}
return(true);
}
template <const int NP>
static int clip(const btVector4* pi,btVector4* po)
{
btScalar s[NP];
int m=0;
for(int i=0;i<NP;++i)
{
s[i]=pi[i][2]+pi[i][3];
if(s[i]<0) m+=1<<i;
}
if(m==((1<<NP)-1)) return(0);
if(m!=0)
{
int n=0;
for(int i=NP-1,j=0;j<NP;i=j++)
{
const btVector4& a=pi[i];
const btVector4& b=pi[j];
const btScalar t=s[i]/(a[3]+a[2]-b[3]-b[2]);
if((t>0)&&(t<1))
{
po[n][0] = a[0]+(b[0]-a[0])*t;
po[n][1] = a[1]+(b[1]-a[1])*t;
po[n][2] = a[2]+(b[2]-a[2])*t;
po[n][3] = a[3]+(b[3]-a[3])*t;
++n;
}
if(s[j]>0) po[n++]=b;
}
return(n);
}
for(int i=0;i<NP;++i) po[i]=pi[i];
return(NP);
}
template <typename POLICY>
inline bool draw( const btVector4& a,
const btVector4& b,
const btVector4& c,
const btScalar minarea)
{
const btScalar a2=cross(b-a,c-a)[2];
if(a2>0)
{
if(a2<minarea) return(true);
const int x[]={ (int)(a.x()*scales[0]+offsets[0]),
(int)(b.x()*scales[0]+offsets[0]),
(int)(c.x()*scales[0]+offsets[0])};
const int y[]={ (int)(a.y()*scales[1]+offsets[1]),
(int)(b.y()*scales[1]+offsets[1]),
(int)(c.y()*scales[1]+offsets[1])};
const btScalar z[]={ a.z(),b.z(),c.z()};
const int mix=btMax(0,btMin(x[0],btMin(x[1],x[2])));
const int mxx=btMin(sizes[0],1+btMax(x[0],btMax(x[1],x[2])));
const int miy=btMax(0,btMin(y[0],btMin(y[1],y[2])));
const int mxy=btMin(sizes[1],1+btMax(y[0],btMax(y[1],y[2])));
const int width=mxx-mix;
const int height=mxy-miy;
if((width*height)>0)
{
const int dx[]={ y[0]-y[1],
y[1]-y[2],
y[2]-y[0]};
const int dy[]={ x[1]-x[0]-dx[0]*width,
x[2]-x[1]-dx[1]*width,
x[0]-x[2]-dx[2]*width};
const int a=x[2]*y[0]+x[0]*y[1]-x[2]*y[1]-x[0]*y[2]+x[1]*y[2]-x[1]*y[0];
const btScalar ia=1/(btScalar)a;
const btScalar dzx=ia*(y[2]*(z[1]-z[0])+y[1]*(z[0]-z[2])+y[0]*(z[2]-z[1]));
const btScalar dzy=ia*(x[2]*(z[0]-z[1])+x[0]*(z[1]-z[2])+x[1]*(z[2]-z[0]))-(dzx*width);
int c[]={ miy*x[1]+mix*y[0]-x[1]*y[0]-mix*y[1]+x[0]*y[1]-miy*x[0],
miy*x[2]+mix*y[1]-x[2]*y[1]-mix*y[2]+x[1]*y[2]-miy*x[1],
miy*x[0]+mix*y[2]-x[0]*y[2]-mix*y[0]+x[2]*y[0]-miy*x[2]};
btScalar v=ia*((z[2]*c[0])+(z[0]*c[1])+(z[1]*c[2]));
btScalar* scan=&buffer[miy*sizes[1]];
for(int iy=miy;iy<mxy;++iy)
{
for(int ix=mix;ix<mxx;++ix)
{
if((c[0]>=0)&&(c[1]>=0)&&(c[2]>=0))
{
if(POLICY::Process(scan[ix],v)) return(true);
}
c[0]+=dx[0];c[1]+=dx[1];c[2]+=dx[2];v+=dzx;
}
c[0]+=dy[0];c[1]+=dy[1];c[2]+=dy[2];v+=dzy;
scan+=sizes[0];
}
}
}
return(false);
}
template <const int NP,typename POLICY>
inline bool clipDraw( const btVector4* p,
btScalar minarea)
{
btVector4 o[NP*2];
const int n=clip<NP>(p,o);
bool earlyexit=false;
project(o,n);
for(int i=2;i<n;++i)
{
earlyexit|=draw<POLICY>(o[0],o[i-1],o[i],minarea);
}
return(earlyexit);
}
void appendOccluder( const btVector3& a,
const btVector3& b,
const btVector3& c)
{
const btVector4 p[]={transform(a),transform(b),transform(c)};
clipDraw<3,WriteOCL>(p,ocarea);
}
void appendOccluder( const btVector3& a,
const btVector3& b,
const btVector3& c,
const btVector3& d)
{
const btVector4 p[]={transform(a),transform(b),transform(c),transform(d)};
clipDraw<4,WriteOCL>(p,ocarea);
}
void appendOccluder( const btVector3& c,
const btVector3& e)
{
const btVector4 x[]={ transform(btVector3(c[0]-e[0],c[1]-e[1],c[2]-e[2])),
transform(btVector3(c[0]+e[0],c[1]-e[1],c[2]-e[2])),
transform(btVector3(c[0]+e[0],c[1]+e[1],c[2]-e[2])),
transform(btVector3(c[0]-e[0],c[1]+e[1],c[2]-e[2])),
transform(btVector3(c[0]-e[0],c[1]-e[1],c[2]+e[2])),
transform(btVector3(c[0]+e[0],c[1]-e[1],c[2]+e[2])),
transform(btVector3(c[0]+e[0],c[1]+e[1],c[2]+e[2])),
transform(btVector3(c[0]-e[0],c[1]+e[1],c[2]+e[2]))};
static const int d[]={ 1,0,3,2,
4,5,6,7,
4,7,3,0,
6,5,1,2,
7,6,2,3,
5,4,0,1};
for(int i=0;i<(sizeof(d)/sizeof(d[0]));)
{
const btVector4 p[]={ x[d[i++]],
x[d[i++]],
x[d[i++]],
x[d[i++]]};
clipDraw<4,WriteOCL>(p,ocarea);
}
}
inline bool queryOccluder( const btVector3& a,
const btVector3& b,
const btVector3& c)
{
const btVector4 p[]={transform(a),transform(b),transform(c)};
return(clipDraw<3,QueryOCL>(p,qrarea));
}
inline bool queryOccluder( const btVector3& a,
const btVector3& b,
const btVector3& c,
const btVector3& d)
{
const btVector4 p[]={transform(a),transform(b),transform(c),transform(d)};
return(clipDraw<4,QueryOCL>(p,qrarea));
}
inline bool queryOccluder( const btVector3& c,
const btVector3& e)
{
const btVector4 x[]={ transform(btVector3(c[0]-e[0],c[1]-e[1],c[2]-e[2])),
transform(btVector3(c[0]+e[0],c[1]-e[1],c[2]-e[2])),
transform(btVector3(c[0]+e[0],c[1]+e[1],c[2]-e[2])),
transform(btVector3(c[0]-e[0],c[1]+e[1],c[2]-e[2])),
transform(btVector3(c[0]-e[0],c[1]-e[1],c[2]+e[2])),
transform(btVector3(c[0]+e[0],c[1]-e[1],c[2]+e[2])),
transform(btVector3(c[0]+e[0],c[1]+e[1],c[2]+e[2])),
transform(btVector3(c[0]-e[0],c[1]+e[1],c[2]+e[2]))};
for(int i=0;i<8;++i)
{
if((x[i][2]+x[i][3])<=0) return(true);
}
static const int d[]={ 1,0,3,2,
4,5,6,7,
4,7,3,0,
6,5,1,2,
7,6,2,3,
5,4,0,1};
for(int i=0;i<(sizeof(d)/sizeof(d[0]));)
{
const btVector4 p[]={ x[d[i++]],
x[d[i++]],
x[d[i++]],
x[d[i++]]};
if(clipDraw<4,QueryOCL>(p,qrarea)) return(true);
}
return(false);
}
};
OcclusionBuffer ocb;
BulletSAPCompleteBoxPruningTest::BulletSAPCompleteBoxPruningTest(int numBoxes,int method) : BulletSAPCompleteBoxPruningTest::BulletSAPCompleteBoxPruningTest(int numBoxes,int method) :
mBar (null), mBar (null),
mNbBoxes (numBoxes), mNbBoxes (numBoxes),
@@ -39,7 +348,7 @@ BulletSAPCompleteBoxPruningTest::BulletSAPCompleteBoxPruningTest(int numBoxes,in
btVector3 aabbMax(200,200,200); btVector3 aabbMax(200,200,200);
int maxNumBoxes = numBoxes; int maxNumBoxes = numBoxes;
m_isdbvt=false;
switch (method) switch (method)
{ {
case 1: case 1:
@@ -102,6 +411,7 @@ BulletSAPCompleteBoxPruningTest::BulletSAPCompleteBoxPruningTest(int numBoxes,in
break; break;
case 7: case 7:
m_broadphase = new btDbvtBroadphase(); m_broadphase = new btDbvtBroadphase();
m_isdbvt = true;
methodname = "btDbvtBroadphase"; methodname = "btDbvtBroadphase";
break; break;
default: default:
@@ -122,11 +432,13 @@ BulletSAPCompleteBoxPruningTest::~BulletSAPCompleteBoxPruningTest()
void BulletSAPCompleteBoxPruningTest::Init() void BulletSAPCompleteBoxPruningTest::Init()
{ {
btClock clock;
m_firstTime = true; m_firstTime = true;
SRand(0); SRand(0);
mBoxes = new AABB[mNbBoxes]; mBoxes = new AABB[mNbBoxes];
mFlags = new bool[mNbBoxes];
mBoxPtrs = new const AABB*[mNbBoxes]; mBoxPtrs = new const AABB*[mNbBoxes];
mBoxTime = new float[mNbBoxes]; mBoxTime = new float[mNbBoxes];
for(udword i=0;i<mNbBoxes;i++) for(udword i=0;i<mNbBoxes;i++)
@@ -146,11 +458,12 @@ void BulletSAPCompleteBoxPruningTest::Init()
btVector3 aabbMax(Center.x+Extents.x,Center.y+Extents.y,Center.z+Extents.z); btVector3 aabbMax(Center.x+Extents.x,Center.y+Extents.y,Center.z+Extents.z);
int shapeType =0; int shapeType =0;
void* userPtr = 0; void* userPtr = 0;
btBroadphaseProxy* proxy = m_broadphase->createProxy(aabbMin,aabbMax,shapeType,(void*)i,1,1,0,0);//m_dispatcher); btBroadphaseProxy* proxy = m_broadphase->createProxy(aabbMin,aabbMax,shapeType,&mBoxes[i],1,1,0,0);//m_dispatcher);
m_proxies.push_back( proxy ); m_proxies.push_back( proxy );
mBoxTime[i] = 2000.0f*UnitRandomFloat(); mBoxTime[i] = 2000.0f*UnitRandomFloat();
} }
printf("Initialization of %s with %u boxes: %ums\r\n",methodname,mNbBoxes,clock.getTimeMilliseconds());
} }
void BulletSAPCompleteBoxPruningTest::Release() void BulletSAPCompleteBoxPruningTest::Release()
@@ -166,6 +479,23 @@ void BulletSAPCompleteBoxPruningTest::Select()
mBar = TwNewBar("OPC_CompleteBoxPruning"); mBar = TwNewBar("OPC_CompleteBoxPruning");
TwAddVarRW(mBar, "Speed", TW_TYPE_FLOAT, &mSpeed, " min=0.0 max=0.01 step=0.00001"); TwAddVarRW(mBar, "Speed", TW_TYPE_FLOAT, &mSpeed, " min=0.0 max=0.01 step=0.00001");
TwAddVarRW(mBar, "Amplitude", TW_TYPE_FLOAT, &mAmplitude, " min=10.0 max=200.0 step=0.1"); TwAddVarRW(mBar, "Amplitude", TW_TYPE_FLOAT, &mAmplitude, " min=10.0 max=200.0 step=0.1");
if(m_isdbvt)
{
TwAddVarRW(mBar, "Enable culling",TW_TYPE_BOOLCPP,&enableCulling,"");
TwAddVarRW(mBar, "Enable occlusion",TW_TYPE_BOOLCPP,&enableOcclusion,"");
TwAddVarRW(mBar, "Show culling",TW_TYPE_BOOLCPP,&showCulling,"");
TwAddVarRW(mBar, "Show occlusion",TW_TYPE_BOOLCPP,&showOcclusion,"");
TwAddVarRW(mBar, "Cull far plane",TW_TYPE_BOOLCPP,&cullFarPlane,"");
TwAddVarRW(mBar, "OC Min area",TW_TYPE_FLOAT,&ocb.ocarea,"min=0.0 max=1.0 step=0.001");
TwAddVarRW(mBar, "QR Min area",TW_TYPE_FLOAT,&ocb.qrarea,"min=0.0 max=1.0 step=0.001");
TwAddVarRW(mBar, "Dyn lkhd",TW_TYPE_INT32,&((btDbvtBroadphase*)m_broadphase)->m_sets[0].m_lkhd,"min=-1 max=32");
TwAddVarRW(mBar, "Fix lkhd",TW_TYPE_INT32,&((btDbvtBroadphase*)m_broadphase)->m_sets[1].m_lkhd,"min=-1 max=32");
TwAddVarRW(mBar, "Dyn opt/f(%)",TW_TYPE_INT32,&((btDbvtBroadphase*)m_broadphase)->m_dupdates,"min=0 max=100");
TwAddVarRW(mBar, "Fix opt/f(%)",TW_TYPE_INT32,&((btDbvtBroadphase*)m_broadphase)->m_fupdates,"min=0 max=100");
TwAddVarRO(mBar, "Dyn leafs",TW_TYPE_INT32,&((btDbvtBroadphase*)m_broadphase)->m_sets[0].m_leafs,"");
TwAddVarRO(mBar, "Fix leafs",TW_TYPE_INT32,&((btDbvtBroadphase*)m_broadphase)->m_sets[1].m_leafs,"");
TwAddVarRO(mBar, "Visible",TW_TYPE_INT32,&visiblecount,"");
}
} }
printf("SubMethod: %s\r\n",methodname); printf("SubMethod: %s\r\n",methodname);
} }
@@ -239,8 +569,7 @@ void BulletSAPCompleteBoxPruningTest::PerformTest()
// printf("%d pairs colliding\r ", mPairs.GetNbPairs()); // printf("%d pairs colliding\r ", mPairs.GetNbPairs());
bool* Flags = (bool*)_alloca(sizeof(bool)*mNbBoxes); ZeroMemory(mFlags,sizeof(bool)*mNbBoxes);
ZeroMemory(Flags, sizeof(bool)*mNbBoxes);
btOverlappingPairCache* pairCache = m_broadphase->getOverlappingPairCache(); btOverlappingPairCache* pairCache = m_broadphase->getOverlappingPairCache();
const btBroadphasePair* pairPtr = pairCache->getOverlappingPairArrayPtr(); const btBroadphasePair* pairPtr = pairCache->getOverlappingPairArrayPtr();
@@ -249,8 +578,11 @@ void BulletSAPCompleteBoxPruningTest::PerformTest()
{ {
// Flags[pairPtr[i].m_pProxy0->getUid()-1] = true; // Flags[pairPtr[i].m_pProxy0->getUid()-1] = true;
// Flags[pairPtr[i].m_pProxy1->getUid()-1] = true; // Flags[pairPtr[i].m_pProxy1->getUid()-1] = true;
Flags[int(pairPtr[i].m_pProxy0->m_clientObject)] = true; int j;
Flags[int(pairPtr[i].m_pProxy1->m_clientObject)] = true; j=((AABB*)pairPtr[i].m_pProxy0->m_clientObject)-mBoxes;
mFlags[j] = true;
j=((AABB*)pairPtr[i].m_pProxy1->m_clientObject)-mBoxes;
mFlags[j] = true;
} }
@@ -304,11 +636,13 @@ void BulletSAPCompleteBoxPruningTest::PerformTest()
glEnable(GL_DEPTH_TEST); glEnable(GL_DEPTH_TEST);
//glDisable(GL_DEPTH_TEST);
glDisable(GL_BLEND); glDisable(GL_BLEND);
// Render boxes // Render boxes
OBB CurrentBox; Render();
/*OBB CurrentBox;
CurrentBox.mRot.Identity(); CurrentBox.mRot.Identity();
for(udword i=0;i<mNbBoxes;i++) for(udword i=0;i<mNbBoxes;i++)
{ {
@@ -317,7 +651,7 @@ void BulletSAPCompleteBoxPruningTest::PerformTest()
mBoxes[i].GetCenter(CurrentBox.mCenter); mBoxes[i].GetCenter(CurrentBox.mCenter);
mBoxes[i].GetExtents(CurrentBox.mExtents); mBoxes[i].GetExtents(CurrentBox.mExtents);
DrawOBB(CurrentBox); DrawOBB(CurrentBox);
} }*/
char Buffer[4096]; char Buffer[4096];
sprintf(Buffer, "CompleteBoxPruning: %5.1f us (%d cycles) : %d pairs\n", mProfiler.mMsTime, mProfiler.mCycles, sprintf(Buffer, "CompleteBoxPruning: %5.1f us (%d cycles) : %d pairs\n", mProfiler.mMsTime, mProfiler.mCycles,
@@ -328,6 +662,286 @@ void BulletSAPCompleteBoxPruningTest::PerformTest()
GLFontRenderer::print(10.0f, 10.0f, 0.02f, Buffer); GLFontRenderer::print(10.0f, 10.0f, 0.02f, Buffer);
} }
//
static void DrawVolume(const btDbvt::Volume& volume,const btVector3& color)
{
const btVector3 mins=volume.Mins();
const btVector3 maxs=volume.Maxs();
glColor3f(color.x(),color.y(),color.z());
glVertex3f(mins.x(),mins.y(),mins.z());
glVertex3f(maxs.x(),mins.y(),mins.z());
glVertex3f(maxs.x(),mins.y(),mins.z());
glVertex3f(maxs.x(),maxs.y(),mins.z());
glVertex3f(maxs.x(),maxs.y(),mins.z());
glVertex3f(mins.x(),maxs.y(),mins.z());
glVertex3f(mins.x(),maxs.y(),mins.z());
glVertex3f(mins.x(),mins.y(),mins.z());
glVertex3f(mins.x(),mins.y(),maxs.z());
glVertex3f(maxs.x(),mins.y(),maxs.z());
glVertex3f(maxs.x(),mins.y(),maxs.z());
glVertex3f(maxs.x(),maxs.y(),maxs.z());
glVertex3f(maxs.x(),maxs.y(),maxs.z());
glVertex3f(mins.x(),maxs.y(),maxs.z());
glVertex3f(mins.x(),maxs.y(),maxs.z());
glVertex3f(mins.x(),mins.y(),maxs.z());
glVertex3f(mins.x(),mins.y(),mins.z());
glVertex3f(mins.x(),mins.y(),maxs.z());
glVertex3f(maxs.x(),mins.y(),mins.z());
glVertex3f(maxs.x(),mins.y(),maxs.z());
glVertex3f(maxs.x(),maxs.y(),mins.z());
glVertex3f(maxs.x(),maxs.y(),maxs.z());
glVertex3f(mins.x(),maxs.y(),mins.z());
glVertex3f(mins.x(),maxs.y(),maxs.z());
}
//
void BulletSAPCompleteBoxPruningTest::RenderAll()
{
OBB CurrentBox;
CurrentBox.mRot.Identity();
for(udword i=0;i<mNbBoxes;i++)
{
if(mFlags[i]) glColor3f(1.0f, 0.0f, 0.0f);
else glColor3f(0.0f, 1.0f, 0.0f);
mBoxes[i].GetCenter(CurrentBox.mCenter);
mBoxes[i].GetExtents(CurrentBox.mExtents);
DrawOBB(CurrentBox);
}
}
//
void BulletSAPCompleteBoxPruningTest::Render()
{
visiblecount=mNbBoxes;
if((!m_isdbvt)||(!enableCulling))
{
RenderAll();
}
else
{
btDbvtBroadphase* pbp=(btDbvtBroadphase*)m_broadphase;
const int margin=0;
const int lc=margin;
const int rc=glutGet(GLUT_WINDOW_WIDTH)-(1+margin);
const int tc=margin;
const int bc=glutGet(GLUT_WINDOW_HEIGHT)-(1+margin);
const btVector3 c00(ComputeWorldRay(lc,tc).x,
ComputeWorldRay(lc,tc).y,
ComputeWorldRay(lc,tc).z);
const btVector3 c10(ComputeWorldRay(rc,tc).x,
ComputeWorldRay(rc,tc).y,
ComputeWorldRay(rc,tc).z);
const btVector3 c01(ComputeWorldRay(lc,bc).x,
ComputeWorldRay(lc,bc).y,
ComputeWorldRay(lc,bc).z);
const btVector3 c11(ComputeWorldRay(rc,bc).x,
ComputeWorldRay(rc,bc).y,
ComputeWorldRay(rc,bc).z);
const btVector3 eye(GetCameraPos().x,GetCameraPos().y,GetCameraPos().z);
const btVector3 dir(GetCameraDir().x,GetCameraDir().y,GetCameraDir().z);
const btVector3 x00=eye+c00*100;
const btVector3 x10=eye+c10*100;
const btVector3 x01=eye+c01*100;
const btVector3 x11=eye+c11*100;
ocb.initialize();
ocb.eye=eye;
btVector3 planes_n[5];
btScalar planes_o[5];
static const btScalar farplane=200;
static const int nplanes=sizeof(planes_n)/sizeof(planes_n[0]);
const int acplanes=cullFarPlane?5:4;
planes_n[0] = cross(c01,c00).normalized();
planes_n[1] = cross(c10,c11).normalized();
planes_n[2] = cross(c00,c10).normalized();
planes_n[3] = cross(c11,c01).normalized();
planes_n[4] = -dir;
planes_o[4] = -dot(eye+dir*farplane,planes_n[4]);
for(int i=0;i<4;++i) planes_o[i]=-dot(eye,planes_n[i]);
struct SceneRenderer : btDbvt::ICollide
{
int drawn;
BulletSAPCompleteBoxPruningTest* self;
OBB box;
OcclusionBuffer* ocb;
SceneRenderer()
{
drawn=0;
box.mRot.Identity();
}
bool Descent(const btDbvt::Node* node)
{
return(ocb->queryOccluder(node->volume.Center(),node->volume.Extent()));
}
void Process(const btDbvt::Node* leaf)
{
btBroadphaseProxy* proxy=(btBroadphaseProxy*)leaf->data;
int i=((AABB*)proxy->m_clientObject)-self->mBoxes;
if(self->mFlags[i]) glColor3f(1.0f, 0.0f, 0.0f);
else glColor3f(0.0f, 1.0f, 0.0f);
self->mBoxes[i].GetCenter(box.mCenter);
self->mBoxes[i].GetExtents(box.mExtents);
DrawOBB(box);drawn++;
if(ocb)
{
ocb->appendOccluder(btVector3(box.mCenter.x,box.mCenter.y,box.mCenter.z),
btVector3(box.mExtents.x,box.mExtents.y,box.mExtents.z));
}
}
} srenderer;
srenderer.self=this;
srenderer.ocb=0;
if(enableOcclusion)
{
srenderer.ocb=&ocb;
btDbvt::collideOCL(pbp->m_sets[1].m_root,planes_n,planes_o,dir,acplanes,srenderer);
btDbvt::collideOCL(pbp->m_sets[0].m_root,planes_n,planes_o,dir,acplanes,srenderer);
}
else
{
btDbvt::collideKDOP(pbp->m_sets[1].m_root,planes_n,planes_o,acplanes,srenderer);
btDbvt::collideKDOP(pbp->m_sets[0].m_root,planes_n,planes_o,acplanes,srenderer);
}
visiblecount=srenderer.drawn;
if(showOcclusion&&enableOcclusion)
{
const btScalar ratio=((float)glutGet(GLUT_WINDOW_HEIGHT))/((float)glutGet(GLUT_WINDOW_WIDTH));
glMatrixMode(GL_PROJECTION);
glLoadIdentity();
glOrtho(-1,1,-1,1,-1,1);
glMatrixMode(GL_MODELVIEW);
glLoadIdentity();
const float mm[]={ 1,0,0,0,
0,1,0,0,
0,0,0,1,
0,0,0,1};
glMultMatrixf(mm);
glDisable(GL_DEPTH_TEST);
glDisable(GL_LIGHTING);
const float size=0.6f;
const float orgx=0.3f;
const float orgy=0.25f;
const float left=orgx;
const float right=orgx+size;
const float top=orgy+size;
const float bottom=orgy;
ocb.drawBuffer(left,bottom,right,top);
}
if(showCulling)
{
const btScalar ratio=((float)glutGet(GLUT_WINDOW_HEIGHT))/((float)glutGet(GLUT_WINDOW_WIDTH));
static const float scale=0.004;
glMatrixMode(GL_PROJECTION);
glLoadIdentity();
glOrtho(-1,1,-1*ratio,1*ratio,-1,1);
glMatrixMode(GL_MODELVIEW);
glLoadIdentity();
const float mm[]={ 1,0,0,0,
0,0,1,0,
0,1,0,0,
0,0,0,1};
glMultMatrixf(mm);
glScalef(scale,scale,scale);
glDisable(GL_DEPTH_TEST);
glDisable(GL_LIGHTING);
glBegin(GL_LINES);
glColor4f(1,1,1,1);
struct DebugRenderer : btDbvt::ICollide
{
OcclusionBuffer* ocb;
int sid;
bool AllLeafs(const btDbvt::Node* node)
{
Process(node);
return(false);
}
bool Descent(const btDbvt::Node* node)
{
return(ocb->queryOccluder(node->volume.Center(),node->volume.Extent()));
}
void Process(const btDbvt::Node* node)
{
if(ocb)
{
ocb->appendOccluder(node->volume.Center(),node->volume.Extent());
}
if(sid>=0)
{
const float f=sid/1023.;
DrawVolume(node->volume,btVector3(1,f,f));
sid=(sid+1)%1024;
}
else
{
if(node->isinternal())
DrawVolume(node->volume,btVector3(0,1,0));
else
DrawVolume(node->volume,btVector3(1,0,1));
}
}
} drenderer;
if(enableOcclusion)
{
drenderer.ocb=&ocb;
drenderer.sid=0;
ocb.clear();
btDbvt::collideOCL(pbp->m_sets[1].m_root,planes_n,planes_o,dir,acplanes,drenderer);
btDbvt::collideOCL(pbp->m_sets[0].m_root,planes_n,planes_o,dir,acplanes,drenderer);
}
else
{
drenderer.ocb=0;
drenderer.sid=-1;
btDbvt::collideKDOP(pbp->m_sets[1].m_root,planes_n,planes_o,acplanes,drenderer);
btDbvt::collideKDOP(pbp->m_sets[0].m_root,planes_n,planes_o,acplanes,drenderer);
}
glEnd();
glBegin(GL_LINES);
glColor4f(1,1,1,1);
glVertex3f(eye.x(),eye.y(),eye.z());
glVertex3f(x00.x(),x00.y(),x00.z());
glVertex3f(eye.x(),eye.y(),eye.z());
glVertex3f(x10.x(),x10.y(),x10.z());
glVertex3f(eye.x(),eye.y(),eye.z());
glVertex3f(x01.x(),x01.y(),x01.z());
glVertex3f(eye.x(),eye.y(),eye.z());
glVertex3f(x11.x(),x11.y(),x11.z());
glVertex3f(x00.x(),x00.y(),x00.z());
glVertex3f(x10.x(),x10.y(),x10.z());
glVertex3f(x10.x(),x10.y(),x10.z());
glVertex3f(x11.x(),x11.y(),x11.z());
glVertex3f(x11.x(),x11.y(),x11.z());
glVertex3f(x01.x(),x01.y(),x01.z());
glVertex3f(x01.x(),x01.y(),x01.z());
glVertex3f(x00.x(),x00.y(),x00.z());
glEnd();
}
}
}
void BulletSAPCompleteBoxPruningTest::KeyboardCallback(unsigned char key, int x, int y) void BulletSAPCompleteBoxPruningTest::KeyboardCallback(unsigned char key, int x, int y)
{ {
} }

17
Extras/CDTestFramework/CDTestFramework.cpp
View File

@@ -21,17 +21,12 @@ subject to the following restrictions:
#include "CapsuleMeshQuery.h" #include "CapsuleMeshQuery.h"
#include "CompleteBoxPruning.h" #include "CompleteBoxPruning.h"
#include "BulletSAPCompleteBoxPruningTest.h" #include "BulletSAPCompleteBoxPruningTest.h"
#include "DbvtTest.h"
#include "BipartiteBoxPruning.h" #include "BipartiteBoxPruning.h"
#include "RenderingHelpers.h" #include "RenderingHelpers.h"
#include "Terrain.h" #include "Terrain.h"
#include "Camera.h" #include "Camera.h"
#include "GLFontRenderer.h" #include "GLFontRenderer.h"
///#define NUM_SAP_BOXES 16384
//#define NUM_SAP_BOXES 8192
//#define NUM_SAP_BOXES 4096
#define NUM_SAP_BOXES 8192 #define NUM_SAP_BOXES 8192
int percentUpdate = 10; int percentUpdate = 10;
@@ -64,9 +59,9 @@ enum TestIndex
// TEST_OBB_MESH_QUERY, // TEST_OBB_MESH_QUERY,
// TEST_CAPSULE_MESH_QUERY, // TEST_CAPSULE_MESH_QUERY,
// TEST_COMPLETE_BOX_PRUNING=0, // TEST_COMPLETE_BOX_PRUNING=0,
TEST_COMPLETE_BOX_PRUNING_8192, // TEST_COMPLETE_BOX_PRUNING_8192,
// TEST_BULLET_SAP_1024, // TEST_BULLET_SAP_1024,
TEST_BULLET_SAP_8192, // TEST_BULLET_SAP_8192,
// TEST_BULLET_SAP_SORTEDPAIRS_8192, // TEST_BULLET_SAP_SORTEDPAIRS_8192,
TEST_BULLET_MULTISAP_8192, TEST_BULLET_MULTISAP_8192,
// TEST_BIPARTITE_BOX_PRUNING, // TEST_BIPARTITE_BOX_PRUNING,
@@ -291,9 +286,9 @@ int main(int argc, char** argv)
// {TEST_OBB_MESH_QUERY, "OBB-mesh query"}, // {TEST_OBB_MESH_QUERY, "OBB-mesh query"},
// {TEST_CAPSULE_MESH_QUERY, "Capsule-mesh query"}, // {TEST_CAPSULE_MESH_QUERY, "Capsule-mesh query"},
// {TEST_COMPLETE_BOX_PRUNING, "OPCODE SAP 1024"}, // {TEST_COMPLETE_BOX_PRUNING, "OPCODE SAP 1024"},
{TEST_COMPLETE_BOX_PRUNING_8192, "OPCODE SAP 8192"}, // {TEST_COMPLETE_BOX_PRUNING_8192, "OPCODE SAP 8192"},
// {TEST_BULLET_SAP_1024, "Bullet SAP HASHPAIR 1024"}, // {TEST_BULLET_SAP_1024, "Bullet SAP HASHPAIR 1024"},
{TEST_BULLET_SAP_8192, "Bullet SAP HASHPAIR 8192"}, // {TEST_BULLET_SAP_8192, "Bullet SAP HASHPAIR 8192"},
// {TEST_BULLET_SAP_SORTEDPAIRS_8192, "Bullet SAP SORTEDPAIR 8192"}, // {TEST_BULLET_SAP_SORTEDPAIRS_8192, "Bullet SAP SORTEDPAIR 8192"},
{TEST_BULLET_MULTISAP_8192, "Bullet MultiSAP 8192"}, {TEST_BULLET_MULTISAP_8192, "Bullet MultiSAP 8192"},
// {TEST_BIPARTITE_BOX_PRUNING, "Bipartite box pruning"}, // {TEST_BIPARTITE_BOX_PRUNING, "Bipartite box pruning"},
@@ -310,10 +305,10 @@ int main(int argc, char** argv)
// gCollisionTests[TEST_OBB_MESH_QUERY] = new OBBMeshQuery; // gCollisionTests[TEST_OBB_MESH_QUERY] = new OBBMeshQuery;
// gCollisionTests[TEST_CAPSULE_MESH_QUERY] = new CapsuleMeshQuery; // gCollisionTests[TEST_CAPSULE_MESH_QUERY] = new CapsuleMeshQuery;
// gCollisionTests[TEST_COMPLETE_BOX_PRUNING] = new CompleteBoxPruningTest(NUM_SAP_BOXES); // gCollisionTests[TEST_COMPLETE_BOX_PRUNING] = new CompleteBoxPruningTest(NUM_SAP_BOXES);
gCollisionTests[TEST_COMPLETE_BOX_PRUNING_8192] = new CompleteBoxPruningTest(NUM_SAP_BOXES); // gCollisionTests[TEST_COMPLETE_BOX_PRUNING_8192] = new CompleteBoxPruningTest(NUM_SAP_BOXES);
// gCollisionTests[TEST_COMPLETE_BOX_PRUNING_8192] = new CompleteBoxPruningTest(NUM_SAP_BOXES); // gCollisionTests[TEST_COMPLETE_BOX_PRUNING_8192] = new CompleteBoxPruningTest(NUM_SAP_BOXES);
// gCollisionTests[TEST_BULLET_SAP_1024] = new BulletSAPCompleteBoxPruningTest(NUM_SAP_BOXES,1); // gCollisionTests[TEST_BULLET_SAP_1024] = new BulletSAPCompleteBoxPruningTest(NUM_SAP_BOXES,1);
gCollisionTests[TEST_BULLET_SAP_8192] = new BulletSAPCompleteBoxPruningTest(NUM_SAP_BOXES,1); // gCollisionTests[TEST_BULLET_SAP_8192] = new BulletSAPCompleteBoxPruningTest(NUM_SAP_BOXES,1);
// gCollisionTests[TEST_BULLET_SAP_SORTEDPAIRS_8192] = new BulletSAPCompleteBoxPruningTest(NUM_SAP_BOXES,3); // gCollisionTests[TEST_BULLET_SAP_SORTEDPAIRS_8192] = new BulletSAPCompleteBoxPruningTest(NUM_SAP_BOXES,3);
gCollisionTests[TEST_BULLET_MULTISAP_8192] = new BulletSAPCompleteBoxPruningTest(NUM_SAP_BOXES,6); gCollisionTests[TEST_BULLET_MULTISAP_8192] = new BulletSAPCompleteBoxPruningTest(NUM_SAP_BOXES,6);
// gCollisionTests[TEST_BIPARTITE_BOX_PRUNING] = new BipartiteBoxPruningTest; // gCollisionTests[TEST_BIPARTITE_BOX_PRUNING] = new BipartiteBoxPruningTest;

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

@@ -18,27 +18,35 @@ subject to the following restrictions:
// //
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) 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, static inline btDbvt::Volume merge( const btDbvt::Volume& a,
const btDbvt::Volume& b) const btDbvt::Volume& b)
{ {
btDbvt::Volume res; btDbvt::Volume res;
Merge(a,b,res); Merge(a,b,res);
return(res); return(res);
} }
// volume+edge lengths // volume+edge lengths
static inline btScalar size(const btDbvt::Volume& a) static inline btScalar size(const btDbvt::Volume& a)
{ {
const btVector3 edges=a.Lengths(); const btVector3 edges=a.Lengths();
return( edges.x()*edges.y()*edges.z()+ return( edges.x()*edges.y()*edges.z()+
edges.x()+edges.y()+edges.z()); edges.x()+edges.y()+edges.z());
} }
@@ -46,22 +54,21 @@ static inline btScalar size(const btDbvt::Volume& a)
static inline void deletenode( btDbvt* pdbvt, static inline void deletenode( btDbvt* pdbvt,
btDbvt::Node* node) btDbvt::Node* node)
{ {
delete pdbvt->m_free; btAlignedFree(pdbvt->m_free);
pdbvt->m_free=node; pdbvt->m_free=node;
} }
// //
static inline void recursedeletenode( btDbvt* pdbvt, 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[0]);
recursedeletenode(pdbvt,node->childs[1]); recursedeletenode(pdbvt,node->childs[1]);
} }
if(node==pdbvt->m_root) if(node==pdbvt->m_root) pdbvt->m_root=0;
pdbvt->m_root=0; deletenode(pdbvt,node);
deletenode(pdbvt,node);
} }
// //
@@ -70,16 +77,16 @@ static inline btDbvt::Node* createnode( btDbvt* pdbvt,
const btDbvt::Volume& volume, const btDbvt::Volume& volume,
void* data) void* data)
{ {
btDbvt::Node* node; btDbvt::Node* node;
if(pdbvt->m_free) if(pdbvt->m_free)
{ node=pdbvt->m_free;pdbvt->m_free=0; } { node=pdbvt->m_free;pdbvt->m_free=0; }
else else
{ node=new btDbvt::Node(); } { node=new(btAlignedAlloc(sizeof(btDbvt::Node),16)) btDbvt::Node(); }
node->parent = parent; node->parent = parent;
node->volume = volume; node->volume = volume;
node->data = data; node->data = data;
node->childs[1] = 0; node->childs[1] = 0;
return(node); return(node);
} }
// //
@@ -87,7 +94,7 @@ static inline void insertleaf( btDbvt* pdbvt,
btDbvt::Node* root, btDbvt::Node* root,
btDbvt::Node* leaf) btDbvt::Node* leaf)
{ {
if(!pdbvt->m_root) if(!pdbvt->m_root)
{ {
pdbvt->m_root = leaf; pdbvt->m_root = leaf;
leaf->parent = 0; leaf->parent = 0;
@@ -112,10 +119,10 @@ static inline void insertleaf( btDbvt* pdbvt,
node->childs[0] = root;root->parent=node; node->childs[0] = root;root->parent=node;
node->childs[1] = leaf;leaf->parent=node; node->childs[1] = leaf;leaf->parent=node;
do { do {
if(prev->volume.Contain(node->volume)) if(!prev->volume.Contain(node->volume))
break;
else
Merge(prev->childs[0]->volume,prev->childs[1]->volume,prev->volume); Merge(prev->childs[0]->volume,prev->childs[1]->volume,prev->volume);
else
break;
node=prev; node=prev;
} while(0!=(prev=node->parent)); } while(0!=(prev=node->parent));
} }
@@ -132,7 +139,7 @@ static inline void insertleaf( btDbvt* pdbvt,
static inline btDbvt::Node* removeleaf( btDbvt* pdbvt, 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; pdbvt->m_root=0;
return(0); return(0);
@@ -175,7 +182,7 @@ static void fetchleafs( btDbvt* pdbvt,
tNodeArray& leafs, tNodeArray& leafs,
int depth=-1) int depth=-1)
{ {
if(root->isinternal()&&depth) if(root->isinternal()&&depth)
{ {
fetchleafs(pdbvt,root->childs[0],leafs,depth-1); fetchleafs(pdbvt,root->childs[0],leafs,depth-1);
fetchleafs(pdbvt,root->childs[1],leafs,depth-1); fetchleafs(pdbvt,root->childs[1],leafs,depth-1);
@@ -194,9 +201,9 @@ static void split( const tNodeArray& leafs,
const btVector3& org, const btVector3& org,
const btVector3& axis) const btVector3& axis)
{ {
left.resize(0); left.resize(0);
right.resize(0); right.resize(0);
for(int i=0,ni=leafs.size();i<ni;++i) for(int i=0,ni=leafs.size();i<ni;++i)
{ {
if(dot(axis,leafs[i]->volume.Center()-org)<0) if(dot(axis,leafs[i]->volume.Center()-org)<0)
left.push_back(leafs[i]); left.push_back(leafs[i]);
@@ -208,19 +215,19 @@ static void split( const tNodeArray& leafs,
// //
static btDbvt::Volume bounds( const tNodeArray& leafs) static btDbvt::Volume bounds( const tNodeArray& leafs)
{ {
btDbvt::Volume volume=leafs[0]->volume; btDbvt::Volume volume=leafs[0]->volume;
for(int i=1,ni=leafs.size();i<ni;++i) 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, static void bottomup( btDbvt* pdbvt,
tNodeArray& leafs) tNodeArray& leafs)
{ {
while(leafs.size()>1) while(leafs.size()>1)
{ {
btScalar minsize=SIMD_INFINITY; btScalar minsize=SIMD_INFINITY;
int minidx[2]={-1,-1}; int minidx[2]={-1,-1};
@@ -254,10 +261,10 @@ static btDbvt::Node* topdown(btDbvt* pdbvt,
tNodeArray& leafs, tNodeArray& leafs,
int bu_treshold) int bu_treshold)
{ {
static const btVector3 axis[]={btVector3(1,0,0), static const btVector3 axis[]={btVector3(1,0,0),
btVector3(0,1,0), btVector3(0,1,0),
btVector3(0,0,1)}; btVector3(0,0,1)};
if(leafs.size()>1) if(leafs.size()>1)
{ {
if(leafs.size()>bu_treshold) if(leafs.size()>bu_treshold)
{ {
@@ -279,7 +286,7 @@ static btDbvt::Node* topdown(btDbvt* pdbvt,
{ {
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]))); const int midp=abs(splitcount[i][0]-splitcount[i][1]);
if(midp<bestmidp) if(midp<bestmidp)
{ {
bestaxis=i; bestaxis=i;
@@ -315,15 +322,15 @@ static btDbvt::Node* topdown(btDbvt* pdbvt,
return(leafs[0]); return(leafs[0]);
} }
} }
return(leafs[0]); return(leafs[0]);
} }
// //
static inline btDbvt::Node* refit( btDbvt* pdbvt, static inline btDbvt::Node* refit( btDbvt* pdbvt,
btDbvt::Node* node) btDbvt::Node* node)
{ {
btDbvt::Node* parent=node->parent; btDbvt::Node* parent=node->parent;
if(parent) if(parent)
{ {
const int idx=indexof(node); const int idx=indexof(node);
tNodeArray leafs; tNodeArray leafs;
@@ -337,7 +344,7 @@ static inline btDbvt::Node* refit( btDbvt* pdbvt,
parent->childs[idx]=node; parent->childs[idx]=node;
} }
} }
return(node); return(node);
} }
// //
@@ -345,32 +352,33 @@ static inline btDbvt::Node* refit( btDbvt* pdbvt,
// //
// //
btDbvt::btDbvt() btDbvt::btDbvt()
{ {
m_root = 0; m_root = 0;
m_free = 0; m_free = 0;
m_lkhd = 2; m_lkhd = -1;
m_leafs = 0; m_leafs = 0;
m_opath = 0;
} }
// //
btDbvt::~btDbvt() btDbvt::~btDbvt()
{ {
clear(); clear();
} }
// //
void btDbvt::clear() void btDbvt::clear()
{ {
if(m_root) recursedeletenode(this,m_root); if(m_root) recursedeletenode(this,m_root);
delete m_free; btAlignedFree(m_free);
m_free=0; m_free=0;
} }
// //
void btDbvt::optimizeBottomUp() void btDbvt::optimizeBottomUp()
{ {
if(m_root) if(m_root)
{ {
tNodeArray leafs; tNodeArray leafs;
leafs.reserve(m_leafs); leafs.reserve(m_leafs);
@@ -383,7 +391,7 @@ void btDbvt::optimizeBottomUp()
// //
void btDbvt::optimizeTopDown(int bu_treshold) void btDbvt::optimizeTopDown(int bu_treshold)
{ {
if(m_root) if(m_root)
{ {
tNodeArray leafs; tNodeArray leafs;
leafs.reserve(m_leafs); leafs.reserve(m_leafs);
@@ -392,103 +400,126 @@ void btDbvt::optimizeTopDown(int 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) btDbvt::Node* btDbvt::insert(const Volume& volume,void* data)
{ {
Node* leaf=createnode(this,0,volume,data); Node* leaf=createnode(this,0,volume,data);
insertleaf(this,m_root,leaf); insertleaf(this,m_root,leaf);
++m_leafs; ++m_leafs;
return(leaf); return(leaf);
} }
// //
void btDbvt::update(Node* leaf,int lookahead) void btDbvt::update(Node* leaf,int lookahead)
{ {
Node* root=removeleaf(this,leaf); Node* root=removeleaf(this,leaf);
if(root) if(root)
{
if(lookahead>=0)
{ {
for(int i=0;(i<lookahead)&&root->parent;++i) for(int i=0;(i<lookahead)&&root->parent;++i)
{ {
root=root->parent; root=root->parent;
} }
} else root=m_root;
} }
insertleaf(this,root,leaf); insertleaf(this,root,leaf);
} }
// //
void btDbvt::update(Node* leaf,const Volume& volume) void btDbvt::update(Node* leaf,const Volume& volume)
{ {
Node* root=removeleaf(this,leaf); Node* root=removeleaf(this,leaf);
if(root) if(root)
{
if(m_lkhd>=0)
{ {
for(int i=0;(i<m_lkhd)&&root->parent;++i) for(int i=0;(i<m_lkhd)&&root->parent;++i)
{ {
root=root->parent; root=root->parent;
} }
} else root=m_root;
} }
leaf->volume=volume; leaf->volume=volume;
insertleaf(this,root,leaf); insertleaf(this,root,leaf);
} }
// //
bool btDbvt::update(Node* leaf,Volume volume,const btVector3& velocity,btScalar margin) bool btDbvt::update(Node* leaf,Volume volume,const btVector3& velocity,btScalar margin)
{ {
if(leaf->volume.Contain(volume)) return(false); if(leaf->volume.Contain(volume)) return(false);
volume.Expand(btVector3(margin,margin,margin)); volume.Expand(btVector3(margin,margin,margin));
volume.SignedExpand(velocity); volume.SignedExpand(velocity);
update(leaf,volume); update(leaf,volume);
return(true); return(true);
} }
// //
bool btDbvt::update(Node* leaf,Volume volume,const btVector3& velocity) bool btDbvt::update(Node* leaf,Volume volume,const btVector3& velocity)
{ {
if(leaf->volume.Contain(volume)) return(false); if(leaf->volume.Contain(volume)) return(false);
volume.SignedExpand(velocity); volume.SignedExpand(velocity);
update(leaf,volume); update(leaf,volume);
return(true); return(true);
} }
// //
bool btDbvt::update(Node* leaf,Volume volume,btScalar margin) bool btDbvt::update(Node* leaf,Volume volume,btScalar margin)
{ {
if(leaf->volume.Contain(volume)) return(false); if(leaf->volume.Contain(volume)) return(false);
volume.Expand(btVector3(margin,margin,margin)); volume.Expand(btVector3(margin,margin,margin));
update(leaf,volume); update(leaf,volume);
return(true); return(true);
} }
// //
void btDbvt::remove(Node* leaf) void btDbvt::remove(Node* leaf)
{ {
removeleaf(this,leaf); removeleaf(this,leaf);
deletenode(this,leaf); deletenode(this,leaf);
--m_leafs; --m_leafs;
} }
// //
void btDbvt::collide(btDbvt* tree, void btDbvt::write(IWriter* iwriter) const
ICollide* icollide) const
{ {
collideGeneric(tree,icollide); btDbvtNodeEnumerator nodes;
} nodes.nodes.reserve(m_leafs*2);
enumNodes(m_root,nodes);
// iwriter->Prepare(m_root,nodes.nodes.size());
void btDbvt::collide(btDbvt::Node* node, for(int i=0;i<nodes.nodes.size();++i)
ICollide* icollide) const {
{ const Node* n=nodes.nodes[i];
collideGeneric(node,icollide); int p=-1;
} if(n->parent) p=nodes.nodes.findLinearSearch(n->parent);
if(n->isinternal())
// {
void btDbvt::collide(const Volume& volume, const int c0=nodes.nodes.findLinearSearch(n->childs[0]);
ICollide* icollide) const const int c1=nodes.nodes.findLinearSearch(n->childs[1]);
{ iwriter->WriteNode(n,i,p,c0,c1);
collideGeneric(volume,icollide); }
} else
{
// iwriter->WriteLeaf(n,i,p);
void btDbvt::collide(ICollide* icollide) const }
{ }
collideGeneric(icollide);
} }

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

@@ -20,6 +20,26 @@ subject to the following restrictions:
#include "LinearMath/btAlignedObjectArray.h" #include "LinearMath/btAlignedObjectArray.h"
#include "LinearMath/btVector3.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 // Defaults volumes
// //
@@ -27,39 +47,45 @@ subject to the following restrictions:
/* btDbvtAabbMm */ /* btDbvtAabbMm */
struct btDbvtAabbMm struct btDbvtAabbMm
{ {
inline btVector3 Center() const { return((mi+mx)/2); } inline btVector3 Center() const { return((mi+mx)/2); }
inline btVector3 Extent() const { return((mx-mi)/2); } inline btVector3 Extent() const { return((mx-mi)/2); }
inline const btVector3& Mins() const { return(mi); } inline const btVector3& Mins() const { return(mi); }
inline const btVector3& Maxs() const { return(mx); } inline const btVector3& Maxs() const { return(mx); }
inline btVector3 Lengths() const { return(mx-mi); } inline btVector3 Lengths() const { return(mx-mi); }
static inline btDbvtAabbMm FromCE(const btVector3& c,const btVector3& e); static inline btDbvtAabbMm FromCE(const btVector3& c,const btVector3& e);
static inline btDbvtAabbMm FromCR(const btVector3& c,btScalar r); static inline btDbvtAabbMm FromCR(const btVector3& c,btScalar r);
static inline btDbvtAabbMm FromMM(const btVector3& mi,const btVector3& mx); 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* pts,int n);
static inline btDbvtAabbMm FromPoints(const btVector3** ppts,int n); static inline btDbvtAabbMm FromPoints(const btVector3** ppts,int n);
inline void Expand(const btVector3 e); inline void Expand(const btVector3 e);
inline void SignedExpand(const btVector3 e); inline void SignedExpand(const btVector3 e);
inline bool Contain(const btDbvtAabbMm& a) const; inline bool Contain(const btDbvtAabbMm& a) const;
inline friend bool Intersect( const btDbvtAabbMm& a, 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); const btDbvtAabbMm& b);
inline friend bool Intersect( const btDbvtAabbMm& a, inline friend bool Intersect( const btDbvtAabbMm& a,
const btVector3& b); const btVector3& b);
inline friend btScalar Proximity( const btDbvtAabbMm& a, 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); const btDbvtAabbMm& b);
inline friend void Merge( const btDbvtAabbMm& a, inline friend void Merge( const btDbvtAabbMm& a,
const btDbvtAabbMm& b, const btDbvtAabbMm& b,
btDbvtAabbMm& r); btDbvtAabbMm& r);
inline friend bool NotEqual( const btDbvtAabbMm& a, inline friend bool NotEqual( const btDbvtAabbMm& a,
const btDbvtAabbMm& b); const btDbvtAabbMm& b);
private: private:
btVector3 mi,mx; btVector3 mi,mx;
}; };
// //
// Dynamic bounding volume tree // Dynamic bounding volume tree
// //
struct btDbvt struct btDbvt
{ {
// Types // Types
typedef btDbvtAabbMm Volume; typedef btDbvtAabbMm Volume;
/* Node */ /* Node */
@@ -75,30 +101,47 @@ struct btDbvt
}; };
}; };
/* Stack element */ /* Stack element */
struct sStkElm struct sStkNN
{ {
const Node* a; const Node* a;
const Node* b; const Node* b;
sStkElm(const Node* na,const Node* nb) : a(na),b(nb) {} sStkNN(const Node* na,const Node* nb) : a(na),b(nb) {}
}; };
struct sStkNP
// Interfaces {
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 */ /* ICollide */
struct 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 Prepare(const Node* root,int numnodes)=0;
virtual void Process(const Node*,const Node*)=0; virtual void WriteNode(const Node*,int index,int parent,int child0,int child1)=0;
virtual void Process(const Node*)=0; virtual void WriteLeaf(const Node*,int index,int parent)=0;
virtual bool Descent(const Node*)=0;
}; };
// Constants // Constants
enum { enum {
TREETREE_STACKSIZE = 128, SIMPLE_STACKSIZE = 64,
VOLUMETREE_STACKSIZE = 64 DOUBLE_STACKSIZE = SIMPLE_STACKSIZE*2,
}; };
// Fields // Fields
@@ -106,6 +149,7 @@ struct btDbvt
Node* m_free; Node* m_free;
int m_lkhd; int m_lkhd;
int m_leafs; int m_leafs;
unsigned m_opath;
// Methods // Methods
btDbvt(); btDbvt();
~btDbvt(); ~btDbvt();
@@ -113,36 +157,55 @@ struct btDbvt
bool empty() const { return(0==m_root); } bool empty() const { return(0==m_root); }
void optimizeBottomUp(); void optimizeBottomUp();
void optimizeTopDown(int bu_treshold=128); void optimizeTopDown(int bu_treshold=128);
void optimizeIncremental(int passes);
Node* insert(const Volume& box,void* data); 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); 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,btScalar margin);
bool update(Node* leaf,Volume volume,const btVector3& velocity); bool update(Node* leaf,Volume volume,const btVector3& velocity);
bool update(Node* leaf,Volume volume,btScalar margin); bool update(Node* leaf,Volume volume,btScalar margin);
void remove(Node* leaf); void remove(Node* leaf);
void collide(btDbvt* tree, void write(IWriter* iwriter) const;
ICollide* icollide) const; // DBVT_IPOLICY must support ICollide policy/interface
void collide(btDbvt::Node* node, DBVT_PREFIX
ICollide* icollide) const; static void enumNodes( const Node* root,
void collide(const Volume& volume, DBVT_IPOLICY);
ICollide* icollide) const; DBVT_PREFIX
void collide(const btVector3& org, static void enumLeafs( const Node* root,
const btVector3& dir, DBVT_IPOLICY);
ICollide* icollide) const; DBVT_PREFIX
void collide(ICollide* icollide) const; static void collideTT( const Node* root0,
// Generics : T must implement ICollide const Node* root1,
DBVT_IPOLICY);
void collideGeneric( btDbvt* tree,ICollide* policy) const; DBVT_PREFIX
static void collideTV( const Node* root,
void collideGeneric( btDbvt::Node* node,ICollide* policy) const; const Volume& volume,
DBVT_IPOLICY);
void collideGeneric(const Volume& volume,ICollide* policy) const; DBVT_PREFIX
static void collideRAY( const Node* root,
void collideGeneric(ICollide* policy) const; 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&) {} btDbvt(const btDbvt&) {}
}; };
// //
// Inline's // Inline's
@@ -151,69 +214,69 @@ private:
// //
inline btDbvtAabbMm btDbvtAabbMm::FromCE(const btVector3& c,const btVector3& e) inline btDbvtAabbMm btDbvtAabbMm::FromCE(const btVector3& c,const btVector3& e)
{ {
btDbvtAabbMm box; btDbvtAabbMm box;
box.mi=c-e;box.mx=c+e; box.mi=c-e;box.mx=c+e;
return(box); return(box);
} }
// //
inline btDbvtAabbMm btDbvtAabbMm::FromCR(const btVector3& c,btScalar r) 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) inline btDbvtAabbMm btDbvtAabbMm::FromMM(const btVector3& mi,const btVector3& mx)
{ {
btDbvtAabbMm box; btDbvtAabbMm box;
box.mi=mi;box.mx=mx; box.mi=mi;box.mx=mx;
return(box); return(box);
} }
// //
inline btDbvtAabbMm btDbvtAabbMm::FromPoints(const btVector3* pts,int n) inline btDbvtAabbMm btDbvtAabbMm::FromPoints(const btVector3* pts,int n)
{ {
btDbvtAabbMm box; btDbvtAabbMm box;
box.mi=box.mx=pts[0]; box.mi=box.mx=pts[0];
for(int i=1;i<n;++i) for(int i=1;i<n;++i)
{ {
box.mi.setMin(pts[i]); box.mi.setMin(pts[i]);
box.mx.setMax(pts[i]); box.mx.setMax(pts[i]);
} }
return(box); return(box);
} }
// //
inline btDbvtAabbMm btDbvtAabbMm::FromPoints(const btVector3** ppts,int n) inline btDbvtAabbMm btDbvtAabbMm::FromPoints(const btVector3** ppts,int n)
{ {
btDbvtAabbMm box; btDbvtAabbMm box;
box.mi=box.mx=*ppts[0]; box.mi=box.mx=*ppts[0];
for(int i=1;i<n;++i) for(int i=1;i<n;++i)
{ {
box.mi.setMin(*ppts[i]); box.mi.setMin(*ppts[i]);
box.mx.setMax(*ppts[i]); box.mx.setMax(*ppts[i]);
} }
return(box); return(box);
} }
// //
inline void btDbvtAabbMm::Expand(const btVector3 e) inline void btDbvtAabbMm::Expand(const btVector3 e)
{ {
mi-=e;mx+=e; mi-=e;mx+=e;
} }
// //
inline void btDbvtAabbMm::SignedExpand(const btVector3 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.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.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.z()>0) mx.setZ(mx.z()+e.z()); else mi.setZ(mi.z()+e.z());
} }
// //
inline bool btDbvtAabbMm::Contain(const btDbvtAabbMm& a) const 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.y()<=a.mi.y())&&
(mi.z()<=a.mi.z())&& (mi.z()<=a.mi.z())&&
(mx.x()>=a.mx.x())&& (mx.x()>=a.mx.x())&&
@@ -221,24 +284,62 @@ inline bool btDbvtAabbMm::Contain(const btDbvtAabbMm& a) const
(mx.z()>=a.mx.z())); (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, inline bool Intersect( const btDbvtAabbMm& a,
const btDbvtAabbMm& b) const btDbvtAabbMm& b)
{ {
#if 0 #if 0
const btScalar mi[]={ b.mx.x()-a.mi.x(), const btScalar mi[]={ b.mx.x()-a.mi.x(),
b.mx.y()-a.mi.y(), b.mx.y()-a.mi.y(),
b.mx.z()-a.mi.z()}; b.mx.z()-a.mi.z()};
const unsigned* imi=(const unsigned*)mi; const unsigned* imi=(const unsigned*)mi;
if((imi[0]|imi[1]|imi[2])&0x80000000) return(false); if((imi[0]|imi[1]|imi[2])&0x80000000) return(false);
const btScalar mx[]={ a.mx.x()-b.mi.x(), const btScalar mx[]={ a.mx.x()-b.mi.x(),
a.mx.y()-b.mi.y(), a.mx.y()-b.mi.y(),
a.mx.z()-b.mi.z()}; a.mx.z()-b.mi.z()};
const unsigned* imx=(const unsigned*)mx; const unsigned* imx=(const unsigned*)mx;
if((imx[0]|imx[1]|imx[2])&0x80000000) return(false); if((imx[0]|imx[1]|imx[2])&0x80000000) return(false);
return(true); return(true);
#else #else
return( (a.mi.x()<=b.mx.x())&& return( (a.mi.x()<=b.mx.x())&&
(a.mi.y()<=b.mx.y())&& (a.mi.y()<=b.mx.y())&&
(a.mi.z()<=b.mx.z())&& (a.mi.z()<=b.mx.z())&&
(a.mx.x()>=b.mi.x())&& (a.mx.x()>=b.mi.x())&&
@@ -251,7 +352,7 @@ inline bool Intersect( const btDbvtAabbMm& a,
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.y()>=a.mi.y())&&
(b.z()>=a.mi.z())&& (b.z()>=a.mi.z())&&
(b.x()<=a.mx.x())&& (b.x()<=a.mx.x())&&
@@ -259,12 +360,34 @@ inline bool Intersect( const btDbvtAabbMm& a,
(b.z()<=a.mx.z())); (b.z()<=a.mx.z()));
} }
//
inline bool Intersect( const btDbvtAabbMm& a,
const btVector3& org,
const btVector3& invdir,
const unsigned* signs)
{
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, inline btScalar Proximity( const btDbvtAabbMm& a,
const btDbvtAabbMm& b) const btDbvtAabbMm& b)
{ {
const btVector3 d=(a.mi+a.mx)-(b.mi+b.mx); const btVector3 d=(a.mi+a.mx)-(b.mi+b.mx);
return(btFabs(d.x())+btFabs(d.y())+btFabs(d.z())); return(btFabs(d.x())+btFabs(d.y())+btFabs(d.z()));
} }
// //
@@ -272,16 +395,16 @@ inline void Merge( const btDbvtAabbMm& a,
const btDbvtAabbMm& b, const btDbvtAabbMm& b,
btDbvtAabbMm& r) btDbvtAabbMm& r)
{ {
r=a; r=a;
r.mi.setMin(b.mi); r.mi.setMin(b.mi);
r.mx.setMax(b.mx); r.mx.setMax(b.mx);
} }
// //
inline bool NotEqual( const btDbvtAabbMm& a, 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.y()!=b.mi.y())||
(a.mi.z()!=b.mi.z())|| (a.mi.z()!=b.mi.z())||
(a.mx.x()!=b.mx.x())|| (a.mx.x()!=b.mx.x())||
@@ -290,27 +413,62 @@ 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; enumNodes(root->childs[0],policy);
stack.reserve(TREETREE_STACKSIZE); enumNodes(root->childs[1],policy);
stack.push_back(sStkElm(m_root,node)); }
}
//
DBVT_PREFIX
inline void btDbvt::enumLeafs( const Node* root,
DBVT_IPOLICY)
{
DBVT_CHECKTYPE
if(root->isinternal())
{
enumLeafs(root->childs[0],policy);
enumLeafs(root->childs[1],policy);
}
else
{
policy.Process(root);
}
}
//
DBVT_PREFIX
inline void btDbvt::collideTT( const Node* root0,
const Node* root1,
DBVT_IPOLICY)
{
DBVT_CHECKTYPE
if(root0&&root1)
{
btAlignedObjectArray<sStkNN> stack;
stack.reserve(DOUBLE_STACKSIZE);
stack.push_back(sStkNN(root0,root1));
do { do {
sStkElm p=stack[stack.size()-1]; sStkNN p=stack[stack.size()-1];
stack.pop_back(); stack.pop_back();
if(p.a==p.b) 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(sStkNN(p.a->childs[0],p.a->childs[0]));
stack.push_back(sStkElm(p.a->childs[1],p.a->childs[1])); stack.push_back(sStkNN(p.a->childs[1],p.a->childs[1]));
stack.push_back(sStkElm(p.a->childs[0],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)) else if(Intersect(p.a->volume,p.b->volume))
@@ -319,27 +477,27 @@ inline void btDbvt::collideGeneric( btDbvt::Node* node,ICollide* policy) cons
{ {
if(p.b->isinternal()) if(p.b->isinternal())
{ {
stack.push_back(sStkElm(p.a->childs[0],p.b->childs[0])); stack.push_back(sStkNN(p.a->childs[0],p.b->childs[0]));
stack.push_back(sStkElm(p.a->childs[1],p.b->childs[0])); stack.push_back(sStkNN(p.a->childs[1],p.b->childs[0]));
stack.push_back(sStkElm(p.a->childs[0],p.b->childs[1])); stack.push_back(sStkNN(p.a->childs[0],p.b->childs[1]));
stack.push_back(sStkElm(p.a->childs[1],p.b->childs[1])); stack.push_back(sStkNN(p.a->childs[1],p.b->childs[1]));
} }
else else
{ {
stack.push_back(sStkElm(p.a->childs[0],p.b)); stack.push_back(sStkNN(p.a->childs[0],p.b));
stack.push_back(sStkElm(p.a->childs[1],p.b)); stack.push_back(sStkNN(p.a->childs[1],p.b));
} }
} }
else else
{ {
if(p.b->isinternal()) if(p.b->isinternal())
{ {
stack.push_back(sStkElm(p.a,p.b->childs[0])); stack.push_back(sStkNN(p.a,p.b->childs[0]));
stack.push_back(sStkElm(p.a,p.b->childs[1])); stack.push_back(sStkNN(p.a,p.b->childs[1]));
} }
else else
{ {
policy->Process(p.a,p.b); policy.Process(p.a,p.b);
} }
} }
} }
@@ -348,19 +506,17 @@ inline void btDbvt::collideGeneric( btDbvt::Node* node,ICollide* policy) cons
} }
// //
inline void btDbvt::collideGeneric( btDbvt* tree,ICollide* policy) const DBVT_PREFIX
inline void btDbvt::collideTV( const Node* root,
const Volume& volume,
DBVT_IPOLICY)
{ {
collideGeneric(tree->m_root,policy); DBVT_CHECKTYPE
} if(root)
//
inline void btDbvt::collideGeneric(const Volume& volume,ICollide* policy) const
{
if(m_root)
{ {
btAlignedObjectArray<const Node*> stack; btAlignedObjectArray<const Node*> stack;
stack.reserve(VOLUMETREE_STACKSIZE); stack.reserve(SIMPLE_STACKSIZE);
stack.push_back(m_root); stack.push_back(root);
do { do {
const Node* n=stack[stack.size()-1]; const Node* n=stack[stack.size()-1];
stack.pop_back(); stack.pop_back();
@@ -373,7 +529,7 @@ inline void btDbvt::collideGeneric(const Volume& volume,ICollide* policy) cons
} }
else else
{ {
policy->Process(n); policy.Process(n);
} }
} }
} while(stack.size()>0); } while(stack.size()>0);
@@ -381,26 +537,206 @@ inline void btDbvt::collideGeneric(const Volume& volume,ICollide* policy) cons
} }
// //
DBVT_PREFIX
inline void btDbvt::collideGeneric(ICollide* policy) const inline void btDbvt::collideRAY( const Node* root,
const btVector3& origin,
const btVector3& direction,
DBVT_IPOLICY)
{ {
if(m_root) DBVT_CHECKTYPE
if(root)
{
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(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; btAlignedObjectArray<const Node*> stack;
stack.reserve(VOLUMETREE_STACKSIZE); stack.reserve(SIMPLE_STACKSIZE);
stack.push_back(m_root); stack.push_back(root);
do { do {
const Node* n=stack[stack.size()-1]; const Node* n=stack[stack.size()-1];
stack.pop_back(); stack.pop_back();
if(policy->Descent(n)) if(policy.Descent(n))
{ {
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 else
{ policy->Process(n); } { policy.Process(n); }
} }
} while(stack.size()>0); } 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 #endif

355
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" #include "btDbvtBroadphase.h"
// //
@@ -23,7 +7,7 @@ subject to the following restrictions:
#if DBVT_BP_PROFILE #if DBVT_BP_PROFILE
#include <stdio.h> #include <stdio.h>
struct ProfileScope struct ProfileScope
{ {
ProfileScope(btClock& clock,unsigned long& value) ProfileScope(btClock& clock,unsigned long& value)
{ {
m_clock=&clock; m_clock=&clock;
@@ -37,7 +21,7 @@ struct ProfileScope
btClock* m_clock; btClock* m_clock;
unsigned long* m_value; unsigned long* m_value;
unsigned long m_base; unsigned long m_base;
}; };
#define SPC(_value_) ProfileScope spc_scope(m_clock,_value_) #define SPC(_value_) ProfileScope spc_scope(m_clock,_value_)
#else #else
#define SPC(_value_) #define SPC(_value_)
@@ -47,82 +31,84 @@ struct ProfileScope
// Helpers // 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> template <typename T>
static inline void listappend(T* item,T*& list) static inline void listappend(T* item,T*& list)
{ {
item->links[0]=0; item->links[0]=0;
item->links[1]=list; item->links[1]=list;
if(list) list->links[0]=item; if(list) list->links[0]=item;
list=item; list=item;
} }
// //
template <typename T> template <typename T>
static inline void listremove(T* item,T*& list) 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[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[1]) item->links[1]->links[0]=item->links[0];
} }
// //
template <typename T> template <typename T>
static inline int listcount(T* root) static inline int listcount(T* root)
{ {
int n=0; int n=0;
while(root) { ++n;root=root->links[1]; } while(root) { ++n;root=root->links[1]; }
return(n); return(n);
} }
// //
template <typename T> template <typename T>
static inline void clear(T& value) static inline void clear(T& value)
{ {
static const T zerodummy; static const struct ZeroDummy : T {} zerodummy;
value=zerodummy; value=zerodummy;
} }
// //
// Collider // Colliders
// //
struct btDbvtBroadphaseCollider : btDbvt::ICollide
/* Leaf collider */
struct btDbvtLeafCollider : btDbvt::ICollide
{ {
btDbvtBroadphase* pbp; btDbvtBroadphase* pbp;
int pid; btDbvtProxy* ppx;
btDbvtBroadphaseCollider(btDbvtBroadphase* p,int id) : pbp(p),pid(id) {} btDbvtLeafCollider(btDbvtBroadphase* p,btDbvtProxy* px) : pbp(p),ppx(px) {}
void Process(const btDbvt::Node* na)
virtual void Process(const btDbvt::Node* /*na*/)
{ {
} const btDbvt::Node* nb=ppx->leaf;
virtual bool Descent(const btDbvt::Node*) if(nb!=na)
{
return false;
}
virtual void Process(const btDbvt::Node* na,const btDbvt::Node* nb)
{ {
btDbvtProxy* pa=(btDbvtProxy*)na->data; btDbvtProxy* pa=(btDbvtProxy*)na->data;
btDbvtProxy* pb=(btDbvtProxy*)nb->data; btDbvtProxy* pb=(btDbvtProxy*)nb->data;
#if DBVT_BP_DISCRETPAIRS #if DBVT_BP_DISCRETPAIRS
if(Intersect(pa->aabb,pb->aabb)) if(Intersect(pa->aabb,pb->aabb))
#endif #endif
{ {
btBroadphasePair* pp=pbp->m_paircache->addOverlappingPair(pa,pb); if(pa>pb) btSwap(pa,pb);
if(pp) pbp->m_paircache->addOverlappingPair(pa,pb);
pp->m_userInfo=*(void**)&pid; }
}
}
};
/* 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,32 +118,30 @@ struct btDbvtBroadphaseCollider : btDbvt::ICollide
// //
// //
btDbvtBroadphase::btDbvtBroadphase() btDbvtBroadphase::btDbvtBroadphase(btOverlappingPairCache* paircache)
{ {
m_invalidPair = 0; m_releasepaircache = (paircache!=0)?false:true;
m_fcursor = 0; m_stageCurrent = 0;
m_dcursor = 0; m_fupdates = 1;
m_stageCurrent = 0; m_dupdates = 1;
m_fupdates = 1; m_paircache = paircache?
m_dupdates = 0; paircache :
//m_paircache = new btSortedOverlappingPairCache(); new(btAlignedAlloc(sizeof(btHashedOverlappingPairCache),16)) btHashedOverlappingPairCache();
m_paircache = new btHashedOverlappingPairCache(); m_gid = 0;
m_pid = 0;
m_gid = 0; for(int i=0;i<=STAGECOUNT;++i)
m_pid = 0;
for(int i=0;i<=STAGECOUNT;++i)
{ {
m_stageRoots[i]=0; m_stageRoots[i]=0;
} }
#if DBVT_BP_PROFILE #if DBVT_BP_PROFILE
clear(m_profiling); clear(m_profiling);
#endif #endif
} }
// //
btDbvtBroadphase::~btDbvtBroadphase() btDbvtBroadphase::~btDbvtBroadphase()
{ {
delete m_paircache; if(m_releasepaircache) btAlignedFree(m_paircache);
} }
// //
@@ -170,27 +154,29 @@ btBroadphaseProxy* btDbvtBroadphase::createProxy( const btVector3& aabbMin,
btDispatcher* /*dispatcher*/, btDispatcher* /*dispatcher*/,
void* /*multiSapProxy*/) void* /*multiSapProxy*/)
{ {
btDbvtProxy* proxy=new btDbvtProxy(userPtr,collisionFilterGroup,collisionFilterMask); btDbvtProxy* proxy=new(btAlignedAlloc(sizeof(btDbvtProxy),16)) btDbvtProxy( userPtr,
proxy->aabb = btDbvtAabbMm::FromMM(aabbMin,aabbMax); collisionFilterGroup,
proxy->leaf = m_sets[0].insert(proxy->aabb,proxy); collisionFilterMask);
proxy->stage = m_stageCurrent; proxy->aabb = btDbvtAabbMm::FromMM(aabbMin,aabbMax);
proxy->m_uniqueId = ++m_gid; proxy->leaf = m_sets[0].insert(proxy->aabb,proxy);
listappend(proxy,m_stageRoots[m_stageCurrent]); proxy->stage = m_stageCurrent;
return(proxy); proxy->m_uniqueId = ++m_gid;
listappend(proxy,m_stageRoots[m_stageCurrent]);
return(proxy);
} }
// //
void btDbvtBroadphase::destroyProxy( btBroadphaseProxy* absproxy, void btDbvtBroadphase::destroyProxy( btBroadphaseProxy* absproxy,
btDispatcher* dispatcher) btDispatcher* dispatcher)
{ {
btDbvtProxy* proxy=(btDbvtProxy*)absproxy; btDbvtProxy* proxy=(btDbvtProxy*)absproxy;
if(proxy->stage==STAGECOUNT) if(proxy->stage==STAGECOUNT)
m_sets[1].remove(proxy->leaf); m_sets[1].remove(proxy->leaf);
else else
m_sets[0].remove(proxy->leaf); m_sets[0].remove(proxy->leaf);
listremove(proxy,m_stageRoots[proxy->stage]); listremove(proxy,m_stageRoots[proxy->stage]);
m_paircache->removeOverlappingPairsContainingProxy(proxy,dispatcher); m_paircache->removeOverlappingPairsContainingProxy(proxy,dispatcher);
delete proxy; btAlignedFree(proxy);
} }
// //
@@ -199,33 +185,45 @@ void btDbvtBroadphase::setAabb( btBroadphaseProxy* absproxy,
const btVector3& aabbMax, const btVector3& aabbMax,
btDispatcher* /*dispatcher*/) btDispatcher* /*dispatcher*/)
{ {
btDbvtProxy* proxy=(btDbvtProxy*)absproxy; btDbvtProxy* proxy=(btDbvtProxy*)absproxy;
btDbvtAabbMm aabb=btDbvtAabbMm::FromMM(aabbMin,aabbMax); btDbvtAabbMm aabb=btDbvtAabbMm::FromMM(aabbMin,aabbMax);
if(proxy->stage==STAGECOUNT) if(proxy->stage==STAGECOUNT)
{/* fixed -> dynamic set */ {/* fixed -> dynamic set */
m_sets[1].remove(proxy->leaf); m_sets[1].remove(proxy->leaf);
proxy->leaf=m_sets[0].insert(aabb,proxy); proxy->leaf=m_sets[0].insert(aabb,proxy);
m_fcursor=0;
} }
else else
{/* dynamic set */ {/* dynamic set */
if(Intersect(proxy->leaf->volume,aabb))
{/* Moving */
const btVector3 delta=(aabbMin+aabbMax)/2-proxy->aabb.Center(); const btVector3 delta=(aabbMin+aabbMax)/2-proxy->aabb.Center();
#ifdef DBVT_BP_MARGIN
m_sets[0].update(proxy->leaf,aabb,delta*PREDICTED_FRAMES,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
} }
listremove(proxy,m_stageRoots[proxy->stage]); else
proxy->aabb = aabb; {/* Teleporting */
proxy->stage = m_stageCurrent; m_sets[0].update(proxy->leaf,aabb);
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) void btDbvtBroadphase::calculateOverlappingPairs(btDispatcher* dispatcher)
{ {
collide(dispatcher); collide(dispatcher);
#if DBVT_BP_PROFILE #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()); 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; unsigned int total=m_profiling.m_total;
if(total<=0) total=1; 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("ddcollide: %u%% (%uus)\r\n",(50+m_profiling.m_ddcollide*100)/total,m_profiling.m_ddcollide/DBVT_BP_PROFILING_RATE);
@@ -236,6 +234,7 @@ void btDbvtBroadphase::calculateOverlappingPairs(btDispatcher* dispatcher)
m_profiling.m_fdcollide+ m_profiling.m_fdcollide+
m_profiling.m_cleanup; m_profiling.m_cleanup;
printf("leaked: %u%% (%uus)\r\n",100-((50+sum*100)/total),(total-sum)/DBVT_BP_PROFILING_RATE); 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); clear(m_profiling);
m_clock.reset(); m_clock.reset();
} }
@@ -245,194 +244,88 @@ void btDbvtBroadphase::calculateOverlappingPairs(btDispatcher* dispatcher)
// //
void btDbvtBroadphase::collide(btDispatcher* dispatcher) void btDbvtBroadphase::collide(btDispatcher* dispatcher)
{ {
SPC(m_profiling.m_total); SPC(m_profiling.m_total);
/* refine dynamic */ /* optimize */
if(m_stageRoots[m_stageCurrent]&&(m_dupdates>0)) 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; btDbvtTreeCollider collider(this);
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 { do {
btDbvtProxy* next=current->links[1]; btDbvtProxy* next=current->links[1];
if(m_dcursor==current) m_dcursor=0;
listremove(current,m_stageRoots[current->stage]); listremove(current,m_stageRoots[current->stage]);
listappend(current,m_stageRoots[STAGECOUNT]); listappend(current,m_stageRoots[STAGECOUNT]);
m_sets[1].collideGeneric(current->leaf,&collider); btDbvt::collideTT(m_sets[1].m_root,current->leaf,collider);
m_sets[0].remove(current->leaf); m_sets[0].remove(current->leaf);
current->leaf = m_sets[1].insert(current->aabb,current); current->leaf = m_sets[1].insert(current->aabb,current);
current->stage = STAGECOUNT; current->stage = STAGECOUNT;
current = next; current = next;
} while(current); } while(current);
} }
/* refine fixed */ /* collide dynamics */
if(m_stageRoots[STAGECOUNT]&&(m_fupdates>0))
{ {
const int count=1+(m_sets[1].m_leafs*m_fupdates)/100; btDbvtTreeCollider collider(this);
for(int i=0;i<count;++i)
{
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); 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); 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); SPC(m_profiling.m_cleanup);
if (!m_paircache->hasDeferredRemoval())
{
btBroadphasePairArray& pairs=m_paircache->getOverlappingPairArray(); btBroadphasePairArray& pairs=m_paircache->getOverlappingPairArray();
if(pairs.size()>0)
{
for(int i=0,ni=pairs.size();i<ni;++i) for(int i=0,ni=pairs.size();i<ni;++i)
{ {
btBroadphasePair& p=pairs[i]; btBroadphasePair& p=pairs[i];
if(m_pid!=(*(int*)&p.m_userInfo))
{
btDbvtProxy* pa=(btDbvtProxy*)p.m_pProxy0; btDbvtProxy* pa=(btDbvtProxy*)p.m_pProxy0;
btDbvtProxy* pb=(btDbvtProxy*)p.m_pProxy1; btDbvtProxy* pb=(btDbvtProxy*)p.m_pProxy1;
if(!Intersect(pa->aabb,pb->aabb)) if(!Intersect(pa->aabb,pb->aabb))
{ {
if(pa>pb) btSwap(pa,pb);
m_paircache->removeOverlappingPair(pa,pb,dispatcher); m_paircache->removeOverlappingPair(pa,pb,dispatcher);
--ni;--i; --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 ++m_pid;
{
//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;
} }
// //
btOverlappingPairCache* btDbvtBroadphase::getOverlappingPairCache() btOverlappingPairCache* btDbvtBroadphase::getOverlappingPairCache()
{ {
return(m_paircache); return(m_paircache);
} }
// //
const btOverlappingPairCache* btDbvtBroadphase::getOverlappingPairCache() const const btOverlappingPairCache* btDbvtBroadphase::getOverlappingPairCache() const
{ {
return(m_paircache); return(m_paircache);
} }
// //
void btDbvtBroadphase::getBroadphaseAabb(btVector3& aabbMin,btVector3& aabbMax) const void btDbvtBroadphase::getBroadphaseAabb(btVector3& aabbMin,btVector3& aabbMax) const
{ {
btDbvtAabbMm bounds; btDbvtAabbMm bounds;
if(!m_sets[0].empty()) if(!m_sets[0].empty())
if(!m_sets[1].empty()) Merge( m_sets[0].m_root->volume, if(!m_sets[1].empty()) Merge( m_sets[0].m_root->volume,
m_sets[1].m_root->volume,bounds); m_sets[1].m_root->volume,bounds);
else else
bounds=m_sets[0].m_root->volume; bounds=m_sets[0].m_root->volume;
else if(!m_sets[1].empty()) bounds=m_sets[1].m_root->volume; else if(!m_sets[1].empty()) bounds=m_sets[1].m_root->volume;
else else
bounds=btDbvtAabbMm::FromCR(btVector3(0,0,0),0); bounds=btDbvtAabbMm::FromCR(btVector3(0,0,0),0);
aabbMin=bounds.Mins(); aabbMin=bounds.Mins();
aabbMax=bounds.Maxs(); aabbMax=bounds.Maxs();
} }
// //

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

507
src/BulletSoftBody/btSoftBody.cpp
View File

@@ -15,13 +15,7 @@ subject to the following restrictions:
///btSoftBody implementation by Nathanael Presson ///btSoftBody implementation by Nathanael Presson
#include "btSoftBody.h" #include "btSoftBody.h"
#if 1
#include <stdio.h>
#define DOTRACE
#endif
#include <string.h>
#include "LinearMath/btQuickprof.h" #include "LinearMath/btQuickprof.h"
#include "BulletCollision/NarrowPhaseCollision/btGjkEpa2.h"
#include "BulletCollision/BroadphaseCollision/btBroadphaseInterface.h" #include "BulletCollision/BroadphaseCollision/btBroadphaseInterface.h"
#include "BulletCollision/CollisionDispatch/btCollisionDispatcher.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> template <typename T>
@@ -199,6 +182,10 @@ template <typename T>
static inline T Sign(const T& x) static inline T Sign(const T& x)
{ return((T)(x<0?-1:+1)); } { 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) static inline btMatrix3x3 ScaleAlongAxis(const btVector3& a,btScalar s)
{ {
const btScalar xx=a.x()*a.x(); 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; 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) 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); 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]; 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) 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); 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 // Private implementation
// //
struct RayCaster : public btDbvt::ICollide struct RayCaster : btDbvt::ICollide
{ {
btVector3 o; btVector3 o;
btVector3 d; btVector3 d;
btVector3 nd;
btScalar mint; btScalar mint;
btSoftBody::Face* face; btSoftBody::Face* face;
int tests; int tests;
@@ -617,18 +581,11 @@ struct RayCaster : public btDbvt::ICollide
{ {
o = org; o = org;
d = dir; d = dir;
nd = dir.normalized();
mint = mxt; mint = mxt;
face = 0; face = 0;
tests = 0; tests = 0;
} }
void Process(const btDbvt::Node* leaf)
virtual void Process(const btDbvt::Node* /*a*/,const btDbvt::Node* /*b*/)
{
}
virtual void Process(const btDbvt::Node* leaf)
{ {
btSoftBody::Face& f=*(btSoftBody::Face*)leaf->data; btSoftBody::Face& f=*(btSoftBody::Face*)leaf->data;
const btScalar t=RayTriangle( o,d, const btScalar t=RayTriangle( o,d,
@@ -643,13 +600,6 @@ struct RayCaster : public btDbvt::ICollide
} }
++tests; ++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,38 +615,6 @@ static int RaycastInternal(const btSoftBody* psb,
int cnt=0; int cnt=0;
if(bcountonly||psb->m_fdbvt.empty()) if(bcountonly||psb->m_fdbvt.empty())
{/* Full search */ {/* 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,
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) for(int i=0,ni=psb->m_faces.size();i<ni;++i)
{ {
const btSoftBody::Face& f=psb->m_faces[i]; const btSoftBody::Face& f=psb->m_faces[i];
@@ -717,11 +635,10 @@ static int RaycastInternal(const btSoftBody* psb,
} }
} }
} }
}
else else
{/* Use dbvt */ {/* Use dbvt */
RayCaster collider(org,dir,mint); RayCaster collider(org,dir,mint);
psb->m_fdbvt.collideGeneric(&collider); btDbvt::collideRAY(psb->m_fdbvt.m_root,org,dir,collider);
if(collider.face) if(collider.face)
{ {
mint=collider.mint; mint=collider.mint;
@@ -742,7 +659,6 @@ for(int i=0;i<psb->m_faces.size();++i)
btSoftBody::Face& f=psb->m_faces[i]; btSoftBody::Face& f=psb->m_faces[i];
f.m_leaf=psb->m_fdbvt.insert(VolumeOf(f,0),&f); 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]; 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); 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 */ /* Area's */
btAlignedObjectArray<int> counts; btAlignedObjectArray<int> counts;
counts.resize(psb->m_nodes.size(),0); 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; const btScalar dt=psb->m_sst.sdt;
for(int i=0,ni=psb->m_anchors.size();i<ni;++i) 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 dt=psb->m_sst.sdt;
const btScalar mrg=psb->getCollisionShape()->getMargin();
for(int i=0,ni=psb->m_rcontacts.size();i<ni;++i) for(int i=0,ni=psb->m_rcontacts.size();i<ni;++i)
{ {
const btSoftBody::RContact& c=psb->m_rcontacts[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); const btScalar dn=dot(vr,cti.m_normal);
if(dn<=SIMD_EPSILON) 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 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_node->m_x-=impulse*c.m_c2;
c.m_cti.m_body->applyImpulse(impulse,c.m_c1); 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) 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) 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]; btSoftBody::Node& b=*l.m_n[1];
const btVector3 del=b.m_x-a.m_x; const btVector3 del=b.m_x-a.m_x;
const btScalar len=del.length2(); 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); a.m_x-=del*(k*a.m_im);
b.m_x+=del*(k*b.m_im); b.m_x+=del*(k*b.m_im);
} }
@@ -1175,68 +1083,26 @@ static void PSolve_Links(btSoftBody* psb)
} }
// //
static void PSolve_Tetras(btSoftBody* psb) static void VSolve_Links(btSoftBody* psb,btScalar kst)
{
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)
{ {
for(int i=0,ni=psb->m_links.size();i<ni;++i) for(int i=0,ni=psb->m_links.size();i<ni;++i)
{ {
btSoftBody::Link& l=psb->m_links[i]; btSoftBody::Link& l=psb->m_links[i];
btSoftBody::Node** n=l.m_n; 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[0]->m_v+= l.m_c3*(j*n[0]->m_im);
n[1]->m_v-= l.m_c3*(j*n[1]->m_im); n[1]->m_v-= l.m_c3*(j*n[1]->m_im);
} }
} }
// //
static void VSolve_Tetras(btSoftBody* psb) static void (* const VSolvers[])(btSoftBody*,btScalar)= {
{
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_Links,
VSolve_Tetras,
}; };
// //
static void (* const PSolvers[])(btSoftBody*)= { static void (* const PSolvers[])(btSoftBody*,btScalar)= {
PSolve_Links, PSolve_Links,
PSolve_Tetras,
PSolve_Anchors, PSolve_Anchors,
PSolve_RContacts, PSolve_RContacts,
PSolve_SContacts, PSolve_SContacts,
@@ -1262,7 +1128,7 @@ btSoftBody::btSoftBody(btSoftBody::btSoftBodyWorldInfo* worldInfo,int node_count
m_cfg.kDF = (btScalar)0.2; m_cfg.kDF = (btScalar)0.2;
m_cfg.kMT = 0; m_cfg.kMT = 0;
m_cfg.kCHR = (btScalar)1.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.kSHR = (btScalar)1.0;
m_cfg.kAHR = (btScalar)0.7; m_cfg.kAHR = (btScalar)0.7;
m_cfg.maxvolume = (btScalar)1; 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_leaf = m_ndbvt.insert(btDbvt::Volume::FromCR(n.m_x,margin),&n);
n.m_material= pm; n.m_material= pm;
} }
m_ndbvt.optimizeTopDown();
UpdateBounds(this); UpdateBounds(this);
@@ -1319,7 +1184,7 @@ btSoftBody::~btSoftBody()
{ {
//for now, delete the internal shape //for now, delete the internal shape
delete m_collisionShape; 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() btSoftBody::Material* btSoftBody::appendMaterial()
{ {
Material* pm=new Material(); Material* pm=new(btAlignedAlloc(sizeof(Material),16)) Material();
if(m_materials.size()>0) if(m_materials.size()>0)
*pm=*m_materials[0]; *pm=*m_materials[0];
else else
@@ -1431,18 +1296,6 @@ appendNote(text,o,btVector4(w,w,w,0), feature->m_n[0],
feature->m_n[2]); 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) 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; 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) void btSoftBody::appendAnchor(int node,btRigidBody* body)
{ {
@@ -1660,49 +1485,6 @@ void btSoftBody::setTotalDensity(btScalar density)
setTotalMass(getVolume()*density,true); 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) void btSoftBody::transform(const btTransform& trs)
{ {
@@ -1715,7 +1497,6 @@ void btSoftBody::transform(const btTransform& trs)
n.m_n=trs.getBasis()*n.m_n; n.m_n=trs.getBasis()*n.m_n;
m_ndbvt.update(n.m_leaf,btDbvt::Volume::FromCR(n.m_x,margin)); m_ndbvt.update(n.m_leaf,btDbvt::Volume::FromCR(n.m_x,margin));
} }
m_ndbvt.optimizeTopDown();
UpdateNormals(this); UpdateNormals(this);
UpdateBounds(this); UpdateBounds(this);
UpdateConstants(this); UpdateConstants(this);
@@ -1750,7 +1531,6 @@ void btSoftBody::scale(const btVector3& scl)
n.m_q*=scl; n.m_q*=scl;
m_ndbvt.update(n.m_leaf,btDbvt::Volume::FromCR(n.m_x,margin)); m_ndbvt.update(n.m_leaf,btDbvt::Volume::FromCR(n.m_x,margin));
} }
m_ndbvt.optimizeTopDown();
UpdateNormals(this); UpdateNormals(this);
UpdateBounds(this); UpdateBounds(this);
UpdateConstants(this); UpdateConstants(this);
@@ -1870,6 +1650,7 @@ int btSoftBody::generateBendingConstraints(int distance,Material* mat)
if(adj[IDX(i,j)]==(unsigned)distance) if(adj[IDX(i,j)]==(unsigned)distance)
{ {
appendLink(i,j,mat); appendLink(i,j,mat);
m_links[m_links.size()-1].m_bbending=1;
++nlinks; ++nlinks;
} }
} }
@@ -1880,24 +1661,20 @@ int btSoftBody::generateBendingConstraints(int distance,Material* mat)
return(0); return(0);
} }
//
int btSoftBody::generateTetrahedralConstraints()
{
/// not implemented
return(0);
}
// //
void btSoftBody::randomizeConstraints() 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) 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) 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()); int ncount(m_nodes.size());
btSymMatrix<int> edges(ncount,-2); btSymMatrix<int> edges(ncount,-2);
int newnodes=0; 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 */ /* Fill edges */
for(int i=0;i<m_links.size();++i) 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[1]-nbase),int(f.m_n[2]-nbase))=-1;
edges(int(f.m_n[2]-nbase),int(f.m_n[0]-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 */ /* Intersect */
for(int i=0;i<ncount;++i) 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 */ /* Cut */
if(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 */ /* Clean orphans */
int nnodes=m_nodes.size(); int nnodes=m_nodes.size();
btAlignedObjectArray<int> ranks; 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 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) for(int i=0;i<m_links.size();++i)
{ {
const int id[]={ int(m_links[i].m_n[0]-nbase), 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::Anchors);
m_cfg.m_psequence.push_back(ePSolver::RContacts); m_cfg.m_psequence.push_back(ePSolver::RContacts);
m_cfg.m_psequence.push_back(ePSolver::SContacts); m_cfg.m_psequence.push_back(ePSolver::SContacts);
m_cfg.m_psequence.push_back(ePSolver::Volume);
m_cfg.m_psequence.push_back(ePSolver::Linear); m_cfg.m_psequence.push_back(ePSolver::Linear);
break; break;
case eSolverPresets::Velocities: case eSolverPresets::Velocities:
m_cfg.m_vsequence.push_back(eVSolver::Volume);
m_cfg.m_vsequence.push_back(eVSolver::Linear); m_cfg.m_vsequence.push_back(eVSolver::Linear);
m_cfg.m_psequence.push_back(ePSolver::Anchors); m_cfg.m_psequence.push_back(ePSolver::Anchors);
m_cfg.m_psequence.push_back(ePSolver::RContacts); m_cfg.m_psequence.push_back(ePSolver::RContacts);
m_cfg.m_psequence.push_back(ePSolver::SContacts); m_cfg.m_psequence.push_back(ePSolver::SContacts);
m_cfg.m_dsequence.push_back(ePSolver::Volume);
m_cfg.m_dsequence.push_back(ePSolver::Linear); m_cfg.m_dsequence.push_back(ePSolver::Linear);
break; break;
} }
@@ -2389,55 +2092,8 @@ void btSoftBody::predictMotion(btScalar dt)
} }
/* Pose */ /* Pose */
UpdatePose(this); UpdatePose(this);
/* Clear contacts */ /* Match */
m_rcontacts.resize(0); if(m_pose.m_bframe&&(m_cfg.kMT>0))
m_scontacts.resize(0);
}
//
void btSoftBody::solveConstraints()
{
/* Solve velocities */
if(m_cfg.viterations>0)
{
/* 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) for(int i=0,ni=m_nodes.size();i<ni;++i)
{ {
@@ -2449,6 +2105,24 @@ if(m_pose.m_bframe&&(m_cfg.kMT>0))
} }
} }
} }
/* Clear contacts */
m_rcontacts.resize(0);
m_scontacts.resize(0);
/* Optimize dbvt's */
m_ndbvt.optimizeIncremental(1);
m_fdbvt.optimizeIncremental(1);
}
//
void btSoftBody::solveConstraints()
{
/* Prepare links */
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);
}
/* Prepare anchors */ /* Prepare anchors */
for(int i=0,ni=m_anchors.size();i<ni;++i) 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_c2 = m_sst.sdt*a.m_node->m_im;
a.m_body->activate(); 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 */ /* Solve positions */
if(m_cfg.piterations>0) 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) 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); 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) 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) for(int i=0,ni=m_nodes.size();i<ni;++i)
@@ -2512,7 +2204,7 @@ for(int isolve=0;isolve<iterations;++isolve)
{ {
for(int iseq=0;iseq<m_cfg.m_psequence.size();++iseq) 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 struct DoCollide : btDbvt::ICollide
{ {
void Process(const btDbvt::Node* leaf)
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)
{ {
Node* node=(Node*)leaf->data; Node* node=(Node*)leaf->data;
DoNode(*node); DoNode(*node);
@@ -2608,7 +2290,7 @@ switch(m_cfg.collisions&fCollision::RVSmask)
docollide.prb = prb; docollide.prb = prb;
docollide.dynmargin = basemargin+timemargin; docollide.dynmargin = basemargin+timemargin;
docollide.stamargin = basemargin; docollide.stamargin = basemargin;
m_ndbvt.collide(volume,&docollide); btDbvt::collideTV(m_ndbvt.m_root,volume,docollide);
} }
break; break;
} }
@@ -2624,15 +2306,6 @@ switch(cf&fCollision::SVSmask)
{ {
struct DoCollide : btDbvt::ICollide 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, void Process(const btDbvt::Node* lnode,
const btDbvt::Node* lface) const btDbvt::Node* lface)
{ {
@@ -2683,13 +2356,15 @@ switch(cf&fCollision::SVSmask)
/* psb0 nodes vs psb1 faces */ /* psb0 nodes vs psb1 faces */
docollide.psb[0]=this; docollide.psb[0]=this;
docollide.psb[1]=psb; docollide.psb[1]=psb;
docollide.psb[0]->m_ndbvt.collide( &docollide.psb[1]->m_fdbvt, btDbvt::collideTT( docollide.psb[0]->m_ndbvt.m_root,
&docollide); docollide.psb[1]->m_fdbvt.m_root,
docollide);
/* psb1 nodes vs psb0 faces */ /* psb1 nodes vs psb0 faces */
docollide.psb[0]=psb; docollide.psb[0]=psb;
docollide.psb[1]=this; docollide.psb[1]=this;
docollide.psb[0]->m_ndbvt.collide( &docollide.psb[1]->m_fdbvt, btDbvt::collideTT( docollide.psb[0]->m_ndbvt.m_root,
&docollide); docollide.psb[1]->m_fdbvt.m_root,
docollide);
} }
break; break;
} }

65
src/BulletSoftBody/btSoftBody.h
View File

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

216
src/BulletSoftBody/btSoftBodyHelpers.cpp
View File

@@ -15,7 +15,6 @@ subject to the following restrictions:
///btSoftBodyHelpers.cpp by Nathanael Presson ///btSoftBodyHelpers.cpp by Nathanael Presson
#include "btSoftBody.h" #include "btSoftBody.h"
#include "BulletCollision/BroadphaseCollision/btDbvt.h"
#include <stdio.h> #include <stdio.h>
#include <string.h> #include <string.h>
#include "btSoftBodyHelpers.h" #include "btSoftBodyHelpers.h"
@@ -24,11 +23,11 @@ subject to the following restrictions:
// //
static void drawVertex( btIDebugDraw* idraw, static void drawVertex( btIDebugDraw* idraw,
const btVector3& x,btScalar s,const btVector3& 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(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,s,0),x+btVector3(0,s,0),c);
idraw->drawLine(x-btVector3(0,0,s),x+btVector3(0,0,s),c); idraw->drawLine(x-btVector3(0,0,s),x+btVector3(0,0,s),c);
} }
// //
static void drawBox( btIDebugDraw* idraw, static void drawBox( btIDebugDraw* idraw,
@@ -36,7 +35,7 @@ static void drawBox( btIDebugDraw* idraw,
const btVector3& maxs, const btVector3& maxs,
const btVector3& color) const btVector3& color)
{ {
const btVector3 c[]={ btVector3(mins.x(),mins.y(),mins.z()), const btVector3 c[]={ btVector3(mins.x(),mins.y(),mins.z()),
btVector3(maxs.x(),mins.y(),mins.z()), btVector3(maxs.x(),mins.y(),mins.z()),
btVector3(maxs.x(),maxs.y(),mins.z()), btVector3(maxs.x(),maxs.y(),mins.z()),
btVector3(mins.x(),maxs.y(),mins.z()), btVector3(mins.x(),maxs.y(),mins.z()),
@@ -44,12 +43,12 @@ static void drawBox( btIDebugDraw* idraw,
btVector3(maxs.x(),mins.y(),maxs.z()), btVector3(maxs.x(),mins.y(),maxs.z()),
btVector3(maxs.x(),maxs.y(),maxs.z()), btVector3(maxs.x(),maxs.y(),maxs.z()),
btVector3(mins.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[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[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[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[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[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); idraw->drawLine(c[2],c[6],color);idraw->drawLine(c[3],c[7],color);
} }
// //
@@ -61,7 +60,7 @@ static void drawTree( btIDebugDraw* idraw,
int mindepth, int mindepth,
int maxdepth) int maxdepth)
{ {
if(node) if(node)
{ {
if(node->isinternal()&&((depth<maxdepth)||(maxdepth<0))) if(node->isinternal()&&((depth<maxdepth)||(maxdepth<0)))
{ {
@@ -92,9 +91,9 @@ static inline btScalar tetravolume(const btVector3& x0,
} }
// //
/* #if 0
static btVector3 stresscolor(btScalar stress) static btVector3 stresscolor(btScalar stress)
{ {
static const btVector3 spectrum[]= { btVector3(1,0,1), static const btVector3 spectrum[]= { btVector3(1,0,1),
btVector3(0,0,1), btVector3(0,0,1),
btVector3(0,1,1), btVector3(0,1,1),
@@ -108,8 +107,9 @@ static btVector3 stresscolor(btScalar stress)
const int sel=(int)stress; const int sel=(int)stress;
const btScalar frc=stress-sel; const btScalar frc=stress-sel;
return(spectrum[sel]+(spectrum[sel+1]-spectrum[sel])*frc); return(spectrum[sel]+(spectrum[sel+1]-spectrum[sel])*frc);
} }
*/ #endif
// //
void btSoftBodyHelpers::Draw( btSoftBody* psb, void btSoftBodyHelpers::Draw( btSoftBody* psb,
btIDebugDraw* idraw, btIDebugDraw* idraw,
@@ -219,24 +219,6 @@ void btSoftBodyHelpers::Draw( btSoftBody* psb,
col,alp);*/ 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);
}
}
/* Notes */ /* Notes */
if(0!=(drawflags&fDrawFlags::Notes)) if(0!=(drawflags&fDrawFlags::Notes))
{ {
@@ -253,7 +235,6 @@ void btSoftBodyHelpers::Draw( btSoftBody* psb,
} }
} }
#if 0
// //
void btSoftBodyHelpers::DrawInfos( btSoftBody* psb, void btSoftBodyHelpers::DrawInfos( btSoftBody* psb,
btIDebugDraw* idraw, btIDebugDraw* idraw,
@@ -279,7 +260,6 @@ void btSoftBodyHelpers::DrawInfos( btSoftBody* psb,
if(text[0]) idraw->draw3dText(n.m_x,text); if(text[0]) idraw->draw3dText(n.m_x,text);
} }
} }
#endif
// //
void btSoftBodyHelpers::DrawNodeTree( btSoftBody* psb, void btSoftBodyHelpers::DrawNodeTree( btSoftBody* psb,
@@ -287,7 +267,7 @@ void btSoftBodyHelpers::DrawNodeTree( btSoftBody* psb,
int mindepth, int mindepth,
int maxdepth) 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);
} }
// //
@@ -296,7 +276,7 @@ void btSoftBodyHelpers::DrawFaceTree( btSoftBody* psb,
int mindepth, int mindepth,
int maxdepth) 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);
} }
// //
@@ -504,18 +484,18 @@ btSoftBody* btSoftBodyHelpers::CreateFromTriMesh(btSoftBody::btSoftBodyWorldInf
btSoftBody* btSoftBodyHelpers::CreateFromConvexHull(btSoftBody::btSoftBodyWorldInfo& worldInfo, const btVector3* vertices, 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; HullResult hres;
HullLibrary hlib;/*??*/ HullLibrary hlib;/*??*/
hdsc.mMaxVertices=static_cast<unsigned int>(nvertices); hdsc.mMaxVertices=nvertices;
hlib.CreateConvexHull(hdsc,hres); hlib.CreateConvexHull(hdsc,hres);
btSoftBody* psb=new btSoftBody(&worldInfo,(int)hres.mNumOutputVertices, btSoftBody* psb=new btSoftBody(&worldInfo,(int)hres.mNumOutputVertices,
&hres.m_OutputVertices[0],0); &hres.m_OutputVertices[0],0);
for(int i=0;i<(int)hres.mNumFaces;++i) for(int i=0;i<(int)hres.mNumFaces;++i)
{ {
const int idx[]={ static_cast<int>(hres.m_Indices[i*3+0]), const int idx[]={ hres.m_Indices[i*3+0],
static_cast<int>(hres.m_Indices[i*3+1]), hres.m_Indices[i*3+1],
static_cast<int>(hres.m_Indices[i*3+2])}; hres.m_Indices[i*3+2]};
if(idx[0]<idx[1]) psb->appendLink( idx[0],idx[1]); if(idx[0]<idx[1]) psb->appendLink( idx[0],idx[1]);
if(idx[1]<idx[2]) psb->appendLink( idx[1],idx[2]); if(idx[1]<idx[2]) psb->appendLink( idx[1],idx[2]);
if(idx[2]<idx[0]) psb->appendLink( idx[2],idx[0]); if(idx[2]<idx[0]) psb->appendLink( idx[2],idx[0]);
@@ -525,155 +505,3 @@ btSoftBody* btSoftBodyHelpers::CreateFromConvexHull(btSoftBody::btSoftBodyWorld
psb->randomizeConstraints(); psb->randomizeConstraints();
return(psb); 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" #include "btSoftBody.h"
//Can't enable this, Bullet doesn't use STL
//#define BT_SOFTBODY_USE_STL 1
// //
// Helpers // Helpers
// //
@@ -37,7 +34,7 @@ struct fDrawFlags { enum _ {
Notes = 0x0080, Notes = 0x0080,
/* presets */ /* presets */
Std = Links+Faces+Tetras+Anchors+Notes, Std = Links+Faces+Tetras+Anchors+Notes,
StdTetra = Std-Faces+Tetras StdTetra = Std-Faces+Tetras,
};}; };};
struct btSoftBodyHelpers struct btSoftBodyHelpers
@@ -46,14 +43,12 @@ struct btSoftBodyHelpers
static void Draw( btSoftBody* psb, static void Draw( btSoftBody* psb,
btIDebugDraw* idraw, btIDebugDraw* idraw,
int drawflags=fDrawFlags::Std); int drawflags=fDrawFlags::Std);
#if 0
/* Draw body infos */ /* Draw body infos */
static void DrawInfos( btSoftBody* psb, static void DrawInfos( btSoftBody* psb,
btIDebugDraw* idraw, btIDebugDraw* idraw,
bool masses, bool masses,
bool areas, bool areas,
bool stress); bool stress);
#endif
/* Draw node tree */ /* Draw node tree */
static void DrawNodeTree( btSoftBody* psb, static void DrawNodeTree( btSoftBody* psb,
btIDebugDraw* idraw, btIDebugDraw* idraw,
@@ -97,27 +92,6 @@ struct btSoftBodyHelpers
static btSoftBody* CreateFromConvexHull( btSoftBody::btSoftBodyWorldInfo& worldInfo, static btSoftBody* CreateFromConvexHull( btSoftBody::btSoftBodyWorldInfo& worldInfo,
const btVector3* vertices, const btVector3* vertices,
int nvertices); 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 #endif //SOFT_BODY_HELPERS_H