00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00016
00017 #ifndef _BT_SOFT_BODY_INTERNALS_H
00018 #define _BT_SOFT_BODY_INTERNALS_H
00019
00020 #include "btSoftBody.h"
00021
00022
00023 #include "LinearMath/btQuickprof.h"
00024 #include "BulletCollision/BroadphaseCollision/btBroadphaseInterface.h"
00025 #include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h"
00026 #include "BulletCollision/CollisionShapes/btConvexInternalShape.h"
00027 #include "BulletCollision/NarrowPhaseCollision/btGjkEpa2.h"
00028 #include <string.h>
00029
00030
00031
00032 template <typename T>
00033 struct btSymMatrix
00034 {
00035 btSymMatrix() : dim(0) {}
00036 btSymMatrix(int n,const T& init=T()) { resize(n,init); }
00037 void resize(int n,const T& init=T()) { dim=n;store.resize((n*(n+1))/2,init); }
00038 int index(int c,int r) const { if(c>r) btSwap(c,r);btAssert(r<dim);return((r*(r+1))/2+c); }
00039 T& operator()(int c,int r) { return(store[index(c,r)]); }
00040 const T& operator()(int c,int r) const { return(store[index(c,r)]); }
00041 btAlignedObjectArray<T> store;
00042 int dim;
00043 };
00044
00045
00046
00047
00048 class btSoftBodyCollisionShape : public btConcaveShape
00049 {
00050 public:
00051 btSoftBody* m_body;
00052
00053 btSoftBodyCollisionShape(btSoftBody* backptr)
00054 {
00055 m_shapeType = SOFTBODY_SHAPE_PROXYTYPE;
00056 m_body=backptr;
00057 }
00058
00059 virtual ~btSoftBodyCollisionShape()
00060 {
00061
00062 }
00063
00064 void processAllTriangles(btTriangleCallback* ,const btVector3& ,const btVector3& ) const
00065 {
00066
00067 btAssert(0);
00068 }
00069
00071 virtual void getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const
00072 {
00073
00074 const btVector3 mins=m_body->m_bounds[0];
00075 const btVector3 maxs=m_body->m_bounds[1];
00076 const btVector3 crns[]={t*btVector3(mins.x(),mins.y(),mins.z()),
00077 t*btVector3(maxs.x(),mins.y(),mins.z()),
00078 t*btVector3(maxs.x(),maxs.y(),mins.z()),
00079 t*btVector3(mins.x(),maxs.y(),mins.z()),
00080 t*btVector3(mins.x(),mins.y(),maxs.z()),
00081 t*btVector3(maxs.x(),mins.y(),maxs.z()),
00082 t*btVector3(maxs.x(),maxs.y(),maxs.z()),
00083 t*btVector3(mins.x(),maxs.y(),maxs.z())};
00084 aabbMin=aabbMax=crns[0];
00085 for(int i=1;i<8;++i)
00086 {
00087 aabbMin.setMin(crns[i]);
00088 aabbMax.setMax(crns[i]);
00089 }
00090 }
00091
00092
00093 virtual void setLocalScaling(const btVector3& )
00094 {
00096 }
00097 virtual const btVector3& getLocalScaling() const
00098 {
00099 static const btVector3 dummy(1,1,1);
00100 return dummy;
00101 }
00102 virtual void calculateLocalInertia(btScalar ,btVector3& ) const
00103 {
00105 btAssert(0);
00106 }
00107 virtual const char* getName()const
00108 {
00109 return "SoftBody";
00110 }
00111
00112 };
00113
00114
00115
00116
00117 class btSoftClusterCollisionShape : public btConvexInternalShape
00118 {
00119 public:
00120 const btSoftBody::Cluster* m_cluster;
00121
00122 btSoftClusterCollisionShape (const btSoftBody::Cluster* cluster) : m_cluster(cluster) { setMargin(0); }
00123
00124
00125 virtual btVector3 localGetSupportingVertex(const btVector3& vec) const
00126 {
00127 btSoftBody::Node* const * n=&m_cluster->m_nodes[0];
00128 btScalar d=btDot(vec,n[0]->m_x);
00129 int j=0;
00130 for(int i=1,ni=m_cluster->m_nodes.size();i<ni;++i)
00131 {
00132 const btScalar k=btDot(vec,n[i]->m_x);
00133 if(k>d) { d=k;j=i; }
00134 }
00135 return(n[j]->m_x);
00136 }
00137 virtual btVector3 localGetSupportingVertexWithoutMargin(const btVector3& vec)const
00138 {
00139 return(localGetSupportingVertex(vec));
00140 }
00141
00142 virtual void batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const
00143 {}
00144
00145
00146 virtual void calculateLocalInertia(btScalar mass,btVector3& inertia) const
00147 {}
00148
00149 virtual void getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const
00150 {}
00151
00152 virtual int getShapeType() const { return SOFTBODY_SHAPE_PROXYTYPE; }
00153
00154
00155 virtual const char* getName()const {return "SOFTCLUSTER";}
00156
00157 virtual void setMargin(btScalar margin)
00158 {
00159 btConvexInternalShape::setMargin(margin);
00160 }
00161 virtual btScalar getMargin() const
00162 {
00163 return getMargin();
00164 }
00165 };
00166
00167
00168
00169
00170
00171
00172 template <typename T>
00173 static inline void ZeroInitialize(T& value)
00174 {
00175 memset(&value,0,sizeof(T));
00176 }
00177
00178 template <typename T>
00179 static inline bool CompLess(const T& a,const T& b)
00180 { return(a<b); }
00181
00182 template <typename T>
00183 static inline bool CompGreater(const T& a,const T& b)
00184 { return(a>b); }
00185
00186 template <typename T>
00187 static inline T Lerp(const T& a,const T& b,btScalar t)
00188 { return(a+(b-a)*t); }
00189
00190 template <typename T>
00191 static inline T InvLerp(const T& a,const T& b,btScalar t)
00192 { return((b+a*t-b*t)/(a*b)); }
00193
00194 static inline btMatrix3x3 Lerp( const btMatrix3x3& a,
00195 const btMatrix3x3& b,
00196 btScalar t)
00197 {
00198 btMatrix3x3 r;
00199 r[0]=Lerp(a[0],b[0],t);
00200 r[1]=Lerp(a[1],b[1],t);
00201 r[2]=Lerp(a[2],b[2],t);
00202 return(r);
00203 }
00204
00205 static inline btVector3 Clamp(const btVector3& v,btScalar maxlength)
00206 {
00207 const btScalar sql=v.length2();
00208 if(sql>(maxlength*maxlength))
00209 return((v*maxlength)/btSqrt(sql));
00210 else
00211 return(v);
00212 }
00213
00214 template <typename T>
00215 static inline T Clamp(const T& x,const T& l,const T& h)
00216 { return(x<l?l:x>h?h:x); }
00217
00218 template <typename T>
00219 static inline T Sq(const T& x)
00220 { return(x*x); }
00221
00222 template <typename T>
00223 static inline T Cube(const T& x)
00224 { return(x*x*x); }
00225
00226 template <typename T>
00227 static inline T Sign(const T& x)
00228 { return((T)(x<0?-1:+1)); }
00229
00230 template <typename T>
00231 static inline bool SameSign(const T& x,const T& y)
00232 { return((x*y)>0); }
00233
00234 static inline btScalar ClusterMetric(const btVector3& x,const btVector3& y)
00235 {
00236 const btVector3 d=x-y;
00237 return(btFabs(d[0])+btFabs(d[1])+btFabs(d[2]));
00238 }
00239
00240 static inline btMatrix3x3 ScaleAlongAxis(const btVector3& a,btScalar s)
00241 {
00242 const btScalar xx=a.x()*a.x();
00243 const btScalar yy=a.y()*a.y();
00244 const btScalar zz=a.z()*a.z();
00245 const btScalar xy=a.x()*a.y();
00246 const btScalar yz=a.y()*a.z();
00247 const btScalar zx=a.z()*a.x();
00248 btMatrix3x3 m;
00249 m[0]=btVector3(1-xx+xx*s,xy*s-xy,zx*s-zx);
00250 m[1]=btVector3(xy*s-xy,1-yy+yy*s,yz*s-yz);
00251 m[2]=btVector3(zx*s-zx,yz*s-yz,1-zz+zz*s);
00252 return(m);
00253 }
00254
00255 static inline btMatrix3x3 Cross(const btVector3& v)
00256 {
00257 btMatrix3x3 m;
00258 m[0]=btVector3(0,-v.z(),+v.y());
00259 m[1]=btVector3(+v.z(),0,-v.x());
00260 m[2]=btVector3(-v.y(),+v.x(),0);
00261 return(m);
00262 }
00263
00264 static inline btMatrix3x3 Diagonal(btScalar x)
00265 {
00266 btMatrix3x3 m;
00267 m[0]=btVector3(x,0,0);
00268 m[1]=btVector3(0,x,0);
00269 m[2]=btVector3(0,0,x);
00270 return(m);
00271 }
00272
00273 static inline btMatrix3x3 Add(const btMatrix3x3& a,
00274 const btMatrix3x3& b)
00275 {
00276 btMatrix3x3 r;
00277 for(int i=0;i<3;++i) r[i]=a[i]+b[i];
00278 return(r);
00279 }
00280
00281 static inline btMatrix3x3 Sub(const btMatrix3x3& a,
00282 const btMatrix3x3& b)
00283 {
00284 btMatrix3x3 r;
00285 for(int i=0;i<3;++i) r[i]=a[i]-b[i];
00286 return(r);
00287 }
00288
00289 static inline btMatrix3x3 Mul(const btMatrix3x3& a,
00290 btScalar b)
00291 {
00292 btMatrix3x3 r;
00293 for(int i=0;i<3;++i) r[i]=a[i]*b;
00294 return(r);
00295 }
00296
00297 static inline void Orthogonalize(btMatrix3x3& m)
00298 {
00299 m[2]=btCross(m[0],m[1]).normalized();
00300 m[1]=btCross(m[2],m[0]).normalized();
00301 m[0]=btCross(m[1],m[2]).normalized();
00302 }
00303
00304 static inline btMatrix3x3 MassMatrix(btScalar im,const btMatrix3x3& iwi,const btVector3& r)
00305 {
00306 const btMatrix3x3 cr=Cross(r);
00307 return(Sub(Diagonal(im),cr*iwi*cr));
00308 }
00309
00310
00311 static inline btMatrix3x3 ImpulseMatrix( btScalar dt,
00312 btScalar ima,
00313 btScalar imb,
00314 const btMatrix3x3& iwi,
00315 const btVector3& r)
00316 {
00317 return(Diagonal(1/dt)*Add(Diagonal(ima),MassMatrix(imb,iwi,r)).inverse());
00318 }
00319
00320
00321 static inline btMatrix3x3 ImpulseMatrix( btScalar ima,const btMatrix3x3& iia,const btVector3& ra,
00322 btScalar imb,const btMatrix3x3& iib,const btVector3& rb)
00323 {
00324 return(Add(MassMatrix(ima,iia,ra),MassMatrix(imb,iib,rb)).inverse());
00325 }
00326
00327
00328 static inline btMatrix3x3 AngularImpulseMatrix( const btMatrix3x3& iia,
00329 const btMatrix3x3& iib)
00330 {
00331 return(Add(iia,iib).inverse());
00332 }
00333
00334
00335 static inline btVector3 ProjectOnAxis( const btVector3& v,
00336 const btVector3& a)
00337 {
00338 return(a*btDot(v,a));
00339 }
00340
00341 static inline btVector3 ProjectOnPlane( const btVector3& v,
00342 const btVector3& a)
00343 {
00344 return(v-ProjectOnAxis(v,a));
00345 }
00346
00347
00348 static inline void ProjectOrigin( const btVector3& a,
00349 const btVector3& b,
00350 btVector3& prj,
00351 btScalar& sqd)
00352 {
00353 const btVector3 d=b-a;
00354 const btScalar m2=d.length2();
00355 if(m2>SIMD_EPSILON)
00356 {
00357 const btScalar t=Clamp<btScalar>(-btDot(a,d)/m2,0,1);
00358 const btVector3 p=a+d*t;
00359 const btScalar l2=p.length2();
00360 if(l2<sqd)
00361 {
00362 prj=p;
00363 sqd=l2;
00364 }
00365 }
00366 }
00367
00368 static inline void ProjectOrigin( const btVector3& a,
00369 const btVector3& b,
00370 const btVector3& c,
00371 btVector3& prj,
00372 btScalar& sqd)
00373 {
00374 const btVector3& q=btCross(b-a,c-a);
00375 const btScalar m2=q.length2();
00376 if(m2>SIMD_EPSILON)
00377 {
00378 const btVector3 n=q/btSqrt(m2);
00379 const btScalar k=btDot(a,n);
00380 const btScalar k2=k*k;
00381 if(k2<sqd)
00382 {
00383 const btVector3 p=n*k;
00384 if( (btDot(btCross(a-p,b-p),q)>0)&&
00385 (btDot(btCross(b-p,c-p),q)>0)&&
00386 (btDot(btCross(c-p,a-p),q)>0))
00387 {
00388 prj=p;
00389 sqd=k2;
00390 }
00391 else
00392 {
00393 ProjectOrigin(a,b,prj,sqd);
00394 ProjectOrigin(b,c,prj,sqd);
00395 ProjectOrigin(c,a,prj,sqd);
00396 }
00397 }
00398 }
00399 }
00400
00401
00402 template <typename T>
00403 static inline T BaryEval( const T& a,
00404 const T& b,
00405 const T& c,
00406 const btVector3& coord)
00407 {
00408 return(a*coord.x()+b*coord.y()+c*coord.z());
00409 }
00410
00411 static inline btVector3 BaryCoord( const btVector3& a,
00412 const btVector3& b,
00413 const btVector3& c,
00414 const btVector3& p)
00415 {
00416 const btScalar w[]={ btCross(a-p,b-p).length(),
00417 btCross(b-p,c-p).length(),
00418 btCross(c-p,a-p).length()};
00419 const btScalar isum=1/(w[0]+w[1]+w[2]);
00420 return(btVector3(w[1]*isum,w[2]*isum,w[0]*isum));
00421 }
00422
00423
00424 static btScalar ImplicitSolve( btSoftBody::ImplicitFn* fn,
00425 const btVector3& a,
00426 const btVector3& b,
00427 const btScalar accuracy,
00428 const int maxiterations=256)
00429 {
00430 btScalar span[2]={0,1};
00431 btScalar values[2]={fn->Eval(a),fn->Eval(b)};
00432 if(values[0]>values[1])
00433 {
00434 btSwap(span[0],span[1]);
00435 btSwap(values[0],values[1]);
00436 }
00437 if(values[0]>-accuracy) return(-1);
00438 if(values[1]<+accuracy) return(-1);
00439 for(int i=0;i<maxiterations;++i)
00440 {
00441 const btScalar t=Lerp(span[0],span[1],values[0]/(values[0]-values[1]));
00442 const btScalar v=fn->Eval(Lerp(a,b,t));
00443 if((t<=0)||(t>=1)) break;
00444 if(btFabs(v)<accuracy) return(t);
00445 if(v<0)
00446 { span[0]=t;values[0]=v; }
00447 else
00448 { span[1]=t;values[1]=v; }
00449 }
00450 return(-1);
00451 }
00452
00453
00454 static inline btVector3 NormalizeAny(const btVector3& v)
00455 {
00456 const btScalar l=v.length();
00457 if(l>SIMD_EPSILON)
00458 return(v/l);
00459 else
00460 return(btVector3(0,0,0));
00461 }
00462
00463
00464 static inline btDbvtVolume VolumeOf( const btSoftBody::Face& f,
00465 btScalar margin)
00466 {
00467 const btVector3* pts[]={ &f.m_n[0]->m_x,
00468 &f.m_n[1]->m_x,
00469 &f.m_n[2]->m_x};
00470 btDbvtVolume vol=btDbvtVolume::FromPoints(pts,3);
00471 vol.Expand(btVector3(margin,margin,margin));
00472 return(vol);
00473 }
00474
00475
00476 static inline btVector3 CenterOf( const btSoftBody::Face& f)
00477 {
00478 return((f.m_n[0]->m_x+f.m_n[1]->m_x+f.m_n[2]->m_x)/3);
00479 }
00480
00481
00482 static inline btScalar AreaOf( const btVector3& x0,
00483 const btVector3& x1,
00484 const btVector3& x2)
00485 {
00486 const btVector3 a=x1-x0;
00487 const btVector3 b=x2-x0;
00488 const btVector3 cr=btCross(a,b);
00489 const btScalar area=cr.length();
00490 return(area);
00491 }
00492
00493
00494 static inline btScalar VolumeOf( const btVector3& x0,
00495 const btVector3& x1,
00496 const btVector3& x2,
00497 const btVector3& x3)
00498 {
00499 const btVector3 a=x1-x0;
00500 const btVector3 b=x2-x0;
00501 const btVector3 c=x3-x0;
00502 return(btDot(a,btCross(b,c)));
00503 }
00504
00505
00506 static void EvaluateMedium( const btSoftBodyWorldInfo* wfi,
00507 const btVector3& x,
00508 btSoftBody::sMedium& medium)
00509 {
00510 medium.m_velocity = btVector3(0,0,0);
00511 medium.m_pressure = 0;
00512 medium.m_density = wfi->air_density;
00513 if(wfi->water_density>0)
00514 {
00515 const btScalar depth=-(btDot(x,wfi->water_normal)+wfi->water_offset);
00516 if(depth>0)
00517 {
00518 medium.m_density = wfi->water_density;
00519 medium.m_pressure = depth*wfi->water_density*wfi->m_gravity.length();
00520 }
00521 }
00522 }
00523
00524
00525 static inline void ApplyClampedForce( btSoftBody::Node& n,
00526 const btVector3& f,
00527 btScalar dt)
00528 {
00529 const btScalar dtim=dt*n.m_im;
00530 if((f*dtim).length2()>n.m_v.length2())
00531 {
00532 n.m_f-=ProjectOnAxis(n.m_v,f.normalized())/dtim;
00533 }
00534 else
00535 {
00536 n.m_f+=f;
00537 }
00538 }
00539
00540
00541 static inline int MatchEdge( const btSoftBody::Node* a,
00542 const btSoftBody::Node* b,
00543 const btSoftBody::Node* ma,
00544 const btSoftBody::Node* mb)
00545 {
00546 if((a==ma)&&(b==mb)) return(0);
00547 if((a==mb)&&(b==ma)) return(1);
00548 return(-1);
00549 }
00550
00551
00552
00553
00554
00555
00556 struct btEigen
00557 {
00558 static int system(btMatrix3x3& a,btMatrix3x3* vectors,btVector3* values=0)
00559 {
00560 static const int maxiterations=16;
00561 static const btScalar accuracy=(btScalar)0.0001;
00562 btMatrix3x3& v=*vectors;
00563 int iterations=0;
00564 vectors->setIdentity();
00565 do {
00566 int p=0,q=1;
00567 if(btFabs(a[p][q])<btFabs(a[0][2])) { p=0;q=2; }
00568 if(btFabs(a[p][q])<btFabs(a[1][2])) { p=1;q=2; }
00569 if(btFabs(a[p][q])>accuracy)
00570 {
00571 const btScalar w=(a[q][q]-a[p][p])/(2*a[p][q]);
00572 const btScalar z=btFabs(w);
00573 const btScalar t=w/(z*(btSqrt(1+w*w)+z));
00574 if(t==t)
00575 {
00576 const btScalar c=1/btSqrt(t*t+1);
00577 const btScalar s=c*t;
00578 mulPQ(a,c,s,p,q);
00579 mulTPQ(a,c,s,p,q);
00580 mulPQ(v,c,s,p,q);
00581 } else break;
00582 } else break;
00583 } while((++iterations)<maxiterations);
00584 if(values)
00585 {
00586 *values=btVector3(a[0][0],a[1][1],a[2][2]);
00587 }
00588 return(iterations);
00589 }
00590 private:
00591 static inline void mulTPQ(btMatrix3x3& a,btScalar c,btScalar s,int p,int q)
00592 {
00593 const btScalar m[2][3]={ {a[p][0],a[p][1],a[p][2]},
00594 {a[q][0],a[q][1],a[q][2]}};
00595 int i;
00596
00597 for(i=0;i<3;++i) a[p][i]=c*m[0][i]-s*m[1][i];
00598 for(i=0;i<3;++i) a[q][i]=c*m[1][i]+s*m[0][i];
00599 }
00600 static inline void mulPQ(btMatrix3x3& a,btScalar c,btScalar s,int p,int q)
00601 {
00602 const btScalar m[2][3]={ {a[0][p],a[1][p],a[2][p]},
00603 {a[0][q],a[1][q],a[2][q]}};
00604 int i;
00605
00606 for(i=0;i<3;++i) a[i][p]=c*m[0][i]-s*m[1][i];
00607 for(i=0;i<3;++i) a[i][q]=c*m[1][i]+s*m[0][i];
00608 }
00609 };
00610
00611
00612
00613
00614
00615 static inline int PolarDecompose( const btMatrix3x3& m,btMatrix3x3& q,btMatrix3x3& s)
00616 {
00617 static const btScalar half=(btScalar)0.5;
00618 static const btScalar accuracy=(btScalar)0.0001;
00619 static const int maxiterations=16;
00620 int i=0;
00621 btScalar det=0;
00622 q = Mul(m,1/btVector3(m[0][0],m[1][1],m[2][2]).length());
00623 det = q.determinant();
00624 if(!btFuzzyZero(det))
00625 {
00626 for(;i<maxiterations;++i)
00627 {
00628 q=Mul(Add(q,Mul(q.adjoint(),1/det).transpose()),half);
00629 const btScalar ndet=q.determinant();
00630 if(Sq(ndet-det)>accuracy) det=ndet; else break;
00631 }
00632
00633 Orthogonalize(q);
00634
00635 s=q.transpose()*m;
00636 }
00637 else
00638 {
00639 q.setIdentity();
00640 s.setIdentity();
00641 }
00642 return(i);
00643 }
00644
00645
00646
00647
00648 struct btSoftColliders
00649 {
00650
00651
00652
00653 struct ClusterBase : btDbvt::ICollide
00654 {
00655 btScalar erp;
00656 btScalar idt;
00657 btScalar m_margin;
00658 btScalar friction;
00659 btScalar threshold;
00660 ClusterBase()
00661 {
00662 erp =(btScalar)1;
00663 idt =0;
00664 m_margin =0;
00665 friction =0;
00666 threshold =(btScalar)0;
00667 }
00668 bool SolveContact( const btGjkEpaSolver2::sResults& res,
00669 btSoftBody::Body ba,btSoftBody::Body bb,
00670 btSoftBody::CJoint& joint)
00671 {
00672 if(res.distance<m_margin)
00673 {
00674 btVector3 norm = res.normal;
00675 norm.normalize();
00676
00677 const btVector3 ra=res.witnesses[0]-ba.xform().getOrigin();
00678 const btVector3 rb=res.witnesses[1]-bb.xform().getOrigin();
00679 const btVector3 va=ba.velocity(ra);
00680 const btVector3 vb=bb.velocity(rb);
00681 const btVector3 vrel=va-vb;
00682 const btScalar rvac=btDot(vrel,norm);
00683 btScalar depth=res.distance-m_margin;
00684
00685
00686 const btVector3 iv=norm*rvac;
00687 const btVector3 fv=vrel-iv;
00688 joint.m_bodies[0] = ba;
00689 joint.m_bodies[1] = bb;
00690 joint.m_refs[0] = ra*ba.xform().getBasis();
00691 joint.m_refs[1] = rb*bb.xform().getBasis();
00692 joint.m_rpos[0] = ra;
00693 joint.m_rpos[1] = rb;
00694 joint.m_cfm = 1;
00695 joint.m_erp = 1;
00696 joint.m_life = 0;
00697 joint.m_maxlife = 0;
00698 joint.m_split = 1;
00699
00700 joint.m_drift = depth*norm;
00701
00702 joint.m_normal = norm;
00703
00704 joint.m_delete = false;
00705 joint.m_friction = fv.length2()<(-rvac*friction)?1:friction;
00706 joint.m_massmatrix = ImpulseMatrix( ba.invMass(),ba.invWorldInertia(),joint.m_rpos[0],
00707 bb.invMass(),bb.invWorldInertia(),joint.m_rpos[1]);
00708
00709 return(true);
00710 }
00711 return(false);
00712 }
00713 };
00714
00715
00716
00717 struct CollideCL_RS : ClusterBase
00718 {
00719 btSoftBody* psb;
00720
00721 btCollisionObject* m_colObj;
00722 void Process(const btDbvtNode* leaf)
00723 {
00724 btSoftBody::Cluster* cluster=(btSoftBody::Cluster*)leaf->data;
00725 btSoftClusterCollisionShape cshape(cluster);
00726
00727 const btConvexShape* rshape=(const btConvexShape*)m_colObj->getCollisionShape();
00728
00730 if(m_colObj->isStaticOrKinematicObject() && cluster->m_containsAnchor)
00731 return;
00732
00733 btGjkEpaSolver2::sResults res;
00734 if(btGjkEpaSolver2::SignedDistance( &cshape,btTransform::getIdentity(),
00735 rshape,m_colObj->getWorldTransform(),
00736 btVector3(1,0,0),res))
00737 {
00738 btSoftBody::CJoint joint;
00739 if(SolveContact(res,cluster,m_colObj,joint))
00740 {
00741 btSoftBody::CJoint* pj=new(btAlignedAlloc(sizeof(btSoftBody::CJoint),16)) btSoftBody::CJoint();
00742 *pj=joint;psb->m_joints.push_back(pj);
00743 if(m_colObj->isStaticOrKinematicObject())
00744 {
00745 pj->m_erp *= psb->m_cfg.kSKHR_CL;
00746 pj->m_split *= psb->m_cfg.kSK_SPLT_CL;
00747 }
00748 else
00749 {
00750 pj->m_erp *= psb->m_cfg.kSRHR_CL;
00751 pj->m_split *= psb->m_cfg.kSR_SPLT_CL;
00752 }
00753 }
00754 }
00755 }
00756 void Process(btSoftBody* ps,btCollisionObject* colOb)
00757 {
00758 psb = ps;
00759 m_colObj = colOb;
00760 idt = ps->m_sst.isdt;
00761 m_margin = m_colObj->getCollisionShape()->getMargin()+psb->getCollisionShape()->getMargin();
00763 friction = btMin(psb->m_cfg.kDF,m_colObj->getFriction());
00764 btVector3 mins;
00765 btVector3 maxs;
00766
00767 ATTRIBUTE_ALIGNED16(btDbvtVolume) volume;
00768 colOb->getCollisionShape()->getAabb(colOb->getWorldTransform(),mins,maxs);
00769 volume=btDbvtVolume::FromMM(mins,maxs);
00770 volume.Expand(btVector3(1,1,1)*m_margin);
00771 ps->m_cdbvt.collideTV(ps->m_cdbvt.m_root,volume,*this);
00772 }
00773 };
00774
00775
00776
00777 struct CollideCL_SS : ClusterBase
00778 {
00779 btSoftBody* bodies[2];
00780 void Process(const btDbvtNode* la,const btDbvtNode* lb)
00781 {
00782 btSoftBody::Cluster* cla=(btSoftBody::Cluster*)la->data;
00783 btSoftBody::Cluster* clb=(btSoftBody::Cluster*)lb->data;
00784
00785
00786 bool connected=false;
00787 if ((bodies[0]==bodies[1])&&(bodies[0]->m_clusterConnectivity.size()))
00788 {
00789 connected = bodies[0]->m_clusterConnectivity[cla->m_clusterIndex+bodies[0]->m_clusters.size()*clb->m_clusterIndex];
00790 }
00791
00792 if (!connected)
00793 {
00794 btSoftClusterCollisionShape csa(cla);
00795 btSoftClusterCollisionShape csb(clb);
00796 btGjkEpaSolver2::sResults res;
00797 if(btGjkEpaSolver2::SignedDistance( &csa,btTransform::getIdentity(),
00798 &csb,btTransform::getIdentity(),
00799 cla->m_com-clb->m_com,res))
00800 {
00801 btSoftBody::CJoint joint;
00802 if(SolveContact(res,cla,clb,joint))
00803 {
00804 btSoftBody::CJoint* pj=new(btAlignedAlloc(sizeof(btSoftBody::CJoint),16)) btSoftBody::CJoint();
00805 *pj=joint;bodies[0]->m_joints.push_back(pj);
00806 pj->m_erp *= btMax(bodies[0]->m_cfg.kSSHR_CL,bodies[1]->m_cfg.kSSHR_CL);
00807 pj->m_split *= (bodies[0]->m_cfg.kSS_SPLT_CL+bodies[1]->m_cfg.kSS_SPLT_CL)/2;
00808 }
00809 }
00810 } else
00811 {
00812 static int count=0;
00813 count++;
00814
00815
00816 }
00817 }
00818 void Process(btSoftBody* psa,btSoftBody* psb)
00819 {
00820 idt = psa->m_sst.isdt;
00821
00822 m_margin = (psa->getCollisionShape()->getMargin()+psb->getCollisionShape()->getMargin());
00823 friction = btMin(psa->m_cfg.kDF,psb->m_cfg.kDF);
00824 bodies[0] = psa;
00825 bodies[1] = psb;
00826 psa->m_cdbvt.collideTT(psa->m_cdbvt.m_root,psb->m_cdbvt.m_root,*this);
00827 }
00828 };
00829
00830
00831
00832 struct CollideSDF_RS : btDbvt::ICollide
00833 {
00834 void Process(const btDbvtNode* leaf)
00835 {
00836 btSoftBody::Node* node=(btSoftBody::Node*)leaf->data;
00837 DoNode(*node);
00838 }
00839 void DoNode(btSoftBody::Node& n) const
00840 {
00841 const btScalar m=n.m_im>0?dynmargin:stamargin;
00842 btSoftBody::RContact c;
00843 if( (!n.m_battach)&&
00844 psb->checkContact(m_colObj1,n.m_x,m,c.m_cti))
00845 {
00846 const btScalar ima=n.m_im;
00847 const btScalar imb= m_rigidBody? m_rigidBody->getInvMass() : 0.f;
00848 const btScalar ms=ima+imb;
00849 if(ms>0)
00850 {
00851 const btTransform& wtr=m_rigidBody?m_rigidBody->getWorldTransform() : m_colObj1->getWorldTransform();
00852 static const btMatrix3x3 iwiStatic(0,0,0,0,0,0,0,0,0);
00853 const btMatrix3x3& iwi=m_rigidBody?m_rigidBody->getInvInertiaTensorWorld() : iwiStatic;
00854 const btVector3 ra=n.m_x-wtr.getOrigin();
00855 const btVector3 va=m_rigidBody ? m_rigidBody->getVelocityInLocalPoint(ra)*psb->m_sst.sdt : btVector3(0,0,0);
00856 const btVector3 vb=n.m_x-n.m_q;
00857 const btVector3 vr=vb-va;
00858 const btScalar dn=btDot(vr,c.m_cti.m_normal);
00859 const btVector3 fv=vr-c.m_cti.m_normal*dn;
00860 const btScalar fc=psb->m_cfg.kDF*m_colObj1->getFriction();
00861 c.m_node = &n;
00862 c.m_c0 = ImpulseMatrix(psb->m_sst.sdt,ima,imb,iwi,ra);
00863 c.m_c1 = ra;
00864 c.m_c2 = ima*psb->m_sst.sdt;
00865 c.m_c3 = fv.length2()<(btFabs(dn)*fc)?0:1-fc;
00866 c.m_c4 = m_colObj1->isStaticOrKinematicObject()?psb->m_cfg.kKHR:psb->m_cfg.kCHR;
00867 psb->m_rcontacts.push_back(c);
00868 if (m_rigidBody)
00869 m_rigidBody->activate();
00870 }
00871 }
00872 }
00873 btSoftBody* psb;
00874 btCollisionObject* m_colObj1;
00875 btRigidBody* m_rigidBody;
00876 btScalar dynmargin;
00877 btScalar stamargin;
00878 };
00879
00880
00881
00882 struct CollideVF_SS : btDbvt::ICollide
00883 {
00884 void Process(const btDbvtNode* lnode,
00885 const btDbvtNode* lface)
00886 {
00887 btSoftBody::Node* node=(btSoftBody::Node*)lnode->data;
00888 btSoftBody::Face* face=(btSoftBody::Face*)lface->data;
00889 btVector3 o=node->m_x;
00890 btVector3 p;
00891 btScalar d=SIMD_INFINITY;
00892 ProjectOrigin( face->m_n[0]->m_x-o,
00893 face->m_n[1]->m_x-o,
00894 face->m_n[2]->m_x-o,
00895 p,d);
00896 const btScalar m=mrg+(o-node->m_q).length()*2;
00897 if(d<(m*m))
00898 {
00899 const btSoftBody::Node* n[]={face->m_n[0],face->m_n[1],face->m_n[2]};
00900 const btVector3 w=BaryCoord(n[0]->m_x,n[1]->m_x,n[2]->m_x,p+o);
00901 const btScalar ma=node->m_im;
00902 btScalar mb=BaryEval(n[0]->m_im,n[1]->m_im,n[2]->m_im,w);
00903 if( (n[0]->m_im<=0)||
00904 (n[1]->m_im<=0)||
00905 (n[2]->m_im<=0))
00906 {
00907 mb=0;
00908 }
00909 const btScalar ms=ma+mb;
00910 if(ms>0)
00911 {
00912 btSoftBody::SContact c;
00913 c.m_normal = p/-btSqrt(d);
00914 c.m_margin = m;
00915 c.m_node = node;
00916 c.m_face = face;
00917 c.m_weights = w;
00918 c.m_friction = btMax(psb[0]->m_cfg.kDF,psb[1]->m_cfg.kDF);
00919 c.m_cfm[0] = ma/ms*psb[0]->m_cfg.kSHR;
00920 c.m_cfm[1] = mb/ms*psb[1]->m_cfg.kSHR;
00921 psb[0]->m_scontacts.push_back(c);
00922 }
00923 }
00924 }
00925 btSoftBody* psb[2];
00926 btScalar mrg;
00927 };
00928 };
00929
00930 #endif //_BT_SOFT_BODY_INTERNALS_H
