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"
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"
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;
45
// btSoftBodyCollisionShape
47
class btSoftBodyCollisionShape : public btConcaveShape
52
btSoftBodyCollisionShape(btSoftBody* backptr)
57
virtual ~btSoftBodyCollisionShape()
62
void processAllTriangles(btTriangleCallback* /*callback*/,const btVector3& /*aabbMin*/,const btVector3& /*aabbMax*/) const
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
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];
85
aabbMin.setMin(crns[i]);
86
aabbMax.setMax(crns[i]);
90
virtual int getShapeType() const
92
return SOFTBODY_SHAPE_PROXYTYPE;
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=dot(vec,n[0]->m_x);
131
for(int i=1,ni=m_cluster->m_nodes.size();i<ni;++i)
133
const btScalar k=dot(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
static const T zerodummy;
180
template <typename T>
181
static inline bool CompLess(const T& a,const T& b)
184
template <typename T>
185
static inline bool CompGreater(const T& a,const T& b)
188
template <typename T>
189
static inline T Lerp(const T& a,const T& b,btScalar t)
190
{ return(a+(b-a)*t); }
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)); }
196
static inline btMatrix3x3 Lerp( const btMatrix3x3& a,
197
const btMatrix3x3& b,
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);
207
static inline btVector3 Clamp(const btVector3& v,btScalar maxlength)
209
const btScalar sql=v.length2();
210
if(sql>(maxlength*maxlength))
211
return((v*maxlength)/btSqrt(sql));
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); }
220
template <typename T>
221
static inline T Sq(const T& x)
224
template <typename T>
225
static inline T Cube(const T& x)
228
template <typename T>
229
static inline T Sign(const T& x)
230
{ return((T)(x<0?-1:+1)); }
232
template <typename T>
233
static inline bool SameSign(const T& x,const T& y)
236
static inline btScalar ClusterMetric(const btVector3& x,const btVector3& y)
238
const btVector3 d=x-y;
239
return(btFabs(d[0])+btFabs(d[1])+btFabs(d[2]));
242
static inline btMatrix3x3 ScaleAlongAxis(const btVector3& a,btScalar s)
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();
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);
257
static inline btMatrix3x3 Cross(const btVector3& v)
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);
266
static inline btMatrix3x3 Diagonal(btScalar x)
269
m[0]=btVector3(x,0,0);
270
m[1]=btVector3(0,x,0);
271
m[2]=btVector3(0,0,x);
275
static inline btMatrix3x3 Add(const btMatrix3x3& a,
276
const btMatrix3x3& b)
279
for(int i=0;i<3;++i) r[i]=a[i]+b[i];
283
static inline btMatrix3x3 Sub(const btMatrix3x3& a,
284
const btMatrix3x3& b)
287
for(int i=0;i<3;++i) r[i]=a[i]-b[i];
291
static inline btMatrix3x3 Mul(const btMatrix3x3& a,
295
for(int i=0;i<3;++i) r[i]=a[i]*b;
299
static inline void Orthogonalize(btMatrix3x3& m)
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();
306
static inline btMatrix3x3 MassMatrix(btScalar im,const btMatrix3x3& iwi,const btVector3& r)
308
const btMatrix3x3 cr=Cross(r);
309
return(Sub(Diagonal(im),cr*iwi*cr));
313
static inline btMatrix3x3 ImpulseMatrix( btScalar dt,
316
const btMatrix3x3& iwi,
319
return(Diagonal(1/dt)*Add(Diagonal(ima),MassMatrix(imb,iwi,r)).inverse());
323
static inline btMatrix3x3 ImpulseMatrix( btScalar ima,const btMatrix3x3& iia,const btVector3& ra,
324
btScalar imb,const btMatrix3x3& iib,const btVector3& rb)
326
return(Add(MassMatrix(ima,iia,ra),MassMatrix(imb,iib,rb)).inverse());
330
static inline btMatrix3x3 AngularImpulseMatrix( const btMatrix3x3& iia,
331
const btMatrix3x3& iib)
333
return(Add(iia,iib).inverse());
337
static inline btVector3 ProjectOnAxis( const btVector3& v,
343
static inline btVector3 ProjectOnPlane( const btVector3& v,
346
return(v-ProjectOnAxis(v,a));
350
static inline void ProjectOrigin( const btVector3& a,
355
const btVector3 d=b-a;
356
const btScalar m2=d.length2();
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();
370
static inline void ProjectOrigin( const btVector3& a,
376
const btVector3& q=cross(b-a,c-a);
377
const btScalar m2=q.length2();
380
const btVector3 n=q/btSqrt(m2);
381
const btScalar k=dot(a,n);
382
const btScalar k2=k*k;
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))
395
ProjectOrigin(a,b,prj,sqd);
396
ProjectOrigin(b,c,prj,sqd);
397
ProjectOrigin(c,a,prj,sqd);
404
template <typename T>
405
static inline T BaryEval( const T& a,
408
const btVector3& coord)
410
return(a*coord.x()+b*coord.y()+c*coord.z());
413
static inline btVector3 BaryCoord( const btVector3& a,
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));
426
static btScalar ImplicitSolve( btSoftBody::ImplicitFn* fn,
429
const btScalar accuracy,
430
const int maxiterations=256)
432
btScalar span[2]={0,1};
433
btScalar values[2]={fn->Eval(a),fn->Eval(b)};
434
if(values[0]>values[1])
436
btSwap(span[0],span[1]);
437
btSwap(values[0],values[1]);
439
if(values[0]>-accuracy) return(-1);
440
if(values[1]<+accuracy) return(-1);
441
for(int i=0;i<maxiterations;++i)
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);
448
{ span[0]=t;values[0]=v; }
450
{ span[1]=t;values[1]=v; }
456
static inline btVector3 NormalizeAny(const btVector3& v)
458
const btScalar l=v.length();
462
return(btVector3(0,0,0));
466
static inline btDbvtVolume VolumeOf( const btSoftBody::Face& f,
469
const btVector3* pts[]={ &f.m_n[0]->m_x,
472
btDbvtVolume vol=btDbvtVolume::FromPoints(pts,3);
473
vol.Expand(btVector3(margin,margin,margin));
478
static inline btVector3 CenterOf( const btSoftBody::Face& f)
480
return((f.m_n[0]->m_x+f.m_n[1]->m_x+f.m_n[2]->m_x)/3);
484
static inline btScalar AreaOf( const btVector3& x0,
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();
496
static inline btScalar VolumeOf( const btVector3& x0,
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)));
508
static void EvaluateMedium( const btSoftBodyWorldInfo* wfi,
510
btSoftBody::sMedium& medium)
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)
517
const btScalar depth=-(dot(x,wfi->water_normal)+wfi->water_offset);
520
medium.m_density = wfi->water_density;
521
medium.m_pressure = depth*wfi->water_density*wfi->m_gravity.length();
527
static inline void ApplyClampedForce( btSoftBody::Node& n,
531
const btScalar dtim=dt*n.m_im;
532
if((f*dtim).length2()>n.m_v.length2())
534
n.m_f-=ProjectOnAxis(n.m_v,f.normalized())/dtim;
543
static inline int MatchEdge( const btSoftBody::Node* a,
544
const btSoftBody::Node* b,
545
const btSoftBody::Node* ma,
546
const btSoftBody::Node* mb)
548
if((a==ma)&&(b==mb)) return(0);
549
if((a==mb)&&(b==ma)) return(1);
554
// btEigen : Extract eigen system,
555
// straitforward implementation of http://math.fullerton.edu/mathews/n2003/JacobiMethodMod.html
556
// outputs are NOT sorted.
560
static int system(btMatrix3x3& a,btMatrix3x3* vectors,btVector3* values=0)
562
static const int maxiterations=16;
563
static const btScalar accuracy=(btScalar)0.0001;
564
btMatrix3x3& v=*vectors;
566
vectors->setIdentity();
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)
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... */
578
const btScalar c=1/btSqrt(t*t+1);
579
const btScalar s=c*t;
585
} while((++iterations)<maxiterations);
588
*values=btVector3(a[0][0],a[1][1],a[2][2]);
593
static inline void mulTPQ(btMatrix3x3& a,btScalar c,btScalar s,int p,int q)
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]}};
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];
602
static inline void mulPQ(btMatrix3x3& a,btScalar c,btScalar s,int p,int q)
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]}};
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];
614
// Polar decomposition,
615
// "Computing the Polar Decomposition with Applications", Nicholas J. Higham, 1986.
617
static inline int PolarDecompose( const btMatrix3x3& m,btMatrix3x3& q,btMatrix3x3& s)
619
static const btScalar half=(btScalar)0.5;
620
static const btScalar accuracy=(btScalar)0.0001;
621
static const int maxiterations=16;
624
q = Mul(m,1/btVector3(m[0][0],m[1][1],m[2][2]).length());
625
det = q.determinant();
626
if(!btFuzzyZero(det))
628
for(;i<maxiterations;++i)
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;
634
/* Final orthogonalization */
650
struct btSoftColliders
655
struct ClusterBase : btDbvt::ICollide
668
threshold =(btScalar)0;
670
bool SolveContact( const btGjkEpaSolver2::sResults& res,
671
btSoftBody::Body ba,btSoftBody::Body bb,
672
btSoftBody::CJoint& joint)
674
if(res.distance<margin)
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;
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]);
710
struct CollideCL_RS : ClusterBase
714
void Process(const btDbvtNode* leaf)
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))
724
btSoftBody::CJoint joint;
725
if(SolveContact(res,cluster,prb,joint))
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())
731
pj->m_erp *= psb->m_cfg.kSKHR_CL;
732
pj->m_split *= psb->m_cfg.kSK_SPLT_CL;
736
pj->m_erp *= psb->m_cfg.kSRHR_CL;
737
pj->m_split *= psb->m_cfg.kSR_SPLT_CL;
742
void Process(btSoftBody* ps,btRigidBody* 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());
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);
763
struct CollideCL_SS : ClusterBase
765
btSoftBody* bodies[2];
766
void Process(const btDbvtNode* la,const btDbvtNode* lb)
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))
777
btSoftBody::CJoint joint;
778
if(SolveContact(res,cla,clb,joint))
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;
787
void Process(btSoftBody* psa,btSoftBody* psb)
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);
794
btDbvt::collideTT(psa->m_cdbvt.m_root,psb->m_cdbvt.m_root,*this);
800
struct CollideSDF_RS : btDbvt::ICollide
802
void Process(const btDbvtNode* leaf)
804
btSoftBody::Node* node=(btSoftBody::Node*)leaf->data;
807
void DoNode(btSoftBody::Node& n) const
809
const btScalar m=n.m_im>0?dynmargin:stamargin;
810
btSoftBody::RContact c;
812
psb->checkContact(prb,n.m_x,m,c.m_cti))
814
const btScalar ima=n.m_im;
815
const btScalar imb=prb->getInvMass();
816
const btScalar ms=ima+imb;
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();
829
c.m_c0 = ImpulseMatrix(psb->m_sst.sdt,ima,imb,iwi,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);
847
struct CollideVF_SS : btDbvt::ICollide
849
void Process(const btDbvtNode* lnode,
850
const btDbvtNode* lface)
852
btSoftBody::Node* node=(btSoftBody::Node*)lnode->data;
853
btSoftBody::Face* face=(btSoftBody::Face*)lface->data;
854
btVector3 o=node->m_x;
856
btScalar d=SIMD_INFINITY;
857
ProjectOrigin( face->m_n[0]->m_x-o,
861
const btScalar m=mrg+(o-node->m_q).length()*2;
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)||
874
const btScalar ms=ma+mb;
877
btSoftBody::SContact c;
878
c.m_normal = p/-btSqrt(d);
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);
895
#endif //_BT_SOFT_BODY_INTERNALS_H
added
removed
extern/bullet2/src/BulletSoftBody/btSoftBodyInternals.h
