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