svn merge -r 37306:39975 https://svn.blender.org/svnroot/bf-blender/trunk/blender
[blender.git] / extern / bullet2 / src / BulletSoftBody / btSoftBodyInternals.h
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 "BulletCollision/BroadphaseCollision/btBroadphaseInterface.h"
25 #include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h"
26 #include "BulletCollision/CollisionShapes/btConvexInternalShape.h"
27 #include "BulletCollision/NarrowPhaseCollision/btGjkEpa2.h"
28 #include <string.h> //for memset
29 //
30 // btSymMatrix
31 //
32 template <typename T>
33 struct btSymMatrix
34 {
35         btSymMatrix() : dim(0)                                  {}
36         btSymMatrix(int n,const T& init=T())    { resize(n,init); }
37         void                                    resize(int n,const T& init=T())                 { dim=n;store.resize((n*(n+1))/2,init); }
38         int                                             index(int c,int r) const                                { if(c>r) btSwap(c,r);btAssert(r<dim);return((r*(r+1))/2+c); }
39         T&                                              operator()(int c,int r)                                 { return(store[index(c,r)]); }
40         const T&                                operator()(int c,int r) const                   { return(store[index(c,r)]); }
41         btAlignedObjectArray<T> store;
42         int                                             dim;
43 };      
44
45 //
46 // btSoftBodyCollisionShape
47 //
48 class btSoftBodyCollisionShape : public btConcaveShape
49 {
50 public:
51         btSoftBody*                                             m_body;
52
53         btSoftBodyCollisionShape(btSoftBody* backptr)
54         {
55                 m_shapeType = SOFTBODY_SHAPE_PROXYTYPE;
56                 m_body=backptr;
57         }
58
59         virtual ~btSoftBodyCollisionShape()
60         {
61
62         }
63
64         void    processAllTriangles(btTriangleCallback* /*callback*/,const btVector3& /*aabbMin*/,const btVector3& /*aabbMax*/) const
65         {
66                 //not yet
67                 btAssert(0);
68         }
69
70         ///getAabb returns the axis aligned bounding box in the coordinate frame of the given transform t.
71         virtual void getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const
72         {
73                 /* t should be identity, but better be safe than...fast? */ 
74                 const btVector3 mins=m_body->m_bounds[0];
75                 const btVector3 maxs=m_body->m_bounds[1];
76                 const btVector3 crns[]={t*btVector3(mins.x(),mins.y(),mins.z()),
77                         t*btVector3(maxs.x(),mins.y(),mins.z()),
78                         t*btVector3(maxs.x(),maxs.y(),mins.z()),
79                         t*btVector3(mins.x(),maxs.y(),mins.z()),
80                         t*btVector3(mins.x(),mins.y(),maxs.z()),
81                         t*btVector3(maxs.x(),mins.y(),maxs.z()),
82                         t*btVector3(maxs.x(),maxs.y(),maxs.z()),
83                         t*btVector3(mins.x(),maxs.y(),maxs.z())};
84                 aabbMin=aabbMax=crns[0];
85                 for(int i=1;i<8;++i)
86                 {
87                         aabbMin.setMin(crns[i]);
88                         aabbMax.setMax(crns[i]);
89                 }
90         }
91
92
93         virtual void    setLocalScaling(const btVector3& /*scaling*/)
94         {               
95                 ///na
96         }
97         virtual const btVector3& getLocalScaling() const
98         {
99                 static const btVector3 dummy(1,1,1);
100                 return dummy;
101         }
102         virtual void    calculateLocalInertia(btScalar /*mass*/,btVector3& /*inertia*/) const
103         {
104                 ///not yet
105                 btAssert(0);
106         }
107         virtual const char*     getName()const
108         {
109                 return "SoftBody";
110         }
111
112 };
113
114 //
115 // btSoftClusterCollisionShape
116 //
117 class btSoftClusterCollisionShape : public btConvexInternalShape
118 {
119 public:
120         const btSoftBody::Cluster*      m_cluster;
121
122         btSoftClusterCollisionShape (const btSoftBody::Cluster* cluster) : m_cluster(cluster) { setMargin(0); }
123
124
125         virtual btVector3       localGetSupportingVertex(const btVector3& vec) const
126         {
127                 btSoftBody::Node* const *                                               n=&m_cluster->m_nodes[0];
128                 btScalar                                                                                d=btDot(vec,n[0]->m_x);
129                 int                                                                                             j=0;
130                 for(int i=1,ni=m_cluster->m_nodes.size();i<ni;++i)
131                 {
132                         const btScalar  k=btDot(vec,n[i]->m_x);
133                         if(k>d) { d=k;j=i; }
134                 }
135                 return(n[j]->m_x);
136         }
137         virtual btVector3       localGetSupportingVertexWithoutMargin(const btVector3& vec)const
138         {
139                 return(localGetSupportingVertex(vec));
140         }
141         //notice that the vectors should be unit length
142         virtual void    batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const
143         {}
144
145
146         virtual void    calculateLocalInertia(btScalar mass,btVector3& inertia) const
147         {}
148
149         virtual void getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const
150         {}
151
152         virtual int     getShapeType() const { return SOFTBODY_SHAPE_PROXYTYPE; }
153
154         //debugging
155         virtual const char*     getName()const {return "SOFTCLUSTER";}
156
157         virtual void    setMargin(btScalar margin)
158         {
159                 btConvexInternalShape::setMargin(margin);
160         }
161         virtual btScalar        getMargin() const
162         {
163                 return getMargin();
164         }
165 };
166
167 //
168 // Inline's
169 //
170
171 //
172 template <typename T>
173 static inline void                      ZeroInitialize(T& value)
174 {
175         memset(&value,0,sizeof(T));
176 }
177 //
178 template <typename T>
179 static inline bool                      CompLess(const T& a,const T& b)
180 { return(a<b); }
181 //
182 template <typename T>
183 static inline bool                      CompGreater(const T& a,const T& b)
184 { return(a>b); }
185 //
186 template <typename T>
187 static inline T                         Lerp(const T& a,const T& b,btScalar t)
188 { return(a+(b-a)*t); }
189 //
190 template <typename T>
191 static inline T                         InvLerp(const T& a,const T& b,btScalar t)
192 { return((b+a*t-b*t)/(a*b)); }
193 //
194 static inline btMatrix3x3       Lerp(   const btMatrix3x3& a,
195                                                                  const btMatrix3x3& b,
196                                                                  btScalar t)
197 {
198         btMatrix3x3     r;
199         r[0]=Lerp(a[0],b[0],t);
200         r[1]=Lerp(a[1],b[1],t);
201         r[2]=Lerp(a[2],b[2],t);
202         return(r);
203 }
204 //
205 static inline btVector3         Clamp(const btVector3& v,btScalar maxlength)
206 {
207         const btScalar sql=v.length2();
208         if(sql>(maxlength*maxlength))
209                 return((v*maxlength)/btSqrt(sql));
210         else
211                 return(v);
212 }
213 //
214 template <typename T>
215 static inline T                         Clamp(const T& x,const T& l,const T& h)
216 { return(x<l?l:x>h?h:x); }
217 //
218 template <typename T>
219 static inline T                         Sq(const T& x)
220 { return(x*x); }
221 //
222 template <typename T>
223 static inline T                         Cube(const T& x)
224 { return(x*x*x); }
225 //
226 template <typename T>
227 static inline T                         Sign(const T& x)
228 { return((T)(x<0?-1:+1)); }
229 //
230 template <typename T>
231 static inline bool                      SameSign(const T& x,const T& y)
232 { return((x*y)>0); }
233 //
234 static inline btScalar          ClusterMetric(const btVector3& x,const btVector3& y)
235 {
236         const btVector3 d=x-y;
237         return(btFabs(d[0])+btFabs(d[1])+btFabs(d[2]));
238 }
239 //
240 static inline btMatrix3x3       ScaleAlongAxis(const btVector3& a,btScalar s)
241 {
242         const btScalar  xx=a.x()*a.x();
243         const btScalar  yy=a.y()*a.y();
244         const btScalar  zz=a.z()*a.z();
245         const btScalar  xy=a.x()*a.y();
246         const btScalar  yz=a.y()*a.z();
247         const btScalar  zx=a.z()*a.x();
248         btMatrix3x3             m;
249         m[0]=btVector3(1-xx+xx*s,xy*s-xy,zx*s-zx);
250         m[1]=btVector3(xy*s-xy,1-yy+yy*s,yz*s-yz);
251         m[2]=btVector3(zx*s-zx,yz*s-yz,1-zz+zz*s);
252         return(m);
253 }
254 //
255 static inline btMatrix3x3       Cross(const btVector3& v)
256 {
257         btMatrix3x3     m;
258         m[0]=btVector3(0,-v.z(),+v.y());
259         m[1]=btVector3(+v.z(),0,-v.x());
260         m[2]=btVector3(-v.y(),+v.x(),0);
261         return(m);
262 }
263 //
264 static inline btMatrix3x3       Diagonal(btScalar x)
265 {
266         btMatrix3x3     m;
267         m[0]=btVector3(x,0,0);
268         m[1]=btVector3(0,x,0);
269         m[2]=btVector3(0,0,x);
270         return(m);
271 }
272 //
273 static inline btMatrix3x3       Add(const btMatrix3x3& a,
274                                                                 const btMatrix3x3& b)
275 {
276         btMatrix3x3     r;
277         for(int i=0;i<3;++i) r[i]=a[i]+b[i];
278         return(r);
279 }
280 //
281 static inline btMatrix3x3       Sub(const btMatrix3x3& a,
282                                                                 const btMatrix3x3& b)
283 {
284         btMatrix3x3     r;
285         for(int i=0;i<3;++i) r[i]=a[i]-b[i];
286         return(r);
287 }
288 //
289 static inline btMatrix3x3       Mul(const btMatrix3x3& a,
290                                                                 btScalar b)
291 {
292         btMatrix3x3     r;
293         for(int i=0;i<3;++i) r[i]=a[i]*b;
294         return(r);
295 }
296 //
297 static inline void                      Orthogonalize(btMatrix3x3& m)
298 {
299         m[2]=btCross(m[0],m[1]).normalized();
300         m[1]=btCross(m[2],m[0]).normalized();
301         m[0]=btCross(m[1],m[2]).normalized();
302 }
303 //
304 static inline btMatrix3x3       MassMatrix(btScalar im,const btMatrix3x3& iwi,const btVector3& r)
305 {
306         const btMatrix3x3       cr=Cross(r);
307         return(Sub(Diagonal(im),cr*iwi*cr));
308 }
309
310 //
311 static inline btMatrix3x3       ImpulseMatrix(  btScalar dt,
312                                                                                   btScalar ima,
313                                                                                   btScalar imb,
314                                                                                   const btMatrix3x3& iwi,
315                                                                                   const btVector3& r)
316 {
317         return(Diagonal(1/dt)*Add(Diagonal(ima),MassMatrix(imb,iwi,r)).inverse());
318 }
319
320 //
321 static inline btMatrix3x3       ImpulseMatrix(  btScalar ima,const btMatrix3x3& iia,const btVector3& ra,
322                                                                                   btScalar imb,const btMatrix3x3& iib,const btVector3& rb)      
323 {
324         return(Add(MassMatrix(ima,iia,ra),MassMatrix(imb,iib,rb)).inverse());
325 }
326
327 //
328 static inline btMatrix3x3       AngularImpulseMatrix(   const btMatrix3x3& iia,
329                                                                                                  const btMatrix3x3& iib)
330 {
331         return(Add(iia,iib).inverse());
332 }
333
334 //
335 static inline btVector3         ProjectOnAxis(  const btVector3& v,
336                                                                                   const btVector3& a)
337 {
338         return(a*btDot(v,a));
339 }
340 //
341 static inline btVector3         ProjectOnPlane( const btVector3& v,
342                                                                                    const btVector3& a)
343 {
344         return(v-ProjectOnAxis(v,a));
345 }
346
347 //
348 static inline void                      ProjectOrigin(  const btVector3& a,
349                                                                                   const btVector3& b,
350                                                                                   btVector3& prj,
351                                                                                   btScalar& sqd)
352 {
353         const btVector3 d=b-a;
354         const btScalar  m2=d.length2();
355         if(m2>SIMD_EPSILON)
356         {       
357                 const btScalar  t=Clamp<btScalar>(-btDot(a,d)/m2,0,1);
358                 const btVector3 p=a+d*t;
359                 const btScalar  l2=p.length2();
360                 if(l2<sqd)
361                 {
362                         prj=p;
363                         sqd=l2;
364                 }
365         }
366 }
367 //
368 static inline void                      ProjectOrigin(  const btVector3& a,
369                                                                                   const btVector3& b,
370                                                                                   const btVector3& c,
371                                                                                   btVector3& prj,
372                                                                                   btScalar& sqd)
373 {
374         const btVector3&        q=btCross(b-a,c-a);
375         const btScalar          m2=q.length2();
376         if(m2>SIMD_EPSILON)
377         {
378                 const btVector3 n=q/btSqrt(m2);
379                 const btScalar  k=btDot(a,n);
380                 const btScalar  k2=k*k;
381                 if(k2<sqd)
382                 {
383                         const btVector3 p=n*k;
384                         if(     (btDot(btCross(a-p,b-p),q)>0)&&
385                                 (btDot(btCross(b-p,c-p),q)>0)&&
386                                 (btDot(btCross(c-p,a-p),q)>0))
387                         {                       
388                                 prj=p;
389                                 sqd=k2;
390                         }
391                         else
392                         {
393                                 ProjectOrigin(a,b,prj,sqd);
394                                 ProjectOrigin(b,c,prj,sqd);
395                                 ProjectOrigin(c,a,prj,sqd);
396                         }
397                 }
398         }
399 }
400
401 //
402 template <typename T>
403 static inline T                         BaryEval(               const T& a,
404                                                                          const T& b,
405                                                                          const T& c,
406                                                                          const btVector3& coord)
407 {
408         return(a*coord.x()+b*coord.y()+c*coord.z());
409 }
410 //
411 static inline btVector3         BaryCoord(      const btVector3& a,
412                                                                           const btVector3& b,
413                                                                           const btVector3& c,
414                                                                           const btVector3& p)
415 {
416         const btScalar  w[]={   btCross(a-p,b-p).length(),
417                 btCross(b-p,c-p).length(),
418                 btCross(c-p,a-p).length()};
419         const btScalar  isum=1/(w[0]+w[1]+w[2]);
420         return(btVector3(w[1]*isum,w[2]*isum,w[0]*isum));
421 }
422
423 //
424 static btScalar                         ImplicitSolve(  btSoftBody::ImplicitFn* fn,
425                                                                                   const btVector3& a,
426                                                                                   const btVector3& b,
427                                                                                   const btScalar accuracy,
428                                                                                   const int maxiterations=256)
429 {
430         btScalar        span[2]={0,1};
431         btScalar        values[2]={fn->Eval(a),fn->Eval(b)};
432         if(values[0]>values[1])
433         {
434                 btSwap(span[0],span[1]);
435                 btSwap(values[0],values[1]);
436         }
437         if(values[0]>-accuracy) return(-1);
438         if(values[1]<+accuracy) return(-1);
439         for(int i=0;i<maxiterations;++i)
440         {
441                 const btScalar  t=Lerp(span[0],span[1],values[0]/(values[0]-values[1]));
442                 const btScalar  v=fn->Eval(Lerp(a,b,t));
443                 if((t<=0)||(t>=1))              break;
444                 if(btFabs(v)<accuracy)  return(t);
445                 if(v<0)
446                 { span[0]=t;values[0]=v; }
447                 else
448                 { span[1]=t;values[1]=v; }
449         }
450         return(-1);
451 }
452
453 //
454 static inline btVector3         NormalizeAny(const btVector3& v)
455 {
456         const btScalar l=v.length();
457         if(l>SIMD_EPSILON)
458                 return(v/l);
459         else
460                 return(btVector3(0,0,0));
461 }
462
463 //
464 static inline btDbvtVolume      VolumeOf(       const btSoftBody::Face& f,
465                                                                          btScalar margin)
466 {
467         const btVector3*        pts[]={ &f.m_n[0]->m_x,
468                 &f.m_n[1]->m_x,
469                 &f.m_n[2]->m_x};
470         btDbvtVolume            vol=btDbvtVolume::FromPoints(pts,3);
471         vol.Expand(btVector3(margin,margin,margin));
472         return(vol);
473 }
474
475 //
476 static inline btVector3                 CenterOf(       const btSoftBody::Face& f)
477 {
478         return((f.m_n[0]->m_x+f.m_n[1]->m_x+f.m_n[2]->m_x)/3);
479 }
480
481 //
482 static inline btScalar                  AreaOf(         const btVector3& x0,
483                                                                            const btVector3& x1,
484                                                                            const btVector3& x2)
485 {
486         const btVector3 a=x1-x0;
487         const btVector3 b=x2-x0;
488         const btVector3 cr=btCross(a,b);
489         const btScalar  area=cr.length();
490         return(area);
491 }
492
493 //
494 static inline btScalar          VolumeOf(       const btVector3& x0,
495                                                                          const btVector3& x1,
496                                                                          const btVector3& x2,
497                                                                          const btVector3& x3)
498 {
499         const btVector3 a=x1-x0;
500         const btVector3 b=x2-x0;
501         const btVector3 c=x3-x0;
502         return(btDot(a,btCross(b,c)));
503 }
504
505 //
506 static void                                     EvaluateMedium( const btSoftBodyWorldInfo* wfi,
507                                                                                    const btVector3& x,
508                                                                                    btSoftBody::sMedium& medium)
509 {
510         medium.m_velocity       =       btVector3(0,0,0);
511         medium.m_pressure       =       0;
512         medium.m_density        =       wfi->air_density;
513         if(wfi->water_density>0)
514         {
515                 const btScalar  depth=-(btDot(x,wfi->water_normal)+wfi->water_offset);
516                 if(depth>0)
517                 {
518                         medium.m_density        =       wfi->water_density;
519                         medium.m_pressure       =       depth*wfi->water_density*wfi->m_gravity.length();
520                 }
521         }
522 }
523
524 //
525 static inline void                      ApplyClampedForce(      btSoftBody::Node& n,
526                                                                                           const btVector3& f,
527                                                                                           btScalar dt)
528 {
529         const btScalar  dtim=dt*n.m_im;
530         if((f*dtim).length2()>n.m_v.length2())
531         {/* Clamp       */ 
532                 n.m_f-=ProjectOnAxis(n.m_v,f.normalized())/dtim;                                                
533         }
534         else
535         {/* Apply       */ 
536                 n.m_f+=f;
537         }
538 }
539
540 //
541 static inline int               MatchEdge(      const btSoftBody::Node* a,
542                                                                   const btSoftBody::Node* b,
543                                                                   const btSoftBody::Node* ma,
544                                                                   const btSoftBody::Node* mb)
545 {
546         if((a==ma)&&(b==mb)) return(0);
547         if((a==mb)&&(b==ma)) return(1);
548         return(-1);
549 }
550
551 //
552 // btEigen : Extract eigen system,
553 // straitforward implementation of http://math.fullerton.edu/mathews/n2003/JacobiMethodMod.html
554 // outputs are NOT sorted.
555 //
556 struct  btEigen
557 {
558         static int                      system(btMatrix3x3& a,btMatrix3x3* vectors,btVector3* values=0)
559         {
560                 static const int                maxiterations=16;
561                 static const btScalar   accuracy=(btScalar)0.0001;
562                 btMatrix3x3&                    v=*vectors;
563                 int                                             iterations=0;
564                 vectors->setIdentity();
565                 do      {
566                         int                             p=0,q=1;
567                         if(btFabs(a[p][q])<btFabs(a[0][2])) { p=0;q=2; }
568                         if(btFabs(a[p][q])<btFabs(a[1][2])) { p=1;q=2; }
569                         if(btFabs(a[p][q])>accuracy)
570                         {
571                                 const btScalar  w=(a[q][q]-a[p][p])/(2*a[p][q]);
572                                 const btScalar  z=btFabs(w);
573                                 const btScalar  t=w/(z*(btSqrt(1+w*w)+z));
574                                 if(t==t)/* [WARNING] let hope that one does not get thrown aways by some compilers... */ 
575                                 {
576                                         const btScalar  c=1/btSqrt(t*t+1);
577                                         const btScalar  s=c*t;
578                                         mulPQ(a,c,s,p,q);
579                                         mulTPQ(a,c,s,p,q);
580                                         mulPQ(v,c,s,p,q);
581                                 } else break;
582                         } else break;
583                 } while((++iterations)<maxiterations);
584                 if(values)
585                 {
586                         *values=btVector3(a[0][0],a[1][1],a[2][2]);
587                 }
588                 return(iterations);
589         }
590 private:
591         static inline void      mulTPQ(btMatrix3x3& a,btScalar c,btScalar s,int p,int q)
592         {
593                 const btScalar  m[2][3]={       {a[p][0],a[p][1],a[p][2]},
594                 {a[q][0],a[q][1],a[q][2]}};
595                 int i;
596
597                 for(i=0;i<3;++i) a[p][i]=c*m[0][i]-s*m[1][i];
598                 for(i=0;i<3;++i) a[q][i]=c*m[1][i]+s*m[0][i];
599         }
600         static inline void      mulPQ(btMatrix3x3& a,btScalar c,btScalar s,int p,int q)
601         {
602                 const btScalar  m[2][3]={       {a[0][p],a[1][p],a[2][p]},
603                 {a[0][q],a[1][q],a[2][q]}};
604                 int i;
605
606                 for(i=0;i<3;++i) a[i][p]=c*m[0][i]-s*m[1][i];
607                 for(i=0;i<3;++i) a[i][q]=c*m[1][i]+s*m[0][i];
608         }
609 };
610
611 //
612 // Polar decomposition,
613 // "Computing the Polar Decomposition with Applications", Nicholas J. Higham, 1986.
614 //
615 static inline int                       PolarDecompose( const btMatrix3x3& m,btMatrix3x3& q,btMatrix3x3& s)
616 {
617         static const btScalar   half=(btScalar)0.5;
618         static const btScalar   accuracy=(btScalar)0.0001;
619         static const int                maxiterations=16;
620         int                                             i=0;
621         btScalar                                det=0;
622         q       =       Mul(m,1/btVector3(m[0][0],m[1][1],m[2][2]).length());
623         det     =       q.determinant();
624         if(!btFuzzyZero(det))
625         {
626                 for(;i<maxiterations;++i)
627                 {
628                         q=Mul(Add(q,Mul(q.adjoint(),1/det).transpose()),half);
629                         const btScalar  ndet=q.determinant();
630                         if(Sq(ndet-det)>accuracy) det=ndet; else break;
631                 }
632                 /* Final orthogonalization      */ 
633                 Orthogonalize(q);
634                 /* Compute 'S'                          */ 
635                 s=q.transpose()*m;
636         }
637         else
638         {
639                 q.setIdentity();
640                 s.setIdentity();
641         }
642         return(i);
643 }
644
645 //
646 // btSoftColliders
647 //
648 struct btSoftColliders
649 {
650         //
651         // ClusterBase
652         //
653         struct  ClusterBase : btDbvt::ICollide
654         {
655                 btScalar                        erp;
656                 btScalar                        idt;
657                 btScalar                        m_margin;
658                 btScalar                        friction;
659                 btScalar                        threshold;
660                 ClusterBase()
661                 {
662                         erp                     =(btScalar)1;
663                         idt                     =0;
664                         m_margin                =0;
665                         friction        =0;
666                         threshold       =(btScalar)0;
667                 }
668                 bool                            SolveContact(   const btGjkEpaSolver2::sResults& res,
669                         btSoftBody::Body ba,btSoftBody::Body bb,
670                         btSoftBody::CJoint& joint)
671                 {
672                         if(res.distance<m_margin)
673                         {
674                                 btVector3 norm = res.normal;
675                                 norm.normalize();//is it necessary?
676
677                                 const btVector3         ra=res.witnesses[0]-ba.xform().getOrigin();
678                                 const btVector3         rb=res.witnesses[1]-bb.xform().getOrigin();
679                                 const btVector3         va=ba.velocity(ra);
680                                 const btVector3         vb=bb.velocity(rb);
681                                 const btVector3         vrel=va-vb;
682                                 const btScalar          rvac=btDot(vrel,norm);
683                                  btScalar               depth=res.distance-m_margin;
684                                 
685 //                              printf("depth=%f\n",depth);
686                                 const btVector3         iv=norm*rvac;
687                                 const btVector3         fv=vrel-iv;
688                                 joint.m_bodies[0]       =       ba;
689                                 joint.m_bodies[1]       =       bb;
690                                 joint.m_refs[0]         =       ra*ba.xform().getBasis();
691                                 joint.m_refs[1]         =       rb*bb.xform().getBasis();
692                                 joint.m_rpos[0]         =       ra;
693                                 joint.m_rpos[1]         =       rb;
694                                 joint.m_cfm                     =       1;
695                                 joint.m_erp                     =       1;
696                                 joint.m_life            =       0;
697                                 joint.m_maxlife         =       0;
698                                 joint.m_split           =       1;
699                                 
700                                 joint.m_drift           =       depth*norm;
701
702                                 joint.m_normal          =       norm;
703 //                              printf("normal=%f,%f,%f\n",res.normal.getX(),res.normal.getY(),res.normal.getZ());
704                                 joint.m_delete          =       false;
705                                 joint.m_friction        =       fv.length2()<(-rvac*friction)?1:friction;
706                                 joint.m_massmatrix      =       ImpulseMatrix(  ba.invMass(),ba.invWorldInertia(),joint.m_rpos[0],
707                                         bb.invMass(),bb.invWorldInertia(),joint.m_rpos[1]);
708
709                                 return(true);
710                         }
711                         return(false);
712                 }
713         };
714         //
715         // CollideCL_RS
716         //
717         struct  CollideCL_RS : ClusterBase
718         {
719                 btSoftBody*             psb;
720                 
721                 btCollisionObject*      m_colObj;
722                 void            Process(const btDbvtNode* leaf)
723                 {
724                         btSoftBody::Cluster*            cluster=(btSoftBody::Cluster*)leaf->data;
725                         btSoftClusterCollisionShape     cshape(cluster);
726                         
727                         const btConvexShape*            rshape=(const btConvexShape*)m_colObj->getCollisionShape();
728
729                         ///don't collide an anchored cluster with a static/kinematic object
730                         if(m_colObj->isStaticOrKinematicObject() && cluster->m_containsAnchor)
731                                 return;
732
733                         btGjkEpaSolver2::sResults       res;            
734                         if(btGjkEpaSolver2::SignedDistance(     &cshape,btTransform::getIdentity(),
735                                 rshape,m_colObj->getWorldTransform(),
736                                 btVector3(1,0,0),res))
737                         {
738                                 btSoftBody::CJoint      joint;
739                                 if(SolveContact(res,cluster,m_colObj,joint))//prb,joint))
740                                 {
741                                         btSoftBody::CJoint*     pj=new(btAlignedAlloc(sizeof(btSoftBody::CJoint),16)) btSoftBody::CJoint();
742                                         *pj=joint;psb->m_joints.push_back(pj);
743                                         if(m_colObj->isStaticOrKinematicObject())
744                                         {
745                                                 pj->m_erp       *=      psb->m_cfg.kSKHR_CL;
746                                                 pj->m_split     *=      psb->m_cfg.kSK_SPLT_CL;
747                                         }
748                                         else
749                                         {
750                                                 pj->m_erp       *=      psb->m_cfg.kSRHR_CL;
751                                                 pj->m_split     *=      psb->m_cfg.kSR_SPLT_CL;
752                                         }
753                                 }
754                         }
755                 }
756                 void            Process(btSoftBody* ps,btCollisionObject* colOb)
757                 {
758                         psb                     =       ps;
759                         m_colObj                        =       colOb;
760                         idt                     =       ps->m_sst.isdt;
761                         m_margin                =       m_colObj->getCollisionShape()->getMargin()+psb->getCollisionShape()->getMargin();
762                         ///Bullet rigid body uses multiply instead of minimum to determine combined friction. Some customization would be useful.
763                         friction        =       btMin(psb->m_cfg.kDF,m_colObj->getFriction());
764                         btVector3                       mins;
765                         btVector3                       maxs;
766
767                         ATTRIBUTE_ALIGNED16(btDbvtVolume)               volume;
768                         colOb->getCollisionShape()->getAabb(colOb->getWorldTransform(),mins,maxs);
769                         volume=btDbvtVolume::FromMM(mins,maxs);
770                         volume.Expand(btVector3(1,1,1)*m_margin);
771                         ps->m_cdbvt.collideTV(ps->m_cdbvt.m_root,volume,*this);
772                 }       
773         };
774         //
775         // CollideCL_SS
776         //
777         struct  CollideCL_SS : ClusterBase
778         {
779                 btSoftBody*     bodies[2];
780                 void            Process(const btDbvtNode* la,const btDbvtNode* lb)
781                 {
782                         btSoftBody::Cluster*            cla=(btSoftBody::Cluster*)la->data;
783                         btSoftBody::Cluster*            clb=(btSoftBody::Cluster*)lb->data;
784
785
786                         bool connected=false;
787                         if ((bodies[0]==bodies[1])&&(bodies[0]->m_clusterConnectivity.size()))
788                         {
789                                 connected = bodies[0]->m_clusterConnectivity[cla->m_clusterIndex+bodies[0]->m_clusters.size()*clb->m_clusterIndex];
790                         }
791
792                         if (!connected)
793                         {
794                                 btSoftClusterCollisionShape     csa(cla);
795                                 btSoftClusterCollisionShape     csb(clb);
796                                 btGjkEpaSolver2::sResults       res;            
797                                 if(btGjkEpaSolver2::SignedDistance(     &csa,btTransform::getIdentity(),
798                                         &csb,btTransform::getIdentity(),
799                                         cla->m_com-clb->m_com,res))
800                                 {
801                                         btSoftBody::CJoint      joint;
802                                         if(SolveContact(res,cla,clb,joint))
803                                         {
804                                                 btSoftBody::CJoint*     pj=new(btAlignedAlloc(sizeof(btSoftBody::CJoint),16)) btSoftBody::CJoint();
805                                                 *pj=joint;bodies[0]->m_joints.push_back(pj);
806                                                 pj->m_erp       *=      btMax(bodies[0]->m_cfg.kSSHR_CL,bodies[1]->m_cfg.kSSHR_CL);
807                                                 pj->m_split     *=      (bodies[0]->m_cfg.kSS_SPLT_CL+bodies[1]->m_cfg.kSS_SPLT_CL)/2;
808                                         }
809                                 }
810                         } else
811                         {
812                                 static int count=0;
813                                 count++;
814                                 //printf("count=%d\n",count);
815                                 
816                         }
817                 }
818                 void            Process(btSoftBody* psa,btSoftBody* psb)
819                 {
820                         idt                     =       psa->m_sst.isdt;
821                         //m_margin              =       (psa->getCollisionShape()->getMargin()+psb->getCollisionShape()->getMargin())/2;
822                         m_margin                =       (psa->getCollisionShape()->getMargin()+psb->getCollisionShape()->getMargin());
823                         friction        =       btMin(psa->m_cfg.kDF,psb->m_cfg.kDF);
824                         bodies[0]       =       psa;
825                         bodies[1]       =       psb;
826                         psa->m_cdbvt.collideTT(psa->m_cdbvt.m_root,psb->m_cdbvt.m_root,*this);
827                 }       
828         };
829         //
830         // CollideSDF_RS
831         //
832         struct  CollideSDF_RS : btDbvt::ICollide
833         {
834                 void            Process(const btDbvtNode* leaf)
835                 {
836                         btSoftBody::Node*       node=(btSoftBody::Node*)leaf->data;
837                         DoNode(*node);
838                 }
839                 void            DoNode(btSoftBody::Node& n) const
840                 {
841                         const btScalar                  m=n.m_im>0?dynmargin:stamargin;
842                         btSoftBody::RContact    c;
843                         if(     (!n.m_battach)&&
844                                 psb->checkContact(m_colObj1,n.m_x,m,c.m_cti))
845                         {
846                                 const btScalar  ima=n.m_im;
847                                 const btScalar  imb= m_rigidBody? m_rigidBody->getInvMass() : 0.f;
848                                 const btScalar  ms=ima+imb;
849                                 if(ms>0)
850                                 {
851                                         const btTransform&      wtr=m_rigidBody?m_rigidBody->getWorldTransform() : m_colObj1->getWorldTransform();
852                                         static const btMatrix3x3        iwiStatic(0,0,0,0,0,0,0,0,0);
853                                         const btMatrix3x3&      iwi=m_rigidBody?m_rigidBody->getInvInertiaTensorWorld() : iwiStatic;
854                                         const btVector3         ra=n.m_x-wtr.getOrigin();
855                                         const btVector3         va=m_rigidBody ? m_rigidBody->getVelocityInLocalPoint(ra)*psb->m_sst.sdt : btVector3(0,0,0);
856                                         const btVector3         vb=n.m_x-n.m_q; 
857                                         const btVector3         vr=vb-va;
858                                         const btScalar          dn=btDot(vr,c.m_cti.m_normal);
859                                         const btVector3         fv=vr-c.m_cti.m_normal*dn;
860                                         const btScalar          fc=psb->m_cfg.kDF*m_colObj1->getFriction();
861                                         c.m_node        =       &n;
862                                         c.m_c0          =       ImpulseMatrix(psb->m_sst.sdt,ima,imb,iwi,ra);
863                                         c.m_c1          =       ra;
864                                         c.m_c2          =       ima*psb->m_sst.sdt;
865                                         c.m_c3          =       fv.length2()<(btFabs(dn)*fc)?0:1-fc;
866                                         c.m_c4          =       m_colObj1->isStaticOrKinematicObject()?psb->m_cfg.kKHR:psb->m_cfg.kCHR;
867                                         psb->m_rcontacts.push_back(c);
868                                         if (m_rigidBody)
869                                                 m_rigidBody->activate();
870                                 }
871                         }
872                 }
873                 btSoftBody*             psb;
874                 btCollisionObject*      m_colObj1;
875                 btRigidBody*    m_rigidBody;
876                 btScalar                dynmargin;
877                 btScalar                stamargin;
878         };
879         //
880         // CollideVF_SS
881         //
882         struct  CollideVF_SS : btDbvt::ICollide
883         {
884                 void            Process(const btDbvtNode* lnode,
885                         const btDbvtNode* lface)
886                 {
887                         btSoftBody::Node*       node=(btSoftBody::Node*)lnode->data;
888                         btSoftBody::Face*       face=(btSoftBody::Face*)lface->data;
889                         btVector3                       o=node->m_x;
890                         btVector3                       p;
891                         btScalar                        d=SIMD_INFINITY;
892                         ProjectOrigin(  face->m_n[0]->m_x-o,
893                                 face->m_n[1]->m_x-o,
894                                 face->m_n[2]->m_x-o,
895                                 p,d);
896                         const btScalar  m=mrg+(o-node->m_q).length()*2;
897                         if(d<(m*m))
898                         {
899                                 const btSoftBody::Node* n[]={face->m_n[0],face->m_n[1],face->m_n[2]};
900                                 const btVector3                 w=BaryCoord(n[0]->m_x,n[1]->m_x,n[2]->m_x,p+o);
901                                 const btScalar                  ma=node->m_im;
902                                 btScalar                                mb=BaryEval(n[0]->m_im,n[1]->m_im,n[2]->m_im,w);
903                                 if(     (n[0]->m_im<=0)||
904                                         (n[1]->m_im<=0)||
905                                         (n[2]->m_im<=0))
906                                 {
907                                         mb=0;
908                                 }
909                                 const btScalar  ms=ma+mb;
910                                 if(ms>0)
911                                 {
912                                         btSoftBody::SContact    c;
913                                         c.m_normal              =       p/-btSqrt(d);
914                                         c.m_margin              =       m;
915                                         c.m_node                =       node;
916                                         c.m_face                =       face;
917                                         c.m_weights             =       w;
918                                         c.m_friction    =       btMax(psb[0]->m_cfg.kDF,psb[1]->m_cfg.kDF);
919                                         c.m_cfm[0]              =       ma/ms*psb[0]->m_cfg.kSHR;
920                                         c.m_cfm[1]              =       mb/ms*psb[1]->m_cfg.kSHR;
921                                         psb[0]->m_scontacts.push_back(c);
922                                 }
923                         }       
924                 }
925                 btSoftBody*             psb[2];
926                 btScalar                mrg;
927         };
928 };
929
930 #endif //_BT_SOFT_BODY_INTERNALS_H