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 */
15 ///btSoftBody implementation by Nathanael Presson
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 {
btSymMatrixbtSymMatrix36 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); }
indexbtSymMatrix39 int index(int c,int r) const { if(c>r) btSwap(c,r);btAssert(r<dim);return((r*(r+1))/2+c); }
operatorbtSymMatrix40 T& operator()(int c,int r) { return(store[index(c,r)]); }
operatorbtSymMatrix41 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
btSoftBodyCollisionShape(btSoftBody * backptr)54 btSoftBodyCollisionShape(btSoftBody* backptr)
55 {
56 m_shapeType = SOFTBODY_SHAPE_PROXYTYPE;
57 m_body=backptr;
58 }
59
~btSoftBodyCollisionShape()60 virtual ~btSoftBodyCollisionShape()
61 {
62
63 }
64
processAllTriangles(btTriangleCallback *,const btVector3 &,const btVector3 &)65 void processAllTriangles(btTriangleCallback* /*callback*/,const btVector3& /*aabbMin*/,const btVector3& /*aabbMax*/) const
66 {
67 //not yet
68 btAssert(0);
69 }
70
71 ///getAabb returns the axis aligned bounding box in the coordinate frame of the given transform t.
getAabb(const btTransform & t,btVector3 & aabbMin,btVector3 & aabbMax)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
setLocalScaling(const btVector3 &)94 virtual void setLocalScaling(const btVector3& /*scaling*/)
95 {
96 ///na
97 }
getLocalScaling()98 virtual const btVector3& getLocalScaling() const
99 {
100 static const btVector3 dummy(1,1,1);
101 return dummy;
102 }
calculateLocalInertia(btScalar,btVector3 &)103 virtual void calculateLocalInertia(btScalar /*mass*/,btVector3& /*inertia*/) const
104 {
105 ///not yet
106 btAssert(0);
107 }
getName()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
btSoftClusterCollisionShape(const btSoftBody::Cluster * cluster)123 btSoftClusterCollisionShape (const btSoftBody::Cluster* cluster) : m_cluster(cluster) { setMargin(0); }
124
125
localGetSupportingVertex(const btVector3 & vec)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 }
localGetSupportingVertexWithoutMargin(const btVector3 & vec)138 virtual btVector3 localGetSupportingVertexWithoutMargin(const btVector3& vec)const
139 {
140 return(localGetSupportingVertex(vec));
141 }
142 //notice that the vectors should be unit length
batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3 * vectors,btVector3 * supportVerticesOut,int numVectors)143 virtual void batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const
144 {}
145
146
calculateLocalInertia(btScalar mass,btVector3 & inertia)147 virtual void calculateLocalInertia(btScalar mass,btVector3& inertia) const
148 {}
149
getAabb(const btTransform & t,btVector3 & aabbMin,btVector3 & aabbMax)150 virtual void getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const
151 {}
152
getShapeType()153 virtual int getShapeType() const { return SOFTBODY_SHAPE_PROXYTYPE; }
154
155 //debugging
getName()156 virtual const char* getName()const {return "SOFTCLUSTER";}
157
setMargin(btScalar margin)158 virtual void setMargin(btScalar margin)
159 {
160 btConvexInternalShape::setMargin(margin);
161 }
getMargin()162 virtual btScalar getMargin() const
163 {
164 return getMargin();
165 }
166 };
167
168 //
169 // Inline's
170 //
171
172 //
173 template <typename T>
ZeroInitialize(T & value)174 static inline void ZeroInitialize(T& value)
175 {
176 memset(&value,0,sizeof(T));
177 }
178 //
179 template <typename T>
CompLess(const T & a,const T & b)180 static inline bool CompLess(const T& a,const T& b)
181 { return(a<b); }
182 //
183 template <typename T>
CompGreater(const T & a,const T & b)184 static inline bool CompGreater(const T& a,const T& b)
185 { return(a>b); }
186 //
187 template <typename T>
Lerp(const T & a,const T & b,btScalar 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>
InvLerp(const T & a,const T & b,btScalar t)192 static inline T InvLerp(const T& a,const T& b,btScalar t)
193 { return((b+a*t-b*t)/(a*b)); }
194 //
Lerp(const btMatrix3x3 & a,const btMatrix3x3 & b,btScalar t)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 //
Clamp(const btVector3 & v,btScalar maxlength)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>
Clamp(const T & x,const T & l,const T & h)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>
Sq(const T & x)220 static inline T Sq(const T& x)
221 { return(x*x); }
222 //
223 template <typename T>
Cube(const T & x)224 static inline T Cube(const T& x)
225 { return(x*x*x); }
226 //
227 template <typename T>
Sign(const T & x)228 static inline T Sign(const T& x)
229 { return((T)(x<0?-1:+1)); }
230 //
231 template <typename T>
SameSign(const T & x,const T & y)232 static inline bool SameSign(const T& x,const T& y)
233 { return((x*y)>0); }
234 //
ClusterMetric(const btVector3 & x,const btVector3 & y)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 //
ScaleAlongAxis(const btVector3 & a,btScalar s)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 //
Cross(const btVector3 & v)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 //
Diagonal(btScalar x)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 //
Add(const btMatrix3x3 & a,const btMatrix3x3 & b)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 //
Sub(const btMatrix3x3 & a,const btMatrix3x3 & b)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 //
Mul(const btMatrix3x3 & a,btScalar b)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 //
Orthogonalize(btMatrix3x3 & m)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 //
MassMatrix(btScalar im,const btMatrix3x3 & iwi,const btVector3 & r)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 //
ImpulseMatrix(btScalar dt,btScalar ima,btScalar imb,const btMatrix3x3 & iwi,const btVector3 & r)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 //
ImpulseMatrix(btScalar ima,const btMatrix3x3 & iia,const btVector3 & ra,btScalar imb,const btMatrix3x3 & iib,const btVector3 & rb)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 //
AngularImpulseMatrix(const btMatrix3x3 & iia,const btMatrix3x3 & iib)329 static inline btMatrix3x3 AngularImpulseMatrix( const btMatrix3x3& iia,
330 const btMatrix3x3& iib)
331 {
332 return(Add(iia,iib).inverse());
333 }
334
335 //
ProjectOnAxis(const btVector3 & v,const btVector3 & a)336 static inline btVector3 ProjectOnAxis( const btVector3& v,
337 const btVector3& a)
338 {
339 return(a*btDot(v,a));
340 }
341 //
ProjectOnPlane(const btVector3 & v,const btVector3 & a)342 static inline btVector3 ProjectOnPlane( const btVector3& v,
343 const btVector3& a)
344 {
345 return(v-ProjectOnAxis(v,a));
346 }
347
348 //
ProjectOrigin(const btVector3 & a,const btVector3 & b,btVector3 & prj,btScalar & sqd)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 //
ProjectOrigin(const btVector3 & a,const btVector3 & b,const btVector3 & c,btVector3 & prj,btScalar & sqd)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>
BaryEval(const T & a,const T & b,const T & c,const btVector3 & coord)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 //
BaryCoord(const btVector3 & a,const btVector3 & b,const btVector3 & c,const btVector3 & p)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 //
NormalizeAny(const btVector3 & v)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 //
VolumeOf(const btSoftBody::Face & f,btScalar margin)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 //
CenterOf(const btSoftBody::Face & f)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 //
AreaOf(const btVector3 & x0,const btVector3 & x1,const btVector3 & x2)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 //
VolumeOf(const btVector3 & x0,const btVector3 & x1,const btVector3 & x2,const btVector3 & x3)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 //
EvaluateMedium(const btSoftBodyWorldInfo * wfi,const btVector3 & x,btSoftBody::sMedium & medium)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 //
ApplyClampedForce(btSoftBody::Node & n,const btVector3 & f,btScalar dt)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 //
MatchEdge(const btSoftBody::Node * a,const btSoftBody::Node * b,const btSoftBody::Node * ma,const btSoftBody::Node * mb)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:
mulTPQbtEigen592 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 }
mulPQbtEigen601 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 //
PolarDecompose(const btMatrix3x3 & m,btMatrix3x3 & q,btMatrix3x3 & s)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;
ClusterBasebtSoftColliders::ClusterBase637 ClusterBase()
638 {
639 erp =(btScalar)1;
640 idt =0;
641 m_margin =0;
642 friction =0;
643 threshold =(btScalar)0;
644 }
SolveContactbtSoftColliders::ClusterBase645 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
ProcessbtSoftColliders::CollideCL_RS699 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
706 ///don't collide an anchored cluster with a static/kinematic object
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 }
ProcessColObjbtSoftColliders::CollideCL_RS733 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();
739 ///Bullet rigid body uses multiply instead of minimum to determine combined friction. Some customization would be useful.
740 friction = btMin(psb->m_cfg.kDF,m_colObjWrap->getCollisionObject()->getFriction());
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];
ProcessbtSoftColliders::CollideCL_SS757 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 }
ProcessSoftSoftbtSoftColliders::CollideCL_SS795 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 {
ProcessbtSoftColliders::CollideSDF_RS811 void Process(const btDbvtNode* leaf)
812 {
813 btSoftBody::Node* node=(btSoftBody::Node*)leaf->data;
814 DoNode(*node);
815 }
DoNodebtSoftColliders::CollideSDF_RS816 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 {
ProcessbtSoftColliders::CollideVF_SS862 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
909