2 Bullet Continuous Collision Detection and Physics Library
3 Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
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:
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.
15 ///btSoftBody implementation by Nathanael Presson
17 #ifndef _BT_SOFT_BODY_INTERNALS_H
18 #define _BT_SOFT_BODY_INTERNALS_H
20 #include "btSoftBody.h"
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
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;
47 // btSoftBodyCollisionShape
49 class btSoftBodyCollisionShape : public btConcaveShape
54 btSoftBodyCollisionShape(btSoftBody* backptr)
56 m_shapeType = SOFTBODY_SHAPE_PROXYTYPE;
60 virtual ~btSoftBodyCollisionShape()
65 void processAllTriangles(btTriangleCallback* /*callback*/,const btVector3& /*aabbMin*/,const btVector3& /*aabbMax*/) const
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
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];
88 aabbMin.setMin(crns[i]);
89 aabbMax.setMax(crns[i]);
94 virtual void setLocalScaling(const btVector3& /*scaling*/)
98 virtual const btVector3& getLocalScaling() const
100 static const btVector3 dummy(1,1,1);
103 virtual void calculateLocalInertia(btScalar /*mass*/,btVector3& /*inertia*/) const
108 virtual const char* getName()const
116 // btSoftClusterCollisionShape
118 class btSoftClusterCollisionShape : public btConvexInternalShape
121 const btSoftBody::Cluster* m_cluster;
123 btSoftClusterCollisionShape (const btSoftBody::Cluster* cluster) : m_cluster(cluster) { setMargin(0); }
126 virtual btVector3 localGetSupportingVertex(const btVector3& vec) const
128 btSoftBody::Node* const * n=&m_cluster->m_nodes[0];
129 btScalar d=btDot(vec,n[0]->m_x);
131 for(int i=1,ni=m_cluster->m_nodes.size();i<ni;++i)
133 const btScalar k=btDot(vec,n[i]->m_x);
138 virtual btVector3 localGetSupportingVertexWithoutMargin(const btVector3& vec)const
140 return(localGetSupportingVertex(vec));
142 //notice that the vectors should be unit length
143 virtual void batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const
147 virtual void calculateLocalInertia(btScalar mass,btVector3& inertia) const
150 virtual void getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const
153 virtual int getShapeType() const { return SOFTBODY_SHAPE_PROXYTYPE; }
156 virtual const char* getName()const {return "SOFTCLUSTER";}
158 virtual void setMargin(btScalar margin)
160 btConvexInternalShape::setMargin(margin);
162 virtual btScalar getMargin() const
173 template <typename T>
174 static inline void ZeroInitialize(T& value)
176 memset(&value,0,sizeof(T));
179 template <typename T>
180 static inline bool CompLess(const T& a,const T& b)
183 template <typename T>
184 static inline bool CompGreater(const T& a,const T& b)
187 template <typename T>
188 static inline T Lerp(const T& a,const T& b,btScalar t)
189 { return(a+(b-a)*t); }
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)); }
195 static inline btMatrix3x3 Lerp( const btMatrix3x3& a,
196 const btMatrix3x3& b,
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);
206 static inline btVector3 Clamp(const btVector3& v,btScalar maxlength)
208 const btScalar sql=v.length2();
209 if(sql>(maxlength*maxlength))
210 return((v*maxlength)/btSqrt(sql));
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); }
219 template <typename T>
220 static inline T Sq(const T& x)
223 template <typename T>
224 static inline T Cube(const T& x)
227 template <typename T>
228 static inline T Sign(const T& x)
229 { return((T)(x<0?-1:+1)); }
231 template <typename T>
232 static inline bool SameSign(const T& x,const T& y)
235 static inline btScalar ClusterMetric(const btVector3& x,const btVector3& y)
237 const btVector3 d=x-y;
238 return(btFabs(d[0])+btFabs(d[1])+btFabs(d[2]));
241 static inline btMatrix3x3 ScaleAlongAxis(const btVector3& a,btScalar s)
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();
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);
256 static inline btMatrix3x3 Cross(const btVector3& v)
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);
265 static inline btMatrix3x3 Diagonal(btScalar x)
268 m[0]=btVector3(x,0,0);
269 m[1]=btVector3(0,x,0);
270 m[2]=btVector3(0,0,x);
274 static inline btMatrix3x3 Add(const btMatrix3x3& a,
275 const btMatrix3x3& b)
278 for(int i=0;i<3;++i) r[i]=a[i]+b[i];
282 static inline btMatrix3x3 Sub(const btMatrix3x3& a,
283 const btMatrix3x3& b)
286 for(int i=0;i<3;++i) r[i]=a[i]-b[i];
290 static inline btMatrix3x3 Mul(const btMatrix3x3& a,
294 for(int i=0;i<3;++i) r[i]=a[i]*b;
298 static inline void Orthogonalize(btMatrix3x3& m)
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();
305 static inline btMatrix3x3 MassMatrix(btScalar im,const btMatrix3x3& iwi,const btVector3& r)
307 const btMatrix3x3 cr=Cross(r);
308 return(Sub(Diagonal(im),cr*iwi*cr));
312 static inline btMatrix3x3 ImpulseMatrix( btScalar dt,
315 const btMatrix3x3& iwi,
318 return(Diagonal(1/dt)*Add(Diagonal(ima),MassMatrix(imb,iwi,r)).inverse());
322 static inline btMatrix3x3 ImpulseMatrix( btScalar ima,const btMatrix3x3& iia,const btVector3& ra,
323 btScalar imb,const btMatrix3x3& iib,const btVector3& rb)
325 return(Add(MassMatrix(ima,iia,ra),MassMatrix(imb,iib,rb)).inverse());
329 static inline btMatrix3x3 AngularImpulseMatrix( const btMatrix3x3& iia,
330 const btMatrix3x3& iib)
332 return(Add(iia,iib).inverse());
336 static inline btVector3 ProjectOnAxis( const btVector3& v,
339 return(a*btDot(v,a));
342 static inline btVector3 ProjectOnPlane( const btVector3& v,
345 return(v-ProjectOnAxis(v,a));
349 static inline void ProjectOrigin( const btVector3& a,
354 const btVector3 d=b-a;
355 const btScalar m2=d.length2();
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();
369 static inline void ProjectOrigin( const btVector3& a,
375 const btVector3& q=btCross(b-a,c-a);
376 const btScalar m2=q.length2();
379 const btVector3 n=q/btSqrt(m2);
380 const btScalar k=btDot(a,n);
381 const btScalar k2=k*k;
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))
394 ProjectOrigin(a,b,prj,sqd);
395 ProjectOrigin(b,c,prj,sqd);
396 ProjectOrigin(c,a,prj,sqd);
403 template <typename T>
404 static inline T BaryEval( const T& a,
407 const btVector3& coord)
409 return(a*coord.x()+b*coord.y()+c*coord.z());
412 static inline btVector3 BaryCoord( const btVector3& a,
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));
425 static btScalar ImplicitSolve( btSoftBody::ImplicitFn* fn,
428 const btScalar accuracy,
429 const int maxiterations=256)
431 btScalar span[2]={0,1};
432 btScalar values[2]={fn->Eval(a),fn->Eval(b)};
433 if(values[0]>values[1])
435 btSwap(span[0],span[1]);
436 btSwap(values[0],values[1]);
438 if(values[0]>-accuracy) return(-1);
439 if(values[1]<+accuracy) return(-1);
440 for(int i=0;i<maxiterations;++i)
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);
447 { span[0]=t;values[0]=v; }
449 { span[1]=t;values[1]=v; }
455 static inline btVector3 NormalizeAny(const btVector3& v)
457 const btScalar l=v.length();
461 return(btVector3(0,0,0));
465 static inline btDbvtVolume VolumeOf( const btSoftBody::Face& f,
468 const btVector3* pts[]={ &f.m_n[0]->m_x,
471 btDbvtVolume vol=btDbvtVolume::FromPoints(pts,3);
472 vol.Expand(btVector3(margin,margin,margin));
477 static inline btVector3 CenterOf( const btSoftBody::Face& f)
479 return((f.m_n[0]->m_x+f.m_n[1]->m_x+f.m_n[2]->m_x)/3);
483 static inline btScalar AreaOf( const btVector3& x0,
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();
495 static inline btScalar VolumeOf( const btVector3& x0,
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)));
507 static void EvaluateMedium( const btSoftBodyWorldInfo* wfi,
509 btSoftBody::sMedium& medium)
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)
516 const btScalar depth=-(btDot(x,wfi->water_normal)+wfi->water_offset);
519 medium.m_density = wfi->water_density;
520 medium.m_pressure = depth*wfi->water_density*wfi->m_gravity.length();
526 static inline void ApplyClampedForce( btSoftBody::Node& n,
530 const btScalar dtim=dt*n.m_im;
531 if((f*dtim).length2()>n.m_v.length2())
533 n.m_f-=ProjectOnAxis(n.m_v,f.normalized())/dtim;
542 static inline int MatchEdge( const btSoftBody::Node* a,
543 const btSoftBody::Node* b,
544 const btSoftBody::Node* ma,
545 const btSoftBody::Node* mb)
547 if((a==ma)&&(b==mb)) return(0);
548 if((a==mb)&&(b==ma)) return(1);
553 // btEigen : Extract eigen system,
554 // straitforward implementation of http://math.fullerton.edu/mathews/n2003/JacobiMethodMod.html
555 // outputs are NOT sorted.
559 static int system(btMatrix3x3& a,btMatrix3x3* vectors,btVector3* values=0)
561 static const int maxiterations=16;
562 static const btScalar accuracy=(btScalar)0.0001;
563 btMatrix3x3& v=*vectors;
565 vectors->setIdentity();
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)
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... */
577 const btScalar c=1/btSqrt(t*t+1);
578 const btScalar s=c*t;
584 } while((++iterations)<maxiterations);
587 *values=btVector3(a[0][0],a[1][1],a[2][2]);
592 static inline void mulTPQ(btMatrix3x3& a,btScalar c,btScalar s,int p,int q)
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]}};
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];
601 static inline void mulPQ(btMatrix3x3& a,btScalar c,btScalar s,int p,int q)
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]}};
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];
613 // Polar decomposition,
614 // "Computing the Polar Decomposition with Applications", Nicholas J. Higham, 1986.
616 static inline int PolarDecompose( const btMatrix3x3& m,btMatrix3x3& q,btMatrix3x3& s)
618 static const btPolarDecomposition polar;
619 return polar.decompose(m, q, s);
625 struct btSoftColliders
630 struct ClusterBase : btDbvt::ICollide
643 threshold =(btScalar)0;
645 bool SolveContact( const btGjkEpaSolver2::sResults& res,
646 btSoftBody::Body ba,const btSoftBody::Body bb,
647 btSoftBody::CJoint& joint)
649 if(res.distance<m_margin)
651 btVector3 norm = res.normal;
652 norm.normalize();//is it necessary?
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;
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;
677 joint.m_drift = depth*norm;
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]);
694 struct CollideCL_RS : ClusterBase
697 const btCollisionObjectWrapper* m_colObjWrap;
699 void Process(const btDbvtNode* leaf)
701 btSoftBody::Cluster* cluster=(btSoftBody::Cluster*)leaf->data;
702 btSoftClusterCollisionShape cshape(cluster);
704 const btConvexShape* rshape=(const btConvexShape*)m_colObjWrap->getCollisionShape();
706 ///don't collide an anchored cluster with a static/kinematic object
707 if(m_colObjWrap->getCollisionObject()->isStaticOrKinematicObject() && cluster->m_containsAnchor)
710 btGjkEpaSolver2::sResults res;
711 if(btGjkEpaSolver2::SignedDistance( &cshape,btTransform::getIdentity(),
712 rshape,m_colObjWrap->getWorldTransform(),
713 btVector3(1,0,0),res))
715 btSoftBody::CJoint joint;
716 if(SolveContact(res,cluster,m_colObjWrap->getCollisionObject(),joint))//prb,joint))
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())
722 pj->m_erp *= psb->m_cfg.kSKHR_CL;
723 pj->m_split *= psb->m_cfg.kSK_SPLT_CL;
727 pj->m_erp *= psb->m_cfg.kSRHR_CL;
728 pj->m_split *= psb->m_cfg.kSR_SPLT_CL;
733 void ProcessColObj(btSoftBody* ps,const btCollisionObjectWrapper* colObWrap)
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());
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);
754 struct CollideCL_SS : ClusterBase
756 btSoftBody* bodies[2];
757 void Process(const btDbvtNode* la,const btDbvtNode* lb)
759 btSoftBody::Cluster* cla=(btSoftBody::Cluster*)la->data;
760 btSoftBody::Cluster* clb=(btSoftBody::Cluster*)lb->data;
763 bool connected=false;
764 if ((bodies[0]==bodies[1])&&(bodies[0]->m_clusterConnectivity.size()))
766 connected = bodies[0]->m_clusterConnectivity[cla->m_clusterIndex+bodies[0]->m_clusters.size()*clb->m_clusterIndex];
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))
778 btSoftBody::CJoint joint;
779 if(SolveContact(res,cla,clb,joint))
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;
791 //printf("count=%d\n",count);
795 void ProcessSoftSoft(btSoftBody* psa,btSoftBody* psb)
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);
803 psa->m_cdbvt.collideTT(psa->m_cdbvt.m_root,psb->m_cdbvt.m_root,*this);
809 struct CollideSDF_RS : btDbvt::ICollide
811 void Process(const btDbvtNode* leaf)
813 btSoftBody::Node* node=(btSoftBody::Node*)leaf->data;
816 void DoNode(btSoftBody::Node& n) const
818 const btScalar m=n.m_im>0?dynmargin:stamargin;
819 btSoftBody::RContact c;
822 psb->checkContact(m_colObj1Wrap,n.m_x,m,c.m_cti))
824 const btScalar ima=n.m_im;
825 const btScalar imb= m_rigidBody? m_rigidBody->getInvMass() : 0.f;
826 const btScalar ms=ima+imb;
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();
840 c.m_c0 = ImpulseMatrix(psb->m_sst.sdt,ima,imb,iwi,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);
847 m_rigidBody->activate();
852 const btCollisionObjectWrapper* m_colObj1Wrap;
853 btRigidBody* m_rigidBody;
860 struct CollideVF_SS : btDbvt::ICollide
862 void Process(const btDbvtNode* lnode,
863 const btDbvtNode* lface)
865 btSoftBody::Node* node=(btSoftBody::Node*)lnode->data;
866 btSoftBody::Face* face=(btSoftBody::Face*)lface->data;
867 btVector3 o=node->m_x;
869 btScalar d=SIMD_INFINITY;
870 ProjectOrigin( face->m_n[0]->m_x-o,
874 const btScalar m=mrg+(o-node->m_q).length()*2;
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)||
887 const btScalar ms=ma+mb;
890 btSoftBody::SContact c;
891 c.m_normal = p/-btSqrt(d);
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);
908 #endif //_BT_SOFT_BODY_INTERNALS_H