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