diff options
Diffstat (limited to 'extern/bullet2/src/BulletSoftBody/btSoftBodyInternals.h')
-rw-r--r-- | extern/bullet2/src/BulletSoftBody/btSoftBodyInternals.h | 747 |
1 files changed, 383 insertions, 364 deletions
diff --git a/extern/bullet2/src/BulletSoftBody/btSoftBodyInternals.h b/extern/bullet2/src/BulletSoftBody/btSoftBodyInternals.h index 8fcf633fecc..5f0f7d54318 100644 --- a/extern/bullet2/src/BulletSoftBody/btSoftBodyInternals.h +++ b/extern/bullet2/src/BulletSoftBody/btSoftBodyInternals.h @@ -31,14 +31,14 @@ subject to the following restrictions: 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; + 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; }; // @@ -48,10 +48,11 @@ class btSoftBodyCollisionShape : public btConcaveShape { public: btSoftBody* m_body; - + btSoftBodyCollisionShape(btSoftBody* backptr) { - m_body=backptr; + m_shapeType = SOFTBODY_SHAPE_PROXYTYPE; + m_body=backptr; } virtual ~btSoftBodyCollisionShape() @@ -68,37 +69,34 @@ public: ///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) + /* 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]); + aabbMin.setMin(crns[i]); + aabbMax.setMax(crns[i]); } } - virtual int getShapeType() const - { - return SOFTBODY_SHAPE_PROXYTYPE; - } + virtual void setLocalScaling(const btVector3& /*scaling*/) { ///na } virtual const btVector3& getLocalScaling() const { - static const btVector3 dummy(1,1,1); - return dummy; + static const btVector3 dummy(1,1,1); + return dummy; } virtual void calculateLocalInertia(btScalar /*mass*/,btVector3& /*inertia*/) const { @@ -121,24 +119,24 @@ 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();i<ni;++i) - { + { const btScalar k=dot(vec,n[i]->m_x); if(k>d) { d=k;j=i; } - } - return(n[j]->m_x); } + 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 {} @@ -151,7 +149,7 @@ public: {} virtual int getShapeType() const { return SOFTBODY_SHAPE_PROXYTYPE; } - + //debugging virtual const char* getName()const {return "SOFTCLUSTER";} @@ -173,8 +171,8 @@ public: template <typename T> static inline void ZeroInitialize(T& value) { -static const T zerodummy; -value=zerodummy; + static const T zerodummy; + value=zerodummy; } // template <typename T> @@ -194,23 +192,23 @@ 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); + 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)); + const btScalar sql=v.length2(); + if(sql>(maxlength*maxlength)) + return((v*maxlength)/btSqrt(sql)); else - return(v); + return(v); } // template <typename T> @@ -235,8 +233,8 @@ static inline bool SameSign(const T& x,const T& y) // 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])); + const btVector3 d=x-y; + return(btFabs(d[0])+btFabs(d[1])+btFabs(d[2])); } // static inline btMatrix3x3 ScaleAlongAxis(const btVector3& a,btScalar s) @@ -273,7 +271,7 @@ static inline btMatrix3x3 Diagonal(btScalar x) } // static inline btMatrix3x3 Add(const btMatrix3x3& a, - const btMatrix3x3& b) + const btMatrix3x3& b) { btMatrix3x3 r; for(int i=0;i<3;++i) r[i]=a[i]+b[i]; @@ -281,7 +279,7 @@ static inline btMatrix3x3 Add(const btMatrix3x3& a, } // static inline btMatrix3x3 Sub(const btMatrix3x3& a, - const btMatrix3x3& b) + const btMatrix3x3& b) { btMatrix3x3 r; for(int i=0;i<3;++i) r[i]=a[i]-b[i]; @@ -289,7 +287,7 @@ static inline btMatrix3x3 Sub(const btMatrix3x3& a, } // static inline btMatrix3x3 Mul(const btMatrix3x3& a, - btScalar b) + btScalar b) { btMatrix3x3 r; for(int i=0;i<3;++i) r[i]=a[i]*b; @@ -298,9 +296,9 @@ static inline btMatrix3x3 Mul(const btMatrix3x3& a, // 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(); + 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) @@ -311,90 +309,90 @@ static inline btMatrix3x3 MassMatrix(btScalar im,const btMatrix3x3& iwi,const bt // static inline btMatrix3x3 ImpulseMatrix( btScalar dt, - btScalar ima, - btScalar imb, - const btMatrix3x3& iwi, - const btVector3& r) + 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) + btScalar imb,const btMatrix3x3& iib,const btVector3& rb) { -return(Add(MassMatrix(ima,iia,ra),MassMatrix(imb,iib,rb)).inverse()); + return(Add(MassMatrix(ima,iia,ra),MassMatrix(imb,iib,rb)).inverse()); } // static inline btMatrix3x3 AngularImpulseMatrix( const btMatrix3x3& iia, - const btMatrix3x3& iib) + const btMatrix3x3& iib) { -return(Add(iia,iib).inverse()); + return(Add(iia,iib).inverse()); } // static inline btVector3 ProjectOnAxis( const btVector3& v, - const btVector3& a) + const btVector3& a) { return(a*dot(v,a)); } // static inline btVector3 ProjectOnPlane( const btVector3& v, - const btVector3& a) + const btVector3& a) { return(v-ProjectOnAxis(v,a)); } // static inline void ProjectOrigin( const btVector3& a, - const btVector3& b, - btVector3& prj, - btScalar& sqd) + const btVector3& b, + btVector3& prj, + btScalar& sqd) { -const btVector3 d=b-a; -const btScalar m2=d.length2(); -if(m2>SIMD_EPSILON) + const btVector3 d=b-a; + const btScalar m2=d.length2(); + if(m2>SIMD_EPSILON) { - const btScalar t=Clamp<btScalar>(-dot(a,d)/m2,0,1); - const btVector3 p=a+d*t; - const btScalar l2=p.length2(); - if(l2<sqd) + const btScalar t=Clamp<btScalar>(-dot(a,d)/m2,0,1); + const btVector3 p=a+d*t; + const btScalar l2=p.length2(); + if(l2<sqd) { - prj=p; - sqd=l2; + prj=p; + sqd=l2; } } } // static inline void ProjectOrigin( const btVector3& a, - const btVector3& b, - const btVector3& c, - btVector3& prj, - btScalar& sqd) -{ -const btVector3& q=cross(b-a,c-a); -const btScalar m2=q.length2(); -if(m2>SIMD_EPSILON) + const btVector3& b, + const btVector3& c, + btVector3& prj, + btScalar& sqd) +{ + const btVector3& q=cross(b-a,c-a); + const btScalar m2=q.length2(); + if(m2>SIMD_EPSILON) { - const btVector3 n=q/btSqrt(m2); - const btScalar k=dot(a,n); - const btScalar k2=k*k; - if(k2<sqd) + const btVector3 n=q/btSqrt(m2); + const btScalar k=dot(a,n); + const btScalar k2=k*k; + if(k2<sqd) { - const btVector3 p=n*k; - if( (dot(cross(a-p,b-p),q)>0)&& - (dot(cross(b-p,c-p),q)>0)&& - (dot(cross(c-p,a-p),q)>0)) + const btVector3 p=n*k; + if( (dot(cross(a-p,b-p),q)>0)&& + (dot(cross(b-p,c-p),q)>0)&& + (dot(cross(c-p,a-p),q)>0)) { - prj=p; - sqd=k2; + prj=p; + sqd=k2; } else { - ProjectOrigin(a,b,prj,sqd); - ProjectOrigin(b,c,prj,sqd); - ProjectOrigin(c,a,prj,sqd); + ProjectOrigin(a,b,prj,sqd); + ProjectOrigin(b,c,prj,sqd); + ProjectOrigin(c,a,prj,sqd); } } } @@ -403,53 +401,53 @@ if(m2>SIMD_EPSILON) // template <typename T> static inline T BaryEval( const T& a, - const T& b, - const T& c, - const btVector3& coord) + 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)); + 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]) + 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]); + 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) + 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) + 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); + return(-1); } // @@ -464,26 +462,26 @@ static inline btVector3 NormalizeAny(const btVector3& v) // 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); + 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); + 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& x1, + const btVector3& x2) { const btVector3 a=x1-x0; const btVector3 b=x2-x0; @@ -494,9 +492,9 @@ static inline btScalar AreaOf( const btVector3& x0, // static inline btScalar VolumeOf( const btVector3& x0, - const btVector3& x1, - const btVector3& x2, - const btVector3& x3) + const btVector3& x1, + const btVector3& x2, + const btVector3& x3) { const btVector3 a=x1-x0; const btVector3 b=x2-x0; @@ -506,8 +504,8 @@ static inline btScalar VolumeOf( const btVector3& x0, // static void EvaluateMedium( const btSoftBodyWorldInfo* wfi, - const btVector3& x, - btSoftBody::sMedium& medium) + const btVector3& x, + btSoftBody::sMedium& medium) { medium.m_velocity = btVector3(0,0,0); medium.m_pressure = 0; @@ -525,8 +523,8 @@ static void EvaluateMedium( const btSoftBodyWorldInfo* wfi, // static inline void ApplyClampedForce( btSoftBody::Node& n, - const btVector3& f, - btScalar dt) + const btVector3& f, + btScalar dt) { const btScalar dtim=dt*n.m_im; if((f*dtim).length2()>n.m_v.length2()) @@ -541,13 +539,13 @@ static inline void ApplyClampedForce( btSoftBody::Node& n, // static inline int MatchEdge( const btSoftBody::Node* a, - const btSoftBody::Node* b, - const btSoftBody::Node* ma, - const btSoftBody::Node* mb) + 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); + if((a==ma)&&(b==mb)) return(0); + if((a==mb)&&(b==ma)) return(1); + return(-1); } // @@ -557,56 +555,56 @@ return(-1); // struct btEigen { -static int system(btMatrix3x3& a,btMatrix3x3* vectors,btVector3* values=0) + 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) + 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 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); + 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) + if(values) { - *values=btVector3(a[0][0],a[1][1],a[2][2]); + *values=btVector3(a[0][0],a[1][1],a[2][2]); } - return(iterations); + return(iterations); } private: -static inline void mulTPQ(btMatrix3x3& a,btScalar c,btScalar s,int p,int q) + 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; + 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]; + 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) + 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; + 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]; + 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]; } }; @@ -641,7 +639,7 @@ static inline int PolarDecompose( const btMatrix3x3& m,btMatrix3x3& q,btMatrix q.setIdentity(); s.setIdentity(); } -return(i); + return(i); } // @@ -654,54 +652,54 @@ struct btSoftColliders // struct ClusterBase : btDbvt::ICollide { - btScalar erp; - btScalar idt; - btScalar margin; - btScalar friction; - btScalar threshold; - ClusterBase() + btScalar erp; + btScalar idt; + btScalar margin; + btScalar friction; + btScalar threshold; + ClusterBase() { - erp =(btScalar)1; - idt =0; - margin =0; - friction =0; - threshold =(btScalar)0; + 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) + bool SolveContact( const btGjkEpaSolver2::sResults& res, + btSoftBody::Body ba,btSoftBody::Body bb, + btSoftBody::CJoint& joint) { - if(res.distance<margin) + if(res.distance<margin) { - 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=dot(vrel,res.normal); - const btScalar depth=res.distance-margin; - const btVector3 iv=res.normal*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*res.normal; - joint.m_normal = res.normal; - 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); + 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=dot(vrel,res.normal); + const btScalar depth=res.distance-margin; + const btVector3 iv=res.normal*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*res.normal; + joint.m_normal = res.normal; + 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); + return(false); } }; // @@ -709,52 +707,53 @@ struct btSoftColliders // struct CollideCL_RS : ClusterBase { - btSoftBody* psb; - btRigidBody* prb; - void Process(const btDbvtNode* leaf) + 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*)prb->getCollisionShape(); - btGjkEpaSolver2::sResults res; - if(btGjkEpaSolver2::SignedDistance( &cshape,btTransform::getIdentity(), - rshape,prb->getInterpolationWorldTransform(), - btVector3(1,0,0),res)) + btSoftBody::Cluster* cluster=(btSoftBody::Cluster*)leaf->data; + btSoftClusterCollisionShape cshape(cluster); + const btConvexShape* rshape=(const btConvexShape*)m_colObj->getCollisionShape(); + 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,prb,joint)) + 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(prb->isStaticOrKinematicObject()) + 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; + 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; + pj->m_erp *= psb->m_cfg.kSRHR_CL; + pj->m_split *= psb->m_cfg.kSR_SPLT_CL; } } } } - void Process(btSoftBody* ps,btRigidBody* pr) + void Process(btSoftBody* ps,btCollisionObject* colOb) { - 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; - - ATTRIBUTE_ALIGNED16(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); + psb = ps; + m_colObj = colOb; + idt = ps->m_sst.isdt; + margin = m_colObj->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)*margin); + ps->m_cdbvt.collideTV(ps->m_cdbvt.m_root,volume,*this); } }; // @@ -762,36 +761,53 @@ struct btSoftColliders // struct CollideCL_SS : ClusterBase { - btSoftBody* bodies[2]; - void Process(const btDbvtNode* la,const btDbvtNode* lb) + 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::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())) { - btSoftBody::CJoint joint; - if(SolveContact(res,cla,clb,joint)) + 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* 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; + 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) + 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); + 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; + psa->m_cdbvt.collideTT(psa->m_cdbvt.m_root,psb->m_cdbvt.m_root,*this); } }; // @@ -799,96 +815,99 @@ struct btSoftColliders // struct CollideSDF_RS : btDbvt::ICollide { - void Process(const btDbvtNode* leaf) + void Process(const btDbvtNode* leaf) { - btSoftBody::Node* node=(btSoftBody::Node*)leaf->data; - DoNode(*node); + btSoftBody::Node* node=(btSoftBody::Node*)leaf->data; + DoNode(*node); } - void DoNode(btSoftBody::Node& n) const + 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 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=prb->getInvMass(); - const btScalar ms=ima+imb; - if(ms>0) + 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=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(); + 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=dot(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; - btRigidBody* prb; - btScalar dynmargin; - btScalar stamargin; + 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) + 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)) + 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)) + 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; + mb=0; } - const btScalar ms=ma+mb; - if(ms>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::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; + btSoftBody* psb[2]; + btScalar mrg; }; }; |