- Timestamp:
- Oct 20, 2008, 5:40:38 PM (16 years ago)
- File:
-
- 1 edited
Legend:
- Unmodified
- Added
- Removed
-
code/branches/physics/src/bullet/BulletSoftBody/btSoftBodyInternals.h
r1963 r1972 32 32 struct btSymMatrix 33 33 { 34 btSymMatrix() : dim(0) {}35 btSymMatrix(int n,const T& init=T()) { resize(n,init); }36 37 38 39 40 41 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 42 }; 43 43 … … 49 49 public: 50 50 btSoftBody* m_body; 51 51 52 52 btSoftBodyCollisionShape(btSoftBody* backptr) 53 53 { … … 70 70 virtual void getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const 71 71 { 72 73 74 75 76 t*btVector3(maxs.x(),mins.y(),mins.z()),77 t*btVector3(maxs.x(),maxs.y(),mins.z()),78 t*btVector3(mins.x(),maxs.y(),mins.z()),79 t*btVector3(mins.x(),mins.y(),maxs.z()),80 t*btVector3(maxs.x(),mins.y(),maxs.z()),81 t*btVector3(maxs.x(),maxs.y(),maxs.z()),82 t*btVector3(mins.x(),maxs.y(),maxs.z())};83 84 85 { 86 87 88 } 89 } 90 91 72 /* t should be identity, but better be safe than...fast? */ 73 const btVector3 mins=m_body->m_bounds[0]; 74 const btVector3 maxs=m_body->m_bounds[1]; 75 const btVector3 crns[]={t*btVector3(mins.x(),mins.y(),mins.z()), 76 t*btVector3(maxs.x(),mins.y(),mins.z()), 77 t*btVector3(maxs.x(),maxs.y(),mins.z()), 78 t*btVector3(mins.x(),maxs.y(),mins.z()), 79 t*btVector3(mins.x(),mins.y(),maxs.z()), 80 t*btVector3(maxs.x(),mins.y(),maxs.z()), 81 t*btVector3(maxs.x(),maxs.y(),maxs.z()), 82 t*btVector3(mins.x(),maxs.y(),maxs.z())}; 83 aabbMin=aabbMax=crns[0]; 84 for(int i=1;i<8;++i) 85 { 86 aabbMin.setMin(crns[i]); 87 aabbMax.setMax(crns[i]); 88 } 89 } 90 91 92 92 virtual void setLocalScaling(const btVector3& /*scaling*/) 93 93 { … … 96 96 virtual const btVector3& getLocalScaling() const 97 97 { 98 99 98 static const btVector3 dummy(1,1,1); 99 return dummy; 100 100 } 101 101 virtual void calculateLocalInertia(btScalar /*mass*/,btVector3& /*inertia*/) const … … 120 120 121 121 btSoftClusterCollisionShape (const btSoftBody::Cluster* cluster) : m_cluster(cluster) { setMargin(0); } 122 123 122 123 124 124 virtual btVector3 localGetSupportingVertex(const btVector3& vec) const 125 {125 { 126 126 btSoftBody::Node* const * n=&m_cluster->m_nodes[0]; 127 127 btScalar d=dot(vec,n[0]->m_x); 128 128 int j=0; 129 129 for(int i=1,ni=m_cluster->m_nodes.size();i<ni;++i) 130 {130 { 131 131 const btScalar k=dot(vec,n[i]->m_x); 132 132 if(k>d) { d=k;j=i; } 133 }133 } 134 134 return(n[j]->m_x); 135 }135 } 136 136 virtual btVector3 localGetSupportingVertexWithoutMargin(const btVector3& vec)const 137 {137 { 138 138 return(localGetSupportingVertex(vec)); 139 }139 } 140 140 //notice that the vectors should be unit length 141 141 virtual void batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const … … 150 150 151 151 virtual int getShapeType() const { return SOFTBODY_SHAPE_PROXYTYPE; } 152 152 153 153 //debugging 154 154 virtual const char* getName()const {return "SOFTCLUSTER";} … … 172 172 static inline void ZeroInitialize(T& value) 173 173 { 174 175 174 static const T zerodummy; 175 value=zerodummy; 176 176 } 177 177 // … … 193 193 // 194 194 static inline btMatrix3x3 Lerp( const btMatrix3x3& a, 195 196 197 { 198 199 200 201 202 195 const btMatrix3x3& b, 196 btScalar t) 197 { 198 btMatrix3x3 r; 199 r[0]=Lerp(a[0],b[0],t); 200 r[1]=Lerp(a[1],b[1],t); 201 r[2]=Lerp(a[2],b[2],t); 202 return(r); 203 203 } 204 204 // 205 205 static inline btVector3 Clamp(const btVector3& v,btScalar maxlength) 206 206 { 207 208 209 207 const btScalar sql=v.length2(); 208 if(sql>(maxlength*maxlength)) 209 return((v*maxlength)/btSqrt(sql)); 210 210 else 211 211 return(v); 212 212 } 213 213 // … … 234 234 static inline btScalar ClusterMetric(const btVector3& x,const btVector3& y) 235 235 { 236 237 236 const btVector3 d=x-y; 237 return(btFabs(d[0])+btFabs(d[1])+btFabs(d[2])); 238 238 } 239 239 // … … 272 272 // 273 273 static inline btMatrix3x3 Add(const btMatrix3x3& a, 274 274 const btMatrix3x3& b) 275 275 { 276 276 btMatrix3x3 r; … … 280 280 // 281 281 static inline btMatrix3x3 Sub(const btMatrix3x3& a, 282 282 const btMatrix3x3& b) 283 283 { 284 284 btMatrix3x3 r; … … 288 288 // 289 289 static inline btMatrix3x3 Mul(const btMatrix3x3& a, 290 290 btScalar b) 291 291 { 292 292 btMatrix3x3 r; … … 297 297 static inline void Orthogonalize(btMatrix3x3& m) 298 298 { 299 300 301 299 m[2]=cross(m[0],m[1]).normalized(); 300 m[1]=cross(m[2],m[0]).normalized(); 301 m[0]=cross(m[1],m[2]).normalized(); 302 302 } 303 303 // … … 310 310 // 311 311 static inline btMatrix3x3 ImpulseMatrix( btScalar dt, 312 313 314 315 312 btScalar ima, 313 btScalar imb, 314 const btMatrix3x3& iwi, 315 const btVector3& r) 316 316 { 317 317 return(Diagonal(1/dt)*Add(Diagonal(ima),MassMatrix(imb,iwi,r)).inverse()); … … 320 320 // 321 321 static inline btMatrix3x3 ImpulseMatrix( btScalar ima,const btMatrix3x3& iia,const btVector3& ra, 322 323 { 324 322 btScalar imb,const btMatrix3x3& iib,const btVector3& rb) 323 { 324 return(Add(MassMatrix(ima,iia,ra),MassMatrix(imb,iib,rb)).inverse()); 325 325 } 326 326 327 327 // 328 328 static inline btMatrix3x3 AngularImpulseMatrix( const btMatrix3x3& iia, 329 330 { 331 329 const btMatrix3x3& iib) 330 { 331 return(Add(iia,iib).inverse()); 332 332 } 333 333 334 334 // 335 335 static inline btVector3 ProjectOnAxis( const btVector3& v, 336 336 const btVector3& a) 337 337 { 338 338 return(a*dot(v,a)); … … 340 340 // 341 341 static inline btVector3 ProjectOnPlane( const btVector3& v, 342 342 const btVector3& a) 343 343 { 344 344 return(v-ProjectOnAxis(v,a)); … … 347 347 // 348 348 static inline void ProjectOrigin( const btVector3& a, 349 350 351 352 { 353 354 355 349 const btVector3& b, 350 btVector3& prj, 351 btScalar& sqd) 352 { 353 const btVector3 d=b-a; 354 const btScalar m2=d.length2(); 355 if(m2>SIMD_EPSILON) 356 356 { 357 const btScalar t=Clamp<btScalar>(-dot(a,d)/m2,0,1); 358 const btVector3 p=a+d*t; 359 const btScalar l2=p.length2(); 360 if(l2<sqd) 361 { 357 const btScalar t=Clamp<btScalar>(-dot(a,d)/m2,0,1); 358 const btVector3 p=a+d*t; 359 const btScalar l2=p.length2(); 360 if(l2<sqd) 361 { 362 prj=p; 363 sqd=l2; 364 } 365 } 366 } 367 // 368 static inline void ProjectOrigin( const btVector3& a, 369 const btVector3& b, 370 const btVector3& c, 371 btVector3& prj, 372 btScalar& sqd) 373 { 374 const btVector3& q=cross(b-a,c-a); 375 const btScalar m2=q.length2(); 376 if(m2>SIMD_EPSILON) 377 { 378 const btVector3 n=q/btSqrt(m2); 379 const btScalar k=dot(a,n); 380 const btScalar k2=k*k; 381 if(k2<sqd) 382 { 383 const btVector3 p=n*k; 384 if( (dot(cross(a-p,b-p),q)>0)&& 385 (dot(cross(b-p,c-p),q)>0)&& 386 (dot(cross(c-p,a-p),q)>0)) 387 { 362 388 prj=p; 363 sqd=l2; 364 } 365 } 366 } 367 // 368 static inline void ProjectOrigin( const btVector3& a, 369 const btVector3& b, 370 const btVector3& c, 371 btVector3& prj, 372 btScalar& sqd) 373 { 374 const btVector3& q=cross(b-a,c-a); 375 const btScalar m2=q.length2(); 376 if(m2>SIMD_EPSILON) 377 { 378 const btVector3 n=q/btSqrt(m2); 379 const btScalar k=dot(a,n); 380 const btScalar k2=k*k; 381 if(k2<sqd) 382 { 383 const btVector3 p=n*k; 384 if( (dot(cross(a-p,b-p),q)>0)&& 385 (dot(cross(b-p,c-p),q)>0)&& 386 (dot(cross(c-p,a-p),q)>0)) 387 { 388 prj=p; 389 sqd=k2; 389 sqd=k2; 390 390 } 391 391 else 392 392 { 393 394 395 393 ProjectOrigin(a,b,prj,sqd); 394 ProjectOrigin(b,c,prj,sqd); 395 ProjectOrigin(c,a,prj,sqd); 396 396 } 397 397 } … … 402 402 template <typename T> 403 403 static inline T BaryEval( const T& a, 404 405 406 404 const T& b, 405 const T& c, 406 const btVector3& coord) 407 407 { 408 408 return(a*coord.x()+b*coord.y()+c*coord.z()); … … 410 410 // 411 411 static inline btVector3 BaryCoord( const btVector3& a, 412 413 414 415 { 416 417 cross(b-p,c-p).length(),418 cross(c-p,a-p).length()};419 420 412 const btVector3& b, 413 const btVector3& c, 414 const btVector3& p) 415 { 416 const btScalar w[]={ cross(a-p,b-p).length(), 417 cross(b-p,c-p).length(), 418 cross(c-p,a-p).length()}; 419 const btScalar isum=1/(w[0]+w[1]+w[2]); 420 return(btVector3(w[1]*isum,w[2]*isum,w[0]*isum)); 421 421 } 422 422 423 423 // 424 424 static btScalar ImplicitSolve( btSoftBody::ImplicitFn* fn, 425 426 427 428 429 { 430 431 432 433 { 434 435 436 } 437 438 439 440 { 441 442 443 444 445 425 const btVector3& a, 426 const btVector3& b, 427 const btScalar accuracy, 428 const int maxiterations=256) 429 { 430 btScalar span[2]={0,1}; 431 btScalar values[2]={fn->Eval(a),fn->Eval(b)}; 432 if(values[0]>values[1]) 433 { 434 btSwap(span[0],span[1]); 435 btSwap(values[0],values[1]); 436 } 437 if(values[0]>-accuracy) return(-1); 438 if(values[1]<+accuracy) return(-1); 439 for(int i=0;i<maxiterations;++i) 440 { 441 const btScalar t=Lerp(span[0],span[1],values[0]/(values[0]-values[1])); 442 const btScalar v=fn->Eval(Lerp(a,b,t)); 443 if((t<=0)||(t>=1)) break; 444 if(btFabs(v)<accuracy) return(t); 445 if(v<0) 446 446 { span[0]=t;values[0]=v; } 447 447 else 448 448 { span[1]=t;values[1]=v; } 449 449 } 450 450 return(-1); 451 451 } 452 452 … … 463 463 // 464 464 static inline btDbvtVolume VolumeOf( const btSoftBody::Face& f, 465 466 { 467 468 &f.m_n[1]->m_x,469 &f.m_n[2]->m_x};470 471 472 465 btScalar margin) 466 { 467 const btVector3* pts[]={ &f.m_n[0]->m_x, 468 &f.m_n[1]->m_x, 469 &f.m_n[2]->m_x}; 470 btDbvtVolume vol=btDbvtVolume::FromPoints(pts,3); 471 vol.Expand(btVector3(margin,margin,margin)); 472 return(vol); 473 473 } 474 474 … … 476 476 static inline btVector3 CenterOf( const btSoftBody::Face& f) 477 477 { 478 478 return((f.m_n[0]->m_x+f.m_n[1]->m_x+f.m_n[2]->m_x)/3); 479 479 } 480 480 481 481 // 482 482 static inline btScalar AreaOf( const btVector3& x0, 483 484 483 const btVector3& x1, 484 const btVector3& x2) 485 485 { 486 486 const btVector3 a=x1-x0; … … 493 493 // 494 494 static inline btScalar VolumeOf( const btVector3& x0, 495 496 497 495 const btVector3& x1, 496 const btVector3& x2, 497 const btVector3& x3) 498 498 { 499 499 const btVector3 a=x1-x0; … … 505 505 // 506 506 static void EvaluateMedium( const btSoftBodyWorldInfo* wfi, 507 508 507 const btVector3& x, 508 btSoftBody::sMedium& medium) 509 509 { 510 510 medium.m_velocity = btVector3(0,0,0); … … 524 524 // 525 525 static inline void ApplyClampedForce( btSoftBody::Node& n, 526 527 526 const btVector3& f, 527 btScalar dt) 528 528 { 529 529 const btScalar dtim=dt*n.m_im; … … 540 540 // 541 541 static inline int MatchEdge( const btSoftBody::Node* a, 542 543 544 545 { 546 547 548 542 const btSoftBody::Node* b, 543 const btSoftBody::Node* ma, 544 const btSoftBody::Node* mb) 545 { 546 if((a==ma)&&(b==mb)) return(0); 547 if((a==mb)&&(b==ma)) return(1); 548 return(-1); 549 549 } 550 550 … … 556 556 struct btEigen 557 557 { 558 559 { 560 561 562 563 564 565 566 567 568 569 558 static int system(btMatrix3x3& a,btMatrix3x3* vectors,btVector3* values=0) 559 { 560 static const int maxiterations=16; 561 static const btScalar accuracy=(btScalar)0.0001; 562 btMatrix3x3& v=*vectors; 563 int iterations=0; 564 vectors->setIdentity(); 565 do { 566 int p=0,q=1; 567 if(btFabs(a[p][q])<btFabs(a[0][2])) { p=0;q=2; } 568 if(btFabs(a[p][q])<btFabs(a[1][2])) { p=1;q=2; } 569 if(btFabs(a[p][q])>accuracy) 570 570 { 571 572 573 574 571 const btScalar w=(a[q][q]-a[p][p])/(2*a[p][q]); 572 const btScalar z=btFabs(w); 573 const btScalar t=w/(z*(btSqrt(1+w*w)+z)); 574 if(t==t)/* [WARNING] let hope that one does not get thrown aways by some compilers... */ 575 575 { 576 577 578 579 580 576 const btScalar c=1/btSqrt(t*t+1); 577 const btScalar s=c*t; 578 mulPQ(a,c,s,p,q); 579 mulTPQ(a,c,s,p,q); 580 mulPQ(v,c,s,p,q); 581 581 } else break; 582 582 } else break; 583 583 } while((++iterations)<maxiterations); 584 585 { 586 587 } 588 584 if(values) 585 { 586 *values=btVector3(a[0][0],a[1][1],a[2][2]); 587 } 588 return(iterations); 589 589 } 590 590 private: 591 592 { 593 594 {a[q][0],a[q][1],a[q][2]}};595 596 597 598 599 } 600 601 { 602 603 {a[0][q],a[1][q],a[2][q]}};604 605 606 607 591 static inline void mulTPQ(btMatrix3x3& a,btScalar c,btScalar s,int p,int q) 592 { 593 const btScalar m[2][3]={ {a[p][0],a[p][1],a[p][2]}, 594 {a[q][0],a[q][1],a[q][2]}}; 595 int i; 596 597 for(i=0;i<3;++i) a[p][i]=c*m[0][i]-s*m[1][i]; 598 for(i=0;i<3;++i) a[q][i]=c*m[1][i]+s*m[0][i]; 599 } 600 static inline void mulPQ(btMatrix3x3& a,btScalar c,btScalar s,int p,int q) 601 { 602 const btScalar m[2][3]={ {a[0][p],a[1][p],a[2][p]}, 603 {a[0][q],a[1][q],a[2][q]}}; 604 int i; 605 606 for(i=0;i<3;++i) a[i][p]=c*m[0][i]-s*m[1][i]; 607 for(i=0;i<3;++i) a[i][q]=c*m[1][i]+s*m[0][i]; 608 608 } 609 609 }; … … 640 640 s.setIdentity(); 641 641 } 642 642 return(i); 643 643 } 644 644 … … 653 653 struct ClusterBase : btDbvt::ICollide 654 654 { 655 656 657 658 659 660 ClusterBase()661 { 662 663 664 665 666 667 } 668 669 btSoftBody::Body ba,btSoftBody::Body bb,670 btSoftBody::CJoint& joint)671 { 672 655 btScalar erp; 656 btScalar idt; 657 btScalar margin; 658 btScalar friction; 659 btScalar threshold; 660 ClusterBase() 661 { 662 erp =(btScalar)1; 663 idt =0; 664 margin =0; 665 friction =0; 666 threshold =(btScalar)0; 667 } 668 bool SolveContact( const btGjkEpaSolver2::sResults& res, 669 btSoftBody::Body ba,btSoftBody::Body bb, 670 btSoftBody::CJoint& joint) 671 { 672 if(res.distance<margin) 673 673 { 674 675 676 677 678 679 680 681 682 683 684 685 686 687 688 689 690 691 692 693 694 695 696 697 698 699 bb.invMass(),bb.invWorldInertia(),joint.m_rpos[1]);700 674 const btVector3 ra=res.witnesses[0]-ba.xform().getOrigin(); 675 const btVector3 rb=res.witnesses[1]-bb.xform().getOrigin(); 676 const btVector3 va=ba.velocity(ra); 677 const btVector3 vb=bb.velocity(rb); 678 const btVector3 vrel=va-vb; 679 const btScalar rvac=dot(vrel,res.normal); 680 const btScalar depth=res.distance-margin; 681 const btVector3 iv=res.normal*rvac; 682 const btVector3 fv=vrel-iv; 683 joint.m_bodies[0] = ba; 684 joint.m_bodies[1] = bb; 685 joint.m_refs[0] = ra*ba.xform().getBasis(); 686 joint.m_refs[1] = rb*bb.xform().getBasis(); 687 joint.m_rpos[0] = ra; 688 joint.m_rpos[1] = rb; 689 joint.m_cfm = 1; 690 joint.m_erp = 1; 691 joint.m_life = 0; 692 joint.m_maxlife = 0; 693 joint.m_split = 1; 694 joint.m_drift = depth*res.normal; 695 joint.m_normal = res.normal; 696 joint.m_delete = false; 697 joint.m_friction = fv.length2()<(-rvac*friction)?1:friction; 698 joint.m_massmatrix = ImpulseMatrix( ba.invMass(),ba.invWorldInertia(),joint.m_rpos[0], 699 bb.invMass(),bb.invWorldInertia(),joint.m_rpos[1]); 700 return(true); 701 701 } 702 702 return(false); 703 703 } 704 704 }; … … 708 708 struct CollideCL_RS : ClusterBase 709 709 { 710 711 712 713 { 714 715 716 717 718 719 rshape,prb->getInterpolationWorldTransform(),720 btVector3(1,0,0),res))710 btSoftBody* psb; 711 btRigidBody* prb; 712 void Process(const btDbvtNode* leaf) 713 { 714 btSoftBody::Cluster* cluster=(btSoftBody::Cluster*)leaf->data; 715 btSoftClusterCollisionShape cshape(cluster); 716 const btConvexShape* rshape=(const btConvexShape*)prb->getCollisionShape(); 717 btGjkEpaSolver2::sResults res; 718 if(btGjkEpaSolver2::SignedDistance( &cshape,btTransform::getIdentity(), 719 rshape,prb->getInterpolationWorldTransform(), 720 btVector3(1,0,0),res)) 721 721 { 722 723 722 btSoftBody::CJoint joint; 723 if(SolveContact(res,cluster,prb,joint)) 724 724 { 725 726 727 725 btSoftBody::CJoint* pj=new(btAlignedAlloc(sizeof(btSoftBody::CJoint),16)) btSoftBody::CJoint(); 726 *pj=joint;psb->m_joints.push_back(pj); 727 if(prb->isStaticOrKinematicObject()) 728 728 { 729 730 729 pj->m_erp *= psb->m_cfg.kSKHR_CL; 730 pj->m_split *= psb->m_cfg.kSK_SPLT_CL; 731 731 } 732 732 else 733 733 { 734 735 734 pj->m_erp *= psb->m_cfg.kSRHR_CL; 735 pj->m_split *= psb->m_cfg.kSR_SPLT_CL; 736 736 } 737 737 } 738 738 } 739 739 } 740 741 { 742 743 744 745 746 pr->getCollisionShape()->getMargin();747 748 749 750 751 752 753 754 755 740 void Process(btSoftBody* ps,btRigidBody* pr) 741 { 742 psb = ps; 743 prb = pr; 744 idt = ps->m_sst.isdt; 745 margin = ps->getCollisionShape()->getMargin()+ 746 pr->getCollisionShape()->getMargin(); 747 friction = btMin(psb->m_cfg.kDF,prb->getFriction()); 748 btVector3 mins; 749 btVector3 maxs; 750 751 ATTRIBUTE_ALIGNED16(btDbvtVolume) volume; 752 pr->getCollisionShape()->getAabb(pr->getInterpolationWorldTransform(),mins,maxs); 753 volume=btDbvtVolume::FromMM(mins,maxs); 754 volume.Expand(btVector3(1,1,1)*margin); 755 btDbvt::collideTV(ps->m_cdbvt.m_root,volume,*this); 756 756 } 757 757 }; … … 761 761 struct CollideCL_SS : ClusterBase 762 762 { 763 764 765 { 766 767 768 769 770 771 772 &csb,btTransform::getIdentity(),773 cla->m_com-clb->m_com,res))763 btSoftBody* bodies[2]; 764 void Process(const btDbvtNode* la,const btDbvtNode* lb) 765 { 766 btSoftBody::Cluster* cla=(btSoftBody::Cluster*)la->data; 767 btSoftBody::Cluster* clb=(btSoftBody::Cluster*)lb->data; 768 btSoftClusterCollisionShape csa(cla); 769 btSoftClusterCollisionShape csb(clb); 770 btGjkEpaSolver2::sResults res; 771 if(btGjkEpaSolver2::SignedDistance( &csa,btTransform::getIdentity(), 772 &csb,btTransform::getIdentity(), 773 cla->m_com-clb->m_com,res)) 774 774 { 775 776 775 btSoftBody::CJoint joint; 776 if(SolveContact(res,cla,clb,joint)) 777 777 { 778 779 780 781 778 btSoftBody::CJoint* pj=new(btAlignedAlloc(sizeof(btSoftBody::CJoint),16)) btSoftBody::CJoint(); 779 *pj=joint;bodies[0]->m_joints.push_back(pj); 780 pj->m_erp *= btMax(bodies[0]->m_cfg.kSSHR_CL,bodies[1]->m_cfg.kSSHR_CL); 781 pj->m_split *= (bodies[0]->m_cfg.kSS_SPLT_CL+bodies[1]->m_cfg.kSS_SPLT_CL)/2; 782 782 } 783 783 } 784 784 } 785 786 { 787 788 789 790 791 792 785 void Process(btSoftBody* psa,btSoftBody* psb) 786 { 787 idt = psa->m_sst.isdt; 788 margin = (psa->getCollisionShape()->getMargin()+psb->getCollisionShape()->getMargin())/2; 789 friction = btMin(psa->m_cfg.kDF,psb->m_cfg.kDF); 790 bodies[0] = psa; 791 bodies[1] = psb; 792 btDbvt::collideTT(psa->m_cdbvt.m_root,psb->m_cdbvt.m_root,*this); 793 793 } 794 794 }; … … 798 798 struct CollideSDF_RS : btDbvt::ICollide 799 799 { 800 801 { 802 803 804 } 805 806 { 807 808 809 810 800 void Process(const btDbvtNode* leaf) 801 { 802 btSoftBody::Node* node=(btSoftBody::Node*)leaf->data; 803 DoNode(*node); 804 } 805 void DoNode(btSoftBody::Node& n) const 806 { 807 const btScalar m=n.m_im>0?dynmargin:stamargin; 808 btSoftBody::RContact c; 809 if( (!n.m_battach)&& 810 psb->checkContact(prb,n.m_x,m,c.m_cti)) 811 811 { 812 813 814 815 812 const btScalar ima=n.m_im; 813 const btScalar imb=prb->getInvMass(); 814 const btScalar ms=ima+imb; 815 if(ms>0) 816 816 { 817 818 819 820 821 822 823 824 825 826 827 828 829 830 831 832 833 817 const btTransform& wtr=prb->getInterpolationWorldTransform(); 818 const btMatrix3x3& iwi=prb->getInvInertiaTensorWorld(); 819 const btVector3 ra=n.m_x-wtr.getOrigin(); 820 const btVector3 va=prb->getVelocityInLocalPoint(ra)*psb->m_sst.sdt; 821 const btVector3 vb=n.m_x-n.m_q; 822 const btVector3 vr=vb-va; 823 const btScalar dn=dot(vr,c.m_cti.m_normal); 824 const btVector3 fv=vr-c.m_cti.m_normal*dn; 825 const btScalar fc=psb->m_cfg.kDF*prb->getFriction(); 826 c.m_node = &n; 827 c.m_c0 = ImpulseMatrix(psb->m_sst.sdt,ima,imb,iwi,ra); 828 c.m_c1 = ra; 829 c.m_c2 = ima*psb->m_sst.sdt; 830 c.m_c3 = fv.length2()<(btFabs(dn)*fc)?0:1-fc; 831 c.m_c4 = prb->isStaticOrKinematicObject()?psb->m_cfg.kKHR:psb->m_cfg.kCHR; 832 psb->m_rcontacts.push_back(c); 833 prb->activate(); 834 834 } 835 835 } 836 836 } 837 838 839 840 837 btSoftBody* psb; 838 btRigidBody* prb; 839 btScalar dynmargin; 840 btScalar stamargin; 841 841 }; 842 842 // … … 845 845 struct CollideVF_SS : btDbvt::ICollide 846 846 { 847 848 const btDbvtNode* lface)849 { 850 851 852 853 854 855 856 face->m_n[1]->m_x-o,857 face->m_n[2]->m_x-o,858 p,d);859 860 847 void Process(const btDbvtNode* lnode, 848 const btDbvtNode* lface) 849 { 850 btSoftBody::Node* node=(btSoftBody::Node*)lnode->data; 851 btSoftBody::Face* face=(btSoftBody::Face*)lface->data; 852 btVector3 o=node->m_x; 853 btVector3 p; 854 btScalar d=SIMD_INFINITY; 855 ProjectOrigin( face->m_n[0]->m_x-o, 856 face->m_n[1]->m_x-o, 857 face->m_n[2]->m_x-o, 858 p,d); 859 const btScalar m=mrg+(o-node->m_q).length()*2; 860 if(d<(m*m)) 861 861 { 862 863 864 865 866 867 868 862 const btSoftBody::Node* n[]={face->m_n[0],face->m_n[1],face->m_n[2]}; 863 const btVector3 w=BaryCoord(n[0]->m_x,n[1]->m_x,n[2]->m_x,p+o); 864 const btScalar ma=node->m_im; 865 btScalar mb=BaryEval(n[0]->m_im,n[1]->m_im,n[2]->m_im,w); 866 if( (n[0]->m_im<=0)|| 867 (n[1]->m_im<=0)|| 868 (n[2]->m_im<=0)) 869 869 { 870 870 mb=0; 871 871 } 872 873 872 const btScalar ms=ma+mb; 873 if(ms>0) 874 874 { 875 876 877 878 879 880 881 882 883 884 875 btSoftBody::SContact c; 876 c.m_normal = p/-btSqrt(d); 877 c.m_margin = m; 878 c.m_node = node; 879 c.m_face = face; 880 c.m_weights = w; 881 c.m_friction = btMax(psb[0]->m_cfg.kDF,psb[1]->m_cfg.kDF); 882 c.m_cfm[0] = ma/ms*psb[0]->m_cfg.kSHR; 883 c.m_cfm[1] = mb/ms*psb[1]->m_cfg.kSHR; 884 psb[0]->m_scontacts.push_back(c); 885 885 } 886 886 } 887 887 } 888 889 888 btSoftBody* psb[2]; 889 btScalar mrg; 890 890 }; 891 891 };
Note: See TracChangeset
for help on using the changeset viewer.
