diff options
Diffstat (limited to 'extern/bullet2/BulletSoftBody/btSoftBodyInternals.h')
| -rw-r--r-- | extern/bullet2/BulletSoftBody/btSoftBodyInternals.h | 931 |
1 files changed, 0 insertions, 931 deletions
diff --git a/extern/bullet2/BulletSoftBody/btSoftBodyInternals.h b/extern/bullet2/BulletSoftBody/btSoftBodyInternals.h deleted file mode 100644 index 2cb7744cbb1..00000000000 --- a/extern/bullet2/BulletSoftBody/btSoftBodyInternals.h +++ /dev/null @@ -1,931 +0,0 @@ -/* -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 <typename T> -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<dim);return((r*(r+1))/2+c); } - T& operator()(int c,int r) { return(store[index(c,r)]); } - const T& operator()(int c,int r) const { return(store[index(c,r)]); } - btAlignedObjectArray<T> store; - int dim; -}; - -// -// btSoftBodyCollisionShape -// -class btSoftBodyCollisionShape : public btConcaveShape -{ -public: - btSoftBody* m_body; - - btSoftBodyCollisionShape(btSoftBody* backptr) - { - m_shapeType = SOFTBODY_SHAPE_PROXYTYPE; - 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 void setLocalScaling(const btVector3& /*scaling*/) - { - ///na - } - 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=btDot(vec,n[0]->m_x); - int j=0; - for(int i=1,ni=m_cluster->m_nodes.size();i<ni;++i) - { - const btScalar k=btDot(vec,n[i]->m_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 <typename T> -static inline void ZeroInitialize(T& value) -{ - static const T zerodummy; - value=zerodummy; -} -// -template <typename T> -static inline bool CompLess(const T& a,const T& b) -{ return(a<b); } -// -template <typename T> -static inline bool CompGreater(const T& a,const T& b) -{ return(a>b); } -// -template <typename T> -static inline T Lerp(const T& a,const T& b,btScalar t) -{ return(a+(b-a)*t); } -// -template <typename T> -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 <typename T> -static inline T Clamp(const T& x,const T& l,const T& h) -{ return(x<l?l:x>h?h:x); } -// -template <typename T> -static inline T Sq(const T& x) -{ return(x*x); } -// -template <typename T> -static inline T Cube(const T& x) -{ return(x*x*x); } -// -template <typename T> -static inline T Sign(const T& x) -{ return((T)(x<0?-1:+1)); } -// -template <typename T> -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]=btCross(m[0],m[1]).normalized(); - m[1]=btCross(m[2],m[0]).normalized(); - m[0]=btCross(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*btDot(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<btScalar>(-btDot(a,d)/m2,0,1); - const btVector3 p=a+d*t; - const btScalar l2=p.length2(); - if(l2<sqd) - { - prj=p; - sqd=l2; - } - } -} -// -static inline void ProjectOrigin( const btVector3& a, - const btVector3& b, - const btVector3& c, - btVector3& prj, - btScalar& sqd) -{ - const btVector3& q=btCross(b-a,c-a); - const btScalar m2=q.length2(); - if(m2>SIMD_EPSILON) - { - const btVector3 n=q/btSqrt(m2); - const btScalar k=btDot(a,n); - const btScalar k2=k*k; - if(k2<sqd) - { - const btVector3 p=n*k; - if( (btDot(btCross(a-p,b-p),q)>0)&& - (btDot(btCross(b-p,c-p),q)>0)&& - (btDot(btCross(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 <typename T> -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[]={ btCross(a-p,b-p).length(), - btCross(b-p,c-p).length(), - btCross(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;i<maxiterations;++i) - { - const btScalar t=Lerp(span[0],span[1],values[0]/(values[0]-values[1])); - const btScalar v=fn->Eval(Lerp(a,b,t)); - if((t<=0)||(t>=1)) break; - if(btFabs(v)<accuracy) return(t); - if(v<0) - { span[0]=t;values[0]=v; } - else - { span[1]=t;values[1]=v; } - } - return(-1); -} - -// -static inline btVector3 NormalizeAny(const btVector3& v) -{ - const btScalar l=v.length(); - if(l>SIMD_EPSILON) - return(v/l); - else - return(btVector3(0,0,0)); -} - -// -static 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=btCross(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(btDot(a,btCross(b,c))); -} - -// -static void EvaluateMedium( const 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=-(btDot(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])<btFabs(a[0][2])) { p=0;q=2; } - if(btFabs(a[p][q])<btFabs(a[1][2])) { p=1;q=2; } - 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)<maxiterations); - if(values) - { - *values=btVector3(a[0][0],a[1][1],a[2][2]); - } - return(iterations); - } -private: - static inline void mulTPQ(btMatrix3x3& a,btScalar c,btScalar s,int p,int q) - { - const btScalar m[2][3]={ {a[p][0],a[p][1],a[p][2]}, - {a[q][0],a[q][1],a[q][2]}}; - int i; - - for(i=0;i<3;++i) a[p][i]=c*m[0][i]-s*m[1][i]; - for(i=0;i<3;++i) a[q][i]=c*m[1][i]+s*m[0][i]; - } - static inline void mulPQ(btMatrix3x3& a,btScalar c,btScalar s,int p,int q) - { - const btScalar m[2][3]={ {a[0][p],a[1][p],a[2][p]}, - {a[0][q],a[1][q],a[2][q]}}; - int i; - - for(i=0;i<3;++i) a[i][p]=c*m[0][i]-s*m[1][i]; - for(i=0;i<3;++i) a[i][q]=c*m[1][i]+s*m[0][i]; - } -}; - -// -// Polar decomposition, -// "Computing the Polar Decomposition with Applications", Nicholas J. Higham, 1986. -// -static inline int PolarDecompose( const btMatrix3x3& m,btMatrix3x3& q,btMatrix3x3& s) -{ - static const btScalar half=(btScalar)0.5; - static const btScalar accuracy=(btScalar)0.0001; - static const int maxiterations=16; - int i=0; - btScalar det=0; - q = Mul(m,1/btVector3(m[0][0],m[1][1],m[2][2]).length()); - det = q.determinant(); - if(!btFuzzyZero(det)) - { - for(;i<maxiterations;++i) - { - q=Mul(Add(q,Mul(q.adjoint(),1/det).transpose()),half); - const btScalar ndet=q.determinant(); - if(Sq(ndet-det)>accuracy) det=ndet; else break; - } - /* 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 m_margin; - btScalar friction; - btScalar threshold; - ClusterBase() - { - erp =(btScalar)1; - idt =0; - m_margin =0; - friction =0; - threshold =(btScalar)0; - } - bool SolveContact( const btGjkEpaSolver2::sResults& res, - btSoftBody::Body ba,btSoftBody::Body bb, - btSoftBody::CJoint& joint) - { - if(res.distance<m_margin) - { - btVector3 norm = res.normal; - norm.normalize();//is it necessary? - - const btVector3 ra=res.witnesses[0]-ba.xform().getOrigin(); - const btVector3 rb=res.witnesses[1]-bb.xform().getOrigin(); - const btVector3 va=ba.velocity(ra); - const btVector3 vb=bb.velocity(rb); - const btVector3 vrel=va-vb; - const btScalar rvac=btDot(vrel,norm); - btScalar depth=res.distance-m_margin; - -// printf("depth=%f\n",depth); - const btVector3 iv=norm*rvac; - const btVector3 fv=vrel-iv; - joint.m_bodies[0] = ba; - joint.m_bodies[1] = bb; - joint.m_refs[0] = ra*ba.xform().getBasis(); - joint.m_refs[1] = rb*bb.xform().getBasis(); - joint.m_rpos[0] = ra; - joint.m_rpos[1] = rb; - joint.m_cfm = 1; - joint.m_erp = 1; - joint.m_life = 0; - joint.m_maxlife = 0; - joint.m_split = 1; - - joint.m_drift = depth*norm; - - joint.m_normal = norm; -// printf("normal=%f,%f,%f\n",res.normal.getX(),res.normal.getY(),res.normal.getZ()); - joint.m_delete = false; - joint.m_friction = fv.length2()<(-rvac*friction)?1:friction; - joint.m_massmatrix = ImpulseMatrix( ba.invMass(),ba.invWorldInertia(),joint.m_rpos[0], - bb.invMass(),bb.invWorldInertia(),joint.m_rpos[1]); - - return(true); - } - return(false); - } - }; - // - // CollideCL_RS - // - struct CollideCL_RS : ClusterBase - { - btSoftBody* psb; - - btCollisionObject* m_colObj; - void Process(const btDbvtNode* leaf) - { - btSoftBody::Cluster* cluster=(btSoftBody::Cluster*)leaf->data; - btSoftClusterCollisionShape cshape(cluster); - - const btConvexShape* rshape=(const btConvexShape*)m_colObj->getCollisionShape(); - - ///don't collide an anchored cluster with a static/kinematic object - if(m_colObj->isStaticOrKinematicObject() && cluster->m_containsAnchor) - return; - - btGjkEpaSolver2::sResults res; - if(btGjkEpaSolver2::SignedDistance( &cshape,btTransform::getIdentity(), - rshape,m_colObj->getInterpolationWorldTransform(), - btVector3(1,0,0),res)) - { - btSoftBody::CJoint joint; - if(SolveContact(res,cluster,m_colObj,joint))//prb,joint)) - { - btSoftBody::CJoint* pj=new(btAlignedAlloc(sizeof(btSoftBody::CJoint),16)) btSoftBody::CJoint(); - *pj=joint;psb->m_joints.push_back(pj); - if(m_colObj->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,btCollisionObject* colOb) - { - psb = ps; - m_colObj = colOb; - idt = ps->m_sst.isdt; - m_margin = m_colObj->getCollisionShape()->getMargin()+psb->getCollisionShape()->getMargin(); - ///Bullet rigid body uses multiply instead of minimum to determine combined friction. Some customization would be useful. - friction = btMin(psb->m_cfg.kDF,m_colObj->getFriction()); - btVector3 mins; - btVector3 maxs; - - ATTRIBUTE_ALIGNED16(btDbvtVolume) volume; - colOb->getCollisionShape()->getAabb(colOb->getInterpolationWorldTransform(),mins,maxs); - volume=btDbvtVolume::FromMM(mins,maxs); - volume.Expand(btVector3(1,1,1)*m_margin); - ps->m_cdbvt.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; - - - bool connected=false; - if ((bodies[0]==bodies[1])&&(bodies[0]->m_clusterConnectivity.size())) - { - connected = bodies[0]->m_clusterConnectivity[cla->m_clusterIndex+bodies[0]->m_clusters.size()*clb->m_clusterIndex]; - } - - if (!connected) - { - 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; - } - } - } else - { - static int count=0; - count++; - //printf("count=%d\n",count); - - } - } - void Process(btSoftBody* psa,btSoftBody* psb) - { - idt = psa->m_sst.isdt; - //m_margin = (psa->getCollisionShape()->getMargin()+psb->getCollisionShape()->getMargin())/2; - m_margin = (psa->getCollisionShape()->getMargin()+psb->getCollisionShape()->getMargin()); - friction = btMin(psa->m_cfg.kDF,psb->m_cfg.kDF); - bodies[0] = psa; - bodies[1] = psb; - psa->m_cdbvt.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(m_colObj1,n.m_x,m,c.m_cti)) - { - const btScalar ima=n.m_im; - const btScalar imb= m_rigidBody? m_rigidBody->getInvMass() : 0.f; - const btScalar ms=ima+imb; - if(ms>0) - { - const btTransform& wtr=m_rigidBody?m_rigidBody->getInterpolationWorldTransform() : m_colObj1->getWorldTransform(); - static const btMatrix3x3 iwiStatic(0,0,0,0,0,0,0,0,0); - const btMatrix3x3& iwi=m_rigidBody?m_rigidBody->getInvInertiaTensorWorld() : iwiStatic; - const btVector3 ra=n.m_x-wtr.getOrigin(); - const btVector3 va=m_rigidBody ? m_rigidBody->getVelocityInLocalPoint(ra)*psb->m_sst.sdt : btVector3(0,0,0); - const btVector3 vb=n.m_x-n.m_q; - const btVector3 vr=vb-va; - const btScalar dn=btDot(vr,c.m_cti.m_normal); - const btVector3 fv=vr-c.m_cti.m_normal*dn; - const btScalar fc=psb->m_cfg.kDF*m_colObj1->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 = m_colObj1->isStaticOrKinematicObject()?psb->m_cfg.kKHR:psb->m_cfg.kCHR; - psb->m_rcontacts.push_back(c); - if (m_rigidBody) - m_rigidBody->activate(); - } - } - } - btSoftBody* psb; - btCollisionObject* m_colObj1; - btRigidBody* m_rigidBody; - 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 |