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