svn merge -r 37306:39975 https://svn.blender.org/svnroot/bf-blender/trunk/blender
[blender.git] / extern / bullet2 / src / BulletSoftBody / btSoftBodyHelpers.cpp
1 /*
2 Bullet Continuous Collision Detection and Physics Library
3 Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
4
5 This software is provided 'as-is', without any express or implied warranty.
6 In no event will the authors be held liable for any damages arising from the use of this software.
7 Permission is granted to anyone to use this software for any purpose,
8 including commercial applications, and to alter it and redistribute it freely,
9 subject to the following restrictions:
10
11 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
12 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
13 3. This notice may not be removed or altered from any source distribution.
14 */
15 ///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 //
24 static 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 //
33 static 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 //
55 static 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 //
81 template <typename T>
82 static 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 //
97 template <typename T,typename Q>
98 static 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 //
107 template <typename T,typename Q>
108 static 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 //
117 template <typename T>
118 static 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 //
125 static 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(btDot(a,btCross(b,c)));
134 }
135
136 //
137 #if 0
138 static 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 //
157 void                    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                 /* Clusters     */ 
169         if(0!=(drawflags&fDrawFlags::Clusters))
170         {
171                 srand(1806);
172                 for(i=0;i<psb->m_clusters.size();++i)
173                 {
174                         if(psb->m_clusters[i]->m_collide)
175                         {
176                                 btVector3                                               color(  rand()/(btScalar)RAND_MAX,
177                                         rand()/(btScalar)RAND_MAX,
178                                         rand()/(btScalar)RAND_MAX);
179                                 color=color.normalized()*0.75;
180                                 btAlignedObjectArray<btVector3> vertices;
181                                 vertices.resize(psb->m_clusters[i]->m_nodes.size());
182                                 for(j=0,nj=vertices.size();j<nj;++j)
183                                 {                               
184                                         vertices[j]=psb->m_clusters[i]->m_nodes[j]->m_x;
185                                 }
186                                 HullDesc                hdsc(QF_TRIANGLES,vertices.size(),&vertices[0]);
187                                 HullResult              hres;
188                                 HullLibrary             hlib;
189                                 hdsc.mMaxVertices=vertices.size();
190                                 hlib.CreateConvexHull(hdsc,hres);
191                                 const btVector3 center=average(hres.m_OutputVertices);
192                                 add(hres.m_OutputVertices,-center);
193                                 mul(hres.m_OutputVertices,(btScalar)1);
194                                 add(hres.m_OutputVertices,center);
195                                 for(j=0;j<(int)hres.mNumFaces;++j)
196                                 {
197                                         const int idx[]={hres.m_Indices[j*3+0],hres.m_Indices[j*3+1],hres.m_Indices[j*3+2]};
198                                         idraw->drawTriangle(hres.m_OutputVertices[idx[0]],
199                                                 hres.m_OutputVertices[idx[1]],
200                                                 hres.m_OutputVertices[idx[2]],
201                                                 color,1);
202                                 }
203                                 hlib.ReleaseResult(hres);
204                         }
205                         /* Velocities   */ 
206 #if 0
207                         for(int j=0;j<psb->m_clusters[i].m_nodes.size();++j)
208                         {
209                                 const btSoftBody::Cluster&      c=psb->m_clusters[i];
210                                 const btVector3                         r=c.m_nodes[j]->m_x-c.m_com;
211                                 const btVector3                         v=c.m_lv+btCross(c.m_av,r);
212                                 idraw->drawLine(c.m_nodes[j]->m_x,c.m_nodes[j]->m_x+v,btVector3(1,0,0));
213                         }
214 #endif
215                         /* Frame                */ 
216         //              btSoftBody::Cluster& c=*psb->m_clusters[i];
217         //              idraw->drawLine(c.m_com,c.m_framexform*btVector3(10,0,0),btVector3(1,0,0));
218         //              idraw->drawLine(c.m_com,c.m_framexform*btVector3(0,10,0),btVector3(0,1,0));
219         //              idraw->drawLine(c.m_com,c.m_framexform*btVector3(0,0,10),btVector3(0,0,1));
220                 }
221         }
222         else
223         {
224                 /* Nodes        */ 
225                 if(0!=(drawflags&fDrawFlags::Nodes))
226                 {
227                         for(i=0;i<psb->m_nodes.size();++i)
228                         {
229                                 const btSoftBody::Node& n=psb->m_nodes[i];
230                                 if(0==(n.m_material->m_flags&btSoftBody::fMaterial::DebugDraw)) continue;
231                                 idraw->drawLine(n.m_x-btVector3(scl,0,0),n.m_x+btVector3(scl,0,0),btVector3(1,0,0));
232                                 idraw->drawLine(n.m_x-btVector3(0,scl,0),n.m_x+btVector3(0,scl,0),btVector3(0,1,0));
233                                 idraw->drawLine(n.m_x-btVector3(0,0,scl),n.m_x+btVector3(0,0,scl),btVector3(0,0,1));
234                         }
235                 }
236                 /* Links        */ 
237                 if(0!=(drawflags&fDrawFlags::Links))
238                 {
239                         for(i=0;i<psb->m_links.size();++i)
240                         {
241                                 const btSoftBody::Link& l=psb->m_links[i];
242                                 if(0==(l.m_material->m_flags&btSoftBody::fMaterial::DebugDraw)) continue;
243                                 idraw->drawLine(l.m_n[0]->m_x,l.m_n[1]->m_x,lcolor);
244                         }
245                 }
246                 /* Normals      */ 
247                 if(0!=(drawflags&fDrawFlags::Normals))
248                 {
249                         for(i=0;i<psb->m_nodes.size();++i)
250                         {
251                                 const btSoftBody::Node& n=psb->m_nodes[i];
252                                 if(0==(n.m_material->m_flags&btSoftBody::fMaterial::DebugDraw)) continue;
253                                 const btVector3                 d=n.m_n*nscl;
254                                 idraw->drawLine(n.m_x,n.m_x+d,ncolor);
255                                 idraw->drawLine(n.m_x,n.m_x-d,ncolor*0.5);
256                         }
257                 }
258                 /* Contacts     */ 
259                 if(0!=(drawflags&fDrawFlags::Contacts))
260                 {
261                         static const btVector3          axis[]={btVector3(1,0,0),
262                                 btVector3(0,1,0),
263                                 btVector3(0,0,1)};
264                         for(i=0;i<psb->m_rcontacts.size();++i)
265                         {               
266                                 const btSoftBody::RContact&     c=psb->m_rcontacts[i];
267                                 const btVector3                         o=      c.m_node->m_x-c.m_cti.m_normal*
268                                         (btDot(c.m_node->m_x,c.m_cti.m_normal)+c.m_cti.m_offset);
269                                 const btVector3                         x=btCross(c.m_cti.m_normal,axis[c.m_cti.m_normal.minAxis()]).normalized();
270                                 const btVector3                         y=btCross(x,c.m_cti.m_normal).normalized();
271                                 idraw->drawLine(o-x*nscl,o+x*nscl,ccolor);
272                                 idraw->drawLine(o-y*nscl,o+y*nscl,ccolor);
273                                 idraw->drawLine(o,o+c.m_cti.m_normal*nscl*3,btVector3(1,1,0));
274                         }
275                 }
276                 /* Faces        */ 
277         if(0!=(drawflags&fDrawFlags::Faces))
278         {
279                 const btScalar  scl=(btScalar)0.8;
280                 const btScalar  alp=(btScalar)1;
281                 const btVector3 col(0,(btScalar)0.7,0);
282                 for(i=0;i<psb->m_faces.size();++i)
283                 {
284                         const btSoftBody::Face& f=psb->m_faces[i];
285                         if(0==(f.m_material->m_flags&btSoftBody::fMaterial::DebugDraw)) continue;
286                         const btVector3                 x[]={f.m_n[0]->m_x,f.m_n[1]->m_x,f.m_n[2]->m_x};
287                         const btVector3                 c=(x[0]+x[1]+x[2])/3;
288                         idraw->drawTriangle((x[0]-c)*scl+c,
289                                 (x[1]-c)*scl+c,
290                                 (x[2]-c)*scl+c,
291                                 col,alp);
292                 }       
293         }
294         /* Tetras       */ 
295         if(0!=(drawflags&fDrawFlags::Tetras))
296         {
297                 const btScalar  scl=(btScalar)0.8;
298                 const btScalar  alp=(btScalar)1;
299                 const btVector3 col((btScalar)0.7,(btScalar)0.7,(btScalar)0.7);
300                 for(int i=0;i<psb->m_tetras.size();++i)
301                 {
302                         const btSoftBody::Tetra&        t=psb->m_tetras[i];
303                         if(0==(t.m_material->m_flags&btSoftBody::fMaterial::DebugDraw)) continue;
304                         const btVector3                         x[]={t.m_n[0]->m_x,t.m_n[1]->m_x,t.m_n[2]->m_x,t.m_n[3]->m_x};
305                         const btVector3                         c=(x[0]+x[1]+x[2]+x[3])/4;
306                         idraw->drawTriangle((x[0]-c)*scl+c,(x[1]-c)*scl+c,(x[2]-c)*scl+c,col,alp);
307                         idraw->drawTriangle((x[0]-c)*scl+c,(x[1]-c)*scl+c,(x[3]-c)*scl+c,col,alp);
308                         idraw->drawTriangle((x[1]-c)*scl+c,(x[2]-c)*scl+c,(x[3]-c)*scl+c,col,alp);
309                         idraw->drawTriangle((x[2]-c)*scl+c,(x[0]-c)*scl+c,(x[3]-c)*scl+c,col,alp);
310                 }       
311         }
312         }
313         /* Anchors      */ 
314         if(0!=(drawflags&fDrawFlags::Anchors))
315         {
316                 for(i=0;i<psb->m_anchors.size();++i)
317                 {
318                         const btSoftBody::Anchor&       a=psb->m_anchors[i];
319                         const btVector3                         q=a.m_body->getWorldTransform()*a.m_local;
320                         drawVertex(idraw,a.m_node->m_x,0.25,btVector3(1,0,0));
321                         drawVertex(idraw,q,0.25,btVector3(0,1,0));
322                         idraw->drawLine(a.m_node->m_x,q,btVector3(1,1,1));
323                 }
324                 for(i=0;i<psb->m_nodes.size();++i)
325                 {
326                         const btSoftBody::Node& n=psb->m_nodes[i];              
327                         if(0==(n.m_material->m_flags&btSoftBody::fMaterial::DebugDraw)) continue;
328                         if(n.m_im<=0)
329                         {
330                                 drawVertex(idraw,n.m_x,0.25,btVector3(1,0,0));
331                         }
332                 }
333         }
334         
335
336         /* Notes        */ 
337         if(0!=(drawflags&fDrawFlags::Notes))
338         {
339                 for(i=0;i<psb->m_notes.size();++i)
340                 {
341                         const btSoftBody::Note& n=psb->m_notes[i];
342                         btVector3                               p=n.m_offset;
343                         for(int j=0;j<n.m_rank;++j)
344                         {
345                                 p+=n.m_nodes[j]->m_x*n.m_coords[j];
346                         }
347                         idraw->draw3dText(p,n.m_text);
348                 }
349         }
350         /* Node tree    */ 
351         if(0!=(drawflags&fDrawFlags::NodeTree))         DrawNodeTree(psb,idraw);
352         /* Face tree    */ 
353         if(0!=(drawflags&fDrawFlags::FaceTree))         DrawFaceTree(psb,idraw);
354         /* Cluster tree */ 
355         if(0!=(drawflags&fDrawFlags::ClusterTree))      DrawClusterTree(psb,idraw);
356         /* Joints               */ 
357         if(0!=(drawflags&fDrawFlags::Joints))
358         {
359                 for(i=0;i<psb->m_joints.size();++i)
360                 {
361                         const btSoftBody::Joint*        pj=psb->m_joints[i];
362                         switch(pj->Type())
363                         {
364                         case    btSoftBody::Joint::eType::Linear:
365                                 {
366                                         const btSoftBody::LJoint*       pjl=(const btSoftBody::LJoint*)pj;
367                                         const btVector3 a0=pj->m_bodies[0].xform()*pjl->m_refs[0];
368                                         const btVector3 a1=pj->m_bodies[1].xform()*pjl->m_refs[1];
369                                         idraw->drawLine(pj->m_bodies[0].xform().getOrigin(),a0,btVector3(1,1,0));
370                                         idraw->drawLine(pj->m_bodies[1].xform().getOrigin(),a1,btVector3(0,1,1));
371                                         drawVertex(idraw,a0,0.25,btVector3(1,1,0));
372                                         drawVertex(idraw,a1,0.25,btVector3(0,1,1));
373                                 }
374                                 break;
375                         case    btSoftBody::Joint::eType::Angular:
376                                 {
377                                         //const btSoftBody::AJoint*     pja=(const btSoftBody::AJoint*)pj;
378                                         const btVector3 o0=pj->m_bodies[0].xform().getOrigin();
379                                         const btVector3 o1=pj->m_bodies[1].xform().getOrigin();
380                                         const btVector3 a0=pj->m_bodies[0].xform().getBasis()*pj->m_refs[0];
381                                         const btVector3 a1=pj->m_bodies[1].xform().getBasis()*pj->m_refs[1];
382                                         idraw->drawLine(o0,o0+a0*10,btVector3(1,1,0));
383                                         idraw->drawLine(o0,o0+a1*10,btVector3(1,1,0));
384                                         idraw->drawLine(o1,o1+a0*10,btVector3(0,1,1));
385                                         idraw->drawLine(o1,o1+a1*10,btVector3(0,1,1));
386                                         break;
387                                 }
388                                 default:
389                                 {
390                                 }
391                                         
392                         }               
393                 }
394         }
395 }
396
397 //
398 void                    btSoftBodyHelpers::DrawInfos(           btSoftBody* psb,
399                                                                                          btIDebugDraw* idraw,
400                                                                                          bool masses,
401                                                                                          bool areas,
402                                                                                          bool /*stress*/)
403 {
404         for(int i=0;i<psb->m_nodes.size();++i)
405         {
406                 const btSoftBody::Node& n=psb->m_nodes[i];
407                 char                                    text[2048]={0};
408                 char                                    buff[1024];
409                 if(masses)
410                 {
411                         sprintf(buff," M(%.2f)",1/n.m_im);
412                         strcat(text,buff);
413                 }
414                 if(areas)
415                 {
416                         sprintf(buff," A(%.2f)",n.m_area);
417                         strcat(text,buff);
418                 }
419                 if(text[0]) idraw->draw3dText(n.m_x,text);
420         }
421 }
422
423 //
424 void                    btSoftBodyHelpers::DrawNodeTree(        btSoftBody* psb,
425                                                                                                 btIDebugDraw* idraw,
426                                                                                                 int mindepth,
427                                                                                                 int maxdepth)
428 {
429         drawTree(idraw,psb->m_ndbvt.m_root,0,btVector3(1,0,1),btVector3(1,1,1),mindepth,maxdepth);
430 }
431
432 //
433 void                    btSoftBodyHelpers::DrawFaceTree(        btSoftBody* psb,
434                                                                                                 btIDebugDraw* idraw,
435                                                                                                 int mindepth,
436                                                                                                 int maxdepth)
437 {
438         drawTree(idraw,psb->m_fdbvt.m_root,0,btVector3(0,1,0),btVector3(1,0,0),mindepth,maxdepth);
439 }
440
441 //
442 void                    btSoftBodyHelpers::DrawClusterTree(     btSoftBody* psb,
443                                                                                                    btIDebugDraw* idraw,
444                                                                                                    int mindepth,
445                                                                                                    int maxdepth)
446 {
447         drawTree(idraw,psb->m_cdbvt.m_root,0,btVector3(0,1,1),btVector3(1,0,0),mindepth,maxdepth);
448 }
449
450 //
451 void                    btSoftBodyHelpers::DrawFrame(           btSoftBody* psb,
452                                                                                          btIDebugDraw* idraw)
453 {
454         if(psb->m_pose.m_bframe)
455         {
456                 static const btScalar   ascl=10;
457                 static const btScalar   nscl=(btScalar)0.1;
458                 const btVector3                 com=psb->m_pose.m_com;
459                 const btMatrix3x3               trs=psb->m_pose.m_rot*psb->m_pose.m_scl;
460                 const btVector3                 Xaxis=(trs*btVector3(1,0,0)).normalized();
461                 const btVector3                 Yaxis=(trs*btVector3(0,1,0)).normalized();
462                 const btVector3                 Zaxis=(trs*btVector3(0,0,1)).normalized();
463                 idraw->drawLine(com,com+Xaxis*ascl,btVector3(1,0,0));
464                 idraw->drawLine(com,com+Yaxis*ascl,btVector3(0,1,0));
465                 idraw->drawLine(com,com+Zaxis*ascl,btVector3(0,0,1));
466                 for(int i=0;i<psb->m_pose.m_pos.size();++i)
467                 {
468                         const btVector3 x=com+trs*psb->m_pose.m_pos[i];
469                         drawVertex(idraw,x,nscl,btVector3(1,0,1));
470                 }
471         }
472 }
473
474 //
475 btSoftBody*             btSoftBodyHelpers::CreateRope(  btSoftBodyWorldInfo& worldInfo, const btVector3& from,
476                                                                                           const btVector3& to,
477                                                                                           int res,
478                                                                                           int fixeds)
479 {
480         /* Create nodes */ 
481         const int               r=res+2;
482         btVector3*              x=new btVector3[r];
483         btScalar*               m=new btScalar[r];
484         int i;
485
486         for(i=0;i<r;++i)
487         {
488                 const btScalar  t=i/(btScalar)(r-1);
489                 x[i]=lerp(from,to,t);
490                 m[i]=1;
491         }
492         btSoftBody*             psb= new btSoftBody(&worldInfo,r,x,m);
493         if(fixeds&1) psb->setMass(0,0);
494         if(fixeds&2) psb->setMass(r-1,0);
495         delete[] x;
496         delete[] m;
497         /* Create links */ 
498         for(i=1;i<r;++i)
499         {
500                 psb->appendLink(i-1,i);
501         }
502         /* Finished             */ 
503         return(psb);
504 }
505
506 //
507 btSoftBody*             btSoftBodyHelpers::CreatePatch(btSoftBodyWorldInfo& worldInfo,const btVector3& corner00,
508                                                                                            const btVector3& corner10,
509                                                                                            const btVector3& corner01,
510                                                                                            const btVector3& corner11,
511                                                                                            int resx,
512                                                                                            int resy,
513                                                                                            int fixeds,
514                                                                                            bool gendiags)
515 {
516 #define IDX(_x_,_y_)    ((_y_)*rx+(_x_))
517         /* Create nodes */ 
518         if((resx<2)||(resy<2)) return(0);
519         const int       rx=resx;
520         const int       ry=resy;
521         const int       tot=rx*ry;
522         btVector3*      x=new btVector3[tot];
523         btScalar*       m=new btScalar[tot];
524         int iy;
525
526         for(iy=0;iy<ry;++iy)
527         {
528                 const btScalar  ty=iy/(btScalar)(ry-1);
529                 const btVector3 py0=lerp(corner00,corner01,ty);
530                 const btVector3 py1=lerp(corner10,corner11,ty);
531                 for(int ix=0;ix<rx;++ix)
532                 {
533                         const btScalar  tx=ix/(btScalar)(rx-1);
534                         x[IDX(ix,iy)]=lerp(py0,py1,tx);
535                         m[IDX(ix,iy)]=1;
536                 }
537         }
538         btSoftBody*             psb=new btSoftBody(&worldInfo,tot,x,m);
539         if(fixeds&1)    psb->setMass(IDX(0,0),0);
540         if(fixeds&2)    psb->setMass(IDX(rx-1,0),0);
541         if(fixeds&4)    psb->setMass(IDX(0,ry-1),0);
542         if(fixeds&8)    psb->setMass(IDX(rx-1,ry-1),0);
543         delete[] x;
544         delete[] m;
545         /* Create links and faces */ 
546         for(iy=0;iy<ry;++iy)
547         {
548                 for(int ix=0;ix<rx;++ix)
549                 {
550                         const int       idx=IDX(ix,iy);
551                         const bool      mdx=(ix+1)<rx;
552                         const bool      mdy=(iy+1)<ry;
553                         if(mdx) psb->appendLink(idx,IDX(ix+1,iy));
554                         if(mdy) psb->appendLink(idx,IDX(ix,iy+1));
555                         if(mdx&&mdy)
556                         {
557                                 if((ix+iy)&1)
558                                 {
559                                         psb->appendFace(IDX(ix,iy),IDX(ix+1,iy),IDX(ix+1,iy+1));
560                                         psb->appendFace(IDX(ix,iy),IDX(ix+1,iy+1),IDX(ix,iy+1));
561                                         if(gendiags)
562                                         {
563                                                 psb->appendLink(IDX(ix,iy),IDX(ix+1,iy+1));
564                                         }
565                                 }
566                                 else
567                                 {
568                                         psb->appendFace(IDX(ix,iy+1),IDX(ix,iy),IDX(ix+1,iy));
569                                         psb->appendFace(IDX(ix,iy+1),IDX(ix+1,iy),IDX(ix+1,iy+1));
570                                         if(gendiags)
571                                         {
572                                                 psb->appendLink(IDX(ix+1,iy),IDX(ix,iy+1));
573                                         }
574                                 }
575                         }
576                 }
577         }
578         /* Finished             */ 
579 #undef IDX
580         return(psb);
581 }
582
583 //
584 btSoftBody*             btSoftBodyHelpers::CreatePatchUV(btSoftBodyWorldInfo& worldInfo,
585                                                                                                  const btVector3& corner00,
586                                                                                                  const btVector3& corner10,
587                                                                                                  const btVector3& corner01,
588                                                                                                  const btVector3& corner11,
589                                                                                                  int resx,
590                                                                                                  int resy,
591                                                                                                  int fixeds,
592                                                                                                  bool gendiags,
593                                                                                                  float* tex_coords)
594 {
595
596         /*
597         *
598         *  corners:
599         *
600         *  [0][0]     corner00 ------- corner01   [resx][0]
601         *                |                |
602         *                |                |
603         *  [0][resy]  corner10 -------- corner11  [resx][resy]
604         *
605         *
606         *
607         *
608         *
609         *
610         *   "fixedgs" map:
611         *
612         *  corner00     -->   +1
613         *  corner01     -->   +2
614         *  corner10     -->   +4
615         *  corner11     -->   +8
616         *  upper middle -->  +16
617         *  left middle  -->  +32
618         *  right middle -->  +64
619         *  lower middle --> +128
620         *  center       --> +256
621         *
622         *
623         *   tex_coords size   (resx-1)*(resy-1)*12
624         *
625         *
626         *
627         *     SINGLE QUAD INTERNALS
628         *
629         *  1) btSoftBody's nodes and links,
630         *     diagonal link is optional ("gendiags")
631         *
632         *
633         *    node00 ------ node01
634         *      | .              
635         *      |   .            
636         *      |     .          
637         *      |       .        
638         *      |         .      
639         *    node10        node11
640         *
641         *
642         *
643         *   2) Faces:
644         *      two triangles,
645         *      UV Coordinates (hier example for single quad)
646         *      
647         *     (0,1)          (0,1)  (1,1)
648         *     1 |\            3 \-----| 2
649         *       | \              \    |
650         *       |  \              \   |
651         *       |   \              \  |
652         *       |    \              \ |
653         *     2 |-----\ 3            \| 1
654         *     (0,0)    (1,0)       (1,0)
655         *
656         *
657         *
658         *
659         *
660         *
661         */
662
663 #define IDX(_x_,_y_)    ((_y_)*rx+(_x_))
664         /* Create nodes         */ 
665         if((resx<2)||(resy<2)) return(0);
666         const int       rx=resx;
667         const int       ry=resy;
668         const int       tot=rx*ry;
669         btVector3*      x=new btVector3[tot];
670         btScalar*       m=new btScalar[tot];
671
672         int iy;
673
674         for(iy=0;iy<ry;++iy)
675         {
676                 const btScalar  ty=iy/(btScalar)(ry-1);
677                 const btVector3 py0=lerp(corner00,corner01,ty);
678                 const btVector3 py1=lerp(corner10,corner11,ty);
679                 for(int ix=0;ix<rx;++ix)
680                 {
681                         const btScalar  tx=ix/(btScalar)(rx-1);
682                         x[IDX(ix,iy)]=lerp(py0,py1,tx);
683                         m[IDX(ix,iy)]=1;
684                 }
685         }
686         btSoftBody*     psb=new btSoftBody(&worldInfo,tot,x,m);
687         if(fixeds&1)            psb->setMass(IDX(0,0),0);
688         if(fixeds&2)            psb->setMass(IDX(rx-1,0),0);
689         if(fixeds&4)            psb->setMass(IDX(0,ry-1),0);
690         if(fixeds&8)            psb->setMass(IDX(rx-1,ry-1),0);
691         if(fixeds&16)           psb->setMass(IDX((rx-1)/2,0),0);
692         if(fixeds&32)           psb->setMass(IDX(0,(ry-1)/2),0);
693         if(fixeds&64)           psb->setMass(IDX(rx-1,(ry-1)/2),0);
694         if(fixeds&128)          psb->setMass(IDX((rx-1)/2,ry-1),0);
695         if(fixeds&256)          psb->setMass(IDX((rx-1)/2,(ry-1)/2),0);
696         delete[] x;
697         delete[] m;
698
699
700         int z = 0;
701         /* Create links and faces       */ 
702         for(iy=0;iy<ry;++iy)
703         {
704                 for(int ix=0;ix<rx;++ix)
705                 {
706                         const bool      mdx=(ix+1)<rx;
707                         const bool      mdy=(iy+1)<ry;
708
709                         int node00=IDX(ix,iy);
710                         int node01=IDX(ix+1,iy);
711                         int node10=IDX(ix,iy+1);
712                         int node11=IDX(ix+1,iy+1);
713
714                         if(mdx) psb->appendLink(node00,node01);
715                         if(mdy) psb->appendLink(node00,node10);
716                         if(mdx&&mdy)
717                         {
718                                 psb->appendFace(node00,node10,node11);
719                                 if (tex_coords) {
720                                         tex_coords[z+0]=CalculateUV(resx,resy,ix,iy,0);
721                                         tex_coords[z+1]=CalculateUV(resx,resy,ix,iy,1);
722                                         tex_coords[z+2]=CalculateUV(resx,resy,ix,iy,0);
723                                         tex_coords[z+3]=CalculateUV(resx,resy,ix,iy,2);
724                                         tex_coords[z+4]=CalculateUV(resx,resy,ix,iy,3);
725                                         tex_coords[z+5]=CalculateUV(resx,resy,ix,iy,2);
726                                 }
727                                 psb->appendFace(node11,node01,node00);
728                                 if (tex_coords) {
729                                         tex_coords[z+6 ]=CalculateUV(resx,resy,ix,iy,3);
730                                         tex_coords[z+7 ]=CalculateUV(resx,resy,ix,iy,2);
731                                         tex_coords[z+8 ]=CalculateUV(resx,resy,ix,iy,3);
732                                         tex_coords[z+9 ]=CalculateUV(resx,resy,ix,iy,1);
733                                         tex_coords[z+10]=CalculateUV(resx,resy,ix,iy,0);
734                                         tex_coords[z+11]=CalculateUV(resx,resy,ix,iy,1);
735                                 }
736                                 if (gendiags) psb->appendLink(node00,node11);
737                                 z += 12;
738                         }
739                 }
740         }
741         /* Finished     */ 
742 #undef IDX
743         return(psb);
744 }
745
746 float   btSoftBodyHelpers::CalculateUV(int resx,int resy,int ix,int iy,int id)
747 {
748
749         /*
750         *
751         *
752         *    node00 --- node01
753         *      |          |
754         *    node10 --- node11
755         *
756         *
757         *   ID map:
758         *
759         *   node00 s --> 0
760         *   node00 t --> 1
761         *
762         *   node01 s --> 3
763         *   node01 t --> 1
764         *
765         *   node10 s --> 0
766         *   node10 t --> 2
767         *
768         *   node11 s --> 3
769         *   node11 t --> 2
770         *
771         *
772         */
773
774         float tc=0.0f;
775         if (id == 0) {
776                 tc = (1.0f/((resx-1))*ix);
777         }
778         else if (id==1) {
779                 tc = (1.0f/((resy-1))*(resy-1-iy));
780         }
781         else if (id==2) {
782                 tc = (1.0f/((resy-1))*(resy-1-iy-1));
783         }
784         else if (id==3) {
785                 tc = (1.0f/((resx-1))*(ix+1));
786         }
787         return tc;
788 }
789 //
790 btSoftBody*             btSoftBodyHelpers::CreateEllipsoid(btSoftBodyWorldInfo& worldInfo,const btVector3& center,
791                                                                                                    const btVector3& radius,
792                                                                                                    int res)
793 {
794         struct  Hammersley
795         {
796                 static void     Generate(btVector3* x,int n)
797                 {
798                         for(int i=0;i<n;i++)
799                         {
800                                 btScalar        p=0.5,t=0;
801                                 for(int j=i;j;p*=0.5,j>>=1) if(j&1) t+=p;
802                                 btScalar        w=2*t-1;
803                                 btScalar        a=(SIMD_PI+2*i*SIMD_PI)/n;
804                                 btScalar        s=btSqrt(1-w*w);
805                                 *x++=btVector3(s*btCos(a),s*btSin(a),w);
806                         }
807                 }
808         };
809         btAlignedObjectArray<btVector3> vtx;
810         vtx.resize(3+res);
811         Hammersley::Generate(&vtx[0],vtx.size());
812         for(int i=0;i<vtx.size();++i)
813         {
814                 vtx[i]=vtx[i]*radius+center;
815         }
816         return(CreateFromConvexHull(worldInfo,&vtx[0],vtx.size()));
817 }
818
819
820
821 //
822 btSoftBody*             btSoftBodyHelpers::CreateFromTriMesh(btSoftBodyWorldInfo& worldInfo,const btScalar*     vertices,
823                                                                                                          const int* triangles,
824                                                                                                          int ntriangles, bool randomizeConstraints)
825 {
826         int             maxidx=0;
827         int i,j,ni;
828
829         for(i=0,ni=ntriangles*3;i<ni;++i)
830         {
831                 maxidx=btMax(triangles[i],maxidx);
832         }
833         ++maxidx;
834         btAlignedObjectArray<bool>              chks;
835         btAlignedObjectArray<btVector3> vtx;
836         chks.resize(maxidx*maxidx,false);
837         vtx.resize(maxidx);
838         for(i=0,j=0,ni=maxidx*3;i<ni;++j,i+=3)
839         {
840                 vtx[j]=btVector3(vertices[i],vertices[i+1],vertices[i+2]);
841         }
842         btSoftBody*             psb=new btSoftBody(&worldInfo,vtx.size(),&vtx[0],0);
843         for( i=0,ni=ntriangles*3;i<ni;i+=3)
844         {
845                 const int idx[]={triangles[i],triangles[i+1],triangles[i+2]};
846 #define IDX(_x_,_y_) ((_y_)*maxidx+(_x_))
847                 for(int j=2,k=0;k<3;j=k++)
848                 {
849                         if(!chks[IDX(idx[j],idx[k])])
850                         {
851                                 chks[IDX(idx[j],idx[k])]=true;
852                                 chks[IDX(idx[k],idx[j])]=true;
853                                 psb->appendLink(idx[j],idx[k]);
854                         }
855                 }
856 #undef IDX
857                 psb->appendFace(idx[0],idx[1],idx[2]);
858         }
859
860         if (randomizeConstraints)
861         {
862                 psb->randomizeConstraints();
863         }
864
865         return(psb);
866 }
867
868 //
869 btSoftBody*             btSoftBodyHelpers::CreateFromConvexHull(btSoftBodyWorldInfo& worldInfo, const btVector3* vertices,
870                                                                                                                 int nvertices, bool randomizeConstraints)
871 {
872         HullDesc                hdsc(QF_TRIANGLES,nvertices,vertices);
873         HullResult              hres;
874         HullLibrary             hlib;/*??*/ 
875         hdsc.mMaxVertices=nvertices;
876         hlib.CreateConvexHull(hdsc,hres);
877         btSoftBody*             psb=new btSoftBody(&worldInfo,(int)hres.mNumOutputVertices,
878                 &hres.m_OutputVertices[0],0);
879         for(int i=0;i<(int)hres.mNumFaces;++i)
880         {
881                 const int idx[]={       hres.m_Indices[i*3+0],
882                         hres.m_Indices[i*3+1],
883                         hres.m_Indices[i*3+2]};
884                 if(idx[0]<idx[1]) psb->appendLink(      idx[0],idx[1]);
885                 if(idx[1]<idx[2]) psb->appendLink(      idx[1],idx[2]);
886                 if(idx[2]<idx[0]) psb->appendLink(      idx[2],idx[0]);
887                 psb->appendFace(idx[0],idx[1],idx[2]);
888         }
889         hlib.ReleaseResult(hres);
890         if (randomizeConstraints)
891         {
892                 psb->randomizeConstraints();
893         }
894         return(psb);
895 }
896
897
898
899
900 static int nextLine(const char* buffer)
901 {
902         int numBytesRead=0;
903
904         while (*buffer != '\n')
905         {
906                 buffer++;
907                 numBytesRead++;
908         }
909
910         
911         if (buffer[0]==0x0a)
912         {
913                 buffer++;
914                 numBytesRead++;
915         }
916         return numBytesRead;
917 }
918
919 /* Create from TetGen .ele, .face, .node data                                                   */ 
920 btSoftBody*     btSoftBodyHelpers::CreateFromTetGenData(btSoftBodyWorldInfo& worldInfo,
921                                                                                                         const char* ele,
922                                                                                                         const char* face,
923                                                                                                         const char* node,
924                                                                                                         bool bfacelinks,
925                                                                                                         bool btetralinks,
926                                                                                                         bool bfacesfromtetras)
927 {
928 btAlignedObjectArray<btVector3> pos;
929 int                                                             nnode=0;
930 int                                                             ndims=0;
931 int                                                             nattrb=0;
932 int                                                             hasbounds=0;
933 int result = sscanf(node,"%d %d %d %d",&nnode,&ndims,&nattrb,&hasbounds);
934 result = sscanf(node,"%d %d %d %d",&nnode,&ndims,&nattrb,&hasbounds);
935 node += nextLine(node);
936
937 pos.resize(nnode);
938 for(int i=0;i<pos.size();++i)
939         {
940         int                     index=0;
941         //int                   bound=0;
942         float   x,y,z;
943         sscanf(node,"%d %f %f %f",&index,&x,&y,&z);
944
945 //      sn>>index;
946 //      sn>>x;sn>>y;sn>>z;
947         node += nextLine(node);
948
949         //for(int j=0;j<nattrb;++j) 
950         //      sn>>a;
951
952         //if(hasbounds) 
953         //      sn>>bound;
954
955         pos[index].setX(btScalar(x));
956         pos[index].setY(btScalar(y));
957         pos[index].setZ(btScalar(z));
958         }
959 btSoftBody*                                             psb=new btSoftBody(&worldInfo,nnode,&pos[0],0);
960 #if 0
961 if(face&&face[0])
962         {
963         int                                                             nface=0;
964         sf>>nface;sf>>hasbounds;
965         for(int i=0;i<nface;++i)
966                 {
967                 int                     index=0;
968                 int                     bound=0;
969                 int                     ni[3];
970                 sf>>index;
971                 sf>>ni[0];sf>>ni[1];sf>>ni[2];
972                 sf>>bound;
973                 psb->appendFace(ni[0],ni[1],ni[2]);     
974                 if(btetralinks)
975                         {
976                         psb->appendLink(ni[0],ni[1],0,true);
977                         psb->appendLink(ni[1],ni[2],0,true);
978                         psb->appendLink(ni[2],ni[0],0,true);
979                         }
980                 }
981         }
982 #endif
983
984 if(ele&&ele[0])
985         {
986         int                                                             ntetra=0;
987         int                                                             ncorner=0;
988         int                                                             neattrb=0;
989         sscanf(ele,"%d %d %d",&ntetra,&ncorner,&neattrb);
990         ele += nextLine(ele);
991         
992         //se>>ntetra;se>>ncorner;se>>neattrb;
993         for(int i=0;i<ntetra;++i)
994                 {
995                 int                     index=0;
996                 int                     ni[4];
997
998                 //se>>index;
999                 //se>>ni[0];se>>ni[1];se>>ni[2];se>>ni[3];
1000                 sscanf(ele,"%d %d %d %d %d",&index,&ni[0],&ni[1],&ni[2],&ni[3]);
1001                 ele+=nextLine(ele);
1002                 //for(int j=0;j<neattrb;++j) 
1003                 //      se>>a;
1004                 psb->appendTetra(ni[0],ni[1],ni[2],ni[3]);
1005                 if(btetralinks)
1006                         {
1007                         psb->appendLink(ni[0],ni[1],0,true);
1008                         psb->appendLink(ni[1],ni[2],0,true);
1009                         psb->appendLink(ni[2],ni[0],0,true);
1010                         psb->appendLink(ni[0],ni[3],0,true);
1011                         psb->appendLink(ni[1],ni[3],0,true);
1012                         psb->appendLink(ni[2],ni[3],0,true);
1013                         }
1014                 }
1015         }
1016 printf("Nodes:  %u\r\n",psb->m_nodes.size());
1017 printf("Links:  %u\r\n",psb->m_links.size());
1018 printf("Faces:  %u\r\n",psb->m_faces.size());
1019 printf("Tetras: %u\r\n",psb->m_tetras.size());
1020 return(psb);
1021 }
1022