src/BulletSoftBody/btSoftBodyInternals.h

   1 /*
   2 Bullet Continuous Collision Detection and Physics Library
   3 Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
   4
   5 This software is provided 'as-is', without any express or implied warranty.
   6 In no event will the authors be held liable for any damages arising from the use of this software.
   7 Permission is granted to anyone to use this software for any purpose,
   8 including commercial applications, and to alter it and redistribute it freely,
   9 subject to the following restrictions:
  10
  11 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.
  12 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
  13 3. This notice may not be removed or altered from any source distribution.
  14 */
  15 ///btSoftBody implementation by Nathanael Presson
  16
  17 #ifndef _BT_SOFT_BODY_INTERNALS_H
  18 #define _BT_SOFT_BODY_INTERNALS_H
  19
  20 #include "btSoftBody.h"
  21
  22
  23 #include "LinearMath/btQuickprof.h"
  24 #include "LinearMath/btPolarDecomposition.h"
  25 #include "BulletCollision/BroadphaseCollision/btBroadphaseInterface.h"
  26 #include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h"
  27 #include "BulletCollision/CollisionShapes/btConvexInternalShape.h"
  28 #include "BulletCollision/NarrowPhaseCollision/btGjkEpa2.h"
  29 #include <string.h> //for memset
  30 //
  31 // btSymMatrix
  32 //
  33 template <typename T>
  34 struct btSymMatrix
  35 {
  36         btSymMatrix() : dim(0)                                  {}
  37         btSymMatrix(int n,const T& init=T())    { resize(n,init); }
  38         void                                    resize(int n,const T& init=T())                 { dim=n;store.resize((n*(n+1))/2,init); }
  39         int                                             index(int c,int r) const                                { if(c>r) btSwap(c,r);btAssert(r<dim);return((r*(r+1))/2+c); }
  40         T&                                              operator()(int c,int r)                                 { return(store[index(c,r)]); }
  41         const T&                                operator()(int c,int r) const                   { return(store[index(c,r)]); }
  42         btAlignedObjectArray<T> store;
  43         int                                             dim;
  44 };
  45
  46 //
  47 // btSoftBodyCollisionShape
  48 //
  49 class btSoftBodyCollisionShape : public btConcaveShape
  50 {
  51 public:
  52         btSoftBody*                                             m_body;
  53
  54         btSoftBodyCollisionShape(btSoftBody* backptr)
  55         {
  56                 m_shapeType = SOFTBODY_SHAPE_PROXYTYPE;
  57                 m_body=backptr;
  58         }
  59
  60         virtual ~btSoftBodyCollisionShape()
  61         {
  62
  63         }
  64
  65         void    processAllTriangles(btTriangleCallback* /*callback*/,const btVector3& /*aabbMin*/,const btVector3& /*aabbMax*/) const
  66         {
  67                 //not yet
  68                 btAssert(0);
  69         }
  70
  71         ///getAabb returns the axis aligned bounding box in the coordinate frame of the given transform t.
  72         virtual void getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const
  73         {
  74                 /* t is usually identity, except when colliding against btCompoundShape. See Issue 512 */
  75                 const btVector3 mins=m_body->m_bounds[0];
  76                 const btVector3 maxs=m_body->m_bounds[1];
  77                 const btVector3 crns[]={t*btVector3(mins.x(),mins.y(),mins.z()),
  78                         t*btVector3(maxs.x(),mins.y(),mins.z()),
  79                         t*btVector3(maxs.x(),maxs.y(),mins.z()),
  80                         t*btVector3(mins.x(),maxs.y(),mins.z()),
  81                         t*btVector3(mins.x(),mins.y(),maxs.z()),
  82                         t*btVector3(maxs.x(),mins.y(),maxs.z()),
  83                         t*btVector3(maxs.x(),maxs.y(),maxs.z()),
  84                         t*btVector3(mins.x(),maxs.y(),maxs.z())};
  85                 aabbMin=aabbMax=crns[0];
  86                 for(int i=1;i<8;++i)
  87                 {
  88                         aabbMin.setMin(crns[i]);
  89                         aabbMax.setMax(crns[i]);
  90                 }
  91         }
  92
  93
  94         virtual void    setLocalScaling(const btVector3& /*scaling*/)
  95         {
  96                 ///na
  97         }
  98         virtual const btVector3& getLocalScaling() const
  99         {
 100                 static const btVector3 dummy(1,1,1);
 101                 return dummy;
 102         }
 103         virtual void    calculateLocalInertia(btScalar /*mass*/,btVector3& /*inertia*/) const
 104         {
 105                 ///not yet
 106                 btAssert(0);
 107         }
 108         virtual const char*     getName()const
 109         {
 110                 return "SoftBody";
 111         }
 112
 113 };
 114
 115 //
 116 // btSoftClusterCollisionShape
 117 //
 118 class btSoftClusterCollisionShape : public btConvexInternalShape
 119 {
 120 public:
 121         const btSoftBody::Cluster*      m_cluster;
 122
 123         btSoftClusterCollisionShape (const btSoftBody::Cluster* cluster) : m_cluster(cluster) { setMargin(0); }
 124
 125
 126         virtual btVector3       localGetSupportingVertex(const btVector3& vec) const
 127         {
 128                 btSoftBody::Node* const *                                               n=&m_cluster->m_nodes[0];
 129                 btScalar                                                                                d=btDot(vec,n[0]->m_x);
 130                 int                                                                                             j=0;
 131                 for(int i=1,ni=m_cluster->m_nodes.size();i<ni;++i)
 132                 {
 133                         const btScalar  k=btDot(vec,n[i]->m_x);
 134                         if(k>d) { d=k;j=i; }
 135                 }
 136                 return(n[j]->m_x);
 137         }
 138         virtual btVector3       localGetSupportingVertexWithoutMargin(const btVector3& vec)const
 139         {
 140                 return(localGetSupportingVertex(vec));
 141         }
 142         //notice that the vectors should be unit length
 143         virtual void    batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const
 144         {}
 145
 146
 147         virtual void    calculateLocalInertia(btScalar mass,btVector3& inertia) const
 148         {}
 149
 150         virtual void getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const
 151         {}
 152
 153         virtual int     getShapeType() const { return SOFTBODY_SHAPE_PROXYTYPE; }
 154
 155         //debugging
 156         virtual const char*     getName()const {return "SOFTCLUSTER";}
 157
 158         virtual void    setMargin(btScalar margin)
 159         {
 160                 btConvexInternalShape::setMargin(margin);
 161         }
 162         virtual btScalar        getMargin() const
 163         {
 164                 return getMargin();
 165         }
 166 };
 167
 168 //
 169 // Inline's
 170 //
 171
 172 //
 173 template <typename T>
 174 static inline void                      ZeroInitialize(T& value)
 175 {
 176         memset(&value,0,sizeof(T));
 177 }
 178 //
 179 template <typename T>
 180 static inline bool                      CompLess(const T& a,const T& b)
 181 { return(a<b); }
 182 //
 183 template <typename T>
 184 static inline bool                      CompGreater(const T& a,const T& b)
 185 { return(a>b); }
 186 //
 187 template <typename T>
 188 static inline T                         Lerp(const T& a,const T& b,btScalar t)
 189 { return(a+(b-a)*t); }
 190 //
 191 template <typename T>
 192 static inline T                         InvLerp(const T& a,const T& b,btScalar t)
 193 { return((b+a*t-b*t)/(a*b)); }
 194 //
 195 static inline btMatrix3x3       Lerp(   const btMatrix3x3& a,
 196                                                                  const btMatrix3x3& b,
 197                                                                  btScalar t)
 198 {
 199         btMatrix3x3     r;
 200         r[0]=Lerp(a[0],b[0],t);
 201         r[1]=Lerp(a[1],b[1],t);
 202         r[2]=Lerp(a[2],b[2],t);
 203         return(r);
 204 }
 205 //
 206 static inline btVector3         Clamp(const btVector3& v,btScalar maxlength)
 207 {
 208         const btScalar sql=v.length2();
 209         if(sql>(maxlength*maxlength))
 210                 return((v*maxlength)/btSqrt(sql));
 211         else
 212                 return(v);
 213 }
 214 //
 215 template <typename T>
 216 static inline T                         Clamp(const T& x,const T& l,const T& h)
 217 { return(x<l?l:x>h?h:x); }
 218 //
 219 template <typename T>
 220 static inline T                         Sq(const T& x)
 221 { return(x*x); }
 222 //
 223 template <typename T>
 224 static inline T                         Cube(const T& x)
 225 { return(x*x*x); }
 226 //
 227 template <typename T>
 228 static inline T                         Sign(const T& x)
 229 { return((T)(x<0?-1:+1)); }
 230 //
 231 template <typename T>
 232 static inline bool                      SameSign(const T& x,const T& y)
 233 { return((x*y)>0); }
 234 //
 235 static inline btScalar          ClusterMetric(const btVector3& x,const btVector3& y)
 236 {
 237         const btVector3 d=x-y;
 238         return(btFabs(d[0])+btFabs(d[1])+btFabs(d[2]));
 239 }
 240 //
 241 static inline btMatrix3x3       ScaleAlongAxis(const btVector3& a,btScalar s)
 242 {
 243         const btScalar  xx=a.x()*a.x();
 244         const btScalar  yy=a.y()*a.y();
 245         const btScalar  zz=a.z()*a.z();
 246         const btScalar  xy=a.x()*a.y();
 247         const btScalar  yz=a.y()*a.z();
 248         const btScalar  zx=a.z()*a.x();
 249         btMatrix3x3             m;
 250         m[0]=btVector3(1-xx+xx*s,xy*s-xy,zx*s-zx);
 251         m[1]=btVector3(xy*s-xy,1-yy+yy*s,yz*s-yz);
 252         m[2]=btVector3(zx*s-zx,yz*s-yz,1-zz+zz*s);
 253         return(m);
 254 }
 255 //
 256 static inline btMatrix3x3       Cross(const btVector3& v)
 257 {
 258         btMatrix3x3     m;
 259         m[0]=btVector3(0,-v.z(),+v.y());
 260         m[1]=btVector3(+v.z(),0,-v.x());
 261         m[2]=btVector3(-v.y(),+v.x(),0);
 262         return(m);
 263 }
 264 //
 265 static inline btMatrix3x3       Diagonal(btScalar x)
 266 {
 267         btMatrix3x3     m;
 268         m[0]=btVector3(x,0,0);
 269         m[1]=btVector3(0,x,0);
 270         m[2]=btVector3(0,0,x);
 271         return(m);
 272 }
 273 //
 274 static inline btMatrix3x3       Add(const btMatrix3x3& a,
 275                                                                 const btMatrix3x3& b)
 276 {
 277         btMatrix3x3     r;
 278         for(int i=0;i<3;++i) r[i]=a[i]+b[i];
 279         return(r);
 280 }
 281 //
 282 static inline btMatrix3x3       Sub(const btMatrix3x3& a,
 283                                                                 const btMatrix3x3& b)
 284 {
 285         btMatrix3x3     r;
 286         for(int i=0;i<3;++i) r[i]=a[i]-b[i];
 287         return(r);
 288 }
 289 //
 290 static inline btMatrix3x3       Mul(const btMatrix3x3& a,
 291                                                                 btScalar b)
 292 {
 293         btMatrix3x3     r;
 294         for(int i=0;i<3;++i) r[i]=a[i]*b;
 295         return(r);
 296 }
 297 //
 298 static inline void                      Orthogonalize(btMatrix3x3& m)
 299 {
 300         m[2]=btCross(m[0],m[1]).normalized();
 301         m[1]=btCross(m[2],m[0]).normalized();
 302         m[0]=btCross(m[1],m[2]).normalized();
 303 }
 304 //
 305 static inline btMatrix3x3       MassMatrix(btScalar im,const btMatrix3x3& iwi,const btVector3& r)
 306 {
 307         const btMatrix3x3       cr=Cross(r);
 308         return(Sub(Diagonal(im),cr*iwi*cr));
 309 }
 310
 311 //
 312 static inline btMatrix3x3       ImpulseMatrix(  btScalar dt,
 313                                                                                   btScalar ima,
 314                                                                                   btScalar imb,
 315                                                                                   const btMatrix3x3& iwi,
 316                                                                                   const btVector3& r)
 317 {
 318         return(Diagonal(1/dt)*Add(Diagonal(ima),MassMatrix(imb,iwi,r)).inverse());
 319 }
 320
 321 //
 322 static inline btMatrix3x3       ImpulseMatrix(  btScalar ima,const btMatrix3x3& iia,const btVector3& ra,
 323                                                                                   btScalar imb,const btMatrix3x3& iib,const btVector3& rb)
 324 {
 325         return(Add(MassMatrix(ima,iia,ra),MassMatrix(imb,iib,rb)).inverse());
 326 }
 327
 328 //
 329 static inline btMatrix3x3       AngularImpulseMatrix(   const btMatrix3x3& iia,
 330                                                                                                  const btMatrix3x3& iib)
 331 {
 332         return(Add(iia,iib).inverse());
 333 }
 334
 335 //
 336 static inline btVector3         ProjectOnAxis(  const btVector3& v,
 337                                                                                   const btVector3& a)
 338 {
 339         return(a*btDot(v,a));
 340 }
 341 //
 342 static inline btVector3         ProjectOnPlane( const btVector3& v,
 343                                                                                    const btVector3& a)
 344 {
 345         return(v-ProjectOnAxis(v,a));
 346 }
 347
 348 //
 349 static inline void                      ProjectOrigin(  const btVector3& a,
 350                                                                                   const btVector3& b,
 351                                                                                   btVector3& prj,
 352                                                                                   btScalar& sqd)
 353 {
 354         const btVector3 d=b-a;
 355         const btScalar  m2=d.length2();
 356         if(m2>SIMD_EPSILON)
 357         {
 358                 const btScalar  t=Clamp<btScalar>(-btDot(a,d)/m2,0,1);
 359                 const btVector3 p=a+d*t;
 360                 const btScalar  l2=p.length2();
 361                 if(l2<sqd)
 362                 {
 363                         prj=p;
 364                         sqd=l2;
 365                 }
 366         }
 367 }
 368 //
 369 static inline void                      ProjectOrigin(  const btVector3& a,
 370                                                                                   const btVector3& b,
 371                                                                                   const btVector3& c,
 372                                                                                   btVector3& prj,
 373                                                                                   btScalar& sqd)
 374 {
 375         const btVector3&        q=btCross(b-a,c-a);
 376         const btScalar          m2=q.length2();
 377         if(m2>SIMD_EPSILON)
 378         {
 379                 const btVector3 n=q/btSqrt(m2);
 380                 const btScalar  k=btDot(a,n);
 381                 const btScalar  k2=k*k;
 382                 if(k2<sqd)
 383                 {
 384                         const btVector3 p=n*k;
 385                         if(     (btDot(btCross(a-p,b-p),q)>0)&&
 386                                 (btDot(btCross(b-p,c-p),q)>0)&&
 387                                 (btDot(btCross(c-p,a-p),q)>0))
 388                         {
 389                                 prj=p;
 390                                 sqd=k2;
 391                         }
 392                         else
 393                         {
 394                                 ProjectOrigin(a,b,prj,sqd);
 395                                 ProjectOrigin(b,c,prj,sqd);
 396                                 ProjectOrigin(c,a,prj,sqd);
 397                         }
 398                 }
 399         }
 400 }
 401
 402 //
 403 template <typename T>
 404 static inline T                         BaryEval(               const T& a,
 405                                                                          const T& b,
 406                                                                          const T& c,
 407                                                                          const btVector3& coord)
 408 {
 409         return(a*coord.x()+b*coord.y()+c*coord.z());
 410 }
 411 //
 412 static inline btVector3         BaryCoord(      const btVector3& a,
 413                                                                           const btVector3& b,
 414                                                                           const btVector3& c,
 415                                                                           const btVector3& p)
 416 {
 417         const btScalar  w[]={   btCross(a-p,b-p).length(),
 418                 btCross(b-p,c-p).length(),
 419                 btCross(c-p,a-p).length()};
 420         const btScalar  isum=1/(w[0]+w[1]+w[2]);
 421         return(btVector3(w[1]*isum,w[2]*isum,w[0]*isum));
 422 }
 423
 424 //
 425 static btScalar                         ImplicitSolve(  btSoftBody::ImplicitFn* fn,
 426                                                                                   const btVector3& a,
 427                                                                                   const btVector3& b,
 428                                                                                   const btScalar accuracy,
 429                                                                                   const int maxiterations=256)
 430 {
 431         btScalar        span[2]={0,1};
 432         btScalar        values[2]={fn->Eval(a),fn->Eval(b)};
 433         if(values[0]>values[1])
 434         {
 435                 btSwap(span[0],span[1]);
 436                 btSwap(values[0],values[1]);
 437         }
 438         if(values[0]>-accuracy) return(-1);
 439         if(values[1]<+accuracy) return(-1);
 440         for(int i=0;i<maxiterations;++i)
 441         {
 442                 const btScalar  t=Lerp(span[0],span[1],values[0]/(values[0]-values[1]));
 443                 const btScalar  v=fn->Eval(Lerp(a,b,t));
 444                 if((t<=0)||(t>=1))              break;
 445                 if(btFabs(v)<accuracy)  return(t);
 446                 if(v<0)
 447                 { span[0]=t;values[0]=v; }
 448                 else
 449                 { span[1]=t;values[1]=v; }
 450         }
 451         return(-1);
 452 }
 453
 454 //
 455 static inline btVector3         NormalizeAny(const btVector3& v)
 456 {
 457         const btScalar l=v.length();
 458         if(l>SIMD_EPSILON)
 459                 return(v/l);
 460         else
 461                 return(btVector3(0,0,0));
 462 }
 463
 464 //
 465 static inline btDbvtVolume      VolumeOf(       const btSoftBody::Face& f,
 466                                                                          btScalar margin)
 467 {
 468         const btVector3*        pts[]={ &f.m_n[0]->m_x,
 469                 &f.m_n[1]->m_x,
 470                 &f.m_n[2]->m_x};
 471         btDbvtVolume            vol=btDbvtVolume::FromPoints(pts,3);
 472         vol.Expand(btVector3(margin,margin,margin));
 473         return(vol);
 474 }
 475
 476 //
 477 static inline btVector3                 CenterOf(       const btSoftBody::Face& f)
 478 {
 479         return((f.m_n[0]->m_x+f.m_n[1]->m_x+f.m_n[2]->m_x)/3);
 480 }
 481
 482 //
 483 static inline btScalar                  AreaOf(         const btVector3& x0,
 484                                                                            const btVector3& x1,
 485                                                                            const btVector3& x2)
 486 {
 487         const btVector3 a=x1-x0;
 488         const btVector3 b=x2-x0;
 489         const btVector3 cr=btCross(a,b);
 490         const btScalar  area=cr.length();
 491         return(area);
 492 }
 493
 494 //
 495 static inline btScalar          VolumeOf(       const btVector3& x0,
 496                                                                          const btVector3& x1,
 497                                                                          const btVector3& x2,
 498                                                                          const btVector3& x3)
 499 {
 500         const btVector3 a=x1-x0;
 501         const btVector3 b=x2-x0;
 502         const btVector3 c=x3-x0;
 503         return(btDot(a,btCross(b,c)));
 504 }
 505
 506 //
 507 static void                                     EvaluateMedium( const btSoftBodyWorldInfo* wfi,
 508                                                                                    const btVector3& x,
 509                                                                                    btSoftBody::sMedium& medium)
 510 {
 511         medium.m_velocity       =       btVector3(0,0,0);
 512         medium.m_pressure       =       0;
 513         medium.m_density        =       wfi->air_density;
 514         if(wfi->water_density>0)
 515         {
 516                 const btScalar  depth=-(btDot(x,wfi->water_normal)+wfi->water_offset);
 517                 if(depth>0)
 518                 {
 519                         medium.m_density        =       wfi->water_density;
 520                         medium.m_pressure       =       depth*wfi->water_density*wfi->m_gravity.length();
 521                 }
 522         }
 523 }
 524
 525 //
 526 static inline void                      ApplyClampedForce(      btSoftBody::Node& n,
 527                                                                                           const btVector3& f,
 528                                                                                           btScalar dt)
 529 {
 530         const btScalar  dtim=dt*n.m_im;
 531         if((f*dtim).length2()>n.m_v.length2())
 532         {/* Clamp       */
 533                 n.m_f-=ProjectOnAxis(n.m_v,f.normalized())/dtim;
 534         }
 535         else
 536         {/* Apply       */
 537                 n.m_f+=f;
 538         }
 539 }
 540
 541 //
 542 static inline int               MatchEdge(      const btSoftBody::Node* a,
 543                                                                   const btSoftBody::Node* b,
 544                                                                   const btSoftBody::Node* ma,
 545                                                                   const btSoftBody::Node* mb)
 546 {
 547         if((a==ma)&&(b==mb)) return(0);
 548         if((a==mb)&&(b==ma)) return(1);
 549         return(-1);
 550 }
 551
 552 //
 553 // btEigen : Extract eigen system,
 554 // straitforward implementation of http://math.fullerton.edu/mathews/n2003/JacobiMethodMod.html
 555 // outputs are NOT sorted.
 556 //
 557 struct  btEigen
 558 {
 559         static int                      system(btMatrix3x3& a,btMatrix3x3* vectors,btVector3* values=0)
 560         {
 561                 static const int                maxiterations=16;
 562                 static const btScalar   accuracy=(btScalar)0.0001;
 563                 btMatrix3x3&                    v=*vectors;
 564                 int                                             iterations=0;
 565                 vectors->setIdentity();
 566                 do      {
 567                         int                             p=0,q=1;
 568                         if(btFabs(a[p][q])<btFabs(a[0][2])) { p=0;q=2; }
 569                         if(btFabs(a[p][q])<btFabs(a[1][2])) { p=1;q=2; }
 570                         if(btFabs(a[p][q])>accuracy)
 571                         {
 572                                 const btScalar  w=(a[q][q]-a[p][p])/(2*a[p][q]);
 573                                 const btScalar  z=btFabs(w);
 574                                 const btScalar  t=w/(z*(btSqrt(1+w*w)+z));
 575                                 if(t==t)/* [WARNING] let hope that one does not get thrown aways by some compilers... */
 576                                 {
 577                                         const btScalar  c=1/btSqrt(t*t+1);
 578                                         const btScalar  s=c*t;
 579                                         mulPQ(a,c,s,p,q);
 580                                         mulTPQ(a,c,s,p,q);
 581                                         mulPQ(v,c,s,p,q);
 582                                 } else break;
 583                         } else break;
 584                 } while((++iterations)<maxiterations);
 585                 if(values)
 586                 {
 587                         *values=btVector3(a[0][0],a[1][1],a[2][2]);
 588                 }
 589                 return(iterations);
 590         }
 591 private:
 592         static inline void      mulTPQ(btMatrix3x3& a,btScalar c,btScalar s,int p,int q)
 593         {
 594                 const btScalar  m[2][3]={       {a[p][0],a[p][1],a[p][2]},
 595                 {a[q][0],a[q][1],a[q][2]}};
 596                 int i;
 597
 598                 for(i=0;i<3;++i) a[p][i]=c*m[0][i]-s*m[1][i];
 599                 for(i=0;i<3;++i) a[q][i]=c*m[1][i]+s*m[0][i];
 600         }
 601         static inline void      mulPQ(btMatrix3x3& a,btScalar c,btScalar s,int p,int q)
 602         {
 603                 const btScalar  m[2][3]={       {a[0][p],a[1][p],a[2][p]},
 604                 {a[0][q],a[1][q],a[2][q]}};
 605                 int i;
 606
 607                 for(i=0;i<3;++i) a[i][p]=c*m[0][i]-s*m[1][i];
 608                 for(i=0;i<3;++i) a[i][q]=c*m[1][i]+s*m[0][i];
 609         }
 610 };
 611
 612 //
 613 // Polar decomposition,
 614 // "Computing the Polar Decomposition with Applications", Nicholas J. Higham, 1986.
 615 //
 616 static inline int                       PolarDecompose( const btMatrix3x3& m,btMatrix3x3& q,btMatrix3x3& s)
 617 {
 618         static const btPolarDecomposition polar;
 619         return polar.decompose(m, q, s);
 620 }
 621
 622 //
 623 // btSoftColliders
 624 //
 625 struct btSoftColliders
 626 {
 627         //
 628         // ClusterBase
 629         //
 630         struct  ClusterBase : btDbvt::ICollide
 631         {
 632                 btScalar                        erp;
 633                 btScalar                        idt;
 634                 btScalar                        m_margin;
 635                 btScalar                        friction;
 636                 btScalar                        threshold;
 637                 ClusterBase()
 638                 {
 639                         erp                     =(btScalar)1;
 640                         idt                     =0;
 641                         m_margin                =0;
 642                         friction        =0;
 643                         threshold       =(btScalar)0;
 644                 }
 645                 bool                            SolveContact(   const btGjkEpaSolver2::sResults& res,
 646                         btSoftBody::Body ba,const btSoftBody::Body bb,
 647                         btSoftBody::CJoint& joint)
 648                 {
 649                         if(res.distance<m_margin)
 650                         {
 651                                 btVector3 norm = res.normal;
 652                                 norm.normalize();//is it necessary?
 653
 654                                 const btVector3         ra=res.witnesses[0]-ba.xform().getOrigin();
 655                                 const btVector3         rb=res.witnesses[1]-bb.xform().getOrigin();
 656                                 const btVector3         va=ba.velocity(ra);
 657                                 const btVector3         vb=bb.velocity(rb);
 658                                 const btVector3         vrel=va-vb;
 659                                 const btScalar          rvac=btDot(vrel,norm);
 660                                  btScalar               depth=res.distance-m_margin;
 661
 662 //                              printf("depth=%f\n",depth);
 663                                 const btVector3         iv=norm*rvac;
 664                                 const btVector3         fv=vrel-iv;
 665                                 joint.m_bodies[0]       =       ba;
 666                                 joint.m_bodies[1]       =       bb;
 667                                 joint.m_refs[0]         =       ra*ba.xform().getBasis();
 668                                 joint.m_refs[1]         =       rb*bb.xform().getBasis();
 669                                 joint.m_rpos[0]         =       ra;
 670                                 joint.m_rpos[1]         =       rb;
 671                                 joint.m_cfm                     =       1;
 672                                 joint.m_erp                     =       1;
 673                                 joint.m_life            =       0;
 674                                 joint.m_maxlife         =       0;
 675                                 joint.m_split           =       1;
 676
 677                                 joint.m_drift           =       depth*norm;
 678
 679                                 joint.m_normal          =       norm;
 680 //                              printf("normal=%f,%f,%f\n",res.normal.getX(),res.normal.getY(),res.normal.getZ());
 681                                 joint.m_delete          =       false;
 682                                 joint.m_friction        =       fv.length2()<(rvac*friction*rvac*friction)?1:friction;
 683                                 joint.m_massmatrix      =       ImpulseMatrix(  ba.invMass(),ba.invWorldInertia(),joint.m_rpos[0],
 684                                         bb.invMass(),bb.invWorldInertia(),joint.m_rpos[1]);
 685
 686                                 return(true);
 687                         }
 688                         return(false);
 689                 }
 690         };
 691         //
 692         // CollideCL_RS
 693         //
 694         struct  CollideCL_RS : ClusterBase
 695         {
 696                 btSoftBody*             psb;
 697                 const btCollisionObjectWrapper* m_colObjWrap;
 698
 699                 void            Process(const btDbvtNode* leaf)
 700                 {
 701                         btSoftBody::Cluster*            cluster=(btSoftBody::Cluster*)leaf->data;
 702                         btSoftClusterCollisionShape     cshape(cluster);
 703
 704                         const btConvexShape*            rshape=(const btConvexShape*)m_colObjWrap->getCollisionShape();
 705
 706                         ///don't collide an anchored cluster with a static/kinematic object
 707                         if(m_colObjWrap->getCollisionObject()->isStaticOrKinematicObject() && cluster->m_containsAnchor)
 708                                 return;
 709
 710                         btGjkEpaSolver2::sResults       res;
 711                         if(btGjkEpaSolver2::SignedDistance(     &cshape,btTransform::getIdentity(),
 712                                 rshape,m_colObjWrap->getWorldTransform(),
 713                                 btVector3(1,0,0),res))
 714                         {
 715                                 btSoftBody::CJoint      joint;
 716                                 if(SolveContact(res,cluster,m_colObjWrap->getCollisionObject(),joint))//prb,joint))
 717                                 {
 718                                         btSoftBody::CJoint*     pj=new(btAlignedAlloc(sizeof(btSoftBody::CJoint),16)) btSoftBody::CJoint();
 719                                         *pj=joint;psb->m_joints.push_back(pj);
 720                                         if(m_colObjWrap->getCollisionObject()->isStaticOrKinematicObject())
 721                                         {
 722                                                 pj->m_erp       *=      psb->m_cfg.kSKHR_CL;
 723                                                 pj->m_split     *=      psb->m_cfg.kSK_SPLT_CL;
 724                                         }
 725                                         else
 726                                         {
 727                                                 pj->m_erp       *=      psb->m_cfg.kSRHR_CL;
 728                                                 pj->m_split     *=      psb->m_cfg.kSR_SPLT_CL;
 729                                         }
 730                                 }
 731                         }
 732                 }
 733                 void            ProcessColObj(btSoftBody* ps,const btCollisionObjectWrapper* colObWrap)
 734                 {
 735                         psb                     =       ps;
 736                         m_colObjWrap                    =       colObWrap;
 737                         idt                     =       ps->m_sst.isdt;
 738                         m_margin                =       m_colObjWrap->getCollisionShape()->getMargin()+psb->getCollisionShape()->getMargin();
 739                         ///Bullet rigid body uses multiply instead of minimum to determine combined friction. Some customization would be useful.
 740                         friction        =       btMin(psb->m_cfg.kDF,m_colObjWrap->getCollisionObject()->getFriction());
 741                         btVector3                       mins;
 742                         btVector3                       maxs;
 743
 744                         ATTRIBUTE_ALIGNED16(btDbvtVolume)               volume;
 745                         colObWrap->getCollisionShape()->getAabb(colObWrap->getWorldTransform(),mins,maxs);
 746                         volume=btDbvtVolume::FromMM(mins,maxs);
 747                         volume.Expand(btVector3(1,1,1)*m_margin);
 748                         ps->m_cdbvt.collideTV(ps->m_cdbvt.m_root,volume,*this);
 749                 }
 750         };
 751         //
 752         // CollideCL_SS
 753         //
 754         struct  CollideCL_SS : ClusterBase
 755         {
 756                 btSoftBody*     bodies[2];
 757                 void            Process(const btDbvtNode* la,const btDbvtNode* lb)
 758                 {
 759                         btSoftBody::Cluster*            cla=(btSoftBody::Cluster*)la->data;
 760                         btSoftBody::Cluster*            clb=(btSoftBody::Cluster*)lb->data;
 761
 762
 763                         bool connected=false;
 764                         if ((bodies[0]==bodies[1])&&(bodies[0]->m_clusterConnectivity.size()))
 765                         {
 766                                 connected = bodies[0]->m_clusterConnectivity[cla->m_clusterIndex+bodies[0]->m_clusters.size()*clb->m_clusterIndex];
 767                         }
 768
 769                         if (!connected)
 770                         {
 771                                 btSoftClusterCollisionShape     csa(cla);
 772                                 btSoftClusterCollisionShape     csb(clb);
 773                                 btGjkEpaSolver2::sResults       res;
 774                                 if(btGjkEpaSolver2::SignedDistance(     &csa,btTransform::getIdentity(),
 775                                         &csb,btTransform::getIdentity(),
 776                                         cla->m_com-clb->m_com,res))
 777                                 {
 778                                         btSoftBody::CJoint      joint;
 779                                         if(SolveContact(res,cla,clb,joint))
 780                                         {
 781                                                 btSoftBody::CJoint*     pj=new(btAlignedAlloc(sizeof(btSoftBody::CJoint),16)) btSoftBody::CJoint();
 782                                                 *pj=joint;bodies[0]->m_joints.push_back(pj);
 783                                                 pj->m_erp       *=      btMax(bodies[0]->m_cfg.kSSHR_CL,bodies[1]->m_cfg.kSSHR_CL);
 784                                                 pj->m_split     *=      (bodies[0]->m_cfg.kSS_SPLT_CL+bodies[1]->m_cfg.kSS_SPLT_CL)/2;
 785                                         }
 786                                 }
 787                         } else
 788                         {
 789                                 static int count=0;
 790                                 count++;
 791                                 //printf("count=%d\n",count);
 792
 793                         }
 794                 }
 795                 void            ProcessSoftSoft(btSoftBody* psa,btSoftBody* psb)
 796                 {
 797                         idt                     =       psa->m_sst.isdt;
 798                         //m_margin              =       (psa->getCollisionShape()->getMargin()+psb->getCollisionShape()->getMargin())/2;
 799                         m_margin                =       (psa->getCollisionShape()->getMargin()+psb->getCollisionShape()->getMargin());
 800                         friction        =       btMin(psa->m_cfg.kDF,psb->m_cfg.kDF);
 801                         bodies[0]       =       psa;
 802                         bodies[1]       =       psb;
 803                         psa->m_cdbvt.collideTT(psa->m_cdbvt.m_root,psb->m_cdbvt.m_root,*this);
 804                 }
 805         };
 806         //
 807         // CollideSDF_RS
 808         //
 809         struct  CollideSDF_RS : btDbvt::ICollide
 810         {
 811                 void            Process(const btDbvtNode* leaf)
 812                 {
 813                         btSoftBody::Node*       node=(btSoftBody::Node*)leaf->data;
 814                         DoNode(*node);
 815                 }
 816                 void            DoNode(btSoftBody::Node& n) const
 817                 {
 818                         const btScalar                  m=n.m_im>0?dynmargin:stamargin;
 819                         btSoftBody::RContact    c;
 820
 821                         if(     (!n.m_battach)&&
 822                                 psb->checkContact(m_colObj1Wrap,n.m_x,m,c.m_cti))
 823                         {
 824                                 const btScalar  ima=n.m_im;
 825                                 const btScalar  imb= m_rigidBody? m_rigidBody->getInvMass() : 0.f;
 826                                 const btScalar  ms=ima+imb;
 827                                 if(ms>0)
 828                                 {
 829                                         const btTransform&      wtr=m_rigidBody?m_rigidBody->getWorldTransform() : m_colObj1Wrap->getCollisionObject()->getWorldTransform();
 830                                         static const btMatrix3x3        iwiStatic(0,0,0,0,0,0,0,0,0);
 831                                         const btMatrix3x3&      iwi=m_rigidBody?m_rigidBody->getInvInertiaTensorWorld() : iwiStatic;
 832                                         const btVector3         ra=n.m_x-wtr.getOrigin();
 833                                         const btVector3         va=m_rigidBody ? m_rigidBody->getVelocityInLocalPoint(ra)*psb->m_sst.sdt : btVector3(0,0,0);
 834                                         const btVector3         vb=n.m_x-n.m_q;
 835                                         const btVector3         vr=vb-va;
 836                                         const btScalar          dn=btDot(vr,c.m_cti.m_normal);
 837                                         const btVector3         fv=vr-c.m_cti.m_normal*dn;
 838                                         const btScalar          fc=psb->m_cfg.kDF*m_colObj1Wrap->getCollisionObject()->getFriction();
 839                                         c.m_node        =       &n;
 840                                         c.m_c0          =       ImpulseMatrix(psb->m_sst.sdt,ima,imb,iwi,ra);
 841                                         c.m_c1          =       ra;
 842                                         c.m_c2          =       ima*psb->m_sst.sdt;
 843                                 c.m_c3          =       fv.length2()<(dn*fc*dn*fc)?0:1-fc;
 844                                         c.m_c4          =       m_colObj1Wrap->getCollisionObject()->isStaticOrKinematicObject()?psb->m_cfg.kKHR:psb->m_cfg.kCHR;
 845                                         psb->m_rcontacts.push_back(c);
 846                                         if (m_rigidBody)
 847                                                 m_rigidBody->activate();
 848                                 }
 849                         }
 850                 }
 851                 btSoftBody*             psb;
 852                 const btCollisionObjectWrapper* m_colObj1Wrap;
 853                 btRigidBody*    m_rigidBody;
 854                 btScalar                dynmargin;
 855                 btScalar                stamargin;
 856         };
 857         //
 858         // CollideVF_SS
 859         //
 860         struct  CollideVF_SS : btDbvt::ICollide
 861         {
 862                 void            Process(const btDbvtNode* lnode,
 863                         const btDbvtNode* lface)
 864                 {
 865                         btSoftBody::Node*       node=(btSoftBody::Node*)lnode->data;
 866                         btSoftBody::Face*       face=(btSoftBody::Face*)lface->data;
 867                         btVector3                       o=node->m_x;
 868                         btVector3                       p;
 869                         btScalar                        d=SIMD_INFINITY;
 870                         ProjectOrigin(  face->m_n[0]->m_x-o,
 871                                 face->m_n[1]->m_x-o,
 872                                 face->m_n[2]->m_x-o,
 873                                 p,d);
 874                         const btScalar  m=mrg+(o-node->m_q).length()*2;
 875                         if(d<(m*m))
 876                         {
 877                                 const btSoftBody::Node* n[]={face->m_n[0],face->m_n[1],face->m_n[2]};
 878                                 const btVector3                 w=BaryCoord(n[0]->m_x,n[1]->m_x,n[2]->m_x,p+o);
 879                                 const btScalar                  ma=node->m_im;
 880                                 btScalar                                mb=BaryEval(n[0]->m_im,n[1]->m_im,n[2]->m_im,w);
 881                                 if(     (n[0]->m_im<=0)||
 882                                         (n[1]->m_im<=0)||
 883                                         (n[2]->m_im<=0))
 884                                 {
 885                                         mb=0;
 886                                 }
 887                                 const btScalar  ms=ma+mb;
 888                                 if(ms>0)
 889                                 {
 890                                         btSoftBody::SContact    c;
 891                                         c.m_normal              =       p/-btSqrt(d);
 892                                         c.m_margin              =       m;
 893                                         c.m_node                =       node;
 894                                         c.m_face                =       face;
 895                                         c.m_weights             =       w;
 896                                         c.m_friction    =       btMax(psb[0]->m_cfg.kDF,psb[1]->m_cfg.kDF);
 897                                         c.m_cfm[0]              =       ma/ms*psb[0]->m_cfg.kSHR;
 898                                         c.m_cfm[1]              =       mb/ms*psb[1]->m_cfg.kSHR;
 899                                         psb[0]->m_scontacts.push_back(c);
 900                                 }
 901                         }
 902                 }
 903                 btSoftBody*             psb[2];
 904                 btScalar                mrg;
 905         };
 906 };
 907
 908 #endif //_BT_SOFT_BODY_INTERNALS_H