From d71f8d6623029465b01c668bd07746366ecd7e9b Mon Sep 17 00:00:00 2001 From: "erwin.coumans" Date: Tue, 22 Jul 2008 02:22:01 +0000 Subject: [PATCH] Added several updates for btSoftBody: convex cluster collision detection, new constraints, new demos (only enabled in SoftBodyDemo, todo for AllBulletDemos) etc. Thanks a lot to Nathanael Presson for this update. --- Demos/OpenGL/DemoApplication.cpp | 1 + Demos/SoftDemo/SoftDemo.cpp | 424 ++- src/BulletSoftBody/btSoftBody.cpp | 2698 +++++++++-------- src/BulletSoftBody/btSoftBody.h | 329 +- src/BulletSoftBody/btSoftBodyHelpers.cpp | 185 +- src/BulletSoftBody/btSoftBodyHelpers.h | 12 +- src/BulletSoftBody/btSoftBodyInternals.h | 891 ++++++ .../btSoftRigidDynamicsWorld.cpp | 23 +- src/BulletSoftBody/btSoftRigidDynamicsWorld.h | 7 + 9 files changed, 3223 insertions(+), 1347 deletions(-) create mode 100644 src/BulletSoftBody/btSoftBodyInternals.h diff --git a/Demos/OpenGL/DemoApplication.cpp b/Demos/OpenGL/DemoApplication.cpp index 0fbbd4804..6a805238e 100644 --- a/Demos/OpenGL/DemoApplication.cpp +++ b/Demos/OpenGL/DemoApplication.cpp @@ -180,6 +180,7 @@ void DemoApplication::updateCamera() { m_cameraPosition[0] = eyePos.getX(); m_cameraPosition[1] = eyePos.getY(); m_cameraPosition[2] = eyePos.getZ(); + m_cameraPosition += m_cameraTargetPosition; if (m_glutScreenWidth > m_glutScreenHeight) { diff --git a/Demos/SoftDemo/SoftDemo.cpp b/Demos/SoftDemo/SoftDemo.cpp index 75c24696b..fcfb86314 100644 --- a/Demos/SoftDemo/SoftDemo.cpp +++ b/Demos/SoftDemo/SoftDemo.cpp @@ -129,10 +129,16 @@ void SoftDemo::clientMoveAndDisplay() m_goal=rayFrom+rayDir*hit; } } - m_node->m_v+=(m_goal-m_node->m_x)/dt; + btVector3 delta=m_goal-m_node->m_x; + static const btScalar maxdrag=10; + if(delta.length2()>(maxdrag*maxdrag)) + { + delta=delta.normalized()*maxdrag; + } + m_node->m_v+=delta/dt; } -//#define FIXED_STEP +#define FIXED_STEP #ifdef FIXED_STEP m_dynamicsWorld->stepSimulation(dt=1.0f/60.f,0); @@ -296,7 +302,7 @@ static void Ctor_RbUpStack(SoftDemo* pdemo,int count) { btTransform startTransform; startTransform.setIdentity(); - startTransform.setOrigin(btVector3(0,1+6*i,0)); + startTransform.setOrigin(btVector3(0,2+6*i,0)); pdemo->localCreateRigidBody(mass,startTransform,shape[i%nshapes]); } } @@ -315,13 +321,14 @@ static void Ctor_BigBall(SoftDemo* pdemo,btScalar mass=10) // // Big plate // -static void Ctor_BigPlate(SoftDemo* pdemo,btScalar mass=15) +static btRigidBody* Ctor_BigPlate(SoftDemo* pdemo,btScalar mass=15,btScalar height=4) { btTransform startTransform; startTransform.setIdentity(); - startTransform.setOrigin(btVector3(0,4,0.5)); + startTransform.setOrigin(btVector3(0,height,0.5)); btRigidBody* body=pdemo->localCreateRigidBody(mass,startTransform,new btBoxShape(btVector3(5,1,5))); body->setFriction(1); + return(body); } // @@ -359,6 +366,7 @@ static btSoftBody* Ctor_SoftBox(SoftDemo* pdemo,const btVector3& p,const btVecto pdemo->getSoftDynamicsWorld()->addSoftBody(psb); return(psb); + } // @@ -598,7 +606,9 @@ static void Init_Aero(SoftDemo* pdemo) btVector3(+s,h,+s), segments,segments, 0,true); - psb->generateBendingConstraints(2); + btSoftBody::Material* pm=psb->appendMaterial(); + pm->m_flags -= btSoftBody::fMaterial::DebugDraw; + psb->generateBendingConstraints(2,pm); psb->m_cfg.kLF = 0.004; psb->m_cfg.kDG = 0.0003; psb->m_cfg.aeromodel = btSoftBody::eAeroModel::V_TwoSided; @@ -779,7 +789,7 @@ static void Init_Bunny(SoftDemo* pdemo) psb->m_cfg.kDF = 0.5; psb->randomizeConstraints(); psb->scale(btVector3(6,6,6)); - psb->setTotalMass(100,true); + psb->setTotalMass(100,true); pdemo->getSoftDynamicsWorld()->addSoftBody(psb); pdemo->m_cutting=true; @@ -795,12 +805,12 @@ static void Init_BunnyMatch(SoftDemo* pdemo) &gIndicesBunny[0][0], BUNNY_NUM_TRIANGLES); psb->m_cfg.kDF = 0.5; - psb->m_materials[0]->m_kLST = 0.1; psb->m_cfg.kMT = 0.05; + psb->m_cfg.piterations = 5; psb->randomizeConstraints(); psb->scale(btVector3(6,6,6)); psb->setTotalMass(100,true); - psb->setPose(true,true); + psb->setPose(false,true); pdemo->getSoftDynamicsWorld()->addSoftBody(psb); } @@ -844,7 +854,7 @@ static void Init_TorusMatch(SoftDemo* pdemo) psb->transform(btTransform(m,btVector3(0,4,0))); psb->scale(btVector3(2,2,2)); psb->setTotalMass(50,true); - psb->setPose(true,true); + psb->setPose(false,true); pdemo->getSoftDynamicsWorld()->addSoftBody(psb); } @@ -866,7 +876,371 @@ static void Init_Cutting1(SoftDemo* pdemo) pdemo->m_cutting=true; } -unsigned current_demo=0; +// +// Clusters +// + +// +static void Ctor_Gear(SoftDemo* pdemo,const btVector3& pos,btScalar speed) +{ +btTransform startTransform; +startTransform.setIdentity(); +startTransform.setOrigin(pos); +btCompoundShape* shape=new btCompoundShape(); +#if 1 +shape->addChildShape(btTransform(btQuaternion(0,0,0)),new btBoxShape(btVector3(5,1,6))); +shape->addChildShape(btTransform(btQuaternion(0,0,SIMD_HALF_PI)),new btBoxShape(btVector3(5,1,6))); +#else +shape->addChildShape(btTransform(btQuaternion(0,0,0)),new btCylinderShapeZ(btVector3(5,1,7))); +shape->addChildShape(btTransform(btQuaternion(0,0,SIMD_HALF_PI)),new btBoxShape(btVector3(4,1,8))); +#endif +btRigidBody* body=pdemo->localCreateRigidBody(10,startTransform,shape); +body->setFriction(1); +btDynamicsWorld* world=pdemo->getDynamicsWorld(); +btHingeConstraint* hinge=new btHingeConstraint(*body,btTransform::getIdentity()); +if(speed!=0) hinge->enableAngularMotor(true,speed,3); +world->addConstraint(hinge); +} + +// +static btSoftBody* Ctor_ClusterBunny(SoftDemo* pdemo,const btVector3& x,const btVector3& a) +{ +btSoftBody* psb=btSoftBodyHelpers::CreateFromTriMesh(pdemo->m_softBodyWorldInfo,gVerticesBunny,&gIndicesBunny[0][0],BUNNY_NUM_TRIANGLES); +btSoftBody::Material* pm=psb->appendMaterial(); +pm->m_kLST = 1; +pm->m_flags -= btSoftBody::fMaterial::DebugDraw; +psb->generateBendingConstraints(2,pm); +psb->m_cfg.piterations = 2; +psb->m_cfg.kDF = 1; +psb->m_cfg.collisions = btSoftBody::fCollision::CL_SS+ + btSoftBody::fCollision::CL_RS; +psb->randomizeConstraints(); +btMatrix3x3 m; +m.setEulerZYX(a.x(),a.y(),a.z()); +psb->transform(btTransform(m,x)); +psb->scale(btVector3(8,8,8)); +psb->setTotalMass(150,true); +psb->generateClusters(1); +pdemo->getSoftDynamicsWorld()->addSoftBody(psb); +return(psb); +} + +// +static btSoftBody* Ctor_ClusterTorus(SoftDemo* pdemo,const btVector3& x,const btVector3& a,const btVector3& s=btVector3(2,2,2)) +{ +btSoftBody* psb=btSoftBodyHelpers::CreateFromTriMesh(pdemo->m_softBodyWorldInfo,gVertices,&gIndices[0][0],NUM_TRIANGLES); +btSoftBody::Material* pm=psb->appendMaterial(); +pm->m_kLST = 1; +pm->m_flags -= btSoftBody::fMaterial::DebugDraw; +psb->generateBendingConstraints(2,pm); +psb->m_cfg.piterations = 2; +psb->m_cfg.collisions = btSoftBody::fCollision::CL_SS+ + btSoftBody::fCollision::CL_RS; +psb->randomizeConstraints(); +psb->scale(s); +psb->rotate(btQuaternion(a[0],a[1],a[2])); +psb->translate(x); +psb->setTotalMass(50,true); +psb->generateClusters(64); +pdemo->getSoftDynamicsWorld()->addSoftBody(psb); +return(psb); +} + +// +static struct MotorControl : btSoftBody::AJoint::IControl +{ + MotorControl() + { + goal=0; + maxtorque=0; + } +btScalar Speed(btSoftBody::AJoint*,btScalar current) + { + return(current+btMin(maxtorque,btMax(-maxtorque,goal-current))); + } +btScalar goal; +btScalar maxtorque; +} motorcontrol; + +// +struct SteerControl : btSoftBody::AJoint::IControl +{ + SteerControl(btScalar s) + { + angle=0; + sign=s; + } +void Prepare(btSoftBody::AJoint* joint) + { + joint->m_refs[0][0]=btCos(angle*sign); + joint->m_refs[0][2]=btSin(angle*sign); + } +btScalar Speed(btSoftBody::AJoint* joint,btScalar current) + { + return(motorcontrol.Speed(joint,current)); + } +btScalar angle; +btScalar sign; +}; + +static SteerControl steercontrol_f(+1); +static SteerControl steercontrol_r(-1); + +// +static void Init_ClusterDeform(SoftDemo* pdemo) +{ +btSoftBody* psb=Ctor_ClusterTorus(pdemo,btVector3(0,0,0),btVector3(SIMD_PI/2,0,SIMD_HALF_PI)); +psb->generateClusters(8); +psb->m_cfg.kDF=1; +} + +// +static void Init_ClusterCollide1(SoftDemo* pdemo) +{ +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, + 1+2+4+8,true); +btSoftBody::Material* pm=psb->appendMaterial(); +pm->m_kLST = 0.4; +pm->m_flags -= btSoftBody::fMaterial::DebugDraw; +psb->m_cfg.kDF = 1; +psb->m_cfg.kSRHR_CL = 1; +psb->m_cfg.kSR_SPLT_CL = 0; +psb->m_cfg.collisions = btSoftBody::fCollision::CL_SS+ + btSoftBody::fCollision::CL_RS; +psb->generateBendingConstraints(2,pm); +psb->setTotalMass(50); +psb->generateClusters(64); +pdemo->getSoftDynamicsWorld()->addSoftBody(psb); + +Ctor_RbUpStack(pdemo,10); +} + +// +static void Init_ClusterCollide2(SoftDemo* pdemo) +{ +struct Functor + { + static btSoftBody* Create(SoftDemo* pdemo,const btVector3& x,const btVector3& a) + { + btSoftBody* psb=btSoftBodyHelpers::CreateFromTriMesh(pdemo->m_softBodyWorldInfo,gVertices, + &gIndices[0][0], + NUM_TRIANGLES); + psb->generateBendingConstraints(2); + psb->m_cfg.piterations=2; + psb->m_cfg.kSSHR_CL =1; + psb->m_cfg.kSS_SPLT_CL =0; + psb->m_cfg.collisions= btSoftBody::fCollision::CL_SS+ + btSoftBody::fCollision::CL_RS; + psb->randomizeConstraints(); + btMatrix3x3 m; + m.setEulerZYX(a.x(),a.y(),a.z()); + psb->transform(btTransform(m,x)); + psb->scale(btVector3(2,2,2)); + psb->setTotalMass(50,true); + psb->generateClusters(16); + pdemo->getSoftDynamicsWorld()->addSoftBody(psb); + return(psb); + } + }; +for(int i=0;i<3;++i) + { + Functor::Create(pdemo,btVector3(3*i,2,0),btVector3(SIMD_PI/2*(1-(i&1)),SIMD_PI/2*(i&1),0)); + } +} + +// +static void Init_ClusterSocket(SoftDemo* pdemo) +{ +btSoftBody* psb=Ctor_ClusterTorus(pdemo,btVector3(0,0,0),btVector3(SIMD_PI/2,0,SIMD_HALF_PI)); +btRigidBody* prb=Ctor_BigPlate(pdemo,50,8); +psb->m_cfg.kDF=1; +btSoftBody::LJoint::Specs lj; +lj.position = btVector3(0,5,0); +psb->appendLinearJoint(lj,prb); +} + +// +static void Init_ClusterHinge(SoftDemo* pdemo) +{ +btSoftBody* psb=Ctor_ClusterTorus(pdemo,btVector3(0,0,0),btVector3(SIMD_PI/2,0,SIMD_HALF_PI)); +btRigidBody* prb=Ctor_BigPlate(pdemo,50,8); +psb->m_cfg.kDF=1; +btSoftBody::AJoint::Specs aj; +aj.axis = btVector3(0,0,1); +psb->appendAngularJoint(aj,prb); +} + +// +static void Init_ClusterCombine(SoftDemo* pdemo) +{ +const btVector3 sz(2,4,2); +btSoftBody* psb0=Ctor_ClusterTorus(pdemo,btVector3(0,8,0),btVector3(SIMD_PI/2,0,SIMD_HALF_PI),sz); +btSoftBody* psb1=Ctor_ClusterTorus(pdemo,btVector3(0,8,10),btVector3(SIMD_PI/2,0,SIMD_HALF_PI),sz); +btSoftBody* psbs[]={psb0,psb1}; +for(int j=0;j<2;++j) + { + psbs[j]->m_cfg.kDF=1; + psbs[j]->m_cfg.kDP=0; + psbs[j]->m_cfg.piterations=1; + psbs[j]->m_clusters[0]->m_matching = 0.05; + psbs[j]->m_clusters[0]->m_ndamping = 0.05; + } +btSoftBody::AJoint::Specs aj; +aj.axis = btVector3(0,0,1); +aj.icontrol = &motorcontrol; +psb0->appendAngularJoint(aj,psb1); + +btSoftBody::LJoint::Specs lj; +lj.position = btVector3(0,8,5); +psb0->appendLinearJoint(lj,psb1); +} + +// +static void Init_ClusterCar(SoftDemo* pdemo) +{ +const btVector3 origin(100,80,0); +const btQuaternion orientation(-SIMD_PI/2,0,0); +const btScalar widthf=8; +const btScalar widthr=9; +const btScalar length=8; +const btScalar height=4; +const btVector3 wheels[]= { + btVector3(+widthf,-height,+length), // Front left + btVector3(-widthf,-height,+length), // Front right + btVector3(+widthr,-height,-length), // Rear left + btVector3(-widthr,-height,-length), // Rear right + }; +btSoftBody* pa=Ctor_ClusterBunny(pdemo,btVector3(0,0,0),btVector3(0,0,0)); +btSoftBody* pfl=Ctor_ClusterTorus(pdemo,wheels[0],btVector3(0,0,SIMD_HALF_PI),btVector3(2,4,2)); +btSoftBody* pfr=Ctor_ClusterTorus(pdemo,wheels[1],btVector3(0,0,SIMD_HALF_PI),btVector3(2,4,2)); +btSoftBody* prl=Ctor_ClusterTorus(pdemo,wheels[2],btVector3(0,0,SIMD_HALF_PI),btVector3(2,5,2)); +btSoftBody* prr=Ctor_ClusterTorus(pdemo,wheels[3],btVector3(0,0,SIMD_HALF_PI),btVector3(2,5,2)); + +pfl->m_cfg.kDF = +pfr->m_cfg.kDF = +prl->m_cfg.kDF = +prr->m_cfg.kDF = 1; + +btSoftBody::LJoint::Specs lspecs; +lspecs.cfm = 1; +lspecs.erp = 1; +lspecs.position = btVector3(0,0,0); + +lspecs.position=wheels[0];pa->appendLinearJoint(lspecs,pfl); +lspecs.position=wheels[1];pa->appendLinearJoint(lspecs,pfr); +lspecs.position=wheels[2];pa->appendLinearJoint(lspecs,prl); +lspecs.position=wheels[3];pa->appendLinearJoint(lspecs,prr); + +btSoftBody::AJoint::Specs aspecs; +aspecs.cfm = 1; +aspecs.erp = 1; +aspecs.axis = btVector3(1,0,0); + +aspecs.icontrol = &steercontrol_f; +pa->appendAngularJoint(aspecs,pfl); +pa->appendAngularJoint(aspecs,pfr); + +aspecs.icontrol = &motorcontrol; +pa->appendAngularJoint(aspecs,prl); +pa->appendAngularJoint(aspecs,prr); + +pa->rotate(orientation); +pfl->rotate(orientation); +pfr->rotate(orientation); +prl->rotate(orientation); +prr->rotate(orientation); +pa->translate(origin); +pfl->translate(origin); +pfr->translate(origin); +prl->translate(origin); +prr->translate(origin); +pfl->m_cfg.piterations = +pfr->m_cfg.piterations = +prl->m_cfg.piterations = +prr->m_cfg.piterations = 1; +pfl->m_clusters[0]->m_matching = +pfr->m_clusters[0]->m_matching = +prl->m_clusters[0]->m_matching = +prr->m_clusters[0]->m_matching = 0.05; +pfl->m_clusters[0]->m_ndamping = +pfr->m_clusters[0]->m_ndamping = +prl->m_clusters[0]->m_ndamping = +prr->m_clusters[0]->m_ndamping = 0.05; + +Ctor_LinearStair(pdemo,btVector3(0,-8,0),btVector3(3,2,40),0,20); +Ctor_RbUpStack(pdemo,50); +pdemo->m_autocam=true; +} + +// +static void Init_ClusterRobot(SoftDemo* pdemo) +{ +struct Functor + { + static btSoftBody* CreateBall(SoftDemo* pdemo,const btVector3& pos) + { + btSoftBody* psb=btSoftBodyHelpers::CreateEllipsoid(pdemo->m_softBodyWorldInfo,pos,btVector3(1,1,1)*3,512); + psb->m_materials[0]->m_kLST = 0.45; + psb->m_cfg.kVC = 20; + psb->setTotalMass(50,true); + psb->setPose(true,false); + psb->generateClusters(1); + pdemo->getSoftDynamicsWorld()->addSoftBody(psb); + return(psb); + } + }; +const btVector3 base=btVector3(0,25,8); +btSoftBody* psb0=Functor::CreateBall(pdemo,base+btVector3(-8,0,0)); +btSoftBody* psb1=Functor::CreateBall(pdemo,base+btVector3(+8,0,0)); +btSoftBody* psb2=Functor::CreateBall(pdemo,base+btVector3(0,0,+8*btSqrt(2))); +const btVector3 ctr=(psb0->clusterCom(0)+psb1->clusterCom(0)+psb2->clusterCom(0))/3; +btCylinderShape* pshp=new btCylinderShape(btVector3(8,1,8)); +btRigidBody* prb=pdemo->localCreateRigidBody(50,btTransform(btQuaternion(0,0,0),ctr+btVector3(0,5,0)),pshp); +btSoftBody::LJoint::Specs ls; +ls.erp=0.5f; +ls.position=psb0->clusterCom(0);psb0->appendLinearJoint(ls,prb); +ls.position=psb1->clusterCom(0);psb1->appendLinearJoint(ls,prb); +ls.position=psb2->clusterCom(0);psb2->appendLinearJoint(ls,prb); + +btBoxShape* pbox=new btBoxShape(btVector3(20,1,40)); +btRigidBody* pgrn=pdemo->localCreateRigidBody(0,btTransform(btQuaternion(0,-SIMD_HALF_PI/2,0),btVector3(0,0,0)),pbox); + +pdemo->m_autocam=true; +} + +// +static void Init_ClusterStackSoft(SoftDemo* pdemo) +{ +for(int i=0;i<10;++i) + { + btSoftBody* psb=Ctor_ClusterTorus(pdemo,btVector3(0,-9+8.25*i,0),btVector3(0,0,0)); + psb->m_cfg.kDF=1; + } +} + +// +static void Init_ClusterStackMixed(SoftDemo* pdemo) +{ +for(int i=0;i<10;++i) + { + if((i+1)&1) + { + Ctor_BigPlate(pdemo,50,-9+4.25*i); + } + else + { + btSoftBody* psb=Ctor_ClusterTorus(pdemo,btVector3(0,-9+4.25*i,0),btVector3(0,0,0)); + psb->m_cfg.kDF=1; + } + } +} + +unsigned current_demo=18; void SoftDemo::clientResetScene() { @@ -880,6 +1254,12 @@ void SoftDemo::clientResetScene() { delete body->getMotionState(); } + while(m_dynamicsWorld->getNumConstraints()) + { + btTypedConstraint* pc=m_dynamicsWorld->getConstraint(0); + m_dynamicsWorld->removeConstraint(pc); + delete pc; + } btSoftBody* softBody = btSoftBody::upcast(obj); if (softBody) { @@ -913,10 +1293,19 @@ void SoftDemo::clientResetScene() Init_Bunny, Init_BunnyMatch, Init_Cutting1, + Init_ClusterDeform, + Init_ClusterCollide1, + Init_ClusterCollide2, + Init_ClusterSocket, + Init_ClusterHinge, + Init_ClusterCombine, + Init_ClusterCar, + Init_ClusterRobot, + Init_ClusterStackSoft, + Init_ClusterStackMixed, }; current_demo=current_demo%(sizeof(demofncs)/sizeof(demofncs[0])); - - + m_softBodyWorldInfo.air_density = (btScalar)1.2; m_softBodyWorldInfo.water_density = 0; m_softBodyWorldInfo.water_offset = 0; @@ -953,7 +1342,7 @@ void SoftDemo::renderme() } ps/=nps; if(m_autocam) - m_cameraTargetPosition+=(ps-m_cameraTargetPosition)*0.01; + m_cameraTargetPosition+=(ps-m_cameraTargetPosition)*0.05; else m_cameraTargetPosition=btVector3(0,0,0); /* Anm */ @@ -1047,6 +1436,7 @@ void SoftDemo::renderme() 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); } + // DemoApplication::renderme(); } @@ -1055,10 +1445,15 @@ void SoftDemo::keyboardCallback(unsigned char key, int x, int y) { switch(key) { + case 'n': motorcontrol.maxtorque=10;motorcontrol.goal+=1;break; + case 'm': motorcontrol.maxtorque=10;motorcontrol.goal-=1;break; + case '4': steercontrol_f.angle+=0.1;steercontrol_r.angle+=0.1;break; + case '6': steercontrol_f.angle-=0.1;steercontrol_r.angle-=0.1;break; case ']': ++current_demo;clientResetScene();break; case '[': --current_demo;clientResetScene();break; case ',': m_raycast=!m_raycast;break; case ';': m_autocam=!m_autocam;break; + case 'c': getSoftDynamicsWorld()->setDrawFlags(getSoftDynamicsWorld()->getDrawFlags()^fDrawFlags::Clusters);break; case '`': { btSoftBodyArray& sbs=getSoftDynamicsWorld()->getSoftBodyArray(); @@ -1367,3 +1762,4 @@ void SoftDemo::exitPhysics() + diff --git a/src/BulletSoftBody/btSoftBody.cpp b/src/BulletSoftBody/btSoftBody.cpp index 6039c0f28..0fa347d2f 100644 --- a/src/BulletSoftBody/btSoftBody.cpp +++ b/src/BulletSoftBody/btSoftBody.cpp @@ -14,1112 +14,7 @@ subject to the following restrictions: */ ///btSoftBody implementation by Nathanael Presson -#include "btSoftBody.h" -#include "LinearMath/btQuickprof.h" -#include "BulletCollision/BroadphaseCollision/btBroadphaseInterface.h" -#include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h" - -// -// btSymMatrix -// -template -struct btSymMatrix -{ - btSymMatrix() : dim(0) {} - btSymMatrix(int n,const T& init=T()) { resize(n,init); } -void resize(int n,const T& init=T()) { dim=n;store.resize((n*(n+1))/2,init); } -T& operator()(int c,int r) - { - if(c>r) btSwap(c,r); - btAssert(rr) btSwap(c,r); - btAssert(r store; -int dim; -}; - - -// -// Collision shape -// - -///btSoftBodyCollisionShape is work-in-progress collision shape for softbodies -class btSoftBodyCollisionShape : public btConcaveShape -{ -public: - btSoftBody* m_body; - - btSoftBodyCollisionShape(btSoftBody* backptr); - - 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 - { - /* t should be identity, but better be safe than...fast? */ - const btVector3 mins=m_body->m_bounds[0]; - const btVector3 maxs=m_body->m_bounds[1]; - const btVector3 crns[]={t*btVector3(mins.x(),mins.y(),mins.z()), - t*btVector3(maxs.x(),mins.y(),mins.z()), - t*btVector3(maxs.x(),maxs.y(),mins.z()), - t*btVector3(mins.x(),maxs.y(),mins.z()), - t*btVector3(mins.x(),mins.y(),maxs.z()), - t*btVector3(maxs.x(),mins.y(),maxs.z()), - t*btVector3(maxs.x(),maxs.y(),maxs.z()), - t*btVector3(mins.x(),maxs.y(),maxs.z())}; - aabbMin=aabbMax=crns[0]; - for(int i=1;i<8;++i) - { - aabbMin.setMin(crns[i]); - aabbMax.setMax(crns[i]); - } - } - - virtual int getShapeType() const - { - return SOFTBODY_SHAPE_PROXYTYPE; - } - virtual void setLocalScaling(const btVector3& /*scaling*/) - { - ///na - btAssert(0); - } - virtual const btVector3& getLocalScaling() const - { - static const btVector3 dummy(1,1,1); - return dummy; - } - virtual void calculateLocalInertia(btScalar /*mass*/,btVector3& /*inertia*/) const - { - ///not yet - btAssert(0); - } - virtual const char* getName()const - { - return "SoftBody"; - } - -}; - -btSoftBodyCollisionShape::btSoftBodyCollisionShape(btSoftBody* backptr) -{ -m_body=backptr; -} - -btSoftBodyCollisionShape::~btSoftBodyCollisionShape() -{ - -} - -void btSoftBodyCollisionShape::processAllTriangles(btTriangleCallback* /*callback*/,const btVector3& /*aabbMin*/,const btVector3& /*aabbMax*/) const -{ - //not yet - btAssert(0); -} - -// -// Helpers -// - -// - -// -template -static inline void ZeroInitialize(T& value) -{ -static const T zerodummy; -value=zerodummy; -} -// -template -static inline bool CompLess(const T& a,const T& b) -{ return(a -static inline bool CompGreater(const T& a,const T& b) -{ return(a>b); } -// -template -static inline T Lerp(const T& a,const T& b,btScalar t) -{ return(a+(b-a)*t); } -// -template -static inline T InvLerp(const T& a,const T& b,btScalar t) -{ return((b+a*t-b*t)/(a*b)); } -// -static inline btMatrix3x3 Lerp( const btMatrix3x3& a, - const btMatrix3x3& b, - btScalar t) -{ -btMatrix3x3 r; -r[0]=Lerp(a[0],b[0],t); -r[1]=Lerp(a[1],b[1],t); -r[2]=Lerp(a[2],b[2],t); -return(r); -} -// -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 Cube(const T& x) -{ return(x*x*x); } -// -template -static inline T Sign(const T& x) -{ return((T)(x<0?-1:+1)); } -// -template -static inline bool SameSign(const T& x,const T& y) -{ return((x*y)>0); } -// -static inline btMatrix3x3 ScaleAlongAxis(const btVector3& a,btScalar s) -{ - const btScalar xx=a.x()*a.x(); - 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=m; - for(int i=0;iaccuracy) det=ndet; else break; - } - /* Final orthogonalization */ - q[0]=q[0].normalized(); - q[2]=cross(q[0],q[1]).normalized(); - q[1]=cross(q[2],q[0]).normalized(); - /* Compute 'S' */ - 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)); -} -// -static inline void ProjectOrigin( const btVector3& a, - const btVector3& b, - btVector3& prj, - btScalar& sqd) -{ -const btVector3 d=b-a; -const btScalar m2=d.length2(); -if(m2>SIMD_EPSILON) - { - const btScalar t=Clamp(-dot(a,d)/m2,0,1); - const btVector3 p=a+d*t; - const btScalar l2=p.length2(); - if(l2SIMD_EPSILON) - { - const btVector3 n=q/btSqrt(m2); - const btScalar k=dot(a,n); - const btScalar k2=k*k; - if(k20)&& - (dot(cross(b-p,c-p),q)>0)&& - (dot(cross(c-p,a-p),q)>0)) - { - prj=p; - sqd=k2; - } - else - { - ProjectOrigin(a,b,prj,sqd); - ProjectOrigin(b,c,prj,sqd); - ProjectOrigin(c,a,prj,sqd); - } - } - } -} -// -template -static inline T BaryEval( const T& a, - const T& b, - const T& c, - const btVector3& coord) -{ - return(a*coord.x()+b*coord.y()+c*coord.z()); -} -// -static inline btVector3 BaryCoord( const btVector3& a, - const btVector3& b, - const btVector3& c, - const btVector3& p) -{ -const btScalar w[]={ cross(a-p,b-p).length(), - cross(b-p,c-p).length(), - cross(c-p,a-p).length()}; -const btScalar isum=1/(w[0]+w[1]+w[2]); -return(btVector3(w[1]*isum,w[2]*isum,w[0]*isum)); -} - -// -static btScalar ImplicitSolve( btSoftBody::ImplicitFn* fn, - const btVector3& a, - const btVector3& b, - const btScalar accuracy, - const int maxiterations=256) -{ -btScalar span[2]={0,1}; -btScalar values[2]={fn->Eval(a),fn->Eval(b)}; -if(values[0]>values[1]) - { - btSwap(span[0],span[1]); - btSwap(values[0],values[1]); - } -if(values[0]>-accuracy) return(-1); -if(values[1]<+accuracy) return(-1); -for(int i=0;iEval(Lerp(a,b,t)); - if((t<=0)||(t>=1)) break; - if(btFabs(v)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]; - int i,ni; - - for(i=0,ni=psb->m_nodes.size();im_nodes[i].m_leaf) - { - psb->m_nodes[i].m_leaf->data=*(void**)&i; - } - } - for(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(i=0,ni=psb->m_faces.size();im_faces[i].m_n[0]=PTR2IDX(psb->m_faces[i].m_n[0],base); - psb->m_faces[i].m_n[1]=PTR2IDX(psb->m_faces[i].m_n[1],base); - psb->m_faces[i].m_n[2]=PTR2IDX(psb->m_faces[i].m_n[2],base); - if(psb->m_faces[i].m_leaf) - { - psb->m_faces[i].m_leaf->data=*(void**)&i; - } - } - for(i=0,ni=psb->m_anchors.size();im_anchors[i].m_node=PTR2IDX(psb->m_anchors[i].m_node,base); - } - for(i=0,ni=psb->m_notes.size();im_notes[i].m_rank;++j) - { - psb->m_notes[i].m_nodes[j]=PTR2IDX(psb->m_notes[i].m_nodes[j],base); - } - } -#undef PTR2IDX -} - -// -static void IndicesToPointers(btSoftBody* psb,const int* map=0) -{ -#define IDX2PTR(_p_,_b_) map?(&(_b_)[map[(((char*)_p_)-(char*)0)]]): \ - (&(_b_)[(((char*)_p_)-(char*)0)]) - btSoftBody::Node* base=&psb->m_nodes[0]; - int i,ni; - - for(i=0,ni=psb->m_nodes.size();im_nodes[i].m_leaf) - { - psb->m_nodes[i].m_leaf->data=&psb->m_nodes[i]; - } - } - for(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(i=0,ni=psb->m_faces.size();im_faces[i].m_n[0]=IDX2PTR(psb->m_faces[i].m_n[0],base); - psb->m_faces[i].m_n[1]=IDX2PTR(psb->m_faces[i].m_n[1],base); - psb->m_faces[i].m_n[2]=IDX2PTR(psb->m_faces[i].m_n[2],base); - if(psb->m_faces[i].m_leaf) - { - psb->m_faces[i].m_leaf->data=&psb->m_faces[i]; - } - } - for(i=0,ni=psb->m_anchors.size();im_anchors[i].m_node=IDX2PTR(psb->m_anchors[i].m_node,base); - } - for(i=0,ni=psb->m_notes.size();im_notes[i].m_rank;++j) - { - psb->m_notes[i].m_nodes[j]=IDX2PTR(psb->m_notes[i].m_nodes[j],base); - } - } -#undef IDX2PTR -} - -// -static inline btDbvtVolume VolumeOf( const btSoftBody::Face& f, - btScalar margin) -{ -const btVector3* pts[]={ &f.m_n[0]->m_x, - &f.m_n[1]->m_x, - &f.m_n[2]->m_x}; -btDbvtVolume vol=btDbvtVolume::FromPoints(pts,3); -vol.Expand(btVector3(margin,margin,margin)); -return(vol); -} - -// -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); - 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); -} - -// -// Private implementation -// - -struct RayCaster : btDbvt::ICollide - { - btVector3 o; - btVector3 d; - btScalar mint; - btSoftBody::Face* face; - int tests; - RayCaster(const btVector3& org,const btVector3& dir,btScalar mxt) - { - o = org; - d = dir; - mint = mxt; - face = 0; - tests = 0; - } - void Process(const btDbvtNode* leaf) - { - btSoftBody::Face& f=*(btSoftBody::Face*)leaf->data; - const btScalar t=RayTriangle( o,d, - f.m_n[0]->m_x, - f.m_n[1]->m_x, - f.m_n[2]->m_x, - mint); - if((t>0)&&(tm_fdbvt.empty()) - {/* Full search */ - for(int i=0,ni=psb->m_faces.size();im_faces[i]; - const btScalar t=RayTriangle( org,dir, - f.m_n[0]->m_x, - f.m_n[1]->m_x, - f.m_n[2]->m_x, - mint); - if(t>0) - { - ++cnt; - if(!bcountonly) - { - feature=btSoftBody::eFeature::Face; - index=i; - mint=t; - } - } - } - } - else - {/* Use dbvt */ - RayCaster collider(org,dir,mint); - btDbvt::collideRAY(psb->m_fdbvt.m_root,org,dir,collider); - if(collider.face) - { - mint=collider.mint; - feature=btSoftBody::eFeature::Face; - index=(int)(collider.face-&psb->m_faces[0]); - cnt=1; - } - } - return(cnt); -} - -// -static void InitializeFaceTree(btSoftBody* psb) -{ -psb->m_fdbvt.clear(); -for(int i=0;im_faces.size();++i) - { - btSoftBody::Face& f=psb->m_faces[i]; - f.m_leaf=psb->m_fdbvt.insert(VolumeOf(f,0),&f); - } -} - -// -static btVector3 EvaluateCom(btSoftBody* psb) -{ -btVector3 com(0,0,0); -if(psb->m_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 EvaluateMedium( const btSoftBody::btSoftBodyWorldInfo* wfi, - const btVector3& x, - btSoftBody::sMedium& medium) -{ - medium.m_velocity = btVector3(0,0,0); - medium.m_pressure = 0; - medium.m_density = wfi->air_density; - if(wfi->water_density>0) - { - const btScalar depth=-(dot(x,wfi->water_normal)+wfi->water_offset); - if(depth>0) - { - medium.m_density = wfi->water_density; - medium.m_pressure = depth*wfi->water_density*wfi->m_gravity.length(); - } - } -} - -// -static bool CheckContact( btSoftBody* psb, - btRigidBody* prb, - const btVector3& x, - btScalar margin, - btSoftBody::sCti& cti) -{ - btVector3 nrm; - btCollisionShape* shp=prb->getCollisionShape(); - const btTransform& wtr=prb->getInterpolationWorldTransform(); - btScalar dst=psb->m_worldInfo->m_sparsesdf.Evaluate( wtr.invXform(x), - shp, - nrm, - margin); - if(dst<0) - { - cti.m_body = prb; - cti.m_normal = wtr.getBasis()*nrm; - cti.m_offset = -dot( cti.m_normal, - x-cti.m_normal*dst); - return(true); - } - return(false); -} - -// -static void UpdateNormals(btSoftBody* psb) -{ - const btVector3 zv(0,0,0); - int i,ni; - - for(i=0,ni=psb->m_nodes.size();im_nodes[i].m_n=zv; - } - for(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(i=0,ni=psb->m_nodes.size();im_nodes[i].m_n.normalize(); - } -} - -// -static void UpdateBounds(btSoftBody* psb) -{ - if(psb->m_ndbvt.m_root) - { - const btVector3& mins=psb->m_ndbvt.m_root->volume.Mins(); - const btVector3& maxs=psb->m_ndbvt.m_root->volume.Maxs(); - const btScalar csm=psb->getCollisionShape()->getMargin(); - const btVector3 mrg=btVector3( csm, - csm, - csm)*1; // ??? to investigate... - psb->m_bounds[0]=mins-mrg; - psb->m_bounds[1]=maxs+mrg; - if(0!=psb->getBroadphaseHandle()) - { - psb->m_worldInfo->m_broadphase->setAabb(psb->getBroadphaseHandle(), - psb->m_bounds[0], - psb->m_bounds[1], - psb->m_worldInfo->m_dispatcher); - } - } - else - { - psb->m_bounds[0]= - psb->m_bounds[1]=btVector3(0,0,0); - } -} - - -// -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); - pose.m_rot=r; - pose.m_scl=pose.m_aqq*r.transpose()*Apq; - if(psb->m_cfg.maxvolume>1) - { - const btScalar idet=Clamp( 1/pose.m_scl.determinant(), - 1,psb->m_cfg.maxvolume); - pose.m_scl=Mul(pose.m_scl,idet); - } - - } -} - -// -static void UpdateConstants(btSoftBody* psb) -{ - int i,ni; - - /* Links */ - for(i=0,ni=psb->m_links.size();im_links[i]; - btSoftBody::Material& m=*l.m_material; - l.m_rl = (l.m_n[0]->m_x-l.m_n[1]->m_x).length(); - l.m_c0 = (l.m_n[0]->m_im+l.m_n[1]->m_im)/m.m_kLST; - l.m_c1 = l.m_rl*l.m_rl; - } - /* Faces */ - for(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(i=0,ni=psb->m_nodes.size();im_nodes[i].m_area = 0; - } - for(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(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) -{ - BT_PROFILE("SoftBody applyForces"); - int i,ni; - 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.aeromodelm_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(i=0,ni=psb->m_nodes.size();im_nodes[i]; - if(n.m_im>0) - { - if(use_medium) - { - EvaluateMedium(psb->m_worldInfo,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) - { - 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); - } - } - } - /* Per face forces */ - for(i=0,ni=psb->m_faces.size();im_faces[i]; - if(as_faero) - { - const btVector3 v=(f.m_n[0]->m_v+f.m_n[1]->m_v+f.m_n[2]->m_v)/3; - const btVector3 x=(f.m_n[0]->m_x+f.m_n[1]->m_x+f.m_n[2]->m_x)/3; - EvaluateMedium(psb->m_worldInfo,x,medium); - const btVector3 rel_v=v-medium.m_velocity; - const btScalar rel_v2=rel_v.length2(); - if(rel_v2>SIMD_EPSILON) - { - 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 kst) -{ - const btScalar kAHR=psb->m_cfg.kAHR*kst; - const btScalar dt=psb->m_sst.sdt; - for(int i=0,ni=psb->m_anchors.size();im_anchors[i]; - const btTransform& t=a.m_body->getInterpolationWorldTransform(); - btSoftBody::Node& n=*a.m_node; - const btVector3 wa=t*a.m_local; - const btVector3 va=a.m_body->getVelocityInLocalPoint(a.m_c1)*dt; - const btVector3 vb=n.m_x-n.m_q; - const btVector3 vr=(va-vb)+(wa-n.m_x)*kAHR; - const btVector3 impulse=a.m_c0*vr; - n.m_x+=impulse*a.m_c2; - a.m_body->applyImpulse(-impulse,a.m_c1); - } -} - -// -static void PSolve_RContacts(btSoftBody* psb,btScalar kst) -{ - const btScalar dt=psb->m_sst.sdt; - const btScalar mrg=psb->getCollisionShape()->getMargin(); - for(int i=0,ni=psb->m_rcontacts.size();im_rcontacts[i]; - const btSoftBody::sCti& cti=c.m_cti; - const btVector3 va=cti.m_body->getVelocityInLocalPoint(c.m_c1)*dt; - const btVector3 vb=c.m_node->m_x-c.m_node->m_q; - const btVector3 vr=vb-va; - const btScalar dn=dot(vr,cti.m_normal); - if(dn<=SIMD_EPSILON) - { - const btScalar dp=btMin(dot(c.m_node->m_x,cti.m_normal)+cti.m_offset,mrg); - const btVector3 fv=vr-cti.m_normal*dn; - const btVector3 impulse=c.m_c0*((vr-fv*c.m_c3+cti.m_normal*(dp*c.m_c4))*kst); - c.m_node->m_x-=impulse*c.m_c2; - c.m_cti.m_body->applyImpulse(impulse,c.m_c1); - } - } -} - -// -static void PSolve_SContacts(btSoftBody* psb,btScalar) -{ -for(int i=0,ni=psb->m_scontacts.size();im_scontacts[i]; - const btVector3& nr=c.m_normal; - btSoftBody::Node& n=*c.m_node; - btSoftBody::Face& f=*c.m_face; - const btVector3 p=BaryEval( f.m_n[0]->m_x, - f.m_n[1]->m_x, - f.m_n[2]->m_x, - c.m_weights); - const btVector3 q=BaryEval( f.m_n[0]->m_q, - f.m_n[1]->m_q, - f.m_n[2]->m_q, - c.m_weights); - const btVector3 vr=(n.m_x-n.m_q)-(p-q); - btVector3 corr(0,0,0); - if(dot(vr,nr)<0) - { - const btScalar j=c.m_margin-(dot(nr,n.m_x)-dot(nr,p)); - corr+=c.m_normal*j; - } - corr -= ProjectOnPlane(vr,nr)*c.m_friction; - n.m_x += corr*c.m_cfm[0]; - f.m_n[0]->m_x -= corr*(c.m_cfm[1]*c.m_weights.x()); - f.m_n[1]->m_x -= corr*(c.m_cfm[1]*c.m_weights.y()); - f.m_n[2]->m_x -= corr*(c.m_cfm[1]*c.m_weights.z()); - } -} - -// -static void PSolve_Links(btSoftBody* psb,btScalar kst) -{ - for(int i=0,ni=psb->m_links.size();im_links[i]; - if(l.m_c0>0) - { - btSoftBody::Node& a=*l.m_n[0]; - btSoftBody::Node& b=*l.m_n[1]; - const btVector3 del=b.m_x-a.m_x; - const btScalar len=del.length2(); - const btScalar 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); - } - } -} - -// -static void VSolve_Links(btSoftBody* psb,btScalar kst) -{ -for(int i=0,ni=psb->m_links.size();im_links[i]; - btSoftBody::Node** n=l.m_n; - const btScalar j=-dot(l.m_c3,n[0]->m_v-n[1]->m_v)*l.m_c2*kst; - n[0]->m_v+= l.m_c3*(j*n[0]->m_im); - n[1]->m_v-= l.m_c3*(j*n[1]->m_im); - } -} - -// -static void (* const VSolvers[])(btSoftBody*,btScalar)= { - VSolve_Links, - }; - -// -static void (* const PSolvers[])(btSoftBody*,btScalar)= { - PSolve_Links, - PSolve_Anchors, - PSolve_RContacts, - PSolve_SContacts, - }; - -// -// btSoftBody -// +#include "btSoftBodyInternals.h" // btSoftBody::btSoftBody(btSoftBody::btSoftBodyWorldInfo* worldInfo,int node_count, const btVector3* x, const btScalar* m) @@ -1140,11 +35,18 @@ btSoftBody::btSoftBody(btSoftBody::btSoftBodyWorldInfo* worldInfo,int node_count m_cfg.kKHR = (btScalar)0.1; m_cfg.kSHR = (btScalar)1.0; m_cfg.kAHR = (btScalar)0.7; + m_cfg.kSRHR_CL = (btScalar)0.1; + m_cfg.kSKHR_CL = (btScalar)1; + m_cfg.kSSHR_CL = (btScalar)0.5; + m_cfg.kSR_SPLT_CL = (btScalar)0.5; + m_cfg.kSK_SPLT_CL = (btScalar)0.5; + m_cfg.kSS_SPLT_CL = (btScalar)0.5; m_cfg.maxvolume = (btScalar)1; m_cfg.timescale = 1; m_cfg.viterations = 0; - m_cfg.piterations = 1; + m_cfg.piterations = 1; m_cfg.diterations = 0; + m_cfg.citerations = 4; m_cfg.collisions = fCollision::Default; m_pose.m_bvolume = false; m_pose.m_bframe = false; @@ -1183,7 +85,7 @@ btSoftBody::btSoftBody(btSoftBody::btSoftBodyWorldInfo* worldInfo,int node_count n.m_leaf = m_ndbvt.insert(btDbvtVolume::FromCR(n.m_x,margin),&n); n.m_material= pm; } - UpdateBounds(this); + updateBounds(); } @@ -1194,6 +96,7 @@ btSoftBody::~btSoftBody() //for now, delete the internal shape delete m_collisionShape; for(int i=0;igetMargin(); m_nodes.push_back(Node()); @@ -1378,6 +281,9 @@ void btSoftBody::appendFace(int node0,int node1,int node2,Material* mat) { appendFace(-1,mat); Face& f=m_faces[m_faces.size()-1]; + btAssert(node0!=node1); + btAssert(node1!=node2); + btAssert(node2!=node0); f.m_n[0] = &m_nodes[node0]; f.m_n[1] = &m_nodes[node1]; f.m_n[2] = &m_nodes[node2]; @@ -1398,6 +304,59 @@ void btSoftBody::appendAnchor(int node,btRigidBody* body) m_anchors.push_back(a); } +// +void btSoftBody::appendLinearJoint(const LJoint::Specs& specs,Cluster* body0,Body body1) +{ +LJoint* pj = new(btAlignedAlloc(sizeof(LJoint),16)) LJoint(); +pj->m_bodies[0] = body0; +pj->m_bodies[1] = body1; +pj->m_refs[0] = pj->m_bodies[0].xform().inverse()*specs.position; +pj->m_refs[1] = pj->m_bodies[1].xform().inverse()*specs.position; +pj->m_cfm = specs.cfm; +pj->m_erp = specs.erp; +pj->m_split = specs.split; +m_joints.push_back(pj); +} + +// +void btSoftBody::appendLinearJoint(const LJoint::Specs& specs,Body body) +{ +appendLinearJoint(specs,m_clusters[0],body); +} + +// +void btSoftBody::appendLinearJoint(const LJoint::Specs& specs,btSoftBody* body) +{ +appendLinearJoint(specs,m_clusters[0],body->m_clusters[0]); +} + +// +void btSoftBody::appendAngularJoint(const AJoint::Specs& specs,Cluster* body0,Body body1) +{ +AJoint* pj = new(btAlignedAlloc(sizeof(AJoint),16)) AJoint(); +pj->m_bodies[0] = body0; +pj->m_bodies[1] = body1; +pj->m_refs[0] = pj->m_bodies[0].xform().inverse().getBasis()*specs.axis; +pj->m_refs[1] = pj->m_bodies[1].xform().inverse().getBasis()*specs.axis; +pj->m_cfm = specs.cfm; +pj->m_erp = specs.erp; +pj->m_split = specs.split; +pj->m_icontrol = specs.icontrol; +m_joints.push_back(pj); +} + +// +void btSoftBody::appendAngularJoint(const AJoint::Specs& specs,Body body) +{ +appendAngularJoint(specs,m_clusters[0],body); +} + +// +void btSoftBody::appendAngularJoint(const AJoint::Specs& specs,btSoftBody* body) +{ +appendAngularJoint(specs,m_clusters[0],body->m_clusters[0]); +} + // void btSoftBody::addForce(const btVector3& force) { @@ -1457,16 +416,13 @@ btScalar btSoftBody::getTotalMass() const // void btSoftBody::setTotalMass(btScalar mass,bool fromfaces) { - int i; if(fromfaces) { - - - for(i=0;im_x, @@ -1477,14 +433,14 @@ void btSoftBody::setTotalMass(btScalar mass,bool fromfaces) f.m_n[j]->m_im+=twicearea; } } - for(i=0;i0 ? @@ -1572,9 +526,9 @@ void btSoftBody::setPose(bool bvolume,bool bframe) kmass/tmass; } /* Pos */ - const btVector3 com=EvaluateCom(this); + const btVector3 com=evaluateCom(); m_pose.m_pos.resize(m_nodes.size()); - for(i=0,ni=m_nodes.size();im_nodes.size();im_nodes[i]->m_x*cluster->m_masses[i]; + } +return(com*cluster->m_imass); +} + +// +btVector3 btSoftBody::clusterCom(int cluster) const +{ +return(clusterCom(m_clusters[cluster])); +} + +// +btVector3 btSoftBody::clusterVelocity(const Cluster* cluster,const btVector3& rpos) +{ +return(cluster->m_lv+cross(cluster->m_av,rpos)); +} + +// +void btSoftBody::clusterVImpulse(Cluster* cluster,const btVector3& rpos,const btVector3& impulse) +{ +const btVector3 li=cluster->m_imass*impulse; +const btVector3 ai=cluster->m_invwi*cross(rpos,impulse); +cluster->m_vimpulses[0]+=li;cluster->m_lv+=li; +cluster->m_vimpulses[1]+=ai;cluster->m_av+=ai; +cluster->m_nvimpulses++; +} + +// +void btSoftBody::clusterDImpulse(Cluster* cluster,const btVector3& rpos,const btVector3& impulse) +{ +const btVector3 li=cluster->m_imass*impulse; +const btVector3 ai=cluster->m_invwi*cross(rpos,impulse); +cluster->m_dimpulses[0]+=li; +cluster->m_dimpulses[1]+=ai; +cluster->m_ndimpulses++; +} + +// +void btSoftBody::clusterImpulse(Cluster* cluster,const btVector3& rpos,const Impulse& impulse) +{ +if(impulse.m_asVelocity) clusterVImpulse(cluster,rpos,impulse.m_velocity); +if(impulse.m_asDrift) clusterDImpulse(cluster,rpos,impulse.m_drift); +} + +// +void btSoftBody::clusterVAImpulse(Cluster* cluster,const btVector3& impulse) +{ +const btVector3 ai=cluster->m_invwi*impulse; +cluster->m_vimpulses[1]+=ai;cluster->m_av+=ai; +cluster->m_nvimpulses++; +} + +// +void btSoftBody::clusterDAImpulse(Cluster* cluster,const btVector3& impulse) +{ +const btVector3 ai=cluster->m_invwi*impulse; +cluster->m_dimpulses[1]+=ai; +cluster->m_ndimpulses++; +} + +// +void btSoftBody::clusterAImpulse(Cluster* cluster,const Impulse& impulse) +{ +if(impulse.m_asVelocity) clusterVAImpulse(cluster,impulse.m_velocity); +if(impulse.m_asDrift) clusterDAImpulse(cluster,impulse.m_drift); +} + +// +void btSoftBody::clusterDCImpulse(Cluster* cluster,const btVector3& impulse) +{ +cluster->m_dimpulses[0]+=impulse*cluster->m_imass; +cluster->m_ndimpulses++; +} + // int btSoftBody::generateBendingConstraints(int distance,Material* mat) { @@ -1625,10 +665,7 @@ int btSoftBody::generateBendingConstraints(int distance,Material* mat) const unsigned inf=(~(unsigned)0)>>1; unsigned* adj=new unsigned[n*n]; #define IDX(_x_,_y_) ((_y_)*n+(_x_)) - - int i,j; - - for( j=0;jm_leaf) m_cdbvt.remove(m_clusters[i]->m_leaf); + btAlignedFree(m_clusters[i]); + } +m_clusters.resize(btMin(k,m_nodes.size())); +for(int i=0;im_collide= true; + } +k=m_clusters.size(); +if(k>0) + { + /* Initialize */ + btAlignedObjectArray centers; + btVector3 cog(0,0,0); + for(int i=0;im_nodes.push_back(&m_nodes[i]); + } + cog/=(btScalar)m_nodes.size(); + centers.resize(k,cog); + /* Iterate */ + const btScalar slope=16; + bool changed; + int iterations=0; + do { + const btScalar w=2-btMin(1,iterations/slope); + changed=false; + iterations++; + for(int i=0;im_nodes.size();++j) + { + c+=m_clusters[i]->m_nodes[j]->m_x; + } + if(m_clusters[i]->m_nodes.size()) + { + c /= (btScalar)m_clusters[i]->m_nodes.size(); + c = centers[i]+(c-centers[i])*w; + changed |= (c-centers[i]).length2()>SIMD_EPSILON; + centers[i] = c; + m_clusters[i]->m_nodes.resize(0); + } + } + for(int i=0;im_nodes.push_back(&m_nodes[i]); + } + } while(changed&&(iterations cids; + cids.resize(m_nodes.size(),-1); + for(int i=0;im_nodes.size();++j) + { + cids[int(m_clusters[i]->m_nodes[j]-&m_nodes[0])]=i; + } + } + for(int i=0;im_nodes.findLinearSearch(&m_nodes[kid])==m_clusters[cid]->m_nodes.size()) + { + m_clusters[cid]->m_nodes.push_back(&m_nodes[kid]); + } + } + } + } + } + /* Master */ + if(m_clusters.size()>1) + { + Cluster* pmaster=new(btAlignedAlloc(sizeof(Cluster),16)) Cluster(); + pmaster->m_collide = false; + pmaster->m_nodes.reserve(m_nodes.size()); + for(int i=0;im_nodes.push_back(&m_nodes[i]); + m_clusters.push_back(pmaster); + btSwap(m_clusters[0],m_clusters[m_clusters.size()-1]); + } + /* Terminate */ + for(int i=0;im_nodes.size()==0) + { + btAlignedFree(m_clusters[i]); + btSwap(m_clusters[i],m_clusters[m_clusters.size()-1]); + m_clusters.pop_back(); + --i; + } + } + + initializeClusters(); + updateClusters(); + return(m_clusters.size()); + } +return(0); +} + // void btSoftBody::refine(ImplicitFn* ifn,btScalar accurary,bool cut) { -Node* nbase = &m_nodes[0]; +const Node* nbase(&m_nodes[0]); int ncount(m_nodes.size()); btSymMatrix edges(ncount,-2); int newnodes=0; -int i,ni; - /* Filter out */ -for(i=0;i=pcount)||(btFabs(ifn->Eval(x))Eval(n[0]->m_x) ranks; btAlignedObjectArray todelete; ranks.resize(nnodes,0); - for(i=0,ni=m_links.size();i=0;--i) + for(int i=nnodes-1;i>=0;--i) { if(!ranks[i]) todelete.push_back(i); } @@ -1947,16 +1107,6 @@ if(cut) m_bUpdateRtCst=true; } -static inline int MatchEdge( const btSoftBody::Node* a, - const btSoftBody::Node* b, - const btSoftBody::Node* ma, - const btSoftBody::Node* mb) -{ -if((a==ma)&&(b==mb)) return(0); -if((a==mb)&&(b==ma)) return(1); -return(-1); -} - // bool btSoftBody::cutLink(const Node* node0,const Node* node1,btScalar position) { @@ -1979,9 +1129,7 @@ Node* pn[2]={ &m_nodes[m_nodes.size()-2], &m_nodes[m_nodes.size()-1]}; pn[0]->m_v=v; pn[1]->m_v=v; -int i,ni; - -for(i=0,ni=m_links.size();im_gravity*m_sst.sdt); - ApplyForces(this,m_sst.sdt); + applyForces(); /* Integrate */ for(int i=0,ni=m_nodes.size();i0)) { + const btMatrix3x3 posetrs=m_pose.m_rot; for(int i=0,ni=m_nodes.size();i0) { - const btVector3 x=m_pose.m_rot*m_pose.m_pos[i]+m_pose.m_com; + const btVector3 x=posetrs*m_pose.m_pos[i]+m_pose.m_com; n.m_x=Lerp(n.m_x,x,m_cfg.kMT); } } @@ -2135,22 +1290,23 @@ void btSoftBody::predictMotion(btScalar dt) /* Optimize dbvt's */ m_ndbvt.optimizeIncremental(1); m_fdbvt.optimizeIncremental(1); + m_cdbvt.optimizeIncremental(1); } // void btSoftBody::solveConstraints() { - int i,ni; - +/* Apply clusters */ +applyClusters(false); /* Prepare links */ -for(i=0,ni=m_links.size();im_x-l.m_n[0]->m_x; + l.m_c3 = l.m_n[1]->m_q-l.m_n[0]->m_q; l.m_c2 = 1/(l.m_c3.length2()*l.m_c0); } /* Prepare anchors */ -for(i=0,ni=m_anchors.size();igetWorldTransform().getBasis()*a.m_local; @@ -2165,13 +1321,13 @@ for(i=0,ni=m_anchors.size();i0) - { + { /* Solve */ for(int isolve=0;isolve0) { for(int isolve=0;isolve0) if(m_cfg.diterations>0) { const btScalar vcf=m_cfg.kVCF*m_sst.isdt; - for(i=0,ni=m_nodes.size();i0) { for(int iseq=0;iseq& bodies) +{ +const int nb=bodies.size(); +int iterations=0; +for(int i=0;im_cfg.citerations); + } +for(int i=0;iprepareClusters(iterations); + } +for(int i=0;isolveClusters(sor); + } + } +for(int i=0;icleanupClusters(); + } +} + // void btSoftBody::integrateMotion() { /* Update */ - UpdateNormals(this); + updateNormals(); +} + +// + btSoftBody::RayCaster::RayCaster(const btVector3& org,const btVector3& dir,btScalar mxt) +{ +o = org; +d = dir; +mint = mxt; +face = 0; +tests = 0; +} + +// +void btSoftBody::RayCaster::Process(const btDbvtNode* leaf) +{ +btSoftBody::Face& f=*(btSoftBody::Face*)leaf->data; +const btScalar t=rayTriangle( o,d, + f.m_n[0]->m_x, + f.m_n[1]->m_x, + f.m_n[2]->m_x, + mint); +if((t>0)&&(tteps)&&(tceps) && + (dot(n,cross(b-hit,c-hit))>ceps) && + (dot(n,cross(c-hit,a-hit))>ceps)) + { + return(t); + } + } + } + return(-1); +} + +// +void btSoftBody::pointersToIndices() +{ +#define PTR2IDX(_p_,_b_) reinterpret_cast((_p_)-(_b_)) + btSoftBody::Node* base=&m_nodes[0]; + for(int i=0,ni=m_nodes.size();idata=*(void**)&i; + } + } + for(int i=0,ni=m_links.size();idata=*(void**)&i; + } + } + for(int i=0,ni=m_anchors.size();idata=&m_nodes[i]; + } + } + for(int i=0,ni=m_links.size();idata=&m_faces[i]; + } + } + for(int i=0,ni=m_anchors.size();im_x, + f.m_n[1]->m_x, + f.m_n[2]->m_x, + mint); + if(t>0) + { + ++cnt; + if(!bcountonly) + { + feature=btSoftBody::eFeature::Face; + index=i; + mint=t; + } + } + } + } + else + {/* Use dbvt */ + RayCaster collider(org,dir,mint); + btDbvt::collideRAY(m_fdbvt.m_root,org,dir,collider); + if(collider.face) + { + mint=collider.mint; + feature=btSoftBody::eFeature::Face; + index=(int)(collider.face-&m_faces[0]); + cnt=1; + } + } + return(cnt); +} + +// +void btSoftBody::initializeFaceTree() +{ +m_fdbvt.clear(); +for(int i=0;igetCollisionShape(); + const btTransform& wtr=prb->getInterpolationWorldTransform(); + btScalar dst=m_worldInfo->m_sparsesdf.Evaluate( wtr.invXform(x), + shp, + nrm, + margin); + if(dst<0) + { + cti.m_body = prb; + cti.m_normal = wtr.getBasis()*nrm; + cti.m_offset = -dot( cti.m_normal, + x-cti.m_normal*dst); + return(true); + } + return(false); +} + +// +void btSoftBody::updateNormals() +{ + const btVector3 zv(0,0,0); + for(int i=0,ni=m_nodes.size();im_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=m_nodes.size();ivolume.Mins(); + const btVector3& maxs=m_ndbvt.m_root->volume.Maxs(); + const btScalar csm=getCollisionShape()->getMargin(); + const btVector3 mrg=btVector3( csm, + csm, + csm)*1; // ??? to investigate... + m_bounds[0]=mins-mrg; + m_bounds[1]=maxs+mrg; + if(0!=getBroadphaseHandle()) + { + m_worldInfo->m_broadphase->setAabb( getBroadphaseHandle(), + m_bounds[0], + m_bounds[1], + m_worldInfo->m_dispatcher); + } + } + else + { + m_bounds[0]= + m_bounds[1]=btVector3(0,0,0); + } +} + + +// +void btSoftBody::updatePose() +{ + if(m_pose.m_bframe) + { + btSoftBody::Pose& pose=m_pose; + const btVector3 com=evaluateCom(); + /* Com */ + pose.m_com = com; + /* Rotation */ + btMatrix3x3 Apq; + const btScalar eps=SIMD_EPSILON; + 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=m_nodes.size();i1) + { + const btScalar idet=Clamp( 1/pose.m_scl.determinant(), + 1,m_cfg.maxvolume); + pose.m_scl=Mul(pose.m_scl,idet); + } + + } +} + +// +void btSoftBody::updateConstants() +{ + /* Links */ + for(int i=0,ni=m_links.size();im_x-l.m_n[1]->m_x).length(); + l.m_c0 = (l.m_n[0]->m_im+l.m_n[1]->m_im)/m.m_kLST; + l.m_c1 = l.m_rl*l.m_rl; + } + /* Faces */ + for(int i=0,ni=m_faces.size();im_x,f.m_n[1]->m_x,f.m_n[2]->m_x); + } + /* Area's */ + btAlignedObjectArray counts; + counts.resize(m_nodes.size(),0); + for(int i=0,ni=m_nodes.size();im_area+=btFabs(f.m_ra); + } + } + for(int i=0,ni=m_nodes.size();i0) + m_nodes[i].m_area/=(btScalar)counts[i]; + else + m_nodes[i].m_area=0; + } +} + +// +void btSoftBody::initializeClusters() +{ +for(int i=0;im_im>0?1/c.m_nodes[j]->m_im:0; + c.m_imass += c.m_masses[j]; + } + c.m_imass = 1/c.m_imass; + c.m_com = btSoftBody::clusterCom(&c); + c.m_lv = btVector3(0,0,0); + c.m_av = btVector3(0,0,0); + c.m_leaf = 0; + /* Inertia */ + btMatrix3x3& ii=c.m_locii; + ii[0]=ii[1]=ii[2]=btVector3(0,0,0); + for(int i=0,ni=c.m_nodes.size();im_x-c.m_com; + const btVector3 q=k*k; + const btScalar m=c.m_masses[i]; + ii[0][0] += m*(q[1]+q[2]); + ii[1][1] += m*(q[0]+q[2]); + ii[2][2] += m*(q[0]+q[1]); + ii[0][1] -= m*k[0]*k[1]; + ii[0][2] -= m*k[0]*k[2]; + ii[1][2] -= m*k[1]*k[2]; + } + ii[1][0]=ii[0][1]; + ii[2][0]=ii[0][2]; + ii[2][1]=ii[1][2]; + ii=ii.inverse(); + /* Frame */ + c.m_framexform.setIdentity(); + c.m_framexform.setOrigin(c.m_com); + c.m_framerefs.resize(c.m_nodes.size()); + for(int i=0;im_x-c.m_com; + } + } +} + +// +void btSoftBody::updateClusters() +{ +BT_PROFILE("UpdateClusters"); +for(int i=0;im_x-c.m_com; + const btVector3& b=c.m_framerefs[i]; + m[0]+=a[0]*b;m[1]+=a[1]*b;m[2]+=a[2]*b; + } + PolarDecompose(m,r,s); + c.m_framexform.setOrigin(c.m_com); + c.m_framexform.setBasis(r); + /* Inertia */ + #if 1/* Constant */ + c.m_invwi=c.m_framexform.getBasis()*c.m_locii*c.m_framexform.getBasis().transpose(); + #else + #if 0/* Sphere */ + const btScalar rk=(2*c.m_extents.length2())/(5*c.m_imass); + const btVector3 inertia(rk,rk,rk); + const btVector3 iin(btFabs(inertia[0])>SIMD_EPSILON?1/inertia[0]:0, + btFabs(inertia[1])>SIMD_EPSILON?1/inertia[1]:0, + btFabs(inertia[2])>SIMD_EPSILON?1/inertia[2]:0); + + c.m_invwi=c.m_xform.getBasis().scaled(iin)*c.m_xform.getBasis().transpose(); + #else/* Actual */ + c.m_invwi[0]=c.m_invwi[1]=c.m_invwi[2]=btVector3(0,0,0); + for(int i=0;im_x-c.m_com; + const btVector3 q=k*k; + const btScalar m=1/c.m_nodes[i]->m_im; + c.m_invwi[0][0] += m*(q[1]+q[2]); + c.m_invwi[1][1] += m*(q[0]+q[2]); + c.m_invwi[2][2] += m*(q[0]+q[1]); + c.m_invwi[0][1] -= m*k[0]*k[1]; + c.m_invwi[0][2] -= m*k[0]*k[2]; + c.m_invwi[1][2] -= m*k[1]*k[2]; + } + c.m_invwi[1][0]=c.m_invwi[0][1]; + c.m_invwi[2][0]=c.m_invwi[0][2]; + c.m_invwi[2][1]=c.m_invwi[1][2]; + c.m_invwi=c.m_invwi.inverse(); + #endif + #endif + /* Velocities */ + c.m_lv=btVector3(0,0,0); + c.m_av=btVector3(0,0,0); + for(int i=0;im_v*c.m_masses[i]; + c.m_lv += v; + c.m_av += cross(c.m_nodes[i]->m_x-c.m_com,v); + } + c.m_lv=c.m_imass*c.m_lv*(1-c.m_ldamping); + c.m_av=c.m_invwi*c.m_av*(1-c.m_adamping); + c.m_vimpulses[0] = + c.m_vimpulses[1] = btVector3(0,0,0); + c.m_dimpulses[0] = + c.m_dimpulses[1] = btVector3(0,0,0); + c.m_nvimpulses = 0; + c.m_ndimpulses = 0; + /* Matching */ + if(c.m_matching>0) + { + for(int j=0;jm_x; + btVector3 mx=mi; + for(int j=1;jm_x); + mx.setMax(c.m_nodes[j]->m_x); + } + const btDbvtVolume bounds=btDbvtVolume::FromMM(mi,mx); + if(c.m_leaf) + m_cdbvt.update(c.m_leaf,bounds,c.m_lv*m_sst.sdt*3,m_sst.radmrg); + else + c.m_leaf=m_cdbvt.insert(bounds,&c); + } + } + } +} + +// +void btSoftBody::cleanupClusters() +{ +for(int i=0;iTerminate(m_sst.sdt); + if(m_joints[i]->m_delete) + { + btAlignedFree(m_joints[i]); + m_joints.remove(m_joints[i--]); + } + } +} + +// +void btSoftBody::prepareClusters(int iterations) +{ +for(int i=0;iPrepare(m_sst.sdt,iterations); + } +} + + +// +void btSoftBody::solveClusters(btScalar sor) +{ +for(int i=0,ni=m_joints.size();iSolve(m_sst.sdt,sor); + } +} + +// +void btSoftBody::applyClusters(bool drift) +{ +BT_PROFILE("ApplyClusters"); +const btScalar f0=m_sst.sdt; +const btScalar f1=f0/2; +btAlignedObjectArray deltas; +btAlignedObjectArray weights; +deltas.resize(m_nodes.size(),btVector3(0,0,0)); +weights.resize(m_nodes.size(),0); +if(drift) + { + for(int i=0;im_x; + const btScalar q=c.m_masses[j]; + deltas[idx] += (v+cross(w,x-c.m_com))*q; + weights[idx] += q; + } + } + } +for(int i=0;i0) m_nodes[i].m_x+=deltas[i]/weights[i]; + } +} + +// +void btSoftBody::dampClusters() +{ +for(int i=0;i0) + { + for(int j=0;j0) + { + const btVector3 vx=c.m_lv+cross(c.m_av,c.m_nodes[j]->m_q-c.m_com); + n.m_v += c.m_ndamping*(vx-n.m_v); + } + } + } + } +} + +// +void btSoftBody::Joint::Prepare(btScalar dt,int) +{ +m_bodies[0].activate(); +m_bodies[1].activate(); +} + +// +void btSoftBody::LJoint::Prepare(btScalar dt,int iterations) +{ +static const btScalar maxdrift=4; +Joint::Prepare(dt,iterations); +m_rpos[0] = m_bodies[0].xform()*m_refs[0]; +m_rpos[1] = m_bodies[1].xform()*m_refs[1]; +m_drift = Clamp(m_rpos[0]-m_rpos[1],maxdrift)*m_erp/dt; +m_rpos[0] -= m_bodies[0].xform().getOrigin(); +m_rpos[1] -= m_bodies[1].xform().getOrigin(); +m_massmatrix = ImpulseMatrix( m_bodies[0].invMass(),m_bodies[0].invWorldInertia(),m_rpos[0], + m_bodies[1].invMass(),m_bodies[1].invWorldInertia(),m_rpos[1]); +if(m_split>0) + { + m_sdrift = m_massmatrix*(m_drift*m_split); + m_drift *= 1-m_split; + } +m_drift /=(btScalar)iterations; +} + +// +void btSoftBody::LJoint::Solve(btScalar dt,btScalar sor) +{ +const btVector3 va=m_bodies[0].velocity(m_rpos[0]); +const btVector3 vb=m_bodies[1].velocity(m_rpos[1]); +const btVector3 vr=va-vb; +btSoftBody::Impulse impulse; +impulse.m_asVelocity = 1; +impulse.m_velocity = m_massmatrix*(m_drift+vr*m_cfm)*sor; +m_bodies[0].applyImpulse(-impulse,m_rpos[0]); +m_bodies[1].applyImpulse( impulse,m_rpos[1]); +} + +// +void btSoftBody::LJoint::Terminate(btScalar dt) +{ +if(m_split>0) + { + m_bodies[0].applyDImpulse(-m_sdrift,m_rpos[0]); + m_bodies[1].applyDImpulse( m_sdrift,m_rpos[1]); + } +} + +// +void btSoftBody::AJoint::Prepare(btScalar dt,int iterations) +{ +static const btScalar maxdrift=SIMD_PI/16; +m_icontrol->Prepare(this); +Joint::Prepare(dt,iterations); +m_axis[0] = m_bodies[0].xform().getBasis()*m_refs[0]; +m_axis[1] = m_bodies[1].xform().getBasis()*m_refs[1]; +m_drift = NormalizeAny(cross(m_axis[1],m_axis[0])); +m_drift *= btMin(maxdrift,btAcos(Clamp(dot(m_axis[0],m_axis[1]),-1,+1))); +m_drift *= m_erp/dt; +m_massmatrix= AngularImpulseMatrix(m_bodies[0].invWorldInertia(),m_bodies[1].invWorldInertia()); +if(m_split>0) + { + m_sdrift = m_massmatrix*(m_drift*m_split); + m_drift *= 1-m_split; + } +m_drift /=(btScalar)iterations; +} + +// +void btSoftBody::AJoint::Solve(btScalar dt,btScalar sor) +{ +const btVector3 va=m_bodies[0].angularVelocity(); +const btVector3 vb=m_bodies[1].angularVelocity(); +const btVector3 vr=va-vb; +const btScalar sp=dot(vr,m_axis[0]); +const btVector3 vc=vr-m_axis[0]*m_icontrol->Speed(this,sp); +btSoftBody::Impulse impulse; +impulse.m_asVelocity = 1; +impulse.m_velocity = m_massmatrix*(m_drift+vc*m_cfm)*sor; +m_bodies[0].applyAImpulse(-impulse); +m_bodies[1].applyAImpulse( impulse); +} + +// +void btSoftBody::AJoint::Terminate(btScalar dt) +{ +if(m_split>0) + { + m_bodies[0].applyDAImpulse(-m_sdrift); + m_bodies[1].applyDAImpulse( m_sdrift); + } +} + +// +void btSoftBody::CJoint::Prepare(btScalar dt,int iterations) +{ +Joint::Prepare(dt,iterations); +const bool dodrift=(m_life==0); +m_delete=(++m_life)>m_maxlife; +if(dodrift) + { + m_drift=m_drift*m_erp/dt; + if(m_split>0) + { + m_sdrift = m_massmatrix*(m_drift*m_split); + m_drift *= 1-m_split; + } + m_drift/=(btScalar)iterations; + } + else + { + m_drift=m_sdrift=btVector3(0,0,0); + } +} + +// +void btSoftBody::CJoint::Solve(btScalar dt,btScalar sor) +{ +const btVector3 va=m_bodies[0].velocity(m_rpos[0]); +const btVector3 vb=m_bodies[1].velocity(m_rpos[1]); +const btVector3 vrel=va-vb; +const btScalar rvac=dot(vrel,m_normal); +btSoftBody::Impulse impulse; +impulse.m_asVelocity = 1; +impulse.m_velocity = m_drift; +if(rvac<0) + { + const btVector3 iv=m_normal*rvac; + const btVector3 fv=vrel-iv; + impulse.m_velocity += iv+fv*m_friction; + } +impulse.m_velocity=m_massmatrix*impulse.m_velocity*sor; +m_bodies[0].applyImpulse(-impulse,m_rpos[0]); +m_bodies[1].applyImpulse( impulse,m_rpos[1]); +} + +// +void btSoftBody::CJoint::Terminate(btScalar dt) +{ +if(m_split>0) + { + m_bodies[0].applyDImpulse(-m_sdrift,m_rpos[0]); + m_bodies[1].applyDImpulse( m_sdrift,m_rpos[1]); + } +} + +// +void btSoftBody::applyForces() +{ + BT_PROFILE("SoftBody applyForces"); + const btScalar dt=m_sst.sdt; + const btScalar kLF=m_cfg.kLF; + const btScalar kDG=m_cfg.kDG; + const btScalar kPR=m_cfg.kPR; + const btScalar kVC=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 && + (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 = getVolume(); + ivolumetp = 1/btFabs(volume)*kPR; + dvolumetv = (m_pose.m_volume-volume)*kVC; + } + /* Per vertex forces */ + for(int i=0,ni=m_nodes.size();i0) + { + if(use_medium) + { + EvaluateMedium(m_worldInfo,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(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); + } + } + } + /* Per face forces */ + for(int i=0,ni=m_faces.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; + EvaluateMedium(m_worldInfo,x,medium); + const btVector3 rel_v=v-medium.m_velocity; + const btScalar rel_v2=rel_v.length2(); + if(rel_v2>SIMD_EPSILON) + { + btVector3 nrm=f.m_normal; + /* Setup normal */ + switch(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); + } + } + } + } +} + +// +void btSoftBody::PSolve_Anchors(btSoftBody* psb,btScalar kst,btScalar ti) +{ + const btScalar kAHR=psb->m_cfg.kAHR*kst; + const btScalar dt=psb->m_sst.sdt; + for(int i=0,ni=psb->m_anchors.size();im_anchors[i]; + const btTransform& t=a.m_body->getInterpolationWorldTransform(); + Node& n=*a.m_node; + const btVector3 wa=t*a.m_local; + const btVector3 va=a.m_body->getVelocityInLocalPoint(a.m_c1)*dt; + const btVector3 vb=n.m_x-n.m_q; + const btVector3 vr=(va-vb)+(wa-n.m_x)*kAHR; + const btVector3 impulse=a.m_c0*vr; + n.m_x+=impulse*a.m_c2; + a.m_body->applyImpulse(-impulse,a.m_c1); + } +} + +// +void btSoftBody::PSolve_RContacts(btSoftBody* psb,btScalar kst,btScalar ti) +{ + const btScalar dt=psb->m_sst.sdt; + const btScalar mrg=psb->getCollisionShape()->getMargin(); + for(int i=0,ni=psb->m_rcontacts.size();im_rcontacts[i]; + const sCti& cti=c.m_cti; + const btVector3 va=cti.m_body->getVelocityInLocalPoint(c.m_c1)*dt; + const btVector3 vb=c.m_node->m_x-c.m_node->m_q; + const btVector3 vr=vb-va; + const btScalar dn=dot(vr,cti.m_normal); + if(dn<=SIMD_EPSILON) + { + const btScalar dp=btMin(dot(c.m_node->m_x,cti.m_normal)+cti.m_offset,mrg); + const btVector3 fv=vr-cti.m_normal*dn; + const btVector3 impulse=c.m_c0*((vr-fv*c.m_c3+cti.m_normal*(dp*c.m_c4))*kst); + c.m_node->m_x-=impulse*c.m_c2; + c.m_cti.m_body->applyImpulse(impulse,c.m_c1); + } + } +} + +// +void btSoftBody::PSolve_SContacts(btSoftBody* psb,btScalar,btScalar ti) +{ +for(int i=0,ni=psb->m_scontacts.size();im_scontacts[i]; + const btVector3& nr=c.m_normal; + Node& n=*c.m_node; + Face& f=*c.m_face; + const btVector3 p=BaryEval( f.m_n[0]->m_x, + f.m_n[1]->m_x, + f.m_n[2]->m_x, + c.m_weights); + const btVector3 q=BaryEval( f.m_n[0]->m_q, + f.m_n[1]->m_q, + f.m_n[2]->m_q, + c.m_weights); + const btVector3 vr=(n.m_x-n.m_q)-(p-q); + btVector3 corr(0,0,0); + if(dot(vr,nr)<0) + { + const btScalar j=c.m_margin-(dot(nr,n.m_x)-dot(nr,p)); + corr+=c.m_normal*j; + } + corr -= ProjectOnPlane(vr,nr)*c.m_friction; + n.m_x += corr*c.m_cfm[0]; + f.m_n[0]->m_x -= corr*(c.m_cfm[1]*c.m_weights.x()); + f.m_n[1]->m_x -= corr*(c.m_cfm[1]*c.m_weights.y()); + f.m_n[2]->m_x -= corr*(c.m_cfm[1]*c.m_weights.z()); + } +} + +// +void btSoftBody::PSolve_Links(btSoftBody* psb,btScalar kst,btScalar ti) +{ + for(int i=0,ni=psb->m_links.size();im_links[i]; + if(l.m_c0>0) + { + Node& a=*l.m_n[0]; + Node& b=*l.m_n[1]; + const btVector3 del=b.m_x-a.m_x; + const btScalar len=del.length2(); + const btScalar k=((l.m_c1-len)/(l.m_c0*(l.m_c1+len)))*kst; + const btScalar t=k*a.m_im; + a.m_x-=del*(k*a.m_im); + b.m_x+=del*(k*b.m_im); + } + } +} + +// +void btSoftBody::VSolve_Links(btSoftBody* psb,btScalar kst) +{ +for(int i=0,ni=psb->m_links.size();im_links[i]; + Node** n=l.m_n; + const btScalar j=-dot(l.m_c3,n[0]->m_v-n[1]->m_v)*l.m_c2*kst; + n[0]->m_v+= l.m_c3*(j*n[0]->m_im); + n[1]->m_v-= l.m_c3*(j*n[1]->m_im); + } +} + +// +btSoftBody::psolver_t btSoftBody::getSolver(ePSolver::_ solver) +{ +switch(solver) + { + case ePSolver::Anchors: return(&btSoftBody::PSolve_Anchors); + case ePSolver::Linear: return(&btSoftBody::PSolve_Links); + case ePSolver::RContacts: return(&btSoftBody::PSolve_RContacts); + case ePSolver::SContacts: return(&btSoftBody::PSolve_SContacts); + } +return(0); +} + +// +btSoftBody::vsolver_t btSoftBody::getSolver(eVSolver::_ solver) +{ +switch(solver) + { + case eVSolver::Linear: return(&btSoftBody::VSolve_Links); + } +return(0); } // @@ -2255,50 +2453,7 @@ switch(m_cfg.collisions&fCollision::RVSmask) { case fCollision::SDF_RS: { - struct DoCollide : btDbvt::ICollide - { - void Process(const btDbvtNode* leaf) - { - btSoftBody::Node* node=(btSoftBody::Node*)leaf->data; - DoNode(*node); - } - void DoNode(btSoftBody::Node& n) const - { - const btScalar m=n.m_im>0?dynmargin:stamargin; - btSoftBody::RContact c; - if( (!n.m_battach)&& - CheckContact(psb,prb,n.m_x,m,c.m_cti)) - { - const btScalar ima=n.m_im; - const btScalar imb=prb->getInvMass(); - const btScalar ms=ima+imb; - if(ms>0) - { - const btTransform& wtr=prb->getInterpolationWorldTransform(); - const btMatrix3x3 iwi=prb->getInvInertiaTensorWorld(); - const btVector3 ra=n.m_x-wtr.getOrigin(); - const btVector3 va=prb->getVelocityInLocalPoint(ra)*psb->m_sst.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=psb->m_cfg.kDF*prb->getFriction(); - c.m_node = &n; - c.m_c0 = ImpulseMatrix(psb->m_sst.sdt,ima,imb,iwi,ra); - c.m_c1 = ra; - c.m_c2 = ima*psb->m_sst.sdt; - c.m_c3 = fv.length2()<(btFabs(dn)*fc)?0:1-fc; - c.m_c4 = prb->isStaticOrKinematicObject()?psb->m_cfg.kKHR:psb->m_cfg.kCHR; - psb->m_rcontacts.push_back(c); - prb->activate(); - } - } - } - btSoftBody* psb; - btRigidBody* prb; - btScalar dynmargin; - btScalar stamargin; - } docollide; + btSoftColliders::CollideSDF_RS docollide; btRigidBody* prb=btRigidBody::upcast(pco); const btTransform wtr=prb->getInterpolationWorldTransform(); const btTransform ctr=prb->getWorldTransform(); @@ -2319,6 +2474,12 @@ switch(m_cfg.collisions&fCollision::RVSmask) btDbvt::collideTV(m_ndbvt.m_root,volume,docollide); } break; + case fCollision::CL_RS: + { + btSoftColliders::CollideCL_RS collider; + collider.Process(this,btRigidBody::upcast(pco)); + } + break; } } @@ -2328,54 +2489,15 @@ void btSoftBody::defaultCollisionHandler(btSoftBody* psb) const int cf=m_cfg.collisions&psb->m_cfg.collisions; switch(cf&fCollision::SVSmask) { + case fCollision::CL_SS: + { + btSoftColliders::CollideCL_SS docollide; + docollide.Process(this,psb); + } + break; case fCollision::VF_SS: { - struct DoCollide : btDbvt::ICollide - { - void Process(const btDbvtNode* lnode, - const btDbvtNode* lface) - { - btSoftBody::Node* node=(btSoftBody::Node*)lnode->data; - btSoftBody::Face* face=(btSoftBody::Face*)lface->data; - btVector3 o=node->m_x; - btVector3 p; - btScalar d=SIMD_INFINITY; - ProjectOrigin( face->m_n[0]->m_x-o, - face->m_n[1]->m_x-o, - face->m_n[2]->m_x-o, - p,d); - const btScalar m=mrg+(o-node->m_q).length()*2; - if(d<(m*m)) - { - const btSoftBody::Node* n[]={face->m_n[0],face->m_n[1],face->m_n[2]}; - const btVector3 w=BaryCoord(n[0]->m_x,n[1]->m_x,n[2]->m_x,p+o); - const btScalar ma=node->m_im; - btScalar mb=BaryEval(n[0]->m_im,n[1]->m_im,n[2]->m_im,w); - if( (n[0]->m_im<=0)|| - (n[1]->m_im<=0)|| - (n[2]->m_im<=0)) - { - mb=0; - } - const btScalar ms=ma+mb; - if(ms>0) - { - btSoftBody::SContact c; - c.m_normal = p/-btSqrt(d); - c.m_margin = m; - c.m_node = node; - c.m_face = face; - c.m_weights = w; - c.m_friction = btMax(psb[0]->m_cfg.kDF,psb[1]->m_cfg.kDF); - c.m_cfm[0] = ma/ms*psb[0]->m_cfg.kSHR; - c.m_cfm[1] = mb/ms*psb[1]->m_cfg.kSHR; - psb[0]->m_scontacts.push_back(c); - } - } - } - btSoftBody* psb[2]; - btScalar mrg; - } docollide; + btSoftColliders::CollideVF_SS docollide; /* common */ docollide.mrg= getCollisionShape()->getMargin()+ psb->getCollisionShape()->getMargin(); @@ -2394,4 +2516,4 @@ switch(cf&fCollision::SVSmask) } break; } -} \ No newline at end of file +} diff --git a/src/BulletSoftBody/btSoftBody.h b/src/BulletSoftBody/btSoftBody.h index c9e5ee03b..eaa43b787 100644 --- a/src/BulletSoftBody/btSoftBody.h +++ b/src/BulletSoftBody/btSoftBody.h @@ -91,10 +91,12 @@ public: ///fCollision struct fCollision { enum _ { RVSmask = 0x000f, ///Rigid versus soft mask - SDF_RS = 0x0001, ///SDF base rigid vs soft + SDF_RS = 0x0001, ///SDF based rigid vs soft + CL_RS = 0x0002, ///Cluster vs convex rigid vs soft SVSmask = 0x00f0, ///Rigid versus soft mask VF_SS = 0x0010, ///Vertex vs face soft vs soft handling + CL_SS = 0x0020, ///Cluster vs cluster soft vs soft handling /* presets */ Default = SDF_RS, END @@ -192,7 +194,7 @@ public: btVector3 m_n; // Normal btScalar m_im; // 1/mass btScalar m_area; // Area - btDbvtNode* m_leaf; // Leaf data + btDbvtNode* m_leaf; // Leaf data int m_battach:1; // Attached }; /* Link */ @@ -212,12 +214,12 @@ public: Node* m_n[3]; // Node pointers btVector3 m_normal; // Normal btScalar m_ra; // Rest area - btDbvtNode* m_leaf; // Leaf data + btDbvtNode* m_leaf; // Leaf data }; /* RContact */ struct RContact { - 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 @@ -268,6 +270,224 @@ public: btMatrix3x3 m_scl; // Scale btMatrix3x3 m_aqq; // Base scaling }; + /* Cluster */ + struct Cluster + { + btAlignedObjectArray m_nodes; + tScalarArray m_masses; + tVector3Array m_framerefs; + btTransform m_framexform; + btScalar m_idmass; + btScalar m_imass; + btMatrix3x3 m_locii; + btMatrix3x3 m_invwi; + btVector3 m_com; + btVector3 m_vimpulses[2]; + btVector3 m_dimpulses[2]; + int m_nvimpulses; + int m_ndimpulses; + btVector3 m_lv; + btVector3 m_av; + btDbvtNode* m_leaf; + btScalar m_ndamping; + btScalar m_ldamping; + btScalar m_adamping; + btScalar m_matching; + bool m_collide; + Cluster() : m_leaf(0),m_ndamping(0),m_ldamping(0),m_adamping(0),m_matching(0) {} + }; + /* Impulse */ + struct Impulse + { + btVector3 m_velocity; + btVector3 m_drift; + int m_asVelocity:1; + int m_asDrift:1; + Impulse() : m_velocity(0,0,0),m_drift(0,0,0),m_asVelocity(0),m_asDrift(0) {} + Impulse operator -() const + { + Impulse i=*this; + i.m_velocity=-i.m_velocity; + i.m_drift=-i.m_drift; + return(i); + } + Impulse operator*(btScalar x) const + { + Impulse i=*this; + i.m_velocity*=x; + i.m_drift*=x; + return(i); + } + }; + /* Body */ + struct Body + { + btSoftBody::Cluster* m_soft; + btRigidBody* m_rigid; + Body() : m_soft(0),m_rigid(0) {} + Body(btSoftBody::Cluster* p) : m_soft(p),m_rigid(0) {} + Body(btRigidBody* p) : m_soft(0),m_rigid(p) {} + void activate() const + { + if(m_rigid) m_rigid->activate(); + } + const btMatrix3x3& invWorldInertia() const + { + static const btMatrix3x3 iwi(0,0,0,0,0,0,0,0,0); + if(m_rigid) return(m_rigid->getInvInertiaTensorWorld()); + if(m_soft) return(m_soft->m_invwi); + return(iwi); + } + btScalar invMass() const + { + if(m_rigid) return(m_rigid->getInvMass()); + if(m_soft) return(m_soft->m_imass); + return(0); + } + const btTransform& xform() const + { + static const btTransform identity=btTransform::getIdentity(); + if(m_rigid) return(m_rigid->getInterpolationWorldTransform()); + if(m_soft) return(m_soft->m_framexform); + return(identity); + } + btVector3 linearVelocity() const + { + if(m_rigid) return(m_rigid->getLinearVelocity()); + if(m_soft) return(m_soft->m_lv); + return(btVector3(0,0,0)); + } + btVector3 angularVelocity(const btVector3& rpos) const + { + if(m_rigid) return(cross(m_rigid->getAngularVelocity(),rpos)); + if(m_soft) return(cross(m_soft->m_av,rpos)); + return(btVector3(0,0,0)); + } + btVector3 angularVelocity() const + { + if(m_rigid) return(m_rigid->getAngularVelocity()); + if(m_soft) return(m_soft->m_av); + return(btVector3(0,0,0)); + } + btVector3 velocity(const btVector3& rpos) const + { + return(linearVelocity()+angularVelocity(rpos)); + } + void applyVImpulse(const btVector3& impulse,const btVector3& rpos) const + { + if(m_rigid) m_rigid->applyImpulse(impulse,rpos); + if(m_soft) btSoftBody::clusterVImpulse(m_soft,rpos,impulse); + } + void applyDImpulse(const btVector3& impulse,const btVector3& rpos) const + { + if(m_rigid) m_rigid->applyImpulse(impulse,rpos); + if(m_soft) btSoftBody::clusterDImpulse(m_soft,rpos,impulse); + } + void applyImpulse(const Impulse& impulse,const btVector3& rpos) const + { + if(impulse.m_asVelocity) applyVImpulse(impulse.m_velocity,rpos); + if(impulse.m_asDrift) applyDImpulse(impulse.m_drift,rpos); + } + void applyVAImpulse(const btVector3& impulse) const + { + if(m_rigid) m_rigid->applyTorqueImpulse(impulse); + if(m_soft) btSoftBody::clusterVAImpulse(m_soft,impulse); + } + void applyDAImpulse(const btVector3& impulse) const + { + if(m_rigid) m_rigid->applyTorqueImpulse(impulse); + if(m_soft) btSoftBody::clusterDAImpulse(m_soft,impulse); + } + void applyAImpulse(const Impulse& impulse) const + { + if(impulse.m_asVelocity) applyVAImpulse(impulse.m_velocity); + if(impulse.m_asDrift) applyDAImpulse(impulse.m_drift); + } + void applyDCImpulse(const btVector3& impulse) const + { + if(m_rigid) m_rigid->applyCentralImpulse(impulse); + if(m_soft) btSoftBody::clusterDCImpulse(m_soft,impulse); + } + }; + /* Joint */ + struct Joint + { + struct eType { enum _ { + Linear, + Angular, + Contact, + };}; + struct Specs + { + Specs() : erp(1),cfm(1),split(1) {} + btScalar erp; + btScalar cfm; + btScalar split; + }; + Body m_bodies[2]; + btVector3 m_refs[2]; + btScalar m_cfm; + btScalar m_erp; + btScalar m_split; + btVector3 m_drift; + btVector3 m_sdrift; + btMatrix3x3 m_massmatrix; + bool m_delete; + virtual ~Joint() {} + Joint() : m_delete(false) {} + virtual void Prepare(btScalar dt,int iterations); + virtual void Solve(btScalar dt,btScalar sor)=0; + virtual void Terminate(btScalar dt)=0; + virtual eType::_ Type() const=0; + }; + /* LJoint */ + struct LJoint : Joint + { + struct Specs : Joint::Specs + { + btVector3 position; + }; + btVector3 m_rpos[2]; + void Prepare(btScalar dt,int iterations); + void Solve(btScalar dt,btScalar sor); + void Terminate(btScalar dt); + eType::_ Type() const { return(eType::Linear); } + }; + /* AJoint */ + struct AJoint : Joint + { + struct IControl + { + virtual void Prepare(AJoint*) {} + virtual btScalar Speed(AJoint*,btScalar current) { return(current); } + static IControl* Default() { static IControl def;return(&def); } + }; + struct Specs : Joint::Specs + { + Specs() : icontrol(IControl::Default()) {} + btVector3 axis; + IControl* icontrol; + }; + btVector3 m_axis[2]; + IControl* m_icontrol; + void Prepare(btScalar dt,int iterations); + void Solve(btScalar dt,btScalar sor); + void Terminate(btScalar dt); + eType::_ Type() const { return(eType::Angular); } + }; + /* CJoint */ + struct CJoint : Joint + { + int m_life; + int m_maxlife; + btVector3 m_rpos[2]; + btVector3 m_normal; + btScalar m_friction; + void Prepare(btScalar dt,int iterations); + void Solve(btScalar dt,btScalar sor); + void Terminate(btScalar dt); + eType::_ Type() const { return(eType::Contact); } + }; /* Config */ struct Config { @@ -284,11 +504,18 @@ public: btScalar kKHR; // Kinetic contacts hardness [0,1] btScalar kSHR; // Soft contacts hardness [0,1] btScalar kAHR; // Anchors hardness [0,1] + btScalar kSRHR_CL; // Soft vs rigid hardness [0,1] (cluster only) + btScalar kSKHR_CL; // Soft vs kinetic hardness [0,1] (cluster only) + btScalar kSSHR_CL; // Soft vs soft hardness [0,1] (cluster only) + btScalar kSR_SPLT_CL; // Soft vs rigid impulse split [0,1] (cluster only) + btScalar kSK_SPLT_CL; // Soft vs rigid impulse split [0,1] (cluster only) + btScalar kSS_SPLT_CL; // Soft vs rigid impulse split [0,1] (cluster only) btScalar maxvolume; // Maximum volume ratio for pose btScalar timescale; // Time scale int viterations; // Velocities solver iterations int piterations; // Positions solver iterations int diterations; // Drift solver iterations + int citerations; // Cluster solver iterations int collisions; // Collisions flags tVSolverArray m_vsequence; // Velocity solvers sequence tPSolverArray m_psequence; // Position solvers sequence @@ -302,12 +529,32 @@ public: btScalar velmrg; // velocity margin btScalar radmrg; // radial margin btScalar updmrg; // Update margin - }; + }; + /* RayCaster */ + struct RayCaster : btDbvt::ICollide + { + btVector3 o; + btVector3 d; + btScalar mint; + btSoftBody::Face* face; + int tests; + RayCaster(const btVector3& org,const btVector3& dir,btScalar mxt); + void Process(const btDbvtNode* leaf); + static inline btScalar rayTriangle(const btVector3& org, + const btVector3& dir, + const btVector3& a, + const btVector3& b, + const btVector3& c, + btScalar maxt=SIMD_INFINITY); + }; // // Typedef's // + typedef void (*psolver_t)(btSoftBody*,btScalar,btScalar); + typedef void (*vsolver_t)(btSoftBody*,btScalar); + typedef btAlignedObjectArray tClusterArray; typedef btAlignedObjectArray tNoteArray; typedef btAlignedObjectArray tNodeArray; typedef btAlignedObjectArray tLeafArray; @@ -317,7 +564,8 @@ public: typedef btAlignedObjectArray tRContactArray; typedef btAlignedObjectArray tSContactArray; typedef btAlignedObjectArray tMaterialArray; - typedef btAlignedObjectArray tSoftBodyArray; + typedef btAlignedObjectArray tJointArray; + typedef btAlignedObjectArray tSoftBodyArray; // // Fields @@ -327,7 +575,7 @@ public: SolverState m_sst; // Solver state Pose m_pose; // Pose void* m_tag; // User data - btSoftBodyWorldInfo* m_worldInfo; // + btSoftBodyWorldInfo* m_worldInfo; // World info tNoteArray m_notes; // Notes tNodeArray m_nodes; // Nodes tLinkArray m_links; // Links @@ -335,18 +583,22 @@ public: tAnchorArray m_anchors; // Anchors tRContactArray m_rcontacts; // Rigid contacts tSContactArray m_scontacts; // Soft contacts + tJointArray m_joints; // Joints tMaterialArray m_materials; // Materials btScalar m_timeacc; // Time accumulator btVector3 m_bounds[2]; // Spatial bounds bool m_bUpdateRtCst; // Update runtime constants btDbvt m_ndbvt; // Nodes tree btDbvt m_fdbvt; // Faces tree + btDbvt m_cdbvt; // Clusters tree + tClusterArray m_clusters; // Clusters + // // Api // /* ctor */ - btSoftBody( btSoftBodyWorldInfo* worldInfo,int node_count, + btSoftBody( btSoftBody::btSoftBodyWorldInfo* worldInfo,int node_count, const btVector3* x, const btScalar* m); /* dtor */ @@ -400,6 +652,14 @@ public: /* Append anchor */ void appendAnchor( int node, btRigidBody* body); + /* Append linear joint */ + void appendLinearJoint(const LJoint::Specs& specs,Cluster* body0,Body body1); + void appendLinearJoint(const LJoint::Specs& specs,Body body=Body()); + void appendLinearJoint(const LJoint::Specs& specs,btSoftBody* body); + /* Append linear joint */ + void appendAngularJoint(const AJoint::Specs& specs,Cluster* body0,Body body1); + void appendAngularJoint(const AJoint::Specs& specs,Body body=Body()); + void appendAngularJoint(const AJoint::Specs& specs,btSoftBody* body); /* Add force (or gravity) to the entire body */ void addForce( const btVector3& force); /* Add force (or gravity) to a node of the body */ @@ -435,11 +695,28 @@ public: bool bframe); /* Return the volume */ btScalar getVolume() const; + /* Cluster count */ + int clusterCount() const; + /* Cluster center of mass */ + static btVector3 clusterCom(const Cluster* cluster); + btVector3 clusterCom(int cluster) const; + /* Cluster velocity at rpos */ + static btVector3 clusterVelocity(const Cluster* cluster,const btVector3& rpos); + /* Cluster impulse */ + static void clusterVImpulse(Cluster* cluster,const btVector3& rpos,const btVector3& impulse); + static void clusterDImpulse(Cluster* cluster,const btVector3& rpos,const btVector3& impulse); + static void clusterImpulse(Cluster* cluster,const btVector3& rpos,const Impulse& impulse); + static void clusterVAImpulse(Cluster* cluster,const btVector3& impulse); + static void clusterDAImpulse(Cluster* cluster,const btVector3& impulse); + static void clusterAImpulse(Cluster* cluster,const Impulse& impulse); + static void clusterDCImpulse(Cluster* cluster,const btVector3& impulse); /* Generate bending constraints based on distance in the adjency graph */ int generateBendingConstraints( int distance, Material* mat=0); /* Randomize constraints to reduce solver bias */ void randomizeConstraints(); + /* Generate clusters (K-mean) */ + int generateClusters(int k,int maxiterations=8192); /* Refine */ void refine(ImplicitFn* ifn,btScalar accurary,bool cut); /* CutLink */ @@ -460,6 +737,8 @@ public: void staticSolve(int iterations); /* solveCommonConstraints */ static void solveCommonConstraints(btSoftBody** bodies,int count,int iterations); + /* solveClusters */ + static void solveClusters(const btAlignedObjectArray& bodies); /* integrateMotion */ void integrateMotion(); /* defaultCollisionHandlers */ @@ -483,12 +762,44 @@ public: return 0; } + // + // ::btCollisionObject + // + virtual void getAabb(btVector3& aabbMin,btVector3& aabbMax) const { aabbMin = m_bounds[0]; aabbMax = m_bounds[1]; } - + // + // Private + // + void pointersToIndices(); + void indicesToPointers(const int* map=0); + int rayCast(const btVector3& org,const btVector3& dir, + btScalar& mint,eFeature::_& feature,int& index,bool bcountonly) const; + void initializeFaceTree(); + btVector3 evaluateCom() const; + bool checkContact(btRigidBody* prb,const btVector3& x,btScalar margin,btSoftBody::sCti& cti) const; + void updateNormals(); + void updateBounds(); + void updatePose(); + void updateConstants(); + void initializeClusters(); + void updateClusters(); + void cleanupClusters(); + void prepareClusters(int iterations); + void solveClusters(btScalar sor); + void applyClusters(bool drift); + void dampClusters(); + void applyForces(); + static void PSolve_Anchors(btSoftBody* psb,btScalar kst,btScalar ti); + static void PSolve_RContacts(btSoftBody* psb,btScalar kst,btScalar ti); + static void PSolve_SContacts(btSoftBody* psb,btScalar,btScalar ti); + static void PSolve_Links(btSoftBody* psb,btScalar kst,btScalar ti); + static void VSolve_Links(btSoftBody* psb,btScalar kst); + static psolver_t getSolver(ePSolver::_ solver); + static vsolver_t getSolver(eVSolver::_ solver); }; diff --git a/src/BulletSoftBody/btSoftBodyHelpers.cpp b/src/BulletSoftBody/btSoftBodyHelpers.cpp index 967755a9d..a91572e83 100644 --- a/src/BulletSoftBody/btSoftBodyHelpers.cpp +++ b/src/BulletSoftBody/btSoftBodyHelpers.cpp @@ -14,7 +14,7 @@ subject to the following restrictions: */ ///btSoftBodyHelpers.cpp by Nathanael Presson -#include "btSoftBody.h" +#include "btSoftBodyInternals.h" #include #include #include "btSoftBodyHelpers.h" @@ -78,6 +78,49 @@ if(node) } // +template +static inline T sum(const btAlignedObjectArray& items) +{ +T v; +if(items.size()) + { + v=items[0]; + for(int i=1,ni=items.size();i +static inline void add(btAlignedObjectArray& items,const Q& value) +{ +for(int i=0,ni=items.size();i +static inline void mul(btAlignedObjectArray& items,const Q& value) +{ +for(int i=0,ni=items.size();i +static inline T average(const btAlignedObjectArray& items) +{ +const btScalar n=(btScalar)(items.size()>0?items.size():1); +return(sum(items)/n); +} + // static inline btScalar tetravolume(const btVector3& x0, const btVector3& x1, @@ -175,9 +218,7 @@ void btSoftBodyHelpers::Draw( btSoftBody* psb, /* Anchors */ if(0!=(drawflags&fDrawFlags::Anchors)) { - int i; - - for(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; @@ -185,7 +226,7 @@ void btSoftBodyHelpers::Draw( btSoftBody* psb, drawVertex(idraw,q,0.25,btVector3(0,1,0)); idraw->drawLine(a.m_node->m_x,q,btVector3(1,1,1)); } - for(i=0;im_nodes.size();++i) + for(int i=0;im_nodes.size();++i) { const btSoftBody::Node& n=psb->m_nodes[i]; if(0==(n.m_material->m_flags&btSoftBody::fMaterial::DebugDraw)) continue; @@ -207,20 +248,66 @@ void btSoftBodyHelpers::Draw( btSoftBody* psb, if(0==(f.m_material->m_flags&btSoftBody::fMaterial::DebugDraw)) continue; const btVector3 x[]={f.m_n[0]->m_x,f.m_n[1]->m_x,f.m_n[2]->m_x}; const btVector3 c=(x[0]+x[1]+x[2])/3; - /*idraw->drawLine((x[0]-c)*scl+c,(x[1]-c)*scl+c,col); - idraw->drawLine((x[1]-c)*scl+c,(x[2]-c)*scl+c,col); - idraw->drawLine((x[2]-c)*scl+c,(x[0]-c)*scl+c,col);*/ idraw->drawTriangle((x[0]-c)*scl+c, (x[1]-c)*scl+c, (x[2]-c)*scl+c, col,alp); - /*idraw->drawTriangle((x[0]-c)*scl+c, - (x[1]-c)*scl+c, - (x[2]-c)*scl+c, - f.m_n[0]->m_n,f.m_n[1]->m_n,f.m_n[2]->m_n, - col,alp);*/ } } + /* Clusters */ + if(0!=(drawflags&fDrawFlags::Clusters)) + { + srand(1806); + for(int i=0;im_clusters.size();++i) + { + if(psb->m_clusters[i]->m_collide) + { + btVector3 color( rand()/(btScalar)RAND_MAX, + rand()/(btScalar)RAND_MAX, + rand()/(btScalar)RAND_MAX); + color=color.normalized()*0.75; + btAlignedObjectArray vertices; + vertices.resize(psb->m_clusters[i]->m_nodes.size()); + for(int j=0,nj=vertices.size();jm_clusters[i]->m_nodes[j]->m_x; + } + HullDesc hdsc(QF_TRIANGLES,vertices.size(),&vertices[0]); + HullResult hres; + HullLibrary hlib; + hdsc.mMaxVertices=vertices.size(); + hlib.CreateConvexHull(hdsc,hres); + const btVector3 center=average(hres.m_OutputVertices); + add(hres.m_OutputVertices,-center); + mul(hres.m_OutputVertices,(btScalar)1); + add(hres.m_OutputVertices,center); + for(int j=0;j<(int)hres.mNumFaces;++j) + { + const int idx[]={hres.m_Indices[j*3+0],hres.m_Indices[j*3+1],hres.m_Indices[j*3+2]}; + idraw->drawTriangle(hres.m_OutputVertices[idx[0]], + hres.m_OutputVertices[idx[1]], + hres.m_OutputVertices[idx[2]], + color,1); + } + hlib.ReleaseResult(hres); + } + /* Velocities */ + #if 0 + for(int j=0;jm_clusters[i].m_nodes.size();++j) + { + const btSoftBody::Cluster& c=psb->m_clusters[i]; + const btVector3 r=c.m_nodes[j]->m_x-c.m_com; + const btVector3 v=c.m_lv+cross(c.m_av,r); + idraw->drawLine(c.m_nodes[j]->m_x,c.m_nodes[j]->m_x+v,btVector3(1,0,0)); + } + #endif + /* Frame */ + btSoftBody::Cluster& c=*psb->m_clusters[i]; + idraw->drawLine(c.m_com,c.m_framexform*btVector3(10,0,0),btVector3(1,0,0)); + idraw->drawLine(c.m_com,c.m_framexform*btVector3(0,10,0),btVector3(0,1,0)); + idraw->drawLine(c.m_com,c.m_framexform*btVector3(0,0,10),btVector3(0,0,1)); + } + } /* Notes */ if(0!=(drawflags&fDrawFlags::Notes)) { @@ -235,6 +322,46 @@ void btSoftBodyHelpers::Draw( btSoftBody* psb, idraw->draw3dText(p,n.m_text); } } + /* Node tree */ + if(0!=(drawflags&fDrawFlags::NodeTree)) DrawNodeTree(psb,idraw); + /* Face tree */ + if(0!=(drawflags&fDrawFlags::FaceTree)) DrawFaceTree(psb,idraw); + /* Cluster tree */ + if(0!=(drawflags&fDrawFlags::ClusterTree)) DrawClusterTree(psb,idraw); + /* Joints */ + if(0!=(drawflags&fDrawFlags::Joints)) + { + for(int i=0;im_joints.size();++i) + { + const btSoftBody::Joint* pj=psb->m_joints[i]; + switch(pj->Type()) + { + case btSoftBody::Joint::eType::Linear: + { + const btSoftBody::LJoint* pjl=(const btSoftBody::LJoint*)pj; + const btVector3 a0=pj->m_bodies[0].xform()*pjl->m_refs[0]; + const btVector3 a1=pj->m_bodies[1].xform()*pjl->m_refs[1]; + idraw->drawLine(pj->m_bodies[0].xform().getOrigin(),a0,btVector3(1,1,0)); + idraw->drawLine(pj->m_bodies[1].xform().getOrigin(),a1,btVector3(0,1,1)); + drawVertex(idraw,a0,0.25,btVector3(1,1,0)); + drawVertex(idraw,a1,0.25,btVector3(0,1,1)); + } + break; + case btSoftBody::Joint::eType::Angular: + { + const btSoftBody::AJoint* pja=(const btSoftBody::AJoint*)pj; + const btVector3 o0=pj->m_bodies[0].xform().getOrigin(); + const btVector3 o1=pj->m_bodies[1].xform().getOrigin(); + const btVector3 a0=pj->m_bodies[0].xform().getBasis()*pj->m_refs[0]; + const btVector3 a1=pj->m_bodies[1].xform().getBasis()*pj->m_refs[1]; + idraw->drawLine(o0,o0+a0*10,btVector3(1,1,0)); + idraw->drawLine(o0,o0+a1*10,btVector3(1,1,0)); + idraw->drawLine(o1,o1+a0*10,btVector3(0,1,1)); + idraw->drawLine(o1,o1+a1*10,btVector3(0,1,1)); + } + } + } + } } // @@ -281,6 +408,15 @@ void btSoftBodyHelpers::DrawFaceTree( btSoftBody* psb, drawTree(idraw,psb->m_fdbvt.m_root,0,btVector3(0,1,0),btVector3(1,0,0),mindepth,maxdepth); } +// +void btSoftBodyHelpers::DrawClusterTree( btSoftBody* psb, + btIDebugDraw* idraw, + int mindepth, + int maxdepth) +{ +drawTree(idraw,psb->m_cdbvt.m_root,0,btVector3(0,1,1),btVector3(1,0,0),mindepth,maxdepth); +} + // void btSoftBodyHelpers::DrawFrame( btSoftBody* psb, btIDebugDraw* idraw) @@ -315,9 +451,7 @@ btSoftBody* btSoftBodyHelpers::CreateRope( btSoftBody::btSoftBodyWorldInfo& wor const int r=res+2; btVector3* x=new btVector3[r]; btScalar* m=new btScalar[r]; - int i; - - for(i=0;iappendLink(i-1,i); } @@ -355,9 +489,7 @@ btSoftBody* btSoftBodyHelpers::CreatePatch(btSoftBody::btSoftBodyWorldInfo& wor const int tot=rx*ry; btVector3* x=new btVector3[tot]; btScalar* m=new btScalar[tot]; - int iy; - - for(iy=0;iy vtx; chks.resize(maxidx*maxidx,false); vtx.resize(maxidx); - for( i=0,j=0,ni=maxidx*3;irandomizeConstraints(); return(psb); -} \ No newline at end of file +} diff --git a/src/BulletSoftBody/btSoftBodyHelpers.h b/src/BulletSoftBody/btSoftBodyHelpers.h index 9832f7408..2e9a989b5 100644 --- a/src/BulletSoftBody/btSoftBodyHelpers.h +++ b/src/BulletSoftBody/btSoftBodyHelpers.h @@ -32,8 +32,13 @@ struct fDrawFlags { enum _ { Contacts = 0x0020, Anchors = 0x0040, Notes = 0x0080, + Clusters = 0x0100, + NodeTree = 0x0200, + FaceTree = 0x0400, + ClusterTree = 0x0800, + Joints = 0x1000, /* presets */ - Std = Links+Faces+Tetras+Anchors+Notes, + Std = Links+Faces+Tetras+Anchors+Notes+Joints, StdTetra = Std-Faces+Tetras, };}; @@ -59,6 +64,11 @@ struct btSoftBodyHelpers btIDebugDraw* idraw, int mindepth=0, int maxdepth=-1); + /* Draw cluster tree */ + static void DrawClusterTree(btSoftBody* psb, + btIDebugDraw* idraw, + int mindepth=0, + int maxdepth=-1); /* Draw rigid frame */ static void DrawFrame( btSoftBody* psb, btIDebugDraw* idraw); diff --git a/src/BulletSoftBody/btSoftBodyInternals.h b/src/BulletSoftBody/btSoftBodyInternals.h new file mode 100644 index 000000000..f6bed3556 --- /dev/null +++ b/src/BulletSoftBody/btSoftBodyInternals.h @@ -0,0 +1,891 @@ +/* +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. +*/ +///btSoftBody implementation by Nathanael Presson + +#ifndef _BT_SOFT_BODY_INTERNALS_H +#define _BT_SOFT_BODY_INTERNALS_H + +#include "btSoftBody.h" + +#include "LinearMath/btQuickprof.h" +#include "BulletCollision/BroadphaseCollision/btBroadphaseInterface.h" +#include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h" +#include "BulletCollision/CollisionShapes/btConvexInternalShape.h" +#include "BulletCollision/NarrowPhaseCollision/btGjkEpa2.h" + +// +// btSymMatrix +// +template +struct btSymMatrix +{ + btSymMatrix() : dim(0) {} + btSymMatrix(int n,const T& init=T()) { resize(n,init); } +void resize(int n,const T& init=T()) { dim=n;store.resize((n*(n+1))/2,init); } +int index(int c,int r) const { if(c>r) btSwap(c,r);btAssert(r store; +int dim; +}; + +// +// btSoftBodyCollisionShape +// +class btSoftBodyCollisionShape : public btConcaveShape +{ +public: + btSoftBody* m_body; + + btSoftBodyCollisionShape(btSoftBody* backptr) + { + m_body=backptr; + } + + virtual ~btSoftBodyCollisionShape() + { + + } + + void processAllTriangles(btTriangleCallback* /*callback*/,const btVector3& /*aabbMin*/,const btVector3& /*aabbMax*/) const + { + //not yet + btAssert(0); + } + + ///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 + { + /* t should be identity, but better be safe than...fast? */ + const btVector3 mins=m_body->m_bounds[0]; + const btVector3 maxs=m_body->m_bounds[1]; + const btVector3 crns[]={t*btVector3(mins.x(),mins.y(),mins.z()), + t*btVector3(maxs.x(),mins.y(),mins.z()), + t*btVector3(maxs.x(),maxs.y(),mins.z()), + t*btVector3(mins.x(),maxs.y(),mins.z()), + t*btVector3(mins.x(),mins.y(),maxs.z()), + t*btVector3(maxs.x(),mins.y(),maxs.z()), + t*btVector3(maxs.x(),maxs.y(),maxs.z()), + t*btVector3(mins.x(),maxs.y(),maxs.z())}; + aabbMin=aabbMax=crns[0]; + for(int i=1;i<8;++i) + { + aabbMin.setMin(crns[i]); + aabbMax.setMax(crns[i]); + } + } + + virtual int getShapeType() const + { + return SOFTBODY_SHAPE_PROXYTYPE; + } + virtual void setLocalScaling(const btVector3& /*scaling*/) + { + ///na + btAssert(0); + } + virtual const btVector3& getLocalScaling() const + { + static const btVector3 dummy(1,1,1); + return dummy; + } + virtual void calculateLocalInertia(btScalar /*mass*/,btVector3& /*inertia*/) const + { + ///not yet + btAssert(0); + } + virtual const char* getName()const + { + return "SoftBody"; + } + +}; + +// +// btSoftClusterCollisionShape +// +class btSoftClusterCollisionShape : public btConvexInternalShape +{ +public: + const btSoftBody::Cluster* m_cluster; + + btSoftClusterCollisionShape (const btSoftBody::Cluster* cluster) : m_cluster(cluster) { setMargin(0); } + + + virtual btVector3 localGetSupportingVertex(const btVector3& vec) const + { + btSoftBody::Node* const * n=&m_cluster->m_nodes[0]; + btScalar d=dot(vec,n[0]->m_x); + int j=0; + for(int i=1,ni=m_cluster->m_nodes.size();im_x); + if(k>d) { d=k;j=i; } + } + return(n[j]->m_x); + } + virtual btVector3 localGetSupportingVertexWithoutMargin(const btVector3& vec)const + { + return(localGetSupportingVertex(vec)); + } + //notice that the vectors should be unit length + virtual void batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const + {} + + + virtual void calculateLocalInertia(btScalar mass,btVector3& inertia) const + {} + + virtual void getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const + {} + + virtual int getShapeType() const { return SOFTBODY_SHAPE_PROXYTYPE; } + + //debugging + virtual const char* getName()const {return "SOFTCLUSTER";} + + virtual void setMargin(btScalar margin) + { + btConvexInternalShape::setMargin(margin); + } + virtual btScalar getMargin() const + { + return getMargin(); + } +}; + +// +// Inline's +// + +// +template +static inline void ZeroInitialize(T& value) +{ +static const T zerodummy; +value=zerodummy; +} +// +template +static inline bool CompLess(const T& a,const T& b) +{ return(a +static inline bool CompGreater(const T& a,const T& b) +{ return(a>b); } +// +template +static inline T Lerp(const T& a,const T& b,btScalar t) +{ return(a+(b-a)*t); } +// +template +static inline T InvLerp(const T& a,const T& b,btScalar t) +{ return((b+a*t-b*t)/(a*b)); } +// +static inline btMatrix3x3 Lerp( const btMatrix3x3& a, + const btMatrix3x3& b, + btScalar t) +{ +btMatrix3x3 r; +r[0]=Lerp(a[0],b[0],t); +r[1]=Lerp(a[1],b[1],t); +r[2]=Lerp(a[2],b[2],t); +return(r); +} +// +static inline btVector3 Clamp(const btVector3& v,btScalar maxlength) +{ +const btScalar sql=v.length2(); +if(sql>(maxlength*maxlength)) + return((v*maxlength)/btSqrt(sql)); + else + return(v); +} +// +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 Cube(const T& x) +{ return(x*x*x); } +// +template +static inline T Sign(const T& x) +{ return((T)(x<0?-1:+1)); } +// +template +static inline bool SameSign(const T& x,const T& y) +{ return((x*y)>0); } +// +static inline btScalar ClusterMetric(const btVector3& x,const btVector3& y) +{ +const btVector3 d=x-y; +return(btFabs(d[0])+btFabs(d[1])+btFabs(d[2])); +} +// +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 void Orthogonalize(btMatrix3x3& m) +{ +m[2]=cross(m[0],m[1]).normalized(); +m[1]=cross(m[2],m[0]).normalized(); +m[0]=cross(m[1],m[2]).normalized(); +} +// +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 btMatrix3x3 ImpulseMatrix( btScalar ima,const btMatrix3x3& iia,const btVector3& ra, + btScalar imb,const btMatrix3x3& iib,const btVector3& rb) +{ +return(Add(MassMatrix(ima,iia,ra),MassMatrix(imb,iib,rb)).inverse()); +} + +// +static inline btMatrix3x3 AngularImpulseMatrix( const btMatrix3x3& iia, + const btMatrix3x3& iib) +{ +return(Add(iia,iib).inverse()); +} + +// +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)); +} + +// +static inline void ProjectOrigin( const btVector3& a, + const btVector3& b, + btVector3& prj, + btScalar& sqd) +{ +const btVector3 d=b-a; +const btScalar m2=d.length2(); +if(m2>SIMD_EPSILON) + { + const btScalar t=Clamp(-dot(a,d)/m2,0,1); + const btVector3 p=a+d*t; + const btScalar l2=p.length2(); + if(l2SIMD_EPSILON) + { + const btVector3 n=q/btSqrt(m2); + const btScalar k=dot(a,n); + const btScalar k2=k*k; + if(k20)&& + (dot(cross(b-p,c-p),q)>0)&& + (dot(cross(c-p,a-p),q)>0)) + { + prj=p; + sqd=k2; + } + else + { + ProjectOrigin(a,b,prj,sqd); + ProjectOrigin(b,c,prj,sqd); + ProjectOrigin(c,a,prj,sqd); + } + } + } +} + +// +template +static inline T BaryEval( const T& a, + const T& b, + const T& c, + const btVector3& coord) +{ + return(a*coord.x()+b*coord.y()+c*coord.z()); +} +// +static inline btVector3 BaryCoord( const btVector3& a, + const btVector3& b, + const btVector3& c, + const btVector3& p) +{ +const btScalar w[]={ cross(a-p,b-p).length(), + cross(b-p,c-p).length(), + cross(c-p,a-p).length()}; +const btScalar isum=1/(w[0]+w[1]+w[2]); +return(btVector3(w[1]*isum,w[2]*isum,w[0]*isum)); +} + +// +static btScalar ImplicitSolve( btSoftBody::ImplicitFn* fn, + const btVector3& a, + const btVector3& b, + const btScalar accuracy, + const int maxiterations=256) +{ +btScalar span[2]={0,1}; +btScalar values[2]={fn->Eval(a),fn->Eval(b)}; +if(values[0]>values[1]) + { + btSwap(span[0],span[1]); + btSwap(values[0],values[1]); + } +if(values[0]>-accuracy) return(-1); +if(values[1]<+accuracy) return(-1); +for(int i=0;iEval(Lerp(a,b,t)); + if((t<=0)||(t>=1)) break; + if(btFabs(v)SIMD_EPSILON) + return(v/l); + else + return(btVector3(0,0,0)); +} + +// +static inline btDbvtVolume VolumeOf( const btSoftBody::Face& f, + btScalar margin) +{ +const btVector3* pts[]={ &f.m_n[0]->m_x, + &f.m_n[1]->m_x, + &f.m_n[2]->m_x}; +btDbvtVolume vol=btDbvtVolume::FromPoints(pts,3); +vol.Expand(btVector3(margin,margin,margin)); +return(vol); +} + +// +static inline btVector3 CenterOf( const btSoftBody::Face& f) +{ +return((f.m_n[0]->m_x+f.m_n[1]->m_x+f.m_n[2]->m_x)/3); +} + +// +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 void EvaluateMedium( const btSoftBody::btSoftBodyWorldInfo* wfi, + const btVector3& x, + btSoftBody::sMedium& medium) +{ + medium.m_velocity = btVector3(0,0,0); + medium.m_pressure = 0; + medium.m_density = wfi->air_density; + if(wfi->water_density>0) + { + const btScalar depth=-(dot(x,wfi->water_normal)+wfi->water_offset); + if(depth>0) + { + medium.m_density = wfi->water_density; + medium.m_pressure = depth*wfi->water_density*wfi->m_gravity.length(); + } + } +} + +// +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 inline int MatchEdge( const btSoftBody::Node* a, + const btSoftBody::Node* b, + const btSoftBody::Node* ma, + const btSoftBody::Node* mb) +{ +if((a==ma)&&(b==mb)) return(0); +if((a==mb)&&(b==ma)) return(1); +return(-1); +} + +// +// btEigen : Extract eigen system, +// straitforward implementation of http://math.fullerton.edu/mathews/n2003/JacobiMethodMod.html +// outputs are NOT sorted. +// +struct btEigen +{ +static int system(btMatrix3x3& a,btMatrix3x3* vectors,btVector3* values=0) + { + static const int maxiterations=16; + static const btScalar accuracy=(btScalar)0.0001; + btMatrix3x3& v=*vectors; + int iterations=0; + vectors->setIdentity(); + do { + int p=0,q=1; + if(btFabs(a[p][q])accuracy) + { + const btScalar w=(a[q][q]-a[p][p])/(2*a[p][q]); + const btScalar z=btFabs(w); + const btScalar t=w/(z*(btSqrt(1+w*w)+z)); + if(t==t)/* [WARNING] let hope that one does not get thrown aways by some compilers... */ + { + const btScalar c=1/btSqrt(t*t+1); + const btScalar s=c*t; + mulPQ(a,c,s,p,q); + mulTPQ(a,c,s,p,q); + mulPQ(v,c,s,p,q); + } else break; + } else break; + } while((++iterations)accuracy) det=ndet; else break; + } + /* Final orthogonalization */ + Orthogonalize(q); + /* Compute 'S' */ + s=q.transpose()*m; + } + else + { + q.setIdentity(); + s.setIdentity(); + } +return(i); +} + +// +// btSoftColliders +// +struct btSoftColliders +{ + // + // ClusterBase + // + struct ClusterBase : btDbvt::ICollide + { + btScalar erp; + btScalar idt; + btScalar margin; + btScalar friction; + btScalar threshold; + ClusterBase() + { + erp =(btScalar)1; + idt =0; + margin =0; + friction =0; + threshold =(btScalar)0; + } + bool SolveContact( const btGjkEpaSolver2::sResults& res, + btSoftBody::Body ba,btSoftBody::Body bb, + btSoftBody::CJoint& joint) + { + if(res.distancedata; + btSoftClusterCollisionShape cshape(cluster); + const btConvexShape* rshape=(const btConvexShape*)prb->getCollisionShape(); + btGjkEpaSolver2::sResults res; + if(btGjkEpaSolver2::SignedDistance( &cshape,btTransform::getIdentity(), + rshape,prb->getInterpolationWorldTransform(), + btVector3(1,0,0),res)) + { + btSoftBody::CJoint joint; + if(SolveContact(res,cluster,prb,joint)) + { + btSoftBody::CJoint* pj=new(btAlignedAlloc(sizeof(btSoftBody::CJoint),16)) btSoftBody::CJoint(); + *pj=joint;psb->m_joints.push_back(pj); + if(prb->isStaticOrKinematicObject()) + { + pj->m_erp *= psb->m_cfg.kSKHR_CL; + pj->m_split *= psb->m_cfg.kSK_SPLT_CL; + } + else + { + pj->m_erp *= psb->m_cfg.kSRHR_CL; + pj->m_split *= psb->m_cfg.kSR_SPLT_CL; + } + } + } + } + void Process(btSoftBody* ps,btRigidBody* pr) + { + psb = ps; + prb = pr; + idt = ps->m_sst.isdt; + margin = ps->getCollisionShape()->getMargin()+ + pr->getCollisionShape()->getMargin(); + friction = btMin(psb->m_cfg.kDF,prb->getFriction()); + btVector3 mins; + btVector3 maxs; + btDbvtVolume volume; + pr->getCollisionShape()->getAabb(pr->getInterpolationWorldTransform(),mins,maxs); + volume=btDbvtVolume::FromMM(mins,maxs); + volume.Expand(btVector3(1,1,1)*margin); + btDbvt::collideTV(ps->m_cdbvt.m_root,volume,*this); + } + }; + // + // CollideCL_SS + // + struct CollideCL_SS : ClusterBase + { + btSoftBody* bodies[2]; + void Process(const btDbvtNode* la,const btDbvtNode* lb) + { + btSoftBody::Cluster* cla=(btSoftBody::Cluster*)la->data; + btSoftBody::Cluster* clb=(btSoftBody::Cluster*)lb->data; + btSoftClusterCollisionShape csa(cla); + btSoftClusterCollisionShape csb(clb); + btGjkEpaSolver2::sResults res; + if(btGjkEpaSolver2::SignedDistance( &csa,btTransform::getIdentity(), + &csb,btTransform::getIdentity(), + cla->m_com-clb->m_com,res)) + { + btSoftBody::CJoint joint; + if(SolveContact(res,cla,clb,joint)) + { + btSoftBody::CJoint* pj=new(btAlignedAlloc(sizeof(btSoftBody::CJoint),16)) btSoftBody::CJoint(); + *pj=joint;bodies[0]->m_joints.push_back(pj); + pj->m_erp *= btMax(bodies[0]->m_cfg.kSSHR_CL,bodies[1]->m_cfg.kSSHR_CL); + pj->m_split *= (bodies[0]->m_cfg.kSS_SPLT_CL+bodies[1]->m_cfg.kSS_SPLT_CL)/2; + } + } + } + void Process(btSoftBody* psa,btSoftBody* psb) + { + idt = psa->m_sst.isdt; + margin = (psa->getCollisionShape()->getMargin()+psb->getCollisionShape()->getMargin())/2; + friction = btMin(psa->m_cfg.kDF,psb->m_cfg.kDF); + bodies[0] = psa; + bodies[1] = psb; + btDbvt::collideTT(psa->m_cdbvt.m_root,psb->m_cdbvt.m_root,*this); + } + }; + // + // CollideSDF_RS + // + struct CollideSDF_RS : btDbvt::ICollide + { + void Process(const btDbvtNode* leaf) + { + btSoftBody::Node* node=(btSoftBody::Node*)leaf->data; + DoNode(*node); + } + void DoNode(btSoftBody::Node& n) const + { + const btScalar m=n.m_im>0?dynmargin:stamargin; + btSoftBody::RContact c; + if( (!n.m_battach)&& + psb->checkContact(prb,n.m_x,m,c.m_cti)) + { + const btScalar ima=n.m_im; + const btScalar imb=prb->getInvMass(); + const btScalar ms=ima+imb; + if(ms>0) + { + const btTransform& wtr=prb->getInterpolationWorldTransform(); + const btMatrix3x3& iwi=prb->getInvInertiaTensorWorld(); + const btVector3 ra=n.m_x-wtr.getOrigin(); + const btVector3 va=prb->getVelocityInLocalPoint(ra)*psb->m_sst.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=psb->m_cfg.kDF*prb->getFriction(); + c.m_node = &n; + c.m_c0 = ImpulseMatrix(psb->m_sst.sdt,ima,imb,iwi,ra); + c.m_c1 = ra; + c.m_c2 = ima*psb->m_sst.sdt; + c.m_c3 = fv.length2()<(btFabs(dn)*fc)?0:1-fc; + c.m_c4 = prb->isStaticOrKinematicObject()?psb->m_cfg.kKHR:psb->m_cfg.kCHR; + psb->m_rcontacts.push_back(c); + prb->activate(); + } + } + } + btSoftBody* psb; + btRigidBody* prb; + btScalar dynmargin; + btScalar stamargin; + }; + // + // CollideVF_SS + // + struct CollideVF_SS : btDbvt::ICollide + { + void Process(const btDbvtNode* lnode, + const btDbvtNode* lface) + { + btSoftBody::Node* node=(btSoftBody::Node*)lnode->data; + btSoftBody::Face* face=(btSoftBody::Face*)lface->data; + btVector3 o=node->m_x; + btVector3 p; + btScalar d=SIMD_INFINITY; + ProjectOrigin( face->m_n[0]->m_x-o, + face->m_n[1]->m_x-o, + face->m_n[2]->m_x-o, + p,d); + const btScalar m=mrg+(o-node->m_q).length()*2; + if(d<(m*m)) + { + const btSoftBody::Node* n[]={face->m_n[0],face->m_n[1],face->m_n[2]}; + const btVector3 w=BaryCoord(n[0]->m_x,n[1]->m_x,n[2]->m_x,p+o); + const btScalar ma=node->m_im; + btScalar mb=BaryEval(n[0]->m_im,n[1]->m_im,n[2]->m_im,w); + if( (n[0]->m_im<=0)|| + (n[1]->m_im<=0)|| + (n[2]->m_im<=0)) + { + mb=0; + } + const btScalar ms=ma+mb; + if(ms>0) + { + btSoftBody::SContact c; + c.m_normal = p/-btSqrt(d); + c.m_margin = m; + c.m_node = node; + c.m_face = face; + c.m_weights = w; + c.m_friction = btMax(psb[0]->m_cfg.kDF,psb[1]->m_cfg.kDF); + c.m_cfm[0] = ma/ms*psb[0]->m_cfg.kSHR; + c.m_cfm[1] = mb/ms*psb[1]->m_cfg.kSHR; + psb[0]->m_scontacts.push_back(c); + } + } + } + btSoftBody* psb[2]; + btScalar mrg; + }; +}; + +#endif //_BT_SOFT_BODY_INTERNALS_H diff --git a/src/BulletSoftBody/btSoftRigidDynamicsWorld.cpp b/src/BulletSoftBody/btSoftRigidDynamicsWorld.cpp index eec011047..e076a0768 100644 --- a/src/BulletSoftBody/btSoftRigidDynamicsWorld.cpp +++ b/src/BulletSoftBody/btSoftRigidDynamicsWorld.cpp @@ -24,7 +24,10 @@ subject to the following restrictions: btSoftRigidDynamicsWorld::btSoftRigidDynamicsWorld(btDispatcher* dispatcher,btBroadphaseInterface* pairCache,btConstraintSolver* constraintSolver,btCollisionConfiguration* collisionConfiguration) :btDiscreteDynamicsWorld(dispatcher,pairCache,constraintSolver,collisionConfiguration) { - +m_drawFlags = fDrawFlags::Std; +m_drawNodeTree = true; +m_drawFaceTree = false; +m_drawClusterTree = false; } btSoftRigidDynamicsWorld::~btSoftRigidDynamicsWorld() @@ -70,7 +73,12 @@ void btSoftRigidDynamicsWorld::updateSoftBodies() void btSoftRigidDynamicsWorld::solveSoftBodiesConstraints() { BT_PROFILE("solveSoftConstraints"); - + + if(m_softBodies.size()) + { + btSoftBody::solveClusters(m_softBodies); + } + for(int i=0;im_softBodies[i]; btSoftBodyHelpers::DrawFrame(psb,m_debugDrawer); - btSoftBodyHelpers::Draw(psb,m_debugDrawer,fDrawFlags::Nodes+fDrawFlags::Std); + btSoftBodyHelpers::Draw(psb,m_debugDrawer,m_drawFlags); if (m_debugDrawer && (m_debugDrawer->getDebugMode() & btIDebugDraw::DBG_DrawAabb)) { - btSoftBodyHelpers::DrawNodeTree(psb,m_debugDrawer); - //btSoftBodyHelpers::DrawFaceTree(psb,m_debugDrawer); + if(m_drawNodeTree) btSoftBodyHelpers::DrawNodeTree(psb,m_debugDrawer); + if(m_drawFaceTree) btSoftBodyHelpers::DrawFaceTree(psb,m_debugDrawer); + if(m_drawClusterTree) btSoftBodyHelpers::DrawClusterTree(psb,m_debugDrawer); } - } - } + } + } } diff --git a/src/BulletSoftBody/btSoftRigidDynamicsWorld.h b/src/BulletSoftBody/btSoftRigidDynamicsWorld.h index 8857b1741..3b498bb58 100644 --- a/src/BulletSoftBody/btSoftRigidDynamicsWorld.h +++ b/src/BulletSoftBody/btSoftRigidDynamicsWorld.h @@ -25,6 +25,10 @@ class btSoftRigidDynamicsWorld : public btDiscreteDynamicsWorld { btSoftBodyArray m_softBodies; + int m_drawFlags; + bool m_drawNodeTree; + bool m_drawFaceTree; + bool m_drawClusterTree; protected: @@ -48,6 +52,9 @@ public: void addSoftBody(btSoftBody* body); void removeSoftBody(btSoftBody* body); + + int getDrawFlags() const { return(m_drawFlags); } + void setDrawFlags(int f) { m_drawFlags=f; } btSoftBodyArray& getSoftBodyArray()