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"
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
30 //
31 // btSymMatrix
32 //
33 template <typename T>
34 struct btSymMatrix
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)]); }
42  btAlignedObjectArray<T> store;
43  int dim;
44 };
45 
46 //
47 // btSoftBodyCollisionShape
48 //
49 class btSoftBodyCollisionShape : public btConcaveShape
50 {
51 public:
52  btSoftBody* m_body;
53 
54  btSoftBodyCollisionShape(btSoftBody* backptr)
55  {
56  m_shapeType = SOFTBODY_SHAPE_PROXYTYPE;
57  m_body=backptr;
58  }
59 
60  virtual ~btSoftBodyCollisionShape()
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 //
118 class btSoftClusterCollisionShape : public btConvexInternalShape
119 {
120 public:
121  const btSoftBody::Cluster* m_cluster;
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  }
138  virtual btVector3 localGetSupportingVertexWithoutMargin(const btVector3& vec)const
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  {
160  btConvexInternalShape::setMargin(margin);
161  }
162  virtual btScalar getMargin() const
163  {
164  return getMargin();
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 //
312 static inline btMatrix3x3 ImpulseMatrix( btScalar dt,
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 //
425 static btScalar ImplicitSolve( btSoftBody::ImplicitFn* fn,
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 //
455 static inline btVector3 NormalizeAny(const btVector3& v)
456 {
457  const btScalar l=v.length();
458  if(l>SIMD_EPSILON)
459  return(v/l);
460  else
461  return(btVector3(0,0,0));
462 }
463 
464 //
465 static inline btDbvtVolume VolumeOf( const btSoftBody::Face& f,
466  btScalar margin)
467 {
468  const btVector3* pts[]={ &f.m_n[0]->m_x,
469  &f.m_n[1]->m_x,
470  &f.m_n[2]->m_x};
471  btDbvtVolume vol=btDbvtVolume::FromPoints(pts,3);
472  vol.Expand(btVector3(margin,margin,margin));
473  return(vol);
474 }
475 
476 //
477 static inline btVector3 CenterOf( const btSoftBody::Face& f)
478 {
479  return((f.m_n[0]->m_x+f.m_n[1]->m_x+f.m_n[2]->m_x)/3);
480 }
481 
482 //
483 static inline btScalar AreaOf( const btVector3& x0,
484  const btVector3& x1,
485  const btVector3& x2)
486 {
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();
491  return(area);
492 }
493 
494 //
495 static inline btScalar VolumeOf( const btVector3& x0,
496  const btVector3& x1,
497  const btVector3& x2,
498  const btVector3& x3)
499 {
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)));
504 }
505 
506 //
507 static void EvaluateMedium( const btSoftBodyWorldInfo* wfi,
508  const btVector3& x,
509  btSoftBody::sMedium& medium)
510 {
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)
515  {
516  const btScalar depth=-(btDot(x,wfi->water_normal)+wfi->water_offset);
517  if(depth>0)
518  {
519  medium.m_density = wfi->water_density;
520  medium.m_pressure = depth*wfi->water_density*wfi->m_gravity.length();
521  }
522  }
523 }
524 
525 //
526 static inline void ApplyClampedForce( btSoftBody::Node& n,
527  const btVector3& f,
528  btScalar dt)
529 {
530  const btScalar dtim=dt*n.m_im;
531  if((f*dtim).length2()>n.m_v.length2())
532  {/* Clamp */
533  n.m_f-=ProjectOnAxis(n.m_v,f.normalized())/dtim;
534  }
535  else
536  {/* Apply */
537  n.m_f+=f;
538  }
539 }
540 
541 //
542 static inline int MatchEdge( const btSoftBody::Node* a,
543  const btSoftBody::Node* b,
544  const btSoftBody::Node* ma,
545  const btSoftBody::Node* mb)
546 {
547  if((a==ma)&&(b==mb)) return(0);
548  if((a==mb)&&(b==ma)) return(1);
549  return(-1);
550 }
551 
552 //
553 // btEigen : Extract eigen system,
554 // straitforward implementation of http://math.fullerton.edu/mathews/n2003/JacobiMethodMod.html
555 // outputs are NOT sorted.
556 //
557 struct btEigen
558 {
559  static int system(btMatrix3x3& a,btMatrix3x3* vectors,btVector3* values=0)
560  {
561  static const int maxiterations=16;
562  static const btScalar accuracy=(btScalar)0.0001;
563  btMatrix3x3& v=*vectors;
564  int iterations=0;
565  vectors->setIdentity();
566  do {
567  int p=0,q=1;
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)
571  {
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... */
576  {
577  const btScalar c=1/btSqrt(t*t+1);
578  const btScalar s=c*t;
579  mulPQ(a,c,s,p,q);
580  mulTPQ(a,c,s,p,q);
581  mulPQ(v,c,s,p,q);
582  } else break;
583  } else break;
584  } while((++iterations)<maxiterations);
585  if(values)
586  {
587  *values=btVector3(a[0][0],a[1][1],a[2][2]);
588  }
589  return(iterations);
590  }
591 private:
592  static inline void mulTPQ(btMatrix3x3& a,btScalar c,btScalar s,int p,int q)
593  {
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]}};
596  int i;
597 
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];
600  }
601  static inline void mulPQ(btMatrix3x3& a,btScalar c,btScalar s,int p,int q)
602  {
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]}};
605  int i;
606 
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];
609  }
610 };
611 
612 //
613 // Polar decomposition,
614 // "Computing the Polar Decomposition with Applications", Nicholas J. Higham, 1986.
615 //
616 static inline int PolarDecompose( const btMatrix3x3& m,btMatrix3x3& q,btMatrix3x3& s)
617 {
618  static const btPolarDecomposition polar;
619  return polar.decompose(m, q, s);
620 }
621 
622 //
623 // btSoftColliders
624 //
625 struct btSoftColliders
626 {
627  //
628  // ClusterBase
629  //
630  struct ClusterBase : btDbvt::ICollide
631  {
632  btScalar erp;
633  btScalar idt;
634  btScalar m_margin;
635  btScalar friction;
636  btScalar threshold;
637  ClusterBase()
638  {
639  erp =(btScalar)1;
640  idt =0;
641  m_margin =0;
642  friction =0;
643  threshold =(btScalar)0;
644  }
645  bool SolveContact( const btGjkEpaSolver2::sResults& res,
646  btSoftBody::Body ba,const btSoftBody::Body bb,
647  btSoftBody::CJoint& joint)
648  {
649  if(res.distance<m_margin)
650  {
651  btVector3 norm = res.normal;
652  norm.normalize();//is it necessary?
653 
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;
661 
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;
671  joint.m_cfm = 1;
672  joint.m_erp = 1;
673  joint.m_life = 0;
674  joint.m_maxlife = 0;
675  joint.m_split = 1;
676 
677  joint.m_drift = depth*norm;
678 
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]);
685 
686  return(true);
687  }
688  return(false);
689  }
690  };
691  //
692  // CollideCL_RS
693  //
694  struct CollideCL_RS : ClusterBase
695  {
696  btSoftBody* psb;
697  const btCollisionObjectWrapper* m_colObjWrap;
698 
699  void Process(const btDbvtNode* leaf)
700  {
701  btSoftBody::Cluster* cluster=(btSoftBody::Cluster*)leaf->data;
702  btSoftClusterCollisionShape cshape(cluster);
703 
704  const btConvexShape* rshape=(const btConvexShape*)m_colObjWrap->getCollisionShape();
705 
707  if(m_colObjWrap->getCollisionObject()->isStaticOrKinematicObject() && cluster->m_containsAnchor)
708  return;
709 
710  btGjkEpaSolver2::sResults res;
711  if(btGjkEpaSolver2::SignedDistance( &cshape,btTransform::getIdentity(),
712  rshape,m_colObjWrap->getWorldTransform(),
713  btVector3(1,0,0),res))
714  {
715  btSoftBody::CJoint joint;
716  if(SolveContact(res,cluster,m_colObjWrap->getCollisionObject(),joint))//prb,joint))
717  {
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())
721  {
722  pj->m_erp *= psb->m_cfg.kSKHR_CL;
723  pj->m_split *= psb->m_cfg.kSK_SPLT_CL;
724  }
725  else
726  {
727  pj->m_erp *= psb->m_cfg.kSRHR_CL;
728  pj->m_split *= psb->m_cfg.kSR_SPLT_CL;
729  }
730  }
731  }
732  }
733  void ProcessColObj(btSoftBody* ps,const btCollisionObjectWrapper* colObWrap)
734  {
735  psb = ps;
736  m_colObjWrap = colObWrap;
737  idt = ps->m_sst.isdt;
738  m_margin = m_colObjWrap->getCollisionShape()->getMargin()+psb->getCollisionShape()->getMargin();
740  friction = btMin(psb->m_cfg.kDF,m_colObjWrap->getCollisionObject()->getFriction());
741  btVector3 mins;
742  btVector3 maxs;
743 
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);
749  }
750  };
751  //
752  // CollideCL_SS
753  //
754  struct CollideCL_SS : ClusterBase
755  {
756  btSoftBody* bodies[2];
757  void Process(const btDbvtNode* la,const btDbvtNode* lb)
758  {
759  btSoftBody::Cluster* cla=(btSoftBody::Cluster*)la->data;
760  btSoftBody::Cluster* clb=(btSoftBody::Cluster*)lb->data;
761 
762 
763  bool connected=false;
764  if ((bodies[0]==bodies[1])&&(bodies[0]->m_clusterConnectivity.size()))
765  {
766  connected = bodies[0]->m_clusterConnectivity[cla->m_clusterIndex+bodies[0]->m_clusters.size()*clb->m_clusterIndex];
767  }
768 
769  if (!connected)
770  {
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))
777  {
778  btSoftBody::CJoint joint;
779  if(SolveContact(res,cla,clb,joint))
780  {
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;
785  }
786  }
787  } else
788  {
789  static int count=0;
790  count++;
791  //printf("count=%d\n",count);
792 
793  }
794  }
795  void ProcessSoftSoft(btSoftBody* psa,btSoftBody* psb)
796  {
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);
801  bodies[0] = psa;
802  bodies[1] = psb;
803  psa->m_cdbvt.collideTT(psa->m_cdbvt.m_root,psb->m_cdbvt.m_root,*this);
804  }
805  };
806  //
807  // CollideSDF_RS
808  //
809  struct CollideSDF_RS : btDbvt::ICollide
810  {
811  void Process(const btDbvtNode* leaf)
812  {
813  btSoftBody::Node* node=(btSoftBody::Node*)leaf->data;
814  DoNode(*node);
815  }
816  void DoNode(btSoftBody::Node& n) const
817  {
818  const btScalar m=n.m_im>0?dynmargin:stamargin;
819  btSoftBody::RContact c;
820 
821  if( (!n.m_battach)&&
822  psb->checkContact(m_colObj1Wrap,n.m_x,m,c.m_cti))
823  {
824  const btScalar ima=n.m_im;
825  const btScalar imb= m_rigidBody? m_rigidBody->getInvMass() : 0.f;
826  const btScalar ms=ima+imb;
827  if(ms>0)
828  {
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();
839  c.m_node = &n;
840  c.m_c0 = ImpulseMatrix(psb->m_sst.sdt,ima,imb,iwi,ra);
841  c.m_c1 = 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);
846  if (m_rigidBody)
847  m_rigidBody->activate();
848  }
849  }
850  }
851  btSoftBody* psb;
852  const btCollisionObjectWrapper* m_colObj1Wrap;
853  btRigidBody* m_rigidBody;
854  btScalar dynmargin;
855  btScalar stamargin;
856  };
857  //
858  // CollideVF_SS
859  //
860  struct CollideVF_SS : btDbvt::ICollide
861  {
862  void Process(const btDbvtNode* lnode,
863  const btDbvtNode* lface)
864  {
865  btSoftBody::Node* node=(btSoftBody::Node*)lnode->data;
866  btSoftBody::Face* face=(btSoftBody::Face*)lface->data;
867  btVector3 o=node->m_x;
868  btVector3 p;
869  btScalar d=SIMD_INFINITY;
870  ProjectOrigin( face->m_n[0]->m_x-o,
871  face->m_n[1]->m_x-o,
872  face->m_n[2]->m_x-o,
873  p,d);
874  const btScalar m=mrg+(o-node->m_q).length()*2;
875  if(d<(m*m))
876  {
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)||
882  (n[1]->m_im<=0)||
883  (n[2]->m_im<=0))
884  {
885  mb=0;
886  }
887  const btScalar ms=ma+mb;
888  if(ms>0)
889  {
890  btSoftBody::SContact c;
891  c.m_normal = p/-btSqrt(d);
892  c.m_margin = m;
893  c.m_node = node;
894  c.m_face = face;
895  c.m_weights = w;
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);
900  }
901  }
902  }
903  btSoftBody* psb[2];
904  btScalar mrg;
905  };
906 };
907 
908 #endif //_BT_SOFT_BODY_INTERNALS_H
Generated on Thu Oct 3 2013 18:25:07 for Bullet Collision Detection & Physics Library by   doxygen 1.8.4