btSoftBodyInternals.h

Go to the documentation of this file.
00001 /*
00002 Bullet Continuous Collision Detection and Physics Library
00003 Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
00004 
00005 This software is provided 'as-is', without any express or implied warranty.
00006 In no event will the authors be held liable for any damages arising from the use of this software.
00007 Permission is granted to anyone to use this software for any purpose,
00008 including commercial applications, and to alter it and redistribute it freely,
00009 subject to the following restrictions:
00010 
00011 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.
00012 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
00013 3. This notice may not be removed or altered from any source distribution.
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> //for memset
00030 //
00031 // btSymMatrix
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 // btSoftBodyCollisionShape
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* /*callback*/,const btVector3& /*aabbMin*/,const btVector3& /*aabbMax*/) const
00066         {
00067                 //not yet
00068                 btAssert(0);
00069         }
00070 
00072         virtual void getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const
00073         {
00074                 /* t is usually identity, except when colliding against btCompoundShape. See Issue 512 */
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& /*scaling*/)
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 /*mass*/,btVector3& /*inertia*/) const
00104         {
00106                 btAssert(0);
00107         }
00108         virtual const char*     getName()const
00109         {
00110                 return "SoftBody";
00111         }
00112 
00113 };
00114 
00115 //
00116 // btSoftClusterCollisionShape
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         //notice that the vectors should be unit length
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         //debugging
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 // Inline's
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         {/* Clamp       */ 
00533                 n.m_f-=ProjectOnAxis(n.m_v,f.normalized())/dtim;                                                
00534         }
00535         else
00536         {/* Apply       */ 
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 // btEigen : Extract eigen system,
00554 // straitforward implementation of http://math.fullerton.edu/mathews/n2003/JacobiMethodMod.html
00555 // outputs are NOT sorted.
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)/* [WARNING] let hope that one does not get thrown aways by some compilers... */ 
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 // Polar decomposition,
00614 // "Computing the Polar Decomposition with Applications", Nicholas J. Higham, 1986.
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 // btSoftColliders
00624 //
00625 struct btSoftColliders
00626 {
00627         //
00628         // ClusterBase
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();//is it necessary?
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 //                              printf("depth=%f\n",depth);
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 //                              printf("normal=%f,%f,%f\n",res.normal.getX(),res.normal.getY(),res.normal.getZ());
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         // CollideCL_RS
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))//prb,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         // CollideCL_SS
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                                 //printf("count=%d\n",count);
00792                                 
00793                         }
00794                 }
00795                 void            ProcessSoftSoft(btSoftBody* psa,btSoftBody* psb)
00796                 {
00797                         idt                     =       psa->m_sst.isdt;
00798                         //m_margin              =       (psa->getCollisionShape()->getMargin()+psb->getCollisionShape()->getMargin())/2;
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         // CollideSDF_RS
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         // CollideVF_SS
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