Bullet Collision Detection & Physics Library
btSoftBodyInternals.h
Go to the documentation of this file.
1 /*
2 Bullet Continuous Collision Detection and Physics Library
3 Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
4 
5 This software is provided 'as-is', without any express or implied warranty.
6 In no event will the authors be held liable for any damages arising from the use of this software.
7 Permission is granted to anyone to use this software for any purpose,
8 including commercial applications, and to alter it and redistribute it freely,
9 subject to the following restrictions:
10 
11 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
12 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
13 3. This notice may not be removed or altered from any source distribution.
14 */
16 
17 #ifndef _BT_SOFT_BODY_INTERNALS_H
18 #define _BT_SOFT_BODY_INTERNALS_H
19 
20 #include "btSoftBody.h"
21 
22 
23 #include "LinearMath/btQuickprof.h"
29 #include <string.h> //for memset
30 //
31 // btSymMatrix
32 //
33 template <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 {
51 public:
53 
55  {
56  m_shapeType = SOFTBODY_SHAPE_PROXYTYPE;
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 {
120 public:
122 
123  btSoftClusterCollisionShape (const btSoftBody::Cluster* cluster) : m_cluster(cluster) { setMargin(0); }
124 
125 
126  virtual btVector3 localGetSupportingVertex(const btVector3& vec) const
127  {
128  btSoftBody::Node* const * n=&m_cluster->m_nodes[0];
129  btScalar d=btDot(vec,n[0]->m_x);
130  int j=0;
131  for(int i=1,ni=m_cluster->m_nodes.size();i<ni;++i)
132  {
133  const btScalar k=btDot(vec,n[i]->m_x);
134  if(k>d) { d=k;j=i; }
135  }
136  return(n[j]->m_x);
137  }
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 //
173 template <typename T>
174 static inline void ZeroInitialize(T& value)
175 {
176  memset(&value,0,sizeof(T));
177 }
178 //
179 template <typename T>
180 static inline bool CompLess(const T& a,const T& b)
181 { return(a<b); }
182 //
183 template <typename T>
184 static inline bool CompGreater(const T& a,const T& b)
185 { return(a>b); }
186 //
187 template <typename T>
188 static inline T Lerp(const T& a,const T& b,btScalar t)
189 { return(a+(b-a)*t); }
190 //
191 template <typename T>
192 static inline T InvLerp(const T& a,const T& b,btScalar t)
193 { return((b+a*t-b*t)/(a*b)); }
194 //
195 static inline btMatrix3x3 Lerp( const btMatrix3x3& a,
196  const btMatrix3x3& b,
197  btScalar t)
198 {
199  btMatrix3x3 r;
200  r[0]=Lerp(a[0],b[0],t);
201  r[1]=Lerp(a[1],b[1],t);
202  r[2]=Lerp(a[2],b[2],t);
203  return(r);
204 }
205 //
206 static inline btVector3 Clamp(const btVector3& v,btScalar maxlength)
207 {
208  const btScalar sql=v.length2();
209  if(sql>(maxlength*maxlength))
210  return((v*maxlength)/btSqrt(sql));
211  else
212  return(v);
213 }
214 //
215 template <typename T>
216 static inline T Clamp(const T& x,const T& l,const T& h)
217 { return(x<l?l:x>h?h:x); }
218 //
219 template <typename T>
220 static inline T Sq(const T& x)
221 { return(x*x); }
222 //
223 template <typename T>
224 static inline T Cube(const T& x)
225 { return(x*x*x); }
226 //
227 template <typename T>
228 static inline T Sign(const T& x)
229 { return((T)(x<0?-1:+1)); }
230 //
231 template <typename T>
232 static inline bool SameSign(const T& x,const T& y)
233 { return((x*y)>0); }
234 //
235 static inline btScalar ClusterMetric(const btVector3& x,const btVector3& y)
236 {
237  const btVector3 d=x-y;
238  return(btFabs(d[0])+btFabs(d[1])+btFabs(d[2]));
239 }
240 //
241 static inline btMatrix3x3 ScaleAlongAxis(const btVector3& a,btScalar s)
242 {
243  const btScalar xx=a.x()*a.x();
244  const btScalar yy=a.y()*a.y();
245  const btScalar zz=a.z()*a.z();
246  const btScalar xy=a.x()*a.y();
247  const btScalar yz=a.y()*a.z();
248  const btScalar zx=a.z()*a.x();
249  btMatrix3x3 m;
250  m[0]=btVector3(1-xx+xx*s,xy*s-xy,zx*s-zx);
251  m[1]=btVector3(xy*s-xy,1-yy+yy*s,yz*s-yz);
252  m[2]=btVector3(zx*s-zx,yz*s-yz,1-zz+zz*s);
253  return(m);
254 }
255 //
256 static inline btMatrix3x3 Cross(const btVector3& v)
257 {
258  btMatrix3x3 m;
259  m[0]=btVector3(0,-v.z(),+v.y());
260  m[1]=btVector3(+v.z(),0,-v.x());
261  m[2]=btVector3(-v.y(),+v.x(),0);
262  return(m);
263 }
264 //
265 static inline btMatrix3x3 Diagonal(btScalar x)
266 {
267  btMatrix3x3 m;
268  m[0]=btVector3(x,0,0);
269  m[1]=btVector3(0,x,0);
270  m[2]=btVector3(0,0,x);
271  return(m);
272 }
273 //
274 static inline btMatrix3x3 Add(const btMatrix3x3& a,
275  const btMatrix3x3& b)
276 {
277  btMatrix3x3 r;
278  for(int i=0;i<3;++i) r[i]=a[i]+b[i];
279  return(r);
280 }
281 //
282 static inline btMatrix3x3 Sub(const btMatrix3x3& a,
283  const btMatrix3x3& b)
284 {
285  btMatrix3x3 r;
286  for(int i=0;i<3;++i) r[i]=a[i]-b[i];
287  return(r);
288 }
289 //
290 static inline btMatrix3x3 Mul(const btMatrix3x3& a,
291  btScalar b)
292 {
293  btMatrix3x3 r;
294  for(int i=0;i<3;++i) r[i]=a[i]*b;
295  return(r);
296 }
297 //
298 static inline void Orthogonalize(btMatrix3x3& m)
299 {
300  m[2]=btCross(m[0],m[1]).normalized();
301  m[1]=btCross(m[2],m[0]).normalized();
302  m[0]=btCross(m[1],m[2]).normalized();
303 }
304 //
305 static inline btMatrix3x3 MassMatrix(btScalar im,const btMatrix3x3& iwi,const btVector3& r)
306 {
307  const btMatrix3x3 cr=Cross(r);
308  return(Sub(Diagonal(im),cr*iwi*cr));
309 }
310 
311 //
313  btScalar ima,
314  btScalar imb,
315  const btMatrix3x3& iwi,
316  const btVector3& r)
317 {
318  return(Diagonal(1/dt)*Add(Diagonal(ima),MassMatrix(imb,iwi,r)).inverse());
319 }
320 
321 //
322 static inline btMatrix3x3 ImpulseMatrix( btScalar ima,const btMatrix3x3& iia,const btVector3& ra,
323  btScalar imb,const btMatrix3x3& iib,const btVector3& rb)
324 {
325  return(Add(MassMatrix(ima,iia,ra),MassMatrix(imb,iib,rb)).inverse());
326 }
327 
328 //
329 static inline btMatrix3x3 AngularImpulseMatrix( const btMatrix3x3& iia,
330  const btMatrix3x3& iib)
331 {
332  return(Add(iia,iib).inverse());
333 }
334 
335 //
336 static inline btVector3 ProjectOnAxis( const btVector3& v,
337  const btVector3& a)
338 {
339  return(a*btDot(v,a));
340 }
341 //
342 static inline btVector3 ProjectOnPlane( const btVector3& v,
343  const btVector3& a)
344 {
345  return(v-ProjectOnAxis(v,a));
346 }
347 
348 //
349 static inline void ProjectOrigin( const btVector3& a,
350  const btVector3& b,
351  btVector3& prj,
352  btScalar& sqd)
353 {
354  const btVector3 d=b-a;
355  const btScalar m2=d.length2();
356  if(m2>SIMD_EPSILON)
357  {
358  const btScalar t=Clamp<btScalar>(-btDot(a,d)/m2,0,1);
359  const btVector3 p=a+d*t;
360  const btScalar l2=p.length2();
361  if(l2<sqd)
362  {
363  prj=p;
364  sqd=l2;
365  }
366  }
367 }
368 //
369 static inline void ProjectOrigin( const btVector3& a,
370  const btVector3& b,
371  const btVector3& c,
372  btVector3& prj,
373  btScalar& sqd)
374 {
375  const btVector3& q=btCross(b-a,c-a);
376  const btScalar m2=q.length2();
377  if(m2>SIMD_EPSILON)
378  {
379  const btVector3 n=q/btSqrt(m2);
380  const btScalar k=btDot(a,n);
381  const btScalar k2=k*k;
382  if(k2<sqd)
383  {
384  const btVector3 p=n*k;
385  if( (btDot(btCross(a-p,b-p),q)>0)&&
386  (btDot(btCross(b-p,c-p),q)>0)&&
387  (btDot(btCross(c-p,a-p),q)>0))
388  {
389  prj=p;
390  sqd=k2;
391  }
392  else
393  {
394  ProjectOrigin(a,b,prj,sqd);
395  ProjectOrigin(b,c,prj,sqd);
396  ProjectOrigin(c,a,prj,sqd);
397  }
398  }
399  }
400 }
401 
402 //
403 template <typename T>
404 static inline T BaryEval( const T& a,
405  const T& b,
406  const T& c,
407  const btVector3& coord)
408 {
409  return(a*coord.x()+b*coord.y()+c*coord.z());
410 }
411 //
412 static inline btVector3 BaryCoord( const btVector3& a,
413  const btVector3& b,
414  const btVector3& c,
415  const btVector3& p)
416 {
417  const btScalar w[]={ btCross(a-p,b-p).length(),
418  btCross(b-p,c-p).length(),
419  btCross(c-p,a-p).length()};
420  const btScalar isum=1/(w[0]+w[1]+w[2]);
421  return(btVector3(w[1]*isum,w[2]*isum,w[0]*isum));
422 }
423 
424 //
426  const btVector3& a,
427  const btVector3& b,
428  const btScalar accuracy,
429  const int maxiterations=256)
430 {
431  btScalar span[2]={0,1};
432  btScalar values[2]={fn->Eval(a),fn->Eval(b)};
433  if(values[0]>values[1])
434  {
435  btSwap(span[0],span[1]);
436  btSwap(values[0],values[1]);
437  }
438  if(values[0]>-accuracy) return(-1);
439  if(values[1]<+accuracy) return(-1);
440  for(int i=0;i<maxiterations;++i)
441  {
442  const btScalar t=Lerp(span[0],span[1],values[0]/(values[0]-values[1]));
443  const btScalar v=fn->Eval(Lerp(a,b,t));
444  if((t<=0)||(t>=1)) break;
445  if(btFabs(v)<accuracy) return(t);
446  if(v<0)
447  { span[0]=t;values[0]=v; }
448  else
449  { span[1]=t;values[1]=v; }
450  }
451  return(-1);
452 }
453 
454 inline 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 //
474 static 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 //
484 static 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 //
496 static 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 //
502 static 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 //
514 static 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 //
529 static 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 //
545 static 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 //
560 struct btEigen
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  }
594 private:
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 //
619 static 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;
646  threshold =(btScalar)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 
707  const btConvexShape* rshape=(const btConvexShape*)m_colObjWrap->getCollisionShape();
708 
710  if(m_colObjWrap->getCollisionObject()->isStaticOrKinematicObject() && cluster->m_containsAnchor)
711  return;
712 
715  rshape,m_colObjWrap->getWorldTransform(),
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);
723  if(m_colObjWrap->getCollisionObject()->isStaticOrKinematicObject())
724  {
725  pj->m_erp *= psb->m_cfg.kSKHR_CL;
726  pj->m_split *= psb->m_cfg.kSK_SPLT_CL;
727  }
728  else
729  {
730  pj->m_erp *= psb->m_cfg.kSRHR_CL;
731  pj->m_split *= psb->m_cfg.kSR_SPLT_CL;
732  }
733  }
734  }
735  }
737  {
738  psb = ps;
739  m_colObjWrap = colObWrap;
740  idt = ps->m_sst.isdt;
741  m_margin = m_colObjWrap->getCollisionShape()->getMargin()+psb->getCollisionShape()->getMargin();
743  friction = btMin(psb->m_cfg.kDF,m_colObjWrap->getCollisionObject()->getFriction());
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  {
759  btSoftBody* bodies[2];
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);
787  pj->m_split *= (bodies[0]->m_cfg.kSS_SPLT_CL+bodies[1]->m_cfg.kSS_SPLT_CL)/2;
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;
802  m_margin = (psa->getCollisionShape()->getMargin()+psb->getCollisionShape()->getMargin());
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  {
816  btSoftBody::Node* node=(btSoftBody::Node*)leaf->data;
817  DoNode(*node);
818  }
819  void DoNode(btSoftBody::Node& n) const
820  {
821  const btScalar m=n.m_im>0?dynmargin:stamargin;
823 
824  if( (!n.m_battach)&&
825  psb->checkContact(m_colObj1Wrap,n.m_x,m,c.m_cti))
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  {
832  const btTransform& wtr=m_rigidBody?m_rigidBody->getWorldTransform() : m_colObj1Wrap->getCollisionObject()->getWorldTransform();
833  static const btMatrix3x3 iwiStatic(0,0,0,0,0,0,0,0,0);
834  const btMatrix3x3& iwi=m_rigidBody?m_rigidBody->getInvInertiaTensorWorld() : iwiStatic;
835  const btVector3 ra=n.m_x-wtr.getOrigin();
836  const btVector3 va=m_rigidBody ? m_rigidBody->getVelocityInLocalPoint(ra)*psb->m_sst.sdt : btVector3(0,0,0);
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;
841  const btScalar fc=psb->m_cfg.kDF*m_colObj1Wrap->getCollisionObject()->getFriction();
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;
847  c.m_c4 = m_colObj1Wrap->getCollisionObject()->isStaticOrKinematicObject()?psb->m_cfg.kKHR:psb->m_cfg.kCHR;
848  psb->m_rcontacts.push_back(c);
849  if (m_rigidBody)
850  m_rigidBody->activate();
851  }
852  }
853  }
859  };
860  //
861  // CollideVF_SS
862  //
864  {
865  void Process(const btDbvtNode* lnode,
866  const btDbvtNode* lface)
867  {
868  btSoftBody::Node* node=(btSoftBody::Node*)lnode->data;
869  btSoftBody::Face* face=(btSoftBody::Face*)lface->data;
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;
902  psb[0]->m_scontacts.push_back(c);
903  }
904  }
905  }
906  btSoftBody* psb[2];
908  };
909 };
910 
911 #endif //_BT_SOFT_BODY_INTERNALS_H
static void Orthogonalize(btMatrix3x3 &m)
#define SIMD_EPSILON
Definition: btScalar.h:521
static T Sq(const T &x)
btScalar length(const btQuaternion &q)
Return the length of a quaternion.
Definition: btQuaternion.h:906
btScalar kSS_SPLT_CL
Definition: btSoftBody.h:587
void push_back(const T &_Val)
Config m_cfg
Definition: btSoftBody.h:653
btMatrix3x3 m_c0
Definition: btSoftBody.h:271
virtual btVector3 localGetSupportingVertexWithoutMargin(const btVector3 &vec) const
static void ZeroInitialize(T &value)
static btVector3 CenterOf(const btSoftBody::Face &f)
btVector3 m_normal
Definition: btSoftBody.h:189
btScalar water_offset
Definition: btSoftBody.h:47
btVector3 m_rpos[2]
Definition: btSoftBody.h:558
btSoftBody implementation by Nathanael Presson
static btMatrix3x3 ImpulseMatrix(btScalar dt, btScalar ima, btScalar imb, const btMatrix3x3 &iwi, const btVector3 &r)
The btAlignedObjectArray template class uses a subset of the stl::vector interface for its methods It...
btScalar kSK_SPLT_CL
Definition: btSoftBody.h:586
void * data
Definition: btDbvt.h:187
btSymMatrix(int n, const T &init=T())
The btConvexInternalShape is an internal base class, shared by most convex shape implementations.
const btCollisionObjectWrapper * m_colObj1Wrap
T & operator()(int c, int r)
tJointArray m_joints
Definition: btSoftBody.h:666
btAlignedObjectArray< bool > m_clusterConnectivity
Definition: btSoftBody.h:676
static btDbvtAabbMm FromPoints(const btVector3 *pts, int n)
Definition: btDbvt.h:433
virtual const btVector3 & getLocalScaling() const
btScalar m_split
Definition: btSoftBody.h:505
btScalar btSqrt(btScalar y)
Definition: btScalar.h:444
#define btAssert(x)
Definition: btScalar.h:131
btAlignedObjectArray< Node * > m_nodes
Definition: btSoftBody.h:325
bool SolveContact(const btGjkEpaSolver2::sResults &res, btSoftBody::Body ba, const btSoftBody::Body bb, btSoftBody::CJoint &joint)
static T Sign(const T &x)
static btMatrix3x3 Mul(const btMatrix3x3 &a, btScalar b)
const btMatrix3x3 & invWorldInertia() const
Definition: btSoftBody.h:400
static btScalar AreaOf(const btVector3 &x0, const btVector3 &x1, const btVector3 &x2)
void DoNode(btSoftBody::Node &n) const
btDbvtNode * m_root
Definition: btDbvt.h:262
static bool CompGreater(const T &a, const T &b)
btVector3 water_normal
Definition: btSoftBody.h:49
static void ProjectOrigin(const btVector3 &a, const btVector3 &b, btVector3 &prj, btScalar &sqd)
static bool SameSign(const T &x, const T &y)
static T InvLerp(const T &a, const T &b, btScalar t)
const btTransform & xform() const
Definition: btSoftBody.h:413
static int PolarDecompose(const btMatrix3x3 &m, btMatrix3x3 &q, btMatrix3x3 &s)
static void EvaluateMedium(const btSoftBodyWorldInfo *wfi, const btVector3 &x, btSoftBody::sMedium &medium)
void ProcessColObj(btSoftBody *ps, const btCollisionObjectWrapper *colObWrap)
btQuaternion inverse(const btQuaternion &q)
Return the inverse of a quaternion.
Definition: btQuaternion.h:920
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...
static btMatrix3x3 Diagonal(btScalar x)
virtual btScalar getMargin() const
Node * m_n[3]
Definition: btSoftBody.h:251
btVector3 & normalize()
Normalize this vector x^2 + y^2 + z^2 = 1.
Definition: btVector3.h:309
const btScalar & x() const
Return the x value.
Definition: btVector3.h:587
The btConvexShape is an abstract shape interface, implemented by all convex shapes such as btBoxShape...
Definition: btConvexShape.h:31
static btDbvtVolume VolumeOf(const btSoftBody::Face &f, btScalar margin)
static btVector3 ProjectOnPlane(const btVector3 &v, const btVector3 &a)
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...
btVector3 btCross(const btVector3 &v1, const btVector3 &v2)
Return the cross product of two vectors.
Definition: btVector3.h:933
void Process(const btDbvtNode *lnode, const btDbvtNode *lface)
#define SIMD_INFINITY
Definition: btScalar.h:522
btScalar kSR_SPLT_CL
Definition: btSoftBody.h:585
virtual void setMargin(btScalar margin)
void processAllTriangles(btTriangleCallback *, const btVector3 &, const btVector3 &) const
virtual void setLocalScaling(const btVector3 &)
int size() const
return the number of elements in the array
btVector3 & getOrigin()
Return the origin vector translation.
Definition: btTransform.h:117
static void ApplyClampedForce(btSoftBody::Node &n, const btVector3 &f, btScalar dt)
btVector3 m_normal
Definition: btSoftBody.h:559
btSoftBodyCollisionShape(btSoftBody *backptr)
btVector3 m_velocity
Definition: btSoftBody.h:196
static btVector3 BaryCoord(const btVector3 &a, const btVector3 &b, const btVector3 &c, const btVector3 &p)
The btTriangleCallback provides a callback for each overlapping triangle when calling processAllTrian...
void btSwap(T &a, T &b)
Definition: btScalar.h:621
static btDbvtAabbMm FromMM(const btVector3 &mi, const btVector3 &mx)
Definition: btDbvt.h:425
int index(int c, int r) const
static btScalar ClusterMetric(const btVector3 &x, const btVector3 &y)
bool isStaticOrKinematicObject() const
virtual void calculateLocalInertia(btScalar, btVector3 &) const
const btTransform & getWorldTransform() const
btMatrix3x3 & getBasis()
Return the basis matrix for the rotation.
Definition: btTransform.h:112
static btVector3 Clamp(const btVector3 &v, btScalar maxlength)
The btRigidBody is the main class for rigid body objects.
Definition: btRigidBody.h:62
btScalar length() const
Return the length of the vector.
Definition: btVector3.h:263
static btMatrix3x3 Cross(const btVector3 &v)
DBVT_INLINE void Expand(const btVector3 &e)
Definition: btDbvt.h:459
static int MatchEdge(const btSoftBody::Node *a, const btSoftBody::Node *b, const btSoftBody::Node *ma, const btSoftBody::Node *mb)
btScalar invMass() const
Definition: btSoftBody.h:407
virtual btVector3 localGetSupportingVertex(const btVector3 &vec) const
btScalar m_cfm[2]
Definition: btSoftBody.h:286
static T BaryEval(const T &a, const T &b, const T &c, const btVector3 &coord)
virtual void batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3 *vectors, btVector3 *supportVerticesOut, int numVectors) const
virtual btScalar getMargin() const
const btScalar & y() const
Return the y value.
Definition: btVector3.h:589
static bool CompLess(const T &a, const T &b)
static btMatrix3x3 ScaleAlongAxis(const btVector3 &a, btScalar s)
const btCollisionShape * getCollisionShape() const
static btMatrix3x3 AngularImpulseMatrix(const btMatrix3x3 &iia, const btMatrix3x3 &iib)
btVector3 can be used to represent 3D points and vectors.
Definition: btVector3.h:83
#define ATTRIBUTE_ALIGNED16(a)
Definition: btScalar.h:82
const btSoftBody::Cluster * m_cluster
const T & operator()(int c, int r) const
btScalar length2() const
Return the length of the vector squared.
Definition: btVector3.h:257
virtual btScalar Eval(const btVector3 &x)=0
virtual void calculateLocalInertia(btScalar mass, btVector3 &inertia) const
The btTransform class supports rigid transforms with only translation and rotation and no scaling/she...
Definition: btTransform.h:34
static T Cube(const T &x)
btVector3 m_refs[2]
Definition: btSoftBody.h:502
static btMatrix3x3 MassMatrix(btScalar im, const btMatrix3x3 &iwi, const btVector3 &r)
static btScalar ImplicitSolve(btSoftBody::ImplicitFn *fn, const btVector3 &a, const btVector3 &b, const btScalar accuracy, const int maxiterations=256)
const btCollisionObjectWrapper * m_colObjWrap
btDbvt m_cdbvt
Definition: btSoftBody.h:673
btVector3 normalized() const
Return a normalized version of this vector.
Definition: btVector3.h:966
static void mulPQ(btMatrix3x3 &a, btScalar c, btScalar s, int p, int q)
virtual const char * getName() const
The btConcaveShape class provides an interface for non-moving (static) concave shapes.
void resize(int n, const T &init=T())
static const btTransform & getIdentity()
Return an identity transform.
Definition: btTransform.h:203
btVector3 witnesses[2]
Definition: btGjkEpa2.h:42
static T Lerp(const T &a, const T &b, btScalar t)
btVector3 m_n
Definition: btSoftBody.h:228
virtual const char * getName() const
void ProcessSoftSoft(btSoftBody *psa, btSoftBody *psb)
static void mulTPQ(btMatrix3x3 &a, btScalar c, btScalar s, int p, int q)
SolverState m_sst
Definition: btSoftBody.h:654
btAlignedObjectArray< T > store
btScalar getFriction() const
tClusterArray m_clusters
Definition: btSoftBody.h:674
virtual btScalar getMargin() const =0
void Process(const btDbvtNode *leaf)
const T & btMax(const T &a, const T &b)
Definition: btMinMax.h:29
static btScalar SignedDistance(const btVector3 &position, btScalar margin, const btConvexShape *shape, const btTransform &wtrs, sResults &results)
Definition: btGjkEpa2.cpp:966
static btMatrix3x3 Sub(const btMatrix3x3 &a, const btMatrix3x3 &b)
void Process(const btDbvtNode *leaf)
btMatrix3x3 m_massmatrix
Definition: btSoftBody.h:508
btScalar water_density
Definition: btSoftBody.h:46
btVector3 m_bounds[2]
Definition: btSoftBody.h:669
btVector3 m_v
Definition: btSoftBody.h:226
#define btAlignedAlloc(size, alignment)
The btMatrix3x3 class implements a 3x3 rotation matrix, to perform linear algebra in combination with...
Definition: btMatrix3x3.h:48
static btVector3 NormalizeAny(const btVector3 &v)
static btMatrix3x3 Add(const btMatrix3x3 &a, const btMatrix3x3 &b)
static btVector3 ProjectOnAxis(const btVector3 &v, const btVector3 &a)
DBVT_PREFIX void collideTT(const btDbvtNode *root0, const btDbvtNode *root1, DBVT_IPOLICY)
Definition: btDbvt.h:738
btScalar air_density
Definition: btSoftBody.h:45
virtual void getAabb(const btTransform &t, btVector3 &aabbMin, btVector3 &aabbMax) const
getAabb&#39;s default implementation is brute force, expected derived classes to implement a fast dedicat...
btSoftClusterCollisionShape(const btSoftBody::Cluster *cluster)
btScalar btDot(const btVector3 &v1, const btVector3 &v2)
Return the dot product between two vectors.
Definition: btVector3.h:903
btVector3 m_q
Definition: btSoftBody.h:225
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
btVector3 m_f
Definition: btSoftBody.h:227
btVector3 m_gravity
Definition: btSoftBody.h:52
void Process(const btDbvtNode *la, const btDbvtNode *lb)
virtual void setMargin(btScalar margin)
This class is used to compute the polar decomposition of a matrix.
The btSoftBody is an class to simulate cloth and volumetric soft bodies.
Definition: btSoftBody.h:71
const T & btMin(const T &a, const T &b)
Definition: btMinMax.h:23
const btCollisionShape * getCollisionShape() const
btVector3 m_drift
Definition: btSoftBody.h:506
DBVT_PREFIX void collideTV(const btDbvtNode *root, const btDbvtVolume &volume, DBVT_IPOLICY) const
Definition: btDbvt.h:935
btVector3 m_x
Definition: btSoftBody.h:224
unsigned int decompose(const btMatrix3x3 &a, btMatrix3x3 &u, btMatrix3x3 &h) const
Decomposes a matrix into orthogonal and symmetric, positive-definite parts.
void setIdentity()
Set the matrix to the identity.
Definition: btMatrix3x3.h:317
btVector3 velocity(const btVector3 &rpos) const
Definition: btSoftBody.h:438
static int system(btMatrix3x3 &a, btMatrix3x3 *vectors, btVector3 *values=0)
virtual int getShapeType() const
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
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
Definition: btScalar.h:292
btScalar m_friction
Definition: btSoftBody.h:560
const btCollisionObject * getCollisionObject() const
btScalar btFabs(btScalar x)
Definition: btScalar.h:475
const btScalar & z() const
Return the z value.
Definition: btVector3.h:591