diff --git a/Demos/OpenGL/GLDebugDrawer.cpp b/Demos/OpenGL/GLDebugDrawer.cpp index 7e939ba95..7927782f1 100644 --- a/Demos/OpenGL/GLDebugDrawer.cpp +++ b/Demos/OpenGL/GLDebugDrawer.cpp @@ -34,6 +34,21 @@ void GLDebugDrawer::drawLine(const btVector3& from,const btVector3& to,const btV } } +void GLDebugDrawer::drawTriangle(const btVector3& a,const btVector3& b,const btVector3& c,const btVector3& color,btScalar alpha) +{ +// if (m_debugMode > 0) + { + const btVector3 n=cross(b-a,c-a).normalized(); + glBegin(GL_TRIANGLES); + glColor4f(color.getX(), color.getY(), color.getZ(),alpha); + glNormal3d(n.getX(),n.getY(),n.getZ()); + glVertex3d(a.getX(),a.getY(),a.getZ()); + glVertex3d(b.getX(),b.getY(),b.getZ()); + glVertex3d(c.getX(),c.getY(),c.getZ()); + glEnd(); + } +} + void GLDebugDrawer::setDebugMode(int debugMode) { m_debugMode = debugMode; @@ -76,3 +91,4 @@ void GLDebugDrawer::drawContactPoint(const btVector3& pointOnB,const btVector3& + diff --git a/Demos/OpenGL/GLDebugDrawer.h b/Demos/OpenGL/GLDebugDrawer.h index 491d43295..b2ad621e7 100644 --- a/Demos/OpenGL/GLDebugDrawer.h +++ b/Demos/OpenGL/GLDebugDrawer.h @@ -15,7 +15,9 @@ public: virtual void drawLine(const btVector3& from,const btVector3& to,const btVector3& color); - + + virtual void drawTriangle(const btVector3& a,const btVector3& b,const btVector3& c,const btVector3& color,btScalar alpha); + virtual void drawContactPoint(const btVector3& PointOnB,const btVector3& normalOnB,btScalar distance,int lifeTime,const btVector3& color); virtual void reportErrorWarning(const char* warningString); diff --git a/Demos/SoftDemo/SoftDemo.cpp b/Demos/SoftDemo/SoftDemo.cpp index 9cbc2acec..5ea5df79d 100644 --- a/Demos/SoftDemo/SoftDemo.cpp +++ b/Demos/SoftDemo/SoftDemo.cpp @@ -106,7 +106,7 @@ void SoftDemo::clientMoveAndDisplay() if (m_dynamicsWorld) { -//#define FIXED_STEP +#define FIXED_STEP #ifdef FIXED_STEP m_dynamicsWorld->stepSimulation(dt=1.0f/60.f,0); @@ -186,7 +186,36 @@ void SoftDemo::displayCallback(void) { } +// +// ImplicitShape +// +// +struct ImplicitSphere : btSoftBody::ImplicitFn +{ +btVector3 center; +btScalar sqradius; + ImplicitSphere() {} + ImplicitSphere(const btVector3& c,btScalar r) : center(c),sqradius(r*r) {} +btScalar Eval(const btVector3& x) + { + return((x-center).length2()-sqradius); + } +}; + +// +// Tetra meshes +// + +struct TetraBunny +{ +#include "bunny.inl" +}; + +struct TetraCube +{ +#include "cube.inl" +}; // @@ -282,7 +311,7 @@ static btSoftBody* Ctor_SoftBox(SoftDemo* pdemo,const btVector3& p,const btVecto p+h*btVector3(-1,+1,+1), p+h*btVector3(+1,+1,+1)}; btSoftBody* psb=btSoftBodyHelpers::CreateFromConvexHull(pdemo->m_softBodyWorldInfo,c,8); - psb->generateBendingConstraints(2,1); + psb->generateBendingConstraints(2); pdemo->getSoftDynamicsWorld()->addSoftBody(psb); return(psb); @@ -300,7 +329,7 @@ static btSoftBody* Ctor_SoftBoulder(SoftDemo* pdemo,const btVector3& p,const btV pts.push_back(Vector3Rand()*s+p); } btSoftBody* psb=btSoftBodyHelpers::CreateFromConvexHull(pdemo->m_softBodyWorldInfo,&pts[0],pts.size()); - psb->generateBendingConstraints(2,1); + psb->generateBendingConstraints(2); pdemo->getSoftDynamicsWorld()->addSoftBody(psb); return(psb); @@ -321,8 +350,8 @@ static void Init_Ropes(SoftDemo* pdemo) btVector3(10,0,i*0.25), 16, 1+2); - psb->m_cfg.iterations = 4; - psb->m_cfg.kLST = 0.1+(i/(btScalar)(n-1))*0.9; + psb->m_cfg.piterations = 4; + psb->m_materials[0]->m_kLST = 0.1+(i/(btScalar)(n-1))*0.9; psb->setTotalMass(20); pdemo->getSoftDynamicsWorld()->addSoftBody(psb); @@ -335,6 +364,7 @@ static void Init_Ropes(SoftDemo* pdemo) static void Init_RopeAttach(SoftDemo* pdemo) { //TRACEDEMO + pdemo->m_softBodyWorldInfo.m_sparsesdf.RemoveReferences(0); struct Functors { static btSoftBody* CtorRope(SoftDemo* pdemo,const btVector3& p) @@ -351,8 +381,8 @@ static void Init_RopeAttach(SoftDemo* pdemo) btRigidBody* body=pdemo->localCreateRigidBody(50,startTransform,new btBoxShape(btVector3(2,6,2))); btSoftBody* psb0=Functors::CtorRope(pdemo,btVector3(0,8,-1)); btSoftBody* psb1=Functors::CtorRope(pdemo,btVector3(0,8,+1)); - psb0->appendAnchor(psb0->getNodes().size()-1,body); - psb1->appendAnchor(psb1->getNodes().size()-1,body); + psb0->appendAnchor(psb0->m_nodes.size()-1,body); + psb1->appendAnchor(psb1->m_nodes.size()-1,body); } // @@ -376,6 +406,7 @@ static void Init_ClothAttach(SoftDemo* pdemo) btRigidBody* body=pdemo->localCreateRigidBody(20,startTransform,new btBoxShape(btVector3(s,1,3))); psb->appendAnchor(0,body); psb->appendAnchor(r-1,body); + pdemo->m_cutting=true; } // @@ -409,8 +440,8 @@ static void Init_Collide(SoftDemo* pdemo) btSoftBody* psb=btSoftBodyHelpers::CreateFromTriMesh(pdemo->m_softBodyWorldInfo,gVertices, &gIndices[0][0], NUM_TRIANGLES); - psb->generateBendingConstraints(2,1); - psb->m_cfg.iterations=2; + psb->generateBendingConstraints(2); + psb->m_cfg.piterations=2; psb->m_cfg.collisions|=btSoftBody::fCollision::VF_SS; psb->randomizeConstraints(); btMatrix3x3 m; @@ -426,6 +457,7 @@ static void Init_Collide(SoftDemo* pdemo) { Functor::Create(pdemo,btVector3(3*i,2,0),btVector3(SIMD_PI/2*(1-(i&1)),SIMD_PI/2*(i&1),0)); } + pdemo->m_cutting=true; } // @@ -441,8 +473,11 @@ static void Init_Collide2(SoftDemo* pdemo) btSoftBody* psb=btSoftBodyHelpers::CreateFromTriMesh(pdemo->m_softBodyWorldInfo,gVerticesBunny, &gIndicesBunny[0][0], BUNNY_NUM_TRIANGLES); - psb->generateBendingConstraints(2,0.5); - psb->m_cfg.iterations = 2; + btSoftBody::Material* pm=psb->appendMaterial(); + pm->m_kLST = 0.5; + pm->m_flags -= btSoftBody::fMaterial::DebugDraw; + psb->generateBendingConstraints(2,pm); + psb->m_cfg.piterations = 2; psb->m_cfg.kDF = 0.5; psb->m_cfg.collisions |= btSoftBody::fCollision::VF_SS; psb->randomizeConstraints(); @@ -459,6 +494,7 @@ static void Init_Collide2(SoftDemo* pdemo) { Functor::Create(pdemo,btVector3(0,-1+5*i,0),btVector3(0,SIMD_PI/2*(i&1),0)); } + pdemo->m_cutting=true; } // @@ -474,25 +510,29 @@ static void Init_Collide3(SoftDemo* pdemo) btVector3(-s,0,+s), btVector3(+s,0,+s), 15,15,1+2+4+8,true); - psb->m_cfg.kLST = 0.4; - psb->m_cfg.collisions |= btSoftBody::fCollision::VF_SS; + psb->m_materials[0]->m_kLST = 0.4; + psb->m_cfg.collisions |= btSoftBody::fCollision::VF_SS; psb->setTotalMass(150); pdemo->getSoftDynamicsWorld()->addSoftBody(psb); } { const btScalar s=4; - const btVector3 o=btVector3(4,10,0); + const btVector3 o=btVector3(5,10,0); btSoftBody* psb=btSoftBodyHelpers::CreatePatch( pdemo->m_softBodyWorldInfo, btVector3(-s,0,-s)+o, btVector3(+s,0,-s)+o, btVector3(-s,0,+s)+o, btVector3(+s,0,+s)+o, 7,7,0,true); - psb->generateBendingConstraints(2,0.5); - psb->m_cfg.kLST = 0.4; - psb->m_cfg.collisions |= btSoftBody::fCollision::VF_SS; + btSoftBody::Material* pm=psb->appendMaterial(); + pm->m_kLST = 0.1; + pm->m_flags -= btSoftBody::fMaterial::DebugDraw; + psb->generateBendingConstraints(2,pm); + psb->m_materials[0]->m_kLST = 0.5; + psb->m_cfg.collisions |= btSoftBody::fCollision::VF_SS; psb->setTotalMass(150); pdemo->getSoftDynamicsWorld()->addSoftBody(psb); + pdemo->m_cutting=true; } } @@ -514,7 +554,7 @@ static void Init_Aero(SoftDemo* pdemo) btVector3(+s,h,+s), segments,segments, 0,true); - psb->generateBendingConstraints(2,1); + psb->generateBendingConstraints(2); psb->m_cfg.kLF = 0.004; psb->m_cfg.kDG = 0.0003; psb->m_cfg.aeromodel = btSoftBody::eAeroModel::V_TwoSided; @@ -561,10 +601,10 @@ static void Init_Pressure(SoftDemo* pdemo) btSoftBody* psb=btSoftBodyHelpers::CreateEllipsoid(pdemo->m_softBodyWorldInfo,btVector3(35,25,0), btVector3(1,1,1)*3, 512); - psb->m_cfg.kLST = 0.1; - psb->m_cfg.kDF = 1; - psb->m_cfg.kDP = 0.001; // fun factor... - psb->m_cfg.kPR = 2500; + psb->m_materials[0]->m_kLST = 0.1; + psb->m_cfg.kDF = 1; + psb->m_cfg.kDP = 0.001; // fun factor... + psb->m_cfg.kPR = 2500; psb->setTotalMass(30,true); pdemo->getSoftDynamicsWorld()->addSoftBody(psb); @@ -582,8 +622,8 @@ static void Init_Volume(SoftDemo* pdemo) btSoftBody* psb=btSoftBodyHelpers::CreateEllipsoid(pdemo->m_softBodyWorldInfo,btVector3(35,25,0), btVector3(1,1,1)*3, 512); - psb->m_cfg.kLST = 0.45; - psb->m_cfg.kVC = 20; + psb->m_materials[0]->m_kLST = 0.45; + psb->m_cfg.kVC = 20; psb->setTotalMass(50,true); psb->setPose(true,false); pdemo->getSoftDynamicsWorld()->addSoftBody(psb); @@ -615,12 +655,11 @@ static void Init_Sticks(SoftDemo* pdemo) org+btVector3(hg*0.001,hg,0), sg, 1); - psb->m_cfg.iterations = 1; psb->m_cfg.kDP = 0.005; psb->m_cfg.kCHR = 0.1; for(int i=0;i<3;++i) { - psb->generateBendingConstraints(2+i,1); + psb->generateBendingConstraints(2+i); } psb->setMass(1,0); psb->setTotalMass(0.01); @@ -646,12 +685,15 @@ static void Init_Cloth(SoftDemo* pdemo) // 31,31, 1+2+4+8,true); - psb->generateBendingConstraints(2,1); - psb->m_cfg.kLST = 0.4; + btSoftBody::Material* pm=psb->appendMaterial(); + pm->m_kLST = 0.4; + pm->m_flags -= btSoftBody::fMaterial::DebugDraw; + psb->generateBendingConstraints(2,pm); psb->setTotalMass(150); pdemo->getSoftDynamicsWorld()->addSoftBody(psb); Ctor_RbUpStack(pdemo,10); + pdemo->m_cutting=true; } // @@ -663,8 +705,11 @@ static void Init_Bunny(SoftDemo* pdemo) btSoftBody* psb=btSoftBodyHelpers::CreateFromTriMesh(pdemo->m_softBodyWorldInfo,gVerticesBunny, &gIndicesBunny[0][0], BUNNY_NUM_TRIANGLES); - psb->generateBendingConstraints(2,0.5); - psb->m_cfg.iterations = 2; + btSoftBody::Material* pm=psb->appendMaterial(); + pm->m_kLST = 0.5; + pm->m_flags -= btSoftBody::fMaterial::DebugDraw; + psb->generateBendingConstraints(2,pm); + psb->m_cfg.piterations = 2; psb->m_cfg.kDF = 0.5; psb->randomizeConstraints(); psb->scale(btVector3(6,6,6)); @@ -682,9 +727,9 @@ static void Init_BunnyMatch(SoftDemo* pdemo) btSoftBody* psb=btSoftBodyHelpers::CreateFromTriMesh(pdemo->m_softBodyWorldInfo, gVerticesBunny, &gIndicesBunny[0][0], BUNNY_NUM_TRIANGLES); - psb->m_cfg.kDF = 0.5; - psb->m_cfg.kLST = 0.1; - psb->m_cfg.kMT = 0.05; + psb->m_cfg.kDF = 0.5; + psb->m_materials[0]->m_kLST = 0.1; + psb->m_cfg.kMT = 0.05; psb->randomizeConstraints(); psb->scale(btVector3(6,6,6)); psb->setTotalMass(100,true); @@ -702,8 +747,8 @@ static void Init_Torus(SoftDemo* pdemo) btSoftBody* psb=btSoftBodyHelpers::CreateFromTriMesh( pdemo->m_softBodyWorldInfo, gVertices, &gIndices[0][0], NUM_TRIANGLES); - psb->generateBendingConstraints(2,1); - psb->m_cfg.iterations=2; + psb->generateBendingConstraints(2); + psb->m_cfg.piterations=2; psb->randomizeConstraints(); btMatrix3x3 m; m.setEulerZYX(SIMD_PI/2,0,0); @@ -723,8 +768,8 @@ static void Init_TorusMatch(SoftDemo* pdemo) btSoftBody* psb=btSoftBodyHelpers::CreateFromTriMesh(pdemo->m_softBodyWorldInfo, gVertices, &gIndices[0][0], NUM_TRIANGLES); - psb->m_cfg.kLST=0.1; - psb->m_cfg.kMT=0.05; + psb->m_materials[0]->m_kLST = 0.1; + psb->m_cfg.kMT = 0.05; psb->randomizeConstraints(); btMatrix3x3 m; m.setEulerZYX(SIMD_PI/2,0,0); @@ -733,11 +778,176 @@ static void Init_TorusMatch(SoftDemo* pdemo) psb->setTotalMass(50,true); psb->setPose(true,true); pdemo->getSoftDynamicsWorld()->addSoftBody(psb); - - } -unsigned current_demo=0; +// +// Cutting1 +// +static void Init_Cutting1(SoftDemo* pdemo) +{ + const btScalar s=6; + const btScalar h=2; + const int r=16; + const btVector3 p[]={ btVector3(+s,h,-s), + btVector3(-s,h,-s), + btVector3(+s,h,+s), + btVector3(-s,h,+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=1; + 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=1; void SoftDemo::clientResetScene() { @@ -766,6 +976,11 @@ void SoftDemo::clientResetScene() /* Init */ void (*demofncs[])(SoftDemo*)= { + Init_Cutting1, +#ifdef BT_SOFTBODY_USE_STL + Init_TetraBunny, + Init_TetraCube, +#endif //BT_SOFTBODY_USE_STL Init_Cloth, Init_Pressure, Init_Volume, @@ -796,6 +1011,7 @@ void SoftDemo::clientResetScene() m_autocam = false; m_raycast = false; + m_cutting = false; demofncs[current_demo](this); } @@ -813,10 +1029,10 @@ void SoftDemo::renderme() for(int ib=0;ibgetNodes().size(); - for(int i=0;igetNodes().size();++i) + nps+=psb->m_nodes.size(); + for(int i=0;im_nodes.size();++i) { - ps+=psb->getNodes()[i].m_x; + ps+=psb->m_nodes[i].m_x; } } ps/=nps; @@ -927,10 +1143,69 @@ void SoftDemo::keyboardCallback(unsigned char key, int x, int y) case '[': --current_demo;clientResetScene();break; case ',': m_raycast=!m_raycast;break; case ';': m_autocam=!m_autocam;break; + case '`': + { + btSoftBodyArray& sbs=getSoftDynamicsWorld()->getSoftBodyArray(); + for(int ib=0;ibstaticSolve(128); + } + } + break; default: DemoApplication::keyboardCallback(key,x,y); } } +// +void SoftDemo::mouseFunc(int button, int state, int x, int y) +{ +if(m_cutting&&(state==0)&&(button==0)) + { + const btVector3 rayFrom=m_cameraPosition; + const btVector3 rayTo=getRayTo(x,y); + const btVector3 rayDir=(rayTo-rayFrom).normalized(); + btSoftBodyArray& sbs=getSoftDynamicsWorld()->getSoftBodyArray(); + btSoftBody::sRayCast results; + results.time=SIMD_INFINITY; + for(int ib=0;ibrayCast(rayFrom,rayDir,res,results.time)) + { + results=res; + } + } + 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; + } + } +DemoApplication::mouseFunc(button,state,x,y); +} + void SoftDemo::initPhysics() { @@ -1051,3 +1326,4 @@ void SoftDemo::exitPhysics() + diff --git a/Demos/SoftDemo/SoftDemo.h b/Demos/SoftDemo/SoftDemo.h index 16affffc1..6c465feeb 100644 --- a/Demos/SoftDemo/SoftDemo.h +++ b/Demos/SoftDemo/SoftDemo.h @@ -53,6 +53,7 @@ public: bool m_autocam; + bool m_cutting; bool m_raycast; btScalar m_animtime; btClock m_clock; @@ -116,6 +117,7 @@ public: void clientResetScene(); void renderme(); void keyboardCallback(unsigned char key, int x, int y); + void mouseFunc(int button, int state, int x, int y); }; @@ -158,3 +160,4 @@ MACRO_SOFT_DEMO(16)//Init_BunnyMatch + diff --git a/Extras/CDTestFramework/BulletSAPCompleteBoxPruningTest.cpp b/Extras/CDTestFramework/BulletSAPCompleteBoxPruningTest.cpp index 83fd2ab68..e98cf8e7d 100644 --- a/Extras/CDTestFramework/BulletSAPCompleteBoxPruningTest.cpp +++ b/Extras/CDTestFramework/BulletSAPCompleteBoxPruningTest.cpp @@ -22,6 +22,7 @@ subject to the following restrictions: #include "RenderingHelpers.h" #include "GLFontRenderer.h" #include "btBulletCollisionCommon.h" +#include "BulletSoftBody/btDbvtBroadphase.h" int numParts =2; @@ -99,7 +100,10 @@ BulletSAPCompleteBoxPruningTest::BulletSAPCompleteBoxPruningTest(int numBoxes,in } break; - + case 7: + m_broadphase = new btDbvtBroadphase(); + methodname = "btDbvtBroadphase"; + break; default: { m_broadphase = new btAxisSweep3(aabbMin,aabbMax,numBoxes,new btNullPairCache()); @@ -179,7 +183,7 @@ bool BulletSAPCompleteBoxPruningTest::UpdateBoxes(int numBoxes) { static bool once=true; - for(udword i=0;igetOverlappingPairCache(); const btBroadphasePair* pairPtr = pairCache->getOverlappingPairArrayPtr(); - for(udword i=0;igetNumOverlappingPairs();i++) + for(udword i=0;i<(udword)pairCache->getNumOverlappingPairs();i++) { // Flags[pairPtr[i].m_pProxy0->getUid()-1] = true; // Flags[pairPtr[i].m_pProxy1->getUid()-1] = true; diff --git a/Extras/CDTestFramework/CDTestFramework.cpp b/Extras/CDTestFramework/CDTestFramework.cpp index 51d283127..9d7202894 100644 --- a/Extras/CDTestFramework/CDTestFramework.cpp +++ b/Extras/CDTestFramework/CDTestFramework.cpp @@ -32,8 +32,9 @@ subject to the following restrictions: //#define NUM_SAP_BOXES 8192 //#define NUM_SAP_BOXES 4096 -#define NUM_SAP_BOXES 4096 +#define NUM_SAP_BOXES 8192 +int percentUpdate = 10; //Broadphase comparison //Static case (updating 10% of objects to same position ( -> no swaps) @@ -73,8 +74,8 @@ enum TestIndex MAX_NB_TESTS }; -static int gTest = TEST_BULLET_MULTISAP_8192; -static int gSelectedTest = TEST_BULLET_MULTISAP_8192; +static int gTest = TEST_DBVT_8192;//TEST_BULLET_MULTISAP_8192; +static int gSelectedTest = TEST_DBVT_8192;//TEST_BULLET_MULTISAP_8192; static CollisionTest* gCollisionTests[MAX_NB_TESTS]; static GLFontRenderer gFnt; @@ -300,6 +301,7 @@ int main(int argc, char** argv) }; 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"); } // Create tests @@ -315,7 +317,7 @@ int main(int argc, char** argv) // 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_BIPARTITE_BOX_PRUNING] = new BipartiteBoxPruningTest; - gCollisionTests[TEST_DBVT_8192] = new DbvtTest(NUM_SAP_BOXES); + gCollisionTests[TEST_DBVT_8192] = new BulletSAPCompleteBoxPruningTest(NUM_SAP_BOXES,7); for(int i=0;iInit(); @@ -418,4 +420,4 @@ void BuildBulletTree() } -#endif \ No newline at end of file +#endif diff --git a/src/BulletCollision/BroadphaseCollision/btOverlappingPairCache.cpp b/src/BulletCollision/BroadphaseCollision/btOverlappingPairCache.cpp index 11bccfc9c..2c073166a 100644 --- a/src/BulletCollision/BroadphaseCollision/btOverlappingPairCache.cpp +++ b/src/BulletCollision/BroadphaseCollision/btOverlappingPairCache.cpp @@ -20,6 +20,8 @@ subject to the following restrictions: #include "btDispatcher.h" #include "btCollisionAlgorithm.h" +#include + int gOverlappingPairs = 0; int gRemovePairs =0; @@ -133,12 +135,12 @@ void btHashedOverlappingPairCache::removeOverlappingPairsContainingProxy(btBroad btBroadphasePair* btHashedOverlappingPairCache::findPair(btBroadphaseProxy* proxy0, btBroadphaseProxy* proxy1) { gFindPairs++; - + if(proxy0>proxy1) btSwap(proxy0,proxy1); int proxyId1 = proxy0->getUid(); int proxyId2 = proxy1->getUid(); - if (proxyId1 > proxyId2) - btSwap(proxyId1, proxyId2); + /*if (proxyId1 > proxyId2) + btSwap(proxyId1, proxyId2);*/ int hash = getHash(proxyId1, proxyId2) & (m_overlappingPairArray.capacity()-1); @@ -196,8 +198,8 @@ void btHashedOverlappingPairCache::growTables() const btBroadphasePair& pair = m_overlappingPairArray[i]; int proxyId1 = pair.m_pProxy0->getUid(); int proxyId2 = pair.m_pProxy1->getUid(); - if (proxyId1 > proxyId2) - btSwap(proxyId1, proxyId2); + /*if (proxyId1 > proxyId2) + btSwap(proxyId1, proxyId2);*/ int hashValue = getHash(proxyId1,proxyId2) & (m_overlappingPairArray.capacity()-1); // New hash value with new mask m_next[i] = m_hashTable[hashValue]; m_hashTable[hashValue] = i; @@ -209,11 +211,12 @@ void btHashedOverlappingPairCache::growTables() btBroadphasePair* btHashedOverlappingPairCache::internalAddPair(btBroadphaseProxy* proxy0, btBroadphaseProxy* proxy1) { + if(proxy0>proxy1) btSwap(proxy0,proxy1); int proxyId1 = proxy0->getUid(); int proxyId2 = proxy1->getUid(); - if (proxyId1 > proxyId2) - btSwap(proxyId1, proxyId2); + /*if (proxyId1 > proxyId2) + btSwap(proxyId1, proxyId2);*/ int hash = getHash(proxyId1, proxyId2) & (m_overlappingPairArray.capacity()-1); @@ -224,7 +227,15 @@ btBroadphasePair* btHashedOverlappingPairCache::internalAddPair(btBroadphaseProx { return pair; } - + /*for(int i=0;i%u\r\n",proxyId1,proxyId2); + internalFindPair(proxy0, proxy1, hash); + } + }*/ int count = m_overlappingPairArray.size(); int oldCapacity = m_overlappingPairArray.capacity(); void* mem = &m_overlappingPairArray.expand(); @@ -255,12 +266,12 @@ btBroadphasePair* btHashedOverlappingPairCache::internalAddPair(btBroadphaseProx void* btHashedOverlappingPairCache::removeOverlappingPair(btBroadphaseProxy* proxy0, btBroadphaseProxy* proxy1,btDispatcher* dispatcher) { gRemovePairs++; - + if(proxy0>proxy1) btSwap(proxy0,proxy1); int proxyId1 = proxy0->getUid(); int proxyId2 = proxy1->getUid(); - if (proxyId1 > proxyId2) - btSwap(proxyId1, proxyId2); + /*if (proxyId1 > proxyId2) + btSwap(proxyId1, proxyId2);*/ int hash = getHash(proxyId1, proxyId2) & (m_overlappingPairArray.capacity()-1); @@ -316,6 +327,7 @@ void* btHashedOverlappingPairCache::removeOverlappingPair(btBroadphaseProxy* pro // Remove the last pair from the hash table. const btBroadphasePair* last = &m_overlappingPairArray[lastPairIndex]; + /* missing swap here too, Nat. */ int lastHash = getHash(last->m_pProxy0->getUid(), last->m_pProxy1->getUid()) & (m_overlappingPairArray.capacity()-1); index = m_hashTable[lastHash]; @@ -416,6 +428,7 @@ btBroadphasePair* btSortedOverlappingPairCache::addOverlappingPair(btBroadphaseP void* mem = &m_overlappingPairArray.expand(); btBroadphasePair* pair = new (mem) btBroadphasePair(*proxy0,*proxy1); gOverlappingPairs++; + gAddedPairs++; return pair; } @@ -500,6 +513,7 @@ void btSortedOverlappingPairCache::cleanOverlappingPair(btBroadphasePair& pair,b pair.m_algorithm->~btCollisionAlgorithm(); dispatcher->freeCollisionAlgorithm(pair.m_algorithm); pair.m_algorithm=0; + gRemovePairs--; } } } diff --git a/src/BulletCollision/BroadphaseCollision/btOverlappingPairCache.h b/src/BulletCollision/BroadphaseCollision/btOverlappingPairCache.h index 2228a8451..dec6aee1c 100644 --- a/src/BulletCollision/BroadphaseCollision/btOverlappingPairCache.h +++ b/src/BulletCollision/BroadphaseCollision/btOverlappingPairCache.h @@ -225,8 +225,10 @@ private: { int proxyId1 = proxy0->getUid(); int proxyId2 = proxy1->getUid(); + #if 0 // wrong, 'equalsPair' use unsorted uids, copy-past devil striked again. Nat. if (proxyId1 > proxyId2) btSwap(proxyId1, proxyId2); + #endif int index = m_hashTable[hash]; @@ -432,3 +434,4 @@ public: #endif //OVERLAPPING_PAIR_CACHE_H + diff --git a/src/BulletSoftBody/CMakeLists.txt b/src/BulletSoftBody/CMakeLists.txt index 05c927452..753ab5484 100644 --- a/src/BulletSoftBody/CMakeLists.txt +++ b/src/BulletSoftBody/CMakeLists.txt @@ -8,6 +8,8 @@ ADD_LIBRARY(LibBulletSoftBody btSoftBody.h btSoftBodyHelpers.cpp btSparseSDF.h + btDbvtBroadphase.cpp + btDbvtBroadphase.h btDbvt.cpp btDbvt.h btSoftBodyHelpers.h @@ -16,6 +18,8 @@ ADD_LIBRARY(LibBulletSoftBody btSoftRigidCollisionAlgorithm.h btSoftSoftCollisionAlgorithm.cpp btSoftSoftCollisionAlgorithm.h + btSoftBodyConcaveCollisionAlgorithm.cpp + btSoftBodyConcaveCollisionAlgorithm.h btSoftRigidDynamicsWorld.h btSoftRigidDynamicsWorld.cpp ) diff --git a/src/BulletSoftBody/btDbvt.cpp b/src/BulletSoftBody/btDbvt.cpp index 189f3a308..52adc3014 100644 --- a/src/BulletSoftBody/btDbvt.cpp +++ b/src/BulletSoftBody/btDbvt.cpp @@ -1,3 +1,19 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ +///btDbvt implementation by Nathanael Presson + #include "btDbvt.h" // @@ -10,16 +26,16 @@ return(node->parent->childs[1]==node); } // -static inline btDbvt::Aabb merge( const btDbvt::Aabb& a, - const btDbvt::Aabb& b) +static inline btDbvt::Volume merge( const btDbvt::Volume& a, + const btDbvt::Volume& b) { -btDbvt::Aabb res; +btDbvt::Volume res; Merge(a,b,res); return(res); } // volume+edge lengths -static inline btScalar size(const btDbvt::Aabb& a) +static inline btScalar size(const btDbvt::Volume& a) { const btVector3 edges=a.Lengths(); return( edges.x()*edges.y()*edges.z()+ @@ -50,7 +66,7 @@ deletenode(pdbvt,node); // static inline btDbvt::Node* createnode( btDbvt* pdbvt, btDbvt::Node* parent, - const btDbvt::Aabb& box, + const btDbvt::Volume& volume, void* data) { btDbvt::Node* node; @@ -59,7 +75,7 @@ if(pdbvt->m_free) else { node=new btDbvt::Node(); } node->parent = parent; -node->box = box; +node->volume = volume; node->data = data; node->childs[1] = 0; return(node); @@ -80,25 +96,25 @@ if(!pdbvt->m_root) if(!root->isleaf()) { do { - if( Proximity(root->childs[0]->box,leaf->box)< - Proximity(root->childs[1]->box,leaf->box)) + if( Proximity(root->childs[0]->volume,leaf->volume)< + Proximity(root->childs[1]->volume,leaf->volume)) root=root->childs[0]; else root=root->childs[1]; } while(!root->isleaf()); } btDbvt::Node* prev=root->parent; - btDbvt::Node* node=createnode(pdbvt,prev,merge(leaf->box,root->box),0); + 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->box.Contain(node->box)) + if(prev->volume.Contain(node->volume)) break; else - Merge(prev->childs[0]->box,prev->childs[1]->box,prev->box); + Merge(prev->childs[0]->volume,prev->childs[1]->volume,prev->volume); node=prev; } while(0!=(prev=node->parent)); } @@ -132,9 +148,9 @@ if(leaf==pdbvt->m_root) deletenode(pdbvt,parent); while(prev) { - const btDbvt::Aabb pb=prev->box; - Merge(prev->childs[0]->box,prev->childs[1]->box,prev->box); - if(NotEqual(pb,prev->box)) + 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; @@ -170,17 +186,6 @@ if(root->isinternal()&&depth) } } -// -static int leafcount(btDbvt::Node* root) -{ -if(root->isinternal()) - { - return( leafcount(root->childs[0])+ - leafcount(root->childs[1])); - } -return(1); -} - // static void split( const tNodeArray& leafs, tNodeArray& left, @@ -192,7 +197,7 @@ left.resize(0); right.resize(0); for(int i=0,ni=leafs.size();ibox.Center()-org)<0) + if(dot(axis,leafs[i]->volume.Center()-org)<0) left.push_back(leafs[i]); else right.push_back(leafs[i]); @@ -200,14 +205,14 @@ for(int i=0,ni=leafs.size();ibox; +btDbvt::Volume volume=leafs[0]->volume; for(int i=1,ni=leafs.size();ibox); + volume=merge(volume,leafs[i]->volume); } -return(box); +return(volume); } // @@ -222,7 +227,7 @@ while(leafs.size()>1) { for(int j=i+1;jbox,leafs[j]->box)); + const btScalar sz=size(merge(leafs[i]->volume,leafs[j]->volume)); if(sz1) } } btDbvt::Node* n[] = {leafs[minidx[0]],leafs[minidx[1]]}; - btDbvt::Node* p = createnode(pdbvt,0,merge(n[0]->box,n[1]->box),0); + 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; @@ -255,15 +260,15 @@ if(leafs.size()>1) { if(leafs.size()>bu_treshold) { - const btDbvt::Aabb box=bounds(leafs); - const btVector3 org=box.Center(); - tNodeArray sets[2]; - int bestaxis=-1; - int bestmidp=leafs.size(); - int splitcount[3][2]={0,0,0,0,0,0}; + 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;ibox.Center()-org; + const btVector3 x=leafs[i]->volume.Center()-org; for(int j=0;j<3;++j) { ++splitcount[j][dot(x,axis[j])>0?1:0]; @@ -296,7 +301,7 @@ if(leafs.size()>1) sets[i&1].push_back(leafs[i]); } } - btDbvt::Node* node=createnode(pdbvt,0,box,0); + 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; @@ -344,6 +349,7 @@ return(node); m_root = 0; m_free = 0; m_lkhd = 2; +m_leafs = 0; } // @@ -360,21 +366,13 @@ delete m_free; m_free=0; } -// -int btDbvt::leafCount() const -{ -if(m_root) return(leafcount(m_root)); - else - return(0); -} - // void btDbvt::optimizeBottomUp() { if(m_root) { tNodeArray leafs; - leafs.reserve(leafCount()); + leafs.reserve(m_leafs); fetchleafs(this,m_root,leafs); bottomup(this,leafs); m_root=leafs[0]; @@ -387,22 +385,37 @@ void btDbvt::optimizeTopDown(int bu_treshold) if(m_root) { tNodeArray leafs; - leafs.reserve(leafCount()); + leafs.reserve(m_leafs); fetchleafs(this,m_root,leafs); m_root=topdown(this,leafs,bu_treshold); } } // -btDbvt::Node* btDbvt::insert(const Aabb& box,void* data) +btDbvt::Node* btDbvt::insert(const Volume& volume,void* data) { -Node* leaf=createnode(this,0,box,data); +Node* leaf=createnode(this,0,volume,data); insertleaf(this,m_root,leaf); +++m_leafs; return(leaf); } // -void btDbvt::update(Node* leaf,const Aabb& box) +void btDbvt::update(Node* leaf,int lookahead) +{ +Node* root=removeleaf(this,leaf); +if(root) + { + for(int i=0;(iparent;++i) + { + root=root->parent; + } + } +insertleaf(this,root,leaf); +} + +// +void btDbvt::update(Node* leaf,const Volume& volume) { Node* root=removeleaf(this,leaf); if(root) @@ -412,19 +425,35 @@ if(root) root=root->parent; } } -leaf->box=box; +leaf->volume=volume; insertleaf(this,root,leaf); } // -bool btDbvt::update(Node* leaf,Aabb box,const btVector3& velocity,btScalar margin) +bool btDbvt::update(Node* leaf,Volume volume,const btVector3& velocity,btScalar margin) { -if(leaf->box.Contain(box)) return(false); -if(margin>0) - box.Expand(btVector3(margin,margin,margin)); -if(velocity.length2()>0) - box.SignedExpand(velocity); -update(leaf,box); +if(leaf->volume.Contain(volume)) return(false); +volume.Expand(btVector3(margin,margin,margin)); +volume.SignedExpand(velocity); +update(leaf,volume); +return(true); +} + +// +bool btDbvt::update(Node* leaf,Volume volume,const btVector3& velocity) +{ +if(leaf->volume.Contain(volume)) return(false); +volume.SignedExpand(velocity); +update(leaf,volume); +return(true); +} + +// +bool btDbvt::update(Node* leaf,Volume volume,btScalar margin) +{ +if(leaf->volume.Contain(volume)) return(false); +volume.Expand(btVector3(margin,margin,margin)); +update(leaf,volume); return(true); } @@ -433,109 +462,32 @@ void btDbvt::remove(Node* leaf) { removeleaf(this,leaf); deletenode(this,leaf); +--m_leafs; } // void btDbvt::collide(btDbvt* tree, ICollide* icollide) const { -if(tree->m_root&&m_root) - { - btAlignedObjectArray stack; - stack.reserve(128); - stack.push_back(sStkElm(m_root,tree->m_root)); - 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->box,p.b->box)) - { - 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 - { - icollide->Process(p.a,p.b); - } - } - } - } while(stack.size()>0); - } +collideGeneric(tree,GCollide(icollide)); +} + +// +void btDbvt::collide(btDbvt::Node* node, + ICollide* icollide) const +{ +collideGeneric(node,GCollide(icollide)); } // -void btDbvt::collide(const Aabb& box, +void btDbvt::collide(const Volume& volume, ICollide* icollide) const { -if(m_root) - { - btAlignedObjectArray stack; - stack.reserve(64); - stack.push_back(m_root); - do { - const Node* n=stack[stack.size()-1]; - stack.pop_back(); - if(Intersect(n->box,box)) - { - if(n->isinternal()) - { - stack.push_back(n->childs[0]); - stack.push_back(n->childs[1]); - } - else - { - icollide->Process(n); - } - } - } while(stack.size()>0); - } +collideGeneric(volume,GCollide(icollide)); } // void btDbvt::collide(ICollide* icollide) const { -if(m_root) - { - btAlignedObjectArray stack; - stack.reserve(64); - stack.push_back(m_root); - do { - const Node* n=stack[stack.size()-1]; - stack.pop_back(); - if(icollide->Descent(n)) - { - if(n->isinternal()) - { stack.push_back(n->childs[0]);stack.push_back(n->childs[1]); } - else - { icollide->Process(n); } - } - } while(stack.size()>0); - } +collideGeneric(GCollide(icollide)); } diff --git a/src/BulletSoftBody/btDbvt.h b/src/BulletSoftBody/btDbvt.h index 1861f61d4..2a1030686 100644 --- a/src/BulletSoftBody/btDbvt.h +++ b/src/BulletSoftBody/btDbvt.h @@ -12,7 +12,7 @@ subject to the following restrictions: 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. */ -///btSoftBody implementation by Nathanael Presson +///btDbvt implementation by Nathanael Presson #ifndef BT_DYNAMIC_BOUNDING_VOLUME_TREE_H #define BT_DYNAMIC_BOUNDING_VOLUME_TREE_H @@ -20,46 +20,52 @@ subject to the following restrictions: #include "LinearMath/btAlignedObjectArray.h" #include "LinearMath/btVector3.h" +// +// Defaults volumes +// + +/* 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); +private: +btVector3 mi,mx; +}; + // // Dynamic bounding volume tree // struct btDbvt { - // Types - - /* Aabb */ - struct Aabb - { - 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 Aabb FromCE(const btVector3& c,const btVector3& e); - static inline Aabb FromCR(const btVector3& c,btScalar r); - static inline Aabb FromMM(const btVector3& mi,const btVector3& mx); - static inline Aabb FromPoints(const btVector3* pts,int n); - static inline Aabb FromPoints(const btVector3** ppts,int n); - inline void Expand(const btVector3 e); - inline void SignedExpand(const btVector3 e); - inline bool Contain(const Aabb& a) const; - inline friend bool Intersect( const Aabb& a, - const Aabb& b); - inline friend bool Intersect( const Aabb& a, - const btVector3& b); - inline friend btScalar Proximity( const Aabb& a, - const Aabb& b); - inline friend void Merge( const Aabb& a, - const Aabb& b, - Aabb& r); - inline friend bool NotEqual( const Aabb& a, - const Aabb& b); - btVector3 mi,mx; - }; + // Types + typedef btDbvtAabbMm Volume; /* Node */ struct Node { - Aabb box; + Volume volume; Node* parent; bool isleaf() const { return(childs[1]==0); } bool isinternal() const { return(!isleaf()); } @@ -83,55 +89,62 @@ struct btDbvt { virtual void Process(const Node*,const Node*) {} virtual void Process(const Node*) {} - virtual bool Descent(const Node*) { return(false); } + virtual bool Descent(const Node*) { return(false); } }; + /* GCollide */ + struct GCollide + { + ICollide* icollide; + GCollide(ICollide* ic) : icollide(ic) {} + void Process(const Node* a,const Node* b) { icollide->Process(a,b); } + void Process(const Node* a) { icollide->Process(a); } + bool Descent(const Node* a) { return(icollide->Descent(a)); } + }; + + // Constants + enum { + TREETREE_STACKSIZE = 128, + VOLUMETREE_STACKSIZE = 64, + }; // Fields Node* m_root; Node* m_free; int m_lkhd; + int m_leafs; // Methods btDbvt(); ~btDbvt(); void clear(); bool empty() const { return(0==m_root); } - int leafCount() const; void optimizeBottomUp(); void optimizeTopDown(int bu_treshold=128); - Node* insert(const Aabb& box,void* data); - void update(Node* leaf,const Aabb& box); - bool update(Node* leaf,Aabb box,const btVector3& velocity,btScalar margin); + Node* insert(const Volume& box,void* data); + 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(const Aabb& box, + 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 - template - void collideGeneric(T& policy) const - { - if(m_root) - { - btAlignedObjectArray stack; - stack.reserve(64); - stack.push_back(m_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); } - } - } while(stack.size()>0); - } - } + // Generics : T must implement ICollide + template + void collideGeneric( btDbvt* tree,T& policy) const; + template + void collideGeneric( btDbvt::Node* node,T& policy) const; + template + void collideGeneric(const Volume& volume,T& policy) const; + template + void collideGeneric(T& policy) const; // private: btDbvt(const btDbvt&) {} @@ -142,31 +155,31 @@ struct btDbvt // // -inline btDbvt::Aabb btDbvt::Aabb::FromCE(const btVector3& c,const btVector3& e) +inline btDbvtAabbMm btDbvtAabbMm::FromCE(const btVector3& c,const btVector3& e) { -Aabb box; +btDbvtAabbMm box; box.mi=c-e;box.mx=c+e; return(box); } // -inline btDbvt::Aabb btDbvt::Aabb::FromCR(const btVector3& c,btScalar r) +inline btDbvtAabbMm btDbvtAabbMm::FromCR(const btVector3& c,btScalar r) { return(FromCE(c,btVector3(r,r,r))); } // -inline btDbvt::Aabb btDbvt::Aabb::FromMM(const btVector3& mi,const btVector3& mx) +inline btDbvtAabbMm btDbvtAabbMm::FromMM(const btVector3& mi,const btVector3& mx) { -Aabb box; +btDbvtAabbMm box; box.mi=mi;box.mx=mx; return(box); } // -inline btDbvt::Aabb btDbvt::Aabb::FromPoints(const btVector3* pts,int n) +inline btDbvtAabbMm btDbvtAabbMm::FromPoints(const btVector3* pts,int n) { -Aabb box; +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()); @@ -204,7 +217,7 @@ if(e.z()>0) mx.setZ(mx.z()+e.z()); else mi.setZ(mi.z()+e.z()); } // -inline bool btDbvt::Aabb::Contain(const Aabb& a) const +inline bool btDbvtAabbMm::Contain(const btDbvtAabbMm& a) const { return( (mi.x()<=a.mi.x())&& (mi.y()<=a.mi.y())&& @@ -215,19 +228,33 @@ return( (mi.x()<=a.mi.x())&& } // -inline bool Intersect( const btDbvt::Aabb& a, - const btDbvt::Aabb& b) +inline bool Intersect( const btDbvtAabbMm& a, + 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); +#else 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())&& (a.mx.y()>=b.mi.y())&& (a.mx.z()>=b.mi.z())); +#endif } // -inline bool Intersect( const btDbvt::Aabb& a, +inline bool Intersect( const btDbvtAabbMm& a, const btVector3& b) { return( (b.x()>=a.mi.x())&& @@ -239,17 +266,17 @@ return( (b.x()>=a.mi.x())&& } // -inline btScalar Proximity( const btDbvt::Aabb& a, - const btDbvt::Aabb& b) +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 btDbvt::Aabb& a, - const btDbvt::Aabb& b, - btDbvt::Aabb& r) +inline void Merge( const btDbvtAabbMm& a, + const btDbvtAabbMm& b, + btDbvtAabbMm& r) { r=a; r.mi.setMin(b.mi); @@ -257,8 +284,8 @@ r.mx.setMax(b.mx); } // -inline bool NotEqual( const btDbvt::Aabb& a, - const btDbvt::Aabb& b) +inline bool NotEqual( const btDbvtAabbMm& a, + const btDbvtAabbMm& b) { return( (a.mi.x()!=b.mi.x())|| (a.mi.y()!=b.mi.y())|| @@ -268,4 +295,121 @@ return( (a.mi.x()!=b.mi.x())|| (a.mx.z()!=b.mx.z())); } +// +// Generic's +// + +// +template +inline void btDbvt::collideGeneric( btDbvt::Node* node,T& policy) const +{ +if(m_root&&node) + { + 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); + } +} + +// +template +inline void btDbvt::collideGeneric( btDbvt* tree,T& policy) const +{ +collideGeneric(tree->m_root,policy); +} + +// +template +inline void btDbvt::collideGeneric(const Volume& volume,T& policy) const +{ +if(m_root) + { + 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); + } +} + +// +template +inline void btDbvt::collideGeneric(T& policy) const +{ +if(m_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)) + { + if(n->isinternal()) + { stack.push_back(n->childs[0]);stack.push_back(n->childs[1]); } + else + { policy.Process(n); } + } + } while(stack.size()>0); + } +} + #endif diff --git a/src/BulletSoftBody/btDbvtBroadphase.cpp b/src/BulletSoftBody/btDbvtBroadphase.cpp new file mode 100644 index 000000000..d3dfaa9af --- /dev/null +++ b/src/BulletSoftBody/btDbvtBroadphase.cpp @@ -0,0 +1,342 @@ +/* +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" + +// +// Profiling +// + +#if DBVT_BP_PROFILE +#include +struct ProfileScope + { + 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_) +#endif + +// +// Helpers +// + +// +static inline int hash(unsigned int i,unsigned int j) +{ +int key=((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; +} + +// +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]; +} + +// +template +static inline int listcount(T* root) +{ +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; +} + +// +// Collider +// +struct btDbvtBroadphaseCollider : btDbvt::ICollide +{ +btDbvtBroadphase* pbp; +int pid; + btDbvtBroadphaseCollider(btDbvtBroadphase* p,int id) : pbp(p),pid(id) {} +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 + { + btBroadphasePair* pp=pbp->m_paircache->addOverlappingPair(pa,pb); + if(pp) pp->m_userInfo=*(void**)&pid; + } + } +}; + +// +// btDbvtBroadphase +// + +// +btDbvtBroadphase::btDbvtBroadphase() +{ +m_fcursor = 0; +m_dcursor = 0; +m_stageCurrent = 0; +m_fupdates = 1; +m_dupdates = 0; +m_paircache = new btHashedOverlappingPairCache(); +m_gid = 0; +m_pid = 0; +for(int i=0;i<=STAGECOUNT;++i) + { + m_stageRoots[i]=0; + } +#if DBVT_BP_PROFILE +clear(m_profiling); +#endif +} + +// +btDbvtBroadphase::~btDbvtBroadphase() +{ +delete 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) +{ +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); +} + +// +void btDbvtBroadphase::destroyProxy( btBroadphaseProxy* absproxy, + btDispatcher* dispatcher) +{ +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; +} + +// +void btDbvtBroadphase::setAabb( btBroadphaseProxy* absproxy, + const btVector3& aabbMin, + const btVector3& aabbMax, + btDispatcher* dispatcher) +{ +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; + } + else + {/* dynamic set */ + const btVector3 delta=(aabbMin+aabbMax)/2-proxy->aabb.Center(); + m_sets[0].update(proxy->leaf,aabb,delta*PREDICTED_FRAMES,DBVT_BP_MARGIN); + } +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); +#if DBVT_BP_PROFILE +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(); + } +#endif +} + +// +void btDbvtBroadphase::collide(btDispatcher* dispatcher) +{ +SPC(m_profiling.m_total); +/* refine dynamic */ +if(m_stageRoots[m_stageCurrent]&&(m_dupdates>0)) + { + 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; + } while(current); + } +/* refine fixed */ +if(m_stageRoots[STAGECOUNT]&&(m_fupdates>0)) + { + 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); + } + { + SPC(m_profiling.m_ddcollide); + m_sets[0].collideGeneric(&m_sets[0],collider); + } +/* clean up */ + { + SPC(m_profiling.m_cleanup); + btBroadphasePairArray& pairs=m_paircache->getOverlappingPairArray(); + for(int i=0,ni=pairs.size();iaabb,pb->aabb)) + { + m_paircache->removeOverlappingPair(pa,pb,dispatcher); + --ni;--i; + } + } + } + } +++m_pid; +} + +// +btOverlappingPairCache* btDbvtBroadphase::getOverlappingPairCache() +{ +return(m_paircache); +} + +// +const btOverlappingPairCache* btDbvtBroadphase::getOverlappingPairCache() const +{ +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(); +} + +// +void btDbvtBroadphase::printStats() +{} + +#if DBVT_BP_PROFILE +#undef SPC +#endif diff --git a/src/BulletSoftBody/btDbvtBroadphase.h b/src/BulletSoftBody/btDbvtBroadphase.h new file mode 100644 index 000000000..9a37fe466 --- /dev/null +++ b/src/BulletSoftBody/btDbvtBroadphase.h @@ -0,0 +1,108 @@ +/* +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 + +#ifndef BT_DBVT_BROADPHASE_H +#define BT_DBVT_BROADPHASE_H + +#include "btDbvt.h" +#include "LinearMath/btPoint3.h" +#include "LinearMath/btVector3.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_MARGIN (btScalar)0.05 + +#if DBVT_BP_PROFILE + #define DBVT_BP_PROFILING_RATE 50 + #include "LinearMath/btQuickprof.h" +#endif + +// +// btDbvtProxy +// +struct btDbvtProxy : btBroadphaseProxy +{ +/* 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; + } +}; + +// +// btDbvtBroadphase +// +struct btDbvtBroadphase : btBroadphaseInterface +{ +/* 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 +#if DBVT_BP_PROFILE +btClock m_clock; +struct { + unsigned long m_total; + unsigned long m_ddcollide; + unsigned long m_fdcollide; + unsigned long m_cleanup; + } 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(); +}; + +#endif + diff --git a/src/BulletSoftBody/btSoftBody.cpp b/src/BulletSoftBody/btSoftBody.cpp index 74ece5793..3faa01264 100644 --- a/src/BulletSoftBody/btSoftBody.cpp +++ b/src/BulletSoftBody/btSoftBody.cpp @@ -25,6 +25,32 @@ subject to the following restrictions: #include "BulletCollision/BroadphaseCollision/btBroadphaseInterface.h" #include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h" +// +// btSymMatrix +// +template +struct btSymMatrix +{ + btSymMatrix() : dim(0) {} + btSymMatrix(int n,const T& init=T()) { resize(n,init); } +void resize(int n,const T& init=T()) { dim=n;store.resize((n*(n+1))/2,init); } +T& operator()(int c,int r) + { + if(c>r) btSwap(c,r); + btAssert(rr) btSwap(c,r); + btAssert(r store; +int dim; +}; + + // // Collision shape // @@ -34,10 +60,7 @@ class btSoftBodyCollisionShape : public btConcaveShape { public: btSoftBody* m_body; - btSoftBody::tNodeArray m_nodes; // Nodes - btSoftBody::tLinkArray m_links; // Links - btSoftBody::tFaceArray m_faces; // Faces - + btSoftBodyCollisionShape(btSoftBody* backptr); virtual ~btSoftBodyCollisionShape(); @@ -125,11 +148,30 @@ static inline void Trace(const btMatrix3x3& m,const char* name) static inline void Trace(const btMatrix3x3&,const char*) {} #endif +// +template +static inline void ZeroInitialize(T& value) +{ +static const T zerodummy; +value=zerodummy; +} +// +template +static inline bool CompLess(const T& a,const T& b) +{ return(a +static inline bool CompGreater(const T& a,const T& b) +{ return(a>b); } // template static inline T Lerp(const T& a,const T& b,btScalar t) { return(a+(b-a)*t); } // +template +static inline T InvLerp(const T& a,const T& b,btScalar t) +{ return((b+a*t-b*t)/(a*b)); } +// static inline btMatrix3x3 Lerp( const btMatrix3x3& a, const btMatrix3x3& b, btScalar t) @@ -249,8 +291,8 @@ static inline void PolarDecompose( const btMatrix3x3& m, } /* Final orthogonalization */ q[0]=q[0].normalized(); - q[1]=q[1].normalized(); q[2]=cross(q[0],q[1]).normalized(); + q[1]=cross(q[2],q[0]).normalized(); /* Compute 'S' */ s=q.transpose()*m; } @@ -347,6 +389,36 @@ const btScalar isum=1/(w[0]+w[1]+w[2]); return(btVector3(w[1]*isum,w[2]*isum,w[0]*isum)); } +// +static btScalar ImplicitSolve( btSoftBody::ImplicitFn* fn, + const btVector3& a, + const btVector3& b, + const btScalar accuracy, + const int maxiterations=256) +{ +btScalar span[2]={0,1}; +btScalar values[2]={fn->Eval(a),fn->Eval(b)}; +if(values[0]>values[1]) + { + btSwap(span[0],span[1]); + btSwap(values[0],values[1]); + } +if(values[0]>-accuracy) return(-1); +if(values[1]<+accuracy) return(-1); +for(int i=0;iEval(Lerp(a,b,t)); + if((t<=0)||(t>=1)) break; + if(btFabs(v)((_p_)-(_b_)) - btSoftBody::Node* base=&psb->getNodes()[0]; - for(int i=0,ni=psb->getLinks().size();im_nodes[0]; + for(int i=0,ni=psb->m_nodes.size();igetLinks()[i].m_n[0]=PTR2IDX(psb->getLinks()[i].m_n[0],base); - psb->getLinks()[i].m_n[1]=PTR2IDX(psb->getLinks()[i].m_n[1],base); + if(psb->m_nodes[i].m_leaf) + { + psb->m_nodes[i].m_leaf->data=*(void**)&i; + } } - for(int i=0,ni=psb->getFaces().size();im_links.size();igetFaces()[i].m_n[0]=PTR2IDX(psb->getFaces()[i].m_n[0],base); - psb->getFaces()[i].m_n[1]=PTR2IDX(psb->getFaces()[i].m_n[1],base); - psb->getFaces()[i].m_n[2]=PTR2IDX(psb->getFaces()[i].m_n[2],base); + psb->m_links[i].m_n[0]=PTR2IDX(psb->m_links[i].m_n[0],base); + psb->m_links[i].m_n[1]=PTR2IDX(psb->m_links[i].m_n[1],base); } + for(int i=0,ni=psb->m_faces.size();im_faces[i].m_n[0]=PTR2IDX(psb->m_faces[i].m_n[0],base); + psb->m_faces[i].m_n[1]=PTR2IDX(psb->m_faces[i].m_n[1],base); + psb->m_faces[i].m_n[2]=PTR2IDX(psb->m_faces[i].m_n[2],base); + if(psb->m_faces[i].m_leaf) + { + 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); + } + for(int i=0,ni=psb->m_notes.size();im_notes[i].m_rank;++j) + { + psb->m_notes[i].m_nodes[j]=PTR2IDX(psb->m_notes[i].m_nodes[j],base); + } + } #undef PTR2IDX } -#endif // -#if 0 -static void IndicesToPointers(btSoftBody* psb) +static void IndicesToPointers(btSoftBody* psb,const int* map=0) { -#define IDX2PTR(_p_,_b_) ((_b_)+(((char*)_p_)-(char*)0)) - btSoftBody::Node* base=&psb->getNodes()[0]; - for(int i=0,ni=psb->getLinks().size();im_nodes[0]; + for(int i=0,ni=psb->m_nodes.size();igetLinks()[i].m_n[0]=IDX2PTR(psb->getLinks()[i].m_n[0],base); - psb->getLinks()[i].m_n[1]=IDX2PTR(psb->getLinks()[i].m_n[1],base); + if(psb->m_nodes[i].m_leaf) + { + psb->m_nodes[i].m_leaf->data=&psb->m_nodes[i]; + } } - for(int i=0,ni=psb->getFaces().size();im_links.size();igetFaces()[i].m_n[0]=IDX2PTR(psb->getFaces()[i].m_n[0],base); - psb->getFaces()[i].m_n[1]=IDX2PTR(psb->getFaces()[i].m_n[1],base); - psb->getFaces()[i].m_n[2]=IDX2PTR(psb->getFaces()[i].m_n[2],base); + psb->m_links[i].m_n[0]=IDX2PTR(psb->m_links[i].m_n[0],base); + psb->m_links[i].m_n[1]=IDX2PTR(psb->m_links[i].m_n[1],base); } + for(int i=0,ni=psb->m_faces.size();im_faces[i].m_n[0]=IDX2PTR(psb->m_faces[i].m_n[0],base); + psb->m_faces[i].m_n[1]=IDX2PTR(psb->m_faces[i].m_n[1],base); + psb->m_faces[i].m_n[2]=IDX2PTR(psb->m_faces[i].m_n[2],base); + if(psb->m_faces[i].m_leaf) + { + 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); + } + for(int i=0,ni=psb->m_notes.size();im_notes[i].m_rank;++j) + { + psb->m_notes[i].m_nodes[j]=IDX2PTR(psb->m_notes[i].m_nodes[j],base); + } + } #undef IDX2PTR } -#endif // -static inline btDbvt::Aabb BoxOf( const btSoftBody::Face& f, - btScalar margin) +static inline btDbvt::Volume VolumeOf( const btSoftBody::Face& f, + btScalar margin) { const btVector3* pts[]={ &f.m_n[0]->m_x, &f.m_n[1]->m_x, &f.m_n[2]->m_x}; -btDbvt::Aabb aabb=btDbvt::Aabb::FromPoints(pts,3); -aabb.Expand(btVector3(margin,margin,margin)); -return(aabb); +btDbvt::Volume vol=btDbvt::Volume::FromPoints(pts,3); +vol.Expand(btVector3(margin,margin,margin)); +return(vol); } // -static inline btScalar AreaOf( const btVector3& x0, - const btVector3& x1, - const btVector3& x2) +static inline btScalar AreaOf( const btVector3& x0, + const btVector3& x1, + const btVector3& x2) { const btVector3 a=x1-x0; const btVector3 b=x2-x0; @@ -425,9 +560,9 @@ static inline btScalar AreaOf( const btVector3& x0, // static inline btScalar VolumeOf( const btVector3& x0, - const btVector3& x1, - const btVector3& x2, - const btVector3& x3) + const btVector3& x1, + const btVector3& x2, + const btVector3& x3) { const btVector3 a=x1-x0; const btVector3 b=x2-x0; @@ -437,11 +572,11 @@ static inline btScalar VolumeOf( const btVector3& x0, // static inline btScalar RayTriangle(const btVector3& org, - const btVector3& dir, - const btVector3& a, - const btVector3& b, - const btVector3& c, - btScalar maxt=SIMD_INFINITY) + const btVector3& dir, + const btVector3& a, + const btVector3& b, + const btVector3& c, + btScalar maxt=SIMD_INFINITY) { static const btScalar ceps=-SIMD_EPSILON*10; static const btScalar teps=SIMD_EPSILON*10; @@ -504,8 +639,8 @@ struct RayCaster : public btDbvt::ICollide } bool Descent(const btDbvt::Node* node) { - const btVector3 ctr=node->box.Center()-o; - const btScalar sqr=node->box.Lengths().length2()/4; + 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); } @@ -517,23 +652,63 @@ static int RaycastInternal(const btSoftBody* psb, const btVector3& org, const btVector3& dir, btScalar& mint, - int& face, + btSoftBody::eFeature::_& feature, + int& index, bool bcountonly) { int cnt=0; if(bcountonly||psb->m_fdbvt.empty()) {/* Full search */ - for(int i=0,ni=psb->getFaces().size();igetFaces()[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) + 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, + 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();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::Face; + index=i; + mint=t; + } + } } } } @@ -544,7 +719,8 @@ static int RaycastInternal(const btSoftBody* psb, if(collider.face) { mint=collider.mint; - face=(int)(collider.face-&psb->getFaces()[0]); + feature=btSoftBody::eFeature::Face; + index=(int)(collider.face-&psb->m_faces[0]); cnt=1; } } @@ -555,10 +731,10 @@ static int RaycastInternal(const btSoftBody* psb, static void InitializeFaceTree(btSoftBody* psb) { psb->m_fdbvt.clear(); -for(int i=0;igetFaces().size();++i) +for(int i=0;im_faces.size();++i) { - btSoftBody::Face& f=psb->getFaces()[i]; - f.m_leaf=psb->m_fdbvt.insert(BoxOf(f,0),&f); + btSoftBody::Face& f=psb->m_faces[i]; + f.m_leaf=psb->m_fdbvt.insert(VolumeOf(f,0),&f); } psb->m_fdbvt.optimizeTopDown(); } @@ -566,12 +742,12 @@ psb->m_fdbvt.optimizeTopDown(); // static btVector3 EvaluateCom(btSoftBody* psb) { - btVector3 com(0,0,0); - if(psb->m_pose.m_bframe) +btVector3 com(0,0,0); +if(psb->m_pose.m_bframe) { - for(int i=0,ni=psb->getNodes().size();im_nodes.size();igetNodes()[i].m_x*psb->m_pose.m_wgh[i]; + com+=psb->m_nodes[i].m_x*psb->m_pose.m_wgh[i]; } } return(com); @@ -605,11 +781,9 @@ static bool CheckContact( btSoftBody* psb, { btVector3 nrm; btCollisionShape* shp=prb->getCollisionShape(); - btAssert(shp->isConvex()); - btConvexShape* csh=static_cast(shp); const btTransform& wtr=prb->getInterpolationWorldTransform(); btScalar dst=psb->m_worldInfo->m_sparsesdf.Evaluate( wtr.invXform(x), - csh, + shp, nrm, margin); if(dst<0) @@ -627,13 +801,13 @@ static bool CheckContact( btSoftBody* psb, static void UpdateNormals(btSoftBody* psb) { const btVector3 zv(0,0,0); - for(int i=0,ni=psb->getNodes().size();im_nodes.size();igetNodes()[i].m_n=zv; + psb->m_nodes[i].m_n=zv; } - for(int i=0,ni=psb->getFaces().size();im_faces.size();igetFaces()[i]; + btSoftBody::Face& f=psb->m_faces[i]; const btVector3 n=cross(f.m_n[1]->m_x-f.m_n[0]->m_x, f.m_n[2]->m_x-f.m_n[0]->m_x); f.m_normal=n.normalized(); @@ -641,9 +815,9 @@ static void UpdateNormals(btSoftBody* psb) f.m_n[1]->m_n+=n; f.m_n[2]->m_n+=n; } - for(int i=0,ni=psb->getNodes().size();im_nodes.size();igetNodes()[i].m_n.normalize(); + psb->m_nodes[i].m_n.normalize(); } } @@ -652,8 +826,8 @@ static void UpdateBounds(btSoftBody* psb) { if(psb->m_ndbvt.m_root) { - const btVector3& mins=psb->m_ndbvt.m_root->box.Mins(); - const btVector3& maxs=psb->m_ndbvt.m_root->box.Maxs(); + const btVector3& mins=psb->m_ndbvt.m_root->volume.Mins(); + const btVector3& maxs=psb->m_ndbvt.m_root->volume.Maxs(); const btScalar csm=psb->getCollisionShape()->getMargin(); const btVector3 mrg=btVector3( csm, csm, @@ -687,12 +861,12 @@ static void UpdatePose(btSoftBody* psb) pose.m_com = com; /* Rotation */ btMatrix3x3 Apq; - const btScalar eps=1/(btScalar)(100*psb->getNodes().size()); + const btScalar eps=1/(btScalar)(100*psb->m_nodes.size()); Apq[0]=Apq[1]=Apq[2]=btVector3(0,0,0); Apq[0].setX(eps);Apq[1].setY(eps*2);Apq[2].setZ(eps*3); - for(int i=0,ni=psb->getNodes().size();im_nodes.size();igetNodes()[i].m_x-com); + const btVector3 a=pose.m_wgh[i]*(psb->m_nodes[i].m_x-com); const btVector3& b=pose.m_pos[i]; Apq[0]+=a.x()*b; Apq[1]+=a.y()*b; @@ -716,42 +890,52 @@ static void UpdatePose(btSoftBody* psb) static void UpdateConstants(btSoftBody* psb) { /* Links */ - for(int i=0,ni=psb->getLinks().size();im_links.size();igetLinks()[i]; + btSoftBody::Link& l=psb->m_links[i]; + btSoftBody::Material& m=*l.m_material; l.m_rl = (l.m_n[0]->m_x-l.m_n[1]->m_x).length(); - l.m_c0 = l.m_n[0]->m_im+l.m_n[1]->m_im; + l.m_c0 = (l.m_n[0]->m_im+l.m_n[1]->m_im)/m.m_kLST; l.m_c1 = l.m_rl*l.m_rl; } /* Faces */ - for(int i=0,ni=psb->getFaces().size();im_faces.size();igetFaces()[i]; + btSoftBody::Face& f=psb->m_faces[i]; f.m_ra = AreaOf(f.m_n[0]->m_x,f.m_n[1]->m_x,f.m_n[2]->m_x); } + /* 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->getNodes().size(),0); - for(int i=0,ni=psb->getNodes().size();im_nodes.size(),0); + for(int i=0,ni=psb->m_nodes.size();igetNodes()[i].m_area = 0; + psb->m_nodes[i].m_area = 0; } - for(int i=0,ni=psb->getFaces().size();im_faces.size();igetFaces()[i]; + btSoftBody::Face& f=psb->m_faces[i]; for(int j=0;j<3;++j) { - const int index=(int)(f.m_n[j]-&psb->getNodes()[0]); + const int index=(int)(f.m_n[j]-&psb->m_nodes[0]); counts[index]++; f.m_n[j]->m_area+=btFabs(f.m_ra); } } - for(int i=0,ni=psb->getNodes().size();im_nodes.size();i0) - psb->getNodes()[i].m_area/=(btScalar)counts[i]; + psb->m_nodes[i].m_area/=(btScalar)counts[i]; else - psb->getNodes()[i].m_area=0; + psb->m_nodes[i].m_area=0; } } @@ -784,16 +968,14 @@ static void ApplyForces(btSoftBody* psb,btScalar dt) const bool as_pressure=kPR!=0; const bool as_volume=kVC>0; const bool as_aero= as_lift || - as_drag ; + as_drag ; const bool as_vaero= as_aero && - (psb->m_cfg.aeromodel< - btSoftBody::eAeroModel::F_TwoSided); + (psb->m_cfg.aeromodelm_cfg.aeromodel>= - btSoftBody::eAeroModel::F_TwoSided); + (psb->m_cfg.aeromodel>=btSoftBody::eAeroModel::F_TwoSided); const bool use_medium= as_aero; const bool use_volume= as_pressure || - as_volume ; + as_volume ; btScalar volume=0; btScalar ivolumetp=0; btScalar dvolumetv=0; @@ -805,9 +987,9 @@ static void ApplyForces(btSoftBody* psb,btScalar dt) dvolumetv = (psb->m_pose.m_volume-volume)*kVC; } /* Per vertex forces */ - for(int i=0,ni=psb->getNodes().size();im_nodes.size();igetNodes()[i]; + btSoftBody::Node& n=psb->m_nodes[i]; if(n.m_im>0) { if(use_medium) @@ -856,9 +1038,9 @@ static void ApplyForces(btSoftBody* psb,btScalar dt) } } /* Per face forces */ - for(int i=0,ni=psb->getFaces().size();im_faces.size();igetFaces()[i]; + btSoftBody::Face& f=psb->m_faces[i]; if(as_faero) { const btVector3 v=(f.m_n[0]->m_v+f.m_n[1]->m_v+f.m_n[2]->m_v)/3; @@ -893,16 +1075,17 @@ static void ApplyForces(btSoftBody* psb,btScalar dt) } // -static void PSolve_Anchors(btSoftBody* psb,btScalar sdt) +static void PSolve_Anchors(btSoftBody* psb) { const btScalar kAHR=psb->m_cfg.kAHR; + const btScalar dt=psb->m_sst.sdt; for(int i=0,ni=psb->m_anchors.size();im_anchors[i]; const btTransform& t=a.m_body->getInterpolationWorldTransform(); btSoftBody::Node& n=*a.m_node; const btVector3 wa=t*a.m_local; - const btVector3 va=a.m_body->getVelocityInLocalPoint(a.m_c1)*sdt; + const btVector3 va=a.m_body->getVelocityInLocalPoint(a.m_c1)*dt; const btVector3 vb=n.m_x-n.m_q; const btVector3 vr=(va-vb)+(wa-n.m_x)*kAHR; const btVector3 impulse=a.m_c0*vr; @@ -912,13 +1095,14 @@ static void PSolve_Anchors(btSoftBody* psb,btScalar sdt) } // -static void PSolve_RContacts(btSoftBody* psb,btScalar sdt) +static void PSolve_RContacts(btSoftBody* psb) { + const btScalar dt=psb->m_sst.sdt; for(int i=0,ni=psb->m_rcontacts.size();im_rcontacts[i]; const btSoftBody::sCti& cti=c.m_cti; - const btVector3 va=cti.m_body->getVelocityInLocalPoint(c.m_c1)*sdt; + const btVector3 va=cti.m_body->getVelocityInLocalPoint(c.m_c1)*dt; const btVector3 vb=c.m_node->m_x-c.m_node->m_q; const btVector3 vr=vb-va; const btScalar dn=dot(vr,cti.m_normal); @@ -966,25 +1150,92 @@ for(int i=0,ni=psb->m_scontacts.size();igetLinks().size();im_links.size();igetLinks()[i]; + btSoftBody::Link& l=psb->m_links[i]; if(l.m_c0>0) { btSoftBody::Node& a=*l.m_n[0]; btSoftBody::Node& b=*l.m_n[1]; const btVector3 del=b.m_x-a.m_x; const btScalar len=del.length2(); - const btScalar kst=l.m_kST*w; - const btScalar k=((l.m_c1-len)/(l.m_c0*(l.m_c1+len)))*kst; + const btScalar k=((l.m_c1-len)/(l.m_c0*(l.m_c1+len))); a.m_x-=del*(k*a.m_im); b.m_x+=del*(k*b.m_im); } } } +// +static void PSolve_Tetras(btSoftBody* psb) +{ +for(int i=0,ni=psb->m_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) +{ +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; + 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 PSolvers[])(btSoftBody*)= { + PSolve_Links, + PSolve_Tetras, + PSolve_Anchors, + PSolve_RContacts, + PSolve_SContacts, + }; + // // btSoftBody // @@ -996,21 +1247,23 @@ btSoftBody::btSoftBody(btSoftBody::btSoftBodyWorldInfo* worldInfo,int node_count /* Init */ m_internalType = CO_SOFT_BODY; m_cfg.aeromodel = eAeroModel::V_Point; + m_cfg.kVCF = 1; m_cfg.kDG = 0; m_cfg.kLF = 0; m_cfg.kDP = 0; m_cfg.kPR = 0; m_cfg.kVC = 0; m_cfg.kDF = (btScalar)0.2; - m_cfg.kLST = 1; m_cfg.kMT = 0; - m_cfg.kSOR = 1; m_cfg.kCHR = (btScalar)1.0; + m_cfg.kKHR = (btScalar)0.5; m_cfg.kSHR = (btScalar)1.0; m_cfg.kAHR = (btScalar)0.7; m_cfg.maxvolume = (btScalar)1; m_cfg.timescale = 1; - m_cfg.iterations = 1; + m_cfg.viterations = 0; + m_cfg.piterations = 1; + m_cfg.diterations = 0; m_cfg.collisions = fCollision::Default; m_pose.m_bvolume = false; m_pose.m_bframe = false; @@ -1024,21 +1277,30 @@ btSoftBody::btSoftBody(btSoftBody::btSoftBodyWorldInfo* worldInfo,int node_count m_bounds[0] = btVector3(0,0,0); m_bounds[1] = btVector3(0,0,0); m_worldTransform.setIdentity(); + setSolver(eSolverPresets::Positions); + /* Default material */ + Material* pm=appendMaterial(); + pm->m_kLST = 1; + pm->m_kAST = 1; + pm->m_kVST = 1; + pm->m_flags = fMaterial::Default; + /* Collision shape */ ///for now, create a collision shape internally setCollisionShape(new btSoftBodyCollisionShape(this)); m_collisionShape->setMargin(0.25); - /* Nodes */ + /* Nodes */ const btScalar margin=getCollisionShape()->getMargin(); - getNodes().resize(node_count); + m_nodes.resize(node_count); for(int i=0,ni=node_count;i0?1/n.m_im:0; - n.m_leaf = m_ndbvt.insert(btDbvt::Aabb::FromCR(n.m_x,margin),&n); + n.m_leaf = m_ndbvt.insert(btDbvt::Volume::FromCR(n.m_x,margin),&n); + n.m_material= pm; } m_ndbvt.optimizeTopDown(); UpdateBounds(this); @@ -1051,21 +1313,22 @@ btSoftBody::~btSoftBody() { //for now, delete the internal shape delete m_collisionShape; + for(int i=0;i0) + *pm=*m_materials[0]; + else + ZeroInitialize(*pm); +m_materials.push_back(pm); +return(pm); +} + +// +void btSoftBody::appendNote( const char* text, + const btVector3& o, + const btVector4& c, + Node* n0, + Node* n1, + Node* n2, + Node* n3) +{ +Note n; +ZeroInitialize(n); +n.m_rank = 0; +n.m_text = text; +n.m_offset = o; +n.m_coords[0] = c.x(); +n.m_coords[1] = c.y(); +n.m_coords[2] = c.z(); +n.m_coords[3] = c.w(); +n.m_nodes[0] = n0;n.m_rank+=n0?1:0; +n.m_nodes[1] = n1;n.m_rank+=n1?1:0; +n.m_nodes[2] = n2;n.m_rank+=n2?1:0; +n.m_nodes[3] = n3;n.m_rank+=n3?1:0; +m_notes.push_back(n); +} + +// +void btSoftBody::appendNote( const char* text, + const btVector3& o, + Node* feature) +{ +appendNote(text,o,btVector4(1,0,0,0),feature); +} + +// +void btSoftBody::appendNote( const char* text, + const btVector3& o, + Link* feature) +{ +static const btScalar w=1/(btScalar)2; +appendNote(text,o,btVector4(w,w,0,0), feature->m_n[0], + feature->m_n[1]); +} + +// +void btSoftBody::appendNote( const char* text, + const btVector3& o, + Face* feature) +{ +static const btScalar w=1/(btScalar)3; +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) +{ +if(m_nodes.capacity()==m_nodes.size()) + { + PointersToIndices(this); + m_nodes.reserve(m_nodes.size()*2+1); + IndicesToPointers(this); + } +const btScalar margin=getCollisionShape()->getMargin(); +m_nodes.push_back(Node()); +Node& n=m_nodes[m_nodes.size()-1]; +ZeroInitialize(n); +n.m_x = x; +n.m_q = n.m_x; +n.m_im = m>0?1/m:0; +n.m_material = m_materials[0]; +n.m_leaf = m_ndbvt.insert(btDbvt::Volume::FromCR(n.m_x,margin),&n); +} + +// +void btSoftBody::appendLink(int model,Material* mat) +{ +Link l; +if(model>=0) + l=m_links[model]; + else + { ZeroInitialize(l);l.m_material=mat?mat:m_materials[0]; } +m_links.push_back(l); +} + // void btSoftBody::appendLink( int node0, int node1, - btScalar kST, - eLType::_ type, + Material* mat, bool bcheckexist) { - appendLink(&getNodes()[node0],&getNodes()[node1],kST,type,bcheckexist); + appendLink(&m_nodes[node0],&m_nodes[node1],mat,bcheckexist); } // void btSoftBody::appendLink( Node* node0, Node* node1, - btScalar kST, - eLType::_ type, + Material* mat, bool bcheckexist) { if((!bcheckexist)||(!checkLink(node0,node1))) { - Link l; - l.m_n[0] = node0; - l.m_n[1] = node1; - l.m_kST = kST; - l.m_rl = (l.m_n[0]->m_x-l.m_n[1]->m_x).length(); - l.m_c0 = 0; - l.m_c1 = 0; - l.m_type = type; - l.m_tag = 0; - getLinks().push_back(l); + appendLink(-1,mat); + Link& l=m_links[m_links.size()-1]; + l.m_n[0] = node0; + l.m_n[1] = node1; + l.m_rl = (l.m_n[0]->m_x-l.m_n[1]->m_x).length(); m_bUpdateRtCst=true; } } // -void btSoftBody::appendFace(int node0,int node1,int node2) +void btSoftBody::appendFace(int model,Material* mat) { - Face f; - f.m_n[0] = &getNodes()[node0]; - f.m_n[1] = &getNodes()[node1]; - f.m_n[2] = &getNodes()[node2]; +Face f; +if(model>=0) + { f=m_faces[model]; } + else + { ZeroInitialize(f);f.m_material=mat?mat:m_materials[0]; } +m_faces.push_back(f); +} + +// +void btSoftBody::appendFace(int node0,int node1,int node2,Material* mat) +{ + appendFace(-1,mat); + Face& f=m_faces[m_faces.size()-1]; + f.m_n[0] = &m_nodes[node0]; + f.m_n[1] = &m_nodes[node1]; + f.m_n[2] = &m_nodes[node2]; f.m_ra = AreaOf( f.m_n[0]->m_x, f.m_n[1]->m_x, - f.m_n[2]->m_x); - f.m_tag = 0; - getFaces().push_back(f); + f.m_n[2]->m_x); + 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; } @@ -1148,7 +1551,7 @@ void btSoftBody::appendFace(int node0,int node1,int node2) void btSoftBody::appendAnchor(int node,btRigidBody* body) { Anchor a; - a.m_node = &getNodes()[node]; + a.m_node = &m_nodes[node]; a.m_body = body; a.m_local = body->getInterpolationWorldTransform().inverse()*a.m_node->m_x; a.m_node->m_battach = 1; @@ -1158,13 +1561,13 @@ void btSoftBody::appendAnchor(int node,btRigidBody* body) // void btSoftBody::addForce(const btVector3& force) { - for(int i=0,ni=getNodes().size();i0) { n.m_f += force; @@ -1174,13 +1577,13 @@ void btSoftBody::addForce(const btVector3& force,int node) // void btSoftBody::addVelocity(const btVector3& velocity) { - for(int i=0,ni=getNodes().size();i0) { n.m_v += velocity; @@ -1190,21 +1593,21 @@ void btSoftBody::addVelocity(const btVector3& velocity,int node) // void btSoftBody::setMass(int node,btScalar mass) { - getNodes()[node].m_im=mass>0?1/mass:0; + m_nodes[node].m_im=mass>0?1/mass:0; m_bUpdateRtCst=true; } // btScalar btSoftBody::getMass(int node) const { - return(getNodes()[node].m_im>0?1/getNodes()[node].m_im:0); + return(m_nodes[node].m_im>0?1/m_nodes[node].m_im:0); } // btScalar btSoftBody::getTotalMass() const { btScalar mass=0; - for(int i=0;im_x, - f.m_n[1]->m_x, - f.m_n[2]->m_x); + f.m_n[1]->m_x, + f.m_n[2]->m_x); for(int j=0;j<3;++j) { f.m_n[j]->m_im+=twicearea; } } - for(int i=0;i 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;igetMargin(); - for(int i=0,ni=getNodes().size();igetMargin(); - for(int i=0,ni=getNodes().size();i0 ? - 1/(getNodes()[i].m_im*tmass) : - kmass/tmass; + Node& n=m_nodes[i]; + m_pose.m_wgh[i]= n.m_im>0 ? + 1/(m_nodes[i].m_im*tmass) : + kmass/tmass; } /* Pos */ const btVector3 com=EvaluateCom(this); - m_pose.m_pos.resize(getNodes().size()); - for(int i=0,ni=getNodes().size();idata; - if(RayTriangle( m_x, - m_axe, - f.m_n[0]->m_x, - f.m_n[1]->m_x, - f.m_n[2]->m_x)>0) - { - ++m_count; - } - } - bool Descent(const btDbvt::Node* n) - { - return(dot(m_x-n->box.Maxs()*m_axe,m_axe)<=0); - } - }; -btDbvt dbvt; -int ires[3]; -btScalar fres[3]; -const btScalar margin=getCollisionShape()->getMargin(); -const btVector3 lengths=m_bounds[1]-m_bounds[0]; -const btScalar smallest=btMin(btMin(lengths.x(),lengths.y()),lengths.z()); -const btScalar ratio=smallest/res; -ires[0]=btMax(2,(int)(lengths.x()/ratio)); -ires[1]=btMax(2,(int)(lengths.y()/ratio)); -ires[2]=btMax(2,(int)(lengths.z()/ratio)); -fres[0]=(btScalar)ires[0]-1; -fres[1]=(btScalar)ires[1]-1; -fres[2]=(btScalar)ires[2]-1; -for(int i=0;i1) - { - m_dfld.pts.push_back(p-m_pose.m_com); - } - } - } - } -} - // btScalar btSoftBody::getVolume() const { btScalar vol=0; - if(getNodes().size()>0) + if(m_nodes.size()>0) { - const btVector3 org=getNodes()[0].m_x; - for(int i=0,ni=getFaces().size();im_x-org,cross(f.m_n[1]->m_x-org,f.m_n[2]->m_x-org)); } vol/=(btScalar)6; @@ -1427,13 +1816,12 @@ btScalar btSoftBody::getVolume() const } // -int btSoftBody::generateBendingConstraints( int distance, - btScalar stiffness) +int btSoftBody::generateBendingConstraints(int distance,Material* mat) { if(distance>1) { /* Build graph */ - const int n=getNodes().size(); + const int n=m_nodes.size(); const unsigned inf=(~(unsigned)0)>>1; unsigned* adj=new unsigned[n*n]; #define IDX(_x_,_y_) ((_y_)*n+(_x_)) @@ -1446,15 +1834,12 @@ int btSoftBody::generateBendingConstraints( int distance, adj[IDX(i,j)]=adj[IDX(j,i)]=0; } } - for(int i=0;i edges(ncount,-2); +int newnodes=0; +/* Fill edges */ +for(int i=0;i0) + { + const btVector3 x=Lerp(a.m_x,b.m_x,t); + const btVector3 v=Lerp(a.m_v,b.m_v,t); + btScalar m=0; + if(a.m_im>0) + { + if(b.m_im>0) + { + const btScalar ma=1/a.m_im; + const btScalar mb=1/b.m_im; + const btScalar mc=Lerp(ma,mb,t); + const btScalar f=(ma+mb)/(ma+mb+mc); + a.m_im=1/(ma*f); + b.m_im=1/(mb*f); + m=mc*f; + } + else + { a.m_im/=0.5;m=1/a.m_im; } + } + else + { + if(b.m_im>0) + { b.m_im/=0.5;m=1/b.m_im; } + else + m=0; + } + appendNode(x,m); + edges(i,j)=m_nodes.size()-1; + m_nodes[edges(i,j)].m_v=v; + ++newnodes; + } + } + } + } +nbase=&m_nodes[0]; +/* Refine links */ +for(int i=0,ni=m_links.size();i0) + { + appendLink(i); + Link* pft[]={ &m_links[i], + &m_links[m_links.size()-1]}; + pft[0]->m_n[0]=&m_nodes[idx[0]]; + pft[0]->m_n[1]=&m_nodes[ni]; + pft[1]->m_n[0]=&m_nodes[ni]; + pft[1]->m_n[1]=&m_nodes[idx[1]]; + } + } + } +/* Refine faces */ +for(int i=0;i0) + { + appendFace(i); + const int l=(k+1)%3; + Face* pft[]={ &m_faces[i], + &m_faces[m_faces.size()-1]}; + pft[0]->m_n[0]=&m_nodes[idx[l]]; + pft[0]->m_n[1]=&m_nodes[idx[j]]; + pft[0]->m_n[2]=&m_nodes[ni]; + pft[1]->m_n[0]=&m_nodes[ni]; + pft[1]->m_n[1]=&m_nodes[idx[k]]; + pft[1]->m_n[2]=&m_nodes[idx[l]]; + appendLink(ni,idx[l],pft[0]->m_material); + --i;break; + } + } + } + } +/* Refine tetras */ +static const int edg[]={0,1,1,2,2,0,0,3,1,3,2,3}; +static const int apx[]={2,3,0,3,1,3,1,2,2,0,0,1}; +for(int i=0;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) + { + btAlignedObjectArray cnodes; + const int pcount=ncount; + ncount=m_nodes.size(); + cnodes.resize(ncount,0); + /* Nodes */ + for(int i=0;i=pcount)||(btFabs(ifn->Eval(x))0) { m*=0.5;m_nodes[i].m_im/=0.5; } + appendNode(x,m); + cnodes[i]=m_nodes.size()-1; + m_nodes[cnodes[i]].m_v=v; + } + } + nbase=&m_nodes[0]; + /* Links */ + for(int i=0,ni=m_links.size();iEval(m_nodes[id[0]].m_x)Eval(m_nodes[id[1]].m_x)Eval(n[0]->m_x)Eval(n[1]->m_x)Eval(n[2]->m_x)Eval(n[0]->m_x)Eval(n[1]->m_x)Eval(n[2]->m_x)Eval(n[3]->m_x) ranks; + btAlignedObjectArray todelete; + ranks.resize(nnodes,0); + for(int i=0,ni=m_links.size();i=0;--i) + { + if(!ranks[i]) todelete.push_back(i); + } + if(todelete.size()) + { + btAlignedObjectArray& map=ranks; + for(int i=0;im_v=v; +pn[1]->m_v=v; +for(int i=0,ni=m_links.size();im_n[1]=pn[mtch]; + pft[1]->m_n[0]=pn[1-mtch]; + done=true; + } + } +for(int i=0,ni=m_faces.size();im_n[l]=pn[mtch]; + pft[1]->m_n[k]=pn[1-mtch]; + appendLink(pn[0],pft[0]->m_n[(l+1)%3],pft[0]->m_material,true); + appendLink(pn[1],pft[0]->m_n[(l+1)%3],pft[0]->m_material,true); + } + } + } +if(!done) + { + m_ndbvt.remove(pn[0]->m_leaf); + m_ndbvt.remove(pn[1]->m_leaf); + m_nodes.pop_back(); + m_nodes.pop_back(); + } +return(done); +} + // bool btSoftBody::rayCast(const btVector3& org, const btVector3& dir, sRayCast& results, btScalar maxtime) { - if(getFaces().size()) - { - if(m_fdbvt.empty()) InitializeFaceTree(this); - results.time = maxtime; - results.face = -1; - return(RaycastInternal( this,org,dir, - results.time, - results.face,false)!=0); - } +if(m_faces.size()&&m_fdbvt.empty()) InitializeFaceTree(this); +results.body = this; +results.time = maxtime; +results.feature = eFeature::None; +results.index = -1; +return(RaycastInternal( this,org,dir, + results.time, + results.feature, + results.index,false)!=0); +} - return(false); +// +void btSoftBody::setSolver(eSolverPresets::_ preset) +{ +m_cfg.m_vsequence.clear(); +m_cfg.m_psequence.clear(); +m_cfg.m_dsequence.clear(); +switch(preset) + { + case eSolverPresets::Positions: + m_cfg.m_psequence.push_back(ePSolver::Anchors); + m_cfg.m_psequence.push_back(ePSolver::RContacts); + m_cfg.m_psequence.push_back(ePSolver::SContacts); + m_cfg.m_psequence.push_back(ePSolver::Volume); + m_cfg.m_psequence.push_back(ePSolver::Linear); + break; + case eSolverPresets::Velocities: + m_cfg.m_vsequence.push_back(eVSolver::Volume); + m_cfg.m_vsequence.push_back(eVSolver::Linear); + + m_cfg.m_psequence.push_back(ePSolver::Anchors); + m_cfg.m_psequence.push_back(ePSolver::RContacts); + m_cfg.m_psequence.push_back(ePSolver::SContacts); + + m_cfg.m_dsequence.push_back(ePSolver::Volume); + m_cfg.m_dsequence.push_back(ePSolver::Linear); + break; + } } // @@ -1535,8 +2343,8 @@ void btSoftBody::predictMotion(btScalar dt) InitializeFaceTree(this); } } + /* Prepare */ - m_sst.iit = 1/(btScalar)m_cfg.iterations; m_sst.sdt = dt*m_cfg.timescale; m_sst.isdt = 1/m_sst.sdt; m_sst.velmrg = m_sst.sdt*3; @@ -1546,14 +2354,14 @@ void btSoftBody::predictMotion(btScalar dt) addVelocity(m_worldInfo->m_gravity*m_sst.sdt); ApplyForces(this,m_sst.sdt); /* Integrate */ - for(int i=0,ni=getNodes().size();im_v+ f.m_n[1]->m_v+ f.m_n[2]->m_v)/3; m_fdbvt.update( f.m_leaf, - BoxOf(f,m_sst.radmrg), + VolumeOf(f,m_sst.radmrg), v*m_sst.velmrg, m_sst.updmrg); } } /* Pose */ UpdatePose(this); - /* Match */ - if(m_pose.m_bframe&&(m_cfg.kMT>0)) - { - for(int i=0,ni=getNodes().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); @@ -1596,31 +2391,126 @@ void btSoftBody::predictMotion(btScalar dt) // void btSoftBody::solveConstraints() { - /* Prepare anchors */ - for(int i=0,ni=m_anchors.size();i0) { - Anchor& a=m_anchors[i]; - const btVector3 ra=a.m_body->getWorldTransform().getBasis()*a.m_local; - a.m_c0 = ImpulseMatrix( m_sst.sdt, - a.m_node->m_im, - a.m_body->getInvMass(), - a.m_body->getInvInertiaTensorWorld(), - ra); - a.m_c1 = ra; - a.m_c2 = m_sst.sdt*a.m_node->m_im; - a.m_body->activate(); + /* Prepare */ + 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)) { - const btScalar lw= Lerp(1,m_cfg.kSOR,isolve*m_sst.iit)*m_cfg.kLST; - PSolve_Anchors(this,m_sst.sdt); - PSolve_RContacts(this,m_sst.sdt); - PSolve_SContacts(this); - PSolve_Links(this,lw); + 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); + } + } + } +/* Prepare anchors */ +for(int i=0,ni=m_anchors.size();igetWorldTransform().getBasis()*a.m_local; + a.m_c0 = ImpulseMatrix( m_sst.sdt, + a.m_node->m_im, + a.m_body->getInvMass(), + a.m_body->getInvInertiaTensorWorld(), + ra); + a.m_c1 = ra; + a.m_c2 = m_sst.sdt*a.m_node->m_im; + a.m_body->activate(); + } +/* Solve positions */ +if(m_cfg.piterations>0) + { + for(int isolve=0;isolve0) + { + const btScalar vcf=m_cfg.kVCF*m_sst.isdt; + for(int i=0,ni=m_nodes.size();im_sst.sdt; c.m_c3 = fv.length2()<(btFabs(dn)*fc)?0:1-fc; - c.m_c4 = psb->m_cfg.kCHR*(btScalar)(prb->isStaticOrKinematicObject()?0.5:1); + c.m_c4 = prb->isStaticOrKinematicObject()?psb->m_cfg.kKHR:psb->m_cfg.kCHR; psb->m_rcontacts.push_back(c); prb->activate(); } @@ -1700,17 +2582,17 @@ switch(m_cfg.collisions&fCollision::RVSmask) const btScalar basemargin=getCollisionShape()->getMargin(); btVector3 mins; btVector3 maxs; - btDbvt::Aabb aabb; + btDbvt::Volume volume; pco->getCollisionShape()->getAabb( pco->getInterpolationWorldTransform(), mins, maxs); - aabb=btDbvt::Aabb::FromMM(mins,maxs); - aabb.Expand(btVector3(basemargin,basemargin,basemargin)); + volume=btDbvt::Volume::FromMM(mins,maxs); + volume.Expand(btVector3(basemargin,basemargin,basemargin)); docollide.psb = this; docollide.prb = prb; docollide.dynmargin = basemargin+timemargin; docollide.stamargin = basemargin; - m_ndbvt.collide(aabb,&docollide); + m_ndbvt.collide(volume,&docollide); } break; } @@ -1787,43 +2669,3 @@ switch(cf&fCollision::SVSmask) break; } } - -// -// Accessor's -// - -// -btSoftBody::tNodeArray& btSoftBody::getNodes() - { - return ((btSoftBodyCollisionShape*)m_collisionShape)->m_nodes; - } - -// -const btSoftBody::tNodeArray& btSoftBody::getNodes() const - { - return ((btSoftBodyCollisionShape*)m_collisionShape)->m_nodes; - } - -// -btSoftBody::tLinkArray& btSoftBody::getLinks() - { - return ((btSoftBodyCollisionShape*)m_collisionShape)->m_links; - } - -// -const btSoftBody::tLinkArray& btSoftBody::getLinks() const - { - return ((btSoftBodyCollisionShape*)m_collisionShape)->m_links; - } - -// -btSoftBody::tFaceArray& btSoftBody::getFaces() - { - return ((btSoftBodyCollisionShape*)m_collisionShape)->m_faces; - } - -// -const btSoftBody::tFaceArray& btSoftBody::getFaces() const - { - return ((btSoftBodyCollisionShape*)m_collisionShape)->m_faces; - } diff --git a/src/BulletSoftBody/btSoftBody.h b/src/BulletSoftBody/btSoftBody.h index c9bc2dd21..04e712934 100644 --- a/src/BulletSoftBody/btSoftBody.h +++ b/src/BulletSoftBody/btSoftBody.h @@ -39,12 +39,6 @@ public: // Enumerations // - ///eLType - struct eLType { enum _ { - Structural, ///Master constraints - Bending, ///Secondary constraints - };}; - ///eAeroModel struct eAeroModel { enum _ { V_Point, ///Vertex normals are oriented toward velocity @@ -54,6 +48,40 @@ public: F_OneSided, ///Face normals are taken as it is };}; + ///eVSolver : velocities solvers + struct eVSolver { enum _ { + Linear, ///Linear solver + Volume, ///Volume solver + };}; + + ///ePSolver : positions solvers + struct ePSolver { enum _ { + Linear, ///Linear solver + Volume, ///Volume solver + Anchors, ///Anchor solver + RContacts, ///Rigid contacts solver + SContacts, ///Soft contacts solver + };}; + + ///eSolverPresets + struct eSolverPresets { enum _ { + Positions, + Velocities, + Default = Positions, + };}; + + ///eFeature + struct eFeature { enum _ { + None, + Node, + Link, + Face, + Tetra, + };}; + + typedef btAlignedObjectArray tVSolverArray; + typedef btAlignedObjectArray tPSolverArray; + // // Flags // @@ -69,6 +97,13 @@ public: Default = SDF_RS, };}; + ///fMaterial + struct fMaterial { enum _ { + DebugDraw = 0x0001, /// Enable debug draw + /* presets */ + Default = DebugDraw, + };}; + // // API Types // @@ -76,8 +111,16 @@ public: /* sRayCast */ struct sRayCast { - int face; /// face - btScalar time; /// time of impact (rayorg+raydir*time) + btSoftBody* body; /// soft body + eFeature::_ feature; /// feature type + int index; /// feature index + btScalar time; /// time of impact (rayorg+raydir*time) + }; + + /* ImplicitFn */ + struct ImplicitFn + { + virtual btScalar Eval(const btVector3& x)=0; }; // @@ -86,7 +129,6 @@ public: typedef btAlignedObjectArray tScalarArray; typedef btAlignedObjectArray tVector3Array; - /* btSoftBodyWorldInfo */ struct btSoftBodyWorldInfo { @@ -121,8 +163,22 @@ public: { void* m_tag; // User data }; + /* Material */ + struct Material : Element + { + btScalar m_kLST; // Linear stiffness coefficient [0,1] + btScalar m_kAST; // Area stiffness coefficient [0,1] + btScalar m_kVST; // Volume stiffness coefficient [0,1] + int m_flags; // Flags + }; + + /* Feature */ + struct Feature : Element + { + Material* m_material; // Material + }; /* Node */ - struct Node : Element + struct Node : Feature { btVector3 m_x; // Position btVector3 m_q; // Previous step position @@ -135,23 +191,33 @@ public: int m_battach:1; // Attached }; /* Link */ - struct Link : Element + struct Link : Feature { Node* m_n[2]; // Node pointers btScalar m_rl; // Rest length - btScalar m_kST; // Stiffness coefficient btScalar m_c0; // (ima+imb)*kLST btScalar m_c1; // rl^2 - btSoftBody::eLType::_ m_type; // Link type + btScalar m_c2; // |gradient|^2/c0 + btVector3 m_c3; // gradient }; /* Face */ - struct Face : Element + struct Face : Feature { Node* m_n[3]; // Node pointers btVector3 m_normal; // Normal 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 { @@ -184,6 +250,15 @@ public: btVector3 m_c1; // Relative anchor btScalar m_c2; // ima*dt }; + /* Note */ + struct Note : Element + { + const char* m_text; // Text + btVector3 m_offset; // Offset + int m_rank; // Rank + Node* m_nodes[4]; // Nodes + btScalar m_coords[4]; // Coordinates + }; /* Pose */ struct Pose { @@ -197,16 +272,11 @@ public: btMatrix3x3 m_scl; // Scale btMatrix3x3 m_aqq; // Base scaling }; - /* DFld */ - struct DFld - { - btAlignedObjectArray pts; - }; /* Config */ struct Config { - btSoftBody::eAeroModel::_ aeromodel; // Aerodynamic model (default: V_Point) - btScalar kLST; // Linear stiffness coefficient [0,1] + eAeroModel::_ aeromodel; // Aerodynamic model (default: V_Point) + btScalar kVCF; // Velocities correction factor (Baumgarte) btScalar kDP; // Damping coefficient [0,1] btScalar kDG; // Drag coefficient [0,+inf] btScalar kLF; // Lift coefficient [0,+inf] @@ -214,19 +284,23 @@ public: btScalar kVC; // Volume conversation coefficient [0,+inf] btScalar kDF; // Dynamic friction coefficient [0,1] btScalar kMT; // Pose matching coefficient [0,1] - btScalar kSOR; // SOR(w) [1,2] default 1, never use with solver!=Accurate btScalar kCHR; // Rigid contacts hardness [0,1] + btScalar kKHR; // Kinetic contacts hardness [0,1] btScalar kSHR; // Soft contacts hardness [0,1] btScalar kAHR; // Anchors hardness [0,1] btScalar maxvolume; // Maximum volume ratio for pose btScalar timescale; // Time scale - int iterations; // Solver iterations + int viterations; // Velocities solver iterations + int piterations; // Positions solver iterations + int diterations; // Drift solver iterations int collisions; // Collisions flags + tVSolverArray m_vsequence; // Velocity solvers sequence + tPSolverArray m_psequence; // Position solvers sequence + tPSolverArray m_dsequence; // Drift solvers sequence }; /* SolverState */ struct SolverState { - btScalar iit; // 1/iterations btScalar sdt; // dt*timescale btScalar isdt; // 1/sdt btScalar velmrg; // velocity margin @@ -238,14 +312,17 @@ public: // Typedef's // + typedef btAlignedObjectArray tNoteArray; typedef btAlignedObjectArray tNodeArray; typedef btAlignedObjectArray tLeafArray; typedef btAlignedObjectArray tLinkArray; typedef btAlignedObjectArray tFaceArray; + typedef btAlignedObjectArray tTetraArray; typedef btAlignedObjectArray tAnchorArray; typedef btAlignedObjectArray tRContactArray; typedef btAlignedObjectArray tSContactArray; - typedef btAlignedObjectArray tSoftBodyArray; + typedef btAlignedObjectArray tMaterialArray; + typedef btAlignedObjectArray tSoftBodyArray; // // Fields @@ -254,18 +331,22 @@ public: Config m_cfg; // Configuration SolverState m_sst; // Solver state Pose m_pose; // Pose - DFld m_dfld; // Distance field void* m_tag; // User data btSoftBodyWorldInfo* m_worldInfo; // + tNoteArray m_notes; // Notes + 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 + tMaterialArray m_materials; // Materials btScalar m_timeacc; // Time accumulator btVector3 m_bounds[2]; // Spatial bounds bool m_bUpdateRtCst; // Update runtime constants btDbvt m_ndbvt; // Nodes tree btDbvt m_fdbvt; // Faces tree - // // Api // @@ -285,71 +366,120 @@ public: bool checkFace( int node0, int node1, int node2) const; + /* Append material */ + Material* appendMaterial(); + /* Append note */ + void appendNote( const char* text, + const btVector3& o, + const btVector4& c=btVector4(1,0,0,0), + Node* n0=0, + Node* n1=0, + Node* n2=0, + Node* n3=0); + void appendNote( const char* text, + const btVector3& o, + Node* feature); + void appendNote( const char* text, + const btVector3& o, + Link* feature); + 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 */ - void appendLink( int node0, - int node1, - btScalar kST, - btSoftBody::eLType::_ type, - bool bcheckexist=false); - void appendLink( btSoftBody::Node* node0, - btSoftBody::Node* node1, - btScalar kST, - btSoftBody::eLType::_ type, - bool bcheckexist=false); + void appendLink(int model=-1,Material* mat=0); + void appendLink( int node0, + int node1, + Material* mat=0, + bool bcheckexist=false); + void appendLink( btSoftBody::Node* node0, + btSoftBody::Node* node1, + Material* mat=0, + bool bcheckexist=false); /* Append face */ - void appendFace( int node0, - int node1, - int node2); + void appendFace(int model=-1,Material* mat=0); + void appendFace( int node0, + 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); + btRigidBody* body); /* Add force (or gravity) to the entire body */ void addForce( const btVector3& force); /* Add force (or gravity) to a node of the body */ void addForce( const btVector3& force, - int node); + int node); /* Add velocity to the entire body */ void addVelocity( const btVector3& velocity); /* Add velocity to a node of the body */ void addVelocity( const btVector3& velocity, - int node); + int node); /* Set mass */ void setMass( int node, - btScalar mass); + btScalar mass); /* Get mass */ btScalar getMass( int node) const; /* Get total mass */ btScalar getTotalMass() const; /* Set total mass (weighted by previous masses) */ void setTotalMass( btScalar mass, - bool fromfaces=false); + 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 */ + void translate( const btVector3& trs); + /* Rotate */ + void rotate( const btQuaternion& rot); /* Scale */ - void scale( const btVector3& scl); + void scale( const btVector3& scl); /* Set current state as pose */ void setPose( bool bvolume, bool bframe); - /* Set current state as distance field */ - void setDistanceField(int nominalresolution); /* Return the volume */ btScalar getVolume() const; /* Generate bending constraints based on distance in the adjency graph */ int generateBendingConstraints( int distance, - btScalar stiffness); + Material* mat=0); + /* Generate tetrahedral constraints */ + int generateTetrahedralConstraints(); /* Randomize constraints to reduce solver bias */ void randomizeConstraints(); + /* Refine */ + void refine(ImplicitFn* ifn,btScalar accurary,bool cut); + /* CutLink */ + bool cutLink(int node0,int node1,btScalar position); + bool cutLink(const Node* node0,const Node* node1,btScalar position); /* Ray casting */ bool rayCast(const btVector3& org, const btVector3& dir, sRayCast& results, btScalar maxtime=SIMD_INFINITY); + /* Solver presets */ + void setSolver(eSolverPresets::_ preset); /* predictMotion */ void predictMotion(btScalar dt); /* solveConstraints */ void solveConstraints(); + /* staticSolve */ + void staticSolve(int iterations); /* solveCommonConstraints */ static void solveCommonConstraints(btSoftBody** bodies,int count,int iterations); /* integrateMotion */ @@ -357,24 +487,7 @@ public: /* defaultCollisionHandlers */ void defaultCollisionHandler(btCollisionObject* pco); void defaultCollisionHandler(btSoftBody* psb); - - // - // Accessor's and cast. - // - - tNodeArray& getNodes(); - const tNodeArray& getNodes() const; - tLinkArray& getLinks(); - const tLinkArray& getLinks() const; - tFaceArray& getFaces(); - const tFaceArray& getFaces() const; - - virtual void getAabb(btVector3& aabbMin,btVector3& aabbMax) const - { - aabbMin = m_bounds[0]; - aabbMax = m_bounds[1]; - } - + // // Cast // @@ -391,6 +504,13 @@ public: return (btSoftBody*)colObj; return 0; } + + virtual void getAabb(btVector3& aabbMin,btVector3& aabbMax) const + { + aabbMin = m_bounds[0]; + aabbMax = m_bounds[1]; + } + }; diff --git a/src/BulletSoftBody/btSoftBodyHelpers.cpp b/src/BulletSoftBody/btSoftBodyHelpers.cpp index 5d2671843..f8742f487 100644 --- a/src/BulletSoftBody/btSoftBodyHelpers.cpp +++ b/src/BulletSoftBody/btSoftBodyHelpers.cpp @@ -71,15 +71,27 @@ if(node) if(depth>=mindepth) { const btScalar scl=(btScalar)(node->isinternal()?1:1); - const btVector3 mi=node->box.Center()-node->box.Extent()*scl; - const btVector3 mx=node->box.Center()+node->box.Extent()*scl; + 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); } } } // -#if 0 +// +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 btVector3 stresscolor(btScalar stress) { static const btVector3 spectrum[]= { btVector3(1,0,1), @@ -96,7 +108,6 @@ static btVector3 stresscolor(btScalar stress) const btScalar frc=stress-sel; return(spectrum[sel]+(spectrum[sel+1]-spectrum[sel])*frc); } -#endif // void btSoftBodyHelpers::Draw( btSoftBody* psb, @@ -105,16 +116,16 @@ void btSoftBodyHelpers::Draw( btSoftBody* psb, { const btScalar scl=(btScalar)0.1; const btScalar nscl=scl*5; - const btVector3 scolor=btVector3(0,0,0); - const btVector3 bcolor=btVector3(1,1,0); + const btVector3 lcolor=btVector3(0,0,0); const btVector3 ncolor=btVector3(1,1,1); const btVector3 ccolor=btVector3(1,0,0); /* Nodes */ if(0!=(drawflags&fDrawFlags::Nodes)) { - for(int i=0;igetNodes().size();++i) + for(int i=0;im_nodes.size();++i) { - const btSoftBody::Node& n=psb->getNodes()[i]; + const btSoftBody::Node& n=psb->m_nodes[i]; + if(0==(n.m_material->m_flags&btSoftBody::fMaterial::DebugDraw)) continue; idraw->drawLine(n.m_x-btVector3(scl,0,0),n.m_x+btVector3(scl,0,0),btVector3(1,0,0)); idraw->drawLine(n.m_x-btVector3(0,scl,0),n.m_x+btVector3(0,scl,0),btVector3(0,1,0)); idraw->drawLine(n.m_x-btVector3(0,0,scl),n.m_x+btVector3(0,0,scl),btVector3(0,0,1)); @@ -123,24 +134,20 @@ void btSoftBodyHelpers::Draw( btSoftBody* psb, /* Links */ if(0!=(drawflags&fDrawFlags::Links)) { - for(int i=0;igetLinks().size();++i) + for(int i=0;im_links.size();++i) { - const btSoftBody::Link& l=psb->getLinks()[i]; - switch(l.m_type) - { - case btSoftBody::eLType::Structural: - if(0!=(drawflags&fDrawFlags::SLinks)) idraw->drawLine(l.m_n[0]->m_x,l.m_n[1]->m_x,scolor);break; - case btSoftBody::eLType::Bending: - if(0!=(drawflags&fDrawFlags::BLinks)) idraw->drawLine(l.m_n[0]->m_x,l.m_n[1]->m_x,bcolor);break; - } + const btSoftBody::Link& l=psb->m_links[i]; + if(0==(l.m_material->m_flags&btSoftBody::fMaterial::DebugDraw)) continue; + idraw->drawLine(l.m_n[0]->m_x,l.m_n[1]->m_x,lcolor); } } /* Normals */ if(0!=(drawflags&fDrawFlags::Normals)) { - for(int i=0;igetNodes().size();++i) + for(int i=0;im_nodes.size();++i) { - const btSoftBody::Node& n=psb->getNodes()[i]; + const btSoftBody::Node& n=psb->m_nodes[i]; + if(0==(n.m_material->m_flags&btSoftBody::fMaterial::DebugDraw)) continue; const btVector3 d=n.m_n*nscl; idraw->drawLine(n.m_x,n.m_x+d,ncolor); idraw->drawLine(n.m_x,n.m_x-d,ncolor*0.5); @@ -175,9 +182,10 @@ void btSoftBodyHelpers::Draw( btSoftBody* psb, drawVertex(idraw,q,0.25,btVector3(0,1,0)); idraw->drawLine(a.m_node->m_x,q,btVector3(1,1,1)); } - for(int i=0;igetNodes().size();++i) + for(int i=0;im_nodes.size();++i) { - const btSoftBody::Node& n=psb->getNodes()[i]; + const btSoftBody::Node& n=psb->m_nodes[i]; + if(0==(n.m_material->m_flags&btSoftBody::fMaterial::DebugDraw)) continue; if(n.m_im<=0) { drawVertex(idraw,n.m_x,0.25,btVector3(1,0,0)); @@ -187,21 +195,61 @@ void btSoftBodyHelpers::Draw( btSoftBody* psb, /* Faces */ if(0!=(drawflags&fDrawFlags::Faces)) { - const btScalar scl=(btScalar)0.7; + const btScalar scl=(btScalar)0.8; const btScalar alp=(btScalar)1; const btVector3 col(0,(btScalar)0.7,0); - for(int i=0;igetFaces().size();++i) + for(int i=0;im_faces.size();++i) { - const btSoftBody::Face& f=psb->getFaces()[i]; + const btSoftBody::Face& f=psb->m_faces[i]; + if(0==(f.m_material->m_flags&btSoftBody::fMaterial::DebugDraw)) continue; const btVector3 x[]={f.m_n[0]->m_x,f.m_n[1]->m_x,f.m_n[2]->m_x}; const btVector3 c=(x[0]+x[1]+x[2])/3; + /*idraw->drawLine((x[0]-c)*scl+c,(x[1]-c)*scl+c,col); + idraw->drawLine((x[1]-c)*scl+c,(x[2]-c)*scl+c,col); + idraw->drawLine((x[2]-c)*scl+c,(x[0]-c)*scl+c,col);*/ idraw->drawTriangle((x[0]-c)*scl+c, + (x[1]-c)*scl+c, + (x[2]-c)*scl+c, + col,alp); + /*idraw->drawTriangle((x[0]-c)*scl+c, (x[1]-c)*scl+c, (x[2]-c)*scl+c, f.m_n[0]->m_n,f.m_n[1]->m_n,f.m_n[2]->m_n, - col,alp); + col,alp);*/ } } + /* 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); + } + } + /* Notes */ + if(0!=(drawflags&fDrawFlags::Notes)) + { + for(int i=0;im_notes.size();++i) + { + 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); + } + } } // @@ -211,9 +259,9 @@ void btSoftBodyHelpers::DrawInfos( btSoftBody* psb, bool areas, bool /*stress*/) { - for(int i=0;igetNodes().size();++i) + for(int i=0;im_nodes.size();++i) { - const btSoftBody::Node& n=psb->getNodes()[i]; + const btSoftBody::Node& n=psb->m_nodes[i]; char text[2048]={0}; char buff[1024]; if(masses) @@ -269,11 +317,6 @@ void btSoftBodyHelpers::DrawFrame( btSoftBody* psb, const btVector3 x=com+trs*psb->m_pose.m_pos[i]; drawVertex(idraw,x,nscl,btVector3(1,0,1)); } - for(int i=0;im_dfld.pts.size();++i) - { - const btVector3 x=com+trs*psb->m_dfld.pts[i]; - drawVertex(idraw,x,nscl*(btScalar)0.5,btVector3(0,0,1)); - } } } @@ -301,7 +344,7 @@ btSoftBody* btSoftBodyHelpers::CreateRope( btSoftBody::btSoftBodyWorldInfo& wor /* Create links */ for(int i=1;iappendLink(i-1,i,1,btSoftBody::eLType::Structural); + psb->appendLink(i-1,i); } /* Finished */ return(psb); @@ -352,10 +395,8 @@ btSoftBody* btSoftBodyHelpers::CreatePatch(btSoftBody::btSoftBodyWorldInfo& wor const int idx=IDX(ix,iy); const bool mdx=(ix+1)appendLink(idx,IDX(ix+1,iy), - 1,btSoftBody::eLType::Structural); - if(mdy) psb->appendLink(idx,IDX(ix,iy+1), - 1,btSoftBody::eLType::Structural); + if(mdx) psb->appendLink(idx,IDX(ix+1,iy)); + if(mdy) psb->appendLink(idx,IDX(ix,iy+1)); if(mdx&&mdy) { if((ix+iy)&1) @@ -364,8 +405,7 @@ btSoftBody* btSoftBodyHelpers::CreatePatch(btSoftBody::btSoftBodyWorldInfo& wor psb->appendFace(IDX(ix,iy),IDX(ix+1,iy+1),IDX(ix,iy+1)); if(gendiags) { - psb->appendLink(IDX(ix,iy),IDX(ix+1,iy+1), - 1,btSoftBody::eLType::Structural); + psb->appendLink(IDX(ix,iy),IDX(ix+1,iy+1)); } } else @@ -374,8 +414,7 @@ btSoftBody* btSoftBodyHelpers::CreatePatch(btSoftBody::btSoftBodyWorldInfo& wor psb->appendFace(IDX(ix,iy+1),IDX(ix+1,iy),IDX(ix+1,iy+1)); if(gendiags) { - psb->appendLink(IDX(ix+1,iy),IDX(ix,iy+1), - 1,btSoftBody::eLType::Structural); + psb->appendLink(IDX(ix+1,iy),IDX(ix,iy+1)); } } } @@ -448,7 +487,7 @@ btSoftBody* btSoftBodyHelpers::CreateFromTriMesh(btSoftBody::btSoftBodyWorldInf { chks[IDX(idx[j],idx[k])]=true; chks[IDX(idx[k],idx[k])]=true; - psb->appendLink(idx[j],idx[k],1,btSoftBody::eLType::Structural); + psb->appendLink(idx[j],idx[k]); } } #undef IDX @@ -474,12 +513,9 @@ btSoftBody* btSoftBodyHelpers::CreateFromConvexHull(btSoftBody::btSoftBodyWorld 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], - 1,btSoftBody::eLType::Structural); - if(idx[1]appendLink( idx[1],idx[2], - 1,btSoftBody::eLType::Structural); - if(idx[2]appendLink( idx[2],idx[0], - 1,btSoftBody::eLType::Structural); + if(idx[0]appendLink( idx[0],idx[1]); + if(idx[1]appendLink( idx[1],idx[2]); + if(idx[2]appendLink( idx[2],idx[0]); psb->appendFace(idx[0],idx[1],idx[2]); } hlib.ReleaseResult(hres); @@ -487,4 +523,154 @@ btSoftBody* btSoftBodyHelpers::CreateFromConvexHull(btSoftBody::btSoftBodyWorld 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 diff --git a/src/BulletSoftBody/btSoftBodyHelpers.h b/src/BulletSoftBody/btSoftBodyHelpers.h index a45967e2d..af25be664 100644 --- a/src/BulletSoftBody/btSoftBodyHelpers.h +++ b/src/BulletSoftBody/btSoftBodyHelpers.h @@ -18,6 +18,9 @@ subject to the following restrictions: #include "btSoftBody.h" +//Can't enable this, Bullet doesn't use STL +//#define BT_SOFTBODY_USE_STL 1 + // // Helpers // @@ -25,16 +28,15 @@ subject to the following restrictions: /* fDrawFlags */ struct fDrawFlags { enum _ { Nodes = 0x0001, - SLinks = 0x0002, - BLinks = 0x0004, - Faces = 0x0008, - Tetras = 0x0010, - Normals = 0x0020, - Contacts = 0x0040, - Anchors = 0x0080, + Links = 0x0002, + Faces = 0x0004, + Tetras = 0x0008, + Normals = 0x0010, + Contacts = 0x0020, + Anchors = 0x0040, + Notes = 0x0080, /* presets */ - Links = SLinks+BLinks, - Std = SLinks+Faces+Anchors, + Std = Links+Faces+Tetras+Anchors+Notes, StdTetra = Std-Faces+Tetras, };}; @@ -64,30 +66,56 @@ struct btSoftBodyHelpers static void DrawFrame( btSoftBody* psb, btIDebugDraw* idraw); /* Create a rope */ - static btSoftBody* CreateRope( btSoftBody::btSoftBodyWorldInfo& worldInfo,const btVector3& from, - const btVector3& to, - int res, - int fixeds); + static btSoftBody* CreateRope( btSoftBody::btSoftBodyWorldInfo& worldInfo, + const btVector3& from, + const btVector3& to, + int res, + int fixeds); /* Create a patch */ - static btSoftBody* CreatePatch( btSoftBody::btSoftBodyWorldInfo& worldInfo,const btVector3& corner00, - const btVector3& corner10, - const btVector3& corner01, - const btVector3& corner11, - int resx, - int resy, - int fixeds, - bool gendiags); + static btSoftBody* CreatePatch(btSoftBody::btSoftBodyWorldInfo& worldInfo, + const btVector3& corner00, + const btVector3& corner10, + const btVector3& corner01, + const btVector3& corner11, + int resx, + int resy, + int fixeds, + bool gendiags); /* Create an ellipsoid */ - static btSoftBody* CreateEllipsoid(btSoftBody::btSoftBodyWorldInfo& worldInfo,const btVector3& center, - const btVector3& radius, - int res); - /* Create from convex-hull */ - static btSoftBody* CreateFromConvexHull( btSoftBody::btSoftBodyWorldInfo& worldInfo, const btVector3* vertices, - int nvertices); + static btSoftBody* CreateEllipsoid(btSoftBody::btSoftBodyWorldInfo& worldInfo, + const btVector3& center, + const btVector3& radius, + int res); /* Create from trimesh */ - static btSoftBody* CreateFromTriMesh( btSoftBody::btSoftBodyWorldInfo& worldInfo, const btScalar* vertices, - const int* triangles, - int ntriangles); + static btSoftBody* CreateFromTriMesh( btSoftBody::btSoftBodyWorldInfo& worldInfo, + const btScalar* vertices, + const int* triangles, + int ntriangles); + /* Create from convex-hull */ + 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 diff --git a/src/BulletSoftBody/btSoftRigidDynamicsWorld.cpp b/src/BulletSoftBody/btSoftRigidDynamicsWorld.cpp index da2681ed7..eec011047 100644 --- a/src/BulletSoftBody/btSoftRigidDynamicsWorld.cpp +++ b/src/BulletSoftBody/btSoftRigidDynamicsWorld.cpp @@ -106,7 +106,7 @@ void btSoftRigidDynamicsWorld::debugDrawWorld() { btSoftBody* psb=(btSoftBody*)this->m_softBodies[i]; btSoftBodyHelpers::DrawFrame(psb,m_debugDrawer); - btSoftBodyHelpers::Draw(psb,m_debugDrawer,fDrawFlags::Std); + btSoftBodyHelpers::Draw(psb,m_debugDrawer,fDrawFlags::Nodes+fDrawFlags::Std); if (m_debugDrawer && (m_debugDrawer->getDebugMode() & btIDebugDraw::DBG_DrawAabb)) { btSoftBodyHelpers::DrawNodeTree(psb,m_debugDrawer); diff --git a/src/BulletSoftBody/btSparseSDF.h b/src/BulletSoftBody/btSparseSDF.h index 4f7c9656d..381c24609 100644 --- a/src/BulletSoftBody/btSparseSDF.h +++ b/src/BulletSoftBody/btSparseSDF.h @@ -109,6 +109,28 @@ struct btSparseSdf /* else setup a priority list... */ } // + int RemoveReferences(btCollisionShape* pcs) + { + int refcount=0; + for(int i=0;inext; + if(pc->pclient==pcs) + { + if(pp) pp->next=pn; else root=pn; + delete pc;pc=pp;++refcount; + } + pp=pc;pc=pn; + } + } + return(refcount); + } + // btScalar Evaluate( const btVector3& x, btCollisionShape* shape, btVector3& normal,