Planet
navi homePPSaboutscreenshotsdownloaddevelopmentforum

source: code/branches/physics_new/src/bullet/BulletSoftBody/btSoftBodyHelpers.cpp @ 2119

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

Merged physics branch into physics_new branch.
  • Property svn:eol-style set to native
File size: 18.4 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{
38const 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())};
46idraw->drawLine(c[0],c[1],color);idraw->drawLine(c[1],c[2],color);
47idraw->drawLine(c[2],c[3],color);idraw->drawLine(c[3],c[0],color);
48idraw->drawLine(c[4],c[5],color);idraw->drawLine(c[5],c[6],color);
49idraw->drawLine(c[6],c[7],color);idraw->drawLine(c[7],c[4],color);
50idraw->drawLine(c[0],c[4],color);idraw->drawLine(c[1],c[5],color);
51idraw->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{
63if(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{
84T       v;
85if(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        }
93return(v);
94}
95
96//
97template <typename T,typename Q>
98static inline void                      add(btAlignedObjectArray<T>& items,const Q& value)
99{
100for(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{
110for(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{
120const btScalar  n=(btScalar)(items.size()>0?items.size():1);
121return(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{
401drawTree(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{
410drawTree(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{
419drawTree(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::CreateEllipsoid(btSoftBodyWorldInfo& worldInfo,const btVector3& center,
557                                                                const btVector3& radius,
558                                                                int res)
559{
560        struct  Hammersley
561        {
562                static void     Generate(btVector3* x,int n)
563                {
564                        for(int i=0;i<n;i++)
565                        {
566                                btScalar        p=0.5,t=0;
567                                for(int j=i;j;p*=0.5,j>>=1) if(j&1) t+=p;
568                                btScalar        w=2*t-1;
569                                btScalar        a=(SIMD_PI+2*i*SIMD_PI)/n;
570                                btScalar        s=btSqrt(1-w*w);
571                                *x++=btVector3(s*btCos(a),s*btSin(a),w);
572                        }
573                }
574        };
575        btAlignedObjectArray<btVector3> vtx;
576        vtx.resize(3+res);
577        Hammersley::Generate(&vtx[0],vtx.size());
578        for(int i=0;i<vtx.size();++i)
579        {
580                vtx[i]=vtx[i]*radius+center;
581        }
582        return(CreateFromConvexHull(worldInfo,&vtx[0],vtx.size()));
583}
584
585
586
587//
588btSoftBody*             btSoftBodyHelpers::CreateFromTriMesh(btSoftBodyWorldInfo& worldInfo,const btScalar*     vertices,
589                                                                  const int* triangles,
590                                                                  int ntriangles)
591{
592        int             maxidx=0;
593        int i,j,ni;
594
595        for(i=0,ni=ntriangles*3;i<ni;++i)
596        {
597                maxidx=btMax(triangles[i],maxidx);
598        }
599        ++maxidx;
600        btAlignedObjectArray<bool>              chks;
601        btAlignedObjectArray<btVector3> vtx;
602        chks.resize(maxidx*maxidx,false);
603        vtx.resize(maxidx);
604        for(i=0,j=0,ni=maxidx*3;i<ni;++j,i+=3)
605        {
606                vtx[j]=btVector3(vertices[i],vertices[i+1],vertices[i+2]);
607        }
608        btSoftBody*             psb=new btSoftBody(&worldInfo,vtx.size(),&vtx[0],0);
609        for( i=0,ni=ntriangles*3;i<ni;i+=3)
610        {
611                const int idx[]={triangles[i],triangles[i+1],triangles[i+2]};
612#define IDX(_x_,_y_) ((_y_)*maxidx+(_x_))
613                for(int j=2,k=0;k<3;j=k++)
614                {
615                        if(!chks[IDX(idx[j],idx[k])])
616                        {
617                                chks[IDX(idx[j],idx[k])]=true;
618                                chks[IDX(idx[k],idx[k])]=true;
619                                psb->appendLink(idx[j],idx[k]);
620                        }
621                }
622#undef IDX
623                psb->appendFace(idx[0],idx[1],idx[2]);
624        }
625        psb->randomizeConstraints();
626        return(psb);
627}
628
629//
630btSoftBody*             btSoftBodyHelpers::CreateFromConvexHull(btSoftBodyWorldInfo& worldInfo, const btVector3* vertices,
631                                                                         int nvertices)
632{
633        HullDesc                hdsc(QF_TRIANGLES,nvertices,vertices);
634        HullResult              hres;
635        HullLibrary             hlib;/*??*/ 
636        hdsc.mMaxVertices=nvertices;
637        hlib.CreateConvexHull(hdsc,hres);
638        btSoftBody*             psb=new btSoftBody(&worldInfo,(int)hres.mNumOutputVertices,
639                &hres.m_OutputVertices[0],0);
640        for(int i=0;i<(int)hres.mNumFaces;++i)
641        {
642                const int idx[]={       hres.m_Indices[i*3+0],
643                        hres.m_Indices[i*3+1],
644                        hres.m_Indices[i*3+2]};
645                if(idx[0]<idx[1]) psb->appendLink(      idx[0],idx[1]);
646                if(idx[1]<idx[2]) psb->appendLink(      idx[1],idx[2]);
647                if(idx[2]<idx[0]) psb->appendLink(      idx[2],idx[0]);
648                psb->appendFace(idx[0],idx[1],idx[2]);
649        }
650        hlib.ReleaseResult(hres);
651        psb->randomizeConstraints();
652        return(psb);
653}
Note: See TracBrowser for help on using the repository browser.