Planet
navi homePPSaboutscreenshotsdownloaddevelopmentforum

source: code/branches/physics_new/src/bullet/BulletSoftBody/btSoftBodyInternals.h @ 2119

Last change on this file since 2119 was 2119, checked in by rgrieder, 15 years ago

Merged physics branch into physics_new branch.
  • Property svn:eol-style set to native
File size: 22.6 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); }
36void                                    resize(int n,const T& init=T())                 { dim=n;store.resize((n*(n+1))/2,init); }
37int                                             index(int c,int r) const                                { if(c>r) btSwap(c,r);btAssert(r<dim);return((r*(r+1))/2+c); }
38T&                                              operator()(int c,int r)                                 { return(store[index(c,r)]); }
39const T&                                operator()(int c,int r) const                   { return(store[index(c,r)]); }
40btAlignedObjectArray<T> store;
41int                                             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{
174static const T  zerodummy;
175value=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{
198btMatrix3x3     r;
199r[0]=Lerp(a[0],b[0],t);
200r[1]=Lerp(a[1],b[1],t);
201r[2]=Lerp(a[2],b[2],t);
202return(r);
203}
204//
205static inline btVector3         Clamp(const btVector3& v,btScalar maxlength)
206{
207const btScalar sql=v.length2();
208if(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{
236const btVector3 d=x-y;
237return(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{
299m[2]=cross(m[0],m[1]).normalized();
300m[1]=cross(m[2],m[0]).normalized();
301m[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{
324return(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{
331return(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{
353const btVector3 d=b-a;
354const btScalar  m2=d.length2();
355if(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{
374const btVector3&        q=cross(b-a,c-a);
375const btScalar          m2=q.length2();
376if(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{
416const btScalar  w[]={   cross(a-p,b-p).length(),
417                                                cross(b-p,c-p).length(),
418                                                cross(c-p,a-p).length()};
419const btScalar  isum=1/(w[0]+w[1]+w[2]);
420return(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{
430btScalar        span[2]={0,1};
431btScalar        values[2]={fn->Eval(a),fn->Eval(b)};
432if(values[0]>values[1])
433        {
434        btSwap(span[0],span[1]);
435        btSwap(values[0],values[1]);
436        }
437if(values[0]>-accuracy) return(-1);
438if(values[1]<+accuracy) return(-1);
439for(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        }
450return(-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{
467const btVector3*        pts[]={ &f.m_n[0]->m_x,
468                                                        &f.m_n[1]->m_x,
469                                                        &f.m_n[2]->m_x};
470btDbvtVolume            vol=btDbvtVolume::FromPoints(pts,3);
471vol.Expand(btVector3(margin,margin,margin));
472return(vol);
473}
474
475//
476static inline btVector3                 CenterOf(       const btSoftBody::Face& f)
477{
478return((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{
546if((a==ma)&&(b==mb)) return(0);
547if((a==mb)&&(b==ma)) return(1);
548return(-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{
558static 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:
591static 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        }
600static 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        }
642return(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.