Planet
navi homePPSaboutscreenshotsdownloaddevelopmentforum

source: code/branches/physics/src/bullet/BulletSoftBody/btSoftBodyHelpers.cpp @ 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.9 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///btSoftBodyHelpers.cpp by Nathanael Presson
16
17#include "btSoftBodyInternals.h"
18#include <stdio.h>
19#include <string.h>
20#include "btSoftBodyHelpers.h"
21#include "LinearMath/btConvexHull.h"
22
23//
24static void                             drawVertex(     btIDebugDraw* idraw,
25                                                                   const btVector3& x,btScalar s,const btVector3& c)
26{
27        idraw->drawLine(x-btVector3(s,0,0),x+btVector3(s,0,0),c);
28        idraw->drawLine(x-btVector3(0,s,0),x+btVector3(0,s,0),c);
29        idraw->drawLine(x-btVector3(0,0,s),x+btVector3(0,0,s),c);
30}
31
32//
33static void                             drawBox(        btIDebugDraw* idraw,
34                                                                const btVector3& mins,
35                                                                const btVector3& maxs,
36                                                                const btVector3& color)
37{
38        const btVector3 c[]={   btVector3(mins.x(),mins.y(),mins.z()),
39                btVector3(maxs.x(),mins.y(),mins.z()),
40                btVector3(maxs.x(),maxs.y(),mins.z()),
41                btVector3(mins.x(),maxs.y(),mins.z()),
42                btVector3(mins.x(),mins.y(),maxs.z()),
43                btVector3(maxs.x(),mins.y(),maxs.z()),
44                btVector3(maxs.x(),maxs.y(),maxs.z()),
45                btVector3(mins.x(),maxs.y(),maxs.z())};
46        idraw->drawLine(c[0],c[1],color);idraw->drawLine(c[1],c[2],color);
47        idraw->drawLine(c[2],c[3],color);idraw->drawLine(c[3],c[0],color);
48        idraw->drawLine(c[4],c[5],color);idraw->drawLine(c[5],c[6],color);
49        idraw->drawLine(c[6],c[7],color);idraw->drawLine(c[7],c[4],color);
50        idraw->drawLine(c[0],c[4],color);idraw->drawLine(c[1],c[5],color);
51        idraw->drawLine(c[2],c[6],color);idraw->drawLine(c[3],c[7],color);
52}
53
54//
55static void                             drawTree(       btIDebugDraw* idraw,
56                                                                 const btDbvtNode* node,
57                                                                 int depth,
58                                                                 const btVector3& ncolor,
59                                                                 const btVector3& lcolor,
60                                                                 int mindepth,
61                                                                 int maxdepth)
62{
63        if(node)
64        {
65                if(node->isinternal()&&((depth<maxdepth)||(maxdepth<0)))
66                {
67                        drawTree(idraw,node->childs[0],depth+1,ncolor,lcolor,mindepth,maxdepth);
68                        drawTree(idraw,node->childs[1],depth+1,ncolor,lcolor,mindepth,maxdepth);
69                }
70                if(depth>=mindepth)
71                {
72                        const btScalar  scl=(btScalar)(node->isinternal()?1:1);
73                        const btVector3 mi=node->volume.Center()-node->volume.Extents()*scl;
74                        const btVector3 mx=node->volume.Center()+node->volume.Extents()*scl;
75                        drawBox(idraw,mi,mx,node->isleaf()?lcolor:ncolor);
76                }
77        }
78}
79
80//
81template <typename T>
82static inline T                         sum(const btAlignedObjectArray<T>& items)
83{
84        T       v;
85        if(items.size())
86        {
87                v=items[0];
88                for(int i=1,ni=items.size();i<ni;++i)
89                {
90                        v+=items[i];
91                }
92        }
93        return(v);
94}
95
96//
97template <typename T,typename Q>
98static inline void                      add(btAlignedObjectArray<T>& items,const Q& value)
99{
100        for(int i=0,ni=items.size();i<ni;++i)
101        {
102                items[i]+=value;
103        }
104}
105
106//
107template <typename T,typename Q>
108static inline void                      mul(btAlignedObjectArray<T>& items,const Q& value)
109{
110        for(int i=0,ni=items.size();i<ni;++i)
111        {
112                items[i]*=value;
113        }
114}
115
116//
117template <typename T>
118static inline T                         average(const btAlignedObjectArray<T>& items)
119{
120        const btScalar  n=(btScalar)(items.size()>0?items.size():1);
121        return(sum(items)/n);
122}
123
124//
125static inline btScalar          tetravolume(const btVector3& x0,
126                                                                                const btVector3& x1,
127                                                                                const btVector3& x2,
128                                                                                const btVector3& x3)
129{
130        const btVector3 a=x1-x0;
131        const btVector3 b=x2-x0;
132        const btVector3 c=x3-x0;
133        return(dot(a,cross(b,c)));
134}
135
136//
137#if 0
138static btVector3                stresscolor(btScalar stress)
139{
140        static const btVector3  spectrum[]=     {       btVector3(1,0,1),
141                btVector3(0,0,1),
142                btVector3(0,1,1),
143                btVector3(0,1,0),
144                btVector3(1,1,0),
145                btVector3(1,0,0),
146                btVector3(1,0,0)};
147        static const int                ncolors=sizeof(spectrum)/sizeof(spectrum[0])-1;
148        static const btScalar   one=1;
149        stress=btMax<btScalar>(0,btMin<btScalar>(1,stress))*ncolors;
150        const int                               sel=(int)stress;
151        const btScalar                  frc=stress-sel;
152        return(spectrum[sel]+(spectrum[sel+1]-spectrum[sel])*frc);
153}
154#endif
155
156//
157void                    btSoftBodyHelpers::Draw(        btSoftBody* psb,
158                                                                                btIDebugDraw* idraw,
159                                                                                int drawflags)
160{
161        const btScalar          scl=(btScalar)0.1;
162        const btScalar          nscl=scl*5;
163        const btVector3         lcolor=btVector3(0,0,0);
164        const btVector3         ncolor=btVector3(1,1,1);
165        const btVector3         ccolor=btVector3(1,0,0);
166        int i,j,nj;
167
168        /* Nodes        */ 
169        if(0!=(drawflags&fDrawFlags::Nodes))
170        {
171                for(i=0;i<psb->m_nodes.size();++i)
172                {
173                        const btSoftBody::Node& n=psb->m_nodes[i];
174                        if(0==(n.m_material->m_flags&btSoftBody::fMaterial::DebugDraw)) continue;
175                        idraw->drawLine(n.m_x-btVector3(scl,0,0),n.m_x+btVector3(scl,0,0),btVector3(1,0,0));
176                        idraw->drawLine(n.m_x-btVector3(0,scl,0),n.m_x+btVector3(0,scl,0),btVector3(0,1,0));
177                        idraw->drawLine(n.m_x-btVector3(0,0,scl),n.m_x+btVector3(0,0,scl),btVector3(0,0,1));
178                }
179        }
180        /* Links        */ 
181        if(0!=(drawflags&fDrawFlags::Links))
182        {
183                for(i=0;i<psb->m_links.size();++i)
184                {
185                        const btSoftBody::Link& l=psb->m_links[i];
186                        if(0==(l.m_material->m_flags&btSoftBody::fMaterial::DebugDraw)) continue;
187                        idraw->drawLine(l.m_n[0]->m_x,l.m_n[1]->m_x,lcolor);
188                }
189        }
190        /* Normals      */ 
191        if(0!=(drawflags&fDrawFlags::Normals))
192        {
193                for(i=0;i<psb->m_nodes.size();++i)
194                {
195                        const btSoftBody::Node& n=psb->m_nodes[i];
196                        if(0==(n.m_material->m_flags&btSoftBody::fMaterial::DebugDraw)) continue;
197                        const btVector3                 d=n.m_n*nscl;
198                        idraw->drawLine(n.m_x,n.m_x+d,ncolor);
199                        idraw->drawLine(n.m_x,n.m_x-d,ncolor*0.5);
200                }
201        }
202        /* Contacts     */ 
203        if(0!=(drawflags&fDrawFlags::Contacts))
204        {
205                static const btVector3          axis[]={btVector3(1,0,0),
206                        btVector3(0,1,0),
207                        btVector3(0,0,1)};
208                for(i=0;i<psb->m_rcontacts.size();++i)
209                {               
210                        const btSoftBody::RContact&     c=psb->m_rcontacts[i];
211                        const btVector3                         o=      c.m_node->m_x-c.m_cti.m_normal*
212                                (dot(c.m_node->m_x,c.m_cti.m_normal)+c.m_cti.m_offset);
213                        const btVector3                         x=cross(c.m_cti.m_normal,axis[c.m_cti.m_normal.minAxis()]).normalized();
214                        const btVector3                         y=cross(x,c.m_cti.m_normal).normalized();
215                        idraw->drawLine(o-x*nscl,o+x*nscl,ccolor);
216                        idraw->drawLine(o-y*nscl,o+y*nscl,ccolor);
217                        idraw->drawLine(o,o+c.m_cti.m_normal*nscl*3,btVector3(1,1,0));
218                }
219        }
220        /* Anchors      */ 
221        if(0!=(drawflags&fDrawFlags::Anchors))
222        {
223                for(i=0;i<psb->m_anchors.size();++i)
224                {
225                        const btSoftBody::Anchor&       a=psb->m_anchors[i];
226                        const btVector3                         q=a.m_body->getWorldTransform()*a.m_local;
227                        drawVertex(idraw,a.m_node->m_x,0.25,btVector3(1,0,0));
228                        drawVertex(idraw,q,0.25,btVector3(0,1,0));
229                        idraw->drawLine(a.m_node->m_x,q,btVector3(1,1,1));
230                }
231                for(i=0;i<psb->m_nodes.size();++i)
232                {
233                        const btSoftBody::Node& n=psb->m_nodes[i];             
234                        if(0==(n.m_material->m_flags&btSoftBody::fMaterial::DebugDraw)) continue;
235                        if(n.m_im<=0)
236                        {
237                                drawVertex(idraw,n.m_x,0.25,btVector3(1,0,0));
238                        }
239                }
240        }
241        /* Faces        */ 
242        if(0!=(drawflags&fDrawFlags::Faces))
243        {
244                const btScalar  scl=(btScalar)0.8;
245                const btScalar  alp=(btScalar)1;
246                const btVector3 col(0,(btScalar)0.7,0);
247                for(i=0;i<psb->m_faces.size();++i)
248                {
249                        const btSoftBody::Face& f=psb->m_faces[i];
250                        if(0==(f.m_material->m_flags&btSoftBody::fMaterial::DebugDraw)) continue;
251                        const btVector3                 x[]={f.m_n[0]->m_x,f.m_n[1]->m_x,f.m_n[2]->m_x};
252                        const btVector3                 c=(x[0]+x[1]+x[2])/3;
253                        idraw->drawTriangle((x[0]-c)*scl+c,
254                                (x[1]-c)*scl+c,
255                                (x[2]-c)*scl+c,
256                                col,alp);
257                }       
258        }
259        /* Clusters     */ 
260        if(0!=(drawflags&fDrawFlags::Clusters))
261        {
262                srand(1806);
263                for(i=0;i<psb->m_clusters.size();++i)
264                {
265                        if(psb->m_clusters[i]->m_collide)
266                        {
267                                btVector3                                               color(  rand()/(btScalar)RAND_MAX,
268                                        rand()/(btScalar)RAND_MAX,
269                                        rand()/(btScalar)RAND_MAX);
270                                color=color.normalized()*0.75;
271                                btAlignedObjectArray<btVector3> vertices;
272                                vertices.resize(psb->m_clusters[i]->m_nodes.size());
273                                for(j=0,nj=vertices.size();j<nj;++j)
274                                {                               
275                                        vertices[j]=psb->m_clusters[i]->m_nodes[j]->m_x;
276                                }
277                                HullDesc                hdsc(QF_TRIANGLES,vertices.size(),&vertices[0]);
278                                HullResult              hres;
279                                HullLibrary             hlib;
280                                hdsc.mMaxVertices=vertices.size();
281                                hlib.CreateConvexHull(hdsc,hres);
282                                const btVector3 center=average(hres.m_OutputVertices);
283                                add(hres.m_OutputVertices,-center);
284                                mul(hres.m_OutputVertices,(btScalar)1);
285                                add(hres.m_OutputVertices,center);
286                                for(j=0;j<(int)hres.mNumFaces;++j)
287                                {
288                                        const int idx[]={hres.m_Indices[j*3+0],hres.m_Indices[j*3+1],hres.m_Indices[j*3+2]};
289                                        idraw->drawTriangle(hres.m_OutputVertices[idx[0]],
290                                                hres.m_OutputVertices[idx[1]],
291                                                hres.m_OutputVertices[idx[2]],
292                                                color,1);
293                                }
294                                hlib.ReleaseResult(hres);
295                        }
296                        /* Velocities   */ 
297#if 0
298                        for(int j=0;j<psb->m_clusters[i].m_nodes.size();++j)
299                        {
300                                const btSoftBody::Cluster&      c=psb->m_clusters[i];
301                                const btVector3                         r=c.m_nodes[j]->m_x-c.m_com;
302                                const btVector3                         v=c.m_lv+cross(c.m_av,r);
303                                idraw->drawLine(c.m_nodes[j]->m_x,c.m_nodes[j]->m_x+v,btVector3(1,0,0));
304                        }
305#endif
306                        /* Frame                */ 
307                        btSoftBody::Cluster& c=*psb->m_clusters[i];
308                        idraw->drawLine(c.m_com,c.m_framexform*btVector3(10,0,0),btVector3(1,0,0));
309                        idraw->drawLine(c.m_com,c.m_framexform*btVector3(0,10,0),btVector3(0,1,0));
310                        idraw->drawLine(c.m_com,c.m_framexform*btVector3(0,0,10),btVector3(0,0,1));
311                }
312        }
313        /* Notes        */ 
314        if(0!=(drawflags&fDrawFlags::Notes))
315        {
316                for(i=0;i<psb->m_notes.size();++i)
317                {
318                        const btSoftBody::Note& n=psb->m_notes[i];
319                        btVector3                               p=n.m_offset;
320                        for(int j=0;j<n.m_rank;++j)
321                        {
322                                p+=n.m_nodes[j]->m_x*n.m_coords[j];
323                        }
324                        idraw->draw3dText(p,n.m_text);
325                }
326        }
327        /* Node tree    */ 
328        if(0!=(drawflags&fDrawFlags::NodeTree))         DrawNodeTree(psb,idraw);
329        /* Face tree    */ 
330        if(0!=(drawflags&fDrawFlags::FaceTree))         DrawFaceTree(psb,idraw);
331        /* Cluster tree */ 
332        if(0!=(drawflags&fDrawFlags::ClusterTree))      DrawClusterTree(psb,idraw);
333        /* Joints               */ 
334        if(0!=(drawflags&fDrawFlags::Joints))
335        {
336                for(i=0;i<psb->m_joints.size();++i)
337                {
338                        const btSoftBody::Joint*        pj=psb->m_joints[i];
339                        switch(pj->Type())
340                        {
341                        case    btSoftBody::Joint::eType::Linear:
342                                {
343                                        const btSoftBody::LJoint*       pjl=(const btSoftBody::LJoint*)pj;
344                                        const btVector3 a0=pj->m_bodies[0].xform()*pjl->m_refs[0];
345                                        const btVector3 a1=pj->m_bodies[1].xform()*pjl->m_refs[1];
346                                        idraw->drawLine(pj->m_bodies[0].xform().getOrigin(),a0,btVector3(1,1,0));
347                                        idraw->drawLine(pj->m_bodies[1].xform().getOrigin(),a1,btVector3(0,1,1));
348                                        drawVertex(idraw,a0,0.25,btVector3(1,1,0));
349                                        drawVertex(idraw,a1,0.25,btVector3(0,1,1));
350                                }
351                                break;
352                        case    btSoftBody::Joint::eType::Angular:
353                                {
354                                        const btSoftBody::AJoint*       pja=(const btSoftBody::AJoint*)pj;
355                                        const btVector3 o0=pj->m_bodies[0].xform().getOrigin();
356                                        const btVector3 o1=pj->m_bodies[1].xform().getOrigin();
357                                        const btVector3 a0=pj->m_bodies[0].xform().getBasis()*pj->m_refs[0];
358                                        const btVector3 a1=pj->m_bodies[1].xform().getBasis()*pj->m_refs[1];
359                                        idraw->drawLine(o0,o0+a0*10,btVector3(1,1,0));
360                                        idraw->drawLine(o0,o0+a1*10,btVector3(1,1,0));
361                                        idraw->drawLine(o1,o1+a0*10,btVector3(0,1,1));
362                                        idraw->drawLine(o1,o1+a1*10,btVector3(0,1,1));
363                                }
364                        }               
365                }
366        }
367}
368
369//
370void                    btSoftBodyHelpers::DrawInfos(           btSoftBody* psb,
371                                                                                         btIDebugDraw* idraw,
372                                                                                         bool masses,
373                                                                                         bool areas,
374                                                                                         bool /*stress*/)
375{
376        for(int i=0;i<psb->m_nodes.size();++i)
377        {
378                const btSoftBody::Node& n=psb->m_nodes[i];
379                char                                    text[2048]={0};
380                char                                    buff[1024];
381                if(masses)
382                {
383                        sprintf(buff," M(%.2f)",1/n.m_im);
384                        strcat(text,buff);
385                }
386                if(areas)
387                {
388                        sprintf(buff," A(%.2f)",n.m_area);
389                        strcat(text,buff);
390                }
391                if(text[0]) idraw->draw3dText(n.m_x,text);
392        }
393}
394
395//
396void                    btSoftBodyHelpers::DrawNodeTree(        btSoftBody* psb,
397                                                                                                btIDebugDraw* idraw,
398                                                                                                int mindepth,
399                                                                                                int maxdepth)
400{
401        drawTree(idraw,psb->m_ndbvt.m_root,0,btVector3(1,0,1),btVector3(1,1,1),mindepth,maxdepth);
402}
403
404//
405void                    btSoftBodyHelpers::DrawFaceTree(        btSoftBody* psb,
406                                                                                                btIDebugDraw* idraw,
407                                                                                                int mindepth,
408                                                                                                int maxdepth)
409{
410        drawTree(idraw,psb->m_fdbvt.m_root,0,btVector3(0,1,0),btVector3(1,0,0),mindepth,maxdepth);
411}
412
413//
414void                    btSoftBodyHelpers::DrawClusterTree(     btSoftBody* psb,
415                                                                                                   btIDebugDraw* idraw,
416                                                                                                   int mindepth,
417                                                                                                   int maxdepth)
418{
419        drawTree(idraw,psb->m_cdbvt.m_root,0,btVector3(0,1,1),btVector3(1,0,0),mindepth,maxdepth);
420}
421
422//
423void                    btSoftBodyHelpers::DrawFrame(           btSoftBody* psb,
424                                                                                         btIDebugDraw* idraw)
425{
426        if(psb->m_pose.m_bframe)
427        {
428                static const btScalar   ascl=10;
429                static const btScalar   nscl=(btScalar)0.1;
430                const btVector3                 com=psb->m_pose.m_com;
431                const btMatrix3x3               trs=psb->m_pose.m_rot*psb->m_pose.m_scl;
432                const btVector3                 Xaxis=(trs*btVector3(1,0,0)).normalized();
433                const btVector3                 Yaxis=(trs*btVector3(0,1,0)).normalized();
434                const btVector3                 Zaxis=(trs*btVector3(0,0,1)).normalized();
435                idraw->drawLine(com,com+Xaxis*ascl,btVector3(1,0,0));
436                idraw->drawLine(com,com+Yaxis*ascl,btVector3(0,1,0));
437                idraw->drawLine(com,com+Zaxis*ascl,btVector3(0,0,1));
438                for(int i=0;i<psb->m_pose.m_pos.size();++i)
439                {
440                        const btVector3 x=com+trs*psb->m_pose.m_pos[i];
441                        drawVertex(idraw,x,nscl,btVector3(1,0,1));
442                }
443        }
444}
445
446//
447btSoftBody*             btSoftBodyHelpers::CreateRope(  btSoftBodyWorldInfo& worldInfo, const btVector3& from,
448                                                                                          const btVector3& to,
449                                                                                          int res,
450                                                                                          int fixeds)
451{
452        /* Create nodes */ 
453        const int               r=res+2;
454        btVector3*              x=new btVector3[r];
455        btScalar*               m=new btScalar[r];
456        int i;
457
458        for(i=0;i<r;++i)
459        {
460                const btScalar  t=i/(btScalar)(r-1);
461                x[i]=lerp(from,to,t);
462                m[i]=1;
463        }
464        btSoftBody*             psb= new btSoftBody(&worldInfo,r,x,m);
465        if(fixeds&1) psb->setMass(0,0);
466        if(fixeds&2) psb->setMass(r-1,0);
467        delete[] x;
468        delete[] m;
469        /* Create links */ 
470        for(i=1;i<r;++i)
471        {
472                psb->appendLink(i-1,i);
473        }
474        /* Finished             */ 
475        return(psb);
476}
477
478//
479btSoftBody*             btSoftBodyHelpers::CreatePatch(btSoftBodyWorldInfo& worldInfo,const btVector3& corner00,
480                                                                                           const btVector3& corner10,
481                                                                                           const btVector3& corner01,
482                                                                                           const btVector3& corner11,
483                                                                                           int resx,
484                                                                                           int resy,
485                                                                                           int fixeds,
486                                                                                           bool gendiags)
487{
488#define IDX(_x_,_y_)    ((_y_)*rx+(_x_))
489        /* Create nodes */ 
490        if((resx<2)||(resy<2)) return(0);
491        const int       rx=resx;
492        const int       ry=resy;
493        const int       tot=rx*ry;
494        btVector3*      x=new btVector3[tot];
495        btScalar*       m=new btScalar[tot];
496        int iy;
497
498        for(iy=0;iy<ry;++iy)
499        {
500                const btScalar  ty=iy/(btScalar)(ry-1);
501                const btVector3 py0=lerp(corner00,corner01,ty);
502                const btVector3 py1=lerp(corner10,corner11,ty);
503                for(int ix=0;ix<rx;++ix)
504                {
505                        const btScalar  tx=ix/(btScalar)(rx-1);
506                        x[IDX(ix,iy)]=lerp(py0,py1,tx);
507                        m[IDX(ix,iy)]=1;
508                }
509        }
510        btSoftBody*             psb=new btSoftBody(&worldInfo,tot,x,m);
511        if(fixeds&1)    psb->setMass(IDX(0,0),0);
512        if(fixeds&2)    psb->setMass(IDX(rx-1,0),0);
513        if(fixeds&4)    psb->setMass(IDX(0,ry-1),0);
514        if(fixeds&8)    psb->setMass(IDX(rx-1,ry-1),0);
515        delete[] x;
516        delete[] m;
517        /* Create links and faces */ 
518        for(iy=0;iy<ry;++iy)
519        {
520                for(int ix=0;ix<rx;++ix)
521                {
522                        const int       idx=IDX(ix,iy);
523                        const bool      mdx=(ix+1)<rx;
524                        const bool      mdy=(iy+1)<ry;
525                        if(mdx) psb->appendLink(idx,IDX(ix+1,iy));
526                        if(mdy) psb->appendLink(idx,IDX(ix,iy+1));
527                        if(mdx&&mdy)
528                        {
529                                if((ix+iy)&1)
530                                {
531                                        psb->appendFace(IDX(ix,iy),IDX(ix+1,iy),IDX(ix+1,iy+1));
532                                        psb->appendFace(IDX(ix,iy),IDX(ix+1,iy+1),IDX(ix,iy+1));
533                                        if(gendiags)
534                                        {
535                                                psb->appendLink(IDX(ix,iy),IDX(ix+1,iy+1));
536                                        }
537                                }
538                                else
539                                {
540                                        psb->appendFace(IDX(ix,iy+1),IDX(ix,iy),IDX(ix+1,iy));
541                                        psb->appendFace(IDX(ix,iy+1),IDX(ix+1,iy),IDX(ix+1,iy+1));
542                                        if(gendiags)
543                                        {
544                                                psb->appendLink(IDX(ix+1,iy),IDX(ix,iy+1));
545                                        }
546                                }
547                        }
548                }
549        }
550        /* Finished             */ 
551#undef IDX
552        return(psb);
553}
554
555//
556btSoftBody*             btSoftBodyHelpers::CreatePatchUV(btSoftBodyWorldInfo& worldInfo,
557                                                                                                 const btVector3& corner00,
558                                                                                                 const btVector3& corner10,
559                                                                                                 const btVector3& corner01,
560                                                                                                 const btVector3& corner11,
561                                                                                                 int resx,
562                                                                                                 int resy,
563                                                                                                 int fixeds,
564                                                                                                 bool gendiags,
565                                                                                                 float* tex_coords)
566{
567
568        /*
569        *
570        *  corners:
571        *
572        *  [0][0]     corner00 ------- corner01   [resx][0]
573        *                |                |
574        *                |                |
575        *  [0][resy]  corner10 -------- corner11  [resx][resy]
576        *
577        *
578        *
579        *
580        *
581        *
582        *   "fixedgs" map:
583        *
584        *  corner00     -->   +1
585        *  corner01     -->   +2
586        *  corner10     -->   +4
587        *  corner11     -->   +8
588        *  upper middle -->  +16
589        *  left middle  -->  +32
590        *  right middle -->  +64
591        *  lower middle --> +128
592        *  center       --> +256
593        *
594        *
595        *   tex_coords size   (resx-1)*(resy-1)*12
596        *
597        *
598        *
599        *     SINGLE QUAD INTERNALS
600        *
601        *  1) btSoftBody's nodes and links,
602        *     diagonal link is optional ("gendiags")
603        *
604        *
605        *    node00 ------ node01
606        *      | .             
607        *      |   .           
608        *      |     .         
609        *      |       .       
610        *      |         .     
611        *    node10        node11
612        *
613        *
614        *
615        *   2) Faces:
616        *      two triangles,
617        *      UV Coordinates (hier example for single quad)
618        *     
619        *     (0,1)          (0,1)  (1,1)
620        *     1 |\            3 \-----| 2
621        *       | \              \    |
622        *       |  \              \   |
623        *       |   \              \  |
624        *       |    \              \ |
625        *     2 |-----\ 3            \| 1
626        *     (0,0)    (1,0)       (1,0)
627        *
628        *
629        *
630        *
631        *
632        *
633        */
634
635#define IDX(_x_,_y_)    ((_y_)*rx+(_x_))
636        /* Create nodes         */ 
637        if((resx<2)||(resy<2)) return(0);
638        const int       rx=resx;
639        const int       ry=resy;
640        const int       tot=rx*ry;
641        btVector3*      x=new btVector3[tot];
642        btScalar*       m=new btScalar[tot];
643
644        for(int iy=0;iy<ry;++iy)
645        {
646                const btScalar  ty=iy/(btScalar)(ry-1);
647                const btVector3 py0=lerp(corner00,corner01,ty);
648                const btVector3 py1=lerp(corner10,corner11,ty);
649                for(int ix=0;ix<rx;++ix)
650                {
651                        const btScalar  tx=ix/(btScalar)(rx-1);
652                        x[IDX(ix,iy)]=lerp(py0,py1,tx);
653                        m[IDX(ix,iy)]=1;
654                }
655        }
656        btSoftBody*     psb=new btSoftBody(&worldInfo,tot,x,m);
657        if(fixeds&1)            psb->setMass(IDX(0,0),0);
658        if(fixeds&2)            psb->setMass(IDX(rx-1,0),0);
659        if(fixeds&4)            psb->setMass(IDX(0,ry-1),0);
660        if(fixeds&8)            psb->setMass(IDX(rx-1,ry-1),0);
661        if(fixeds&16)           psb->setMass(IDX((rx-1)/2,0),0);
662        if(fixeds&32)           psb->setMass(IDX(0,(ry-1)/2),0);
663        if(fixeds&64)           psb->setMass(IDX(rx-1,(ry-1)/2),0);
664        if(fixeds&128)          psb->setMass(IDX((rx-1)/2,ry-1),0);
665        if(fixeds&256)          psb->setMass(IDX((rx-1)/2,(ry-1)/2),0);
666        delete[] x;
667        delete[] m;
668
669
670        int z = 0;
671        /* Create links and faces       */ 
672        for(int iy=0;iy<ry;++iy)
673        {
674                for(int ix=0;ix<rx;++ix)
675                {
676                        const bool      mdx=(ix+1)<rx;
677                        const bool      mdy=(iy+1)<ry;
678
679                        int node00=IDX(ix,iy);
680                        int node01=IDX(ix+1,iy);
681                        int node10=IDX(ix,iy+1);
682                        int node11=IDX(ix+1,iy+1);
683
684                        if(mdx) psb->appendLink(node00,node01);
685                        if(mdy) psb->appendLink(node00,node10);
686                        if(mdx&&mdy)
687                        {
688                                psb->appendFace(node00,node10,node11);
689                                if (tex_coords) {
690                                        tex_coords[z+0]=CalculateUV(resx,resy,ix,iy,0);
691                                        tex_coords[z+1]=CalculateUV(resx,resy,ix,iy,1);
692                                        tex_coords[z+2]=CalculateUV(resx,resy,ix,iy,0);
693                                        tex_coords[z+3]=CalculateUV(resx,resy,ix,iy,2);
694                                        tex_coords[z+4]=CalculateUV(resx,resy,ix,iy,3);
695                                        tex_coords[z+5]=CalculateUV(resx,resy,ix,iy,2);
696                                }
697                                psb->appendFace(node11,node01,node00);
698                                if (tex_coords) {
699                                        tex_coords[z+6 ]=CalculateUV(resx,resy,ix,iy,3);
700                                        tex_coords[z+7 ]=CalculateUV(resx,resy,ix,iy,2);
701                                        tex_coords[z+8 ]=CalculateUV(resx,resy,ix,iy,3);
702                                        tex_coords[z+9 ]=CalculateUV(resx,resy,ix,iy,1);
703                                        tex_coords[z+10]=CalculateUV(resx,resy,ix,iy,0);
704                                        tex_coords[z+11]=CalculateUV(resx,resy,ix,iy,1);
705                                }
706                                if (gendiags) psb->appendLink(node00,node11);
707                                z += 12;
708                        }
709                }
710        }
711        /* Finished     */ 
712#undef IDX
713        return(psb);
714}
715
716float   btSoftBodyHelpers::CalculateUV(int resx,int resy,int ix,int iy,int id)
717{
718
719        /*
720        *
721        *
722        *    node00 --- node01
723        *      |          |
724        *    node10 --- node11
725        *
726        *
727        *   ID map:
728        *
729        *   node00 s --> 0
730        *   node00 t --> 1
731        *
732        *   node01 s --> 3
733        *   node01 t --> 1
734        *
735        *   node10 s --> 0
736        *   node10 t --> 2
737        *
738        *   node11 s --> 3
739        *   node11 t --> 2
740        *
741        *
742        */
743
744        float tc=0.0f;
745        if (id == 0) {
746                tc = (1.0f/((resx-1))*ix);
747        }
748        else if (id==1) {
749                tc = (1.0f/((resy-1))*(resy-1-iy));
750        }
751        else if (id==2) {
752                tc = (1.0f/((resy-1))*(resy-1-iy-1));
753        }
754        else if (id==3) {
755                tc = (1.0f/((resx-1))*(ix+1));
756        }
757        return tc;
758}
759//
760btSoftBody*             btSoftBodyHelpers::CreateEllipsoid(btSoftBodyWorldInfo& worldInfo,const btVector3& center,
761                                                                                                   const btVector3& radius,
762                                                                                                   int res)
763{
764        struct  Hammersley
765        {
766                static void     Generate(btVector3* x,int n)
767                {
768                        for(int i=0;i<n;i++)
769                        {
770                                btScalar        p=0.5,t=0;
771                                for(int j=i;j;p*=0.5,j>>=1) if(j&1) t+=p;
772                                btScalar        w=2*t-1;
773                                btScalar        a=(SIMD_PI+2*i*SIMD_PI)/n;
774                                btScalar        s=btSqrt(1-w*w);
775                                *x++=btVector3(s*btCos(a),s*btSin(a),w);
776                        }
777                }
778        };
779        btAlignedObjectArray<btVector3> vtx;
780        vtx.resize(3+res);
781        Hammersley::Generate(&vtx[0],vtx.size());
782        for(int i=0;i<vtx.size();++i)
783        {
784                vtx[i]=vtx[i]*radius+center;
785        }
786        return(CreateFromConvexHull(worldInfo,&vtx[0],vtx.size()));
787}
788
789
790
791//
792btSoftBody*             btSoftBodyHelpers::CreateFromTriMesh(btSoftBodyWorldInfo& worldInfo,const btScalar*     vertices,
793                                                                                                         const int* triangles,
794                                                                                                         int ntriangles)
795{
796        int             maxidx=0;
797        int i,j,ni;
798
799        for(i=0,ni=ntriangles*3;i<ni;++i)
800        {
801                maxidx=btMax(triangles[i],maxidx);
802        }
803        ++maxidx;
804        btAlignedObjectArray<bool>              chks;
805        btAlignedObjectArray<btVector3> vtx;
806        chks.resize(maxidx*maxidx,false);
807        vtx.resize(maxidx);
808        for(i=0,j=0,ni=maxidx*3;i<ni;++j,i+=3)
809        {
810                vtx[j]=btVector3(vertices[i],vertices[i+1],vertices[i+2]);
811        }
812        btSoftBody*             psb=new btSoftBody(&worldInfo,vtx.size(),&vtx[0],0);
813        for( i=0,ni=ntriangles*3;i<ni;i+=3)
814        {
815                const int idx[]={triangles[i],triangles[i+1],triangles[i+2]};
816#define IDX(_x_,_y_) ((_y_)*maxidx+(_x_))
817                for(int j=2,k=0;k<3;j=k++)
818                {
819                        if(!chks[IDX(idx[j],idx[k])])
820                        {
821                                chks[IDX(idx[j],idx[k])]=true;
822                                chks[IDX(idx[k],idx[k])]=true;
823                                psb->appendLink(idx[j],idx[k]);
824                        }
825                }
826#undef IDX
827                psb->appendFace(idx[0],idx[1],idx[2]);
828        }
829        psb->randomizeConstraints();
830        return(psb);
831}
832
833//
834btSoftBody*             btSoftBodyHelpers::CreateFromConvexHull(btSoftBodyWorldInfo& worldInfo, const btVector3* vertices,
835                                                                                                                int nvertices)
836{
837        HullDesc                hdsc(QF_TRIANGLES,nvertices,vertices);
838        HullResult              hres;
839        HullLibrary             hlib;/*??*/ 
840        hdsc.mMaxVertices=nvertices;
841        hlib.CreateConvexHull(hdsc,hres);
842        btSoftBody*             psb=new btSoftBody(&worldInfo,(int)hres.mNumOutputVertices,
843                &hres.m_OutputVertices[0],0);
844        for(int i=0;i<(int)hres.mNumFaces;++i)
845        {
846                const int idx[]={       hres.m_Indices[i*3+0],
847                        hres.m_Indices[i*3+1],
848                        hres.m_Indices[i*3+2]};
849                if(idx[0]<idx[1]) psb->appendLink(      idx[0],idx[1]);
850                if(idx[1]<idx[2]) psb->appendLink(      idx[1],idx[2]);
851                if(idx[2]<idx[0]) psb->appendLink(      idx[2],idx[0]);
852                psb->appendFace(idx[0],idx[1],idx[2]);
853        }
854        hlib.ReleaseResult(hres);
855        psb->randomizeConstraints();
856        return(psb);
857}
Note: See TracBrowser for help on using the repository browser.