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