| /* |
| 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 |