diff --git a/Demos/SoftDemo/SoftDemo.cpp b/Demos/SoftDemo/SoftDemo.cpp index 79a3dd47b..cab6d6700 100644 --- a/Demos/SoftDemo/SoftDemo.cpp +++ b/Demos/SoftDemo/SoftDemo.cpp @@ -99,13 +99,32 @@ void SoftDemo::clientMoveAndDisplay() - float dt = getDeltaTimeMicroseconds() * 0.000001f; - - // printf("dt = %f: ",dt); - + float dt = 1.0/60.; 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 #ifdef FIXED_STEP m_dynamicsWorld->stepSimulation(dt=1.0f/60.f,0); @@ -137,6 +156,10 @@ void SoftDemo::clientMoveAndDisplay() #endif + if(m_drag) + { + m_node->m_v*=0; + } m_softBodyWorldInfo.m_sparsesdf.GarbageCollect(); @@ -670,6 +693,28 @@ static void Init_Sticks(SoftDemo* 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. // @@ -715,6 +760,7 @@ static void Init_Bunny(SoftDemo* pdemo) psb->scale(btVector3(6,6,6)); psb->setTotalMass(100,true); 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->setTotalMass(50,true); pdemo->getSoftDynamicsWorld()->addSoftBody(psb); + pdemo->m_cutting=true; } @@ -798,155 +845,6 @@ static void Init_Cutting1(SoftDemo* pdemo) 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; void SoftDemo::clientResetScene() @@ -977,11 +875,6 @@ void SoftDemo::clientResetScene() void (*demofncs[])(SoftDemo*)= { Init_Cloth, - Init_Cutting1, -#ifdef BT_SOFTBODY_USE_STL - Init_TetraBunny, - Init_TetraCube, -#endif //BT_SOFTBODY_USE_STL Init_Pressure, Init_Volume, Init_Ropes, @@ -998,6 +891,7 @@ void SoftDemo::clientResetScene() Init_TorusMatch, Init_Bunny, Init_BunnyMatch, + Init_Cutting1, }; current_demo=current_demo%(sizeof(demofncs)/sizeof(demofncs[0])); @@ -1012,6 +906,7 @@ void SoftDemo::clientResetScene() m_autocam = false; m_raycast = false; m_cutting = false; + m_results.time = SIMD_INFINITY; demofncs[current_demo](this); } @@ -1158,52 +1053,105 @@ void SoftDemo::keyboardCallback(unsigned char key, int x, int y) } // -void SoftDemo::mouseFunc(int button, int state, int x, int y) +void SoftDemo::mouseMotionFunc(int x,int y) { -if(m_cutting&&(state==0)&&(button==0)) +if(m_node&&(m_results.timegetSoftBodyArray(); - btSoftBody::sRayCast results; - results.time=SIMD_INFINITY; - for(int ib=0;ibrayCast(rayFrom,rayDir,res,results.time)) + #define SQ(_x_) (_x_)*(_x_) + if((SQ(x-m_lastmousepos[0])+SQ(y-m_lastmousepos[1]))>6) { - results=res; + m_drag=true; } + #undef SQ } - if(results.timem_faces[results.index]; - btScalar bestarea=SIMD_INFINITY; - const btSoftBody::Node* n[2]={0,0}; - for(int i=2,j=0;j<3;i=j++) - { - const btScalar a2=cross(f.m_n[i]->m_x-x,f.m_n[j]->m_x-x).length2(); - if(a2cutLink(n[0],n[1],0.5); - #endif - ImplicitSphere isphere(rayFrom+rayDir*results.time,1); - printf("Mass before: %f\r\n",results.body->getTotalMass()); - results.body->refine(&isphere,0.0001,true); - printf("Mass after: %f\r\n",results.body->getTotalMass()); - return; + m_lastmousepos[0] = x; + m_lastmousepos[1] = y; } } -DemoApplication::mouseFunc(button,state,x,y); + else + { + DemoApplication::mouseMotionFunc(x,y); + } +} + +// +void SoftDemo::mouseFunc(int button, int state, int x, int y) +{ +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 rayTo=getRayTo(x,y); + const btVector3 rayDir=(rayTo-rayFrom).normalized(); + btSoftBodyArray& sbs=getSoftDynamicsWorld()->getSoftBodyArray(); + for(int ib=0;ibrayCast(rayFrom,rayDir,res,m_results.time)) + { + m_results=res; + } + } + if(m_results.timem_faces[m_results.index]; + m_node=f.m_n[0]; + 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]; + } + } + } + break; + } + if(m_node) m_goal=m_node->m_x; + return; + } + } + } + break; + case 1: + if((!m_drag)&&m_cutting&&(m_results.timegetTotalMass()); + 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); + } } diff --git a/Demos/SoftDemo/SoftDemo.h b/Demos/SoftDemo/SoftDemo.h index 6c465feeb..568d87b3f 100644 --- a/Demos/SoftDemo/SoftDemo.h +++ b/Demos/SoftDemo/SoftDemo.h @@ -57,6 +57,12 @@ public: bool m_raycast; btScalar m_animtime; 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 @@ -80,6 +86,10 @@ public: void exitPhysics(); + SoftDemo() : m_drag(false) + { + + } virtual ~SoftDemo() { exitPhysics(); @@ -118,6 +128,7 @@ public: void renderme(); void keyboardCallback(unsigned char key, int x, int y); void mouseFunc(int button, int state, int x, int y); + void mouseMotionFunc(int x,int y); }; diff --git a/Extras/CDTestFramework/BulletSAPCompleteBoxPruningTest.cpp b/Extras/CDTestFramework/BulletSAPCompleteBoxPruningTest.cpp index e98cf8e7d..1f9bcd99d 100644 --- a/Extras/CDTestFramework/BulletSAPCompleteBoxPruningTest.cpp +++ b/Extras/CDTestFramework/BulletSAPCompleteBoxPruningTest.cpp @@ -22,10 +22,319 @@ subject to the following restrictions: #include "RenderingHelpers.h" #include "GLFontRenderer.h" #include "btBulletCollisionCommon.h" -#include "BulletSoftBody/btDbvtBroadphase.h" +#include "BulletCollision/BroadphaseCollision/btDbvtBroadphase.h" +#include "Camera.h" 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 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 data; + data.resize(buffer.size()); + for(int i=0;i +static int clip(const btVector4* pi,btVector4* po) + { + btScalar s[NP]; + int m=0; + for(int i=0;i0)&&(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 +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(a20) + { + 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=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 +inline bool clipDraw( const btVector4* p, + btScalar minarea) + { + btVector4 o[NP*2]; + const int n=clip(p,o); + bool earlyexit=false; + project(o,n); + for(int i=2;i(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) : mBar (null), mNbBoxes (numBoxes), @@ -39,7 +348,7 @@ BulletSAPCompleteBoxPruningTest::BulletSAPCompleteBoxPruningTest(int numBoxes,in btVector3 aabbMax(200,200,200); int maxNumBoxes = numBoxes; - + m_isdbvt=false; switch (method) { case 1: @@ -101,8 +410,9 @@ BulletSAPCompleteBoxPruningTest::BulletSAPCompleteBoxPruningTest(int numBoxes,in } break; case 7: - m_broadphase = new btDbvtBroadphase(); - methodname = "btDbvtBroadphase"; + m_broadphase = new btDbvtBroadphase(); + m_isdbvt = true; + methodname = "btDbvtBroadphase"; break; default: { @@ -122,11 +432,13 @@ BulletSAPCompleteBoxPruningTest::~BulletSAPCompleteBoxPruningTest() void BulletSAPCompleteBoxPruningTest::Init() { - m_firstTime = true; + btClock clock; + m_firstTime = true; SRand(0); mBoxes = new AABB[mNbBoxes]; + mFlags = new bool[mNbBoxes]; mBoxPtrs = new const AABB*[mNbBoxes]; mBoxTime = new float[mNbBoxes]; for(udword i=0;icreateProxy(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 ); mBoxTime[i] = 2000.0f*UnitRandomFloat(); } + printf("Initialization of %s with %u boxes: %ums\r\n",methodname,mNbBoxes,clock.getTimeMilliseconds()); } void BulletSAPCompleteBoxPruningTest::Release() @@ -166,6 +479,23 @@ void BulletSAPCompleteBoxPruningTest::Select() mBar = TwNewBar("OPC_CompleteBoxPruning"); 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"); + 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); } @@ -198,8 +528,8 @@ bool BulletSAPCompleteBoxPruningTest::UpdateBoxes(int numBoxes) } return true; } -extern int doTree; -extern int percentUpdate; +extern int doTree; +extern int percentUpdate; void BulletSAPCompleteBoxPruningTest::PerformTest() { @@ -239,8 +569,7 @@ void BulletSAPCompleteBoxPruningTest::PerformTest() // printf("%d pairs colliding\r ", mPairs.GetNbPairs()); - bool* Flags = (bool*)_alloca(sizeof(bool)*mNbBoxes); - ZeroMemory(Flags, sizeof(bool)*mNbBoxes); + ZeroMemory(mFlags,sizeof(bool)*mNbBoxes); btOverlappingPairCache* pairCache = m_broadphase->getOverlappingPairCache(); const btBroadphasePair* pairPtr = pairCache->getOverlappingPairArrayPtr(); @@ -249,8 +578,11 @@ void BulletSAPCompleteBoxPruningTest::PerformTest() { // Flags[pairPtr[i].m_pProxy0->getUid()-1] = true; // Flags[pairPtr[i].m_pProxy1->getUid()-1] = true; - Flags[int(pairPtr[i].m_pProxy0->m_clientObject)] = true; - Flags[int(pairPtr[i].m_pProxy1->m_clientObject)] = true; + int j; + 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); + //glDisable(GL_DEPTH_TEST); glDisable(GL_BLEND); // Render boxes - OBB CurrentBox; + Render(); + /*OBB CurrentBox; CurrentBox.mRot.Identity(); for(udword i=0;iqueryOccluder(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) { } diff --git a/Extras/CDTestFramework/CDTestFramework.cpp b/Extras/CDTestFramework/CDTestFramework.cpp index 9d7202894..137bfdabd 100644 --- a/Extras/CDTestFramework/CDTestFramework.cpp +++ b/Extras/CDTestFramework/CDTestFramework.cpp @@ -21,20 +21,15 @@ subject to the following restrictions: #include "CapsuleMeshQuery.h" #include "CompleteBoxPruning.h" #include "BulletSAPCompleteBoxPruningTest.h" -#include "DbvtTest.h" #include "BipartiteBoxPruning.h" #include "RenderingHelpers.h" #include "Terrain.h" #include "Camera.h" #include "GLFontRenderer.h" -///#define NUM_SAP_BOXES 16384 -//#define NUM_SAP_BOXES 8192 -//#define NUM_SAP_BOXES 4096 - #define NUM_SAP_BOXES 8192 -int percentUpdate = 10; +int percentUpdate = 10; //Broadphase comparison //Static case (updating 10% of objects to same position ( -> no swaps) @@ -64,11 +59,11 @@ enum TestIndex // TEST_OBB_MESH_QUERY, // TEST_CAPSULE_MESH_QUERY, // TEST_COMPLETE_BOX_PRUNING=0, - TEST_COMPLETE_BOX_PRUNING_8192, + // TEST_COMPLETE_BOX_PRUNING_8192, // TEST_BULLET_SAP_1024, - TEST_BULLET_SAP_8192, + // TEST_BULLET_SAP_8192, // TEST_BULLET_SAP_SORTEDPAIRS_8192, - TEST_BULLET_MULTISAP_8192, + TEST_BULLET_MULTISAP_8192, // TEST_BIPARTITE_BOX_PRUNING, TEST_DBVT_8192, MAX_NB_TESTS @@ -291,17 +286,17 @@ int main(int argc, char** argv) // {TEST_OBB_MESH_QUERY, "OBB-mesh query"}, // {TEST_CAPSULE_MESH_QUERY, "Capsule-mesh query"}, // {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_8192, "Bullet SAP HASHPAIR 8192"}, + // {TEST_BULLET_SAP_8192, "Bullet SAP HASHPAIR 8192"}, // {TEST_BULLET_SAP_SORTEDPAIRS_8192, "Bullet SAP SORTEDPAIR 8192"}, {TEST_BULLET_MULTISAP_8192, "Bullet MultiSAP 8192"}, // {TEST_BIPARTITE_BOX_PRUNING, "Bipartite box pruning"}, {TEST_DBVT_8192, "DBVT 8192"}, }; TwType testType = TwDefineEnum("CollisionTest", testEV, MAX_NB_TESTS); - TwAddVarRW(gMainBar, "CollisionTests", testType, &gSelectedTest, ""); - TwAddVarRW(gMainBar, "% of updates",TW_TYPE_INT32,&percentUpdate,"min=0 max=100"); + TwAddVarRW(gMainBar, "CollisionTests", testType, &gSelectedTest, ""); + TwAddVarRW(gMainBar, "% of updates",TW_TYPE_INT32,&percentUpdate,"min=0 max=100"); } // Create tests @@ -310,12 +305,12 @@ int main(int argc, char** argv) // gCollisionTests[TEST_OBB_MESH_QUERY] = new OBBMeshQuery; // gCollisionTests[TEST_CAPSULE_MESH_QUERY] = new CapsuleMeshQuery; // 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_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_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_DBVT_8192] = new BulletSAPCompleteBoxPruningTest(NUM_SAP_BOXES,7); @@ -420,4 +415,4 @@ void BuildBulletTree() } -#endif +#endif \ No newline at end of file diff --git a/src/BulletCollision/BroadphaseCollision/btDbvt.cpp b/src/BulletCollision/BroadphaseCollision/btDbvt.cpp index a5de5e7ec..291586a70 100644 --- a/src/BulletCollision/BroadphaseCollision/btDbvt.cpp +++ b/src/BulletCollision/BroadphaseCollision/btDbvt.cpp @@ -17,235 +17,242 @@ subject to the following restrictions: #include "btDbvt.h" // -typedef btAlignedObjectArray tNodeArray; +typedef btAlignedObjectArray tNodeArray; +typedef btAlignedObjectArray tConstNodeArray; + +// +struct btDbvtNodeEnumerator : btDbvt::ICollide +{ +tConstNodeArray nodes; +void Process(const btDbvt::Node* n) { nodes.push_back(n); } +}; // static inline int indexof(const btDbvt::Node* node) { - return(node->parent->childs[1]==node); +return(node->parent->childs[1]==node); } // static inline btDbvt::Volume merge( const btDbvt::Volume& a, - const btDbvt::Volume& b) + const btDbvt::Volume& b) { - btDbvt::Volume res; - Merge(a,b,res); - return(res); +btDbvt::Volume res; +Merge(a,b,res); +return(res); } // volume+edge lengths static inline btScalar size(const btDbvt::Volume& a) { - const btVector3 edges=a.Lengths(); - return( edges.x()*edges.y()*edges.z()+ +const btVector3 edges=a.Lengths(); +return( edges.x()*edges.y()*edges.z()+ edges.x()+edges.y()+edges.z()); } // static inline void deletenode( btDbvt* pdbvt, - btDbvt::Node* node) + btDbvt::Node* node) { - delete pdbvt->m_free; - pdbvt->m_free=node; +btAlignedFree(pdbvt->m_free); +pdbvt->m_free=node; } - + // static inline void recursedeletenode( btDbvt* pdbvt, - btDbvt::Node* node) + btDbvt::Node* node) { - if(!node->isleaf()) +if(!node->isleaf()) { - recursedeletenode(pdbvt,node->childs[0]); - recursedeletenode(pdbvt,node->childs[1]); + recursedeletenode(pdbvt,node->childs[0]); + recursedeletenode(pdbvt,node->childs[1]); } - if(node==pdbvt->m_root) - pdbvt->m_root=0; - deletenode(pdbvt,node); +if(node==pdbvt->m_root) pdbvt->m_root=0; +deletenode(pdbvt,node); } // static inline btDbvt::Node* createnode( btDbvt* pdbvt, - btDbvt::Node* parent, - const btDbvt::Volume& volume, - void* data) + btDbvt::Node* parent, + const btDbvt::Volume& volume, + void* data) { - btDbvt::Node* node; - if(pdbvt->m_free) +btDbvt::Node* node; +if(pdbvt->m_free) { node=pdbvt->m_free;pdbvt->m_free=0; } else - { node=new btDbvt::Node(); } - node->parent = parent; - node->volume = volume; - node->data = data; - node->childs[1] = 0; - return(node); + { node=new(btAlignedAlloc(sizeof(btDbvt::Node),16)) btDbvt::Node(); } +node->parent = parent; +node->volume = volume; +node->data = data; +node->childs[1] = 0; +return(node); } // static inline void insertleaf( btDbvt* pdbvt, - btDbvt::Node* root, - btDbvt::Node* leaf) + btDbvt::Node* root, + btDbvt::Node* leaf) { - if(!pdbvt->m_root) +if(!pdbvt->m_root) { - pdbvt->m_root = leaf; - leaf->parent = 0; + pdbvt->m_root = leaf; + leaf->parent = 0; } else { - if(!root->isleaf()) + if(!root->isleaf()) { - do { - if( Proximity(root->childs[0]->volume,leaf->volume)< - Proximity(root->childs[1]->volume,leaf->volume)) - root=root->childs[0]; + do { + if( Proximity(root->childs[0]->volume,leaf->volume)< + Proximity(root->childs[1]->volume,leaf->volume)) + root=root->childs[0]; else - root=root->childs[1]; + root=root->childs[1]; } while(!root->isleaf()); } - btDbvt::Node* prev=root->parent; - btDbvt::Node* node=createnode(pdbvt,prev,merge(leaf->volume,root->volume),0); - if(prev) + btDbvt::Node* prev=root->parent; + btDbvt::Node* node=createnode(pdbvt,prev,merge(leaf->volume,root->volume),0); + if(prev) { - prev->childs[indexof(root)] = node; - node->childs[0] = root;root->parent=node; - node->childs[1] = leaf;leaf->parent=node; - do { - if(prev->volume.Contain(node->volume)) - break; + prev->childs[indexof(root)] = node; + node->childs[0] = root;root->parent=node; + node->childs[1] = leaf;leaf->parent=node; + do { + if(!prev->volume.Contain(node->volume)) + Merge(prev->childs[0]->volume,prev->childs[1]->volume,prev->volume); else - Merge(prev->childs[0]->volume,prev->childs[1]->volume,prev->volume); - node=prev; + break; + node=prev; } while(0!=(prev=node->parent)); } else { - node->childs[0] = root;root->parent=node; - node->childs[1] = leaf;leaf->parent=node; - pdbvt->m_root = node; + node->childs[0] = root;root->parent=node; + node->childs[1] = leaf;leaf->parent=node; + pdbvt->m_root = node; } } } - + // static inline btDbvt::Node* removeleaf( btDbvt* pdbvt, - btDbvt::Node* leaf) + btDbvt::Node* leaf) { - if(leaf==pdbvt->m_root) +if(leaf==pdbvt->m_root) { - pdbvt->m_root=0; - return(0); + pdbvt->m_root=0; + return(0); } else { - btDbvt::Node* parent=leaf->parent; - btDbvt::Node* prev=parent->parent; - btDbvt::Node* sibling=parent->childs[1-indexof(leaf)]; - if(prev) + btDbvt::Node* parent=leaf->parent; + btDbvt::Node* prev=parent->parent; + btDbvt::Node* sibling=parent->childs[1-indexof(leaf)]; + if(prev) { - prev->childs[indexof(parent)]=sibling; - sibling->parent=prev; - deletenode(pdbvt,parent); - while(prev) + prev->childs[indexof(parent)]=sibling; + sibling->parent=prev; + deletenode(pdbvt,parent); + while(prev) { - const btDbvt::Volume pb=prev->volume; - Merge(prev->childs[0]->volume,prev->childs[1]->volume,prev->volume); - if(NotEqual(pb,prev->volume)) + const btDbvt::Volume pb=prev->volume; + Merge(prev->childs[0]->volume,prev->childs[1]->volume,prev->volume); + if(NotEqual(pb,prev->volume)) { - sibling = prev; - prev = prev->parent; + sibling = prev; + prev = prev->parent; } else break; } - return(prev?prev:pdbvt->m_root); + return(prev?prev:pdbvt->m_root); } else { - pdbvt->m_root=sibling; - sibling->parent=0; - deletenode(pdbvt,parent); - return(pdbvt->m_root); + pdbvt->m_root=sibling; + sibling->parent=0; + deletenode(pdbvt,parent); + return(pdbvt->m_root); } } } // static void fetchleafs( btDbvt* pdbvt, - btDbvt::Node* root, - tNodeArray& leafs, - int depth=-1) + btDbvt::Node* root, + tNodeArray& leafs, + int depth=-1) { - if(root->isinternal()&&depth) +if(root->isinternal()&&depth) { - fetchleafs(pdbvt,root->childs[0],leafs,depth-1); - fetchleafs(pdbvt,root->childs[1],leafs,depth-1); - deletenode(pdbvt,root); + fetchleafs(pdbvt,root->childs[0],leafs,depth-1); + fetchleafs(pdbvt,root->childs[1],leafs,depth-1); + deletenode(pdbvt,root); } else { - leafs.push_back(root); + leafs.push_back(root); } } // static void split( const tNodeArray& leafs, - tNodeArray& left, - tNodeArray& right, - const btVector3& org, - const btVector3& axis) + tNodeArray& left, + tNodeArray& right, + const btVector3& org, + const btVector3& axis) { - left.resize(0); - right.resize(0); - for(int i=0,ni=leafs.size();ivolume.Center()-org)<0) - left.push_back(leafs[i]); + if(dot(axis,leafs[i]->volume.Center()-org)<0) + left.push_back(leafs[i]); else - right.push_back(leafs[i]); + right.push_back(leafs[i]); } } // static btDbvt::Volume bounds( const tNodeArray& leafs) { - btDbvt::Volume volume=leafs[0]->volume; - for(int i=1,ni=leafs.size();ivolume; +for(int i=1,ni=leafs.size();ivolume); + volume=merge(volume,leafs[i]->volume); } - return(volume); +return(volume); } // static void bottomup( btDbvt* pdbvt, - tNodeArray& leafs) + tNodeArray& leafs) { - while(leafs.size()>1) +while(leafs.size()>1) { - btScalar minsize=SIMD_INFINITY; - int minidx[2]={-1,-1}; - for(int i=0;ivolume,leafs[j]->volume)); - if(szvolume,leafs[j]->volume)); + if(szvolume,n[1]->volume),0); - p->childs[0] = n[0]; - p->childs[1] = n[1]; - n[0]->parent = p; - n[1]->parent = p; - leafs[minidx[0]] = p; - leafs.swap(minidx[1],leafs.size()-1); - leafs.pop_back(); + btDbvt::Node* n[] = {leafs[minidx[0]],leafs[minidx[1]]}; + btDbvt::Node* p = createnode(pdbvt,0,merge(n[0]->volume,n[1]->volume),0); + p->childs[0] = n[0]; + p->childs[1] = n[1]; + n[0]->parent = p; + n[1]->parent = p; + leafs[minidx[0]] = p; + leafs.swap(minidx[1],leafs.size()-1); + leafs.pop_back(); } } @@ -254,90 +261,90 @@ static btDbvt::Node* topdown(btDbvt* pdbvt, tNodeArray& leafs, int bu_treshold) { - static const btVector3 axis[]={btVector3(1,0,0), - btVector3(0,1,0), - btVector3(0,0,1)}; - if(leafs.size()>1) +static const btVector3 axis[]={btVector3(1,0,0), + btVector3(0,1,0), + btVector3(0,0,1)}; +if(leafs.size()>1) { - if(leafs.size()>bu_treshold) + if(leafs.size()>bu_treshold) { - const btDbvt::Volume vol=bounds(leafs); - const btVector3 org=vol.Center(); - tNodeArray sets[2]; - int bestaxis=-1; - int bestmidp=leafs.size(); - int splitcount[3][2]={0,0,0,0,0,0}; - for(int i=0;ivolume.Center()-org; - for(int j=0;j<3;++j) + const btVector3 x=leafs[i]->volume.Center()-org; + for(int j=0;j<3;++j) { - ++splitcount[j][dot(x,axis[j])>0?1:0]; + ++splitcount[j][dot(x,axis[j])>0?1:0]; } } - for(int i=0;i<3;++i) + for(int i=0;i<3;++i) { - if((splitcount[i][0]>0)&&(splitcount[i][1]>0)) + if((splitcount[i][0]>0)&&(splitcount[i][1]>0)) { - const int midp=static_cast(btFabs(static_cast(splitcount[i][0]-splitcount[i][1]))); - if(midp=0) + if(bestaxis>=0) { - sets[0].reserve(splitcount[bestaxis][0]); - sets[1].reserve(splitcount[bestaxis][1]); - split(leafs,sets[0],sets[1],org,axis[bestaxis]); + sets[0].reserve(splitcount[bestaxis][0]); + sets[1].reserve(splitcount[bestaxis][1]); + split(leafs,sets[0],sets[1],org,axis[bestaxis]); } else { - sets[0].reserve(leafs.size()/2+1); - sets[1].reserve(leafs.size()/2); - for(int i=0,ni=leafs.size();ichilds[0]=topdown(pdbvt,sets[0],bu_treshold); - node->childs[1]=topdown(pdbvt,sets[1],bu_treshold); - node->childs[0]->parent=node; - node->childs[1]->parent=node; - return(node); + btDbvt::Node* node=createnode(pdbvt,0,vol,0); + node->childs[0]=topdown(pdbvt,sets[0],bu_treshold); + node->childs[1]=topdown(pdbvt,sets[1],bu_treshold); + node->childs[0]->parent=node; + node->childs[1]->parent=node; + return(node); } else { - bottomup(pdbvt,leafs); - return(leafs[0]); + bottomup(pdbvt,leafs); + return(leafs[0]); } } - return(leafs[0]); +return(leafs[0]); } // static inline btDbvt::Node* refit( btDbvt* pdbvt, - btDbvt::Node* node) + btDbvt::Node* node) { - btDbvt::Node* parent=node->parent; - if(parent) +btDbvt::Node* parent=node->parent; +if(parent) { - const int idx=indexof(node); - tNodeArray leafs; - leafs.reserve(64); - fetchleafs(pdbvt,node,leafs,3); - if(leafs.size()>=2) + const int idx=indexof(node); + tNodeArray leafs; + leafs.reserve(64); + fetchleafs(pdbvt,node,leafs,3); + if(leafs.size()>=2) { - bottomup(pdbvt,leafs); - node=leafs[0]; - node->parent=parent; - parent->childs[idx]=node; + bottomup(pdbvt,leafs); + node=leafs[0]; + node->parent=parent; + parent->childs[idx]=node; } } - return(node); +return(node); } // @@ -345,150 +352,174 @@ static inline btDbvt::Node* refit( btDbvt* pdbvt, // // -btDbvt::btDbvt() + btDbvt::btDbvt() { - m_root = 0; - m_free = 0; - m_lkhd = 2; - m_leafs = 0; +m_root = 0; +m_free = 0; +m_lkhd = -1; +m_leafs = 0; +m_opath = 0; } // -btDbvt::~btDbvt() + btDbvt::~btDbvt() { - clear(); +clear(); } // void btDbvt::clear() { - if(m_root) recursedeletenode(this,m_root); - delete m_free; - m_free=0; +if(m_root) recursedeletenode(this,m_root); +btAlignedFree(m_free); +m_free=0; } // void btDbvt::optimizeBottomUp() { - if(m_root) +if(m_root) { - tNodeArray leafs; - leafs.reserve(m_leafs); - fetchleafs(this,m_root,leafs); - bottomup(this,leafs); - m_root=leafs[0]; + tNodeArray leafs; + leafs.reserve(m_leafs); + fetchleafs(this,m_root,leafs); + bottomup(this,leafs); + m_root=leafs[0]; } } // void btDbvt::optimizeTopDown(int bu_treshold) { - if(m_root) +if(m_root) { - tNodeArray leafs; - leafs.reserve(m_leafs); - fetchleafs(this,m_root,leafs); - m_root=topdown(this,leafs,bu_treshold); + tNodeArray leafs; + leafs.reserve(m_leafs); + fetchleafs(this,m_root,leafs); + m_root=topdown(this,leafs,bu_treshold); + } +} + +// +void btDbvt::optimizeIncremental(int passes) +{ +if(m_root&&(passes>0)) + { + do { + Node* node=m_root; + unsigned bit=0; + while(node->isinternal()) + { + node=node->childs[(m_opath>>bit)&1]; + bit=(bit+1)&(sizeof(unsigned)*8-1); + } + update(node); + ++m_opath; + } while(--passes); } } // btDbvt::Node* btDbvt::insert(const Volume& volume,void* data) { - Node* leaf=createnode(this,0,volume,data); - insertleaf(this,m_root,leaf); - ++m_leafs; - return(leaf); +Node* leaf=createnode(this,0,volume,data); +insertleaf(this,m_root,leaf); +++m_leafs; +return(leaf); } // void btDbvt::update(Node* leaf,int lookahead) { - Node* root=removeleaf(this,leaf); - if(root) +Node* root=removeleaf(this,leaf); +if(root) { - for(int i=0;(iparent;++i) + if(lookahead>=0) { + for(int i=0;(iparent;++i) + { root=root->parent; - } + } + } else root=m_root; } - insertleaf(this,root,leaf); +insertleaf(this,root,leaf); } // void btDbvt::update(Node* leaf,const Volume& volume) { - Node* root=removeleaf(this,leaf); - if(root) +Node* root=removeleaf(this,leaf); +if(root) { - for(int i=0;(iparent;++i) + if(m_lkhd>=0) { + for(int i=0;(iparent;++i) + { root=root->parent; - } + } + } else root=m_root; } - leaf->volume=volume; - insertleaf(this,root,leaf); +leaf->volume=volume; +insertleaf(this,root,leaf); } // bool btDbvt::update(Node* leaf,Volume volume,const btVector3& velocity,btScalar margin) { - if(leaf->volume.Contain(volume)) return(false); - volume.Expand(btVector3(margin,margin,margin)); - volume.SignedExpand(velocity); - update(leaf,volume); - return(true); +if(leaf->volume.Contain(volume)) return(false); +volume.Expand(btVector3(margin,margin,margin)); +volume.SignedExpand(velocity); +update(leaf,volume); +return(true); } // bool btDbvt::update(Node* leaf,Volume volume,const btVector3& velocity) { - if(leaf->volume.Contain(volume)) return(false); - volume.SignedExpand(velocity); - update(leaf,volume); - return(true); +if(leaf->volume.Contain(volume)) return(false); +volume.SignedExpand(velocity); +update(leaf,volume); +return(true); } // bool btDbvt::update(Node* leaf,Volume volume,btScalar margin) { - if(leaf->volume.Contain(volume)) return(false); - volume.Expand(btVector3(margin,margin,margin)); - update(leaf,volume); - return(true); +if(leaf->volume.Contain(volume)) return(false); +volume.Expand(btVector3(margin,margin,margin)); +update(leaf,volume); +return(true); } // void btDbvt::remove(Node* leaf) { - removeleaf(this,leaf); - deletenode(this,leaf); - --m_leafs; +removeleaf(this,leaf); +deletenode(this,leaf); +--m_leafs; } // -void btDbvt::collide(btDbvt* tree, - ICollide* icollide) const +void btDbvt::write(IWriter* iwriter) const { - collideGeneric(tree,icollide); -} - -// -void btDbvt::collide(btDbvt::Node* node, - ICollide* icollide) const -{ - collideGeneric(node,icollide); -} - -// -void btDbvt::collide(const Volume& volume, - ICollide* icollide) const -{ - collideGeneric(volume,icollide); -} - -// -void btDbvt::collide(ICollide* icollide) const -{ - collideGeneric(icollide); +btDbvtNodeEnumerator nodes; +nodes.nodes.reserve(m_leafs*2); +enumNodes(m_root,nodes); +iwriter->Prepare(m_root,nodes.nodes.size()); +for(int i=0;iparent) p=nodes.nodes.findLinearSearch(n->parent); + if(n->isinternal()) + { + const int c0=nodes.nodes.findLinearSearch(n->childs[0]); + const int c1=nodes.nodes.findLinearSearch(n->childs[1]); + iwriter->WriteNode(n,i,p,c0,c1); + } + else + { + iwriter->WriteLeaf(n,i,p); + } + } } diff --git a/src/BulletCollision/BroadphaseCollision/btDbvt.h b/src/BulletCollision/BroadphaseCollision/btDbvt.h index f60d79e5c..66ec55521 100644 --- a/src/BulletCollision/BroadphaseCollision/btDbvt.h +++ b/src/BulletCollision/BroadphaseCollision/btDbvt.h @@ -20,6 +20,26 @@ subject to the following restrictions: #include "LinearMath/btAlignedObjectArray.h" #include "LinearMath/btVector3.h" +// +// Compile time configuration +// + +#ifdef WIN32 +#define DBVT_USE_TEMPLATE +#endif + +#ifdef DBVT_USE_TEMPLATE +#define DBVT_VIRTUAL +#define DBVT_PREFIX template +#define DBVT_IPOLICY T& policy +#define DBVT_CHECKTYPE static const ICollide& typechecker=*(T*)0; +#else +#define DBVT_VIRTUAL virtual +#define DBVT_PREFIX +#define DBVT_IPOLICY ICollide& policy +#define DBVT_CHECKTYPE +#endif + // // Defaults volumes // @@ -27,122 +47,165 @@ subject to the following restrictions: /* btDbvtAabbMm */ struct btDbvtAabbMm { - inline btVector3 Center() const { return((mi+mx)/2); } - inline btVector3 Extent() const { return((mx-mi)/2); } - inline const btVector3& Mins() const { return(mi); } - inline const btVector3& Maxs() const { return(mx); } - inline btVector3 Lengths() const { return(mx-mi); } - static inline btDbvtAabbMm FromCE(const btVector3& c,const btVector3& e); - static inline btDbvtAabbMm FromCR(const btVector3& c,btScalar r); - static inline btDbvtAabbMm FromMM(const btVector3& mi,const btVector3& mx); - static inline btDbvtAabbMm FromPoints(const btVector3* pts,int n); - static inline btDbvtAabbMm FromPoints(const btVector3** ppts,int n); - inline void Expand(const btVector3 e); - inline void SignedExpand(const btVector3 e); - inline bool Contain(const btDbvtAabbMm& a) const; - inline friend bool Intersect( const btDbvtAabbMm& a, - const btDbvtAabbMm& b); - inline friend bool Intersect( const btDbvtAabbMm& a, - const btVector3& b); - inline friend btScalar Proximity( const btDbvtAabbMm& a, - const btDbvtAabbMm& b); - inline friend void Merge( const btDbvtAabbMm& a, - const btDbvtAabbMm& b, - btDbvtAabbMm& r); - inline friend bool NotEqual( const btDbvtAabbMm& a, - const btDbvtAabbMm& b); +inline btVector3 Center() const { return((mi+mx)/2); } +inline btVector3 Extent() const { return((mx-mi)/2); } +inline const btVector3& Mins() const { return(mi); } +inline const btVector3& Maxs() const { return(mx); } +inline btVector3 Lengths() const { return(mx-mi); } +static inline btDbvtAabbMm FromCE(const btVector3& c,const btVector3& e); +static inline btDbvtAabbMm FromCR(const btVector3& c,btScalar r); +static inline btDbvtAabbMm FromMM(const btVector3& mi,const btVector3& mx); +static inline btDbvtAabbMm FromPoints(const btVector3* pts,int n); +static inline btDbvtAabbMm FromPoints(const btVector3** ppts,int n); +inline void Expand(const btVector3 e); +inline void SignedExpand(const btVector3 e); +inline bool Contain(const btDbvtAabbMm& a) const; +inline int Classify(const btVector3& n,btScalar o,int s) const; +inline btScalar ProjectMinimum(const btVector3& v,unsigned signs) const; +inline friend bool Intersect( const btDbvtAabbMm& a, + const btDbvtAabbMm& b); +inline friend bool Intersect( const btDbvtAabbMm& a, + const btVector3& b); +inline friend bool Intersect( const btDbvtAabbMm& a, + const btVector3& org, + const btVector3& invdir, + const unsigned* signs); +inline friend btScalar Proximity( const btDbvtAabbMm& a, + const btDbvtAabbMm& b); +inline friend void Merge( const btDbvtAabbMm& a, + const btDbvtAabbMm& b, + btDbvtAabbMm& r); +inline friend bool NotEqual( const btDbvtAabbMm& a, + const btDbvtAabbMm& b); private: - btVector3 mi,mx; +btVector3 mi,mx; }; // // Dynamic bounding volume tree // struct btDbvt -{ + { // Types typedef btDbvtAabbMm Volume; /* Node */ struct Node - { + { Volume volume; Node* parent; bool isleaf() const { return(childs[1]==0); } bool isinternal() const { return(!isleaf()); } union { - Node* childs[2]; - void* data; + Node* childs[2]; + void* data; + }; }; - }; /* Stack element */ - struct sStkElm - { + struct sStkNN + { const Node* a; const Node* b; - sStkElm(const Node* na,const Node* nb) : a(na),b(nb) {} - }; - - // Interfaces - + sStkNN(const Node* na,const Node* nb) : a(na),b(nb) {} + }; + struct sStkNP + { + const Node* node; + unsigned mask; + sStkNP(const Node* n,unsigned m) : node(n),mask(m) {} + }; + struct sStkNPS + { + const Node* node; + unsigned mask; + btScalar value; + sStkNPS(const Node* n,unsigned m,btScalar v) : node(n),mask(m),value(v) {} + }; + // Policies/Interfaces + /* ICollide */ struct ICollide - { - virtual ~ICollide() + { + DBVT_VIRTUAL void Process(const Node*,const Node*) {} + DBVT_VIRTUAL void Process(const Node*) {} + DBVT_VIRTUAL bool Descent(const Node*) { return(true); } + DBVT_VIRTUAL bool AllLeafs(const Node*) { return(true); } + }; + /* IWriter */ + struct IWriter { - } - virtual void Process(const Node*,const Node*)=0; - virtual void Process(const Node*)=0; - virtual bool Descent(const Node*)=0; - }; - + virtual void Prepare(const Node* root,int numnodes)=0; + virtual void WriteNode(const Node*,int index,int parent,int child0,int child1)=0; + virtual void WriteLeaf(const Node*,int index,int parent)=0; + }; + // Constants enum { - TREETREE_STACKSIZE = 128, - VOLUMETREE_STACKSIZE = 64 - }; - + SIMPLE_STACKSIZE = 64, + DOUBLE_STACKSIZE = SIMPLE_STACKSIZE*2, + }; + // Fields Node* m_root; Node* m_free; int m_lkhd; int m_leafs; + unsigned m_opath; // Methods - btDbvt(); - ~btDbvt(); + btDbvt(); + ~btDbvt(); void clear(); bool empty() const { return(0==m_root); } void optimizeBottomUp(); void optimizeTopDown(int bu_treshold=128); + void optimizeIncremental(int passes); Node* insert(const Volume& box,void* data); - void update(Node* leaf,int lookahead=1); + void update(Node* leaf,int lookahead=-1); void update(Node* leaf,const Volume& volume); bool update(Node* leaf,Volume volume,const btVector3& velocity,btScalar margin); bool update(Node* leaf,Volume volume,const btVector3& velocity); bool update(Node* leaf,Volume volume,btScalar margin); void remove(Node* leaf); - void collide(btDbvt* tree, - ICollide* icollide) const; - void collide(btDbvt::Node* node, - ICollide* icollide) const; - void collide(const Volume& volume, - ICollide* icollide) const; - void collide(const btVector3& org, - const btVector3& dir, - ICollide* icollide) const; - void collide(ICollide* icollide) const; - // Generics : T must implement ICollide - - void collideGeneric( btDbvt* tree,ICollide* policy) const; - - void collideGeneric( btDbvt::Node* node,ICollide* policy) const; - - void collideGeneric(const Volume& volume,ICollide* policy) const; - - void collideGeneric(ICollide* policy) const; + void write(IWriter* iwriter) const; + // DBVT_IPOLICY must support ICollide policy/interface + DBVT_PREFIX + static void enumNodes( const Node* root, + DBVT_IPOLICY); + DBVT_PREFIX + static void enumLeafs( const Node* root, + DBVT_IPOLICY); + DBVT_PREFIX + static void collideTT( const Node* root0, + const Node* root1, + DBVT_IPOLICY); + DBVT_PREFIX + static void collideTV( const Node* root, + const Volume& volume, + DBVT_IPOLICY); + DBVT_PREFIX + static void collideRAY( const Node* root, + const btVector3& origin, + const btVector3& direction, + DBVT_IPOLICY); + DBVT_PREFIX + static void collideKDOP(const Node* root, + const btVector3* normals, + const btScalar* offsets, + int count, + DBVT_IPOLICY); + DBVT_PREFIX + static void collideOCL( const Node* root, + const btVector3* normals, + const btScalar* offsets, + const btVector3& sortaxis, + int count, + DBVT_IPOLICY); + DBVT_PREFIX + static void collideTU( const Node* root, + DBVT_IPOLICY); // -private: + private: btDbvt(const btDbvt&) {} -}; + }; // // Inline's @@ -151,69 +214,69 @@ private: // inline btDbvtAabbMm btDbvtAabbMm::FromCE(const btVector3& c,const btVector3& e) { - btDbvtAabbMm box; - box.mi=c-e;box.mx=c+e; - return(box); +btDbvtAabbMm box; +box.mi=c-e;box.mx=c+e; +return(box); } - + // inline btDbvtAabbMm btDbvtAabbMm::FromCR(const btVector3& c,btScalar r) { - return(FromCE(c,btVector3(r,r,r))); +return(FromCE(c,btVector3(r,r,r))); } - + // inline btDbvtAabbMm btDbvtAabbMm::FromMM(const btVector3& mi,const btVector3& mx) { - btDbvtAabbMm box; - box.mi=mi;box.mx=mx; - return(box); +btDbvtAabbMm box; +box.mi=mi;box.mx=mx; +return(box); } - + // inline btDbvtAabbMm btDbvtAabbMm::FromPoints(const btVector3* pts,int n) { - btDbvtAabbMm box; - box.mi=box.mx=pts[0]; - for(int i=1;i0) mx.setX(mx.x()+e.x()); else mi.setX(mi.x()+e.x()); - if(e.y()>0) mx.setY(mx.y()+e.y()); else mi.setY(mi.y()+e.y()); - if(e.z()>0) mx.setZ(mx.z()+e.z()); else mi.setZ(mi.z()+e.z()); +if(e.x()>0) mx.setX(mx.x()+e.x()); else mi.setX(mi.x()+e.x()); +if(e.y()>0) mx.setY(mx.y()+e.y()); else mi.setY(mi.y()+e.y()); +if(e.z()>0) mx.setZ(mx.z()+e.z()); else mi.setZ(mi.z()+e.z()); } - + // inline bool btDbvtAabbMm::Contain(const btDbvtAabbMm& a) const { - return( (mi.x()<=a.mi.x())&& +return( (mi.x()<=a.mi.x())&& (mi.y()<=a.mi.y())&& (mi.z()<=a.mi.z())&& (mx.x()>=a.mx.x())&& @@ -221,24 +284,62 @@ inline bool btDbvtAabbMm::Contain(const btDbvtAabbMm& a) const (mx.z()>=a.mx.z())); } +// +inline int btDbvtAabbMm::Classify(const btVector3& n,btScalar o,int s) const +{ +btVector3 pi,px; +switch(s) + { + case (0+0+0): px=btVector3(mi.x(),mi.y(),mi.z()); + pi=btVector3(mx.x(),mx.y(),mx.z());break; + case (1+0+0): px=btVector3(mx.x(),mi.y(),mi.z()); + pi=btVector3(mi.x(),mx.y(),mx.z());break; + case (0+2+0): px=btVector3(mi.x(),mx.y(),mi.z()); + pi=btVector3(mx.x(),mi.y(),mx.z());break; + case (1+2+0): px=btVector3(mx.x(),mx.y(),mi.z()); + pi=btVector3(mi.x(),mi.y(),mx.z());break; + case (0+0+4): px=btVector3(mi.x(),mi.y(),mx.z()); + pi=btVector3(mx.x(),mx.y(),mi.z());break; + case (1+0+4): px=btVector3(mx.x(),mi.y(),mx.z()); + pi=btVector3(mi.x(),mx.y(),mi.z());break; + case (0+2+4): px=btVector3(mi.x(),mx.y(),mx.z()); + pi=btVector3(mx.x(),mi.y(),mi.z());break; + case (1+2+4): px=btVector3(mx.x(),mx.y(),mx.z()); + pi=btVector3(mi.x(),mi.y(),mi.z());break; + } +if((dot(n,px)+o)<0) return(-1); +if((dot(n,pi)+o)>=0) return(+1); +return(0); +} + +// +inline btScalar btDbvtAabbMm::ProjectMinimum(const btVector3& v,unsigned signs) const +{ +const btVector3* b[]={&mx,&mi}; +const btVector3 p( b[(signs>>0)&1]->x(), + b[(signs>>1)&1]->y(), + b[(signs>>2)&1]->z()); +return(dot(p,v)); +} + // inline bool Intersect( const btDbvtAabbMm& a, - const btDbvtAabbMm& b) + const btDbvtAabbMm& b) { #if 0 - const btScalar mi[]={ b.mx.x()-a.mi.x(), - b.mx.y()-a.mi.y(), - b.mx.z()-a.mi.z()}; - const unsigned* imi=(const unsigned*)mi; - if((imi[0]|imi[1]|imi[2])&0x80000000) return(false); - const btScalar mx[]={ a.mx.x()-b.mi.x(), - a.mx.y()-b.mi.y(), - a.mx.z()-b.mi.z()}; - const unsigned* imx=(const unsigned*)mx; - if((imx[0]|imx[1]|imx[2])&0x80000000) return(false); - return(true); +const btScalar mi[]={ b.mx.x()-a.mi.x(), + b.mx.y()-a.mi.y(), + b.mx.z()-a.mi.z()}; +const unsigned* imi=(const unsigned*)mi; +if((imi[0]|imi[1]|imi[2])&0x80000000) return(false); +const btScalar mx[]={ a.mx.x()-b.mi.x(), + a.mx.y()-b.mi.y(), + a.mx.z()-b.mi.z()}; +const unsigned* imx=(const unsigned*)mx; +if((imx[0]|imx[1]|imx[2])&0x80000000) return(false); +return(true); #else - return( (a.mi.x()<=b.mx.x())&& +return( (a.mi.x()<=b.mx.x())&& (a.mi.y()<=b.mx.y())&& (a.mi.z()<=b.mx.z())&& (a.mx.x()>=b.mi.x())&& @@ -249,9 +350,9 @@ inline bool Intersect( const btDbvtAabbMm& a, // inline bool Intersect( const btDbvtAabbMm& a, - const btVector3& b) + const btVector3& b) { - return( (b.x()>=a.mi.x())&& +return( (b.x()>=a.mi.x())&& (b.y()>=a.mi.y())&& (b.z()>=a.mi.z())&& (b.x()<=a.mx.x())&& @@ -260,28 +361,50 @@ inline bool Intersect( const btDbvtAabbMm& a, } // -inline btScalar Proximity( const btDbvtAabbMm& a, - const btDbvtAabbMm& b) +inline bool Intersect( const btDbvtAabbMm& a, + const btVector3& org, + const btVector3& invdir, + const unsigned* signs) { - const btVector3 d=(a.mi+a.mx)-(b.mi+b.mx); - return(btFabs(d.x())+btFabs(d.y())+btFabs(d.z())); +const btVector3* bounds[2]={&a.mi,&a.mx}; +btScalar txmin=(bounds[ signs[0]]->x()-org[0])*invdir[0]; +btScalar txmax=(bounds[1-signs[0]]->x()-org[0])*invdir[0]; +const btScalar tymin=(bounds[ signs[1]]->y()-org[1])*invdir[1]; +const btScalar tymax=(bounds[1-signs[1]]->y()-org[1])*invdir[1]; +if((txmin>tymax)||(tymin>txmax)) return(false); +if(tymin>txmin) txmin=tymin; +if(tymaxz()-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(tzmax0); +} + +// +inline btScalar Proximity( const btDbvtAabbMm& a, + const btDbvtAabbMm& b) +{ +const btVector3 d=(a.mi+a.mx)-(b.mi+b.mx); +return(btFabs(d.x())+btFabs(d.y())+btFabs(d.z())); } // inline void Merge( const btDbvtAabbMm& a, - const btDbvtAabbMm& b, - btDbvtAabbMm& r) + const btDbvtAabbMm& b, + btDbvtAabbMm& r) { - r=a; - r.mi.setMin(b.mi); - r.mx.setMax(b.mx); +r=a; +r.mi.setMin(b.mi); +r.mx.setMax(b.mx); } // inline bool NotEqual( const btDbvtAabbMm& a, - const btDbvtAabbMm& b) + const btDbvtAabbMm& b) { - return( (a.mi.x()!=b.mi.x())|| +return( (a.mi.x()!=b.mi.x())|| (a.mi.y()!=b.mi.y())|| (a.mi.z()!=b.mi.z())|| (a.mx.x()!=b.mx.x())|| @@ -290,117 +413,330 @@ inline bool NotEqual( const btDbvtAabbMm& a, } // -// Generic's +// Inline's // // -inline void btDbvt::collideGeneric( btDbvt::Node* node,ICollide* policy) const +DBVT_PREFIX +inline void btDbvt::enumNodes( const Node* root, + DBVT_IPOLICY) { - if(m_root&&node) +DBVT_CHECKTYPE +policy.Process(root); +if(root->isinternal()) { - btAlignedObjectArray stack; - stack.reserve(TREETREE_STACKSIZE); - stack.push_back(sStkElm(m_root,node)); - do { - sStkElm p=stack[stack.size()-1]; - stack.pop_back(); - if(p.a==p.b) - { - if(p.a->isinternal()) - { - stack.push_back(sStkElm(p.a->childs[0],p.a->childs[0])); - stack.push_back(sStkElm(p.a->childs[1],p.a->childs[1])); - stack.push_back(sStkElm(p.a->childs[0],p.a->childs[1])); - } - } - else if(Intersect(p.a->volume,p.b->volume)) - { - if(p.a->isinternal()) - { - if(p.b->isinternal()) - { - stack.push_back(sStkElm(p.a->childs[0],p.b->childs[0])); - stack.push_back(sStkElm(p.a->childs[1],p.b->childs[0])); - stack.push_back(sStkElm(p.a->childs[0],p.b->childs[1])); - stack.push_back(sStkElm(p.a->childs[1],p.b->childs[1])); - } - else - { - stack.push_back(sStkElm(p.a->childs[0],p.b)); - stack.push_back(sStkElm(p.a->childs[1],p.b)); - } - } - else - { - if(p.b->isinternal()) - { - stack.push_back(sStkElm(p.a,p.b->childs[0])); - stack.push_back(sStkElm(p.a,p.b->childs[1])); - } - else - { - policy->Process(p.a,p.b); - } - } - } - } while(stack.size()>0); + enumNodes(root->childs[0],policy); + enumNodes(root->childs[1],policy); } } // -inline void btDbvt::collideGeneric( btDbvt* tree,ICollide* policy) const +DBVT_PREFIX +inline void btDbvt::enumLeafs( const Node* root, + DBVT_IPOLICY) { - collideGeneric(tree->m_root,policy); -} - -// -inline void btDbvt::collideGeneric(const Volume& volume,ICollide* policy) const -{ - if(m_root) +DBVT_CHECKTYPE +if(root->isinternal()) { - btAlignedObjectArray stack; - stack.reserve(VOLUMETREE_STACKSIZE); - stack.push_back(m_root); - do { - const Node* n=stack[stack.size()-1]; - stack.pop_back(); - if(Intersect(n->volume,volume)) - { - if(n->isinternal()) - { - stack.push_back(n->childs[0]); - stack.push_back(n->childs[1]); - } - else - { - policy->Process(n); - } - } - } while(stack.size()>0); + enumLeafs(root->childs[0],policy); + enumLeafs(root->childs[1],policy); + } + else + { + policy.Process(root); } } // - -inline void btDbvt::collideGeneric(ICollide* policy) const +DBVT_PREFIX +inline void btDbvt::collideTT( const Node* root0, + const Node* root1, + DBVT_IPOLICY) { - if(m_root) +DBVT_CHECKTYPE +if(root0&&root1) + { + btAlignedObjectArray stack; + stack.reserve(DOUBLE_STACKSIZE); + stack.push_back(sStkNN(root0,root1)); + do { + sStkNN p=stack[stack.size()-1]; + stack.pop_back(); + if(p.a==p.b) + { + if(p.a->isinternal()) + { + stack.push_back(sStkNN(p.a->childs[0],p.a->childs[0])); + stack.push_back(sStkNN(p.a->childs[1],p.a->childs[1])); + stack.push_back(sStkNN(p.a->childs[0],p.a->childs[1])); + } + } + else if(Intersect(p.a->volume,p.b->volume)) + { + if(p.a->isinternal()) + { + if(p.b->isinternal()) + { + stack.push_back(sStkNN(p.a->childs[0],p.b->childs[0])); + stack.push_back(sStkNN(p.a->childs[1],p.b->childs[0])); + stack.push_back(sStkNN(p.a->childs[0],p.b->childs[1])); + stack.push_back(sStkNN(p.a->childs[1],p.b->childs[1])); + } + else + { + stack.push_back(sStkNN(p.a->childs[0],p.b)); + stack.push_back(sStkNN(p.a->childs[1],p.b)); + } + } + else + { + if(p.b->isinternal()) + { + stack.push_back(sStkNN(p.a,p.b->childs[0])); + stack.push_back(sStkNN(p.a,p.b->childs[1])); + } + else + { + policy.Process(p.a,p.b); + } + } + } + } while(stack.size()>0); + } +} + +// +DBVT_PREFIX +inline void btDbvt::collideTV( const Node* root, + const Volume& volume, + DBVT_IPOLICY) +{ +DBVT_CHECKTYPE +if(root) + { + btAlignedObjectArray stack; + stack.reserve(SIMPLE_STACKSIZE); + stack.push_back(root); + do { + const Node* n=stack[stack.size()-1]; + stack.pop_back(); + if(Intersect(n->volume,volume)) + { + if(n->isinternal()) + { + stack.push_back(n->childs[0]); + stack.push_back(n->childs[1]); + } + else + { + policy.Process(n); + } + } + } while(stack.size()>0); + } +} + +// +DBVT_PREFIX +inline void btDbvt::collideRAY( const Node* root, + const btVector3& origin, + const btVector3& direction, + DBVT_IPOLICY) +{ +DBVT_CHECKTYPE +if(root) { - btAlignedObjectArray stack; - stack.reserve(VOLUMETREE_STACKSIZE); - stack.push_back(m_root); - do { - const Node* n=stack[stack.size()-1]; - stack.pop_back(); - if(policy->Descent(n)) + const btVector3 normal=direction.normalized(); + const btVector3 invdir( 1/normal.x(), + 1/normal.y(), + 1/normal.z()); + const unsigned signs[]={ direction.x()<0, + direction.y()<0, + direction.z()<0}; + btAlignedObjectArray stack; + stack.reserve(SIMPLE_STACKSIZE); + stack.push_back(root); + do { + const Node* node=stack[stack.size()-1]; + stack.pop_back(); + if(Intersect(node->volume,origin,invdir,signs)) { - if(n->isinternal()) + if(node->isinternal()) + { + stack.push_back(node->childs[0]); + stack.push_back(node->childs[1]); + } + else + { + policy.Process(node); + } + } + } while(stack.size()); + } +} + +// +DBVT_PREFIX +inline void btDbvt::collideKDOP(const Node* root, + const btVector3* normals, + const btScalar* offsets, + int count, + DBVT_IPOLICY) +{ +DBVT_CHECKTYPE +if(root) + { + const int inside=(1< stack; + int signs[sizeof(unsigned)*8]; + btAssert(count<(sizeof(signs)/sizeof(signs[0]))); + for(int i=0;i=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)&&(ivolume.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< stack; + int signs[sizeof(unsigned)*8]; + btAssert(count<(sizeof(signs)/sizeof(signs[0]))); + for(int i=0;i=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)&&(ivolume.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 stack; + stack.reserve(SIMPLE_STACKSIZE); + stack.push_back(root); + do { + const Node* n=stack[stack.size()-1]; + stack.pop_back(); + if(policy.Descent(n)) + { + if(n->isinternal()) { stack.push_back(n->childs[0]);stack.push_back(n->childs[1]); } else - { policy->Process(n); } + { policy.Process(n); } } } while(stack.size()>0); } } +// +// PP Cleanup +// + +#ifdef DBVT_USE_TEMPLATE + #undef DBVT_USE_TEMPLATE +#endif +#undef DBVT_VIRTUAL +#undef DBVT_PREFIX +#undef DBVT_IPOLICY +#undef DBVT_CHECKTYPE + #endif diff --git a/src/BulletCollision/BroadphaseCollision/btDbvtBroadphase.cpp b/src/BulletCollision/BroadphaseCollision/btDbvtBroadphase.cpp index 3cc091fa9..2a5fc9ceb 100644 --- a/src/BulletCollision/BroadphaseCollision/btDbvtBroadphase.cpp +++ b/src/BulletCollision/BroadphaseCollision/btDbvtBroadphase.cpp @@ -1,19 +1,3 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2007 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ -///btDbvtBroadphase implementation by Nathanael Presson - #include "btDbvtBroadphase.h" // @@ -23,21 +7,21 @@ subject to the following restrictions: #if DBVT_BP_PROFILE #include struct ProfileScope -{ - ProfileScope(btClock& clock,unsigned long& value) { + ProfileScope(btClock& clock,unsigned long& value) + { m_clock=&clock; m_value=&value; m_base=clock.getTimeMicroseconds(); - } + } ~ProfileScope() - { + { (*m_value)+=m_clock->getTimeMicroseconds()-m_base; - } + } btClock* m_clock; unsigned long* m_value; unsigned long m_base; -}; + }; #define SPC(_value_) ProfileScope spc_scope(m_clock,_value_) #else #define SPC(_value_) @@ -47,82 +31,84 @@ struct ProfileScope // Helpers // -// -static inline int hash(unsigned int i,unsigned int j) -{ - int key=static_cast(((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 static inline void listappend(T* item,T*& list) { - item->links[0]=0; - item->links[1]=list; - if(list) list->links[0]=item; - list=item; +item->links[0]=0; +item->links[1]=list; +if(list) list->links[0]=item; +list=item; } // template static inline void listremove(T* item,T*& list) { - if(item->links[0]) item->links[0]->links[1]=item->links[1]; else list=item->links[1]; - if(item->links[1]) item->links[1]->links[0]=item->links[0]; +if(item->links[0]) item->links[0]->links[1]=item->links[1]; else list=item->links[1]; +if(item->links[1]) item->links[1]->links[0]=item->links[0]; } // template static inline int listcount(T* root) { - int n=0; - while(root) { ++n;root=root->links[1]; } - return(n); +int n=0; +while(root) { ++n;root=root->links[1]; } +return(n); } // template static inline void clear(T& value) { - static const T zerodummy; - value=zerodummy; +static const struct ZeroDummy : T {} zerodummy; +value=zerodummy; } // -// Collider +// Colliders // -struct btDbvtBroadphaseCollider : btDbvt::ICollide + +/* Leaf collider */ +struct btDbvtLeafCollider : btDbvt::ICollide { - btDbvtBroadphase* pbp; - int pid; - btDbvtBroadphaseCollider(btDbvtBroadphase* p,int id) : pbp(p),pid(id) {} - - virtual void Process(const btDbvt::Node* /*na*/) +btDbvtBroadphase* pbp; +btDbvtProxy* ppx; + btDbvtLeafCollider(btDbvtBroadphase* p,btDbvtProxy* px) : pbp(p),ppx(px) {} +void Process(const btDbvt::Node* na) { - } - virtual bool Descent(const btDbvt::Node*) - { - return false; - } - - virtual void Process(const btDbvt::Node* na,const btDbvt::Node* nb) - { - btDbvtProxy* pa=(btDbvtProxy*)na->data; - btDbvtProxy* pb=(btDbvtProxy*)nb->data; -#if DBVT_BP_DISCRETPAIRS - if(Intersect(pa->aabb,pb->aabb)) -#endif + const btDbvt::Node* nb=ppx->leaf; + if(nb!=na) { - btBroadphasePair* pp=pbp->m_paircache->addOverlappingPair(pa,pb); - if(pp) - pp->m_userInfo=*(void**)&pid; + btDbvtProxy* pa=(btDbvtProxy*)na->data; + btDbvtProxy* pb=(btDbvtProxy*)nb->data; + #if DBVT_BP_DISCRETPAIRS + if(Intersect(pa->aabb,pb->aabb)) + #endif + { + if(pa>pb) btSwap(pa,pb); + pbp->m_paircache->addOverlappingPair(pa,pb); + } + } + } +}; + +/* Tree collider */ +struct btDbvtTreeCollider : btDbvt::ICollide +{ +btDbvtBroadphase* pbp; + btDbvtTreeCollider(btDbvtBroadphase* p) : pbp(p) {} +void Process(const btDbvt::Node* na,const btDbvt::Node* nb) + { + btDbvtProxy* pa=(btDbvtProxy*)na->data; + btDbvtProxy* pb=(btDbvtProxy*)nb->data; + #if DBVT_BP_DISCRETPAIRS + if(Intersect(pa->aabb,pb->aabb)) + #endif + { + if(pa>pb) btSwap(pa,pb); + pbp->m_paircache->addOverlappingPair(pa,pb); } } }; @@ -132,112 +118,125 @@ struct btDbvtBroadphaseCollider : btDbvt::ICollide // // -btDbvtBroadphase::btDbvtBroadphase() +btDbvtBroadphase::btDbvtBroadphase(btOverlappingPairCache* paircache) { - m_invalidPair = 0; - m_fcursor = 0; - m_dcursor = 0; - m_stageCurrent = 0; - m_fupdates = 1; - m_dupdates = 0; - //m_paircache = new btSortedOverlappingPairCache(); - m_paircache = new btHashedOverlappingPairCache(); - - m_gid = 0; - m_pid = 0; - for(int i=0;i<=STAGECOUNT;++i) +m_releasepaircache = (paircache!=0)?false:true; +m_stageCurrent = 0; +m_fupdates = 1; +m_dupdates = 1; +m_paircache = paircache? + paircache : + new(btAlignedAlloc(sizeof(btHashedOverlappingPairCache),16)) btHashedOverlappingPairCache(); +m_gid = 0; +m_pid = 0; +for(int i=0;i<=STAGECOUNT;++i) { - m_stageRoots[i]=0; + m_stageRoots[i]=0; } #if DBVT_BP_PROFILE - clear(m_profiling); +clear(m_profiling); #endif } // btDbvtBroadphase::~btDbvtBroadphase() { - delete m_paircache; +if(m_releasepaircache) btAlignedFree(m_paircache); } // btBroadphaseProxy* btDbvtBroadphase::createProxy( const btVector3& aabbMin, - const btVector3& aabbMax, - int /*shapeType*/, - void* userPtr, - short int collisionFilterGroup, - short int collisionFilterMask, - btDispatcher* /*dispatcher*/, - void* /*multiSapProxy*/) + const btVector3& aabbMax, + int /*shapeType*/, + void* userPtr, + short int collisionFilterGroup, + short int collisionFilterMask, + btDispatcher* /*dispatcher*/, + void* /*multiSapProxy*/) { - btDbvtProxy* proxy=new btDbvtProxy(userPtr,collisionFilterGroup,collisionFilterMask); - proxy->aabb = btDbvtAabbMm::FromMM(aabbMin,aabbMax); - proxy->leaf = m_sets[0].insert(proxy->aabb,proxy); - proxy->stage = m_stageCurrent; - proxy->m_uniqueId = ++m_gid; - listappend(proxy,m_stageRoots[m_stageCurrent]); - return(proxy); +btDbvtProxy* proxy=new(btAlignedAlloc(sizeof(btDbvtProxy),16)) btDbvtProxy( userPtr, + collisionFilterGroup, + collisionFilterMask); +proxy->aabb = btDbvtAabbMm::FromMM(aabbMin,aabbMax); +proxy->leaf = m_sets[0].insert(proxy->aabb,proxy); +proxy->stage = m_stageCurrent; +proxy->m_uniqueId = ++m_gid; +listappend(proxy,m_stageRoots[m_stageCurrent]); +return(proxy); } // void btDbvtBroadphase::destroyProxy( btBroadphaseProxy* absproxy, - btDispatcher* dispatcher) + btDispatcher* dispatcher) { - btDbvtProxy* proxy=(btDbvtProxy*)absproxy; - if(proxy->stage==STAGECOUNT) - m_sets[1].remove(proxy->leaf); +btDbvtProxy* proxy=(btDbvtProxy*)absproxy; +if(proxy->stage==STAGECOUNT) + m_sets[1].remove(proxy->leaf); else - m_sets[0].remove(proxy->leaf); - listremove(proxy,m_stageRoots[proxy->stage]); - m_paircache->removeOverlappingPairsContainingProxy(proxy,dispatcher); - delete proxy; + m_sets[0].remove(proxy->leaf); +listremove(proxy,m_stageRoots[proxy->stage]); +m_paircache->removeOverlappingPairsContainingProxy(proxy,dispatcher); +btAlignedFree(proxy); } // void btDbvtBroadphase::setAabb( btBroadphaseProxy* absproxy, - const btVector3& aabbMin, - const btVector3& aabbMax, - btDispatcher* /*dispatcher*/) + const btVector3& aabbMin, + const btVector3& aabbMax, + btDispatcher* /*dispatcher*/) { - btDbvtProxy* proxy=(btDbvtProxy*)absproxy; - btDbvtAabbMm aabb=btDbvtAabbMm::FromMM(aabbMin,aabbMax); - if(proxy->stage==STAGECOUNT) +btDbvtProxy* proxy=(btDbvtProxy*)absproxy; +btDbvtAabbMm aabb=btDbvtAabbMm::FromMM(aabbMin,aabbMax); +if(proxy->stage==STAGECOUNT) {/* fixed -> dynamic set */ - m_sets[1].remove(proxy->leaf); - proxy->leaf=m_sets[0].insert(aabb,proxy); - m_fcursor=0; + m_sets[1].remove(proxy->leaf); + proxy->leaf=m_sets[0].insert(aabb,proxy); } else {/* dynamic set */ + if(Intersect(proxy->leaf->volume,aabb)) + {/* Moving */ const btVector3 delta=(aabbMin+aabbMax)/2-proxy->aabb.Center(); - m_sets[0].update(proxy->leaf,aabb,delta*PREDICTED_FRAMES,DBVT_BP_MARGIN); + #ifdef DBVT_BP_MARGIN + m_sets[0].update(proxy->leaf,aabb,delta*PREDICTED_FRAMES,DBVT_BP_MARGIN); + #else + m_sets[0].update(proxy->leaf,aabb,delta*PREDICTED_FRAMES); + #endif + } + else + {/* Teleporting */ + m_sets[0].update(proxy->leaf,aabb); + } } - listremove(proxy,m_stageRoots[proxy->stage]); - proxy->aabb = aabb; - proxy->stage = m_stageCurrent; - listappend(proxy,m_stageRoots[m_stageCurrent]); +listremove(proxy,m_stageRoots[proxy->stage]); +proxy->aabb = aabb; +proxy->stage = m_stageCurrent; +listappend(proxy,m_stageRoots[m_stageCurrent]); } // void btDbvtBroadphase::calculateOverlappingPairs(btDispatcher* dispatcher) { - collide(dispatcher); +collide(dispatcher); #if DBVT_BP_PROFILE - if(0==(m_pid%DBVT_BP_PROFILING_RATE)) +if(0==(m_pid%DBVT_BP_PROFILING_RATE)) { - printf("fixed(%u) dynamics(%u) pairs(%u)\r\n",m_sets[1].m_leafs,m_sets[0].m_leafs,m_paircache->getNumOverlappingPairs()); - unsigned int total=m_profiling.m_total; - if(total<=0) total=1; - printf("ddcollide: %u%% (%uus)\r\n",(50+m_profiling.m_ddcollide*100)/total,m_profiling.m_ddcollide/DBVT_BP_PROFILING_RATE); - printf("fdcollide: %u%% (%uus)\r\n",(50+m_profiling.m_fdcollide*100)/total,m_profiling.m_fdcollide/DBVT_BP_PROFILING_RATE); - printf("cleanup: %u%% (%uus)\r\n",(50+m_profiling.m_cleanup*100)/total,m_profiling.m_cleanup/DBVT_BP_PROFILING_RATE); - printf("total: %uus\r\n",total/DBVT_BP_PROFILING_RATE); - const unsigned long sum=m_profiling.m_ddcollide+ - m_profiling.m_fdcollide+ - m_profiling.m_cleanup; - printf("leaked: %u%% (%uus)\r\n",100-((50+sum*100)/total),(total-sum)/DBVT_BP_PROFILING_RATE); - clear(m_profiling); - m_clock.reset(); + printf("fixed(%u) dynamics(%u) pairs(%u)\r\n",m_sets[1].m_leafs,m_sets[0].m_leafs,m_paircache->getNumOverlappingPairs()); + printf("mode: %s\r\n",m_mode==MODE_FULL?"full":"incremental"); + printf("cleanup: %s\r\n",m_cleanupmode==CLEANUP_FULL?"full":"incremental"); + unsigned int total=m_profiling.m_total; + if(total<=0) total=1; + printf("ddcollide: %u%% (%uus)\r\n",(50+m_profiling.m_ddcollide*100)/total,m_profiling.m_ddcollide/DBVT_BP_PROFILING_RATE); + printf("fdcollide: %u%% (%uus)\r\n",(50+m_profiling.m_fdcollide*100)/total,m_profiling.m_fdcollide/DBVT_BP_PROFILING_RATE); + printf("cleanup: %u%% (%uus)\r\n",(50+m_profiling.m_cleanup*100)/total,m_profiling.m_cleanup/DBVT_BP_PROFILING_RATE); + printf("total: %uus\r\n",total/DBVT_BP_PROFILING_RATE); + const unsigned long sum=m_profiling.m_ddcollide+ + m_profiling.m_fdcollide+ + m_profiling.m_cleanup; + printf("leaked: %u%% (%uus)\r\n",100-((50+sum*100)/total),(total-sum)/DBVT_BP_PROFILING_RATE); + printf("job counts: %u%%\r\n",(m_profiling.m_jobcount*100)/((m_sets[0].m_leafs+m_sets[1].m_leafs)*DBVT_BP_PROFILING_RATE)); + clear(m_profiling); + m_clock.reset(); } #endif } @@ -245,194 +244,88 @@ void btDbvtBroadphase::calculateOverlappingPairs(btDispatcher* dispatcher) // void btDbvtBroadphase::collide(btDispatcher* dispatcher) { - SPC(m_profiling.m_total); - /* refine dynamic */ - if(m_stageRoots[m_stageCurrent]&&(m_dupdates>0)) +SPC(m_profiling.m_total); +/* optimize */ +m_sets[0].optimizeIncremental(1+(m_sets[0].m_leafs*m_dupdates)/100); +m_sets[1].optimizeIncremental(1+(m_sets[1].m_leafs*m_fupdates)/100); +/* dynamic -> fixed set */ +m_stageCurrent=(m_stageCurrent+1)%STAGECOUNT; +btDbvtProxy* current=m_stageRoots[m_stageCurrent]; +if(current) { - const int count=1+(m_sets[0].m_leafs*m_dupdates)/100; - for(int i=0;ileaf); - m_dcursor=m_dcursor->links[1]; - } - } - /* dynamic -> fixed set */ - m_stageCurrent=(m_stageCurrent+1)%STAGECOUNT; - btDbvtProxy* current=m_stageRoots[m_stageCurrent]; - if(current) - { - btDbvtBroadphaseCollider collider(this,m_pid); - do { - btDbvtProxy* next=current->links[1]; - if(m_dcursor==current) m_dcursor=0; - listremove(current,m_stageRoots[current->stage]); - listappend(current,m_stageRoots[STAGECOUNT]); - m_sets[1].collideGeneric(current->leaf,&collider); - m_sets[0].remove(current->leaf); - current->leaf = m_sets[1].insert(current->aabb,current); - current->stage = STAGECOUNT; - current = next; + btDbvtTreeCollider collider(this); + do { + btDbvtProxy* next=current->links[1]; + listremove(current,m_stageRoots[current->stage]); + listappend(current,m_stageRoots[STAGECOUNT]); + btDbvt::collideTT(m_sets[1].m_root,current->leaf,collider); + m_sets[0].remove(current->leaf); + current->leaf = m_sets[1].insert(current->aabb,current); + current->stage = STAGECOUNT; + current = next; } while(current); } - /* refine fixed */ - if(m_stageRoots[STAGECOUNT]&&(m_fupdates>0)) +/* collide dynamics */ { - const int count=1+(m_sets[1].m_leafs*m_fupdates)/100; - for(int i=0;ileaf); - m_fcursor=m_fcursor->links[1]; - } - } - /* collide dynamics */ - btDbvtBroadphaseCollider collider(this,m_pid); - { SPC(m_profiling.m_fdcollide); - m_sets[0].collideGeneric(&m_sets[1],&collider); - } - { + btDbvt::collideTT(m_sets[0].m_root,m_sets[1].m_root,collider); + } + { SPC(m_profiling.m_ddcollide); - m_sets[0].collideGeneric(&m_sets[0],&collider); + btDbvt::collideTT(m_sets[0].m_root,m_sets[0].m_root,collider); + } } - /* clean up */ - ///m_paircache->processAllOverlappingPairs(0,dispatcher); +/* clean up */ { - SPC(m_profiling.m_cleanup); - if (!m_paircache->hasDeferredRemoval()) + SPC(m_profiling.m_cleanup); + btBroadphasePairArray& pairs=m_paircache->getOverlappingPairArray(); + if(pairs.size()>0) { - btBroadphasePairArray& pairs=m_paircache->getOverlappingPairArray(); - for(int i=0,ni=pairs.size();iaabb,pb->aabb)) { - btDbvtProxy* pa=(btDbvtProxy*)p.m_pProxy0; - btDbvtProxy* pb=(btDbvtProxy*)p.m_pProxy1; - if(!Intersect(pa->aabb,pb->aabb)) - { - m_paircache->removeOverlappingPair(pa,pb,dispatcher); - --ni;--i; - } + if(pa>pb) btSwap(pa,pb); + m_paircache->removeOverlappingPair(pa,pb,dispatcher); + --ni;--i; } } - } else - { - if ( m_paircache->hasDeferredRemoval()) - { - - btBroadphasePairArray& overlappingPairArray = m_paircache->getOverlappingPairArray(); - - //perform a sort, to find duplicates and to sort 'invalid' pairs to the end - overlappingPairArray.quickSort(btBroadphasePairSortPredicate()); - - overlappingPairArray.resize(overlappingPairArray.size() - m_invalidPair); - m_invalidPair = 0; - - - btBroadphasePair previousPair; - previousPair.m_pProxy0 = 0; - previousPair.m_pProxy1 = 0; - previousPair.m_algorithm = 0; - - - int i; - for (i=0;iaabb,pb->aabb); - - if (hasOverlap) - { - needsRemoval = false;//callback->processOverlap(pair); - } else - { - needsRemoval = true; - } - } else - { - //remove duplicate - needsRemoval = true; - //should have no algorithm - btAssert(!pair.m_algorithm); - } - - if (needsRemoval) - { - m_paircache->cleanOverlappingPair(pair,dispatcher); - - // m_overlappingPairArray.swap(i,m_overlappingPairArray.size()-1); - // m_overlappingPairArray.pop_back(); - pair.m_pProxy0 = 0; - pair.m_pProxy1 = 0; - m_invalidPair++; - - } - - } - - ///if you don't like to skip the invalid pairs in the array, execute following code: - #define CLEAN_INVALID_PAIRS 1 - #ifdef CLEAN_INVALID_PAIRS - - //perform a sort, to sort 'invalid' pairs to the end - overlappingPairArray.quickSort(btBroadphasePairSortPredicate()); - - overlappingPairArray.resize(overlappingPairArray.size() - m_invalidPair); - m_invalidPair = 0; - #endif//CLEAN_INVALID_PAIRS - - } } - - } - - - ++m_pid; +++m_pid; } // btOverlappingPairCache* btDbvtBroadphase::getOverlappingPairCache() { - return(m_paircache); +return(m_paircache); } // const btOverlappingPairCache* btDbvtBroadphase::getOverlappingPairCache() const { - return(m_paircache); +return(m_paircache); } // void btDbvtBroadphase::getBroadphaseAabb(btVector3& aabbMin,btVector3& aabbMax) const { - btDbvtAabbMm bounds; - if(!m_sets[0].empty()) - if(!m_sets[1].empty()) Merge( m_sets[0].m_root->volume, - m_sets[1].m_root->volume,bounds); - else - bounds=m_sets[0].m_root->volume; - else if(!m_sets[1].empty()) bounds=m_sets[1].m_root->volume; - else - bounds=btDbvtAabbMm::FromCR(btVector3(0,0,0),0); - aabbMin=bounds.Mins(); - aabbMax=bounds.Maxs(); +btDbvtAabbMm bounds; +if(!m_sets[0].empty()) + if(!m_sets[1].empty()) Merge( m_sets[0].m_root->volume, + m_sets[1].m_root->volume,bounds); + else + bounds=m_sets[0].m_root->volume; +else if(!m_sets[1].empty()) bounds=m_sets[1].m_root->volume; + else + bounds=btDbvtAabbMm::FromCR(btVector3(0,0,0),0); +aabbMin=bounds.Mins(); +aabbMax=bounds.Maxs(); } // @@ -441,4 +334,4 @@ void btDbvtBroadphase::printStats() #if DBVT_BP_PROFILE #undef SPC -#endif +#endif \ No newline at end of file diff --git a/src/BulletCollision/BroadphaseCollision/btDbvtBroadphase.h b/src/BulletCollision/BroadphaseCollision/btDbvtBroadphase.h index 4406499b4..d78e13394 100644 --- a/src/BulletCollision/BroadphaseCollision/btDbvtBroadphase.h +++ b/src/BulletCollision/BroadphaseCollision/btDbvtBroadphase.h @@ -13,31 +13,23 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ ///btDbvtBroadphase implementation by Nathanael Presson - #ifndef BT_DBVT_BROADPHASE_H #define BT_DBVT_BROADPHASE_H -#include "btDbvt.h" -#include "LinearMath/btPoint3.h" -#include "LinearMath/btVector3.h" +#include "BulletCollision/BroadphaseCollision/btDbvt.h" #include "BulletCollision/BroadphaseCollision/btOverlappingPairCache.h" -#include "BulletCollision/BroadphaseCollision/btBroadphaseInterface.h" -#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" -#include "BulletCollision/BroadphaseCollision/btOverlappingPairCallback.h" -#include "BulletCollision/BroadphaseCollision/btOverlappingPairCallback.h" // // Compile time config // -//#define DBVT_BP_PROFILE 1 - -#define DBVT_BP_DISCRETPAIRS 0 +#define DBVT_BP_PROFILE 0 +#define DBVT_BP_DISCRETPAIRS 1 #define DBVT_BP_MARGIN (btScalar)0.05 #if DBVT_BP_PROFILE -#define DBVT_BP_PROFILING_RATE 50 -#include "LinearMath/btQuickprof.h" + #define DBVT_BP_PROFILING_RATE 256 + #include "LinearMath/btQuickprof.h" #endif // @@ -45,66 +37,66 @@ subject to the following restrictions: // struct btDbvtProxy : btBroadphaseProxy { - /* Fields */ - btDbvtAabbMm aabb; - btDbvt::Node* leaf; - btDbvtProxy* links[2]; - int stage; - /* ctor */ - btDbvtProxy(void* userPtr,short int collisionFilterGroup, short int collisionFilterMask) : +/* Fields */ +btDbvtAabbMm aabb; +btDbvt::Node* leaf; +btDbvtProxy* links[2]; +int stage; +/* ctor */ +btDbvtProxy(void* userPtr,short int collisionFilterGroup, short int collisionFilterMask) : btBroadphaseProxy(userPtr,collisionFilterGroup,collisionFilterMask) { - links[0]=links[1]=0; + links[0]=links[1]=0; } }; +typedef btAlignedObjectArray btDbvtProxyArray; + // // btDbvtBroadphase // struct btDbvtBroadphase : btBroadphaseInterface { - /* Config */ - enum { +/* Config */ +enum { DYNAMIC_SET = 0, /* Dynamic set index */ FIXED_SET = 1, /* Fixed set index */ STAGECOUNT = 2, /* Number of stages */ - PREDICTED_FRAMES = 2 /* Frames prediction */ - }; - /* Fields */ - btDbvt m_sets[2]; // Dbvt sets - btDbvtProxy* m_stageRoots[STAGECOUNT+1]; // Stages list - int m_stageCurrent; // Current stage - btOverlappingPairCache* m_paircache; // Pair cache - btDbvtProxy* m_fcursor; // Current fixed cursor - btDbvtProxy* m_dcursor; // Current dynamic cursor - int m_fupdates; // % of fixed updates per frame - int m_dupdates; // % of dynamic updates per frame - int m_pid; // Parse id - int m_gid; // Gen id - int m_invalidPair; - + PREDICTED_FRAMES = 2 /* Frames prediction */ + }; +/* Fields */ +btDbvt m_sets[2]; // Dbvt sets +btDbvtProxy* m_stageRoots[STAGECOUNT+1]; // Stages list +btOverlappingPairCache* m_paircache; // Pair cache +int m_stageCurrent; // Current stage +int m_fupdates; // % of fixed updates per frame +int m_dupdates; // % of dynamic updates per frame +int m_pid; // Parse id +int m_gid; // Gen id +bool m_releasepaircache; // Release pair cache on delete #if DBVT_BP_PROFILE - btClock m_clock; - struct { +btClock m_clock; +struct { unsigned long m_total; unsigned long m_ddcollide; unsigned long m_fdcollide; unsigned long m_cleanup; - } m_profiling; + unsigned long m_jobcount; + } m_profiling; #endif - /* Methods */ - btDbvtBroadphase(); - ~btDbvtBroadphase(); - void collide(btDispatcher* dispatcher); - /* btBroadphaseInterface Implementation */ - btBroadphaseProxy* createProxy(const btVector3& aabbMin,const btVector3& aabbMax,int shapeType,void* userPtr,short int collisionFilterGroup,short int collisionFilterMask,btDispatcher* dispatcher,void* multiSapProxy); - void destroyProxy(btBroadphaseProxy* proxy,btDispatcher* dispatcher); - void setAabb(btBroadphaseProxy* proxy,const btVector3& aabbMin,const btVector3& aabbMax,btDispatcher* dispatcher); - void calculateOverlappingPairs(btDispatcher* dispatcher); - btOverlappingPairCache* getOverlappingPairCache(); - const btOverlappingPairCache* getOverlappingPairCache() const; - void getBroadphaseAabb(btVector3& aabbMin,btVector3& aabbMax) const; - void printStats(); +/* Methods */ +btDbvtBroadphase(btOverlappingPairCache* paircache=0); +~btDbvtBroadphase(); +void collide(btDispatcher* dispatcher); +/* btBroadphaseInterface Implementation */ +btBroadphaseProxy* createProxy(const btVector3& aabbMin,const btVector3& aabbMax,int shapeType,void* userPtr,short int collisionFilterGroup,short int collisionFilterMask,btDispatcher* dispatcher,void* multiSapProxy); +void destroyProxy(btBroadphaseProxy* proxy,btDispatcher* dispatcher); +void setAabb(btBroadphaseProxy* proxy,const btVector3& aabbMin,const btVector3& aabbMax,btDispatcher* dispatcher); +void calculateOverlappingPairs(btDispatcher* dispatcher); +btOverlappingPairCache* getOverlappingPairCache(); +const btOverlappingPairCache* getOverlappingPairCache() const; +void getBroadphaseAabb(btVector3& aabbMin,btVector3& aabbMax) const; +void printStats(); }; #endif diff --git a/src/BulletSoftBody/btSoftBody.cpp b/src/BulletSoftBody/btSoftBody.cpp index b4836be54..aea0e7a69 100644 --- a/src/BulletSoftBody/btSoftBody.cpp +++ b/src/BulletSoftBody/btSoftBody.cpp @@ -15,13 +15,7 @@ subject to the following restrictions: ///btSoftBody implementation by Nathanael Presson #include "btSoftBody.h" -#if 1 -#include -#define DOTRACE -#endif -#include #include "LinearMath/btQuickprof.h" -#include "BulletCollision/NarrowPhaseCollision/btGjkEpa2.h" #include "BulletCollision/BroadphaseCollision/btBroadphaseInterface.h" #include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h" @@ -136,17 +130,6 @@ void btSoftBodyCollisionShape::processAllTriangles(btTriangleCallback* /*callbac // // -#ifdef DOTRACE -static inline void Trace(const btMatrix3x3& m,const char* name) -{ - printf("%s[0]: %.2f,\t%.2f,\t%.2f\r\n",name,m[0].x(),m[0].y(),m[0].z()); - printf("%s[1]: %.2f,\t%.2f,\t%.2f\r\n",name,m[1].x(),m[1].y(),m[1].z()); - printf("%s[2]: %.2f,\t%.2f,\t%.2f\r\n",name,m[2].x(),m[2].y(),m[2].z()); - printf("\r\n"); -} -#else -static inline void Trace(const btMatrix3x3&,const char*) {} -#endif // template @@ -199,6 +182,10 @@ template static inline T Sign(const T& x) { return((T)(x<0?-1:+1)); } // +template +static inline bool SameSign(const T& x,const T& y) +{ return((x*y)>0); } +// static inline btMatrix3x3 ScaleAlongAxis(const btVector3& a,btScalar s) { const btScalar xx=a.x()*a.x(); @@ -456,17 +443,6 @@ static void PointersToIndices(btSoftBody* psb) psb->m_faces[i].m_leaf->data=*(void**)&i; } } - for(int i=0,ni=psb->m_tetras.size();im_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();im_anchors[i].m_node=PTR2IDX(psb->m_anchors[i].m_node,base); @@ -509,17 +485,6 @@ static void IndicesToPointers(btSoftBody* psb,const int* map=0) psb->m_faces[i].m_leaf->data=&psb->m_faces[i]; } } - for(int i=0,ni=psb->m_tetras.size();im_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();im_anchors[i].m_node=IDX2PTR(psb->m_anchors[i].m_node,base); @@ -605,11 +570,10 @@ static inline btScalar RayTriangle(const btVector3& org, // Private implementation // -struct RayCaster : public btDbvt::ICollide +struct RayCaster : btDbvt::ICollide { btVector3 o; btVector3 d; - btVector3 nd; btScalar mint; btSoftBody::Face* face; int tests; @@ -617,18 +581,11 @@ struct RayCaster : public btDbvt::ICollide { o = org; d = dir; - nd = dir.normalized(); mint = mxt; face = 0; tests = 0; } - - virtual void Process(const btDbvt::Node* /*a*/,const btDbvt::Node* /*b*/) - { - - } - - virtual void Process(const btDbvt::Node* leaf) + void Process(const btDbvt::Node* leaf) { btSoftBody::Face& f=*(btSoftBody::Face*)leaf->data; const btScalar t=RayTriangle( o,d, @@ -643,13 +600,6 @@ struct RayCaster : public btDbvt::ICollide } ++tests; } - virtual bool Descent(const btDbvt::Node* node) - { - const btVector3 ctr=node->volume.Center()-o; - const btScalar sqr=node->volume.Lengths().length2()/4; - const btScalar prj=dot(ctr,nd); - return((ctr-(nd*prj)).length2()<=sqr); - } }; @@ -665,55 +615,22 @@ static int RaycastInternal(const btSoftBody* psb, int cnt=0; if(bcountonly||psb->m_fdbvt.empty()) {/* Full search */ - if(!psb->m_faces.size()) - {/* Tetras */ - for(int i=0,ni=psb->m_tetras.size();im_tetras[i]; - const btSoftBody::Node* const* n=t.m_n; - const int f[]={ 0,1,2, - 0,1,3, - 1,2,3, - 2,0,3}; - for(int j=0;j<12;j+=3) - { - const btScalar t=RayTriangle( org,dir, - n[f[j+0]]->m_x, - n[f[j+1]]->m_x, - n[f[j+2]]->m_x, + for(int i=0,ni=psb->m_faces.size();im_faces[i]; + const btScalar t=RayTriangle( org,dir, + f.m_n[0]->m_x, + f.m_n[1]->m_x, + f.m_n[2]->m_x, mint); - if(t>0) - { - ++cnt; - if(!bcountonly) - { - feature=btSoftBody::eFeature::Tetra; - index=-1; - mint=t; - } - } - } - } - } - else - {/* Faces */ - for(int i=0,ni=psb->m_faces.size();i0) { - const btSoftBody::Face& f=psb->m_faces[i]; - const btScalar t=RayTriangle( org,dir, - f.m_n[0]->m_x, - f.m_n[1]->m_x, - f.m_n[2]->m_x, - mint); - if(t>0) + ++cnt; + if(!bcountonly) { - ++cnt; - if(!bcountonly) - { - feature=btSoftBody::eFeature::Face; - index=i; - mint=t; - } + feature=btSoftBody::eFeature::Face; + index=i; + mint=t; } } } @@ -721,7 +638,7 @@ static int RaycastInternal(const btSoftBody* psb, else {/* Use dbvt */ RayCaster collider(org,dir,mint); - psb->m_fdbvt.collideGeneric(&collider); + btDbvt::collideRAY(psb->m_fdbvt.m_root,org,dir,collider); if(collider.face) { mint=collider.mint; @@ -742,7 +659,6 @@ for(int i=0;im_faces.size();++i) btSoftBody::Face& f=psb->m_faces[i]; f.m_leaf=psb->m_fdbvt.insert(VolumeOf(f,0),&f); } -psb->m_fdbvt.optimizeTopDown(); } // @@ -910,15 +826,6 @@ static void UpdateConstants(btSoftBody* psb) btSoftBody::Face& f=psb->m_faces[i]; f.m_ra = AreaOf(f.m_n[0]->m_x,f.m_n[1]->m_x,f.m_n[2]->m_x); } - /* Tetras */ - for(int i=0,ni=psb->m_tetras.size();im_tetras[i]; - btSoftBody::Material& m=*t.m_material; - btSoftBody::Node** n=t.m_n; - t.m_rv = VolumeOf(n[0]->m_x,n[1]->m_x,n[2]->m_x,n[3]->m_x); - t.m_c1 = (4*m.m_kVST)/(n[0]->m_im+n[1]->m_im+n[2]->m_im+n[3]->m_im); - } /* Area's */ btAlignedObjectArray counts; counts.resize(psb->m_nodes.size(),0); @@ -1081,9 +988,9 @@ static void ApplyForces(btSoftBody* psb,btScalar dt) } // -static void PSolve_Anchors(btSoftBody* psb) +static void PSolve_Anchors(btSoftBody* psb,btScalar kst) { - const btScalar kAHR=psb->m_cfg.kAHR; + const btScalar kAHR=psb->m_cfg.kAHR*kst; const btScalar dt=psb->m_sst.sdt; for(int i=0,ni=psb->m_anchors.size();im_sst.sdt; + const btScalar mrg=psb->getCollisionShape()->getMargin(); for(int i=0,ni=psb->m_rcontacts.size();im_rcontacts[i]; @@ -1114,9 +1022,9 @@ static void PSolve_RContacts(btSoftBody* psb) const btScalar dn=dot(vr,cti.m_normal); if(dn<=SIMD_EPSILON) { - const btScalar dp=dot(c.m_node->m_x,cti.m_normal)+cti.m_offset; + const btScalar dp=btMin(dot(c.m_node->m_x,cti.m_normal)+cti.m_offset,mrg); const btVector3 fv=vr-cti.m_normal*dn; - const btVector3 impulse=c.m_c0*(vr-fv*c.m_c3+cti.m_normal*(dp*c.m_c4)); + const btVector3 impulse=c.m_c0*((vr-fv*c.m_c3+cti.m_normal*(dp*c.m_c4))*kst); c.m_node->m_x-=impulse*c.m_c2; c.m_cti.m_body->applyImpulse(impulse,c.m_c1); } @@ -1124,7 +1032,7 @@ static void PSolve_RContacts(btSoftBody* psb) } // -static void PSolve_SContacts(btSoftBody* psb) +static void PSolve_SContacts(btSoftBody* psb,btScalar) { for(int i=0,ni=psb->m_scontacts.size();im_scontacts.size();im_links.size();im_tetras.size();im_tetras[i]; - btSoftBody::Node** n=t.m_n; - const btVector3 g0=cross(n[3]->m_x-n[1]->m_x,n[2]->m_x-n[1]->m_x); - const btVector3 g1=cross(n[3]->m_x-n[2]->m_x,n[0]->m_x-n[2]->m_x); - const btVector3 g2=cross(n[3]->m_x-n[0]->m_x,n[1]->m_x-n[0]->m_x); - const btVector3 g3=cross(n[1]->m_x-n[0]->m_x,n[2]->m_x-n[0]->m_x); - const btScalar v1=VolumeOf(n[0]->m_x,n[1]->m_x,n[2]->m_x,n[3]->m_x); - const btScalar k=(t.m_rv-v1)/(g0.length2()+g1.length2()+g2.length2()+g3.length2()); - const btScalar j=k*t.m_c1; - n[0]->m_x += g0*j*n[0]->m_im; - n[1]->m_x += g1*j*n[1]->m_im; - n[2]->m_x += g2*j*n[2]->m_im; - n[3]->m_x += g3*j*n[3]->m_im; - } -} - -// -static void VSolve_Links(btSoftBody* psb) +static void VSolve_Links(btSoftBody* psb,btScalar kst) { for(int i=0,ni=psb->m_links.size();im_links[i]; btSoftBody::Node** n=l.m_n; - const btScalar j=-dot(l.m_c3,n[0]->m_v-n[1]->m_v)*l.m_c2; + const btScalar j=-dot(l.m_c3,n[0]->m_v-n[1]->m_v)*l.m_c2*kst; n[0]->m_v+= l.m_c3*(j*n[0]->m_im); n[1]->m_v-= l.m_c3*(j*n[1]->m_im); } } // -static void VSolve_Tetras(btSoftBody* psb) -{ -for(int i=0,ni=psb->m_tetras.size();im_tetras[i]; - btSoftBody::Node** n=t.m_n; - const btScalar r= dot(t.m_c0[0],n[0]->m_v)+ - dot(t.m_c0[1],n[1]->m_v)+ - dot(t.m_c0[2],n[2]->m_v)+ - dot(t.m_c0[3],n[3]->m_v); - const btScalar j=r*t.m_c2; - n[0]->m_v += t.m_c0[0]*(j*n[0]->m_im); - n[1]->m_v += t.m_c0[1]*(j*n[1]->m_im); - n[2]->m_v += t.m_c0[2]*(j*n[2]->m_im); - n[3]->m_v += t.m_c0[3]*(j*n[3]->m_im); - } -} - -// -static void (* const VSolvers[])(btSoftBody*)= { - VSolve_Links, - VSolve_Tetras, - }; +static void (* const VSolvers[])(btSoftBody*,btScalar)= { + VSolve_Links, + }; // -static void (* const PSolvers[])(btSoftBody*)= { - PSolve_Links, - PSolve_Tetras, - PSolve_Anchors, - PSolve_RContacts, - PSolve_SContacts, - }; +static void (* const PSolvers[])(btSoftBody*,btScalar)= { + PSolve_Links, + PSolve_Anchors, + PSolve_RContacts, + PSolve_SContacts, + }; // // btSoftBody @@ -1262,7 +1128,7 @@ btSoftBody::btSoftBody(btSoftBody::btSoftBodyWorldInfo* worldInfo,int node_count m_cfg.kDF = (btScalar)0.2; m_cfg.kMT = 0; m_cfg.kCHR = (btScalar)1.0; - m_cfg.kKHR = (btScalar)0.5; + m_cfg.kKHR = (btScalar)0.1; m_cfg.kSHR = (btScalar)1.0; m_cfg.kAHR = (btScalar)0.7; m_cfg.maxvolume = (btScalar)1; @@ -1308,7 +1174,6 @@ btSoftBody::btSoftBody(btSoftBody::btSoftBodyWorldInfo* worldInfo,int node_count n.m_leaf = m_ndbvt.insert(btDbvt::Volume::FromCR(n.m_x,margin),&n); n.m_material= pm; } - m_ndbvt.optimizeTopDown(); UpdateBounds(this); @@ -1319,7 +1184,7 @@ btSoftBody::~btSoftBody() { //for now, delete the internal shape delete m_collisionShape; - for(int i=0;i0) *pm=*m_materials[0]; else @@ -1430,18 +1295,6 @@ appendNote(text,o,btVector4(w,w,w,0), feature->m_n[0], feature->m_n[1], feature->m_n[2]); } - -// -void btSoftBody::appendNote( const char* text, - const btVector3& o, - Tetra* feature) -{ -static const btScalar w=1/(btScalar)4; -appendNote(text,o,btVector4(w,w,w,w), feature->m_n[0], - feature->m_n[1], - feature->m_n[2], - feature->m_n[3]); -} // void btSoftBody::appendNode( const btVector3& x,btScalar m) @@ -1525,34 +1378,6 @@ void btSoftBody::appendFace(int node0,int node1,int node2,Material* mat) m_bUpdateRtCst=true; } -// -void btSoftBody::appendTetra(int model,Material* mat) -{ -Tetra t; -if(model>=0) - t=m_tetras[model]; - else - { ZeroInitialize(t);t.m_material=mat?mat:m_materials[0]; } -m_tetras.push_back(t); -} - -// -void btSoftBody::appendTetra(int node0, - int node1, - int node2, - int node3, - Material* mat) -{ - appendTetra(-1,mat); - Tetra& t=m_tetras[m_tetras.size()-1]; - t.m_n[0] = &m_nodes[node0]; - t.m_n[1] = &m_nodes[node1]; - t.m_n[2] = &m_nodes[node2]; - t.m_n[3] = &m_nodes[node3]; - t.m_rv = VolumeOf(t.m_n[0]->m_x,t.m_n[1]->m_x,t.m_n[2]->m_x,t.m_n[3]->m_x); - m_bUpdateRtCst=true; -} - // void btSoftBody::appendAnchor(int node,btRigidBody* body) { @@ -1660,49 +1485,6 @@ void btSoftBody::setTotalDensity(btScalar density) setTotalMass(getVolume()*density,true); } -// -void btSoftBody::setVolumeMass(btScalar mass) -{ -btAlignedObjectArray ranks; -ranks.resize(m_nodes.size(),0); -for(int i=0;im_im+=btFabs(t.m_rv); - ranks[int(t.m_n[j]-&m_nodes[0])]+=1; - } - } -for(int i=0;i0) - { - 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 edges(ncount,-2); int newnodes=0; +/* Filter out */ +for(int i=0;iEval(l.m_n[0]->m_x),ifn->Eval(l.m_n[1]->m_x))) + { + btSwap(m_links[i],m_links[m_links.size()-1]); + m_links.pop_back();--i; + } + } + } /* Fill edges */ for(int i=0;im_rv); - pft[0]->m_n[0]=&m_nodes[ia]; - pft[0]->m_n[1]=&m_nodes[xb]; - pft[0]->m_n[2]=&m_nodes[xa]; - pft[0]->m_n[3]=&m_nodes[ni]; - if(Sign(VolumeOf( pft[0]->m_n[0]->m_x, - pft[0]->m_n[1]->m_x, - pft[0]->m_n[2]->m_x, - pft[0]->m_n[3]->m_x))!=sig) - { - btSwap(pft[0]->m_n[0],pft[0]->m_n[1]); - } - pft[1]->m_n[0]=&m_nodes[ib]; - pft[1]->m_n[1]=&m_nodes[xa]; - pft[1]->m_n[2]=&m_nodes[xb]; - pft[1]->m_n[3]=&m_nodes[ni]; - if(Sign(VolumeOf( pft[1]->m_n[0]->m_x, - pft[1]->m_n[1]->m_x, - pft[1]->m_n[2]->m_x, - pft[1]->m_n[3]->m_x))!=sig) - { - btSwap(pft[1]->m_n[0],pft[1]->m_n[1]); - } - appendLink(ni,xa,pft[0]->m_material,true); - appendLink(ni,xb,pft[0]->m_material,true); - --i;break; - } - } - } - } /* Cut */ if(cut) { @@ -2145,22 +1871,6 @@ if(cut) } } } - /* Tetras */ - for(int i=0,ni=m_tetras.size();iEval(n[0]->m_x)Eval(n[1]->m_x)Eval(n[2]->m_x)Eval(n[3]->m_x) ranks; @@ -2174,10 +1884,6 @@ if(cut) { for(int j=0;j<3;++j) ranks[int(m_faces[i].m_n[j]-nbase)]++; } - for(int i=0,ni=m_tetras.size();i0)) + { + for(int i=0,ni=m_nodes.size();i0) + { + const btVector3 x=m_pose.m_rot*m_pose.m_pos[i]+m_pose.m_com; + n.m_x=Lerp(n.m_x,x,m_cfg.kMT); + } + } + } + /* Clear contacts */ m_rcontacts.resize(0); m_scontacts.resize(0); + /* Optimize dbvt's */ + m_ndbvt.optimizeIncremental(1); + m_fdbvt.optimizeIncremental(1); } // void btSoftBody::solveConstraints() { -/* Solve velocities */ -if(m_cfg.viterations>0) +/* Prepare links */ +for(int i=0,ni=m_links.size();im_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();im_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;isolve0)) - { - for(int i=0,ni=m_nodes.size();i0) - { - const btVector3 x=m_pose.m_rot*m_pose.m_pos[i]+m_pose.m_com; - n.m_x=Lerp(n.m_x,x,m_cfg.kMT); - } - } + Link& l=m_links[i]; + l.m_c3 = l.m_n[1]->m_x-l.m_n[0]->m_x; + l.m_c2 = 1/(l.m_c3.length2()*l.m_c0); } /* Prepare anchors */ for(int i=0,ni=m_anchors.size();im_im; a.m_body->activate(); } +/* Solve velocities */ +if(m_cfg.viterations>0) + { + /* Solve */ + for(int isolve=0;isolve0) { @@ -2470,7 +2162,7 @@ if(m_cfg.piterations>0) { for(int iseq=0;iseq0) { for(int iseq=0;iseq0) Node& n=m_nodes[i]; n.m_v += (n.m_x-n.m_q)*vcf; } - } + } } // @@ -2512,7 +2204,7 @@ for(int isolve=0;isolvedata; DoNode(*node); @@ -2608,7 +2290,7 @@ switch(m_cfg.collisions&fCollision::RVSmask) docollide.prb = prb; docollide.dynmargin = basemargin+timemargin; docollide.stamargin = basemargin; - m_ndbvt.collide(volume,&docollide); + btDbvt::collideTV(m_ndbvt.m_root,volume,docollide); } break; } @@ -2624,15 +2306,6 @@ switch(cf&fCollision::SVSmask) { struct DoCollide : btDbvt::ICollide { - virtual bool Descent(const btDbvt::Node*) - { - return false; - } - virtual void Process(const btDbvt::Node* /*leaf*/) - { - - } - void Process(const btDbvt::Node* lnode, const btDbvt::Node* lface) { @@ -2683,14 +2356,16 @@ switch(cf&fCollision::SVSmask) /* psb0 nodes vs psb1 faces */ docollide.psb[0]=this; docollide.psb[1]=psb; - docollide.psb[0]->m_ndbvt.collide( &docollide.psb[1]->m_fdbvt, - &docollide); + btDbvt::collideTT( docollide.psb[0]->m_ndbvt.m_root, + docollide.psb[1]->m_fdbvt.m_root, + docollide); /* psb1 nodes vs psb0 faces */ docollide.psb[0]=psb; docollide.psb[1]=this; - docollide.psb[0]->m_ndbvt.collide( &docollide.psb[1]->m_fdbvt, - &docollide); + btDbvt::collideTT( docollide.psb[0]->m_ndbvt.m_root, + docollide.psb[1]->m_fdbvt.m_root, + docollide); } break; } -} +} \ No newline at end of file diff --git a/src/BulletSoftBody/btSoftBody.h b/src/BulletSoftBody/btSoftBody.h index a6c7b3322..1a24f6fe0 100644 --- a/src/BulletSoftBody/btSoftBody.h +++ b/src/BulletSoftBody/btSoftBody.h @@ -45,29 +45,31 @@ public: V_TwoSided, ///Vertex normals are fliped to match velocity V_OneSided, ///Vertex normals are taken as it is F_TwoSided, ///Face normals are fliped to match velocity - F_OneSided ///Face normals are taken as it is + F_OneSided, ///Face normals are taken as it is + END };}; ///eVSolver : velocities solvers struct eVSolver { enum _ { Linear, ///Linear solver - Volume ///Volume solver + END };}; ///ePSolver : positions solvers struct ePSolver { enum _ { Linear, ///Linear solver - Volume, ///Volume solver Anchors, ///Anchor solver RContacts, ///Rigid contacts solver - SContacts ///Soft contacts solver + SContacts, ///Soft contacts solver + END };}; ///eSolverPresets struct eSolverPresets { enum _ { Positions, Velocities, - Default = Positions + Default = Positions, + END };}; ///eFeature @@ -76,7 +78,7 @@ public: Node, Link, Face, - Tetra + END };}; typedef btAlignedObjectArray tVSolverArray; @@ -94,14 +96,16 @@ public: SVSmask = 0x00f0, ///Rigid versus soft mask VF_SS = 0x0010, ///Vertex vs face soft vs soft handling /* presets */ - Default = SDF_RS + Default = SDF_RS, + END };}; ///fMaterial struct fMaterial { enum _ { DebugDraw = 0x0001, /// Enable debug draw /* presets */ - Default = DebugDraw + Default = DebugDraw, + END };}; // @@ -120,7 +124,6 @@ public: /* ImplicitFn */ struct ImplicitFn { - virtual ~ImplicitFn() {} virtual btScalar Eval(const btVector3& x)=0; }; @@ -163,16 +166,13 @@ public: struct Element { void* m_tag; // User data - Element() - { - m_tag=0; - } + Element() : m_tag(0) {} }; /* Material */ struct Material : Element { btScalar m_kLST; // Linear stiffness coefficient [0,1] - btScalar m_kAST; // Area stiffness coefficient [0,1] + btScalar m_kAST; // Area/Angular stiffness coefficient [0,1] btScalar m_kVST; // Volume stiffness coefficient [0,1] int m_flags; // Flags }; @@ -199,7 +199,8 @@ public: struct Link : Feature { Node* m_n[2]; // Node pointers - btScalar m_rl; // Rest length + btScalar m_rl; // Rest length + int m_bbending:1; // Bending link btScalar m_c0; // (ima+imb)*kLST btScalar m_c1; // rl^2 btScalar m_c2; // |gradient|^2/c0 @@ -213,16 +214,6 @@ public: btScalar m_ra; // Rest area btDbvt::Node* m_leaf; // Leaf data }; - /* Tetra */ - struct Tetra : Feature - { - Node* m_n[4]; // Node pointers - btScalar m_rv; // Rest volume - btDbvt::Node* m_leaf; // Leaf data - btVector3 m_c0[4]; // gradients - btScalar m_c1; // (4*kVST)/(im0+im1+im2+im3) - btScalar m_c2; // m_c1/sum(|g0..3|^2) - }; /* RContact */ struct RContact { @@ -322,7 +313,6 @@ public: typedef btAlignedObjectArray tLeafArray; typedef btAlignedObjectArray tLinkArray; typedef btAlignedObjectArray tFaceArray; - typedef btAlignedObjectArray tTetraArray; typedef btAlignedObjectArray tAnchorArray; typedef btAlignedObjectArray tRContactArray; typedef btAlignedObjectArray tSContactArray; @@ -342,7 +332,6 @@ public: tNodeArray m_nodes; // Nodes tLinkArray m_links; // Links tFaceArray m_faces; // Faces - tTetraArray m_tetras; // Tetras tAnchorArray m_anchors; // Anchors tRContactArray m_rcontacts; // Rigid contacts tSContactArray m_scontacts; // Soft contacts @@ -365,12 +354,12 @@ public: /* Check for existing link */ bool checkLink( int node0, int node1) const; - bool checkLink( const btSoftBody::Node* node0, - const btSoftBody::Node* node1) const; + bool checkLink( const Node* node0, + const Node* node1) const; /* Check for existring face */ bool checkFace( int node0, - int node1, - int node2) const; + int node1, + int node2) const; /* Append material */ Material* appendMaterial(); /* Append note */ @@ -390,9 +379,6 @@ public: void appendNote( const char* text, const btVector3& o, Face* feature); - void appendNote( const char* text, - const btVector3& o, - Tetra* feature); /* Append node */ void appendNode( const btVector3& x,btScalar m); /* Append link */ @@ -401,8 +387,8 @@ public: int node1, Material* mat=0, bool bcheckexist=false); - void appendLink( btSoftBody::Node* node0, - btSoftBody::Node* node1, + void appendLink( Node* node0, + Node* node1, Material* mat=0, bool bcheckexist=false); /* Append face */ @@ -411,13 +397,6 @@ public: int node1, int node2, Material* mat=0); - /* Append tetrahedron */ - void appendTetra(int model=-1,Material* mat=0); - void appendTetra(int node0, - int node1, - int node2, - int node3, - Material* mat=0); /* Append anchor */ void appendAnchor( int node, btRigidBody* body); @@ -443,10 +422,6 @@ public: bool fromfaces=false); /* Set total density */ void setTotalDensity(btScalar density); - /* Set volume mass (using tetrahedrons) */ - void setVolumeMass( btScalar mass); - /* Set volume density (using tetrahedrons) */ - void setVolumeDensity( btScalar density); /* Transform */ void transform( const btTransform& trs); /* Translate */ @@ -463,8 +438,6 @@ public: /* Generate bending constraints based on distance in the adjency graph */ int generateBendingConstraints( int distance, Material* mat=0); - /* Generate tetrahedral constraints */ - int generateTetrahedralConstraints(); /* Randomize constraints to reduce solver bias */ void randomizeConstraints(); /* Refine */ diff --git a/src/BulletSoftBody/btSoftBodyHelpers.cpp b/src/BulletSoftBody/btSoftBodyHelpers.cpp index efdeb8032..a85b63526 100644 --- a/src/BulletSoftBody/btSoftBodyHelpers.cpp +++ b/src/BulletSoftBody/btSoftBodyHelpers.cpp @@ -15,7 +15,6 @@ subject to the following restrictions: ///btSoftBodyHelpers.cpp by Nathanael Presson #include "btSoftBody.h" -#include "BulletCollision/BroadphaseCollision/btDbvt.h" #include #include #include "btSoftBodyHelpers.h" @@ -23,97 +22,98 @@ subject to the following restrictions: // static void drawVertex( btIDebugDraw* idraw, - const btVector3& x,btScalar s,const btVector3& c) -{ - idraw->drawLine(x-btVector3(s,0,0),x+btVector3(s,0,0),c); - idraw->drawLine(x-btVector3(0,s,0),x+btVector3(0,s,0),c); - idraw->drawLine(x-btVector3(0,0,s),x+btVector3(0,0,s),c); -} + const btVector3& x,btScalar s,const btVector3& c) + { + idraw->drawLine(x-btVector3(s,0,0),x+btVector3(s,0,0),c); + idraw->drawLine(x-btVector3(0,s,0),x+btVector3(0,s,0),c); + idraw->drawLine(x-btVector3(0,0,s),x+btVector3(0,0,s),c); + } // static void drawBox( btIDebugDraw* idraw, - const btVector3& mins, - const btVector3& maxs, - const btVector3& color) + const btVector3& mins, + const btVector3& maxs, + const btVector3& color) { - const btVector3 c[]={ btVector3(mins.x(),mins.y(),mins.z()), - btVector3(maxs.x(),mins.y(),mins.z()), - btVector3(maxs.x(),maxs.y(),mins.z()), - btVector3(mins.x(),maxs.y(),mins.z()), - btVector3(mins.x(),mins.y(),maxs.z()), - btVector3(maxs.x(),mins.y(),maxs.z()), - btVector3(maxs.x(),maxs.y(),maxs.z()), - btVector3(mins.x(),maxs.y(),maxs.z())}; - idraw->drawLine(c[0],c[1],color);idraw->drawLine(c[1],c[2],color); - idraw->drawLine(c[2],c[3],color);idraw->drawLine(c[3],c[0],color); - idraw->drawLine(c[4],c[5],color);idraw->drawLine(c[5],c[6],color); - idraw->drawLine(c[6],c[7],color);idraw->drawLine(c[7],c[4],color); - idraw->drawLine(c[0],c[4],color);idraw->drawLine(c[1],c[5],color); - idraw->drawLine(c[2],c[6],color);idraw->drawLine(c[3],c[7],color); +const btVector3 c[]={ btVector3(mins.x(),mins.y(),mins.z()), + btVector3(maxs.x(),mins.y(),mins.z()), + btVector3(maxs.x(),maxs.y(),mins.z()), + btVector3(mins.x(),maxs.y(),mins.z()), + btVector3(mins.x(),mins.y(),maxs.z()), + btVector3(maxs.x(),mins.y(),maxs.z()), + btVector3(maxs.x(),maxs.y(),maxs.z()), + btVector3(mins.x(),maxs.y(),maxs.z())}; +idraw->drawLine(c[0],c[1],color);idraw->drawLine(c[1],c[2],color); +idraw->drawLine(c[2],c[3],color);idraw->drawLine(c[3],c[0],color); +idraw->drawLine(c[4],c[5],color);idraw->drawLine(c[5],c[6],color); +idraw->drawLine(c[6],c[7],color);idraw->drawLine(c[7],c[4],color); +idraw->drawLine(c[0],c[4],color);idraw->drawLine(c[1],c[5],color); +idraw->drawLine(c[2],c[6],color);idraw->drawLine(c[3],c[7],color); } // static void drawTree( btIDebugDraw* idraw, - const btDbvt::Node* node, - int depth, - const btVector3& ncolor, - const btVector3& lcolor, - int mindepth, - int maxdepth) + const btDbvt::Node* node, + int depth, + const btVector3& ncolor, + const btVector3& lcolor, + int mindepth, + int maxdepth) { - if(node) +if(node) { - if(node->isinternal()&&((depthisinternal()&&((depthchilds[0],depth+1,ncolor,lcolor,mindepth,maxdepth); - drawTree(idraw,node->childs[1],depth+1,ncolor,lcolor,mindepth,maxdepth); + drawTree(idraw,node->childs[0],depth+1,ncolor,lcolor,mindepth,maxdepth); + drawTree(idraw,node->childs[1],depth+1,ncolor,lcolor,mindepth,maxdepth); } - if(depth>=mindepth) + if(depth>=mindepth) { - const btScalar scl=(btScalar)(node->isinternal()?1:1); - const btVector3 mi=node->volume.Center()-node->volume.Extent()*scl; - const btVector3 mx=node->volume.Center()+node->volume.Extent()*scl; - drawBox(idraw,mi,mx,node->isleaf()?lcolor:ncolor); + const btScalar scl=(btScalar)(node->isinternal()?1:1); + const btVector3 mi=node->volume.Center()-node->volume.Extent()*scl; + const btVector3 mx=node->volume.Center()+node->volume.Extent()*scl; + drawBox(idraw,mi,mx,node->isleaf()?lcolor:ncolor); } } } // -// -static inline btScalar tetravolume(const btVector3& x0, - const btVector3& x1, - const btVector3& x2, - const btVector3& x3) -{ - const btVector3 a=x1-x0; - const btVector3 b=x2-x0; - const btVector3 c=x3-x0; - return(dot(a,cross(b,c))); +// +static inline btScalar tetravolume(const btVector3& x0, + const btVector3& x1, + const btVector3& x2, + const btVector3& x3) +{ + const btVector3 a=x1-x0; + const btVector3 b=x2-x0; + const btVector3 c=x3-x0; + return(dot(a,cross(b,c))); } // -/* +#if 0 static btVector3 stresscolor(btScalar stress) -{ + { static const btVector3 spectrum[]= { btVector3(1,0,1), - btVector3(0,0,1), - btVector3(0,1,1), - btVector3(0,1,0), - btVector3(1,1,0), - btVector3(1,0,0), - btVector3(1,0,0)}; + btVector3(0,0,1), + btVector3(0,1,1), + btVector3(0,1,0), + btVector3(1,1,0), + btVector3(1,0,0), + btVector3(1,0,0)}; static const int ncolors=sizeof(spectrum)/sizeof(spectrum[0])-1; static const btScalar one=1; stress=btMax(0,btMin(1,stress))*ncolors; const int sel=(int)stress; const btScalar frc=stress-sel; return(spectrum[sel]+(spectrum[sel+1]-spectrum[sel])*frc); -} -*/ + } +#endif + // void btSoftBodyHelpers::Draw( btSoftBody* psb, - btIDebugDraw* idraw, - int drawflags) + btIDebugDraw* idraw, + int drawflags) { const btScalar scl=(btScalar)0.1; const btScalar nscl=scl*5; @@ -209,32 +209,14 @@ void btSoftBodyHelpers::Draw( btSoftBody* psb, idraw->drawLine((x[1]-c)*scl+c,(x[2]-c)*scl+c,col); idraw->drawLine((x[2]-c)*scl+c,(x[0]-c)*scl+c,col);*/ idraw->drawTriangle((x[0]-c)*scl+c, + (x[1]-c)*scl+c, + (x[2]-c)*scl+c, + col,alp); + /*idraw->drawTriangle((x[0]-c)*scl+c, (x[1]-c)*scl+c, (x[2]-c)*scl+c, - col,alp); - /*idraw->drawTriangle((x[0]-c)*scl+c, - (x[1]-c)*scl+c, - (x[2]-c)*scl+c, - f.m_n[0]->m_n,f.m_n[1]->m_n,f.m_n[2]->m_n, - col,alp);*/ - } - } - /* Tetras */ - if(0!=(drawflags&fDrawFlags::Tetras)) - { - const btScalar scl=(btScalar)0.8; - const btScalar alp=(btScalar)1; - const btVector3 col((btScalar)0.7,(btScalar)0.7,(btScalar)0.7); - for(int i=0;im_tetras.size();++i) - { - const btSoftBody::Tetra& t=psb->m_tetras[i]; - if(0==(t.m_material->m_flags&btSoftBody::fMaterial::DebugDraw)) continue; - const btVector3 x[]={t.m_n[0]->m_x,t.m_n[1]->m_x,t.m_n[2]->m_x,t.m_n[3]->m_x}; - const btVector3 c=(x[0]+x[1]+x[2]+x[3])/4; - idraw->drawTriangle((x[0]-c)*scl+c,(x[1]-c)*scl+c,(x[2]-c)*scl+c,col,alp); - idraw->drawTriangle((x[0]-c)*scl+c,(x[1]-c)*scl+c,(x[3]-c)*scl+c,col,alp); - idraw->drawTriangle((x[1]-c)*scl+c,(x[2]-c)*scl+c,(x[3]-c)*scl+c,col,alp); - idraw->drawTriangle((x[2]-c)*scl+c,(x[0]-c)*scl+c,(x[3]-c)*scl+c,col,alp); + f.m_n[0]->m_n,f.m_n[1]->m_n,f.m_n[2]->m_n, + col,alp);*/ } } /* Notes */ @@ -245,21 +227,20 @@ void btSoftBodyHelpers::Draw( btSoftBody* psb, const btSoftBody::Note& n=psb->m_notes[i]; btVector3 p=n.m_offset; for(int j=0;jm_x*n.m_coords[j]; - } + } idraw->draw3dText(p,n.m_text); } } } -#if 0 // void btSoftBodyHelpers::DrawInfos( btSoftBody* psb, - btIDebugDraw* idraw, - bool masses, - bool areas, - bool /*stress*/) + btIDebugDraw* idraw, + bool masses, + bool areas, + bool /*stress*/) { for(int i=0;im_nodes.size();++i) { @@ -279,29 +260,28 @@ void btSoftBodyHelpers::DrawInfos( btSoftBody* psb, if(text[0]) idraw->draw3dText(n.m_x,text); } } -#endif // void btSoftBodyHelpers::DrawNodeTree( btSoftBody* psb, - btIDebugDraw* idraw, - int mindepth, - int maxdepth) + btIDebugDraw* idraw, + int mindepth, + int maxdepth) { - drawTree(idraw,psb->m_ndbvt.m_root,0,btVector3(1,0,1),btVector3(1,1,1),mindepth,maxdepth); +drawTree(idraw,psb->m_ndbvt.m_root,0,btVector3(1,0,1),btVector3(1,1,1),mindepth,maxdepth); } // void btSoftBodyHelpers::DrawFaceTree( btSoftBody* psb, - btIDebugDraw* idraw, - int mindepth, - int maxdepth) + btIDebugDraw* idraw, + int mindepth, + int maxdepth) { - drawTree(idraw,psb->m_fdbvt.m_root,0,btVector3(0,1,0),btVector3(1,0,0),mindepth,maxdepth); +drawTree(idraw,psb->m_fdbvt.m_root,0,btVector3(0,1,0),btVector3(1,0,0),mindepth,maxdepth); } // void btSoftBodyHelpers::DrawFrame( btSoftBody* psb, - btIDebugDraw* idraw) + btIDebugDraw* idraw) { if(psb->m_pose.m_bframe) { @@ -325,9 +305,9 @@ void btSoftBodyHelpers::DrawFrame( btSoftBody* psb, // btSoftBody* btSoftBodyHelpers::CreateRope( btSoftBody::btSoftBodyWorldInfo& worldInfo, const btVector3& from, - const btVector3& to, - int res, - int fixeds) + const btVector3& to, + int res, + int fixeds) { /* Create nodes */ const int r=res+2; @@ -355,13 +335,13 @@ btSoftBody* btSoftBodyHelpers::CreateRope( btSoftBody::btSoftBodyWorldInfo& wor // btSoftBody* btSoftBodyHelpers::CreatePatch(btSoftBody::btSoftBodyWorldInfo& worldInfo,const btVector3& corner00, - const btVector3& corner10, - const btVector3& corner01, - const btVector3& corner11, - int resx, - int resy, - int fixeds, - bool gendiags) + const btVector3& corner10, + const btVector3& corner01, + const btVector3& corner11, + int resx, + int resy, + int fixeds, + bool gendiags) { #define IDX(_x_,_y_) ((_y_)*rx+(_x_)) /* Create nodes */ @@ -430,8 +410,8 @@ btSoftBody* btSoftBodyHelpers::CreatePatch(btSoftBody::btSoftBodyWorldInfo& wor // btSoftBody* btSoftBodyHelpers::CreateEllipsoid(btSoftBody::btSoftBodyWorldInfo& worldInfo,const btVector3& center, - const btVector3& radius, - int res) + const btVector3& radius, + int res) { struct Hammersley { @@ -462,8 +442,8 @@ btSoftBody* btSoftBodyHelpers::CreateEllipsoid(btSoftBody::btSoftBodyWorldInfo& // btSoftBody* btSoftBodyHelpers::CreateFromTriMesh(btSoftBody::btSoftBodyWorldInfo& worldInfo,const btScalar* vertices, - const int* triangles, - int ntriangles) + const int* triangles, + int ntriangles) { int maxidx=0; for(int i=0,ni=ntriangles*3;i(nvertices),vertices); + HullDesc hdsc(QF_TRIANGLES,nvertices,vertices); HullResult hres; HullLibrary hlib;/*??*/ - hdsc.mMaxVertices=static_cast(nvertices); + hdsc.mMaxVertices=nvertices; hlib.CreateConvexHull(hdsc,hres); btSoftBody* psb=new btSoftBody(&worldInfo,(int)hres.mNumOutputVertices, &hres.m_OutputVertices[0],0); for(int i=0;i<(int)hres.mNumFaces;++i) { - const int idx[]={ static_cast(hres.m_Indices[i*3+0]), - static_cast(hres.m_Indices[i*3+1]), - static_cast(hres.m_Indices[i*3+2])}; + const int idx[]={ hres.m_Indices[i*3+0], + hres.m_Indices[i*3+1], + hres.m_Indices[i*3+2]}; if(idx[0]appendLink( idx[0],idx[1]); if(idx[1]appendLink( idx[1],idx[2]); if(idx[2]appendLink( idx[2],idx[0]); @@ -524,156 +504,4 @@ btSoftBody* btSoftBodyHelpers::CreateFromConvexHull(btSoftBody::btSoftBodyWorld hlib.ReleaseResult(hres); psb->randomizeConstraints(); return(psb); -} - -#if BT_SOFTBODY_USE_STL - -#include -#include -#include -#include -#include - -// -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;im_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;im_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 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>index; - sn>>x;sn>>y;sn>>z; - for(int j=0;j>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>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>index; - se>>ni[0];se>>ni[1];se>>ni[2];se>>ni[3]; - for(int j=0;j>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 - +} \ No newline at end of file diff --git a/src/BulletSoftBody/btSoftBodyHelpers.h b/src/BulletSoftBody/btSoftBodyHelpers.h index c3b5e538f..9832f7408 100644 --- a/src/BulletSoftBody/btSoftBodyHelpers.h +++ b/src/BulletSoftBody/btSoftBodyHelpers.h @@ -18,9 +18,6 @@ subject to the following restrictions: #include "btSoftBody.h" -//Can't enable this, Bullet doesn't use STL -//#define BT_SOFTBODY_USE_STL 1 - // // Helpers // @@ -37,7 +34,7 @@ struct fDrawFlags { enum _ { Notes = 0x0080, /* presets */ Std = Links+Faces+Tetras+Anchors+Notes, - StdTetra = Std-Faces+Tetras + StdTetra = Std-Faces+Tetras, };}; struct btSoftBodyHelpers @@ -46,14 +43,12 @@ struct btSoftBodyHelpers static void Draw( btSoftBody* psb, btIDebugDraw* idraw, int drawflags=fDrawFlags::Std); -#if 0 /* Draw body infos */ static void DrawInfos( btSoftBody* psb, btIDebugDraw* idraw, bool masses, bool areas, bool stress); -#endif /* Draw node tree */ static void DrawNodeTree( btSoftBody* psb, btIDebugDraw* idraw, @@ -97,27 +92,6 @@ struct btSoftBodyHelpers static btSoftBody* CreateFromConvexHull( btSoftBody::btSoftBodyWorldInfo& worldInfo, const btVector3* vertices, int nvertices); - #if BT_SOFTBODY_USE_STL - /* Export TetGen compatible .smesh file */ - static void ExportAsSMeshFile( btSoftBody* psb, - const char* filename); - /* Create from TetGen .ele, .face, .node files */ - static btSoftBody* CreateFromTetGenFile( btSoftBody::btSoftBodyWorldInfo& worldInfo, - const char* ele, - const char* face, - const char* node, - bool bfacelinks, - bool btetralinks, - bool bfacesfromtetras); - /* Create from TetGen .ele, .face, .node data */ - static btSoftBody* CreateFromTetGenData( btSoftBody::btSoftBodyWorldInfo& worldInfo, - const char* ele, - const char* face, - const char* node, - bool bfacelinks, - bool btetralinks, - bool bfacesfromtetras); - #endif }; #endif //SOFT_BODY_HELPERS_H