Bullet Collision Detection & Physics Library
btSoftBodyInternals.h
Go to the documentation of this file.
1/*
2Bullet Continuous Collision Detection and Physics Library
3Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
4
5This software is provided 'as-is', without any express or implied warranty.
6In no event will the authors be held liable for any damages arising from the use of this software.
7Permission is granted to anyone to use this software for any purpose,
8including commercial applications, and to alter it and redistribute it freely,
9subject to the following restrictions:
10
111. 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.
122. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
133. This notice may not be removed or altered from any source distribution.
14*/
16
17#ifndef _BT_SOFT_BODY_INTERNALS_H
18#define _BT_SOFT_BODY_INTERNALS_H
19
20#include "btSoftBody.h"
21
22
29#include <string.h> //for memset
30//
31// btSymMatrix
32//
33template <typename T>
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)]); }
43 int dim;
44};
45
46//
47// btSoftBodyCollisionShape
48//
50{
51public:
53
55 {
57 m_body=backptr;
58 }
59
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
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 {
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 {
106 btAssert(0);
107 }
108 virtual const char* getName()const
109 {
110 return "SoftBody";
111 }
112
113};
114
115//
116// btSoftClusterCollisionShape
117//
119{
120public:
122
124
125
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 }
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 {
161 }
162 virtual btScalar getMargin() const
163 {
165 }
166};
167
168//
169// Inline's
170//
171
172//
173template <typename T>
174static inline void ZeroInitialize(T& value)
175{
176 memset(&value,0,sizeof(T));
177}
178//
179template <typename T>
180static inline bool CompLess(const T& a,const T& b)
181{ return(a<b); }
182//
183template <typename T>
184static inline bool CompGreater(const T& a,const T& b)
185{ return(a>b); }
186//
187template <typename T>
188static inline T Lerp(const T& a,const T& b,btScalar t)
189{ return(a+(b-a)*t); }
190//
191template <typename T>
192static inline T InvLerp(const T& a,const T& b,btScalar t)
193{ return((b+a*t-b*t)/(a*b)); }
194//
195static 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//
206static 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//
215template <typename T>
216static inline T Clamp(const T& x,const T& l,const T& h)
217{ return(x<l?l:x>h?h:x); }
218//
219template <typename T>
220static inline T Sq(const T& x)
221{ return(x*x); }
222//
223template <typename T>
224static inline T Cube(const T& x)
225{ return(x*x*x); }
226//
227template <typename T>
228static inline T Sign(const T& x)
229{ return((T)(x<0?-1:+1)); }
230//
231template <typename T>
232static inline bool SameSign(const T& x,const T& y)
233{ return((x*y)>0); }
234//
235static 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//
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//
256static 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//
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//
274static 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//
282static 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//
290static 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//
298static 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//
305static 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//
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//
322static 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//
330 const btMatrix3x3& iib)
331{
332 return(Add(iia,iib).inverse());
333}
334
335//
336static inline btVector3 ProjectOnAxis( const btVector3& v,
337 const btVector3& a)
338{
339 return(a*btDot(v,a));
340}
341//
342static inline btVector3 ProjectOnPlane( const btVector3& v,
343 const btVector3& a)
344{
345 return(v-ProjectOnAxis(v,a));
346}
347
348//
349static 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//
369static 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//
403template <typename T>
404static 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//
412static 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//
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
454inline static void EvaluateMedium( const btSoftBodyWorldInfo* wfi,
455 const btVector3& x,
456 btSoftBody::sMedium& medium)
457{
458 medium.m_velocity = btVector3(0,0,0);
459 medium.m_pressure = 0;
460 medium.m_density = wfi->air_density;
461 if(wfi->water_density>0)
462 {
463 const btScalar depth=-(btDot(x,wfi->water_normal)+wfi->water_offset);
464 if(depth>0)
465 {
466 medium.m_density = wfi->water_density;
467 medium.m_pressure = depth*wfi->water_density*wfi->m_gravity.length();
468 }
469 }
470}
471
472
473//
474static inline btVector3 NormalizeAny(const btVector3& v)
475{
476 const btScalar l=v.length();
477 if(l>SIMD_EPSILON)
478 return(v/l);
479 else
480 return(btVector3(0,0,0));
481}
482
483//
484static inline btDbvtVolume VolumeOf( const btSoftBody::Face& f,
485 btScalar margin)
486{
487 const btVector3* pts[]={ &f.m_n[0]->m_x,
488 &f.m_n[1]->m_x,
489 &f.m_n[2]->m_x};
491 vol.Expand(btVector3(margin,margin,margin));
492 return(vol);
493}
494
495//
496static inline btVector3 CenterOf( const btSoftBody::Face& f)
497{
498 return((f.m_n[0]->m_x+f.m_n[1]->m_x+f.m_n[2]->m_x)/3);
499}
500
501//
502static inline btScalar AreaOf( const btVector3& x0,
503 const btVector3& x1,
504 const btVector3& x2)
505{
506 const btVector3 a=x1-x0;
507 const btVector3 b=x2-x0;
508 const btVector3 cr=btCross(a,b);
509 const btScalar area=cr.length();
510 return(area);
511}
512
513//
514static inline btScalar VolumeOf( const btVector3& x0,
515 const btVector3& x1,
516 const btVector3& x2,
517 const btVector3& x3)
518{
519 const btVector3 a=x1-x0;
520 const btVector3 b=x2-x0;
521 const btVector3 c=x3-x0;
522 return(btDot(a,btCross(b,c)));
523}
524
525//
526
527
528//
529static inline void ApplyClampedForce( btSoftBody::Node& n,
530 const btVector3& f,
531 btScalar dt)
532{
533 const btScalar dtim=dt*n.m_im;
534 if((f*dtim).length2()>n.m_v.length2())
535 {/* Clamp */
536 n.m_f-=ProjectOnAxis(n.m_v,f.normalized())/dtim;
537 }
538 else
539 {/* Apply */
540 n.m_f+=f;
541 }
542}
543
544//
545static inline int MatchEdge( const btSoftBody::Node* a,
546 const btSoftBody::Node* b,
547 const btSoftBody::Node* ma,
548 const btSoftBody::Node* mb)
549{
550 if((a==ma)&&(b==mb)) return(0);
551 if((a==mb)&&(b==ma)) return(1);
552 return(-1);
553}
554
555//
556// btEigen : Extract eigen system,
557// straitforward implementation of http://math.fullerton.edu/mathews/n2003/JacobiMethodMod.html
558// outputs are NOT sorted.
559//
561{
562 static int system(btMatrix3x3& a,btMatrix3x3* vectors,btVector3* values=0)
563 {
564 static const int maxiterations=16;
565 static const btScalar accuracy=(btScalar)0.0001;
566 btMatrix3x3& v=*vectors;
567 int iterations=0;
568 vectors->setIdentity();
569 do {
570 int p=0,q=1;
571 if(btFabs(a[p][q])<btFabs(a[0][2])) { p=0;q=2; }
572 if(btFabs(a[p][q])<btFabs(a[1][2])) { p=1;q=2; }
573 if(btFabs(a[p][q])>accuracy)
574 {
575 const btScalar w=(a[q][q]-a[p][p])/(2*a[p][q]);
576 const btScalar z=btFabs(w);
577 const btScalar t=w/(z*(btSqrt(1+w*w)+z));
578 if(t==t)/* [WARNING] let hope that one does not get thrown aways by some compilers... */
579 {
580 const btScalar c=1/btSqrt(t*t+1);
581 const btScalar s=c*t;
582 mulPQ(a,c,s,p,q);
583 mulTPQ(a,c,s,p,q);
584 mulPQ(v,c,s,p,q);
585 } else break;
586 } else break;
587 } while((++iterations)<maxiterations);
588 if(values)
589 {
590 *values=btVector3(a[0][0],a[1][1],a[2][2]);
591 }
592 return(iterations);
593 }
594private:
595 static inline void mulTPQ(btMatrix3x3& a,btScalar c,btScalar s,int p,int q)
596 {
597 const btScalar m[2][3]={ {a[p][0],a[p][1],a[p][2]},
598 {a[q][0],a[q][1],a[q][2]}};
599 int i;
600
601 for(i=0;i<3;++i) a[p][i]=c*m[0][i]-s*m[1][i];
602 for(i=0;i<3;++i) a[q][i]=c*m[1][i]+s*m[0][i];
603 }
604 static inline void mulPQ(btMatrix3x3& a,btScalar c,btScalar s,int p,int q)
605 {
606 const btScalar m[2][3]={ {a[0][p],a[1][p],a[2][p]},
607 {a[0][q],a[1][q],a[2][q]}};
608 int i;
609
610 for(i=0;i<3;++i) a[i][p]=c*m[0][i]-s*m[1][i];
611 for(i=0;i<3;++i) a[i][q]=c*m[1][i]+s*m[0][i];
612 }
613};
614
615//
616// Polar decomposition,
617// "Computing the Polar Decomposition with Applications", Nicholas J. Higham, 1986.
618//
619static inline int PolarDecompose( const btMatrix3x3& m,btMatrix3x3& q,btMatrix3x3& s)
620{
621 static const btPolarDecomposition polar;
622 return polar.decompose(m, q, s);
623}
624
625//
626// btSoftColliders
627//
629{
630 //
631 // ClusterBase
632 //
634 {
641 {
642 erp =(btScalar)1;
643 idt =0;
644 m_margin =0;
645 friction =0;
647 }
650 btSoftBody::CJoint& joint)
651 {
652 if(res.distance<m_margin)
653 {
654 btVector3 norm = res.normal;
655 norm.normalize();//is it necessary?
656
657 const btVector3 ra=res.witnesses[0]-ba.xform().getOrigin();
658 const btVector3 rb=res.witnesses[1]-bb.xform().getOrigin();
659 const btVector3 va=ba.velocity(ra);
660 const btVector3 vb=bb.velocity(rb);
661 const btVector3 vrel=va-vb;
662 const btScalar rvac=btDot(vrel,norm);
663 btScalar depth=res.distance-m_margin;
664
665// printf("depth=%f\n",depth);
666 const btVector3 iv=norm*rvac;
667 const btVector3 fv=vrel-iv;
668 joint.m_bodies[0] = ba;
669 joint.m_bodies[1] = bb;
670 joint.m_refs[0] = ra*ba.xform().getBasis();
671 joint.m_refs[1] = rb*bb.xform().getBasis();
672 joint.m_rpos[0] = ra;
673 joint.m_rpos[1] = rb;
674 joint.m_cfm = 1;
675 joint.m_erp = 1;
676 joint.m_life = 0;
677 joint.m_maxlife = 0;
678 joint.m_split = 1;
679
680 joint.m_drift = depth*norm;
681
682 joint.m_normal = norm;
683// printf("normal=%f,%f,%f\n",res.normal.getX(),res.normal.getY(),res.normal.getZ());
684 joint.m_delete = false;
685 joint.m_friction = fv.length2()<(rvac*friction*rvac*friction)?1:friction;
686 joint.m_massmatrix = ImpulseMatrix( ba.invMass(),ba.invWorldInertia(),joint.m_rpos[0],
687 bb.invMass(),bb.invWorldInertia(),joint.m_rpos[1]);
688
689 return(true);
690 }
691 return(false);
692 }
693 };
694 //
695 // CollideCL_RS
696 //
698 {
701
702 void Process(const btDbvtNode* leaf)
703 {
705 btSoftClusterCollisionShape cshape(cluster);
706
708
711 return;
712
716 btVector3(1,0,0),res))
717 {
718 btSoftBody::CJoint joint;
719 if(SolveContact(res,cluster,m_colObjWrap->getCollisionObject(),joint))//prb,joint))
720 {
722 *pj=joint;psb->m_joints.push_back(pj);
724 {
725 pj->m_erp *= psb->m_cfg.kSKHR_CL;
727 }
728 else
729 {
730 pj->m_erp *= psb->m_cfg.kSRHR_CL;
732 }
733 }
734 }
735 }
737 {
738 psb = ps;
739 m_colObjWrap = colObWrap;
740 idt = ps->m_sst.isdt;
744 btVector3 mins;
745 btVector3 maxs;
746
748 colObWrap->getCollisionShape()->getAabb(colObWrap->getWorldTransform(),mins,maxs);
749 volume=btDbvtVolume::FromMM(mins,maxs);
750 volume.Expand(btVector3(1,1,1)*m_margin);
751 ps->m_cdbvt.collideTV(ps->m_cdbvt.m_root,volume,*this);
752 }
753 };
754 //
755 // CollideCL_SS
756 //
758 {
760 void Process(const btDbvtNode* la,const btDbvtNode* lb)
761 {
764
765
766 bool connected=false;
767 if ((bodies[0]==bodies[1])&&(bodies[0]->m_clusterConnectivity.size()))
768 {
769 connected = bodies[0]->m_clusterConnectivity[cla->m_clusterIndex+bodies[0]->m_clusters.size()*clb->m_clusterIndex];
770 }
771
772 if (!connected)
773 {
779 cla->m_com-clb->m_com,res))
780 {
781 btSoftBody::CJoint joint;
782 if(SolveContact(res,cla,clb,joint))
783 {
785 *pj=joint;bodies[0]->m_joints.push_back(pj);
786 pj->m_erp *= btMax(bodies[0]->m_cfg.kSSHR_CL,bodies[1]->m_cfg.kSSHR_CL);
788 }
789 }
790 } else
791 {
792 static int count=0;
793 count++;
794 //printf("count=%d\n",count);
795
796 }
797 }
799 {
800 idt = psa->m_sst.isdt;
801 //m_margin = (psa->getCollisionShape()->getMargin()+psb->getCollisionShape()->getMargin())/2;
803 friction = btMin(psa->m_cfg.kDF,psb->m_cfg.kDF);
804 bodies[0] = psa;
805 bodies[1] = psb;
806 psa->m_cdbvt.collideTT(psa->m_cdbvt.m_root,psb->m_cdbvt.m_root,*this);
807 }
808 };
809 //
810 // CollideSDF_RS
811 //
813 {
814 void Process(const btDbvtNode* leaf)
815 {
817 DoNode(*node);
818 }
820 {
821 const btScalar m=n.m_im>0?dynmargin:stamargin;
823
824 if( (!n.m_battach)&&
826 {
827 const btScalar ima=n.m_im;
828 const btScalar imb= m_rigidBody? m_rigidBody->getInvMass() : 0.f;
829 const btScalar ms=ima+imb;
830 if(ms>0)
831 {
833 static const btMatrix3x3 iwiStatic(0,0,0,0,0,0,0,0,0);
835 const btVector3 ra=n.m_x-wtr.getOrigin();
837 const btVector3 vb=n.m_x-n.m_q;
838 const btVector3 vr=vb-va;
839 const btScalar dn=btDot(vr,c.m_cti.m_normal);
840 const btVector3 fv=vr-c.m_cti.m_normal*dn;
842 c.m_node = &n;
843 c.m_c0 = ImpulseMatrix(psb->m_sst.sdt,ima,imb,iwi,ra);
844 c.m_c1 = ra;
845 c.m_c2 = ima*psb->m_sst.sdt;
846 c.m_c3 = fv.length2()<(dn*fc*dn*fc)?0:1-fc;
849 if (m_rigidBody)
851 }
852 }
853 }
859 };
860 //
861 // CollideVF_SS
862 //
864 {
865 void Process(const btDbvtNode* lnode,
866 const btDbvtNode* lface)
867 {
870 btVector3 o=node->m_x;
871 btVector3 p;
873 ProjectOrigin( face->m_n[0]->m_x-o,
874 face->m_n[1]->m_x-o,
875 face->m_n[2]->m_x-o,
876 p,d);
877 const btScalar m=mrg+(o-node->m_q).length()*2;
878 if(d<(m*m))
879 {
880 const btSoftBody::Node* n[]={face->m_n[0],face->m_n[1],face->m_n[2]};
881 const btVector3 w=BaryCoord(n[0]->m_x,n[1]->m_x,n[2]->m_x,p+o);
882 const btScalar ma=node->m_im;
883 btScalar mb=BaryEval(n[0]->m_im,n[1]->m_im,n[2]->m_im,w);
884 if( (n[0]->m_im<=0)||
885 (n[1]->m_im<=0)||
886 (n[2]->m_im<=0))
887 {
888 mb=0;
889 }
890 const btScalar ms=ma+mb;
891 if(ms>0)
892 {
894 c.m_normal = p/-btSqrt(d);
895 c.m_margin = m;
896 c.m_node = node;
897 c.m_face = face;
898 c.m_weights = w;
899 c.m_friction = btMax(psb[0]->m_cfg.kDF,psb[1]->m_cfg.kDF);
900 c.m_cfm[0] = ma/ms*psb[0]->m_cfg.kSHR;
901 c.m_cfm[1] = mb/ms*psb[1]->m_cfg.kSHR;
903 }
904 }
905 }
908 };
909};
910
911#endif //_BT_SOFT_BODY_INTERNALS_H
#define btAlignedAlloc(size, alignment)
@ SOFTBODY_SHAPE_PROXYTYPE
const T & btMax(const T &a, const T &b)
Definition: btMinMax.h:29
const T & btMin(const T &a, const T &b)
Definition: btMinMax.h:23
btQuaternion inverse(const btQuaternion &q)
Return the inverse of a quaternion.
Definition: btQuaternion.h:900
btScalar length(const btQuaternion &q)
Return the length of a quaternion.
Definition: btQuaternion.h:886
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
Definition: btScalar.h:292
#define ATTRIBUTE_ALIGNED16(a)
Definition: btScalar.h:82
btScalar btSqrt(btScalar y)
Definition: btScalar.h:444
btScalar btFabs(btScalar x)
Definition: btScalar.h:475
#define SIMD_INFINITY
Definition: btScalar.h:522
#define SIMD_EPSILON
Definition: btScalar.h:521
void btSwap(T &a, T &b)
Definition: btScalar.h:621
#define btAssert(x)
Definition: btScalar.h:131
static btVector3 ProjectOnAxis(const btVector3 &v, const btVector3 &a)
static bool CompGreater(const T &a, const T &b)
static btMatrix3x3 Mul(const btMatrix3x3 &a, btScalar b)
static btScalar ImplicitSolve(btSoftBody::ImplicitFn *fn, const btVector3 &a, const btVector3 &b, const btScalar accuracy, const int maxiterations=256)
static void Orthogonalize(btMatrix3x3 &m)
static btMatrix3x3 Cross(const btVector3 &v)
static btMatrix3x3 MassMatrix(btScalar im, const btMatrix3x3 &iwi, const btVector3 &r)
static btDbvtVolume VolumeOf(const btSoftBody::Face &f, btScalar margin)
static T Lerp(const T &a, const T &b, btScalar t)
static btMatrix3x3 AngularImpulseMatrix(const btMatrix3x3 &iia, const btMatrix3x3 &iib)
static btScalar AreaOf(const btVector3 &x0, const btVector3 &x1, const btVector3 &x2)
static T Sq(const T &x)
static btVector3 NormalizeAny(const btVector3 &v)
static int PolarDecompose(const btMatrix3x3 &m, btMatrix3x3 &q, btMatrix3x3 &s)
static int MatchEdge(const btSoftBody::Node *a, const btSoftBody::Node *b, const btSoftBody::Node *ma, const btSoftBody::Node *mb)
static bool CompLess(const T &a, const T &b)
static T BaryEval(const T &a, const T &b, const T &c, const btVector3 &coord)
static btVector3 CenterOf(const btSoftBody::Face &f)
static bool SameSign(const T &x, const T &y)
static void EvaluateMedium(const btSoftBodyWorldInfo *wfi, const btVector3 &x, btSoftBody::sMedium &medium)
static btVector3 BaryCoord(const btVector3 &a, const btVector3 &b, const btVector3 &c, const btVector3 &p)
static btMatrix3x3 ScaleAlongAxis(const btVector3 &a, btScalar s)
static void ZeroInitialize(T &value)
static btMatrix3x3 Add(const btMatrix3x3 &a, const btMatrix3x3 &b)
static T InvLerp(const T &a, const T &b, btScalar t)
static btVector3 ProjectOnPlane(const btVector3 &v, const btVector3 &a)
static btScalar ClusterMetric(const btVector3 &x, const btVector3 &y)
static btMatrix3x3 ImpulseMatrix(btScalar dt, btScalar ima, btScalar imb, const btMatrix3x3 &iwi, const btVector3 &r)
static void ApplyClampedForce(btSoftBody::Node &n, const btVector3 &f, btScalar dt)
static void ProjectOrigin(const btVector3 &a, const btVector3 &b, btVector3 &prj, btScalar &sqd)
static btMatrix3x3 Sub(const btMatrix3x3 &a, const btMatrix3x3 &b)
static T Sign(const T &x)
static btVector3 Clamp(const btVector3 &v, btScalar maxlength)
static btMatrix3x3 Diagonal(btScalar x)
static T Cube(const T &x)
btScalar btDot(const btVector3 &v1, const btVector3 &v2)
Return the dot product between two vectors.
Definition: btVector3.h:901
btVector3 btCross(const btVector3 &v1, const btVector3 &v2)
Return the cross product of two vectors.
Definition: btVector3.h:931
The btAlignedObjectArray template class uses a subset of the stl::vector interface for its methods It...
int size() const
return the number of elements in the array
void push_back(const T &_Val)
bool isStaticOrKinematicObject() const
btTransform & getWorldTransform()
void activate(bool forceActivation=false) const
btScalar getFriction() const
const btCollisionShape * getCollisionShape() const
virtual btScalar getMargin() const =0
virtual void getAabb(const btTransform &t, btVector3 &aabbMin, btVector3 &aabbMax) const =0
getAabb returns the axis aligned bounding box in the coordinate frame of the given transform t.
The btConcaveShape class provides an interface for non-moving (static) concave shapes.
The btConvexInternalShape is an internal base class, shared by most convex shape implementations.
virtual void setMargin(btScalar margin)
virtual btScalar getMargin() const
The btConvexShape is an abstract shape interface, implemented by all convex shapes such as btBoxShape...
Definition: btConvexShape.h:32
The btMatrix3x3 class implements a 3x3 rotation matrix, to perform linear algebra in combination with...
Definition: btMatrix3x3.h:48
void setIdentity()
Set the matrix to the identity.
Definition: btMatrix3x3.h:317
This class is used to compute the polar decomposition of a matrix.
unsigned int decompose(const btMatrix3x3 &a, btMatrix3x3 &u, btMatrix3x3 &h) const
Decomposes a matrix into orthogonal and symmetric, positive-definite parts.
The btRigidBody is the main class for rigid body objects.
Definition: btRigidBody.h:63
btVector3 getVelocityInLocalPoint(const btVector3 &rel_pos) const
Definition: btRigidBody.h:382
btScalar getInvMass() const
Definition: btRigidBody.h:273
const btMatrix3x3 & getInvInertiaTensorWorld() const
Definition: btRigidBody.h:274
virtual void calculateLocalInertia(btScalar, btVector3 &) const
void processAllTriangles(btTriangleCallback *, const btVector3 &, const btVector3 &) const
virtual void setLocalScaling(const btVector3 &)
virtual const char * getName() const
virtual const btVector3 & getLocalScaling() const
btSoftBodyCollisionShape(btSoftBody *backptr)
virtual void getAabb(const btTransform &t, btVector3 &aabbMin, btVector3 &aabbMax) const
getAabb returns the axis aligned bounding box in the coordinate frame of the given transform t.
The btSoftBody is an class to simulate cloth and volumetric soft bodies.
Definition: btSoftBody.h:72
btDbvt m_cdbvt
Definition: btSoftBody.h:673
tJointArray m_joints
Definition: btSoftBody.h:666
btAlignedObjectArray< bool > m_clusterConnectivity
Definition: btSoftBody.h:676
SolverState m_sst
Definition: btSoftBody.h:654
Config m_cfg
Definition: btSoftBody.h:653
tRContactArray m_rcontacts
Definition: btSoftBody.h:664
btVector3 m_bounds[2]
Definition: btSoftBody.h:669
tClusterArray m_clusters
Definition: btSoftBody.h:674
tSContactArray m_scontacts
Definition: btSoftBody.h:665
bool checkContact(const btCollisionObjectWrapper *colObjWrap, const btVector3 &x, btScalar margin, btSoftBody::sCti &cti) const
virtual void setMargin(btScalar margin)
virtual void getAabb(const btTransform &t, btVector3 &aabbMin, btVector3 &aabbMax) const
getAabb's default implementation is brute force, expected derived classes to implement a fast dedicat...
btSoftClusterCollisionShape(const btSoftBody::Cluster *cluster)
virtual int getShapeType() const
virtual void calculateLocalInertia(btScalar mass, btVector3 &inertia) const
virtual btVector3 localGetSupportingVertexWithoutMargin(const btVector3 &vec) const
virtual btScalar getMargin() const
const btSoftBody::Cluster * m_cluster
virtual btVector3 localGetSupportingVertex(const btVector3 &vec) const
virtual void batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3 *vectors, btVector3 *supportVerticesOut, int numVectors) const
virtual const char * getName() const
The btTransform class supports rigid transforms with only translation and rotation and no scaling/she...
Definition: btTransform.h:34
btMatrix3x3 & getBasis()
Return the basis matrix for the rotation.
Definition: btTransform.h:112
static const btTransform & getIdentity()
Return an identity transform.
Definition: btTransform.h:203
btVector3 & getOrigin()
Return the origin vector translation.
Definition: btTransform.h:117
The btTriangleCallback provides a callback for each overlapping triangle when calling processAllTrian...
btVector3 can be used to represent 3D points and vectors.
Definition: btVector3.h:84
void setMax(const btVector3 &other)
Set each element to the max of the current values and the values of another btVector3.
Definition: btVector3.h:621
const btScalar & z() const
Return the z value.
Definition: btVector3.h:591
btScalar length() const
Return the length of the vector.
Definition: btVector3.h:263
btVector3 normalized() const
Return a normalized version of this vector.
Definition: btVector3.h:964
btScalar length2() const
Return the length of the vector squared.
Definition: btVector3.h:257
const btScalar & x() const
Return the x value.
Definition: btVector3.h:587
void setMin(const btVector3 &other)
Set each element to the min of the current values and the values of another btVector3.
Definition: btVector3.h:638
btVector3 & normalize()
Normalize this vector x^2 + y^2 + z^2 = 1.
Definition: btVector3.h:309
const btScalar & y() const
Return the y value.
Definition: btVector3.h:589
const btCollisionShape * getCollisionShape() const
const btCollisionObject * getCollisionObject() const
const btTransform & getWorldTransform() const
static btDbvtAabbMm FromMM(const btVector3 &mi, const btVector3 &mx)
Definition: btDbvt.h:425
static btDbvtAabbMm FromPoints(const btVector3 *pts, int n)
Definition: btDbvt.h:433
DBVT_INLINE void Expand(const btVector3 &e)
Definition: btDbvt.h:459
void * data
Definition: btDbvt.h:187
DBVT_PREFIX void collideTV(const btDbvtNode *root, const btDbvtVolume &volume, DBVT_IPOLICY) const
Definition: btDbvt.h:935
btDbvtNode * m_root
Definition: btDbvt.h:262
DBVT_PREFIX void collideTT(const btDbvtNode *root0, const btDbvtNode *root1, DBVT_IPOLICY)
Definition: btDbvt.h:738
static void mulPQ(btMatrix3x3 &a, btScalar c, btScalar s, int p, int q)
static int system(btMatrix3x3 &a, btMatrix3x3 *vectors, btVector3 *values=0)
static void mulTPQ(btMatrix3x3 &a, btScalar c, btScalar s, int p, int q)
btVector3 witnesses[2]
Definition: btGjkEpa2.h:42
static btScalar SignedDistance(const btVector3 &position, btScalar margin, const btConvexShape *shape, const btTransform &wtrs, sResults &results)
Definition: btGjkEpa2.cpp:966
btScalar air_density
Definition: btSoftBody.h:45
btScalar water_density
Definition: btSoftBody.h:46
btVector3 m_gravity
Definition: btSoftBody.h:52
btVector3 water_normal
Definition: btSoftBody.h:49
btScalar water_offset
Definition: btSoftBody.h:47
btScalar invMass() const
Definition: btSoftBody.h:407
const btMatrix3x3 & invWorldInertia() const
Definition: btSoftBody.h:400
btVector3 velocity(const btVector3 &rpos) const
Definition: btSoftBody.h:438
const btTransform & xform() const
Definition: btSoftBody.h:413
btVector3 m_rpos[2]
Definition: btSoftBody.h:558
btVector3 m_normal
Definition: btSoftBody.h:559
btScalar m_friction
Definition: btSoftBody.h:560
btAlignedObjectArray< Node * > m_nodes
Definition: btSoftBody.h:325
btScalar kSK_SPLT_CL
Definition: btSoftBody.h:586
btScalar kSS_SPLT_CL
Definition: btSoftBody.h:587
btScalar kSR_SPLT_CL
Definition: btSoftBody.h:585
Node * m_n[3]
Definition: btSoftBody.h:251
virtual btScalar Eval(const btVector3 &x)=0
btVector3 m_drift
Definition: btSoftBody.h:506
btScalar m_split
Definition: btSoftBody.h:505
btMatrix3x3 m_massmatrix
Definition: btSoftBody.h:508
btVector3 m_refs[2]
Definition: btSoftBody.h:502
btVector3 m_x
Definition: btSoftBody.h:224
btVector3 m_v
Definition: btSoftBody.h:226
btVector3 m_q
Definition: btSoftBody.h:225
btVector3 m_n
Definition: btSoftBody.h:228
btVector3 m_f
Definition: btSoftBody.h:227
btMatrix3x3 m_c0
Definition: btSoftBody.h:271
btScalar m_cfm[2]
Definition: btSoftBody.h:286
btVector3 m_normal
Definition: btSoftBody.h:189
btVector3 m_velocity
Definition: btSoftBody.h:196
bool SolveContact(const btGjkEpaSolver2::sResults &res, btSoftBody::Body ba, const btSoftBody::Body bb, btSoftBody::CJoint &joint)
void Process(const btDbvtNode *leaf)
void ProcessColObj(btSoftBody *ps, const btCollisionObjectWrapper *colObWrap)
const btCollisionObjectWrapper * m_colObjWrap
void Process(const btDbvtNode *la, const btDbvtNode *lb)
void ProcessSoftSoft(btSoftBody *psa, btSoftBody *psb)
void DoNode(btSoftBody::Node &n) const
void Process(const btDbvtNode *leaf)
const btCollisionObjectWrapper * m_colObj1Wrap
void Process(const btDbvtNode *lnode, const btDbvtNode *lface)
btSoftBody implementation by Nathanael Presson
int index(int c, int r) const
T & operator()(int c, int r)
btAlignedObjectArray< T > store
void resize(int n, const T &init=T())
btSymMatrix(int n, const T &init=T())
const T & operator()(int c, int r) const