Planet
navi homePPSaboutscreenshotsdownloaddevelopmentforum

source: code/branches/physics/src/bullet/BulletSoftBody/btSoftBodyInternals.h @ 1963

Last change on this file since 1963 was 1963, checked in by rgrieder, 16 years ago

Added Bullet physics engine.
  • Property svn:eol-style set to native
File size: 22.8 KB
Line 
1/*
2Bullet Continuous Collision Detection and Physics Library
3Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
4
5This software is provided 'as-is', without any express or implied warranty.
6In no event will the authors be held liable for any damages arising from the use of this software.
7Permission is granted to anyone to use this software for any purpose,
8including commercial applications, and to alter it and redistribute it freely,
9subject to the following restrictions:
10
111. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
122. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
133. This notice may not be removed or altered from any source distribution.
14*/
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#include "LinearMath/btQuickprof.h"
23#include "BulletCollision/BroadphaseCollision/btBroadphaseInterface.h"
24#include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h"
25#include "BulletCollision/CollisionShapes/btConvexInternalShape.h"
26#include "BulletCollision/NarrowPhaseCollision/btGjkEpa2.h"
27
28//
29// btSymMatrix
30//
31template <typename T>
32struct btSymMatrix
33{
34        btSymMatrix() : dim(0)                                  {}
35        btSymMatrix(int n,const T& init=T())    { resize(n,init); }
36        void                                    resize(int n,const T& init=T())                 { dim=n;store.resize((n*(n+1))/2,init); }
37        int                                             index(int c,int r) const                                { if(c>r) btSwap(c,r);btAssert(r<dim);return((r*(r+1))/2+c); }
38        T&                                              operator()(int c,int r)                                 { return(store[index(c,r)]); }
39        const T&                                operator()(int c,int r) const                   { return(store[index(c,r)]); }
40        btAlignedObjectArray<T> store;
41        int                                             dim;
42};     
43
44//
45// btSoftBodyCollisionShape
46//
47class btSoftBodyCollisionShape : public btConcaveShape
48{
49public:
50        btSoftBody*                                             m_body;
51
52        btSoftBodyCollisionShape(btSoftBody* backptr)
53        {
54                m_shapeType = SOFTBODY_SHAPE_PROXYTYPE;
55                m_body=backptr;
56        }
57
58        virtual ~btSoftBodyCollisionShape()
59        {
60
61        }
62
63        void    processAllTriangles(btTriangleCallback* /*callback*/,const btVector3& /*aabbMin*/,const btVector3& /*aabbMax*/) const
64        {
65                //not yet
66                btAssert(0);
67        }
68
69        ///getAabb returns the axis aligned bounding box in the coordinate frame of the given transform t.
70        virtual void getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const
71        {
72                /* t should be identity, but better be safe than...fast? */ 
73                const btVector3 mins=m_body->m_bounds[0];
74                const btVector3 maxs=m_body->m_bounds[1];
75                const btVector3 crns[]={t*btVector3(mins.x(),mins.y(),mins.z()),
76                        t*btVector3(maxs.x(),mins.y(),mins.z()),
77                        t*btVector3(maxs.x(),maxs.y(),mins.z()),
78                        t*btVector3(mins.x(),maxs.y(),mins.z()),
79                        t*btVector3(mins.x(),mins.y(),maxs.z()),
80                        t*btVector3(maxs.x(),mins.y(),maxs.z()),
81                        t*btVector3(maxs.x(),maxs.y(),maxs.z()),
82                        t*btVector3(mins.x(),maxs.y(),maxs.z())};
83                aabbMin=aabbMax=crns[0];
84                for(int i=1;i<8;++i)
85                {
86                        aabbMin.setMin(crns[i]);
87                        aabbMax.setMax(crns[i]);
88                }
89        }
90
91
92        virtual void    setLocalScaling(const btVector3& /*scaling*/)
93        {               
94                ///na
95        }
96        virtual const btVector3& getLocalScaling() const
97        {
98                static const btVector3 dummy(1,1,1);
99                return dummy;
100        }
101        virtual void    calculateLocalInertia(btScalar /*mass*/,btVector3& /*inertia*/) const
102        {
103                ///not yet
104                btAssert(0);
105        }
106        virtual const char*     getName()const
107        {
108                return "SoftBody";
109        }
110
111};
112
113//
114// btSoftClusterCollisionShape
115//
116class btSoftClusterCollisionShape : public btConvexInternalShape
117{
118public:
119        const btSoftBody::Cluster*      m_cluster;
120
121        btSoftClusterCollisionShape (const btSoftBody::Cluster* cluster) : m_cluster(cluster) { setMargin(0); }
122
123
124        virtual btVector3       localGetSupportingVertex(const btVector3& vec) const
125        {
126                btSoftBody::Node* const *                                               n=&m_cluster->m_nodes[0];
127                btScalar                                                                                d=dot(vec,n[0]->m_x);
128                int                                                                                             j=0;
129                for(int i=1,ni=m_cluster->m_nodes.size();i<ni;++i)
130                {
131                        const btScalar  k=dot(vec,n[i]->m_x);
132                        if(k>d) { d=k;j=i; }
133                }
134                return(n[j]->m_x);
135        }
136        virtual btVector3       localGetSupportingVertexWithoutMargin(const btVector3& vec)const
137        {
138                return(localGetSupportingVertex(vec));
139        }
140        //notice that the vectors should be unit length
141        virtual void    batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const
142        {}
143
144
145        virtual void    calculateLocalInertia(btScalar mass,btVector3& inertia) const
146        {}
147
148        virtual void getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const
149        {}
150
151        virtual int     getShapeType() const { return SOFTBODY_SHAPE_PROXYTYPE; }
152
153        //debugging
154        virtual const char*     getName()const {return "SOFTCLUSTER";}
155
156        virtual void    setMargin(btScalar margin)
157        {
158                btConvexInternalShape::setMargin(margin);
159        }
160        virtual btScalar        getMargin() const
161        {
162                return getMargin();
163        }
164};
165
166//
167// Inline's
168//
169
170//
171template <typename T>
172static inline void                      ZeroInitialize(T& value)
173{
174        static const T  zerodummy;
175        value=zerodummy;
176}
177//
178template <typename T>
179static inline bool                      CompLess(const T& a,const T& b)
180{ return(a<b); }
181//
182template <typename T>
183static inline bool                      CompGreater(const T& a,const T& b)
184{ return(a>b); }
185//
186template <typename T>
187static inline T                         Lerp(const T& a,const T& b,btScalar t)
188{ return(a+(b-a)*t); }
189//
190template <typename T>
191static inline T                         InvLerp(const T& a,const T& b,btScalar t)
192{ return((b+a*t-b*t)/(a*b)); }
193//
194static 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//
205static 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//
214template <typename T>
215static inline T                         Clamp(const T& x,const T& l,const T& h)
216{ return(x<l?l:x>h?h:x); }
217//
218template <typename T>
219static inline T                         Sq(const T& x)
220{ return(x*x); }
221//
222template <typename T>
223static inline T                         Cube(const T& x)
224{ return(x*x*x); }
225//
226template <typename T>
227static inline T                         Sign(const T& x)
228{ return((T)(x<0?-1:+1)); }
229//
230template <typename T>
231static inline bool                      SameSign(const T& x,const T& y)
232{ return((x*y)>0); }
233//
234static 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//
240static 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//
255static 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//
264static 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//
273static 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//
281static 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//
289static 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//
297static inline void                      Orthogonalize(btMatrix3x3& m)
298{
299        m[2]=cross(m[0],m[1]).normalized();
300        m[1]=cross(m[2],m[0]).normalized();
301        m[0]=cross(m[1],m[2]).normalized();
302}
303//
304static 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//
311static 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//
321static 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//
328static inline btMatrix3x3       AngularImpulseMatrix(   const btMatrix3x3& iia,
329                                                                                                 const btMatrix3x3& iib)
330{
331        return(Add(iia,iib).inverse());
332}
333
334//
335static inline btVector3         ProjectOnAxis(  const btVector3& v,
336                                                                                  const btVector3& a)
337{
338        return(a*dot(v,a));
339}
340//
341static inline btVector3         ProjectOnPlane( const btVector3& v,
342                                                                                   const btVector3& a)
343{
344        return(v-ProjectOnAxis(v,a));
345}
346
347//
348static 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>(-dot(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//
368static 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=cross(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=dot(a,n);
380                const btScalar  k2=k*k;
381                if(k2<sqd)
382                {
383                        const btVector3 p=n*k;
384                        if(     (dot(cross(a-p,b-p),q)>0)&&
385                                (dot(cross(b-p,c-p),q)>0)&&
386                                (dot(cross(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//
402template <typename T>
403static 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//
411static inline btVector3         BaryCoord(      const btVector3& a,
412                                                                          const btVector3& b,
413                                                                          const btVector3& c,
414                                                                          const btVector3& p)
415{
416        const btScalar  w[]={   cross(a-p,b-p).length(),
417                cross(b-p,c-p).length(),
418                cross(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//
424static 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//
454static 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//
464static 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//
476static 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//
482static 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=cross(a,b);
489        const btScalar  area=cr.length();
490        return(area);
491}
492
493//
494static 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(dot(a,cross(b,c)));
503}
504
505//
506static 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=-(dot(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//
525static 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//
541static 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//
556struct  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        }
590private:
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//
615static 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//
648struct btSoftColliders
649{
650        //
651        // ClusterBase
652        //
653        struct  ClusterBase : btDbvt::ICollide
654        {
655                btScalar                        erp;
656                btScalar                        idt;
657                btScalar                        margin;
658                btScalar                        friction;
659                btScalar                        threshold;
660                ClusterBase()
661                {
662                        erp                     =(btScalar)1;
663                        idt                     =0;
664                        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<margin)
673                        {
674                                const btVector3         ra=res.witnesses[0]-ba.xform().getOrigin();
675                                const btVector3         rb=res.witnesses[1]-bb.xform().getOrigin();
676                                const btVector3         va=ba.velocity(ra);
677                                const btVector3         vb=bb.velocity(rb);
678                                const btVector3         vrel=va-vb;
679                                const btScalar          rvac=dot(vrel,res.normal);
680                                const btScalar          depth=res.distance-margin;
681                                const btVector3         iv=res.normal*rvac;
682                                const btVector3         fv=vrel-iv;
683                                joint.m_bodies[0]       =       ba;
684                                joint.m_bodies[1]       =       bb;
685                                joint.m_refs[0]         =       ra*ba.xform().getBasis();
686                                joint.m_refs[1]         =       rb*bb.xform().getBasis();
687                                joint.m_rpos[0]         =       ra;
688                                joint.m_rpos[1]         =       rb;
689                                joint.m_cfm                     =       1;
690                                joint.m_erp                     =       1;
691                                joint.m_life            =       0;
692                                joint.m_maxlife         =       0;
693                                joint.m_split           =       1;
694                                joint.m_drift           =       depth*res.normal;
695                                joint.m_normal          =       res.normal;
696                                joint.m_delete          =       false;
697                                joint.m_friction        =       fv.length2()<(-rvac*friction)?1:friction;
698                                joint.m_massmatrix      =       ImpulseMatrix(  ba.invMass(),ba.invWorldInertia(),joint.m_rpos[0],
699                                        bb.invMass(),bb.invWorldInertia(),joint.m_rpos[1]);
700                                return(true);
701                        }
702                        return(false);
703                }
704        };
705        //
706        // CollideCL_RS
707        //
708        struct  CollideCL_RS : ClusterBase
709        {
710                btSoftBody*             psb;
711                btRigidBody*    prb;
712                void            Process(const btDbvtNode* leaf)
713                {
714                        btSoftBody::Cluster*            cluster=(btSoftBody::Cluster*)leaf->data;
715                        btSoftClusterCollisionShape     cshape(cluster);
716                        const btConvexShape*            rshape=(const btConvexShape*)prb->getCollisionShape();
717                        btGjkEpaSolver2::sResults       res;           
718                        if(btGjkEpaSolver2::SignedDistance(     &cshape,btTransform::getIdentity(),
719                                rshape,prb->getInterpolationWorldTransform(),
720                                btVector3(1,0,0),res))
721                        {
722                                btSoftBody::CJoint      joint;
723                                if(SolveContact(res,cluster,prb,joint))
724                                {
725                                        btSoftBody::CJoint*     pj=new(btAlignedAlloc(sizeof(btSoftBody::CJoint),16)) btSoftBody::CJoint();
726                                        *pj=joint;psb->m_joints.push_back(pj);
727                                        if(prb->isStaticOrKinematicObject())
728                                        {
729                                                pj->m_erp       *=      psb->m_cfg.kSKHR_CL;
730                                                pj->m_split     *=      psb->m_cfg.kSK_SPLT_CL;
731                                        }
732                                        else
733                                        {
734                                                pj->m_erp       *=      psb->m_cfg.kSRHR_CL;
735                                                pj->m_split     *=      psb->m_cfg.kSR_SPLT_CL;
736                                        }
737                                }
738                        }
739                }
740                void            Process(btSoftBody* ps,btRigidBody* pr)
741                {
742                        psb                     =       ps;
743                        prb                     =       pr;
744                        idt                     =       ps->m_sst.isdt;
745                        margin          =       ps->getCollisionShape()->getMargin()+
746                                pr->getCollisionShape()->getMargin();
747                        friction        =       btMin(psb->m_cfg.kDF,prb->getFriction());
748                        btVector3                       mins;
749                        btVector3                       maxs;
750
751                        ATTRIBUTE_ALIGNED16(btDbvtVolume)               volume;
752                        pr->getCollisionShape()->getAabb(pr->getInterpolationWorldTransform(),mins,maxs);
753                        volume=btDbvtVolume::FromMM(mins,maxs);
754                        volume.Expand(btVector3(1,1,1)*margin);
755                        btDbvt::collideTV(ps->m_cdbvt.m_root,volume,*this);
756                }       
757        };
758        //
759        // CollideCL_SS
760        //
761        struct  CollideCL_SS : ClusterBase
762        {
763                btSoftBody*     bodies[2];
764                void            Process(const btDbvtNode* la,const btDbvtNode* lb)
765                {
766                        btSoftBody::Cluster*            cla=(btSoftBody::Cluster*)la->data;
767                        btSoftBody::Cluster*            clb=(btSoftBody::Cluster*)lb->data;
768                        btSoftClusterCollisionShape     csa(cla);
769                        btSoftClusterCollisionShape     csb(clb);
770                        btGjkEpaSolver2::sResults       res;           
771                        if(btGjkEpaSolver2::SignedDistance(     &csa,btTransform::getIdentity(),
772                                &csb,btTransform::getIdentity(),
773                                cla->m_com-clb->m_com,res))
774                        {
775                                btSoftBody::CJoint      joint;
776                                if(SolveContact(res,cla,clb,joint))
777                                {
778                                        btSoftBody::CJoint*     pj=new(btAlignedAlloc(sizeof(btSoftBody::CJoint),16)) btSoftBody::CJoint();
779                                        *pj=joint;bodies[0]->m_joints.push_back(pj);
780                                        pj->m_erp       *=      btMax(bodies[0]->m_cfg.kSSHR_CL,bodies[1]->m_cfg.kSSHR_CL);
781                                        pj->m_split     *=      (bodies[0]->m_cfg.kSS_SPLT_CL+bodies[1]->m_cfg.kSS_SPLT_CL)/2;
782                                }
783                        }
784                }
785                void            Process(btSoftBody* psa,btSoftBody* psb)
786                {
787                        idt                     =       psa->m_sst.isdt;
788                        margin          =       (psa->getCollisionShape()->getMargin()+psb->getCollisionShape()->getMargin())/2;
789                        friction        =       btMin(psa->m_cfg.kDF,psb->m_cfg.kDF);
790                        bodies[0]       =       psa;
791                        bodies[1]       =       psb;
792                        btDbvt::collideTT(psa->m_cdbvt.m_root,psb->m_cdbvt.m_root,*this);
793                }       
794        };
795        //
796        // CollideSDF_RS
797        //
798        struct  CollideSDF_RS : btDbvt::ICollide
799        {
800                void            Process(const btDbvtNode* leaf)
801                {
802                        btSoftBody::Node*       node=(btSoftBody::Node*)leaf->data;
803                        DoNode(*node);
804                }
805                void            DoNode(btSoftBody::Node& n) const
806                {
807                        const btScalar                  m=n.m_im>0?dynmargin:stamargin;
808                        btSoftBody::RContact    c;
809                        if(     (!n.m_battach)&&
810                                psb->checkContact(prb,n.m_x,m,c.m_cti))
811                        {
812                                const btScalar  ima=n.m_im;
813                                const btScalar  imb=prb->getInvMass();
814                                const btScalar  ms=ima+imb;
815                                if(ms>0)
816                                {
817                                        const btTransform&      wtr=prb->getInterpolationWorldTransform();
818                                        const btMatrix3x3&      iwi=prb->getInvInertiaTensorWorld();
819                                        const btVector3         ra=n.m_x-wtr.getOrigin();
820                                        const btVector3         va=prb->getVelocityInLocalPoint(ra)*psb->m_sst.sdt;
821                                        const btVector3         vb=n.m_x-n.m_q; 
822                                        const btVector3         vr=vb-va;
823                                        const btScalar          dn=dot(vr,c.m_cti.m_normal);
824                                        const btVector3         fv=vr-c.m_cti.m_normal*dn;
825                                        const btScalar          fc=psb->m_cfg.kDF*prb->getFriction();
826                                        c.m_node        =       &n;
827                                        c.m_c0          =       ImpulseMatrix(psb->m_sst.sdt,ima,imb,iwi,ra);
828                                        c.m_c1          =       ra;
829                                        c.m_c2          =       ima*psb->m_sst.sdt;
830                                        c.m_c3          =       fv.length2()<(btFabs(dn)*fc)?0:1-fc;
831                                        c.m_c4          =       prb->isStaticOrKinematicObject()?psb->m_cfg.kKHR:psb->m_cfg.kCHR;
832                                        psb->m_rcontacts.push_back(c);
833                                        prb->activate();
834                                }
835                        }
836                }
837                btSoftBody*             psb;
838                btRigidBody*    prb;
839                btScalar                dynmargin;
840                btScalar                stamargin;
841        };
842        //
843        // CollideVF_SS
844        //
845        struct  CollideVF_SS : btDbvt::ICollide
846        {
847                void            Process(const btDbvtNode* lnode,
848                        const btDbvtNode* lface)
849                {
850                        btSoftBody::Node*       node=(btSoftBody::Node*)lnode->data;
851                        btSoftBody::Face*       face=(btSoftBody::Face*)lface->data;
852                        btVector3                       o=node->m_x;
853                        btVector3                       p;
854                        btScalar                        d=SIMD_INFINITY;
855                        ProjectOrigin(  face->m_n[0]->m_x-o,
856                                face->m_n[1]->m_x-o,
857                                face->m_n[2]->m_x-o,
858                                p,d);
859                        const btScalar  m=mrg+(o-node->m_q).length()*2;
860                        if(d<(m*m))
861                        {
862                                const btSoftBody::Node* n[]={face->m_n[0],face->m_n[1],face->m_n[2]};
863                                const btVector3                 w=BaryCoord(n[0]->m_x,n[1]->m_x,n[2]->m_x,p+o);
864                                const btScalar                  ma=node->m_im;
865                                btScalar                                mb=BaryEval(n[0]->m_im,n[1]->m_im,n[2]->m_im,w);
866                                if(     (n[0]->m_im<=0)||
867                                        (n[1]->m_im<=0)||
868                                        (n[2]->m_im<=0))
869                                {
870                                        mb=0;
871                                }
872                                const btScalar  ms=ma+mb;
873                                if(ms>0)
874                                {
875                                        btSoftBody::SContact    c;
876                                        c.m_normal              =       p/-btSqrt(d);
877                                        c.m_margin              =       m;
878                                        c.m_node                =       node;
879                                        c.m_face                =       face;
880                                        c.m_weights             =       w;
881                                        c.m_friction    =       btMax(psb[0]->m_cfg.kDF,psb[1]->m_cfg.kDF);
882                                        c.m_cfm[0]              =       ma/ms*psb[0]->m_cfg.kSHR;
883                                        c.m_cfm[1]              =       mb/ms*psb[1]->m_cfg.kSHR;
884                                        psb[0]->m_scontacts.push_back(c);
885                                }
886                        }       
887                }
888                btSoftBody*             psb[2];
889                btScalar                mrg;
890        };
891};
892
893#endif //_BT_SOFT_BODY_INTERNALS_H
Note: See TracBrowser for help on using the repository browser.