From be2490e4fbf3b7b19ff039bd4243ec1a5a214d8c Mon Sep 17 00:00:00 2001 From: "erwin.coumans" Date: Mon, 14 Apr 2008 06:24:56 +0000 Subject: [PATCH] Final 2.68 from Nathanael Presson, mainly soft body related. Added raycast support for soft bodies (press comma-key in the soft body demos to toggle ray cast tests) --- Demos/AllBulletDemos/Main.cpp | 7 +- Demos/SoftDemo/SoftDemo.cpp | 135 +++++--- Demos/SoftDemo/SoftDemo.h | 5 +- src/BulletSoftBody/btDbvt.cpp | 57 ++-- src/BulletSoftBody/btDbvt.h | 54 +++- src/BulletSoftBody/btSoftBody.cpp | 306 +++++++++++++----- src/BulletSoftBody/btSoftBody.h | 67 ++-- src/BulletSoftBody/btSoftBodyHelpers.cpp | 15 +- .../btSoftRigidCollisionAlgorithm.cpp | 21 +- .../btSoftRigidDynamicsWorld.cpp | 19 +- .../btSoftSoftCollisionAlgorithm.cpp | 65 +--- 11 files changed, 472 insertions(+), 279 deletions(-) diff --git a/Demos/AllBulletDemos/Main.cpp b/Demos/AllBulletDemos/Main.cpp index 801600cb4..e3a7253f5 100644 --- a/Demos/AllBulletDemos/Main.cpp +++ b/Demos/AllBulletDemos/Main.cpp @@ -41,7 +41,7 @@ namespace int iterationCount = 10; int width = 640; int height = 480; - int framePeriod = 16; + int framePeriod = 16;//todo: test if this value should be 0 int mainWindow; GLUI *glui; float hz; @@ -66,11 +66,8 @@ void Resize(int w, int h) glMatrixMode(GL_PROJECTION); glLoadIdentity(); - double ratio = (double)tw / (double)th; - - if (demo) - demo->reshape(w, h); + demo->reshape(tw, th); } DemoApplication* CreatDemo(btDemoEntry* entry) diff --git a/Demos/SoftDemo/SoftDemo.cpp b/Demos/SoftDemo/SoftDemo.cpp index d25d30142..79b492aee 100644 --- a/Demos/SoftDemo/SoftDemo.cpp +++ b/Demos/SoftDemo/SoftDemo.cpp @@ -106,7 +106,7 @@ void SoftDemo::clientMoveAndDisplay() if (m_dynamicsWorld) { -#define FIXED_STEP 0 +//#define FIXED_STEP #ifdef FIXED_STEP m_dynamicsWorld->stepSimulation(dt=1.0f/60.f,0); @@ -340,8 +340,6 @@ static void Init_RopeAttach(SoftDemo* pdemo) static btSoftBody* CtorRope(SoftDemo* pdemo,const btVector3& p) { btSoftBody* psb=btSoftBodyHelpers::CreateRope(pdemo->m_softBodyWorldInfo,p,p+btVector3(10,0,0),8,1); - psb->m_cfg.kDF = 0; - psb->m_cfg.kDP = 0.001; psb->setTotalMass(50); pdemo->getSoftDynamicsWorld()->addSoftBody(psb); return(psb); @@ -391,7 +389,7 @@ static void Init_Impact(SoftDemo* pdemo) 0, 1); pdemo->getSoftDynamicsWorld()->addSoftBody(psb); - + psb->m_cfg.kCHR=0.5; btTransform startTransform; startTransform.setIdentity(); startTransform.setOrigin(btVector3(0,20,0)); @@ -428,36 +426,6 @@ static void Init_Collide(SoftDemo* pdemo) { Functor::Create(pdemo,btVector3(3*i,2,0),btVector3(SIMD_PI/2*(1-(i&1)),SIMD_PI/2*(i&1),0)); } - #if 0 - const btScalar s=2; - const btScalar s2=s*1.01; - { - const btScalar h=0; - const btVector3 p[]={ btVector3(-s,h,-s),btVector3(+s,h,-s), - btVector3(+s,h,+s),btVector3(-s,h,+s)}; - btSoftBody* psb=new btSoftBody(&pdemo->m_softBodyWorldInfo,4,p,0); - psb->appendLink(0,1,1,btSoftBody::eLType::Structural); - psb->appendLink(1,2,1,btSoftBody::eLType::Structural); - psb->appendLink(2,3,1,btSoftBody::eLType::Structural); - psb->appendLink(3,0,1,btSoftBody::eLType::Structural); - psb->appendLink(0,2,1,btSoftBody::eLType::Structural); - psb->appendFace(0,1,2); - psb->appendFace(2,3,0); - /*psb->setMass(0,0); - psb->setMass(1,0); - psb->setMass(2,0); - psb->setMass(3,0);*/ - psb->m_cfg.collisions|=btSoftBody::fCollision::VF_SS; - pdemo->getSoftDynamicsWorld()->addSoftBody(psb); - } - { - btVector3 p[]={btVector3(0.5,5,-0.5)}; - btSoftBody* psb=new btSoftBody(&pdemo->m_softBodyWorldInfo,1,p,0); - //psb->setMass(0,0); - psb->m_cfg.collisions|=btSoftBody::fCollision::VF_SS; - pdemo->getSoftDynamicsWorld()->addSoftBody(psb); - } - #endif } // @@ -489,7 +457,7 @@ static void Init_Collide2(SoftDemo* pdemo) }; for(int i=0;i<3;++i) { - Functor::Create(pdemo,btVector3(0,-1+4*i,0),btVector3(0,SIMD_PI/2*(i&1),0)); + Functor::Create(pdemo,btVector3(0,-1+5*i,0),btVector3(0,SIMD_PI/2*(i&1),0)); } } @@ -511,7 +479,7 @@ static void Init_Collide3(SoftDemo* pdemo) psb->setTotalMass(150); pdemo->getSoftDynamicsWorld()->addSoftBody(psb); } - { + { const btScalar s=4; const btVector3 o=btVector3(4,10,0); btSoftBody* psb=btSoftBodyHelpers::CreatePatch( pdemo->m_softBodyWorldInfo, @@ -519,7 +487,7 @@ static void Init_Collide3(SoftDemo* pdemo) btVector3(+s,0,-s)+o, btVector3(-s,0,+s)+o, btVector3(+s,0,+s)+o, - 15,15,0,true); + 7,7,0,true); psb->generateBendingConstraints(2,0.5); psb->m_cfg.kLST = 0.4; psb->m_cfg.collisions |= btSoftBody::fCollision::VF_SS; @@ -595,7 +563,7 @@ static void Init_Pressure(SoftDemo* pdemo) 512); psb->m_cfg.kLST = 0.1; psb->m_cfg.kDF = 1; - psb->m_cfg.kDP = 0.008; // fun factor... + psb->m_cfg.kDP = 0.001; // fun factor... psb->m_cfg.kPR = 2500; psb->setTotalMass(30,true); pdemo->getSoftDynamicsWorld()->addSoftBody(psb); @@ -769,7 +737,7 @@ static void Init_TorusMatch(SoftDemo* pdemo) } -unsigned current_demo=14; +unsigned current_demo=0; void SoftDemo::clientResetScene() { @@ -827,13 +795,12 @@ void SoftDemo::clientResetScene() m_autocam = false; - m_showtrees = 0; + m_raycast = false; demofncs[current_demo](this); } void SoftDemo::renderme() { - DemoApplication::renderme(); btIDebugDraw* idraw=m_dynamicsWorld->getDebugDrawer(); m_dynamicsWorld->debugDrawWorld(); @@ -850,17 +817,88 @@ void SoftDemo::renderme() for(int i=0;igetNodes().size();++i) { ps+=psb->getNodes()[i].m_x; - } - btSoftBodyHelpers::DrawFrame(psb,idraw); - btSoftBodyHelpers::Draw(psb,idraw,fDrawFlags::Std); - if(m_showtrees&1) - btSoftBodyHelpers::DrawNodeTree(psb,idraw); + } } ps/=nps; if(m_autocam) m_cameraTargetPosition+=(ps-m_cameraTargetPosition)*0.01; else m_cameraTargetPosition=btVector3(0,0,0); + /* Anm */ + if(!isIdle()) + m_animtime=::GetTickCount()/1000.; + /* Ray cast */ + if(m_raycast) + { + /* Prepare rays */ + const int res=64; + const btScalar fres=res-1; + const btScalar size=8; + const btScalar dist=10; + btTransform trs; + trs.setOrigin(ps); + const btScalar angle=m_animtime*0.2; + trs.setRotation(btQuaternion(angle,SIMD_PI/4,0)); + const btVector3 dir=trs.getBasis()*btVector3(0,-1,0); + trs.setOrigin(ps-dir*dist); + btAlignedObjectArray origins; + btAlignedObjectArray times; + origins.resize(res*res); + times.resize(res*res,SIMD_INFINITY); + for(int y=0;yrayCast(*org,dir,results,*mint)) + { + *mint=results.time; + } + } + ++org;++mint; + } + long ms=btMax(m_clock.getTimeMilliseconds(),1); + long rayperseconds=(1000*(origins.size()*sbs.size()))/ms; + printf("%d ms (%d rays/s)\r\n",ms,rayperseconds); + } + /* Draw rays */ + const btVector3 c[]={ origins[0], + origins[res-1], + origins[res*(res-1)], + origins[res*(res-1)+res-1]}; + idraw->drawLine(c[0],c[1],btVector3(0,0,0)); + idraw->drawLine(c[1],c[3],btVector3(0,0,0)); + idraw->drawLine(c[3],c[2],btVector3(0,0,0)); + idraw->drawLine(c[2],c[0],btVector3(0,0,0)); + for(int i=0,ni=origins.size();idrawLine(org,org+dir*tim,btVector3(1,0,0)); + } + else + { + idraw->drawLine(org,org-dir*0.1,btVector3(0,0,0)); + } + } + #undef RES + } /* Water level */ static const btVector3 axis[]={btVector3(1,0,0), btVector3(0,1,0), @@ -877,6 +915,8 @@ 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(); + } void SoftDemo::keyboardCallback(unsigned char key, int x, int y) @@ -885,8 +925,8 @@ void SoftDemo::keyboardCallback(unsigned char key, int x, int y) { case ']': ++current_demo;clientResetScene();break; case '[': --current_demo;clientResetScene();break; + case ',': m_raycast=!m_raycast;break; case ';': m_autocam=!m_autocam;break; - case ',': m_showtrees+=1;break; default: DemoApplication::keyboardCallback(key,x,y); } } @@ -1010,3 +1050,4 @@ void SoftDemo::exitPhysics() + diff --git a/Demos/SoftDemo/SoftDemo.h b/Demos/SoftDemo/SoftDemo.h index 12c42c422..384100a51 100644 --- a/Demos/SoftDemo/SoftDemo.h +++ b/Demos/SoftDemo/SoftDemo.h @@ -52,7 +52,9 @@ public: bool m_autocam; - int m_showtrees; + bool m_raycast; + btScalar m_animtime; + btClock m_clock; //keep the collision shapes, for deletion/cleanup @@ -154,3 +156,4 @@ MACRO_SOFT_DEMO(16)//Init_BunnyMatch #endif //CCD_PHYSICS_DEMO_H + diff --git a/src/BulletSoftBody/btDbvt.cpp b/src/BulletSoftBody/btDbvt.cpp index c46c5f955..189f3a308 100644 --- a/src/BulletSoftBody/btDbvt.cpp +++ b/src/BulletSoftBody/btDbvt.cpp @@ -1,10 +1,5 @@ #include "btDbvt.h" -#include - -namespace btdbvt_internals -{ - // typedef btAlignedObjectArray tNodeArray; @@ -23,7 +18,6 @@ Merge(a,b,res); return(res); } - // volume+edge lengths static inline btScalar size(const btDbvt::Aabb& a) { @@ -266,14 +260,20 @@ if(leafs.size()>1) tNodeArray sets[2]; int bestaxis=-1; int bestmidp=leafs.size(); - sets[0].reserve(leafs.size()); - sets[1].reserve(leafs.size()); + int splitcount[3][2]={0,0,0,0,0,0}; + for(int i=0;ibox.Center()-org; + for(int j=0;j<3;++j) + { + ++splitcount[j][dot(x,axis[j])>0?1:0]; + } + } for(int i=0;i<3;++i) { - split(leafs,sets[0],sets[1],org,axis[i]); - if((sets[0].size()>0)&&(sets[1].size()>0)) + if((splitcount[i][0]>0)&&(splitcount[i][1]>0)) { - const int midp=abs(sets[0].size()-sets[1].size()); + const int midp=abs(splitcount[i][0]-splitcount[i][1]); if(midp1) } if(bestaxis>=0) { + sets[0].reserve(splitcount[bestaxis][0]); + sets[1].reserve(splitcount[bestaxis][1]); split(leafs,sets[0],sets[1],org,axis[bestaxis]); } else { - sets[0].resize(0); - sets[1].resize(0); + sets[0].reserve(leafs.size()/2+1); + sets[1].reserve(leafs.size()/2); for(int i=0,ni=leafs.size();im_root&&m_root) { - btAlignedObjectArray stack; stack.reserve(128); stack.push_back(sStkElm(m_root,tree->m_root)); @@ -524,9 +519,23 @@ if(m_root) } // -void btDbvt::collide(const btVector3& org, - const btVector3& dir, - ICollide* icollide) const +void btDbvt::collide(ICollide* icollide) const { -/* not implemented */ +if(m_root) + { + btAlignedObjectArray stack; + stack.reserve(64); + stack.push_back(m_root); + do { + const Node* n=stack[stack.size()-1]; + stack.pop_back(); + if(icollide->Descent(n)) + { + if(n->isinternal()) + { stack.push_back(n->childs[0]);stack.push_back(n->childs[1]); } + else + { icollide->Process(n); } + } + } while(stack.size()>0); + } } diff --git a/src/BulletSoftBody/btDbvt.h b/src/BulletSoftBody/btDbvt.h index 02bd5f3b4..1861f61d4 100644 --- a/src/BulletSoftBody/btDbvt.h +++ b/src/BulletSoftBody/btDbvt.h @@ -27,7 +27,7 @@ struct btDbvt { // Types - /* Aabb */ + /* Aabb */ struct Aabb { inline btVector3 Center() const { return((mi+mx)/2); } @@ -45,6 +45,8 @@ struct btDbvt inline bool Contain(const Aabb& a) const; inline friend bool Intersect( const Aabb& a, const Aabb& b); + inline friend bool Intersect( const Aabb& a, + const btVector3& b); inline friend btScalar Proximity( const Aabb& a, const Aabb& b); inline friend void Merge( const Aabb& a, @@ -54,7 +56,7 @@ struct btDbvt const Aabb& b); btVector3 mi,mx; }; - /* Node */ + /* Node */ struct Node { Aabb box; @@ -66,20 +68,22 @@ struct btDbvt void* data; }; }; - // Interfaces - + /* Stack element */ struct sStkElm { const Node* a; const Node* b; sStkElm(const Node* na,const Node* nb) : a(na),b(nb) {} }; - + + // Interfaces + /* ICollide */ struct ICollide { - virtual void Process(const Node* leaf0,const Node* leaf1) {}; - virtual void Process(const Node* leaf) {}; + virtual void Process(const Node*,const Node*) {} + virtual void Process(const Node*) {} + virtual bool Descent(const Node*) { return(false); } }; // Fields @@ -90,6 +94,7 @@ struct btDbvt btDbvt(); ~btDbvt(); void clear(); + bool empty() const { return(0==m_root); } int leafCount() const; void optimizeBottomUp(); void optimizeTopDown(int bu_treshold=128); @@ -104,6 +109,29 @@ struct btDbvt void collide(const btVector3& org, const btVector3& dir, ICollide* icollide) const; + void collide(ICollide* icollide) const; + // Generics + template + void collideGeneric(T& policy) const + { + if(m_root) + { + btAlignedObjectArray stack; + stack.reserve(64); + stack.push_back(m_root); + do { + const Node* n=stack[stack.size()-1]; + stack.pop_back(); + if(policy.Descent(n)) + { + if(n->isinternal()) + { stack.push_back(n->childs[0]);stack.push_back(n->childs[1]); } + else + { policy.Process(n); } + } + } while(stack.size()>0); + } + } // private: btDbvt(const btDbvt&) {} @@ -197,6 +225,18 @@ return( (a.mi.x()<=b.mx.x())&& (a.mx.y()>=b.mi.y())&& (a.mx.z()>=b.mi.z())); } + +// +inline bool Intersect( const btDbvt::Aabb& a, + const btVector3& b) +{ +return( (b.x()>=a.mi.x())&& + (b.y()>=a.mi.y())&& + (b.z()>=a.mi.z())&& + (b.x()<=a.mx.x())&& + (b.y()<=a.mx.y())&& + (b.z()<=a.mx.z())); +} // inline btScalar Proximity( const btDbvt::Aabb& a, diff --git a/src/BulletSoftBody/btSoftBody.cpp b/src/BulletSoftBody/btSoftBody.cpp index db699ef77..baca4b709 100644 --- a/src/BulletSoftBody/btSoftBody.cpp +++ b/src/BulletSoftBody/btSoftBody.cpp @@ -15,7 +15,10 @@ subject to the following restrictions: ///btSoftBody implementation by Nathanael Presson #include "btSoftBody.h" +#if 1 #include +#define DOTRACE +#endif #include #include "LinearMath/btQuickprof.h" #include "BulletCollision/NarrowPhaseCollision/btGjkEpa2.h" @@ -67,7 +70,7 @@ public: { return SOFTBODY_SHAPE_PROXYTYPE; } - virtual void setLocalScaling(const btVector3& scaling) + virtual void setLocalScaling(const btVector3& /*scaling*/) { ///na btAssert(0); @@ -77,7 +80,7 @@ public: static const btVector3 dummy(1,1,1); return dummy; } - virtual void calculateLocalInertia(btScalar mass,btVector3& inertia) const + virtual void calculateLocalInertia(btScalar /*mass*/,btVector3& /*inertia*/) const { ///not yet btAssert(0); @@ -99,18 +102,18 @@ btSoftBodyCollisionShape::~btSoftBodyCollisionShape() } -void btSoftBodyCollisionShape::processAllTriangles(btTriangleCallback* callback,const btVector3& aabbMin,const btVector3& aabbMax) const +void btSoftBodyCollisionShape::processAllTriangles(btTriangleCallback* /*callback*/,const btVector3& /*aabbMin*/,const btVector3& /*aabbMax*/) const { //not yet btAssert(0); } - // // Helpers // // +#ifdef DOTRACE static inline void Trace(const btMatrix3x3& m,const char* name) { printf("%s[0]: %.2f,\t%.2f,\t%.2f\r\n",name,m[0].x(),m[0].y(),m[0].z()); @@ -118,6 +121,9 @@ static inline void Trace(const btMatrix3x3& m,const char* name) printf("%s[2]: %.2f,\t%.2f,\t%.2f\r\n",name,m[2].x(),m[2].y(),m[2].z()); printf("\r\n"); } +#else +static inline void Trace(const btMatrix3x3&,const char*) {} +#endif // template @@ -144,6 +150,10 @@ 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)); } // @@ -348,6 +358,7 @@ static inline btVector3 NormalizeAny(const btVector3& v) } // +#if 0 static void PointersToIndices(btSoftBody* psb) { #define PTR2IDX(_p_,_b_) reinterpret_cast((_p_)-(_b_)) @@ -365,8 +376,10 @@ static void PointersToIndices(btSoftBody* psb) } #undef PTR2IDX } +#endif // +#if 0 static void IndicesToPointers(btSoftBody* psb) { #define IDX2PTR(_p_,_b_) ((_b_)+(((char*)_p_)-(char*)0)) @@ -384,6 +397,7 @@ static void IndicesToPointers(btSoftBody* psb) } #undef IDX2PTR } +#endif // static inline btDbvt::Aabb BoxOf( const btSoftBody::Face& f, @@ -431,7 +445,7 @@ static inline btScalar RayTriangle(const btVector3& org, { static const btScalar ceps=-SIMD_EPSILON*10; static const btScalar teps=SIMD_EPSILON*10; - const btVector3 n=cross(b-a,c-a).normalized(); + const btVector3 n=cross(b-a,c-a); const btScalar d=dot(a,n); const btScalar den=dot(dir,n); if(!btFuzzyZero(den)) @@ -461,26 +475,91 @@ static int RaycastInternal(const btSoftBody* psb, const btVector3& org, const btVector3& dir, btScalar& mint, + int& face, bool bcountonly) { - int cnt=0; - mint=SIMD_INFINITY; - for(int i=0,ni=psb->getFaces().size();igetFaces()[i]; - const btScalar t=RayTriangle( org,dir, - f.m_n[0]->m_x, - f.m_n[1]->m_x, - f.m_n[2]->m_x, - mint); - if(t>0) + btVector3 o; + btVector3 d; + btVector3 nd; + btScalar mint; + btSoftBody::Face* face; + int tests; + RayCaster(const btVector3& org,const btVector3& dir,btScalar mxt) { - ++cnt;if(!bcountonly) mint=t; + o = org; + d = dir; + nd = dir.normalized(); + mint = mxt; + face = 0; + tests = 0; + } + void Process(const btDbvt::Node* 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)&&(tbox.Center()-o; + const btScalar sqr=node->box.Lengths().length2()/4; + const btScalar prj=dot(ctr,nd); + return((ctr-(nd*prj)).length2()<=sqr); + } + }; + int cnt=0; + if(bcountonly||psb->m_fdbvt.empty()) + {/* Full search */ + for(int i=0,ni=psb->getFaces().size();igetFaces()[i]; + const btScalar t=RayTriangle( org,dir, + f.m_n[0]->m_x, + f.m_n[1]->m_x, + f.m_n[2]->m_x, + mint); + if(t>0) + { + ++cnt;if(!bcountonly) { face=i;mint=t; } + } + } + } + else + {/* Use dbvt */ + RayCaster collider(org,dir,mint); + psb->m_fdbvt.collideGeneric(collider); + if(collider.face) + { + mint=collider.mint; + face=(int)(collider.face-&psb->getFaces()[0]); + cnt=1; + } } - } return(cnt); } +// +static void InitializeFaceTree(btSoftBody* psb) +{ +psb->m_fdbvt.clear(); +for(int i=0;igetFaces().size();++i) + { + btSoftBody::Face& f=psb->getFaces()[i]; + f.m_leaf=psb->m_fdbvt.insert(BoxOf(f,0),&f); + } +psb->m_fdbvt.optimizeTopDown(); +} // static btVector3 EvaluateCom(btSoftBody* psb) @@ -519,13 +598,13 @@ static void EvaluateMedium( const btSoftBody::btSoftBodyWorldInfo* wfi, static bool CheckContact( btSoftBody* psb, btRigidBody* prb, const btVector3& x, + btScalar margin, btSoftBody::sCti& cti) { btVector3 nrm; btCollisionShape* shp=prb->getCollisionShape(); btAssert(shp->isConvex()); btConvexShape* csh=static_cast(shp); - const btScalar margin=psb->getCollisionShape()->getMargin(); const btTransform& wtr=prb->getInterpolationWorldTransform(); btScalar dst=psb->m_worldInfo->m_sparsesdf.Evaluate( wtr.invXform(x), csh, @@ -693,6 +772,7 @@ static inline void ApplyClampedForce( btSoftBody::Node& n, // static void ApplyForces(btSoftBody* psb,btScalar dt) { + BT_PROFILE("SoftBody applyForces"); const btScalar kLF=psb->m_cfg.kLF; const btScalar kDG=psb->m_cfg.kDG; const btScalar kPR=psb->m_cfg.kPR; @@ -832,7 +912,6 @@ static void PSolve_Anchors(btSoftBody* psb,btScalar sdt) // static void PSolve_RContacts(btSoftBody* psb,btScalar sdt) { - const btScalar kCHR=psb->m_cfg.kCHR; for(int i=0,ni=psb->m_rcontacts.size();im_rcontacts[i]; @@ -840,12 +919,12 @@ static void PSolve_RContacts(btSoftBody* psb,btScalar sdt) const btVector3 va=cti.m_body->getVelocityInLocalPoint(c.m_c1)*sdt; const btVector3 vb=c.m_node->m_x-c.m_node->m_q; const btVector3 vr=vb-va; - const btScalar dn=dot(vr,cti.m_normal); + const btScalar dn=dot(vr,cti.m_normal); if(dn<=SIMD_EPSILON) { const btScalar dp=dot(c.m_node->m_x,cti.m_normal)+cti.m_offset; const btVector3 fv=vr-cti.m_normal*dn; - const btVector3 impulse=c.m_c0*(vr-fv*c.m_c3+cti.m_normal*(dp*kCHR)); + const btVector3 impulse=c.m_c0*(vr-fv*c.m_c3+cti.m_normal*(dp*c.m_c4)); c.m_node->m_x-=impulse*c.m_c2; c.m_cti.m_body->applyImpulse(impulse,c.m_c1); } @@ -870,7 +949,7 @@ for(int i=0,ni=psb->m_scontacts.size();im_q, c.m_weights); const btVector3 vr=(n.m_x-n.m_q)-(p-q); - btVector3 corr(0,0,0); + btVector3 corr(0,0,0); if(dot(vr,nr)<0) { const btScalar j=c.m_margin-(dot(nr,n.m_x)-dot(nr,p)); @@ -912,9 +991,8 @@ static void PSolve_Links(btSoftBody* psb,btScalar w) btSoftBody::btSoftBody(btSoftBody::btSoftBodyWorldInfo* worldInfo,int node_count, const btVector3* x, const btScalar* m) :m_worldInfo(worldInfo) { - m_internalType=CO_SOFT_BODY; - /* Init */ + m_internalType = CO_SOFT_BODY; m_cfg.aeromodel = eAeroModel::V_Point; m_cfg.kDG = 0; m_cfg.kLF = 0; @@ -1254,6 +1332,81 @@ void btSoftBody::setPose(bool bvolume,bool bframe) UpdateConstants(this); } +// +void btSoftBody::setDistanceField(int res) +{ +struct SignChecker : btDbvt::ICollide + { + btVector3 m_x; + btVector3 m_axe; + int m_count; + SignChecker(const btVector3& x, + const btVector3& a, + btDbvt& tree) : m_x(x),m_axe(a),m_count(0) + { + tree.collide(this); + } + void Process(const btDbvt::Node* n) + { + const Face& f=*(const Face*)n->data; + if(RayTriangle( m_x, + m_axe, + f.m_n[0]->m_x, + f.m_n[1]->m_x, + f.m_n[2]->m_x)>0) + { + ++m_count; + } + } + bool Descent(const btDbvt::Node* n) + { + return(dot(m_x-n->box.Maxs()*m_axe,m_axe)<=0); + } + }; +btDbvt dbvt; +int ires[3]; +btScalar fres[3]; +const btScalar margin=getCollisionShape()->getMargin(); +const btVector3 lengths=m_bounds[1]-m_bounds[0]; +const btScalar smallest=btMin(btMin(lengths.x(),lengths.y()),lengths.z()); +const btScalar ratio=smallest/res; +ires[0]=btMax(2,(int)(lengths.x()/ratio)); +ires[1]=btMax(2,(int)(lengths.y()/ratio)); +ires[2]=btMax(2,(int)(lengths.z()/ratio)); +fres[0]=(btScalar)ires[0]-1; +fres[1]=(btScalar)ires[1]-1; +fres[2]=(btScalar)ires[2]-1; +for(int i=0;i1) + { + m_dfld.pts.push_back(p-m_pose.m_com); + } + } + } + } +} + // btScalar btSoftBody::getVolume() const { @@ -1348,14 +1501,21 @@ void btSoftBody::randomizeConstraints() } // -btScalar btSoftBody::raycast(const btVector3& org, - const btVector3& dir) const +bool btSoftBody::rayCast(const btVector3& org, + const btVector3& dir, + sRayCast& results, + btScalar maxtime) { - btScalar mint=0; - if(RaycastInternal(this,org,dir,mint,false)) - return(mint); - else - return(-1); + if(getFaces().size()) + { + if(m_fdbvt.empty()) InitializeFaceTree(this); + results.time = maxtime; + results.face = -1; + return(RaycastInternal( this,org,dir, + results.time, + results.face,false)!=0); + } + return(false); } // @@ -1369,24 +1529,20 @@ void btSoftBody::predictMotion(btScalar dt) m_fdbvt.clear(); if(m_cfg.collisions&fCollision::VF_SS) { - for(int i=0;igetMargin(); + m_sst.updmrg = m_sst.radmrg*(btScalar)0.25; /* Forces */ addVelocity(m_worldInfo->m_gravity*m_sst.sdt); ApplyForces(this,m_sst.sdt); /* Integrate */ - const btScalar margin=getCollisionShape()->getMargin(); - const btScalar updmrg=margin*(btScalar)1.0; for(int i=0,ni=getNodes().size();im_v+ f.m_n[1]->m_v+ f.m_n[2]->m_v)/3; - m_fdbvt.update(f.m_leaf,BoxOf(f,margin),v*m_sst.velmrg,updmrg); + m_fdbvt.update( f.m_leaf, + BoxOf(f,m_sst.radmrg), + v*m_sst.velmrg, + m_sst.updmrg); } } /* Pose */ @@ -1459,18 +1619,9 @@ void btSoftBody::solveConstraints() } // -void btSoftBody::solveCommonConstraints(btSoftBody** bodies,int count,int iterations) +void btSoftBody::solveCommonConstraints(btSoftBody** /*bodies*/,int /*count*/,int /*iterations*/) { /// placeholder -#if 0 -for(int isolve=0;isolve0?dynmargin:stamargin; + RContact c; if( (!n.m_battach)&& - CheckContact(psb,prb,n.m_x,c.m_cti)) + CheckContact(psb,prb,n.m_x,m,c.m_cti)) { const btScalar ima=n.m_im; const btScalar imb=prb->getInvMass(); @@ -1527,6 +1679,7 @@ switch(m_cfg.collisions&fCollision::RVSmask) 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 = psb->m_cfg.kCHR*(btScalar)(prb->isStaticOrKinematicObject()?0.5:1); psb->m_rcontacts.push_back(c); prb->activate(); } @@ -1534,18 +1687,26 @@ switch(m_cfg.collisions&fCollision::RVSmask) } btSoftBody* psb; btRigidBody* prb; - } docollide; - const btScalar margin=getCollisionShape()->getMargin(); - btVector3 mins; - btVector3 maxs; - btDbvt::Aabb aabb; + btScalar dynmargin; + btScalar stamargin; + } docollide; + btRigidBody* prb=btRigidBody::upcast(pco); + const btTransform wtr=prb->getInterpolationWorldTransform(); + const btTransform ctr=prb->getWorldTransform(); + const btScalar timemargin=(wtr.getOrigin()-ctr.getOrigin()).length(); + const btScalar basemargin=getCollisionShape()->getMargin(); + btVector3 mins; + btVector3 maxs; + btDbvt::Aabb aabb; pco->getCollisionShape()->getAabb( pco->getInterpolationWorldTransform(), mins, maxs); aabb=btDbvt::Aabb::FromMM(mins,maxs); - aabb.Expand(btVector3(margin,margin,margin)); - docollide.psb=this; - docollide.prb=btRigidBody::upcast(pco); + aabb.Expand(btVector3(basemargin,basemargin,basemargin)); + docollide.psb = this; + docollide.prb = prb; + docollide.dynmargin = basemargin+timemargin; + docollide.stamargin = basemargin; m_ndbvt.collide(aabb,&docollide); } break; @@ -1577,15 +1738,16 @@ switch(cf&fCollision::SVSmask) const btScalar m=mrg+(o-node->m_q).length()*2; if(d<(m*m)) { - const btVector3 w=BaryCoord(face->m_n[0]->m_x, - face->m_n[1]->m_x, - face->m_n[2]->m_x, - p+o); + const 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; - const btScalar mb=BaryEval(face->m_n[0]->m_im, - face->m_n[1]->m_im, - face->m_n[2]->m_im, - w); + 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) { @@ -1600,7 +1762,7 @@ switch(cf&fCollision::SVSmask) c.m_cfm[1] = mb/ms*psb[1]->m_cfg.kSHR; psb[0]->m_scontacts.push_back(c); } - } + } } btSoftBody* psb[2]; btScalar mrg; diff --git a/src/BulletSoftBody/btSoftBody.h b/src/BulletSoftBody/btSoftBody.h index 2ffec085a..4ecb9db1a 100644 --- a/src/BulletSoftBody/btSoftBody.h +++ b/src/BulletSoftBody/btSoftBody.h @@ -69,6 +69,17 @@ public: Default = SDF_RS, };}; + // + // API Types + // + + /* sRayCast */ + struct sRayCast + { + int face; /// face + btScalar time; /// time of impact (rayorg+raydir*time) + }; + // // Internal types // @@ -150,6 +161,7 @@ public: btVector3 m_c1; // Relative anchor btScalar m_c2; // ima*dt btScalar m_c3; // Friction + btScalar m_c4; // Hardness }; /* SContact */ struct SContact @@ -184,7 +196,12 @@ public: btMatrix3x3 m_rot; // Rotation btMatrix3x3 m_scl; // Scale btMatrix3x3 m_aqq; // Base scaling - }; + }; + /* DFld */ + struct DFld + { + btAlignedObjectArray pts; + }; /* Config */ struct Config { @@ -213,6 +230,8 @@ public: btScalar sdt; // dt*timescale btScalar isdt; // 1/sdt btScalar velmrg; // velocity margin + btScalar radmrg; // radial margin + btScalar updmrg; // Update margin }; // @@ -220,6 +239,7 @@ public: // typedef btAlignedObjectArray tNodeArray; + typedef btAlignedObjectArray tLeafArray; typedef btAlignedObjectArray tLinkArray; typedef btAlignedObjectArray tFaceArray; typedef btAlignedObjectArray tAnchorArray; @@ -234,6 +254,7 @@ public: Config m_cfg; // Configuration SolverState m_sst; // Solver state Pose m_pose; // Pose + DFld m_dfld; // Distance field void* m_tag; // User data btSoftBodyWorldInfo* m_worldInfo; // tAnchorArray m_anchors; // Anchors @@ -310,7 +331,9 @@ public: void scale( const btVector3& scl); /* Set current state as pose */ void setPose( bool bvolume, - bool bframe); + bool bframe); + /* Set current state as distance field */ + void setDistanceField(int nominalresolution); /* Return the volume */ btScalar getVolume() const; /* Generate bending constraints based on distance in the adjency graph */ @@ -319,8 +342,10 @@ public: /* Randomize constraints to reduce solver bias */ void randomizeConstraints(); /* Ray casting */ - btScalar raycast(const btVector3& org, - const btVector3& dir) const; + bool rayCast(const btVector3& org, + const btVector3& dir, + sRayCast& results, + btScalar maxtime=SIMD_INFINITY); /* predictMotion */ void predictMotion(btScalar dt); /* solveConstraints */ @@ -333,23 +358,8 @@ public: void defaultCollisionHandler(btCollisionObject* pco); void defaultCollisionHandler(btSoftBody* psb); - ///to keep collision detection and dynamics separate we don't store a rigidbody pointer - ///but a rigidbody is derived from btCollisionObject, so we can safely perform an upcast - static const btSoftBody* upcast(const btCollisionObject* colObj) - { - if (colObj->getInternalType()==CO_SOFT_BODY) - return (const btSoftBody*)colObj; - return 0; - } - static btSoftBody* upcast(btCollisionObject* colObj) - { - if (colObj->getInternalType()==CO_SOFT_BODY) - return (btSoftBody*)colObj; - return 0; - } - // - // ... + // Accessor's and cast. // tNodeArray& getNodes(); @@ -359,6 +369,23 @@ public: tFaceArray& getFaces(); const tFaceArray& getFaces() const; + // + // Cast + // + + static const btSoftBody* upcast(const btCollisionObject* colObj) + { + if (colObj->getInternalType()==CO_SOFT_BODY) + return (const btSoftBody*)colObj; + return 0; + } + static btSoftBody* upcast(btCollisionObject* colObj) + { + if (colObj->getInternalType()==CO_SOFT_BODY) + return (btSoftBody*)colObj; + return 0; + } + }; diff --git a/src/BulletSoftBody/btSoftBodyHelpers.cpp b/src/BulletSoftBody/btSoftBodyHelpers.cpp index 7ffab5967..5d2671843 100644 --- a/src/BulletSoftBody/btSoftBodyHelpers.cpp +++ b/src/BulletSoftBody/btSoftBodyHelpers.cpp @@ -79,6 +79,7 @@ if(node) } // +#if 0 static btVector3 stresscolor(btScalar stress) { static const btVector3 spectrum[]= { btVector3(1,0,1), @@ -95,6 +96,7 @@ static btVector3 stresscolor(btScalar stress) const btScalar frc=stress-sel; return(spectrum[sel]+(spectrum[sel+1]-spectrum[sel])*frc); } +#endif // void btSoftBodyHelpers::Draw( btSoftBody* psb, @@ -103,7 +105,6 @@ void btSoftBodyHelpers::Draw( btSoftBody* psb, { const btScalar scl=(btScalar)0.1; const btScalar nscl=scl*5; - const btScalar alpha=(btScalar)0.5; const btVector3 scolor=btVector3(0,0,0); const btVector3 bcolor=btVector3(1,1,0); const btVector3 ncolor=btVector3(1,1,1); @@ -208,7 +209,7 @@ void btSoftBodyHelpers::DrawInfos( btSoftBody* psb, btIDebugDraw* idraw, bool masses, bool areas, - bool stress) + bool /*stress*/) { for(int i=0;igetNodes().size();++i) { @@ -266,10 +267,13 @@ void btSoftBodyHelpers::DrawFrame( btSoftBody* psb, for(int i=0;im_pose.m_pos.size();++i) { const btVector3 x=com+trs*psb->m_pose.m_pos[i]; - idraw->drawLine(x-btVector3(1,0,0)*nscl,x+btVector3(1,0,0)*nscl,btVector3(1,0,1)); - idraw->drawLine(x-btVector3(0,1,0)*nscl,x+btVector3(0,1,0)*nscl,btVector3(1,0,1)); - idraw->drawLine(x-btVector3(0,0,1)*nscl,x+btVector3(0,0,1)*nscl,btVector3(1,0,1)); + drawVertex(idraw,x,nscl,btVector3(1,0,1)); } + for(int i=0;im_dfld.pts.size();++i) + { + const btVector3 x=com+trs*psb->m_dfld.pts[i]; + drawVertex(idraw,x,nscl*(btScalar)0.5,btVector3(0,0,1)); + } } } @@ -483,3 +487,4 @@ btSoftBody* btSoftBodyHelpers::CreateFromConvexHull(btSoftBody::btSoftBodyWorld return(psb); } + diff --git a/src/BulletSoftBody/btSoftRigidCollisionAlgorithm.cpp b/src/BulletSoftBody/btSoftRigidCollisionAlgorithm.cpp index 936205920..f6279fc0e 100644 --- a/src/BulletSoftBody/btSoftRigidCollisionAlgorithm.cpp +++ b/src/BulletSoftBody/btSoftRigidCollisionAlgorithm.cpp @@ -24,30 +24,12 @@ subject to the following restrictions: //#include -btSoftRigidCollisionAlgorithm::btSoftRigidCollisionAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* col0,btCollisionObject* col1, bool isSwapped) +btSoftRigidCollisionAlgorithm::btSoftRigidCollisionAlgorithm(btPersistentManifold* /*mf*/,const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* /*col0*/,btCollisionObject* /*col1*/, bool isSwapped) : btCollisionAlgorithm(ci), //m_ownManifold(false), //m_manifoldPtr(mf), m_isSwapped(isSwapped) { - - //m_softBody = m_isSwapped? (btSoftBody*)col1 : (btSoftBody*)col0; - //m_rigidCollisionObject = m_isSwapped? col0 : col1; - - //quick fix, add overlapping rigidbody to softbody, so it can be handled within btSoftBody::Step method - //m_softBody->m_overlappingRigidBodies.push_back(m_rigidCollisionObject); - - ///store the contacts straight into the btSoftBody for now? - - /*if (!m_manifoldPtr && m_dispatcher->needsCollision(sphereObj,boxObj)) - { - m_manifoldPtr = m_dispatcher->getNewManifold(sphereObj,boxObj); - m_ownManifold = true; - } - */ - - - } @@ -94,3 +76,4 @@ btScalar btSoftRigidCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* } + diff --git a/src/BulletSoftBody/btSoftRigidDynamicsWorld.cpp b/src/BulletSoftBody/btSoftRigidDynamicsWorld.cpp index 944245bbc..da2681ed7 100644 --- a/src/BulletSoftBody/btSoftRigidDynamicsWorld.cpp +++ b/src/BulletSoftBody/btSoftRigidDynamicsWorld.cpp @@ -46,7 +46,6 @@ void btSoftRigidDynamicsWorld::predictUnconstraintMotion(btScalar timeStep) void btSoftRigidDynamicsWorld::internalSingleStepSimulation( btScalar timeStep) { - btDiscreteDynamicsWorld::internalSingleStepSimulation( timeStep ); ///solve soft bodies constraints @@ -100,33 +99,19 @@ void btSoftRigidDynamicsWorld::debugDrawWorld() { btDiscreteDynamicsWorld::debugDrawWorld(); - if (getDebugDrawer() && getDebugDrawer()->getDebugMode() & (btIDebugDraw::DBG_DrawWireframe | btIDebugDraw::DBG_DrawAabb)) + if (getDebugDrawer()) { int i; for ( i=0;im_softBodies.size();i++) { btSoftBody* psb=(btSoftBody*)this->m_softBodies[i]; btSoftBodyHelpers::DrawFrame(psb,m_debugDrawer); - btSoftBodyHelpers::Draw(psb,m_debugDrawer,fDrawFlags::Std+fDrawFlags::Nodes); + btSoftBodyHelpers::Draw(psb,m_debugDrawer,fDrawFlags::Std); if (m_debugDrawer && (m_debugDrawer->getDebugMode() & btIDebugDraw::DBG_DrawAabb)) { btSoftBodyHelpers::DrawNodeTree(psb,m_debugDrawer); //btSoftBodyHelpers::DrawFaceTree(psb,m_debugDrawer); } } - - if (getDebugDrawer()) - { - for (int i=0;im_softBodies.size();i++) - { - btSoftBody* psb=(btSoftBody*)this->m_softBodies[i]; - btSoftBodyHelpers::DrawFrame(psb,m_debugDrawer); - btSoftBodyHelpers::Draw(psb,m_debugDrawer,fDrawFlags::Std); - if (m_debugDrawer && (m_debugDrawer->getDebugMode() & btIDebugDraw::DBG_DrawAabb)) - { - btSoftBodyHelpers::DrawNodeTree(psb,m_debugDrawer); - } - } - } } } diff --git a/src/BulletSoftBody/btSoftSoftCollisionAlgorithm.cpp b/src/BulletSoftBody/btSoftSoftCollisionAlgorithm.cpp index f623ce349..2ae8c26e2 100644 --- a/src/BulletSoftBody/btSoftSoftCollisionAlgorithm.cpp +++ b/src/BulletSoftBody/btSoftSoftCollisionAlgorithm.cpp @@ -21,84 +21,25 @@ subject to the following restrictions: #define USE_PERSISTENT_CONTACTS 1 -btSoftSoftCollisionAlgorithm::btSoftSoftCollisionAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* obj0,btCollisionObject* obj1) +btSoftSoftCollisionAlgorithm::btSoftSoftCollisionAlgorithm(btPersistentManifold* /*mf*/,const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* /*obj0*/,btCollisionObject* /*obj1*/) : btCollisionAlgorithm(ci) //m_ownManifold(false), //m_manifoldPtr(mf) { - /*m_softBody0 = (btSoftBody*) obj0; - m_softBody1 = (btSoftBody*) obj1; - - m_softBody0->m_overlappingSoftBodies.push_back(m_softBody1); - m_softBody1->m_overlappingSoftBodies.push_back(m_softBody0);*/ - - /*if (!m_manifoldPtr && m_dispatcher->needsCollision(obj0,obj1)) - { - m_manifoldPtr = m_dispatcher->getNewManifold(obj0,obj1); - m_ownManifold = true; - } - */ - } btSoftSoftCollisionAlgorithm::~btSoftSoftCollisionAlgorithm() { - //m_softBody0->m_overlappingSoftBodies.remove(m_softBody1); - //m_softBody1->m_overlappingSoftBodies.remove(m_softBody0); - - //this gets called when the overlap stops. - - //here is where contacts (manifolds) should be removed - - /* - if (m_ownManifold) - { - if (m_manifoldPtr) - m_dispatcher->releaseManifold(m_manifoldPtr); - } - */ - } -void btSoftSoftCollisionAlgorithm::processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut) +void btSoftSoftCollisionAlgorithm::processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& /*dispatchInfo*/,btManifoldResult* /*resultOut*/) { - btSoftBody* soft0 = (btSoftBody*)body0; btSoftBody* soft1 = (btSoftBody*)body1; - soft0->defaultCollisionHandler(soft1); - /* - btBoxShape* box0 = (btBoxShape*)col0->getCollisionShape(); - btBoxShape* box1 = (btBoxShape*)col1->getCollisionShape(); - - - - /// report a contact. internally this will be kept persistent, and contact reduction is done - resultOut->setPersistentManifold(m_manifoldPtr); -#ifndef USE_PERSISTENT_CONTACTS - m_manifoldPtr->clearManifold(); -#endif //USE_PERSISTENT_CONTACTS - - btDiscreteCollisionDetectorInterface::ClosestPointInput input; - input.m_maximumDistanceSquared = 1e30f; - input.m_transformA = body0->getWorldTransform(); - input.m_transformB = body1->getWorldTransform(); - - btBoxBoxDetector detector(box0,box1); - detector.getClosestPoints(input,*resultOut,dispatchInfo.m_debugDraw); - -#ifdef USE_PERSISTENT_CONTACTS - // refreshContactPoints is only necessary when using persistent contact points. otherwise all points are newly added - if (m_ownManifold) - { - resultOut->refreshContactPoints(); - } -#endif //USE_PERSISTENT_CONTACTS -*/ - } -btScalar btSoftSoftCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut) +btScalar btSoftSoftCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* /*body0*/,btCollisionObject* /*body1*/,const btDispatcherInfo& /*dispatchInfo*/,btManifoldResult* /*resultOut*/) { //not yet return 1.f;