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