From b37a37a28ea6d0534b78abb5fcb7e92b39cfa7fd Mon Sep 17 00:00:00 2001 From: "erwin.coumans" Date: Sun, 6 Apr 2008 06:53:00 +0000 Subject: [PATCH] Added initial broadphase support for softbody versus softbody and softbody versus rigidbody (see btSoftSoftCollisionAlgorithm and btSoftRididCollisionAlgorithm) Added SOFTBODY_SHAPE_PROXYTYPE. Some refactoring for btSoftBody, needs more work. --- Demos/SoftDemo/SoftDemo.cpp | 947 ++++---- Demos/SoftDemo/SoftDemo.h | 52 +- ...oftBodyRigidBodyCollisionConfiguration.cpp | 77 + ...tSoftBodyRigidBodyCollisionConfiguration.h | 46 + .../btSoftRigidCollisionAlgorithm.cpp | 94 + .../SoftDemo/btSoftRigidCollisionAlgorithm.h | 69 + .../SoftDemo/btSoftSoftCollisionAlgorithm.cpp | 104 + Demos/SoftDemo/btSoftSoftCollisionAlgorithm.h | 62 + .../BroadphaseCollision/btBroadphaseProxy.h | 2 + .../btDefaultCollisionConfiguration.h | 2 +- src/BulletDynamics/SoftBody/btSoftBody.cpp | 1896 +++++++++-------- src/BulletDynamics/SoftBody/btSoftBody.h | 414 ++-- .../SoftBody/btSoftBodyHelpers.cpp | 516 +++-- .../SoftBody/btSoftBodyHelpers.h | 90 + 14 files changed, 2512 insertions(+), 1859 deletions(-) create mode 100644 Demos/SoftDemo/btSoftBodyRigidBodyCollisionConfiguration.cpp create mode 100644 Demos/SoftDemo/btSoftBodyRigidBodyCollisionConfiguration.h create mode 100644 Demos/SoftDemo/btSoftRigidCollisionAlgorithm.cpp create mode 100644 Demos/SoftDemo/btSoftRigidCollisionAlgorithm.h create mode 100644 Demos/SoftDemo/btSoftSoftCollisionAlgorithm.cpp create mode 100644 Demos/SoftDemo/btSoftSoftCollisionAlgorithm.h create mode 100644 src/BulletDynamics/SoftBody/btSoftBodyHelpers.h diff --git a/Demos/SoftDemo/SoftDemo.cpp b/Demos/SoftDemo/SoftDemo.cpp index 9b978ff2c..aac2826b2 100644 --- a/Demos/SoftDemo/SoftDemo.cpp +++ b/Demos/SoftDemo/SoftDemo.cpp @@ -26,7 +26,8 @@ subject to the following restrictions: #include "../GimpactTestDemo/TorusMesh.h" #include //printf debugging #include "../../Extras/ConvexHull/btConvexHull.h" - +#include "btSoftBodyRigidBodyCollisionConfiguration.h" +#include "BulletDynamics/SoftBody/btSoftBodyHelpers.h" static float gCollisionMargin = 0.05f/*0.05f*/; #include "SoftDemo.h" @@ -42,34 +43,32 @@ const int maxProxies = 32766; const int maxOverlap = 65535; // -btSoftBody* CreateFromConvexHull( btSoftBody::ISoftBody* isoftbody, - const btVector3* vertices, - int nvertices) +btSoftBody* btSoftBodyHelpers::CreateFromConvexHull(btSoftBody::btSoftBodyWorldInfo& worldInfo, const btVector3* vertices, + int nvertices) { -HullDesc hdsc(QF_TRIANGLES,nvertices,vertices); -HullResult hres; -HullLibrary hlib;/*??*/ -hdsc.mMaxVertices=nvertices; -hlib.CreateConvexHull(hdsc,hres); -btSoftBody* psb=btSoftBody::Create( isoftbody, - (int)hres.mNumOutputVertices, - hres.mOutputVertices,0); -for(int i=0;i<(int)hres.mNumFaces;++i) + HullDesc hdsc(QF_TRIANGLES,nvertices,vertices); + HullResult hres; + HullLibrary hlib;/*??*/ + hdsc.mMaxVertices=nvertices; + hlib.CreateConvexHull(hdsc,hres); + btSoftBody* psb=new btSoftBody(worldInfo,(int)hres.mNumOutputVertices, + hres.mOutputVertices,0); + for(int i=0;i<(int)hres.mNumFaces;++i) { - const int idx[]={ hres.mIndices[i*3+0], - hres.mIndices[i*3+1], - hres.mIndices[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); - psb->AppendFace(idx[0],idx[1],idx[2]); + const int idx[]={ hres.mIndices[i*3+0], + hres.mIndices[i*3+1], + hres.mIndices[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); + psb->AppendFace(idx[0],idx[1],idx[2]); } -hlib.ReleaseResult(hres); -psb->RandomizeConstraints(); -return(psb); + hlib.ReleaseResult(hres); + psb->RandomizeConstraints(); + return(psb); } @@ -101,7 +100,7 @@ void SoftDemo::createStack( btCollisionShape* boxShape, float halfCubeSize, int -rowSize * halfCubeSize + halfCubeSize + j * 2.0f * halfCubeSize, halfCubeSize + i * halfCubeSize * 2.0f, zPos); - + trans.setOrigin(pos); btScalar mass = 1.f; @@ -117,79 +116,64 @@ void SoftDemo::createStack( btCollisionShape* boxShape, float halfCubeSize, int - -// -// ISoftBody implementation -// - -// -void SoftDemo::SoftBodyImpl::Attach(btSoftBody*) -{} - -// -void SoftDemo::SoftBodyImpl::Detach(btSoftBody*) -{} - -// -void SoftDemo::SoftBodyImpl::StartCollide(const btVector3&,const btVector3&) -{} - +#ifdef DHSJHDSAKJDHSJKADHSAKJDHSA // bool SoftDemo::SoftBodyImpl::CheckContactPrecise(const btVector3& x, btSoftBody::ISoftBody::sCti& cti) { -btScalar maxdepth=0; -btGjkEpaSolver2::sResults res; -btDynamicsWorld* pdw=pdemo->m_dynamicsWorld; -btCollisionObjectArray& coa=pdw->getCollisionObjectArray(); -for(int i=0,ni=coa.size();im_dynamicsWorld; + + btCollisionObjectArray& coa=pdw->getCollisionObjectArray(); + for(int i=0,ni=coa.size();igetCollisionShape(); - if(shp->isConvex()) + btRigidBody* prb=(btRigidBody*)(coa[i]); + btCollisionShape* shp=prb->getCollisionShape(); + if(shp->isConvex()) { - btConvexShape* csh=static_cast(shp); - const btTransform& wtr=prb->getWorldTransform(); - const btScalar dst=btGjkEpaSolver2::SignedDistance(x,0.1,csh,wtr,res); - if(dst(shp); + const btTransform& wtr=prb->getWorldTransform(); + const btScalar dst=btGjkEpaSolver2::SignedDistance(x,0.1,csh,wtr,res); + if(dstm_dynamicsWorld; -btCollisionObjectArray& coa=pdw->getCollisionObjectArray(); -for(int i=0,ni=coa.size();im_dynamicsWorld; + btCollisionObjectArray& coa=pdw->getCollisionObjectArray(); + for(int i=0,ni=coa.size();igetCollisionShape(); - btConvexShape* csh=static_cast(shp); - const btTransform& wtr=prb->getWorldTransform(); - btScalar dst=pdemo->m_sparsesdf.Evaluate(wtr.invXform(x),csh,nrm); - nrm=wtr.getBasis()*nrm; - btVector3 wit=x-nrm*dst; - if(dstgetCollisionShape(); + btConvexShape* csh=static_cast(shp); + const btTransform& wtr=prb->getWorldTransform(); + btScalar dst=pdemo->m_sparsesdf.Evaluate(wtr.invXform(x),csh,nrm); + nrm=wtr.getBasis()*nrm; + btVector3 wit=x-nrm*dst; + if(dst0) + medium.m_velocity = btVector3(0,0,0); + medium.m_pressure = 0; + medium.m_density = air_density; + if(water_density>0) { - const btScalar depth=-(dot(x,water_normal)+water_offset); - if(depth>0) + const btScalar depth=-(dot(x,water_normal)+water_offset); + if(depth>0) { - medium.m_density = water_density; - medium.m_pressure = depth * - water_density * - pdemo->m_dynamicsWorld->getGravity().length(); + medium.m_density = water_density; + medium.m_pressure = depth * + water_density * + pdemo->m_dynamicsWorld->getGravity().length(); } } } +#endif extern int gNumManifold; @@ -230,16 +215,16 @@ void SoftDemo::clientMoveAndDisplay() float dt = getDeltaTimeMicroseconds() * 0.000001f; - -// printf("dt = %f: ",dt); - + // printf("dt = %f: ",dt); + + if (m_dynamicsWorld) { #define FIXED_STEP 0 #ifdef FIXED_STEP - m_dynamicsWorld->stepSimulation(dt=1.0f/60.f,0); - + m_dynamicsWorld->stepSimulation(dt=1.0f/60.f,0); + #else //during idle mode, just run 1 simulation step maximum int maxSimSubSteps = m_idle ? 1 : 1; @@ -266,35 +251,43 @@ void SoftDemo::clientMoveAndDisplay() #endif //VERBOSE_TIMESTEPPING_CONSOLEOUTPUT #endif - - /* soft bodies */ + + /* soft bodies collision detection */ for(int ib=0;ibupdateAabb(dt); + } + + /* soft bodies simulation */ + for(int ib=0;ibAddVelocity(m_dynamicsWorld->getGravity()*dt); psb->Step(dt); - } - m_sparsesdf.GarbageCollect(); - + } + + m_softBodyWorldInfo.m_sparsesdf.GarbageCollect(); + //optional but useful: debug drawing - + m_dynamicsWorld->debugDrawWorld(); } - + #ifdef USE_QUICKPROF - btProfiler::beginBlock("render"); + btProfiler::beginBlock("render"); #endif //USE_QUICKPROF - + renderme(); - + //render the graphics objects, with center of mass shift - updateCamera(); + updateCamera(); #ifdef USE_QUICKPROF - btProfiler::endBlock("render"); + btProfiler::endBlock("render"); #endif glFlush(); //some additional debugging info @@ -315,7 +308,7 @@ void SoftDemo::displayCallback(void) { glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); - + renderme(); glFlush(); @@ -332,18 +325,18 @@ void SoftDemo::displayCallback(void) { static inline btScalar UnitRand() { -return(rand()/(btScalar)RAND_MAX); + return(rand()/(btScalar)RAND_MAX); } static inline btScalar SignedUnitRand() { -return(UnitRand()*2-1); + return(UnitRand()*2-1); } static inline btVector3 Vector3Rand() { -const btVector3 p=btVector3(SignedUnitRand(),SignedUnitRand(),SignedUnitRand()); -return(p.normalized()); + const btVector3 p=btVector3(SignedUnitRand(),SignedUnitRand(),SignedUnitRand()); + return(p.normalized()); } // @@ -351,17 +344,17 @@ return(p.normalized()); // static void Ctor_RbUpStack(SoftDemo* pdemo,int count) { -float mass=10; -btCollisionShape* shape[]={ new btSphereShape(1.5), - new btBoxShape(btVector3(1,1,1)), - new btCylinderShapeX(btVector3(4,1,1))}; -static const int nshapes=sizeof(shape)/sizeof(shape[0]); -for(int i=0;ilocalCreateRigidBody(mass,startTransform,shape[i%nshapes]); + btTransform startTransform; + startTransform.setIdentity(); + startTransform.setOrigin(btVector3(0,1+6*i,0)); + btRigidBody* body=pdemo->localCreateRigidBody(mass,startTransform,shape[i%nshapes]); } } @@ -370,10 +363,10 @@ for(int i=0;ilocalCreateRigidBody(mass,startTransform,new btSphereShape(3)); + btTransform startTransform; + startTransform.setIdentity(); + startTransform.setOrigin(btVector3(0,13,0)); + btRigidBody* body=pdemo->localCreateRigidBody(mass,startTransform,new btSphereShape(3)); } // @@ -381,11 +374,11 @@ btRigidBody* body=pdemo->localCreateRigidBody(mass,startTransform,new btSphereS // static void Ctor_BigPlate(SoftDemo* pdemo,btScalar mass=15) { -btTransform startTransform; -startTransform.setIdentity(); -startTransform.setOrigin(btVector3(0,4,0.5)); -btRigidBody* body=pdemo->localCreateRigidBody(mass,startTransform,new btBoxShape(btVector3(5,1,5))); -body->setFriction(1); + btTransform startTransform; + startTransform.setIdentity(); + startTransform.setOrigin(btVector3(0,4,0.5)); + btRigidBody* body=pdemo->localCreateRigidBody(mass,startTransform,new btBoxShape(btVector3(5,1,5))); + body->setFriction(1); } // @@ -393,14 +386,14 @@ body->setFriction(1); // static void Ctor_LinearStair(SoftDemo* pdemo,const btVector3& org,const btVector3& sizes,btScalar angle,int count) { -btBoxShape* shape=new btBoxShape(sizes); -for(int i=0;ilocalCreateRigidBody(0,startTransform,shape); - body->setFriction(1); + btTransform startTransform; + startTransform.setIdentity(); + startTransform.setOrigin(org+btVector3(sizes.x()*i*2,sizes.y()*i*2,0)); + btRigidBody* body=pdemo->localCreateRigidBody(0,startTransform,shape); + body->setFriction(1); } } @@ -409,19 +402,20 @@ for(int i=0;im_softbodyimpl,c,8); -psb->GenerateBendingConstraints(2,1); -pdemo->m_softbodies.push_back(psb); -return(psb); + const btVector3 h=s*0.5; + const btVector3 c[]={ p+h*btVector3(-1,-1,-1), + p+h*btVector3(+1,-1,-1), + p+h*btVector3(-1,+1,-1), + p+h*btVector3(+1,+1,-1), + p+h*btVector3(-1,-1,+1), + p+h*btVector3(+1,-1,+1), + 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); + pdemo->m_softbodies.push_back(psb); + + return(psb); } // @@ -429,16 +423,17 @@ return(psb); // static btSoftBody* Ctor_SoftBoulder(SoftDemo* pdemo,const btVector3& p,const btVector3& s,int np,int id) { -btAlignedObjectArray pts; -if(id) srand(id); -for(int i=0;i pts; + if(id) srand(id); + for(int i=0;im_softbodyimpl,&pts[0],pts.size()); -psb->GenerateBendingConstraints(2,1); -pdemo->m_softbodies.push_back(psb); -return(psb); + btSoftBody* psb=btSoftBodyHelpers::CreateFromConvexHull(pdemo->m_softBodyWorldInfo,&pts[0],pts.size()); + psb->GenerateBendingConstraints(2,1); + pdemo->m_softbodies.push_back(psb); + + return(psb); } //#define TRACEDEMO { pdemo->demoname=__FUNCTION__+5;printf("Launching demo: " __FUNCTION__ "\r\n"); } @@ -448,19 +443,19 @@ return(psb); // static void Init_Ropes(SoftDemo* pdemo) { -//TRACEDEMO -const int n=15; -for(int i=0;im_softbodyimpl, - btVector3(-10,0,i*0.25), - 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->SetTotalMass(20); - pdemo->m_softbodies.push_back(psb); + btSoftBody* psb=btSoftBodyHelpers::CreateRope(pdemo->m_softBodyWorldInfo, btVector3(-10,0,i*0.25), + 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->SetTotalMass(20); + pdemo->m_softbodies.push_back(psb); + } } @@ -469,27 +464,26 @@ for(int i=0;im_softbodyimpl, - p,p+btVector3(10,0,0),8,1); - psb->m_cfg.kDF = 0; - psb->SetTotalMass(50); - pdemo->m_softbodies.push_back(psb); - return(psb); + btSoftBody* psb=btSoftBodyHelpers::CreateRope(pdemo->m_softBodyWorldInfo,p,p+btVector3(10,0,0),8,1); + psb->m_cfg.kDF = 0; + psb->SetTotalMass(50); + pdemo->m_softbodies.push_back(psb); + return(psb); } }; -btTransform startTransform; -startTransform.setIdentity(); -startTransform.setOrigin(btVector3(12,8,0)); -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->m_nodes.size()-1,body); -psb1->AppendAnchor(psb1->m_nodes.size()-1,body); + btTransform startTransform; + startTransform.setIdentity(); + startTransform.setOrigin(btVector3(12,8,0)); + 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); } // @@ -497,22 +491,22 @@ psb1->AppendAnchor(psb1->m_nodes.size()-1,body); // static void Init_ClothAttach(SoftDemo* pdemo) { -//TRACEDEMO -const btScalar s=4; -const btScalar h=6; -const int r=9; -btSoftBody* psb=CreatePatch(&pdemo->m_softbodyimpl, - btVector3(-s,h,-s), - btVector3(+s,h,-s), - btVector3(-s,h,+s), - btVector3(+s,h,+s),r,r,4+8,true); -pdemo->m_softbodies.push_back(psb); -btTransform startTransform; -startTransform.setIdentity(); -startTransform.setOrigin(btVector3(0,h,-(s+3.5))); -btRigidBody* body=pdemo->localCreateRigidBody(20,startTransform,new btBoxShape(btVector3(s,1,3))); -psb->AppendAnchor(0,body); -psb->AppendAnchor(r-1,body); + //TRACEDEMO + const btScalar s=4; + const btScalar h=6; + const int r=9; + btSoftBody* psb=btSoftBodyHelpers::CreatePatch(pdemo->m_softBodyWorldInfo,btVector3(-s,h,-s), + btVector3(+s,h,-s), + btVector3(-s,h,+s), + btVector3(+s,h,+s),r,r,4+8,true); + pdemo->m_softbodies.push_back(psb); + + btTransform startTransform; + startTransform.setIdentity(); + startTransform.setOrigin(btVector3(0,h,-(s+3.5))); + btRigidBody* body=pdemo->localCreateRigidBody(20,startTransform,new btBoxShape(btVector3(s,1,3))); + psb->AppendAnchor(0,body); + psb->AppendAnchor(r-1,body); } // @@ -520,17 +514,17 @@ psb->AppendAnchor(r-1,body); // static void Init_Impact(SoftDemo* pdemo) { -//TRACEDEMO -btSoftBody* psb=CreateRope( &pdemo->m_softbodyimpl, - btVector3(0,0,0), - btVector3(0,-1,0), - 0, - 1); -pdemo->m_softbodies.push_back(psb); -btTransform startTransform; -startTransform.setIdentity(); -startTransform.setOrigin(btVector3(0,20,0)); -btRigidBody* body=pdemo->localCreateRigidBody(10,startTransform,new btBoxShape(btVector3(2,2,2))); + //TRACEDEMO + btSoftBody* psb=btSoftBodyHelpers::CreateRope(pdemo->m_softBodyWorldInfo, btVector3(0,0,0), + btVector3(0,-1,0), + 0, + 1); + pdemo->m_softbodies.push_back(psb); + + btTransform startTransform; + startTransform.setIdentity(); + startTransform.setOrigin(btVector3(0,20,0)); + btRigidBody* body=pdemo->localCreateRigidBody(10,startTransform,new btBoxShape(btVector3(2,2,2))); } // @@ -538,38 +532,38 @@ btRigidBody* body=pdemo->localCreateRigidBody(10,startTransform,new btBoxShape( // static void Init_Aero(SoftDemo* pdemo) { -//TRACEDEMO -const btScalar s=2; -const btScalar h=10; -const int segments=6; -const int count=50; -for(int i=0;im_softbodyimpl, - btVector3(-s,h,-s), - btVector3(+s,h,-s), - btVector3(-s,h,+s), - btVector3(+s,h,+s), - segments,segments, - 0,true); - psb->GenerateBendingConstraints(2,1); - psb->m_cfg.kLF = 0.004; - psb->m_cfg.kDG = 0.0003; - psb->m_cfg.aeromodel = btSoftBody::eAeroModel::V_TwoSided; - btTransform trs; - btQuaternion rot; - btVector3 ra=Vector3Rand()*0.1; - btVector3 rp=Vector3Rand()*15+btVector3(0,20,80); - rot.setEuler(SIMD_PI/8+ra.x(),-SIMD_PI/7+ra.y(),ra.z()); - trs.setIdentity(); - trs.setOrigin(rp); - trs.setRotation(rot); - psb->Transform(trs); - psb->SetTotalMass(0.1); - psb->AddForce(btVector3(0,2,0),0); - pdemo->m_softbodies.push_back(psb); + btSoftBody* psb=btSoftBodyHelpers::CreatePatch(pdemo->m_softBodyWorldInfo,btVector3(-s,h,-s), + btVector3(+s,h,-s), + btVector3(-s,h,+s), + btVector3(+s,h,+s), + segments,segments, + 0,true); + psb->GenerateBendingConstraints(2,1); + psb->m_cfg.kLF = 0.004; + psb->m_cfg.kDG = 0.0003; + psb->m_cfg.aeromodel = btSoftBody::eAeroModel::V_TwoSided; + btTransform trs; + btQuaternion rot; + btVector3 ra=Vector3Rand()*0.1; + btVector3 rp=Vector3Rand()*15+btVector3(0,20,80); + rot.setEuler(SIMD_PI/8+ra.x(),-SIMD_PI/7+ra.y(),ra.z()); + trs.setIdentity(); + trs.setOrigin(rp); + trs.setRotation(rot); + psb->Transform(trs); + psb->SetTotalMass(0.1); + psb->AddForce(btVector3(0,2,0),0); + pdemo->m_softbodies.push_back(psb); + } -pdemo->m_autocam=true; + pdemo->m_autocam=true; } // @@ -577,15 +571,15 @@ pdemo->m_autocam=true; // static void Init_Friction(SoftDemo* pdemo) { -//TRACEDEMO -const btScalar bs=2; -const btScalar ts=bs+bs/4; -for(int i=0,ni=20;im_cfg.kDF = 0.1 * ((i+1)/(btScalar)ni); - psb->AddVelocity(btVector3(0,0,-10)); + const btVector3 p(-ni*ts/2+i*ts,-10+bs,40); + btSoftBody* psb=Ctor_SoftBox(pdemo,p,btVector3(bs,bs,bs)); + psb->m_cfg.kDF = 0.1 * ((i+1)/(btScalar)ni); + psb->AddVelocity(btVector3(0,0,-10)); } } @@ -594,19 +588,19 @@ for(int i=0,ni=20;im_softbodyimpl, - btVector3(35,25,0), - btVector3(1,1,1)*3, - 512); -psb->m_cfg.kLST = 0.1; -psb->m_cfg.kDF = 1; -psb->m_cfg.kPR = 2500; -psb->SetTotalMass(30,true); -pdemo->m_softbodies.push_back(psb); -Ctor_BigPlate(pdemo); -Ctor_LinearStair(pdemo,btVector3(0,0,0),btVector3(2,1,5),0,10); -pdemo->m_autocam=true; + //TRACEDEMO + 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.kPR = 2500; + psb->SetTotalMass(30,true); + pdemo->m_softbodies.push_back(psb); + + Ctor_BigPlate(pdemo); + Ctor_LinearStair(pdemo,btVector3(0,0,0),btVector3(2,1,5),0,10); + pdemo->m_autocam=true; } // @@ -614,19 +608,19 @@ pdemo->m_autocam=true; // static void Init_Volume(SoftDemo* pdemo) { -//TRACEDEMO -btSoftBody* psb=CreateEllipsoid(&pdemo->m_softbodyimpl, - btVector3(35,25,0), - btVector3(1,1,1)*3, - 512); -psb->m_cfg.kLST = 0.45; -psb->m_cfg.kVC = 20; -psb->SetTotalMass(50,true); -psb->SetPose(true,false); -pdemo->m_softbodies.push_back(psb); -Ctor_BigPlate(pdemo); -Ctor_LinearStair(pdemo,btVector3(0,0,0),btVector3(2,1,5),0,10); -pdemo->m_autocam=true; + //TRACEDEMO + 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->SetTotalMass(50,true); + psb->SetPose(true,false); + pdemo->m_softbodies.push_back(psb); + + Ctor_BigPlate(pdemo); + Ctor_LinearStair(pdemo,btVector3(0,0,0),btVector3(2,1,5),0,10); + pdemo->m_autocam=true; } // @@ -634,37 +628,37 @@ pdemo->m_autocam=true; // static void Init_Sticks(SoftDemo* pdemo) { -//TRACEDEMO -const int n=16; -const int sg=4; -const btScalar sz=5; -const btScalar hg=4; -const btScalar in=1/(btScalar)(n-1); -for(int y=0;ym_softbodyimpl, - org, - 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) + const btVector3 org(-sz+sz*2*x*in, + -10, + -sz+sz*2*y*in); + btSoftBody* psb=btSoftBodyHelpers::CreateRope( pdemo->m_softBodyWorldInfo, org, + 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,1); } - psb->SetMass(1,0); - psb->SetTotalMass(0.01); - pdemo->m_softbodies.push_back(psb); + psb->SetMass(1,0); + psb->SetTotalMass(0.01); + pdemo->m_softbodies.push_back(psb); + } } -Ctor_BigBall(pdemo); + Ctor_BigBall(pdemo); } // @@ -672,20 +666,22 @@ Ctor_BigBall(pdemo); // static void Init_Cloth(SoftDemo* pdemo) { -//TRACEDEMO -const btScalar s=8; -btSoftBody* psb=CreatePatch(&pdemo->m_softbodyimpl, - btVector3(-s,0,-s), - btVector3(+s,0,-s), - btVector3(-s,0,+s), - btVector3(+s,0,+s), - 31,31, - 1+2+4+8,true); -psb->GenerateBendingConstraints(2,1); -psb->m_cfg.kLST = 0.4; -psb->SetTotalMass(150); -pdemo->m_softbodies.push_back(psb); -Ctor_RbUpStack(pdemo,10); + //TRACEDEMO + const btScalar s=8; + btSoftBody* psb=btSoftBodyHelpers::CreatePatch( pdemo->m_softBodyWorldInfo,btVector3(-s,0,-s), + btVector3(+s,0,-s), + btVector3(-s,0,+s), + btVector3(+s,0,+s), + 31,31, + + // 31,31, + 1+2+4+8,true); + psb->GenerateBendingConstraints(2,1); + psb->m_cfg.kLST = 0.4; + psb->SetTotalMass(150); + pdemo->m_softbodies.push_back(psb); + + Ctor_RbUpStack(pdemo,10); } // @@ -693,18 +689,18 @@ Ctor_RbUpStack(pdemo,10); // static void Init_Bunny(SoftDemo* pdemo) { -//TRACEDEMO -btSoftBody* psb=CreateFromTriMesh( &pdemo->m_softbodyimpl, - gVerticesBunny, - &gIndicesBunny[0][0], - BUNNY_NUM_TRIANGLES); -psb->GenerateBendingConstraints(2,0.5); -psb->m_cfg.iterations = 2; -psb->m_cfg.kDF = 0.5; -psb->RandomizeConstraints(); -psb->Scale(btVector3(6,6,6)); -psb->SetTotalMass(100,true); -pdemo->m_softbodies.push_back(psb); + //TRACEDEMO + btSoftBody* psb=btSoftBodyHelpers::CreateFromTriMesh(pdemo->m_softBodyWorldInfo,gVerticesBunny, + &gIndicesBunny[0][0], + BUNNY_NUM_TRIANGLES); + psb->GenerateBendingConstraints(2,0.5); + psb->m_cfg.iterations = 2; + psb->m_cfg.kDF = 0.5; + psb->RandomizeConstraints(); + psb->Scale(btVector3(6,6,6)); + psb->SetTotalMass(100,true); + pdemo->m_softbodies.push_back(psb); + } // @@ -712,20 +708,20 @@ pdemo->m_softbodies.push_back(psb); // static void Init_BunnyMatch(SoftDemo* pdemo) { -//TRACEDEMO -btSoftBody* psb=CreateFromTriMesh( &pdemo->m_softbodyimpl, - gVerticesBunny, - &gIndicesBunny[0][0], - BUNNY_NUM_TRIANGLES); -psb->GenerateBendingConstraints(2,0.5); -psb->m_cfg.kDF = 0.5; -psb->m_cfg.kLST = 0.1; -psb->m_cfg.kMT = 0.01; -psb->RandomizeConstraints(); -psb->Scale(btVector3(6,6,6)); -psb->SetTotalMass(100,true); -psb->SetPose(true,true); -pdemo->m_softbodies.push_back(psb); + //TRACEDEMO + btSoftBody* psb=btSoftBodyHelpers::CreateFromTriMesh(pdemo->m_softBodyWorldInfo, gVerticesBunny, + &gIndicesBunny[0][0], + BUNNY_NUM_TRIANGLES); + psb->GenerateBendingConstraints(2,0.5); + psb->m_cfg.kDF = 0.5; + psb->m_cfg.kLST = 0.1; + psb->m_cfg.kMT = 0.01; + psb->RandomizeConstraints(); + psb->Scale(btVector3(6,6,6)); + psb->SetTotalMass(100,true); + psb->SetPose(true,true); + pdemo->m_softbodies.push_back(psb); + } // @@ -733,20 +729,20 @@ pdemo->m_softbodies.push_back(psb); // static void Init_Torus(SoftDemo* pdemo) { -//TRACEDEMO -btSoftBody* psb=CreateFromTriMesh( &pdemo->m_softbodyimpl, - gVertices, - &gIndices[0][0], - NUM_TRIANGLES); -psb->GenerateBendingConstraints(2,1); -psb->m_cfg.iterations=2; -psb->RandomizeConstraints(); -btMatrix3x3 m; -m.setEulerZYX(SIMD_PI/2,0,0); -psb->Transform(btTransform(m,btVector3(0,4,0))); -psb->Scale(btVector3(2,2,2)); -psb->SetTotalMass(50,true); -pdemo->m_softbodies.push_back(psb); + //TRACEDEMO + btSoftBody* psb=btSoftBodyHelpers::CreateFromTriMesh( pdemo->m_softBodyWorldInfo, gVertices, + &gIndices[0][0], + NUM_TRIANGLES); + psb->GenerateBendingConstraints(2,1); + psb->m_cfg.iterations=2; + psb->RandomizeConstraints(); + btMatrix3x3 m; + m.setEulerZYX(SIMD_PI/2,0,0); + psb->Transform(btTransform(m,btVector3(0,4,0))); + psb->Scale(btVector3(2,2,2)); + psb->SetTotalMass(50,true); + pdemo->m_softbodies.push_back(psb); + } // @@ -754,118 +750,124 @@ pdemo->m_softbodies.push_back(psb); // static void Init_TorusMatch(SoftDemo* pdemo) { -//TRACEDEMO -btSoftBody* psb=CreateFromTriMesh( &pdemo->m_softbodyimpl, - gVertices, - &gIndices[0][0], - NUM_TRIANGLES); -psb->GenerateBendingConstraints(2,1); -psb->m_cfg.kLST=0.1; -psb->m_cfg.kMT=0.05; -psb->RandomizeConstraints(); -btMatrix3x3 m; -m.setEulerZYX(SIMD_PI/2,0,0); -psb->Transform(btTransform(m,btVector3(0,4,0))); -psb->Scale(btVector3(2,2,2)); -psb->SetTotalMass(50,true); -psb->SetPose(true,true); -pdemo->m_softbodies.push_back(psb); + //TRACEDEMO + btSoftBody* psb=btSoftBodyHelpers::CreateFromTriMesh(pdemo->m_softBodyWorldInfo, gVertices, + &gIndices[0][0], + NUM_TRIANGLES); + psb->GenerateBendingConstraints(2,1); + psb->m_cfg.kLST=0.1; + psb->m_cfg.kMT=0.05; + psb->RandomizeConstraints(); + btMatrix3x3 m; + m.setEulerZYX(SIMD_PI/2,0,0); + psb->Transform(btTransform(m,btVector3(0,4,0))); + psb->Scale(btVector3(2,2,2)); + psb->SetTotalMass(50,true); + psb->SetPose(true,true); + pdemo->m_softbodies.push_back(psb); + + } static unsigned current_demo=0; void SoftDemo::clientResetScene() { -DemoApplication::clientResetScene(); -/* Clean up */ -for(int i=m_dynamicsWorld->getNumCollisionObjects()-1;i>0;i--) + DemoApplication::clientResetScene(); + /* Clean up */ + for(int i=m_dynamicsWorld->getNumCollisionObjects()-1;i>0;i--) { - btCollisionObject* obj=m_dynamicsWorld->getCollisionObjectArray()[i]; - btRigidBody* body=btRigidBody::upcast(obj); - if(body&&body->getMotionState()) + btCollisionObject* obj=m_dynamicsWorld->getCollisionObjectArray()[i]; + btRigidBody* body=btRigidBody::upcast(obj); + if(body&&body->getMotionState()) { - delete body->getMotionState(); + delete body->getMotionState(); } - m_dynamicsWorld->removeCollisionObject(obj); - delete obj; + m_dynamicsWorld->removeCollisionObject(obj); + delete obj; } -for(int i=0;igetDebugDrawer(); -/* Bodies */ -btVector3 ps(0,0,0); -int nps=0; -for(int ib=0;ibgetDebugDrawer(); + /* Bodies */ + btVector3 ps(0,0,0); + int nps=0; + for(int ib=0;ibm_nodes.size(); - for(int i=0;im_nodes.size();++i) + btSoftBody* psb=m_softbodies[ib]; + nps+=psb->getNodes().size(); + for(int i=0;igetNodes().size();++i) { - ps+=psb->m_nodes[i].m_x; + ps+=psb->getNodes()[i].m_x; } - DrawFrame(psb,idraw); - Draw(psb,idraw,fDrawFlags::Std); + btSoftBodyHelpers::DrawFrame(psb,idraw); + btSoftBodyHelpers::Draw(psb,idraw,fDrawFlags::Std); } -ps/=nps; -if(m_autocam) - m_cameraTargetPosition+=(ps-m_cameraTargetPosition)*0.01; + ps/=nps; + if(m_autocam) + m_cameraTargetPosition+=(ps-m_cameraTargetPosition)*0.01; else - m_cameraTargetPosition=btVector3(0,0,0); -/* Water level */ -static const btVector3 axis[]={btVector3(1,0,0), - btVector3(0,1,0), - btVector3(0,0,1)}; -if(m_softbodyimpl.water_density>0) + m_cameraTargetPosition=btVector3(0,0,0); + /* Water level */ + static const btVector3 axis[]={btVector3(1,0,0), + btVector3(0,1,0), + btVector3(0,0,1)}; + if(m_softBodyWorldInfo.water_density>0) { - const btVector3 c= btVector3((btScalar)0.25,(btScalar)0.25,1); - const btScalar a= (btScalar)0.5; - const btVector3 n= m_softbodyimpl.water_normal; - const btVector3 o= -n*m_softbodyimpl.water_offset; - const btVector3 x= cross(n,axis[n.minAxis()]).normalized(); - const btVector3 y= cross(x,n).normalized(); - const btScalar s= 25; - idraw->drawTriangle(o-x*s-y*s,o+x*s-y*s,o+x*s+y*s,c,a); - idraw->drawTriangle(o-x*s-y*s,o+x*s+y*s,o-x*s+y*s,c,a); + const btVector3 c= btVector3((btScalar)0.25,(btScalar)0.25,1); + const btScalar a= (btScalar)0.5; + const btVector3 n= m_softBodyWorldInfo.water_normal; + const btVector3 o= -n*m_softBodyWorldInfo.water_offset; + const btVector3 x= cross(n,axis[n.minAxis()]).normalized(); + const btVector3 y= cross(x,n).normalized(); + const btScalar s= 25; + idraw->drawTriangle(o-x*s-y*s,o+x*s-y*s,o+x*s+y*s,c,a); + idraw->drawTriangle(o-x*s-y*s,o+x*s+y*s,o-x*s+y*s,c,a); } } void SoftDemo::keyboardCallback(unsigned char key, int x, int y) { -switch(key) + switch(key) { case ']': ++current_demo;clientResetScene();break; case '[': --current_demo;clientResetScene();break; @@ -878,44 +880,40 @@ switch(key) void SoftDemo::initPhysics() { -//#define USE_GROUND_PLANE 1 -#ifdef USE_GROUND_PLANE - m_collisionShapes.push_back(new btStaticPlaneShape(btVector3(0,1,0),0.5)); -#else - - ///Please don't make the box sizes larger then 1000: the collision detection will be inaccurate. - ///See http://www.continuousphysics.com/Bullet/phpBB2/viewtopic.php?t=346 + m_collisionShapes.push_back(new btBoxShape (btVector3(200,CUBE_HALF_EXTENTS,200))); - //m_collisionShapes.push_back(new btCylinderShapeZ (btVector3(5,5,5))); - //m_collisionShapes.push_back(new btSphereShape(5)); -#endif m_collisionShapes.push_back(new btCylinderShape (btVector3(CUBE_HALF_EXTENTS,CUBE_HALF_EXTENTS,CUBE_HALF_EXTENTS))); - + m_dispatcher=0; - m_collisionConfiguration = new btDefaultCollisionConfiguration(); - - + + ///register some softbody collision algorithms on top of the default btDefaultCollisionConfiguration + m_collisionConfiguration = new btSoftBodyRigidBodyCollisionConfiguration(); + + m_dispatcher = new btCollisionDispatcher(m_collisionConfiguration); + m_softBodyWorldInfo.m_dispatcher = m_dispatcher; + + //////////////////////////// + ///Register softbody versus softbody collision algorithm + + + ///Register softbody versus rigidbody collision algorithm + + + //////////////////////////// btVector3 worldAabbMin(-1000,-1000,-1000); btVector3 worldAabbMax(1000,1000,1000); m_broadphase = new btAxisSweep3(worldAabbMin,worldAabbMax,maxProxies); -/// For large worlds or over 16384 objects, use the bt32BitAxisSweep3 broadphase -// m_broadphase = new bt32BitAxisSweep3(worldAabbMin,worldAabbMax,maxProxies); -/// When trying to debug broadphase issues, try to use the btSimpleBroadphase -// m_broadphase = new btSimpleBroadphase; - - //box-box is in Extras/AlternativeCollisionAlgorithms:it requires inclusion of those files + m_softBodyWorldInfo.m_broadphase = m_broadphase; btSequentialImpulseConstraintSolver* solver = new btSequentialImpulseConstraintSolver(); m_solver = solver; - //default solverMode is SOLVER_RANDMIZE_ORDER. Warmstarting seems not to improve convergence, see - //solver->setSolverMode(0);//btSequentialImpulseConstraintSolver::SOLVER_USE_WARMSTARTING | btSequentialImpulseConstraintSolver::SOLVER_RANDMIZE_ORDER); btDiscreteDynamicsWorld* world = new btDiscreteDynamicsWorld(m_dispatcher,m_broadphase,m_solver,m_collisionConfiguration); m_dynamicsWorld = world; @@ -924,7 +922,7 @@ void SoftDemo::initPhysics() m_dynamicsWorld->getDispatchInfo().m_enableSPU = true; m_dynamicsWorld->setGravity(btVector3(0,-10,0)); - + int i; @@ -933,18 +931,17 @@ void SoftDemo::initPhysics() tr.setIdentity(); tr.setOrigin(btVector3(0,-20,0)); - + btRigidBody* body = localCreateRigidBody(0.f,tr,m_collisionShapes[0]); - -// clientResetScene(); -m_softbodyimpl.pdemo=this; -m_sparsesdf.Initialize(); -clientResetScene(); + // clientResetScene(); + + m_softBodyWorldInfo.m_sparsesdf.Initialize(); + clientResetScene(); } - + @@ -993,7 +990,7 @@ void SoftDemo::exitPhysics() delete m_collisionConfiguration; - + } diff --git a/Demos/SoftDemo/SoftDemo.h b/Demos/SoftDemo/SoftDemo.h index 8f1572279..2628f3d04 100644 --- a/Demos/SoftDemo/SoftDemo.h +++ b/Demos/SoftDemo/SoftDemo.h @@ -21,7 +21,7 @@ subject to the following restrictions: #include "DemoApplication.h" #include "LinearMath/btAlignedObjectArray.h" #include "BulletDynamics/SoftBody/btSoftBody.h" -#include "BulletDynamics/SoftBody/btSparseSDF.h" + class btBroadphaseInterface; class btCollisionShape; @@ -31,33 +31,29 @@ class btConstraintSolver; struct btCollisionAlgorithmCreateFunc; class btDefaultCollisionConfiguration; +///collisions between two btSoftBody's +class btSoftSoftCollisionAlgorithm; + +///collisions between a btSoftBody and a btRigidBody +class btSoftRididCollisionAlgorithm; + ///CcdPhysicsDemo shows basic stacking using Bullet physics, and allows toggle of Ccd (using key '1') class SoftDemo : public DemoApplication { public: -struct SoftBodyImpl : btSoftBody::ISoftBody - { - void Attach(btSoftBody*); - void Detach(btSoftBody*); - void StartCollide(const btVector3&,const btVector3&); - bool CheckContactPrecise(const btVector3&, - btSoftBody::ISoftBody::sCti&); - bool CheckContact( const btVector3&, - btSoftBody::ISoftBody::sCti&); - void EndCollide(); - void EvaluateMedium( const btVector3&, - btSoftBody::ISoftBody::sMedium&); - SoftDemo* pdemo; - btScalar air_density; - btScalar water_density; - btScalar water_offset; - btVector3 water_normal; - } m_softbodyimpl; + + btAlignedObjectArray m_SoftSoftCollisionAlgorithms; + + btAlignedObjectArray m_SoftRigidCollisionAlgorithms; + + btSoftBody::btSoftBodyWorldInfo m_softBodyWorldInfo; + btAlignedObjectArray m_softbodies; - btSparseSdf<3> m_sparsesdf; - bool m_autocam; + + bool m_autocam; + //keep the collision shapes, for deletion/cleanup btAlignedObjectArray m_collisionShapes; @@ -66,12 +62,6 @@ struct SoftBodyImpl : btSoftBody::ISoftBody btCollisionDispatcher* m_dispatcher; -#ifdef USE_PARALLEL_DISPATCHER -#ifdef WIN32 - class Win32ThreadSupport* m_threadSupportCollision; - class Win32ThreadSupport* m_threadSupportSolver; -#endif -#endif btConstraintSolver* m_solver; @@ -80,7 +70,7 @@ struct SoftBodyImpl : btSoftBody::ISoftBody btDefaultCollisionConfiguration* m_collisionConfiguration; - public: +public: void initPhysics(); @@ -94,9 +84,9 @@ struct SoftBodyImpl : btSoftBody::ISoftBody virtual void clientMoveAndDisplay(); virtual void displayCallback(); - + void createStack( btCollisionShape* boxShape, float halfCubeSize, int size, float zPos ); - + static DemoApplication* Create() { SoftDemo* demo = new SoftDemo; @@ -104,7 +94,7 @@ struct SoftBodyImpl : btSoftBody::ISoftBody demo->initPhysics(); return demo; } - + // void clientResetScene(); void renderme(); diff --git a/Demos/SoftDemo/btSoftBodyRigidBodyCollisionConfiguration.cpp b/Demos/SoftDemo/btSoftBodyRigidBodyCollisionConfiguration.cpp new file mode 100644 index 000000000..5bd80f52f --- /dev/null +++ b/Demos/SoftDemo/btSoftBodyRigidBodyCollisionConfiguration.cpp @@ -0,0 +1,77 @@ +/* +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. +*/ + +#include "btSoftBodyRigidBodyCollisionConfiguration.h" +#include "btSoftRigidCollisionAlgorithm.h" +#include "btSoftSoftCollisionAlgorithm.h" + + +btSoftBodyRigidBodyCollisionConfiguration::btSoftBodyRigidBodyCollisionConfiguration(btStackAlloc* stackAlloc,btPoolAllocator* persistentManifoldPool,btPoolAllocator* collisionAlgorithmPool) +:btDefaultCollisionConfiguration(stackAlloc,persistentManifoldPool,collisionAlgorithmPool) +{ + void* mem; + + mem = btAlignedAlloc(sizeof(btSoftSoftCollisionAlgorithm::CreateFunc),16); + m_softSoftCreateFunc = new(mem) btSoftSoftCollisionAlgorithm::CreateFunc; + + mem = btAlignedAlloc(sizeof(btSoftRigidCollisionAlgorithm::CreateFunc),16); + m_softRigidCreateFunc = new(mem) btSoftRigidCollisionAlgorithm::CreateFunc; + + mem = btAlignedAlloc(sizeof(btSoftRigidCollisionAlgorithm::CreateFunc),16); + m_swappedSoftRigidCreateFunc = new(mem) btSoftRigidCollisionAlgorithm::CreateFunc; + m_swappedSoftRigidCreateFunc->m_swapped=true; + +} + +btSoftBodyRigidBodyCollisionConfiguration::~btSoftBodyRigidBodyCollisionConfiguration() +{ + m_softSoftCreateFunc->~btCollisionAlgorithmCreateFunc(); + btAlignedFree( m_softSoftCreateFunc); + + m_softRigidCreateFunc->~btCollisionAlgorithmCreateFunc(); + btAlignedFree( m_softRigidCreateFunc); + + m_swappedSoftRigidCreateFunc->~btCollisionAlgorithmCreateFunc(); + btAlignedFree( m_swappedSoftRigidCreateFunc); + + +} + +///creation of soft-soft and soft-rigid, and otherwise fallback to base class implementation +btCollisionAlgorithmCreateFunc* btSoftBodyRigidBodyCollisionConfiguration::getCollisionAlgorithmCreateFunc(int proxyType0,int proxyType1) +{ + + ///try to handle the softbody interactions first + + if ((proxyType0 == SOFTBODY_SHAPE_PROXYTYPE ) && (proxyType1==SOFTBODY_SHAPE_PROXYTYPE)) + { + return m_softSoftCreateFunc; + } + + ///other can't be also softbody, so assume rigid for now + if (proxyType0 == SOFTBODY_SHAPE_PROXYTYPE ) + { + return m_softRigidCreateFunc; + } + + ///other can't be also softbody, so assume rigid for now + if (proxyType1 == SOFTBODY_SHAPE_PROXYTYPE ) + { + return m_swappedSoftRigidCreateFunc; + } + + ///fallback to the regular rigid collision shape + return btDefaultCollisionConfiguration::getCollisionAlgorithmCreateFunc(proxyType0,proxyType1); +} \ No newline at end of file diff --git a/Demos/SoftDemo/btSoftBodyRigidBodyCollisionConfiguration.h b/Demos/SoftDemo/btSoftBodyRigidBodyCollisionConfiguration.h new file mode 100644 index 000000000..e38289b76 --- /dev/null +++ b/Demos/SoftDemo/btSoftBodyRigidBodyCollisionConfiguration.h @@ -0,0 +1,46 @@ +/* +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. +*/ + +#ifndef BT_SOFTBODY_RIGIDBODY_COLLISION_CONFIGURATION +#define BT_SOFTBODY_RIGIDBODY_COLLISION_CONFIGURATION + +#include "BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.h" + +class btVoronoiSimplexSolver; +class btGjkEpaPenetrationDepthSolver; + + +///btSoftBodyRigidBodyCollisionConfiguration add softbody interaction on top of btDefaultCollisionConfiguration +class btSoftBodyRigidBodyCollisionConfiguration : public btDefaultCollisionConfiguration +{ + + //default CreationFunctions, filling the m_doubleDispatch table + btCollisionAlgorithmCreateFunc* m_softSoftCreateFunc; + btCollisionAlgorithmCreateFunc* m_softRigidCreateFunc; + btCollisionAlgorithmCreateFunc* m_swappedSoftRigidCreateFunc; + +public: + + btSoftBodyRigidBodyCollisionConfiguration(btStackAlloc* stackAlloc=0,btPoolAllocator* persistentManifoldPool=0,btPoolAllocator* collisionAlgorithmPool=0); + + virtual ~btSoftBodyRigidBodyCollisionConfiguration(); + + ///creation of soft-soft and soft-rigid, and otherwise fallback to base class implementation + virtual btCollisionAlgorithmCreateFunc* getCollisionAlgorithmCreateFunc(int proxyType0,int proxyType1); + +}; + +#endif //BT_SOFTBODY_RIGIDBODY_COLLISION_CONFIGURATION + diff --git a/Demos/SoftDemo/btSoftRigidCollisionAlgorithm.cpp b/Demos/SoftDemo/btSoftRigidCollisionAlgorithm.cpp new file mode 100644 index 000000000..9d4f880b2 --- /dev/null +++ b/Demos/SoftDemo/btSoftRigidCollisionAlgorithm.cpp @@ -0,0 +1,94 @@ +/* +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. +*/ + +#include "btSoftRigidCollisionAlgorithm.h" +#include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h" +#include "BulletCollision/CollisionShapes/btSphereShape.h" +#include "BulletCollision/CollisionShapes/btBoxShape.h" +#include "BulletCollision/CollisionDispatch/btCollisionObject.h" +#include "BulletDynamics/SoftBody/btSoftBody.h" +///TODO: include all the shapes that the softbody can collide with +///alternatively, implement special case collision algorithms (just like for rigid collision shapes) + +//#include + +btSoftRigidCollisionAlgorithm::btSoftRigidCollisionAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* col0,btCollisionObject* col1, bool isSwapped) +: btCollisionAlgorithm(ci), +//m_ownManifold(false), +//m_manifoldPtr(mf), +m_isSwapped(isSwapped) +{ + + m_softBody = m_isSwapped? (btSoftBody*)col1 : (btSoftBody*)col0; + m_rigidCollisionObject = m_isSwapped? col0 : col1; + + //quick fix, add overlapping rigidbody to softbody, so it can be handled within btSoftBody::Step method + m_softBody->m_overlappingRigidBodies.push_back(m_rigidCollisionObject); + + ///store the contacts straight into the btSoftBody for now? + + /*if (!m_manifoldPtr && m_dispatcher->needsCollision(sphereObj,boxObj)) + { + m_manifoldPtr = m_dispatcher->getNewManifold(sphereObj,boxObj); + m_ownManifold = true; + } + */ + + + +} + + +btSoftRigidCollisionAlgorithm::~btSoftRigidCollisionAlgorithm() +{ + + m_softBody->m_overlappingRigidBodies.remove(m_rigidCollisionObject); + + /*if (m_ownManifold) + { + if (m_manifoldPtr) + m_dispatcher->releaseManifold(m_manifoldPtr); + } + */ + +} + + +#include + +void btSoftRigidCollisionAlgorithm::processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut) +{ + (void)dispatchInfo; + (void)resultOut; + //printf("btSoftRigidCollisionAlgorithm\n"); + + btSoftBody* softBody = m_isSwapped? (btSoftBody*)body1 : (btSoftBody*)body0; + btCollisionObject* rigidCollisionObject = m_isSwapped? body0 : body1; + ///do your stuff here + + +} + +btScalar btSoftRigidCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* col0,btCollisionObject* col1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut) +{ + (void)resultOut; + (void)dispatchInfo; + (void)col0; + (void)col1; + + //not yet + return btScalar(1.); +} + diff --git a/Demos/SoftDemo/btSoftRigidCollisionAlgorithm.h b/Demos/SoftDemo/btSoftRigidCollisionAlgorithm.h new file mode 100644 index 000000000..6800d9f4b --- /dev/null +++ b/Demos/SoftDemo/btSoftRigidCollisionAlgorithm.h @@ -0,0 +1,69 @@ +/* +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. +*/ + +#ifndef SOFT_RIGID_COLLISION_ALGORITHM_H +#define SOFT_RIGID_COLLISION_ALGORITHM_H + +#include "BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h" +#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" +#include "BulletCollision/CollisionDispatch/btCollisionCreateFunc.h" +class btPersistentManifold; +#include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h" + +#include "LinearMath/btVector3.h" +struct btSoftBody; + +/// btSoftRigidCollisionAlgorithm provides collision detection between btSoftBody and btRigidBody +class btSoftRigidCollisionAlgorithm : public btCollisionAlgorithm +{ +// bool m_ownManifold; +// btPersistentManifold* m_manifoldPtr; + + btSoftBody* m_softBody; + btCollisionObject* m_rigidCollisionObject; + + ///for rigid versus soft (instead of soft versus rigid), we use this swapped boolean + bool m_isSwapped; + +public: + + btSoftRigidCollisionAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* col0,btCollisionObject* col1, bool isSwapped); + + virtual ~btSoftRigidCollisionAlgorithm(); + + virtual void processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut); + + virtual btScalar calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut); + + + struct CreateFunc :public btCollisionAlgorithmCreateFunc + { + virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, btCollisionObject* body0,btCollisionObject* body1) + { + void* mem = ci.m_dispatcher1->allocateCollisionAlgorithm(sizeof(btSoftRigidCollisionAlgorithm)); + if (!m_swapped) + { + return new(mem) btSoftRigidCollisionAlgorithm(0,ci,body0,body1,false); + } else + { + return new(mem) btSoftRigidCollisionAlgorithm(0,ci,body0,body1,true); + } + } + }; + +}; + +#endif //SOFT_RIGID_COLLISION_ALGORITHM_H + diff --git a/Demos/SoftDemo/btSoftSoftCollisionAlgorithm.cpp b/Demos/SoftDemo/btSoftSoftCollisionAlgorithm.cpp new file mode 100644 index 000000000..e6fd73596 --- /dev/null +++ b/Demos/SoftDemo/btSoftSoftCollisionAlgorithm.cpp @@ -0,0 +1,104 @@ +/* +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. +*/ + +#include "btSoftSoftCollisionAlgorithm.h" +#include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h" +#include "BulletCollision/CollisionShapes/btBoxShape.h" +#include "BulletCollision/CollisionDispatch/btCollisionObject.h" +#include "BulletDynamics/SoftBody/btSoftBody.h" + +#define USE_PERSISTENT_CONTACTS 1 + +btSoftSoftCollisionAlgorithm::btSoftSoftCollisionAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* obj0,btCollisionObject* obj1) +: btCollisionAlgorithm(ci) +//m_ownManifold(false), +//m_manifoldPtr(mf) +{ + m_softBody0 = (btSoftBody*) obj0; + m_softBody1 = (btSoftBody*) obj1; + + m_softBody0->m_overlappingSoftBodies.push_back(m_softBody1); + m_softBody1->m_overlappingSoftBodies.push_back(m_softBody0); + + /*if (!m_manifoldPtr && m_dispatcher->needsCollision(obj0,obj1)) + { + m_manifoldPtr = m_dispatcher->getNewManifold(obj0,obj1); + m_ownManifold = true; + } + */ + +} + +btSoftSoftCollisionAlgorithm::~btSoftSoftCollisionAlgorithm() +{ + m_softBody0->m_overlappingSoftBodies.remove(m_softBody1); + m_softBody1->m_overlappingSoftBodies.remove(m_softBody0); + + //this gets called when the overlap stops. + + //here is where contacts (manifolds) should be removed + + /* + if (m_ownManifold) + { + if (m_manifoldPtr) + m_dispatcher->releaseManifold(m_manifoldPtr); + } + */ + +} + +void btSoftSoftCollisionAlgorithm::processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut) +{ + + btCollisionObject* col0 = body0; + btCollisionObject* col1 = body1; + + /* + btBoxShape* box0 = (btBoxShape*)col0->getCollisionShape(); + btBoxShape* box1 = (btBoxShape*)col1->getCollisionShape(); + + + + /// report a contact. internally this will be kept persistent, and contact reduction is done + resultOut->setPersistentManifold(m_manifoldPtr); +#ifndef USE_PERSISTENT_CONTACTS + m_manifoldPtr->clearManifold(); +#endif //USE_PERSISTENT_CONTACTS + + btDiscreteCollisionDetectorInterface::ClosestPointInput input; + input.m_maximumDistanceSquared = 1e30f; + input.m_transformA = body0->getWorldTransform(); + input.m_transformB = body1->getWorldTransform(); + + btBoxBoxDetector detector(box0,box1); + detector.getClosestPoints(input,*resultOut,dispatchInfo.m_debugDraw); + +#ifdef USE_PERSISTENT_CONTACTS + // refreshContactPoints is only necessary when using persistent contact points. otherwise all points are newly added + if (m_ownManifold) + { + resultOut->refreshContactPoints(); + } +#endif //USE_PERSISTENT_CONTACTS +*/ + +} + +btScalar btSoftSoftCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut) +{ + //not yet + return 1.f; +} diff --git a/Demos/SoftDemo/btSoftSoftCollisionAlgorithm.h b/Demos/SoftDemo/btSoftSoftCollisionAlgorithm.h new file mode 100644 index 000000000..bcca080df --- /dev/null +++ b/Demos/SoftDemo/btSoftSoftCollisionAlgorithm.h @@ -0,0 +1,62 @@ +/* +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. +*/ + +#ifndef SOFT_SOFT_COLLISION_ALGORITHM_H +#define SOFT_SOFT_COLLISION_ALGORITHM_H + +#include "BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h" +#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" +#include "BulletCollision/BroadphaseCollision/btDispatcher.h" +#include "BulletCollision/CollisionDispatch/btCollisionCreateFunc.h" + +class btPersistentManifold; +struct btSoftBody; + +///collision detection between two btSoftBody shapes +class btSoftSoftCollisionAlgorithm : public btCollisionAlgorithm +{ + bool m_ownManifold; + btPersistentManifold* m_manifoldPtr; + + btSoftBody* m_softBody0; + btSoftBody* m_softBody1; + + +public: + btSoftSoftCollisionAlgorithm(const btCollisionAlgorithmConstructionInfo& ci) + : btCollisionAlgorithm(ci) {} + + virtual void processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut); + + virtual btScalar calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut); + + btSoftSoftCollisionAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* body0,btCollisionObject* body1); + + virtual ~btSoftSoftCollisionAlgorithm(); + + struct CreateFunc :public btCollisionAlgorithmCreateFunc + { + virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, btCollisionObject* body0,btCollisionObject* body1) + { + int bbsize = sizeof(btSoftSoftCollisionAlgorithm); + void* ptr = ci.m_dispatcher1->allocateCollisionAlgorithm(bbsize); + return new(ptr) btSoftSoftCollisionAlgorithm(0,ci,body0,body1); + } + }; + +}; + +#endif //SOFT_SOFT_COLLISION_ALGORITHM_H + diff --git a/src/BulletCollision/BroadphaseCollision/btBroadphaseProxy.h b/src/BulletCollision/BroadphaseCollision/btBroadphaseProxy.h index 365a45612..bb96d07ba 100644 --- a/src/BulletCollision/BroadphaseCollision/btBroadphaseProxy.h +++ b/src/BulletCollision/BroadphaseCollision/btBroadphaseProxy.h @@ -59,6 +59,8 @@ CONCAVE_SHAPES_END_HERE, COMPOUND_SHAPE_PROXYTYPE, + SOFTBODY_SHAPE_PROXYTYPE, + MAX_BROADPHASE_COLLISION_TYPES }; diff --git a/src/BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.h b/src/BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.h index 56f52c33b..ebac071c8 100644 --- a/src/BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.h +++ b/src/BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.h @@ -81,7 +81,7 @@ public: } - btCollisionAlgorithmCreateFunc* getCollisionAlgorithmCreateFunc(int proxyType0,int proxyType1); + virtual btCollisionAlgorithmCreateFunc* getCollisionAlgorithmCreateFunc(int proxyType0,int proxyType1); }; diff --git a/src/BulletDynamics/SoftBody/btSoftBody.cpp b/src/BulletDynamics/SoftBody/btSoftBody.cpp index 670bef316..2ade5cd57 100644 --- a/src/BulletDynamics/SoftBody/btSoftBody.cpp +++ b/src/BulletDynamics/SoftBody/btSoftBody.cpp @@ -17,627 +17,717 @@ subject to the following restrictions: #include "btSoftBody.h" #include #include +#include "BulletCollision/NarrowPhaseCollision/btGjkEpa2.h" +#include "BulletCollision/BroadphaseCollision/btBroadphaseInterface.h" +#include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h" + + +btSoftBody::btSoftBody(btSoftBody::btSoftBodyWorldInfo& worldInfo,int node_count, const btVector3* x, const btScalar* m) +:m_worldInfo(worldInfo) +{ + ///for now, create a collision shape internally + m_softBodyCollisionShape = new btSoftBodyCollisionShape(); + + setCollisionShape(m_softBodyCollisionShape); + + /* Init */ + m_cfg.aeromodel = eAeroModel::V_Point; + 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.kAHR = (btScalar)0.5; + m_cfg.timescale = 1; + m_cfg.timestep = (btScalar)(1/60.); + m_cfg.maxsteps = 60; + m_cfg.iterations = 1; + m_cfg.becollide = true; + m_cfg.bscollide = false; + m_pose.m_bvolume = false; + m_pose.m_bframe = false; + m_pose.m_volume = 0; + m_pose.m_com = btVector3(0,0,0); + m_pose.m_trs.setIdentity(); + m_tag = 0; + m_timeacc = 0; + m_bUpdateRtCst = true; + m_bounds[0] = btVector3(0,0,0); + m_bounds[1] = btVector3(0,0,0); + /* Nodes */ + getNodes().resize(node_count); + for(int i=0,ni=node_count;i0?1/n.m_im:0; + } + updateTransform(); + ///register to the broadphase + setBroadphaseHandle( m_worldInfo.m_broadphase->createProxy(m_bounds[0],m_bounds[1],SOFTBODY_SHAPE_PROXYTYPE,this,1,1,m_worldInfo.m_dispatcher,0)); + + +} + +///destructor +btSoftBody::~btSoftBody() +{ + //remove from broadphase + m_worldInfo.m_broadphase->destroyProxy(getBroadphaseHandle(),m_worldInfo.m_dispatcher); + + ///for now, delete the internal shape + delete m_softBodyCollisionShape; +} + +btVector3 btSoftBody::btSoftBodyCollisionShape::m_sScaling(0,0,0); + +btSoftBody::btSoftBodyCollisionShape::btSoftBodyCollisionShape() +{ +} + +btSoftBody::btSoftBodyCollisionShape::~btSoftBodyCollisionShape() +{ + +} + +void btSoftBody::btSoftBodyCollisionShape::processAllTriangles(btTriangleCallback* callback,const btVector3& aabbMin,const btVector3& aabbMax) const +{ + //not yet + btAssert(0); +} + + + +// +void btSoftBody::updateBounds() +{ + if(getNodes().size()>0) + { + m_bounds[0]= + m_bounds[1]=getNodes()[0].m_x; + for(int i=1,ni=getNodes().size();isetAabb(getBroadphaseHandle(),m_bounds[0],m_bounds[1],m_worldInfo.m_dispatcher); + } + + } + else + { + m_bounds[0]= + m_bounds[1]=btVector3(0,0,0); + } +} namespace btsoftbody_internals { -// -// Helpers -// + // + // Helpers + // -// -static inline void Trace(const btMatrix3x3& m,const char* name) -{ -printf("%s[0]: %.2f,\t%.2f,\t%.2f\r\n",name,m[0].x(),m[0].y(),m[0].z()); -printf("%s[1]: %.2f,\t%.2f,\t%.2f\r\n",name,m[1].x(),m[1].y(),m[1].z()); -printf("%s[2]: %.2f,\t%.2f,\t%.2f\r\n",name,m[2].x(),m[2].y(),m[2].z()); -} - -// -template -static inline T Lerp(const T& a,const T& b,btScalar t) -{ return(a+(b-a)*t); } -// -template -static inline T Clamp(const T& x,const T& l,const T& h) -{ return(xh?h:x); } -// -template -static inline T Sq(const T& x) -{ return(x*x); } -// -template -static inline T Sign(const T& x) -{ return((T)(x<0?-1:+1)); } -// -static inline btMatrix3x3 ScaleAlongAxis(const btVector3& a,btScalar s) -{ -const btScalar xx=a.x()*a.x(); -const btScalar yy=a.y()*a.y(); -const btScalar zz=a.z()*a.z(); -const btScalar xy=a.x()*a.y(); -const btScalar yz=a.y()*a.z(); -const btScalar zx=a.z()*a.x(); -btMatrix3x3 m; -m[0]=btVector3(1-xx+xx*s,xy*s-xy,zx*s-zx); -m[1]=btVector3(xy*s-xy,1-yy+yy*s,yz*s-yz); -m[2]=btVector3(zx*s-zx,yz*s-yz,1-zz+zz*s); -return(m); -} -// -static inline btMatrix3x3 Cross(const btVector3& v) -{ -btMatrix3x3 m; -m[0]=btVector3(0,-v.z(),+v.y()); -m[1]=btVector3(+v.z(),0,-v.x()); -m[2]=btVector3(-v.y(),+v.x(),0); -return(m); -} -// -static inline btMatrix3x3 Diagonal(btScalar x) -{ -btMatrix3x3 m; -m[0]=btVector3(x,0,0); -m[1]=btVector3(0,x,0); -m[2]=btVector3(0,0,x); -return(m); -} -// -static inline btMatrix3x3 Add(const btMatrix3x3& a, - const btMatrix3x3& b) -{ -btMatrix3x3 r; -for(int i=0;i<3;++i) r[i]=a[i]+b[i]; -return(r); -} -// -static inline btMatrix3x3 Sub(const btMatrix3x3& a, - const btMatrix3x3& b) -{ -btMatrix3x3 r; -for(int i=0;i<3;++i) r[i]=a[i]-b[i]; -return(r); -} -// -static inline btMatrix3x3 Mul(const btMatrix3x3& a, - btScalar b) -{ -btMatrix3x3 r; -for(int i=0;i<3;++i) r[i]=a[i]*b; -return(r); -} -// -static inline btMatrix3x3 MassMatrix(btScalar im,const btMatrix3x3& iwi,const btVector3& r) -{ -const btMatrix3x3 cr=Cross(r); -return(Sub(Diagonal(im),cr*iwi*cr)); -} -// -static inline btMatrix3x3 ImpulseMatrix( btScalar dt, - btScalar ima, - btScalar imb, - const btMatrix3x3& iwi, - const btVector3& r) -{ -return( Diagonal(1/dt)* - Add(Diagonal(ima),MassMatrix(imb,iwi,r)).inverse()); -} -// -static inline void PolarDecompose( const btMatrix3x3& m, - btMatrix3x3& q, - btMatrix3x3& s) -{ -static const btScalar half=(btScalar)0.5; -static const btScalar accuracy=(btScalar)0.00001; -static const int maxiterations=64; -btScalar det=m.determinant(); -if(!btFuzzyZero(det)) + // + static inline void Trace(const btMatrix3x3& m,const char* name) { - q=m; - for(int i=0;i + static inline T Lerp(const T& a,const T& b,btScalar t) + { return(a+(b-a)*t); } + // + template + static inline T Clamp(const T& x,const T& l,const T& h) + { return(xh?h:x); } + // + template + static inline T Sq(const T& x) + { return(x*x); } + // + template + static inline T Sign(const T& x) + { return((T)(x<0?-1:+1)); } + // + static inline btMatrix3x3 ScaleAlongAxis(const btVector3& a,btScalar s) + { + const btScalar xx=a.x()*a.x(); + const btScalar yy=a.y()*a.y(); + const btScalar zz=a.z()*a.z(); + const btScalar xy=a.x()*a.y(); + const btScalar yz=a.y()*a.z(); + const btScalar zx=a.z()*a.x(); + btMatrix3x3 m; + m[0]=btVector3(1-xx+xx*s,xy*s-xy,zx*s-zx); + m[1]=btVector3(xy*s-xy,1-yy+yy*s,yz*s-yz); + m[2]=btVector3(zx*s-zx,yz*s-yz,1-zz+zz*s); + return(m); + } + // + static inline btMatrix3x3 Cross(const btVector3& v) + { + btMatrix3x3 m; + m[0]=btVector3(0,-v.z(),+v.y()); + m[1]=btVector3(+v.z(),0,-v.x()); + m[2]=btVector3(-v.y(),+v.x(),0); + return(m); + } + // + static inline btMatrix3x3 Diagonal(btScalar x) + { + btMatrix3x3 m; + m[0]=btVector3(x,0,0); + m[1]=btVector3(0,x,0); + m[2]=btVector3(0,0,x); + return(m); + } + // + static inline btMatrix3x3 Add(const btMatrix3x3& a, + const btMatrix3x3& b) + { + btMatrix3x3 r; + for(int i=0;i<3;++i) r[i]=a[i]+b[i]; + return(r); + } + // + static inline btMatrix3x3 Sub(const btMatrix3x3& a, + const btMatrix3x3& b) + { + btMatrix3x3 r; + for(int i=0;i<3;++i) r[i]=a[i]-b[i]; + return(r); + } + // + static inline btMatrix3x3 Mul(const btMatrix3x3& a, + btScalar b) + { + btMatrix3x3 r; + for(int i=0;i<3;++i) r[i]=a[i]*b; + return(r); + } + // + static inline btMatrix3x3 MassMatrix(btScalar im,const btMatrix3x3& iwi,const btVector3& r) + { + const btMatrix3x3 cr=Cross(r); + return(Sub(Diagonal(im),cr*iwi*cr)); + } + // + static inline btMatrix3x3 ImpulseMatrix( btScalar dt, + btScalar ima, + btScalar imb, + const btMatrix3x3& iwi, + const btVector3& r) + { + return( Diagonal(1/dt)* + Add(Diagonal(ima),MassMatrix(imb,iwi,r)).inverse()); + } + // + static inline void PolarDecompose( const btMatrix3x3& m, + btMatrix3x3& q, + btMatrix3x3& s) + { + static const btScalar half=(btScalar)0.5; + static const btScalar accuracy=(btScalar)0.00001; + static const int maxiterations=64; + btScalar det=m.determinant(); + if(!btFuzzyZero(det)) { - q=Mul(Add(q,Mul(q.adjoint(),1/det).transpose()),half); - const btScalar ndet=q.determinant(); - if(Sq(ndet-det)>accuracy) det=ndet; else break; - } - s=q.transpose()*m; - } - else - { - q.setIdentity(); - s.setIdentity(); - } -} -// -static inline btVector3 ProjectOnAxis( const btVector3& v, - const btVector3& a) -{ -return(a*dot(v,a)); -} -// -static inline btVector3 ProjectOnPlane( const btVector3& v, - const btVector3& a) -{ -return(v-ProjectOnAxis(v,a)); -} -// -template -static inline T BaryEval( const btVector3& coord, - const T& a, - const T& b, - const T& c) -{ -return(a*coord.x()+b*coord.y()+c*coord.z()); -} - -// -static inline btVector3 NormalizeAny(const btVector3& v) -{ -const btScalar l=v.length(); -if(l>SIMD_EPSILON) - return(v/l); - else - return(btVector3(0,0,0)); -} - -// -static void PointersToIndices(btSoftBody* psb) -{ -#define PTR2IDX(_p_,_b_) reinterpret_cast((_p_)-(_b_)) -btSoftBody::Node* base=&psb->m_nodes[0]; -for(int i=0,ni=psb->m_links.size();im_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); - } -#undef PTR2IDX -} - -// -static void IndicesToPointers(btSoftBody* psb) -{ -#define IDX2PTR(_p_,_b_) ((_b_)+(((char*)_p_)-(char*)0)) -btSoftBody::Node* base=&psb->m_nodes[0]; -for(int i=0,ni=psb->m_links.size();im_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); - } -#undef IDX2PTR -} - -// -static inline btScalar AreaOf( const btVector3& x0, - const btVector3& x1, - const btVector3& x2) -{ -const btVector3 a=x1-x0; -const btVector3 b=x2-x0; -const btVector3 cr=cross(a,b); -const btScalar area=cr.length(); -return(area); -} - -// -static inline btScalar VolumeOf( const btVector3& x0, - const btVector3& x1, - const btVector3& x2, - const btVector3& x3) -{ -const btVector3 a=x1-x0; -const btVector3 b=x2-x0; -const btVector3 c=x3-x0; -return(dot(a,cross(b,c))); -} - -// -static inline btScalar RayTriangle(const btVector3& org, - 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; -const btVector3 n=cross(b-a,c-a).normalized(); -const btScalar d=dot(a,n); -const btScalar den=dot(dir,n); -if(!btFuzzyZero(den)) - { - const btScalar num=dot(org,n)-d; - const btScalar t=-num/den; - if((t>teps)&&(tceps) && - (dot(n,cross(b-hit,c-hit))>ceps) && - (dot(n,cross(c-hit,a-hit))>ceps)) + q=m; + for(int i=0;iaccuracy) det=ndet; else break; + } + s=q.transpose()*m; + } + else + { + q.setIdentity(); + s.setIdentity(); + } + } + // + static inline btVector3 ProjectOnAxis( const btVector3& v, + const btVector3& a) + { + return(a*dot(v,a)); + } + // + static inline btVector3 ProjectOnPlane( const btVector3& v, + const btVector3& a) + { + return(v-ProjectOnAxis(v,a)); + } + // + template + static inline T BaryEval( const btVector3& coord, + const T& a, + const T& b, + const T& c) + { + return(a*coord.x()+b*coord.y()+c*coord.z()); + } + + // + static inline btVector3 NormalizeAny(const btVector3& v) + { + const btScalar l=v.length(); + if(l>SIMD_EPSILON) + return(v/l); + else + return(btVector3(0,0,0)); + } + + // + static void PointersToIndices(btSoftBody* psb) + { +#define PTR2IDX(_p_,_b_) reinterpret_cast((_p_)-(_b_)) + btSoftBody::Node* base=&psb->getNodes()[0]; + for(int i=0,ni=psb->getLinks().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); + } + for(int i=0,ni=psb->getFaces().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); + } +#undef PTR2IDX + } + + // + static void IndicesToPointers(btSoftBody* psb) + { +#define IDX2PTR(_p_,_b_) ((_b_)+(((char*)_p_)-(char*)0)) + btSoftBody::Node* base=&psb->getNodes()[0]; + for(int i=0,ni=psb->getLinks().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); + } + for(int i=0,ni=psb->getFaces().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); + } +#undef IDX2PTR + } + + // + static inline btScalar AreaOf( const btVector3& x0, + const btVector3& x1, + const btVector3& x2) + { + const btVector3 a=x1-x0; + const btVector3 b=x2-x0; + const btVector3 cr=cross(a,b); + const btScalar area=cr.length(); + return(area); + } + + // + static inline btScalar VolumeOf( const btVector3& x0, + const btVector3& x1, + const btVector3& x2, + const btVector3& x3) + { + const btVector3 a=x1-x0; + const btVector3 b=x2-x0; + const btVector3 c=x3-x0; + return(dot(a,cross(b,c))); + } + + // + static inline btScalar RayTriangle(const btVector3& org, + 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; + const btVector3 n=cross(b-a,c-a).normalized(); + const btScalar d=dot(a,n); + const btScalar den=dot(dir,n); + if(!btFuzzyZero(den)) + { + const btScalar num=dot(org,n)-d; + const btScalar t=-num/den; + if((t>teps)&&(tceps) && + (dot(n,cross(b-hit,c-hit))>ceps) && + (dot(n,cross(c-hit,a-hit))>ceps)) + { + return(t); + } } } + return(-1); } -return(-1); -} - -// -// Private implementation -// -// -static int RaycastInternal(const btSoftBody* psb, - const btVector3& org, - const btVector3& dir, - btScalar& mint, - bool bcountonly) -{ -int cnt=0; -mint=SIMD_INFINITY; -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) + int cnt=0; + mint=SIMD_INFINITY; + for(int i=0,ni=psb->getFaces().size();im_pose.m_bframe) - { - for(int i=0,ni=psb->m_nodes.size();im_nodes[i].m_x*psb->m_pose.m_wgh[i]; - } - } -return(com); -} - -// -static void UpdateNormals(btSoftBody* psb) -{ -const btVector3 zv(0,0,0); -for(int i=0,ni=psb->m_nodes.size();im_nodes[i].m_n=zv; - } -for(int i=0,ni=psb->m_faces.size();im_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(); - f.m_n[0]->m_n+=n; - f.m_n[1]->m_n+=n; - f.m_n[2]->m_n+=n; - } -for(int i=0,ni=psb->m_nodes.size();im_nodes[i].m_n.normalize(); - } -} - -// -static void UpdateBounds(btSoftBody* psb) -{ -if(psb->m_nodes.size()>0) - { - psb->m_bounds[0]= - psb->m_bounds[1]=psb->m_nodes[0].m_x; - for(int i=1,ni=psb->m_nodes.size();im_nodes[i]; - psb->m_bounds[0].setMin(n.m_x); - psb->m_bounds[1].setMax(n.m_x); - psb->m_bounds[0].setMin(n.m_q); - psb->m_bounds[1].setMax(n.m_q); - } - } - else - { - psb->m_bounds[0]= - psb->m_bounds[1]=btVector3(0,0,0); - } -} - -// -static void UpdateTransform(btSoftBody* psb) -{ -UpdateBounds(psb); -} - -// -static void UpdatePose(btSoftBody* psb) -{ -if(psb->m_pose.m_bframe) - { - btSoftBody::Pose& pose=psb->m_pose; - const btVector3 com=EvaluateCom(psb); - /* Com */ - pose.m_com = com; - /* Rotation */ - btMatrix3x3 Apq; - 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->m_nodes.size();im_nodes[i].m_x-com); - const btVector3& b=pose.m_pos[i]; - Apq[0]+=a.x()*b; - Apq[1]+=a.y()*b; - Apq[2]+=a.z()*b; - } - btMatrix3x3 r,s; - PolarDecompose(Apq,r,s); - psb->m_pose.m_trs=r; - } -} - -// -static void UpdateConstants(btSoftBody* psb) -{ -/* Links */ -for(int i=0,ni=psb->m_links.size();im_links[i]; - 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_c1 = l.m_rl*l.m_rl; - } -/* Faces */ -for(int i=0,ni=psb->m_faces.size();im_faces[i]; - f.m_ra = AreaOf(f.m_n[0]->m_x,f.m_n[1]->m_x,f.m_n[2]->m_x); - } -/* Area's */ -btAlignedObjectArray counts; -counts.resize(psb->m_nodes.size(),0); -for(int i=0,ni=psb->m_nodes.size();im_nodes[i].m_area = 0; - } -for(int i=0,ni=psb->m_faces.size();im_faces[i]; - for(int j=0;j<3;++j) - { - 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->m_nodes.size();i0) - psb->m_nodes[i].m_area/=(btScalar)counts[i]; - else - psb->m_nodes[i].m_area=0; - } -} - -// -static inline void ApplyClampedForce( btSoftBody::Node& n, - const btVector3& f, - btScalar dt) -{ -const btScalar dtim=dt*n.m_im; -if((f*dtim).length2()>n.m_v.length2()) - {/* Clamp */ - n.m_f-=ProjectOnAxis(n.m_v,f.normalized())/dtim; - } - else - {/* Apply */ - n.m_f+=f; - } -} - -// -static void ApplyForces(btSoftBody* psb,btScalar dt) -{ -const btScalar kLF=psb->m_cfg.kLF; -const btScalar kDG=psb->m_cfg.kDG; -const btScalar kPR=psb->m_cfg.kPR; -const btScalar kVC=psb->m_cfg.kVC; -const bool as_lift=kLF>0; -const bool as_drag=kDG>0; -const bool as_pressure=kPR!=0; -const bool as_volume=kVC>0; -const bool as_aero= as_lift || - as_drag ; -const bool as_vaero= as_aero && - (psb->m_cfg.aeromodel< - btSoftBody::eAeroModel::F_TwoSided); -const bool as_faero= as_aero && - (psb->m_cfg.aeromodel>= - btSoftBody::eAeroModel::F_TwoSided); -const bool use_medium= as_aero; -const bool use_volume= as_pressure || - as_volume ; -btScalar volume=0; -btScalar ivolumetp=0; -btScalar dvolumetv=0; -btSoftBody::ISoftBody::sMedium medium; -if(use_volume) - { - volume = psb->GetVolume(); - ivolumetp = 1/btFabs(volume)*kPR; - dvolumetv = (psb->m_pose.m_volume-volume)*kVC; - } -/* Per vertex forces */ -for(int i=0,ni=psb->m_nodes.size();im_nodes[i]; - if(n.m_im>0) - { - if(use_medium) + const btSoftBody::Face& f=psb->getFaces()[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) { - psb->m_isb->EvaluateMedium(n.m_x,medium); - /* Aerodynamics */ - if(as_vaero) - { - const btVector3 rel_v=n.m_v-medium.m_velocity; - const btScalar rel_v2=rel_v.length2(); - if(rel_v2>SIMD_EPSILON) - { - btVector3 nrm=n.m_n; - /* Setup normal */ - switch(psb->m_cfg.aeromodel) + ++cnt;if(!bcountonly) mint=t; + } + } + return(cnt); + } + + + // + static btVector3 EvaluateCom(btSoftBody* psb) + { + btVector3 com(0,0,0); + if(psb->m_pose.m_bframe) + { + for(int i=0,ni=psb->getNodes().size();igetNodes()[i].m_x*psb->m_pose.m_wgh[i]; + } + } + return(com); + } + + // + static void UpdateNormals(btSoftBody* psb) + { + const btVector3 zv(0,0,0); + for(int i=0,ni=psb->getNodes().size();igetNodes()[i].m_n=zv; + } + for(int i=0,ni=psb->getFaces().size();igetFaces()[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(); + f.m_n[0]->m_n+=n; + f.m_n[1]->m_n+=n; + f.m_n[2]->m_n+=n; + } + for(int i=0,ni=psb->getNodes().size();igetNodes()[i].m_n.normalize(); + } + } + + + + // + static void UpdatePose(btSoftBody* psb) + { + if(psb->m_pose.m_bframe) + { + btSoftBody::Pose& pose=psb->m_pose; + const btVector3 com=EvaluateCom(psb); + /* Com */ + pose.m_com = com; + /* Rotation */ + btMatrix3x3 Apq; + const btScalar eps=1/(btScalar)(100*psb->getNodes().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();igetNodes()[i].m_x-com); + const btVector3& b=pose.m_pos[i]; + Apq[0]+=a.x()*b; + Apq[1]+=a.y()*b; + Apq[2]+=a.z()*b; + } + btMatrix3x3 r,s; + PolarDecompose(Apq,r,s); + psb->m_pose.m_trs=r; + } + } + + // + static void UpdateConstants(btSoftBody* psb) + { + /* Links */ + for(int i=0,ni=psb->getLinks().size();igetLinks()[i]; + 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_c1 = l.m_rl*l.m_rl; + } + /* Faces */ + for(int i=0,ni=psb->getFaces().size();igetFaces()[i]; + f.m_ra = AreaOf(f.m_n[0]->m_x,f.m_n[1]->m_x,f.m_n[2]->m_x); + } + /* Area's */ + btAlignedObjectArray counts; + counts.resize(psb->getNodes().size(),0); + for(int i=0,ni=psb->getNodes().size();igetNodes()[i].m_area = 0; + } + for(int i=0,ni=psb->getFaces().size();igetFaces()[i]; + for(int j=0;j<3;++j) + { + const int index=(int)(f.m_n[j]-&psb->getNodes()[0]); + counts[index]++; + f.m_n[j]->m_area+=btFabs(f.m_ra); + } + } + for(int i=0,ni=psb->getNodes().size();i0) + psb->getNodes()[i].m_area/=(btScalar)counts[i]; + else + psb->getNodes()[i].m_area=0; + } + } + + // + static inline void ApplyClampedForce( btSoftBody::Node& n, + const btVector3& f, + btScalar dt) + { + const btScalar dtim=dt*n.m_im; + if((f*dtim).length2()>n.m_v.length2()) + {/* Clamp */ + n.m_f-=ProjectOnAxis(n.m_v,f.normalized())/dtim; + } + else + {/* Apply */ + n.m_f+=f; + } + } + + // + static void ApplyForces(btSoftBody* psb,btScalar dt) + { + const btScalar kLF=psb->m_cfg.kLF; + const btScalar kDG=psb->m_cfg.kDG; + const btScalar kPR=psb->m_cfg.kPR; + const btScalar kVC=psb->m_cfg.kVC; + const bool as_lift=kLF>0; + const bool as_drag=kDG>0; + const bool as_pressure=kPR!=0; + const bool as_volume=kVC>0; + const bool as_aero= as_lift || + as_drag ; + const bool as_vaero= as_aero && + (psb->m_cfg.aeromodel< + btSoftBody::eAeroModel::F_TwoSided); + const bool as_faero= as_aero && + (psb->m_cfg.aeromodel>= + btSoftBody::eAeroModel::F_TwoSided); + const bool use_medium= as_aero; + const bool use_volume= as_pressure || + as_volume ; + btScalar volume=0; + btScalar ivolumetp=0; + btScalar dvolumetv=0; + btSoftBody::sMedium medium; + if(use_volume) + { + volume = psb->GetVolume(); + ivolumetp = 1/btFabs(volume)*kPR; + dvolumetv = (psb->m_pose.m_volume-volume)*kVC; + } + /* Per vertex forces */ + for(int i=0,ni=psb->getNodes().size();igetNodes()[i]; + if(n.m_im>0) + { + if(use_medium) + { + psb->EvaluateMedium(n.m_x,medium); + /* Aerodynamics */ + if(as_vaero) + { + const btVector3 rel_v=n.m_v-medium.m_velocity; + const btScalar rel_v2=rel_v.length2(); + if(rel_v2>SIMD_EPSILON) { - case btSoftBody::eAeroModel::V_Point: - nrm=NormalizeAny(rel_v);break; - case btSoftBody::eAeroModel::V_TwoSided: - nrm*=(btScalar)(dot(nrm,rel_v)<0?-1:+1);break; - } - const btScalar dvn=dot(rel_v,nrm); - /* Compute forces */ - if(dvn>0) - { - btVector3 force(0,0,0); - const btScalar c0 = n.m_area*dvn*rel_v2/2; - const btScalar c1 = c0*medium.m_density; - force += nrm*(-c1*kLF); - force += rel_v.normalized()*(-c1*kDG); - ApplyClampedForce(n,force,dt); + btVector3 nrm=n.m_n; + /* Setup normal */ + switch(psb->m_cfg.aeromodel) + { + case btSoftBody::eAeroModel::V_Point: + nrm=NormalizeAny(rel_v);break; + case btSoftBody::eAeroModel::V_TwoSided: + nrm*=(btScalar)(dot(nrm,rel_v)<0?-1:+1);break; + } + const btScalar dvn=dot(rel_v,nrm); + /* Compute forces */ + if(dvn>0) + { + btVector3 force(0,0,0); + const btScalar c0 = n.m_area*dvn*rel_v2/2; + const btScalar c1 = c0*medium.m_density; + force += nrm*(-c1*kLF); + force += rel_v.normalized()*(-c1*kDG); + ApplyClampedForce(n,force,dt); + } } } } - } - /* Pressure */ - if(as_pressure) - { - n.m_f += n.m_n*(n.m_area*ivolumetp); - } - /* Volume */ - if(as_volume) - { - n.m_f += n.m_n*(n.m_area*dvolumetv); + /* Pressure */ + if(as_pressure) + { + n.m_f += n.m_n*(n.m_area*ivolumetp); + } + /* Volume */ + if(as_volume) + { + n.m_f += n.m_n*(n.m_area*dvolumetv); + } } } - } -/* Per face forces */ -for(int i=0,ni=psb->m_faces.size();im_faces[i]; - if(as_faero) + /* Per face forces */ + for(int i=0,ni=psb->getFaces().size();im_v+f.m_n[1]->m_v+f.m_n[2]->m_v)/3; - const btVector3 x=(f.m_n[0]->m_x+f.m_n[1]->m_x+f.m_n[2]->m_x)/3; - psb->m_isb->EvaluateMedium(x,medium); - const btVector3 rel_v=v-medium.m_velocity; - const btScalar rel_v2=rel_v.length2(); - if(rel_v2>SIMD_EPSILON) + btSoftBody::Face& f=psb->getFaces()[i]; + if(as_faero) { - btVector3 nrm=f.m_normal; - /* Setup normal */ - switch(psb->m_cfg.aeromodel) + const btVector3 v=(f.m_n[0]->m_v+f.m_n[1]->m_v+f.m_n[2]->m_v)/3; + const btVector3 x=(f.m_n[0]->m_x+f.m_n[1]->m_x+f.m_n[2]->m_x)/3; + psb->EvaluateMedium(x,medium); + const btVector3 rel_v=v-medium.m_velocity; + const btScalar rel_v2=rel_v.length2(); + if(rel_v2>SIMD_EPSILON) { - case btSoftBody::eAeroModel::F_TwoSided: - nrm*=(btScalar)(dot(nrm,rel_v)<0?-1:+1);break; - } - const btScalar dvn=dot(rel_v,nrm); - /* Compute forces */ - if(dvn>0) - { - btVector3 force(0,0,0); - const btScalar c0 = f.m_ra*dvn*rel_v2; - const btScalar c1 = c0*medium.m_density; - force += nrm*(-c1*kLF); - force += rel_v.normalized()*(-c1*kDG); - force /= 3; - for(int j=0;j<3;++j) ApplyClampedForce(*f.m_n[j],force,dt); + btVector3 nrm=f.m_normal; + /* Setup normal */ + switch(psb->m_cfg.aeromodel) + { + case btSoftBody::eAeroModel::F_TwoSided: + nrm*=(btScalar)(dot(nrm,rel_v)<0?-1:+1);break; + } + const btScalar dvn=dot(rel_v,nrm); + /* Compute forces */ + if(dvn>0) + { + btVector3 force(0,0,0); + const btScalar c0 = f.m_ra*dvn*rel_v2; + const btScalar c1 = c0*medium.m_density; + force += nrm*(-c1*kLF); + force += rel_v.normalized()*(-c1*kDG); + force /= 3; + for(int j=0;j<3;++j) ApplyClampedForce(*f.m_n[j],force,dt); + } } } } } -} -// -static void PSolve_Anchors(btSoftBody* psb,btScalar sdt) -{ -const btScalar kAHR=psb->m_cfg.kAHR; -for(int i=0,ni=psb->m_anchors.size();im_anchors[i]; - const btTransform& t=a.m_body->getWorldTransform(); - 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 vb=n.m_x-n.m_q; - const btVector3 vr=(va-vb)+(wa-n.m_x)*kAHR; - const btVector3 impulse=a.m_c0*vr; - n.m_x+=impulse*a.m_c2; - a.m_body->applyImpulse(-impulse,a.m_c1); - } -} - -// -static void PSolve_Contacts(btSoftBody* psb,btScalar sdt) -{ -const btScalar kCHR=psb->m_cfg.kCHR; -for(int i=0,ni=psb->m_contacts.size();im_contacts[i]; - const btSoftBody::ISoftBody::sCti& cti=c.m_cti; - const btVector3 va=cti.m_body->getVelocityInLocalPoint(c.m_c1)*sdt; - 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); - if(dn<=SIMD_EPSILON) + const btScalar kAHR=psb->m_cfg.kAHR; + for(int i=0,ni=psb->m_anchors.size();im_x,cti.m_normal)+cti.m_offset; - const btVector3 fv=vr-cti.m_normal*dn; - const btVector3 impulse=c.m_c0*(vr-fv*c.m_c3+cti.m_normal*(dp*kCHR)); - c.m_node->m_x-=impulse*c.m_c2; - c.m_cti.m_body->applyImpulse(impulse,c.m_c1); + const btSoftBody::Anchor& a=psb->m_anchors[i]; + const btTransform& t=a.m_body->getWorldTransform(); + 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 vb=n.m_x-n.m_q; + const btVector3 vr=(va-vb)+(wa-n.m_x)*kAHR; + const btVector3 impulse=a.m_c0*vr; + n.m_x+=impulse*a.m_c2; + a.m_body->applyImpulse(-impulse,a.m_c1); } } -} -// -static void PSolve_Links(btSoftBody* psb,btScalar w) -{ -for(int i=0,ni=psb->m_links.size();im_links[i]; - if(l.m_c0>0) + // + static void PSolve_Contacts(btSoftBody* psb,btScalar sdt) + { + const btScalar kCHR=psb->m_cfg.kCHR; + for(int i=0,ni=psb->m_contacts.size();im_contacts[i]; + const btSoftBody::sCti& cti=c.m_cti; + const btVector3 va=cti.m_body->getVelocityInLocalPoint(c.m_c1)*sdt; + 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); + if(dn<=SIMD_EPSILON) + { + const btScalar dp=dot(c.m_node->m_x,cti.m_normal)+cti.m_offset; + const btVector3 fv=vr-cti.m_normal*dn; + const btVector3 impulse=c.m_c0*(vr-fv*c.m_c3+cti.m_normal*(dp*kCHR)); + c.m_node->m_x-=impulse*c.m_c2; + c.m_cti.m_body->applyImpulse(impulse,c.m_c1); + } + } + } + + // + static void PSolve_Links(btSoftBody* psb,btScalar w) + { + for(int i=0,ni=psb->getLinks().size();igetLinks()[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; + a.m_x-=del*(k*a.m_im); + b.m_x+=del*(k*b.m_im); + } } } -} } @@ -647,417 +737,364 @@ using namespace btsoftbody_internals; // Api // -// -btSoftBody* btSoftBody::Create( ISoftBody* isoftbody, - int node_count, - const btVector3* x, - const btScalar* m) -{ -btSoftBody* sb=new btSoftBody(); -/* Init */ -sb->m_cfg.aeromodel = eAeroModel::V_Point; -sb->m_cfg.kDG = 0; -sb->m_cfg.kLF = 0; -sb->m_cfg.kDP = 0; -sb->m_cfg.kPR = 0; -sb->m_cfg.kVC = 0; -sb->m_cfg.kDF = (btScalar)0.2; -sb->m_cfg.kLST = 1; -sb->m_cfg.kMT = 0; -sb->m_cfg.kSOR = 1; -sb->m_cfg.kCHR = (btScalar)1.0; -sb->m_cfg.kAHR = (btScalar)0.5; -sb->m_cfg.timescale = 1; -sb->m_cfg.timestep = (btScalar)(1/60.); -sb->m_cfg.maxsteps = 60; -sb->m_cfg.iterations = 1; -sb->m_cfg.becollide = true; -sb->m_cfg.bscollide = false; -sb->m_pose.m_bvolume = false; -sb->m_pose.m_bframe = false; -sb->m_pose.m_volume = 0; -sb->m_pose.m_com = btVector3(0,0,0); -sb->m_pose.m_trs.setIdentity(); -sb->m_isb = isoftbody?isoftbody:new ISoftBody(); -sb->m_tag = 0; -sb->m_timeacc = 0; -sb->m_bUpdateRtCst = true; -sb->m_bounds[0] = btVector3(0,0,0); -sb->m_bounds[1] = btVector3(0,0,0); -/* Nodes */ -sb->m_nodes.resize(node_count); -for(int i=0,ni=node_count;im_nodes[i]; - memset(&n,0,sizeof(n)); - n.m_x = x?*x++:btVector3(0,0,0); - n.m_q = n.m_x; - n.m_im = m?*m++:1; - n.m_im = n.m_im>0?1/n.m_im:0; - } -UpdateTransform(sb); -sb->m_isb->Attach(sb); -return(sb); -} // void btSoftBody::Delete() { -m_isb->Detach(this); -delete this; + delete this; } // bool btSoftBody::CheckLink(int node0,int node1) const { -return(CheckLink(&m_nodes[node0],&m_nodes[node1])); + return(CheckLink(&getNodes()[node0],&getNodes()[node1])); } // bool btSoftBody::CheckLink(const Node* node0,const Node* node1) const { -const Node* n[]={node0,node1}; -for(int i=0,ni=m_links.size();im_x-l.m_n[1]->m_x).length(); - l.m_c0 = 0; - l.m_c1 = 0; - l.m_type = type; - l.m_tag = 0; - m_links.push_back(l); - m_bUpdateRtCst=true; + 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); + m_bUpdateRtCst=true; } } // void btSoftBody::AppendFace(int node0,int node1,int node2) { -Face f; -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; -m_faces.push_back(f); -m_bUpdateRtCst=true; + Face f; + f.m_n[0] = &getNodes()[node0]; + f.m_n[1] = &getNodes()[node1]; + f.m_n[2] = &getNodes()[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); + m_bUpdateRtCst=true; } // void btSoftBody::AppendAnchor(int node,btRigidBody* body) { -Anchor a; -a.m_node = &m_nodes[node]; -a.m_body = body; -a.m_local = body->getWorldTransform().inverse()*a.m_node->m_x; -a.m_node->m_battach = 1; -m_anchors.push_back(a); + Anchor a; + a.m_node = &getNodes()[node]; + a.m_body = body; + a.m_local = body->getWorldTransform().inverse()*a.m_node->m_x; + a.m_node->m_battach = 1; + m_anchors.push_back(a); } // void btSoftBody::AddForce(const btVector3& force) { -for(int i=0,ni=m_nodes.size();i0) + Node& n=getNodes()[node]; + if(n.m_im>0) { - n.m_f += force; + n.m_f += force; } } // void btSoftBody::AddVelocity(const btVector3& velocity) { -for(int i=0,ni=m_nodes.size();i0) + Node& n=getNodes()[node]; + if(n.m_im>0) { - n.m_v += velocity; + n.m_v += velocity; } } // void btSoftBody::SetMass(int node,btScalar mass) { -m_nodes[node].m_im=mass>0?1/mass:0; -m_bUpdateRtCst=true; + getNodes()[node].m_im=mass>0?1/mass:0; + m_bUpdateRtCst=true; } - + // btScalar btSoftBody::GetMass(int node) const { -return(m_nodes[node].m_im>0?1/m_nodes[node].m_im:0); + return(getNodes()[node].m_im>0?1/getNodes()[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); - for(int j=0;j<3;++j) + const Face& f=getFaces()[i]; + const btScalar twicearea=AreaOf( f.m_n[0]->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; + f.m_n[j]->m_im+=twicearea; } } - for(int i=0;i0 ? - 1/(m_nodes[i].m_im*tmass) : - kmass/tmass; + Node& n=getNodes()[i]; + m_pose.m_wgh[i]= n.m_im>0 ? + 1/(getNodes()[i].m_im*tmass) : + kmass/tmass; } -/* Pos */ -const btVector3 com=EvaluateCom(this); -m_pose.m_pos.resize(m_nodes.size()); -for(int i=0,ni=m_nodes.size();i0) + btScalar vol=0; + if(getNodes().size()>0) { - const btVector3 org=m_nodes[0].m_x; - for(int i=0,ni=m_faces.size();im_x-org,cross(f.m_n[1]->m_x-org,f.m_n[2]->m_x-org)); + const Face& f=getFaces()[i]; + vol+=dot(f.m_n[0]->m_x-org,cross(f.m_n[1]->m_x-org,f.m_n[2]->m_x-org)); } - vol/=(btScalar)6; + vol/=(btScalar)6; } -return(vol); + return(vol); } // int btSoftBody::GenerateBendingConstraints( int distance, - btScalar stiffness) + btScalar stiffness) { -if(distance>1) + if(distance>1) { - /* Build graph */ - 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_)) - for(int j=0;j>1; + unsigned* adj=new unsigned[n*n]; +#define IDX(_x_,_y_) ((_y_)*n+(_x_)) for(int j=0;jsum) + const unsigned sum=adj[IDX(i,k)]+adj[IDX(k,j)]; + if(adj[IDX(i,j)]>sum) { - adj[IDX(i,j)]=adj[IDX(j,i)]=sum; + adj[IDX(i,j)]=adj[IDX(j,i)]=sum; } } } } - /* Build links */ - int nlinks=0; - for(int j=0;j=m_cfg.timestep) + m_timeacc += dt; + m_timeacc = btMin(m_cfg.timestep*m_cfg.maxsteps,m_timeacc); + while(m_timeacc>=m_cfg.timestep) { - /* Update */ - m_timeacc-=m_cfg.timestep; - if(m_bUpdateRtCst) + /* Update */ + m_timeacc-=m_cfg.timestep; + if(m_bUpdateRtCst) { - m_bUpdateRtCst=false; - UpdateConstants(this); + m_bUpdateRtCst=false; + UpdateConstants(this); } - /* Prepare */ - const btScalar iit = 1/(btScalar)m_cfg.iterations; - const btScalar sdt = m_cfg.timestep*m_cfg.timescale; - const btScalar isdt = 1/sdt; - Node* pnodes = m_nodes.size()>0?&m_nodes[0]:0; - /* Forces */ - ApplyForces(this,sdt); - /* Integrate */ - for(int i=0,ni=m_nodes.size();i0?&getNodes()[0]:0; + /* Forces */ + ApplyForces(this,sdt); + /* Integrate */ + for(int i=0,ni=getNodes().size();i0)) - { - for(int i=0,ni=m_nodes.size();i0) + n.m_q = n.m_x; + n.m_v += n.m_f*n.m_im*sdt; + n.m_x += n.m_v*sdt; + } + /* 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_trs*m_pose.m_pos[i]+ - m_pose.m_com; - n.m_x=Lerp(n.m_x,x,m_cfg.kMT); + const btVector3 x= m_pose.m_trs*m_pose.m_pos[i]+ + m_pose.m_com; + n.m_x=Lerp(n.m_x,x,m_cfg.kMT); } } } - /* External collide */ - m_contacts.resize(0); - if(m_cfg.becollide) + /* External collide */ + m_contacts.resize(0); + + if(m_cfg.becollide) { - m_isb->StartCollide(m_bounds[0],m_bounds[1]); - for(int i=0,ni=m_nodes.size();iCheckContact(n.m_x,c.m_cti)) + Node& n=pnodes[i]; + Contact c; + if((!n.m_battach)&&CheckContact(n.m_x,c.m_cti)) { - const btRigidBody* prb=c.m_cti.m_body; - const btScalar ima=n.m_im; - const btScalar imb=prb->getInvMass(); - const btScalar ms=ima+imb; - if(ms>0) + const btRigidBody* prb=c.m_cti.m_body; + const btScalar ima=n.m_im; + const btScalar imb=prb->getInvMass(); + const btScalar ms=ima+imb; + if(ms>0) { - const btTransform& wtr=prb->getWorldTransform(); - const btMatrix3x3 iwi=prb->getInvInertiaTensorWorld(); - const btVector3 ra=n.m_x-wtr.getOrigin(); - const btVector3 va=prb->getVelocityInLocalPoint(ra)*sdt; - const btVector3 vb=n.m_x-n.m_q; - const btVector3 vr=vb-va; - const btScalar dn=dot(vr,c.m_cti.m_normal); - const btVector3 fv=vr-c.m_cti.m_normal*dn; - const btScalar fc=m_cfg.kDF*prb->getFriction(); - c.m_node = &n; - c.m_c0 = ImpulseMatrix(sdt,ima,imb,iwi,ra); - c.m_c1 = ra; - c.m_c2 = ima*sdt; - c.m_c3 = fv.length2()<(btFabs(dn)*fc)?0:1-fc; - m_contacts.push_back(c); + const btTransform& wtr=prb->getWorldTransform(); + const btMatrix3x3 iwi=prb->getInvInertiaTensorWorld(); + const btVector3 ra=n.m_x-wtr.getOrigin(); + const btVector3 va=prb->getVelocityInLocalPoint(ra)*sdt; + const btVector3 vb=n.m_x-n.m_q; + const btVector3 vr=vb-va; + const btScalar dn=dot(vr,c.m_cti.m_normal); + const btVector3 fv=vr-c.m_cti.m_normal*dn; + const btScalar fc=m_cfg.kDF*prb->getFriction(); + c.m_node = &n; + c.m_c0 = ImpulseMatrix(sdt,ima,imb,iwi,ra); + c.m_c1 = ra; + c.m_c2 = ima*sdt; + c.m_c3 = fv.length2()<(btFabs(dn)*fc)?0:1-fc; + m_contacts.push_back(c); } } } - m_isb->EndCollide(); + EndCollide(); } - /* Prepare anchors */ - for(int i=0,ni=m_anchors.size();igetWorldTransform().getBasis()*a.m_local; - a.m_c0 = ImpulseMatrix( sdt, - a.m_node->m_im, - a.m_body->getInvMass(), - a.m_body->getInvInertiaTensorWorld(), - ra); - a.m_c1 = ra; - a.m_c2 = sdt*a.m_node->m_im; + Anchor& a=m_anchors[i]; + const btVector3 ra=a.m_body->getWorldTransform().getBasis()*a.m_local; + a.m_c0 = ImpulseMatrix( sdt, + a.m_node->m_im, + a.m_body->getInvMass(), + a.m_body->getInvInertiaTensorWorld(), + ra); + a.m_c1 = ra; + a.m_c2 = sdt*a.m_node->m_im; } - /* Solve */ - for(int isolve=0;isolve(1,m_cfg.kSOR,isolve*iit)*m_cfg.kLST; - PSolve_Anchors(this,sdt); - PSolve_Contacts(this,sdt); - PSolve_Links(this,lw); + const btScalar lw= Lerp(1,m_cfg.kSOR,isolve*iit)*m_cfg.kLST; + PSolve_Anchors(this,sdt); + PSolve_Contacts(this,sdt); + PSolve_Links(this,lw); } - /* Velocities */ - const btScalar vc=isdt*(1-m_cfg.kDP); - for(int i=0,ni=m_nodes.size();igetCollisionShape(); + btConvexShape* csh=static_cast(shp); + const btTransform& wtr=prb->getWorldTransform(); + btScalar dst=m_worldInfo.m_sparsesdf.Evaluate(wtr.invXform(x),csh,nrm); + nrm=wtr.getBasis()*nrm; + btVector3 wit=x-nrm*dst; + if(dst m_sparsesdf; + }; + + //reference or copy? + btSoftBodyWorldInfo& m_worldInfo; + + ///constructor + + btSoftBody(btSoftBody::btSoftBodyWorldInfo& worldInfo,int node_count, + const btVector3* x, + const btScalar* m); + + + ///sCti is Softbody contact info + struct sCti + { + btRigidBody* m_body; /* Rigid body */ + btVector3 m_normal; /* Outward normal */ + btScalar m_offset; /* Offset from origin */ + }; + + void StartCollide(const btVector3& aabbMin,const btVector3& aabbMax) + { + //?? + } + void EndCollide() + { + //?? + } + + // + bool CheckContact( const btVector3& x, btSoftBody::sCti& cti); + //// + + ///destructor + virtual ~btSoftBody(); + + + + struct sMedium + { + btVector3 m_velocity; /* Velocity */ + btScalar m_pressure; /* Pressure */ + btScalar m_density; /* Density */ + }; + + virtual void EvaluateMedium( const btVector3& /*position*/, sMedium& medium) + { + medium.m_velocity=btVector3(0,0,0); + medium.m_pressure=0; + medium.m_density=0; + } + + + // // Internal types // - + typedef btAlignedObjectArray tScalarArray; typedef btAlignedObjectArray tVector3Array; - + /* Base type */ struct Element - { + { void* m_tag; // User data - }; - /* Node */ + }; + ///Node struct Node : Element - { + { btVector3 m_x; // Position btVector3 m_q; // Previous step position btVector3 m_v; // Velocity @@ -108,47 +141,47 @@ struct btSoftBody btScalar m_im; // 1/mass btScalar m_area; // Area int m_battach:1; // Attached - }; + }; /* Link */ struct Link : Element - { + { 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 - eLType::_ m_type; // Link type - }; + btSoftBody::eLType::_ m_type; // Link type + }; /* Face */ struct Face : Element - { + { Node* m_n[3]; // Node pointers btVector3 m_normal; // Normal btScalar m_ra; // Rest area - }; + }; /* Contact */ struct Contact - { - ISoftBody::sCti m_cti; // Contact infos + { + btSoftBody::sCti m_cti; // Contact infos Node* m_node; // Owner node btMatrix3x3 m_c0; // Impulse matrix btVector3 m_c1; // Relative anchor btScalar m_c2; // ima*dt btScalar m_c3; // Friction - }; + }; /* Anchor */ struct Anchor - { + { Node* m_node; // Node pointer btVector3 m_local; // Anchor position in body space btRigidBody* m_body; // Body btMatrix3x3 m_c0; // Impulse matrix btVector3 m_c1; // Relative anchor btScalar m_c2; // ima*dt - }; + }; /* Pose */ struct Pose - { + { bool m_bvolume; // Is valid bool m_bframe; // Is frame btScalar m_volume; // Rest volume @@ -156,11 +189,11 @@ struct btSoftBody tScalarArray m_wgh; // Weights btVector3 m_com; // COM btMatrix3x3 m_trs; // Transform - }; + }; /* Config */ struct Config - { - eAeroModel::_ aeromodel; // Aerodynamic model (default: V_Point) + { + btSoftBody::eAeroModel::_ aeromodel; // Aerodynamic model (default: V_Point) btScalar kLST; // Linear stiffness coefficient [0,1] btScalar kDP; // Damping coefficient [0,1] btScalar kDG; // Drag coefficient [0,+inf] @@ -178,93 +211,176 @@ struct btSoftBody int iterations; // Solver iterations bool becollide; // Enable external collisions bool bscollide; // Enable self collisions - }; - + }; + // // Typedef's // - + typedef btAlignedObjectArray tNodeArray; typedef btAlignedObjectArray tLinkArray; typedef btAlignedObjectArray tFaceArray; typedef btAlignedObjectArray tAnchorArray; typedef btAlignedObjectArray tContactArray; - + // // Fields // - + Config m_cfg; // Configuration Pose m_pose; // Pose - ISoftBody* m_isb; // ISoftBody void* m_tag; // User data - tNodeArray m_nodes; // Nodes - tLinkArray m_links; // Links - tFaceArray m_faces; // Faces + + ////////////////////// + + ///btSoftBodyCollisionShape is work-in-progress collision shape for softbodies + class btSoftBodyCollisionShape : public btConcaveShape + { + static btVector3 m_sScaling; + public: + + tNodeArray m_nodes; // Nodes + tLinkArray m_links; // Links + tFaceArray m_faces; // Faces + + btSoftBodyCollisionShape(); + + virtual ~btSoftBodyCollisionShape(); + + virtual void processAllTriangles(btTriangleCallback* callback,const btVector3& aabbMin,const btVector3& aabbMax) const; + + ///getAabb returns the axis aligned bounding box in the coordinate frame of the given transform t. + virtual void getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const + { + ///not yet + btAssert(0); + } + + virtual int getShapeType() const + { + return SOFTBODY_SHAPE_PROXYTYPE; + } + virtual void setLocalScaling(const btVector3& scaling) + { + ///not yet + btAssert(0); + } + virtual const btVector3& getLocalScaling() const + { + ///not yet + btAssert(0); + return m_sScaling; + } + virtual void calculateLocalInertia(btScalar mass,btVector3& inertia) const + { + ///not yet + btAssert(0); + } + virtual const char* getName()const + { + return "SoftBody"; + } + + }; + + + btSoftBodyCollisionShape* m_softBodyCollisionShape; + + btCollisionObjectArray m_overlappingRigidBodies; + + btAlignedObjectArray m_overlappingSoftBodies; + + + ////////////////////// + + inline tNodeArray& getNodes() + { + return m_softBodyCollisionShape->m_nodes; + } + inline const tNodeArray& getNodes() const + { + return m_softBodyCollisionShape->m_nodes; + } + + inline tLinkArray& getLinks() + { + return m_softBodyCollisionShape->m_links; + } + inline const tLinkArray& getLinks() const + { + return m_softBodyCollisionShape->m_links; + } + + inline tFaceArray& getFaces() + { + return m_softBodyCollisionShape->m_faces; + } + + inline const tFaceArray& getFaces() const + { + return m_softBodyCollisionShape->m_faces; + } + + tAnchorArray m_anchors; // Anchors tContactArray m_contacts; // Contacts btScalar m_timeacc; // Time accumulator btVector3 m_bounds[2]; // Spatial bounds bool m_bUpdateRtCst; // Update runtime constants - + // // Api // - /* Create a soft body */ - static btSoftBody* Create( ISoftBody* isoftbody, - int node_count, - const btVector3* x=0, - const btScalar* m=0); /* Delete a body */ void Delete(); /* Check for existing link */ bool CheckLink( int node0, - int node1) const; + int node1) const; bool CheckLink( const btSoftBody::Node* node0, - const btSoftBody::Node* node1) const; + const btSoftBody::Node* node1) const; /* Check for existring face */ bool CheckFace( int node0, - int node1, - int node2) const; + int node1, + int node2) const; /* Append link */ void AppendLink( int node0, - int node1, - btScalar kST, - btSoftBody::eLType::_ type, - bool bcheckexist=false); + 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); + btSoftBody::Node* node1, + btScalar kST, + btSoftBody::eLType::_ type, + bool bcheckexist=false); /* Append face */ void AppendFace( int node0, - int node1, - int node2); + int node1, + int node2); /* 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); /* Transform */ @@ -273,84 +389,30 @@ struct btSoftBody void Scale( const btVector3& scl); /* Set current state as pose */ void SetPose( bool bvolume, - bool bframe); + bool bframe); /* Return the volume */ btScalar GetVolume() const; /* Generate bending constraints based on distance in the adjency graph */ int GenerateBendingConstraints( int distance, - btScalar stiffness); + btScalar stiffness); /* Randomize constraints to reduce solver bias */ void RandomizeConstraints(); /* Ray casting */ btScalar Raycast( const btVector3& org, - const btVector3& dir) const; + const btVector3& dir) const; /* Step */ void Step( btScalar dt); - - }; - -// -// Helpers -// -/* fDrawFlags */ -struct fDrawFlags { enum _ { - Nodes = 0x0001, - SLinks = 0x0002, - BLinks = 0x0004, - Faces = 0x0008, - Tetras = 0x0010, - Normals = 0x0020, - Contacts = 0x0040, - Anchors = 0x0080, - /* presets */ - Links = SLinks+BLinks, - Std = SLinks+Faces+Anchors, - StdTetra = Std-Faces+Tetras, -};}; + void updateBounds(); -/* Draw body */ -void Draw( btSoftBody* psb, - btIDebugDraw* idraw, - int drawflags=fDrawFlags::Std); -/* Draw body infos */ -void DrawInfos( btSoftBody* psb, - btIDebugDraw* idraw, - bool masses, - bool areas, - bool stress); -/* Draw rigid frame */ -void DrawFrame( btSoftBody* psb, - btIDebugDraw* idraw); -/* Create a rope */ -btSoftBody* CreateRope( btSoftBody::ISoftBody* isoftbody, - const btVector3& from, - const btVector3& to, - int res, - int fixeds); -/* Create a patch */ -btSoftBody* CreatePatch( btSoftBody::ISoftBody* isoftbody, - const btVector3& corner00, - const btVector3& corner10, - const btVector3& corner01, - const btVector3& corner11, - int resx, - int resy, - int fixeds, - bool gendiags); -/* Create an ellipsoid */ -btSoftBody* CreateEllipsoid(btSoftBody::ISoftBody* isoftbody, - const btVector3& center, - const btVector3& radius, - int res); -/* Create from convex-hull */ -btSoftBody* CreateFromConvexHull( btSoftBody::ISoftBody* isoftbody, - const btVector3* vertices, - int nvertices); -/* Create from trimesh */ -btSoftBody* CreateFromTriMesh( btSoftBody::ISoftBody* isoftbody, - const btScalar* vertices, - const int* triangles, - int ntriangles); + void updateTransform() + { + updateBounds(); + } -#endif \ No newline at end of file + +}; + + + +#endif //_BT_SOFT_BODY_H diff --git a/src/BulletDynamics/SoftBody/btSoftBodyHelpers.cpp b/src/BulletDynamics/SoftBody/btSoftBodyHelpers.cpp index cc4118be1..131ddd320 100644 --- a/src/BulletDynamics/SoftBody/btSoftBodyHelpers.cpp +++ b/src/BulletDynamics/SoftBody/btSoftBodyHelpers.cpp @@ -17,73 +17,69 @@ subject to the following restrictions: #include "btSoftBody.h" #include #include - -namespace btsoftbody_internals -{ +#include "btSoftBodyHelpers.h" // -static void drawVertex( btIDebugDraw* idraw, - const btVector3& x,btScalar s,const btVector3& c) -{ -idraw->drawLine(x-btVector3(s,0,0),x+btVector3(s,0,0),c); -idraw->drawLine(x-btVector3(0,s,0),x+btVector3(0,s,0),c); -idraw->drawLine(x-btVector3(0,0,s),x+btVector3(0,0,s),c); -} - -// -static btVector3 stresscolor(btScalar stress) -{ -static const btVector3 spectrum[]= { - btVector3(1,0,1), - btVector3(0,0,1), - btVector3(0,1,1), - btVector3(0,1,0), - btVector3(1,1,0), - btVector3(1,0,0), - btVector3(1,0,0), - }; -static const int ncolors=sizeof(spectrum)/sizeof(spectrum[0])-1; -static const btScalar one=1; -stress=btMax(0,btMin(1,stress))*ncolors; -const int sel=(int)stress; -const btScalar frc=stress-sel; -return(spectrum[sel]+(spectrum[sel+1]-spectrum[sel])*frc); -} - -} - -using namespace btsoftbody_internals; - -// -void Draw( btSoftBody* psb, - btIDebugDraw* idraw, - int drawflags) -{ -const btScalar scl=(btScalar)0.1; -const btScalar nscl=scl*5; -const btScalar alpha=(btScalar)0.5; -const btVector3 scolor=btVector3(0,0,0); -const btVector3 bcolor=btVector3(1,1,0); -const btVector3 ncolor=btVector3(1,1,1); -const btVector3 ccolor=btVector3(1,0,0); -/* Nodes */ -if(0!=(drawflags&fDrawFlags::Nodes)) +void btSoftBodyHelpers::drawVertex( btIDebugDraw* idraw, + const btVector3& x,btScalar s,const btVector3& c) { - for(int i=0;im_nodes.size();++i) + idraw->drawLine(x-btVector3(s,0,0),x+btVector3(s,0,0),c); + idraw->drawLine(x-btVector3(0,s,0),x+btVector3(0,s,0),c); + idraw->drawLine(x-btVector3(0,0,s),x+btVector3(0,0,s),c); + } + +// + btVector3 btSoftBodyHelpers::stresscolor(btScalar stress) +{ + static const btVector3 spectrum[]= { + btVector3(1,0,1), + btVector3(0,0,1), + btVector3(0,1,1), + btVector3(0,1,0), + btVector3(1,1,0), + btVector3(1,0,0), + btVector3(1,0,0), + }; + static const int ncolors=sizeof(spectrum)/sizeof(spectrum[0])-1; + static const btScalar one=1; + stress=btMax(0,btMin(1,stress))*ncolors; + const int sel=(int)stress; + const btScalar frc=stress-sel; + return(spectrum[sel]+(spectrum[sel+1]-spectrum[sel])*frc); +} + + + +// +void btSoftBodyHelpers::Draw( btSoftBody* psb, + btIDebugDraw* idraw, + int drawflags) +{ + const btScalar scl=(btScalar)0.1; + const btScalar nscl=scl*5; + const btScalar alpha=(btScalar)0.5; + const btVector3 scolor=btVector3(0,0,0); + const btVector3 bcolor=btVector3(1,1,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) { - const btSoftBody::Node& n=psb->m_nodes[i]; - 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)); + const btSoftBody::Node& n=psb->getNodes()[i]; + 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)); } } -/* Links */ -if(0!=(drawflags&fDrawFlags::Links)) + /* Links */ + if(0!=(drawflags&fDrawFlags::Links)) { - for(int i=0;im_links.size();++i) + for(int i=0;igetLinks().size();++i) { - const btSoftBody::Link& l=psb->m_links[i]; - switch(l.m_type) + 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; @@ -92,308 +88,304 @@ if(0!=(drawflags&fDrawFlags::Links)) } } } -/* Normals */ -if(0!=(drawflags&fDrawFlags::Normals)) + /* Normals */ + if(0!=(drawflags&fDrawFlags::Normals)) { - for(int i=0;im_nodes.size();++i) + for(int i=0;igetNodes().size();++i) { - const btSoftBody::Node& n=psb->m_nodes[i]; - 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); + const btSoftBody::Node& n=psb->getNodes()[i]; + 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); } } -/* Contacts */ -if(0!=(drawflags&fDrawFlags::Contacts)) + /* Contacts */ + if(0!=(drawflags&fDrawFlags::Contacts)) { - static const btVector3 axis[]={btVector3(1,0,0), - btVector3(0,1,0), - btVector3(0,0,1)}; - for(int i=0;im_contacts.size();++i) + static const btVector3 axis[]={btVector3(1,0,0), + btVector3(0,1,0), + btVector3(0,0,1)}; + for(int i=0;im_contacts.size();++i) { - const btSoftBody::Contact& c=psb->m_contacts[i]; - const btVector3 o= c.m_node->m_x-c.m_cti.m_normal* - (dot(c.m_node->m_x,c.m_cti.m_normal)+c.m_cti.m_offset); - const btVector3 x=cross(c.m_cti.m_normal,axis[c.m_cti.m_normal.minAxis()]).normalized(); - const btVector3 y=cross(x,c.m_cti.m_normal).normalized(); - idraw->drawLine(o-x*nscl,o+x*nscl,ccolor); - idraw->drawLine(o-y*nscl,o+y*nscl,ccolor); - idraw->drawLine(o,o+c.m_cti.m_normal*nscl*3,btVector3(1,1,0)); + const btSoftBody::Contact& c=psb->m_contacts[i]; + const btVector3 o= c.m_node->m_x-c.m_cti.m_normal* + (dot(c.m_node->m_x,c.m_cti.m_normal)+c.m_cti.m_offset); + const btVector3 x=cross(c.m_cti.m_normal,axis[c.m_cti.m_normal.minAxis()]).normalized(); + const btVector3 y=cross(x,c.m_cti.m_normal).normalized(); + idraw->drawLine(o-x*nscl,o+x*nscl,ccolor); + idraw->drawLine(o-y*nscl,o+y*nscl,ccolor); + idraw->drawLine(o,o+c.m_cti.m_normal*nscl*3,btVector3(1,1,0)); } } -/* Anchors */ -if(0!=(drawflags&fDrawFlags::Anchors)) + /* Anchors */ + if(0!=(drawflags&fDrawFlags::Anchors)) { - for(int i=0;im_anchors.size();++i) + for(int i=0;im_anchors.size();++i) { - const btSoftBody::Anchor& a=psb->m_anchors[i]; - const btVector3 q=a.m_body->getWorldTransform()*a.m_local; - drawVertex(idraw,a.m_node->m_x,0.25,btVector3(1,0,0)); - drawVertex(idraw,q,0.25,btVector3(0,1,0)); - idraw->drawLine(a.m_node->m_x,q,btVector3(1,1,1)); + const btSoftBody::Anchor& a=psb->m_anchors[i]; + const btVector3 q=a.m_body->getWorldTransform()*a.m_local; + drawVertex(idraw,a.m_node->m_x,0.25,btVector3(1,0,0)); + 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;im_nodes.size();++i) + for(int i=0;igetNodes().size();++i) { - const btSoftBody::Node& n=psb->m_nodes[i]; - if(n.m_im<=0) + const btSoftBody::Node& n=psb->getNodes()[i]; + if(n.m_im<=0) { - drawVertex(idraw,n.m_x,0.25,btVector3(1,0,0)); + drawVertex(idraw,n.m_x,0.25,btVector3(1,0,0)); } } } -/* Faces */ -if(0!=(drawflags&fDrawFlags::Faces)) + /* Faces */ + if(0!=(drawflags&fDrawFlags::Faces)) { - const btScalar scl=(btScalar)0.7; - const btScalar alp=(btScalar)1; - const btVector3 col(0,(btScalar)0.7,0); - for(int i=0;im_faces.size();++i) + const btScalar scl=(btScalar)0.7; + const btScalar alp=(btScalar)1; + const btVector3 col(0,(btScalar)0.7,0); + for(int i=0;igetFaces().size();++i) { - const btSoftBody::Face& f=psb->m_faces[i]; - 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->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); + const btSoftBody::Face& f=psb->getFaces()[i]; + 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->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); } } } // -void DrawInfos( btSoftBody* psb, - btIDebugDraw* idraw, - bool masses, - bool areas, - bool stress) +void btSoftBodyHelpers::DrawInfos( btSoftBody* psb, + btIDebugDraw* idraw, + bool masses, + bool areas, + bool stress) { -for(int i=0;im_nodes.size();++i) + for(int i=0;igetNodes().size();++i) { - const btSoftBody::Node& n=psb->m_nodes[i]; - char text[2048]={0}; - char buff[1024]; - if(masses) + const btSoftBody::Node& n=psb->getNodes()[i]; + char text[2048]={0}; + char buff[1024]; + if(masses) { - sprintf(buff," M(%.2f)",1/n.m_im); - strcat(text,buff); + sprintf(buff," M(%.2f)",1/n.m_im); + strcat(text,buff); } - if(areas) + if(areas) { - sprintf(buff," A(%.2f)",n.m_area); - strcat(text,buff); + sprintf(buff," A(%.2f)",n.m_area); + strcat(text,buff); } - if(text[0]) idraw->draw3dText(n.m_x,text); + if(text[0]) idraw->draw3dText(n.m_x,text); } } // -void DrawFrame( btSoftBody* psb, - btIDebugDraw* idraw) +void btSoftBodyHelpers::DrawFrame( btSoftBody* psb, + btIDebugDraw* idraw) { -if(psb->m_pose.m_bframe) + if(psb->m_pose.m_bframe) { - static const btScalar ascl=10; - static const btScalar nscl=(btScalar)0.1; - const btVector3 com=psb->m_pose.m_com; - const btMatrix3x3& trs=psb->m_pose.m_trs; - const btVector3 Xaxis=(trs*btVector3(1,0,0)).normalized(); - const btVector3 Yaxis=(trs*btVector3(0,1,0)).normalized(); - const btVector3 Zaxis=(trs*btVector3(0,0,1)).normalized(); - idraw->drawLine(com,com+Xaxis*ascl,btVector3(1,0,0)); - idraw->drawLine(com,com+Yaxis*ascl,btVector3(0,1,0)); - idraw->drawLine(com,com+Zaxis*ascl,btVector3(0,0,1)); - for(int i=0;im_pose.m_pos.size();++i) + static const btScalar ascl=10; + static const btScalar nscl=(btScalar)0.1; + const btVector3 com=psb->m_pose.m_com; + const btMatrix3x3& trs=psb->m_pose.m_trs; + const btVector3 Xaxis=(trs*btVector3(1,0,0)).normalized(); + const btVector3 Yaxis=(trs*btVector3(0,1,0)).normalized(); + const btVector3 Zaxis=(trs*btVector3(0,0,1)).normalized(); + idraw->drawLine(com,com+Xaxis*ascl,btVector3(1,0,0)); + idraw->drawLine(com,com+Yaxis*ascl,btVector3(0,1,0)); + idraw->drawLine(com,com+Zaxis*ascl,btVector3(0,0,1)); + for(int i=0;im_pose.m_pos.size();++i) { - const btVector3 x=com+trs*psb->m_pose.m_pos[i]; - idraw->drawLine(x-btVector3(1,0,0)*nscl,x+btVector3(1,0,0)*nscl,btVector3(1,0,1)); - idraw->drawLine(x-btVector3(0,1,0)*nscl,x+btVector3(0,1,0)*nscl,btVector3(1,0,1)); - idraw->drawLine(x-btVector3(0,0,1)*nscl,x+btVector3(0,0,1)*nscl,btVector3(1,0,1)); + const btVector3 x=com+trs*psb->m_pose.m_pos[i]; + idraw->drawLine(x-btVector3(1,0,0)*nscl,x+btVector3(1,0,0)*nscl,btVector3(1,0,1)); + idraw->drawLine(x-btVector3(0,1,0)*nscl,x+btVector3(0,1,0)*nscl,btVector3(1,0,1)); + idraw->drawLine(x-btVector3(0,0,1)*nscl,x+btVector3(0,0,1)*nscl,btVector3(1,0,1)); } } } // -btSoftBody* CreateRope( btSoftBody::ISoftBody* isoftbody, - const btVector3& from, - const btVector3& to, - int res, - int fixeds) +btSoftBody* btSoftBodyHelpers::CreateRope( btSoftBody::btSoftBodyWorldInfo& worldInfo, const btVector3& from, + const btVector3& to, + int res, + int fixeds) { -/* Create nodes */ -const int r=res+2; -btVector3* x=new btVector3[r]; -btScalar* m=new btScalar[r]; -for(int i=0;iSetMass(0,0); -if(fixeds&2) psb->SetMass(r-1,0); -delete[] x; -delete[] m; -/* Create links */ -for(int i=1;iSetMass(0,0); + if(fixeds&2) psb->SetMass(r-1,0); + delete[] x; + delete[] m; + /* Create links */ + for(int i=1;iAppendLink(i-1,i,1,btSoftBody::eLType::Structural); + psb->AppendLink(i-1,i,1,btSoftBody::eLType::Structural); } -/* Finished */ -return(psb); + /* Finished */ + return(psb); } // -btSoftBody* CreatePatch( btSoftBody::ISoftBody* isoftbody, - const btVector3& corner00, - const btVector3& corner10, - const btVector3& corner01, - const btVector3& corner11, - int resx, - int resy, - int fixeds, - bool gendiags) +btSoftBody* btSoftBodyHelpers::CreatePatch(btSoftBody::btSoftBodyWorldInfo& worldInfo,const btVector3& corner00, + const btVector3& corner10, + const btVector3& corner01, + const btVector3& corner11, + int resx, + int resy, + int fixeds, + bool gendiags) { #define IDX(_x_,_y_) ((_y_)*rx+(_x_)) -/* Create nodes */ -if((resx<2)||(resy<2)) return(0); -const int rx=resx; -const int ry=resy; -const int tot=rx*ry; -btVector3* x=new btVector3[tot]; -btScalar* m=new btScalar[tot]; -for(int iy=0;iySetMass(IDX(0,0),0); -if(fixeds&2) psb->SetMass(IDX(rx-1,0),0); -if(fixeds&4) psb->SetMass(IDX(0,ry-1),0); -if(fixeds&8) psb->SetMass(IDX(rx-1,ry-1),0); -delete[] x; -delete[] m; -/* Create links and faces */ -for(int iy=0;iySetMass(IDX(0,0),0); + if(fixeds&2) psb->SetMass(IDX(rx-1,0),0); + if(fixeds&4) psb->SetMass(IDX(0,ry-1),0); + if(fixeds&8) psb->SetMass(IDX(rx-1,ry-1),0); + delete[] x; + delete[] m; + /* Create links and faces */ + for(int iy=0;iyAppendLink(idx,IDX(ix+1,iy), - 1,btSoftBody::eLType::Structural); - if(mdy) psb->AppendLink(idx,IDX(ix,iy+1), - 1,btSoftBody::eLType::Structural); - if(mdx&&mdy) + 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&&mdy) { - if((ix+iy)&1) + if((ix+iy)&1) { - psb->AppendFace(IDX(ix,iy),IDX(ix+1,iy),IDX(ix+1,iy+1)); - psb->AppendFace(IDX(ix,iy),IDX(ix+1,iy+1),IDX(ix,iy+1)); - if(gendiags) + psb->AppendFace(IDX(ix,iy),IDX(ix+1,iy),IDX(ix+1,iy+1)); + 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), + 1,btSoftBody::eLType::Structural); } } else { - psb->AppendFace(IDX(ix,iy+1),IDX(ix,iy),IDX(ix+1,iy)); - psb->AppendFace(IDX(ix,iy+1),IDX(ix+1,iy),IDX(ix+1,iy+1)); - if(gendiags) + psb->AppendFace(IDX(ix,iy+1),IDX(ix,iy),IDX(ix+1,iy)); + 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), + 1,btSoftBody::eLType::Structural); } } } } } -/* Finished */ + /* Finished */ #undef IDX -return(psb); + return(psb); } // -btSoftBody* CreateEllipsoid(btSoftBody::ISoftBody* isoftbody, - const btVector3& center, +btSoftBody* btSoftBodyHelpers::CreateEllipsoid(btSoftBody::btSoftBodyWorldInfo& worldInfo,const btVector3& center, const btVector3& radius, int res) { -struct Hammersley + struct Hammersley { - static void Generate(btVector3* x,int n) + static void Generate(btVector3* x,int n) { - for(int i=0;i>=1) if(j&1) t+=p; - btScalar w=2*t-1; - btScalar a=(SIMD_PI+2*i*SIMD_PI)/n; - btScalar s=btSqrt(1-w*w); - *x++=btVector3(s*btCos(a),s*btSin(a),w); + btScalar p=0.5,t=0; + for(int j=i;j;p*=0.5,j>>=1) if(j&1) t+=p; + btScalar w=2*t-1; + btScalar a=(SIMD_PI+2*i*SIMD_PI)/n; + btScalar s=btSqrt(1-w*w); + *x++=btVector3(s*btCos(a),s*btSin(a),w); } } }; -btAlignedObjectArray vtx; -vtx.resize(3+res); -Hammersley::Generate(&vtx[0],vtx.size()); -for(int i=0;i vtx; + vtx.resize(3+res); + Hammersley::Generate(&vtx[0],vtx.size()); + for(int i=0;i chks; -btAlignedObjectArray vtx; -chks.resize(maxidx*maxidx,false); -vtx.resize(maxidx); -for(int i=0,j=0,ni=maxidx*3;i chks; + btAlignedObjectArray vtx; + chks.resize(maxidx*maxidx,false); + vtx.resize(maxidx); + for(int i=0,j=0,ni=maxidx*3;iAppendLink(idx[j],idx[k],1,btSoftBody::eLType::Structural); + chks[IDX(idx[j],idx[k])]=true; + chks[IDX(idx[k],idx[k])]=true; + psb->AppendLink(idx[j],idx[k],1,btSoftBody::eLType::Structural); } } - #undef IDX - psb->AppendFace(idx[0],idx[1],idx[2]); +#undef IDX + psb->AppendFace(idx[0],idx[1],idx[2]); } -psb->RandomizeConstraints(); -return(psb); + psb->RandomizeConstraints(); + return(psb); } diff --git a/src/BulletDynamics/SoftBody/btSoftBodyHelpers.h b/src/BulletDynamics/SoftBody/btSoftBodyHelpers.h new file mode 100644 index 000000000..15c82e22f --- /dev/null +++ b/src/BulletDynamics/SoftBody/btSoftBodyHelpers.h @@ -0,0 +1,90 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2008 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. +*/ + +#ifndef SOFT_BODY_HELPERS_H +#define SOFT_BODY_HELPERS_H + +#include "btSoftBody.h" + +// +// Helpers +// + +/* fDrawFlags */ +struct fDrawFlags { enum _ { + Nodes = 0x0001, + SLinks = 0x0002, + BLinks = 0x0004, + Faces = 0x0008, + Tetras = 0x0010, + Normals = 0x0020, + Contacts = 0x0040, + Anchors = 0x0080, + /* presets */ + Links = SLinks+BLinks, + Std = SLinks+Faces+Anchors, + StdTetra = Std-Faces+Tetras, +};}; + +struct btSoftBodyHelpers +{ + + static btVector3 stresscolor(btScalar stress); + + static void drawVertex( btIDebugDraw* idraw, + const btVector3& x,btScalar s,const btVector3& c); + + + /* Draw body */ + static void Draw( btSoftBody* psb, + btIDebugDraw* idraw, + int drawflags=fDrawFlags::Std); + /* Draw body infos */ + static void DrawInfos( btSoftBody* psb, + btIDebugDraw* idraw, + bool masses, + bool areas, + bool stress); + /* Draw rigid frame */ + 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); + /* 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); + /* 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); + /* Create from trimesh */ + static btSoftBody* CreateFromTriMesh( btSoftBody::btSoftBodyWorldInfo& worldInfo, const btScalar* vertices, + const int* triangles, + int ntriangles); +}; + +#endif //SOFT_BODY_HELPERS_H