btSoftBodyHelpers.cpp

Go to the documentation of this file.
00001 /*
00002 Bullet Continuous Collision Detection and Physics Library
00003 Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
00004 
00005 This software is provided 'as-is', without any express or implied warranty.
00006 In no event will the authors be held liable for any damages arising from the use of this software.
00007 Permission is granted to anyone to use this software for any purpose,
00008 including commercial applications, and to alter it and redistribute it freely,
00009 subject to the following restrictions:
00010 
00011 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.
00012 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
00013 3. This notice may not be removed or altered from any source distribution.
00014 */
00016 
00017 #include "btSoftBodyInternals.h"
00018 #include <stdio.h>
00019 #include <string.h>
00020 #include "btSoftBodyHelpers.h"
00021 #include "LinearMath/btConvexHull.h"
00022 #include "LinearMath/btConvexHullComputer.h"
00023 
00024 
00025 //
00026 static void                             drawVertex(     btIDebugDraw* idraw,
00027                                                                    const btVector3& x,btScalar s,const btVector3& c)
00028 {
00029         idraw->drawLine(x-btVector3(s,0,0),x+btVector3(s,0,0),c);
00030         idraw->drawLine(x-btVector3(0,s,0),x+btVector3(0,s,0),c);
00031         idraw->drawLine(x-btVector3(0,0,s),x+btVector3(0,0,s),c);
00032 }
00033 
00034 //
00035 static void                             drawBox(        btIDebugDraw* idraw,
00036                                                                 const btVector3& mins,
00037                                                                 const btVector3& maxs,
00038                                                                 const btVector3& color)
00039 {
00040         const btVector3 c[]={   btVector3(mins.x(),mins.y(),mins.z()),
00041                 btVector3(maxs.x(),mins.y(),mins.z()),
00042                 btVector3(maxs.x(),maxs.y(),mins.z()),
00043                 btVector3(mins.x(),maxs.y(),mins.z()),
00044                 btVector3(mins.x(),mins.y(),maxs.z()),
00045                 btVector3(maxs.x(),mins.y(),maxs.z()),
00046                 btVector3(maxs.x(),maxs.y(),maxs.z()),
00047                 btVector3(mins.x(),maxs.y(),maxs.z())};
00048         idraw->drawLine(c[0],c[1],color);idraw->drawLine(c[1],c[2],color);
00049         idraw->drawLine(c[2],c[3],color);idraw->drawLine(c[3],c[0],color);
00050         idraw->drawLine(c[4],c[5],color);idraw->drawLine(c[5],c[6],color);
00051         idraw->drawLine(c[6],c[7],color);idraw->drawLine(c[7],c[4],color);
00052         idraw->drawLine(c[0],c[4],color);idraw->drawLine(c[1],c[5],color);
00053         idraw->drawLine(c[2],c[6],color);idraw->drawLine(c[3],c[7],color);
00054 }
00055 
00056 //
00057 static void                             drawTree(       btIDebugDraw* idraw,
00058                                                                  const btDbvtNode* node,
00059                                                                  int depth,
00060                                                                  const btVector3& ncolor,
00061                                                                  const btVector3& lcolor,
00062                                                                  int mindepth,
00063                                                                  int maxdepth)
00064 {
00065         if(node)
00066         {
00067                 if(node->isinternal()&&((depth<maxdepth)||(maxdepth<0)))
00068                 {
00069                         drawTree(idraw,node->childs[0],depth+1,ncolor,lcolor,mindepth,maxdepth);
00070                         drawTree(idraw,node->childs[1],depth+1,ncolor,lcolor,mindepth,maxdepth);
00071                 }
00072                 if(depth>=mindepth)
00073                 {
00074                         const btScalar  scl=(btScalar)(node->isinternal()?1:1);
00075                         const btVector3 mi=node->volume.Center()-node->volume.Extents()*scl;
00076                         const btVector3 mx=node->volume.Center()+node->volume.Extents()*scl;
00077                         drawBox(idraw,mi,mx,node->isleaf()?lcolor:ncolor);
00078                 }
00079         }
00080 }
00081 
00082 //
00083 template <typename T>
00084 static inline T                         sum(const btAlignedObjectArray<T>& items)
00085 {
00086         T       v;
00087         if(items.size())
00088         {
00089                 v=items[0];
00090                 for(int i=1,ni=items.size();i<ni;++i)
00091                 {
00092                         v+=items[i];
00093                 }
00094         }
00095         return(v);
00096 }
00097 
00098 //
00099 template <typename T,typename Q>
00100 static inline void                      add(btAlignedObjectArray<T>& items,const Q& value)
00101 {
00102         for(int i=0,ni=items.size();i<ni;++i)
00103         {
00104                 items[i]+=value;
00105         }
00106 }
00107 
00108 //
00109 template <typename T,typename Q>
00110 static inline void                      mul(btAlignedObjectArray<T>& items,const Q& value)
00111 {
00112         for(int i=0,ni=items.size();i<ni;++i)
00113         {
00114                 items[i]*=value;
00115         }
00116 }
00117 
00118 //
00119 template <typename T>
00120 static inline T                         average(const btAlignedObjectArray<T>& items)
00121 {
00122         const btScalar  n=(btScalar)(items.size()>0?items.size():1);
00123         return(sum(items)/n);
00124 }
00125 
00126 //
00127 static inline btScalar          tetravolume(const btVector3& x0,
00128                                                                                 const btVector3& x1,
00129                                                                                 const btVector3& x2,
00130                                                                                 const btVector3& x3)
00131 {
00132         const btVector3 a=x1-x0;
00133         const btVector3 b=x2-x0;
00134         const btVector3 c=x3-x0;
00135         return(btDot(a,btCross(b,c)));
00136 }
00137 
00138 //
00139 #if 0
00140 static btVector3                stresscolor(btScalar stress)
00141 {
00142         static const btVector3  spectrum[]=     {       btVector3(1,0,1),
00143                 btVector3(0,0,1),
00144                 btVector3(0,1,1),
00145                 btVector3(0,1,0),
00146                 btVector3(1,1,0),
00147                 btVector3(1,0,0),
00148                 btVector3(1,0,0)};
00149         static const int                ncolors=sizeof(spectrum)/sizeof(spectrum[0])-1;
00150         static const btScalar   one=1;
00151         stress=btMax<btScalar>(0,btMin<btScalar>(1,stress))*ncolors;
00152         const int                               sel=(int)stress;
00153         const btScalar                  frc=stress-sel;
00154         return(spectrum[sel]+(spectrum[sel+1]-spectrum[sel])*frc);
00155 }
00156 #endif
00157 
00158 //
00159 void                    btSoftBodyHelpers::Draw(        btSoftBody* psb,
00160                                                                                 btIDebugDraw* idraw,
00161                                                                                 int drawflags)
00162 {
00163         const btScalar          scl=(btScalar)0.1;
00164         const btScalar          nscl=scl*5;
00165         const btVector3         lcolor=btVector3(0,0,0);
00166         const btVector3         ncolor=btVector3(1,1,1);
00167         const btVector3         ccolor=btVector3(1,0,0);
00168         int i,j,nj;
00169 
00170                 /* Clusters     */ 
00171         if(0!=(drawflags&fDrawFlags::Clusters))
00172         {
00173                 srand(1806);
00174                 for(i=0;i<psb->m_clusters.size();++i)
00175                 {
00176                         if(psb->m_clusters[i]->m_collide)
00177                         {
00178                                 btVector3                                               color(  rand()/(btScalar)RAND_MAX,
00179                                         rand()/(btScalar)RAND_MAX,
00180                                         rand()/(btScalar)RAND_MAX);
00181                                 color=color.normalized()*0.75;
00182                                 btAlignedObjectArray<btVector3> vertices;
00183                                 vertices.resize(psb->m_clusters[i]->m_nodes.size());
00184                                 for(j=0,nj=vertices.size();j<nj;++j)
00185                                 {                               
00186                                         vertices[j]=psb->m_clusters[i]->m_nodes[j]->m_x;
00187                                 }
00188 #define USE_NEW_CONVEX_HULL_COMPUTER
00189 #ifdef USE_NEW_CONVEX_HULL_COMPUTER
00190                                 btConvexHullComputer    computer;
00191                                 int stride = sizeof(btVector3);
00192                                 int count = vertices.size();
00193                                 btScalar shrink=0.f;
00194                                 btScalar shrinkClamp=0.f;
00195                                 computer.compute(&vertices[0].getX(),stride,count,shrink,shrinkClamp);
00196                                 for (int i=0;i<computer.faces.size();i++)
00197                                 {
00198 
00199                                         int face = computer.faces[i];
00200                                         //printf("face=%d\n",face);
00201                                         const btConvexHullComputer::Edge*  firstEdge = &computer.edges[face];
00202                                         const btConvexHullComputer::Edge*  edge = firstEdge->getNextEdgeOfFace();
00203 
00204                                         int v0 = firstEdge->getSourceVertex();
00205                                         int v1 = firstEdge->getTargetVertex();
00206                                         while (edge!=firstEdge)
00207                                         {
00208                                                 int v2 = edge->getTargetVertex();
00209                                                 idraw->drawTriangle(computer.vertices[v0],computer.vertices[v1],computer.vertices[v2],color,1);
00210                                                 edge = edge->getNextEdgeOfFace();
00211                                                 v0=v1;
00212                                                 v1=v2;
00213                                         };
00214                                 }
00215 #else
00216 
00217                                 HullDesc                hdsc(QF_TRIANGLES,vertices.size(),&vertices[0]);
00218                                 HullResult              hres;
00219                                 HullLibrary             hlib;
00220                                 hdsc.mMaxVertices=vertices.size();
00221                                 hlib.CreateConvexHull(hdsc,hres);
00222                                 const btVector3 center=average(hres.m_OutputVertices);
00223                                 add(hres.m_OutputVertices,-center);
00224                                 mul(hres.m_OutputVertices,(btScalar)1);
00225                                 add(hres.m_OutputVertices,center);
00226                                 for(j=0;j<(int)hres.mNumFaces;++j)
00227                                 {
00228                                         const int idx[]={hres.m_Indices[j*3+0],hres.m_Indices[j*3+1],hres.m_Indices[j*3+2]};
00229                                         idraw->drawTriangle(hres.m_OutputVertices[idx[0]],
00230                                                 hres.m_OutputVertices[idx[1]],
00231                                                 hres.m_OutputVertices[idx[2]],
00232                                                 color,1);
00233                                 }
00234                                 hlib.ReleaseResult(hres);
00235 #endif
00236 
00237                         }
00238                         /* Velocities   */ 
00239 #if 0
00240                         for(int j=0;j<psb->m_clusters[i].m_nodes.size();++j)
00241                         {
00242                                 const btSoftBody::Cluster&      c=psb->m_clusters[i];
00243                                 const btVector3                         r=c.m_nodes[j]->m_x-c.m_com;
00244                                 const btVector3                         v=c.m_lv+btCross(c.m_av,r);
00245                                 idraw->drawLine(c.m_nodes[j]->m_x,c.m_nodes[j]->m_x+v,btVector3(1,0,0));
00246                         }
00247 #endif
00248                         /* Frame                */ 
00249         //              btSoftBody::Cluster& c=*psb->m_clusters[i];
00250         //              idraw->drawLine(c.m_com,c.m_framexform*btVector3(10,0,0),btVector3(1,0,0));
00251         //              idraw->drawLine(c.m_com,c.m_framexform*btVector3(0,10,0),btVector3(0,1,0));
00252         //              idraw->drawLine(c.m_com,c.m_framexform*btVector3(0,0,10),btVector3(0,0,1));
00253                 }
00254         }
00255         else
00256         {
00257                 /* Nodes        */ 
00258                 if(0!=(drawflags&fDrawFlags::Nodes))
00259                 {
00260                         for(i=0;i<psb->m_nodes.size();++i)
00261                         {
00262                                 const btSoftBody::Node& n=psb->m_nodes[i];
00263                                 if(0==(n.m_material->m_flags&btSoftBody::fMaterial::DebugDraw)) continue;
00264                                 idraw->drawLine(n.m_x-btVector3(scl,0,0),n.m_x+btVector3(scl,0,0),btVector3(1,0,0));
00265                                 idraw->drawLine(n.m_x-btVector3(0,scl,0),n.m_x+btVector3(0,scl,0),btVector3(0,1,0));
00266                                 idraw->drawLine(n.m_x-btVector3(0,0,scl),n.m_x+btVector3(0,0,scl),btVector3(0,0,1));
00267                         }
00268                 }
00269                 /* Links        */ 
00270                 if(0!=(drawflags&fDrawFlags::Links))
00271                 {
00272                         for(i=0;i<psb->m_links.size();++i)
00273                         {
00274                                 const btSoftBody::Link& l=psb->m_links[i];
00275                                 if(0==(l.m_material->m_flags&btSoftBody::fMaterial::DebugDraw)) continue;
00276                                 idraw->drawLine(l.m_n[0]->m_x,l.m_n[1]->m_x,lcolor);
00277                         }
00278                 }
00279                 /* Normals      */ 
00280                 if(0!=(drawflags&fDrawFlags::Normals))
00281                 {
00282                         for(i=0;i<psb->m_nodes.size();++i)
00283                         {
00284                                 const btSoftBody::Node& n=psb->m_nodes[i];
00285                                 if(0==(n.m_material->m_flags&btSoftBody::fMaterial::DebugDraw)) continue;
00286                                 const btVector3                 d=n.m_n*nscl;
00287                                 idraw->drawLine(n.m_x,n.m_x+d,ncolor);
00288                                 idraw->drawLine(n.m_x,n.m_x-d,ncolor*0.5);
00289                         }
00290                 }
00291                 /* Contacts     */ 
00292                 if(0!=(drawflags&fDrawFlags::Contacts))
00293                 {
00294                         static const btVector3          axis[]={btVector3(1,0,0),
00295                                 btVector3(0,1,0),
00296                                 btVector3(0,0,1)};
00297                         for(i=0;i<psb->m_rcontacts.size();++i)
00298                         {               
00299                                 const btSoftBody::RContact&     c=psb->m_rcontacts[i];
00300                                 const btVector3                         o=      c.m_node->m_x-c.m_cti.m_normal*
00301                                         (btDot(c.m_node->m_x,c.m_cti.m_normal)+c.m_cti.m_offset);
00302                                 const btVector3                         x=btCross(c.m_cti.m_normal,axis[c.m_cti.m_normal.minAxis()]).normalized();
00303                                 const btVector3                         y=btCross(x,c.m_cti.m_normal).normalized();
00304                                 idraw->drawLine(o-x*nscl,o+x*nscl,ccolor);
00305                                 idraw->drawLine(o-y*nscl,o+y*nscl,ccolor);
00306                                 idraw->drawLine(o,o+c.m_cti.m_normal*nscl*3,btVector3(1,1,0));
00307                         }
00308                 }
00309                 /* Faces        */ 
00310         if(0!=(drawflags&fDrawFlags::Faces))
00311         {
00312                 const btScalar  scl=(btScalar)0.8;
00313                 const btScalar  alp=(btScalar)1;
00314                 const btVector3 col(0,(btScalar)0.7,0);
00315                 for(i=0;i<psb->m_faces.size();++i)
00316                 {
00317                         const btSoftBody::Face& f=psb->m_faces[i];
00318                         if(0==(f.m_material->m_flags&btSoftBody::fMaterial::DebugDraw)) continue;
00319                         const btVector3                 x[]={f.m_n[0]->m_x,f.m_n[1]->m_x,f.m_n[2]->m_x};
00320                         const btVector3                 c=(x[0]+x[1]+x[2])/3;
00321                         idraw->drawTriangle((x[0]-c)*scl+c,
00322                                 (x[1]-c)*scl+c,
00323                                 (x[2]-c)*scl+c,
00324                                 col,alp);
00325                 }       
00326         }
00327         /* Tetras       */ 
00328         if(0!=(drawflags&fDrawFlags::Tetras))
00329         {
00330                 const btScalar  scl=(btScalar)0.8;
00331                 const btScalar  alp=(btScalar)1;
00332                 const btVector3 col((btScalar)0.3,(btScalar)0.3,(btScalar)0.7);
00333                 for(int i=0;i<psb->m_tetras.size();++i)
00334                 {
00335                         const btSoftBody::Tetra&        t=psb->m_tetras[i];
00336                         if(0==(t.m_material->m_flags&btSoftBody::fMaterial::DebugDraw)) continue;
00337                         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};
00338                         const btVector3                         c=(x[0]+x[1]+x[2]+x[3])/4;
00339                         idraw->drawTriangle((x[0]-c)*scl+c,(x[1]-c)*scl+c,(x[2]-c)*scl+c,col,alp);
00340                         idraw->drawTriangle((x[0]-c)*scl+c,(x[1]-c)*scl+c,(x[3]-c)*scl+c,col,alp);
00341                         idraw->drawTriangle((x[1]-c)*scl+c,(x[2]-c)*scl+c,(x[3]-c)*scl+c,col,alp);
00342                         idraw->drawTriangle((x[2]-c)*scl+c,(x[0]-c)*scl+c,(x[3]-c)*scl+c,col,alp);
00343                 }       
00344         }
00345         }
00346         /* Anchors      */ 
00347         if(0!=(drawflags&fDrawFlags::Anchors))
00348         {
00349                 for(i=0;i<psb->m_anchors.size();++i)
00350                 {
00351                         const btSoftBody::Anchor&       a=psb->m_anchors[i];
00352                         const btVector3                         q=a.m_body->getWorldTransform()*a.m_local;
00353                         drawVertex(idraw,a.m_node->m_x,0.25,btVector3(1,0,0));
00354                         drawVertex(idraw,q,0.25,btVector3(0,1,0));
00355                         idraw->drawLine(a.m_node->m_x,q,btVector3(1,1,1));
00356                 }
00357                 for(i=0;i<psb->m_nodes.size();++i)
00358                 {
00359                         const btSoftBody::Node& n=psb->m_nodes[i];              
00360                         if(0==(n.m_material->m_flags&btSoftBody::fMaterial::DebugDraw)) continue;
00361                         if(n.m_im<=0)
00362                         {
00363                                 drawVertex(idraw,n.m_x,0.25,btVector3(1,0,0));
00364                         }
00365                 }
00366         }
00367         
00368 
00369         /* Notes        */ 
00370         if(0!=(drawflags&fDrawFlags::Notes))
00371         {
00372                 for(i=0;i<psb->m_notes.size();++i)
00373                 {
00374                         const btSoftBody::Note& n=psb->m_notes[i];
00375                         btVector3                               p=n.m_offset;
00376                         for(int j=0;j<n.m_rank;++j)
00377                         {
00378                                 p+=n.m_nodes[j]->m_x*n.m_coords[j];
00379                         }
00380                         idraw->draw3dText(p,n.m_text);
00381                 }
00382         }
00383         /* Node tree    */ 
00384         if(0!=(drawflags&fDrawFlags::NodeTree))         DrawNodeTree(psb,idraw);
00385         /* Face tree    */ 
00386         if(0!=(drawflags&fDrawFlags::FaceTree))         DrawFaceTree(psb,idraw);
00387         /* Cluster tree */ 
00388         if(0!=(drawflags&fDrawFlags::ClusterTree))      DrawClusterTree(psb,idraw);
00389         /* Joints               */ 
00390         if(0!=(drawflags&fDrawFlags::Joints))
00391         {
00392                 for(i=0;i<psb->m_joints.size();++i)
00393                 {
00394                         const btSoftBody::Joint*        pj=psb->m_joints[i];
00395                         switch(pj->Type())
00396                         {
00397                         case    btSoftBody::Joint::eType::Linear:
00398                                 {
00399                                         const btSoftBody::LJoint*       pjl=(const btSoftBody::LJoint*)pj;
00400                                         const btVector3 a0=pj->m_bodies[0].xform()*pjl->m_refs[0];
00401                                         const btVector3 a1=pj->m_bodies[1].xform()*pjl->m_refs[1];
00402                                         idraw->drawLine(pj->m_bodies[0].xform().getOrigin(),a0,btVector3(1,1,0));
00403                                         idraw->drawLine(pj->m_bodies[1].xform().getOrigin(),a1,btVector3(0,1,1));
00404                                         drawVertex(idraw,a0,0.25,btVector3(1,1,0));
00405                                         drawVertex(idraw,a1,0.25,btVector3(0,1,1));
00406                                 }
00407                                 break;
00408                         case    btSoftBody::Joint::eType::Angular:
00409                                 {
00410                                         //const btSoftBody::AJoint*     pja=(const btSoftBody::AJoint*)pj;
00411                                         const btVector3 o0=pj->m_bodies[0].xform().getOrigin();
00412                                         const btVector3 o1=pj->m_bodies[1].xform().getOrigin();
00413                                         const btVector3 a0=pj->m_bodies[0].xform().getBasis()*pj->m_refs[0];
00414                                         const btVector3 a1=pj->m_bodies[1].xform().getBasis()*pj->m_refs[1];
00415                                         idraw->drawLine(o0,o0+a0*10,btVector3(1,1,0));
00416                                         idraw->drawLine(o0,o0+a1*10,btVector3(1,1,0));
00417                                         idraw->drawLine(o1,o1+a0*10,btVector3(0,1,1));
00418                                         idraw->drawLine(o1,o1+a1*10,btVector3(0,1,1));
00419                                         break;
00420                                 }
00421                                 default:
00422                                 {
00423                                 }
00424                                         
00425                         }               
00426                 }
00427         }
00428 }
00429 
00430 //
00431 void                    btSoftBodyHelpers::DrawInfos(           btSoftBody* psb,
00432                                                                                          btIDebugDraw* idraw,
00433                                                                                          bool masses,
00434                                                                                          bool areas,
00435                                                                                          bool /*stress*/)
00436 {
00437         for(int i=0;i<psb->m_nodes.size();++i)
00438         {
00439                 const btSoftBody::Node& n=psb->m_nodes[i];
00440                 char                                    text[2048]={0};
00441                 char                                    buff[1024];
00442                 if(masses)
00443                 {
00444                         sprintf(buff," M(%.2f)",1/n.m_im);
00445                         strcat(text,buff);
00446                 }
00447                 if(areas)
00448                 {
00449                         sprintf(buff," A(%.2f)",n.m_area);
00450                         strcat(text,buff);
00451                 }
00452                 if(text[0]) idraw->draw3dText(n.m_x,text);
00453         }
00454 }
00455 
00456 //
00457 void                    btSoftBodyHelpers::DrawNodeTree(        btSoftBody* psb,
00458                                                                                                 btIDebugDraw* idraw,
00459                                                                                                 int mindepth,
00460                                                                                                 int maxdepth)
00461 {
00462         drawTree(idraw,psb->m_ndbvt.m_root,0,btVector3(1,0,1),btVector3(1,1,1),mindepth,maxdepth);
00463 }
00464 
00465 //
00466 void                    btSoftBodyHelpers::DrawFaceTree(        btSoftBody* psb,
00467                                                                                                 btIDebugDraw* idraw,
00468                                                                                                 int mindepth,
00469                                                                                                 int maxdepth)
00470 {
00471         drawTree(idraw,psb->m_fdbvt.m_root,0,btVector3(0,1,0),btVector3(1,0,0),mindepth,maxdepth);
00472 }
00473 
00474 //
00475 void                    btSoftBodyHelpers::DrawClusterTree(     btSoftBody* psb,
00476                                                                                                    btIDebugDraw* idraw,
00477                                                                                                    int mindepth,
00478                                                                                                    int maxdepth)
00479 {
00480         drawTree(idraw,psb->m_cdbvt.m_root,0,btVector3(0,1,1),btVector3(1,0,0),mindepth,maxdepth);
00481 }
00482 
00483 //
00484 void                    btSoftBodyHelpers::DrawFrame(           btSoftBody* psb,
00485                                                                                          btIDebugDraw* idraw)
00486 {
00487         if(psb->m_pose.m_bframe)
00488         {
00489                 static const btScalar   ascl=10;
00490                 static const btScalar   nscl=(btScalar)0.1;
00491                 const btVector3                 com=psb->m_pose.m_com;
00492                 const btMatrix3x3               trs=psb->m_pose.m_rot*psb->m_pose.m_scl;
00493                 const btVector3                 Xaxis=(trs*btVector3(1,0,0)).normalized();
00494                 const btVector3                 Yaxis=(trs*btVector3(0,1,0)).normalized();
00495                 const btVector3                 Zaxis=(trs*btVector3(0,0,1)).normalized();
00496                 idraw->drawLine(com,com+Xaxis*ascl,btVector3(1,0,0));
00497                 idraw->drawLine(com,com+Yaxis*ascl,btVector3(0,1,0));
00498                 idraw->drawLine(com,com+Zaxis*ascl,btVector3(0,0,1));
00499                 for(int i=0;i<psb->m_pose.m_pos.size();++i)
00500                 {
00501                         const btVector3 x=com+trs*psb->m_pose.m_pos[i];
00502                         drawVertex(idraw,x,nscl,btVector3(1,0,1));
00503                 }
00504         }
00505 }
00506 
00507 //
00508 btSoftBody*             btSoftBodyHelpers::CreateRope(  btSoftBodyWorldInfo& worldInfo, const btVector3& from,
00509                                                                                           const btVector3& to,
00510                                                                                           int res,
00511                                                                                           int fixeds)
00512 {
00513         /* Create nodes */ 
00514         const int               r=res+2;
00515         btVector3*              x=new btVector3[r];
00516         btScalar*               m=new btScalar[r];
00517         int i;
00518 
00519         for(i=0;i<r;++i)
00520         {
00521                 const btScalar  t=i/(btScalar)(r-1);
00522                 x[i]=lerp(from,to,t);
00523                 m[i]=1;
00524         }
00525         btSoftBody*             psb= new btSoftBody(&worldInfo,r,x,m);
00526         if(fixeds&1) psb->setMass(0,0);
00527         if(fixeds&2) psb->setMass(r-1,0);
00528         delete[] x;
00529         delete[] m;
00530         /* Create links */ 
00531         for(i=1;i<r;++i)
00532         {
00533                 psb->appendLink(i-1,i);
00534         }
00535         /* Finished             */ 
00536         return(psb);
00537 }
00538 
00539 //
00540 btSoftBody*             btSoftBodyHelpers::CreatePatch(btSoftBodyWorldInfo& worldInfo,const btVector3& corner00,
00541                                                                                            const btVector3& corner10,
00542                                                                                            const btVector3& corner01,
00543                                                                                            const btVector3& corner11,
00544                                                                                            int resx,
00545                                                                                            int resy,
00546                                                                                            int fixeds,
00547                                                                                            bool gendiags)
00548 {
00549 #define IDX(_x_,_y_)    ((_y_)*rx+(_x_))
00550         /* Create nodes */ 
00551         if((resx<2)||(resy<2)) return(0);
00552         const int       rx=resx;
00553         const int       ry=resy;
00554         const int       tot=rx*ry;
00555         btVector3*      x=new btVector3[tot];
00556         btScalar*       m=new btScalar[tot];
00557         int iy;
00558 
00559         for(iy=0;iy<ry;++iy)
00560         {
00561                 const btScalar  ty=iy/(btScalar)(ry-1);
00562                 const btVector3 py0=lerp(corner00,corner01,ty);
00563                 const btVector3 py1=lerp(corner10,corner11,ty);
00564                 for(int ix=0;ix<rx;++ix)
00565                 {
00566                         const btScalar  tx=ix/(btScalar)(rx-1);
00567                         x[IDX(ix,iy)]=lerp(py0,py1,tx);
00568                         m[IDX(ix,iy)]=1;
00569                 }
00570         }
00571         btSoftBody*             psb=new btSoftBody(&worldInfo,tot,x,m);
00572         if(fixeds&1)    psb->setMass(IDX(0,0),0);
00573         if(fixeds&2)    psb->setMass(IDX(rx-1,0),0);
00574         if(fixeds&4)    psb->setMass(IDX(0,ry-1),0);
00575         if(fixeds&8)    psb->setMass(IDX(rx-1,ry-1),0);
00576         delete[] x;
00577         delete[] m;
00578         /* Create links and faces */ 
00579         for(iy=0;iy<ry;++iy)
00580         {
00581                 for(int ix=0;ix<rx;++ix)
00582                 {
00583                         const int       idx=IDX(ix,iy);
00584                         const bool      mdx=(ix+1)<rx;
00585                         const bool      mdy=(iy+1)<ry;
00586                         if(mdx) psb->appendLink(idx,IDX(ix+1,iy));
00587                         if(mdy) psb->appendLink(idx,IDX(ix,iy+1));
00588                         if(mdx&&mdy)
00589                         {
00590                                 if((ix+iy)&1)
00591                                 {
00592                                         psb->appendFace(IDX(ix,iy),IDX(ix+1,iy),IDX(ix+1,iy+1));
00593                                         psb->appendFace(IDX(ix,iy),IDX(ix+1,iy+1),IDX(ix,iy+1));
00594                                         if(gendiags)
00595                                         {
00596                                                 psb->appendLink(IDX(ix,iy),IDX(ix+1,iy+1));
00597                                         }
00598                                 }
00599                                 else
00600                                 {
00601                                         psb->appendFace(IDX(ix,iy+1),IDX(ix,iy),IDX(ix+1,iy));
00602                                         psb->appendFace(IDX(ix,iy+1),IDX(ix+1,iy),IDX(ix+1,iy+1));
00603                                         if(gendiags)
00604                                         {
00605                                                 psb->appendLink(IDX(ix+1,iy),IDX(ix,iy+1));
00606                                         }
00607                                 }
00608                         }
00609                 }
00610         }
00611         /* Finished             */ 
00612 #undef IDX
00613         return(psb);
00614 }
00615 
00616 //
00617 btSoftBody*             btSoftBodyHelpers::CreatePatchUV(btSoftBodyWorldInfo& worldInfo,
00618                                                                                                  const btVector3& corner00,
00619                                                                                                  const btVector3& corner10,
00620                                                                                                  const btVector3& corner01,
00621                                                                                                  const btVector3& corner11,
00622                                                                                                  int resx,
00623                                                                                                  int resy,
00624                                                                                                  int fixeds,
00625                                                                                                  bool gendiags,
00626                                                                                                  float* tex_coords)
00627 {
00628 
00629         /*
00630         *
00631         *  corners:
00632         *
00633         *  [0][0]     corner00 ------- corner01   [resx][0]
00634         *                |                |
00635         *                |                |
00636         *  [0][resy]  corner10 -------- corner11  [resx][resy]
00637         *
00638         *
00639         *
00640         *
00641         *
00642         *
00643         *   "fixedgs" map:
00644         *
00645         *  corner00     -->   +1
00646         *  corner01     -->   +2
00647         *  corner10     -->   +4
00648         *  corner11     -->   +8
00649         *  upper middle -->  +16
00650         *  left middle  -->  +32
00651         *  right middle -->  +64
00652         *  lower middle --> +128
00653         *  center       --> +256
00654         *
00655         *
00656         *   tex_coords size   (resx-1)*(resy-1)*12
00657         *
00658         *
00659         *
00660         *     SINGLE QUAD INTERNALS
00661         *
00662         *  1) btSoftBody's nodes and links,
00663         *     diagonal link is optional ("gendiags")
00664         *
00665         *
00666         *    node00 ------ node01
00667         *      | .              
00668         *      |   .            
00669         *      |     .          
00670         *      |       .        
00671         *      |         .      
00672         *    node10        node11
00673         *
00674         *
00675         *
00676         *   2) Faces:
00677         *      two triangles,
00678         *      UV Coordinates (hier example for single quad)
00679         *      
00680         *     (0,1)          (0,1)  (1,1)
00681         *     1 |\            3 \-----| 2
00682         *       | \              \    |
00683         *       |  \              \   |
00684         *       |   \              \  |
00685         *       |    \              \ |
00686         *     2 |-----\ 3            \| 1
00687         *     (0,0)    (1,0)       (1,0)
00688         *
00689         *
00690         *
00691         *
00692         *
00693         *
00694         */
00695 
00696 #define IDX(_x_,_y_)    ((_y_)*rx+(_x_))
00697         /* Create nodes         */ 
00698         if((resx<2)||(resy<2)) return(0);
00699         const int       rx=resx;
00700         const int       ry=resy;
00701         const int       tot=rx*ry;
00702         btVector3*      x=new btVector3[tot];
00703         btScalar*       m=new btScalar[tot];
00704 
00705         int iy;
00706 
00707         for(iy=0;iy<ry;++iy)
00708         {
00709                 const btScalar  ty=iy/(btScalar)(ry-1);
00710                 const btVector3 py0=lerp(corner00,corner01,ty);
00711                 const btVector3 py1=lerp(corner10,corner11,ty);
00712                 for(int ix=0;ix<rx;++ix)
00713                 {
00714                         const btScalar  tx=ix/(btScalar)(rx-1);
00715                         x[IDX(ix,iy)]=lerp(py0,py1,tx);
00716                         m[IDX(ix,iy)]=1;
00717                 }
00718         }
00719         btSoftBody*     psb=new btSoftBody(&worldInfo,tot,x,m);
00720         if(fixeds&1)            psb->setMass(IDX(0,0),0);
00721         if(fixeds&2)            psb->setMass(IDX(rx-1,0),0);
00722         if(fixeds&4)            psb->setMass(IDX(0,ry-1),0);
00723         if(fixeds&8)            psb->setMass(IDX(rx-1,ry-1),0);
00724         if(fixeds&16)           psb->setMass(IDX((rx-1)/2,0),0);
00725         if(fixeds&32)           psb->setMass(IDX(0,(ry-1)/2),0);
00726         if(fixeds&64)           psb->setMass(IDX(rx-1,(ry-1)/2),0);
00727         if(fixeds&128)          psb->setMass(IDX((rx-1)/2,ry-1),0);
00728         if(fixeds&256)          psb->setMass(IDX((rx-1)/2,(ry-1)/2),0);
00729         delete[] x;
00730         delete[] m;
00731 
00732 
00733         int z = 0;
00734         /* Create links and faces       */ 
00735         for(iy=0;iy<ry;++iy)
00736         {
00737                 for(int ix=0;ix<rx;++ix)
00738                 {
00739                         const bool      mdx=(ix+1)<rx;
00740                         const bool      mdy=(iy+1)<ry;
00741 
00742                         int node00=IDX(ix,iy);
00743                         int node01=IDX(ix+1,iy);
00744                         int node10=IDX(ix,iy+1);
00745                         int node11=IDX(ix+1,iy+1);
00746 
00747                         if(mdx) psb->appendLink(node00,node01);
00748                         if(mdy) psb->appendLink(node00,node10);
00749                         if(mdx&&mdy)
00750                         {
00751                                 psb->appendFace(node00,node10,node11);
00752                                 if (tex_coords) {
00753                                         tex_coords[z+0]=CalculateUV(resx,resy,ix,iy,0);
00754                                         tex_coords[z+1]=CalculateUV(resx,resy,ix,iy,1);
00755                                         tex_coords[z+2]=CalculateUV(resx,resy,ix,iy,0);
00756                                         tex_coords[z+3]=CalculateUV(resx,resy,ix,iy,2);
00757                                         tex_coords[z+4]=CalculateUV(resx,resy,ix,iy,3);
00758                                         tex_coords[z+5]=CalculateUV(resx,resy,ix,iy,2);
00759                                 }
00760                                 psb->appendFace(node11,node01,node00);
00761                                 if (tex_coords) {
00762                                         tex_coords[z+6 ]=CalculateUV(resx,resy,ix,iy,3);
00763                                         tex_coords[z+7 ]=CalculateUV(resx,resy,ix,iy,2);
00764                                         tex_coords[z+8 ]=CalculateUV(resx,resy,ix,iy,3);
00765                                         tex_coords[z+9 ]=CalculateUV(resx,resy,ix,iy,1);
00766                                         tex_coords[z+10]=CalculateUV(resx,resy,ix,iy,0);
00767                                         tex_coords[z+11]=CalculateUV(resx,resy,ix,iy,1);
00768                                 }
00769                                 if (gendiags) psb->appendLink(node00,node11);
00770                                 z += 12;
00771                         }
00772                 }
00773         }
00774         /* Finished     */ 
00775 #undef IDX
00776         return(psb);
00777 }
00778 
00779 float   btSoftBodyHelpers::CalculateUV(int resx,int resy,int ix,int iy,int id)
00780 {
00781 
00782         /*
00783         *
00784         *
00785         *    node00 --- node01
00786         *      |          |
00787         *    node10 --- node11
00788         *
00789         *
00790         *   ID map:
00791         *
00792         *   node00 s --> 0
00793         *   node00 t --> 1
00794         *
00795         *   node01 s --> 3
00796         *   node01 t --> 1
00797         *
00798         *   node10 s --> 0
00799         *   node10 t --> 2
00800         *
00801         *   node11 s --> 3
00802         *   node11 t --> 2
00803         *
00804         *
00805         */
00806 
00807         float tc=0.0f;
00808         if (id == 0) {
00809                 tc = (1.0f/((resx-1))*ix);
00810         }
00811         else if (id==1) {
00812                 tc = (1.0f/((resy-1))*(resy-1-iy));
00813         }
00814         else if (id==2) {
00815                 tc = (1.0f/((resy-1))*(resy-1-iy-1));
00816         }
00817         else if (id==3) {
00818                 tc = (1.0f/((resx-1))*(ix+1));
00819         }
00820         return tc;
00821 }
00822 //
00823 btSoftBody*             btSoftBodyHelpers::CreateEllipsoid(btSoftBodyWorldInfo& worldInfo,const btVector3& center,
00824                                                                                                    const btVector3& radius,
00825                                                                                                    int res)
00826 {
00827         struct  Hammersley
00828         {
00829                 static void     Generate(btVector3* x,int n)
00830                 {
00831                         for(int i=0;i<n;i++)
00832                         {
00833                                 btScalar        p=0.5,t=0;
00834                                 for(int j=i;j;p*=0.5,j>>=1) if(j&1) t+=p;
00835                                 btScalar        w=2*t-1;
00836                                 btScalar        a=(SIMD_PI+2*i*SIMD_PI)/n;
00837                                 btScalar        s=btSqrt(1-w*w);
00838                                 *x++=btVector3(s*btCos(a),s*btSin(a),w);
00839                         }
00840                 }
00841         };
00842         btAlignedObjectArray<btVector3> vtx;
00843         vtx.resize(3+res);
00844         Hammersley::Generate(&vtx[0],vtx.size());
00845         for(int i=0;i<vtx.size();++i)
00846         {
00847                 vtx[i]=vtx[i]*radius+center;
00848         }
00849         return(CreateFromConvexHull(worldInfo,&vtx[0],vtx.size()));
00850 }
00851 
00852 
00853 
00854 //
00855 btSoftBody*             btSoftBodyHelpers::CreateFromTriMesh(btSoftBodyWorldInfo& worldInfo,const btScalar*     vertices,
00856                                                                                                          const int* triangles,
00857                                                                                                          int ntriangles, bool randomizeConstraints)
00858 {
00859         int             maxidx=0;
00860         int i,j,ni;
00861 
00862         for(i=0,ni=ntriangles*3;i<ni;++i)
00863         {
00864                 maxidx=btMax(triangles[i],maxidx);
00865         }
00866         ++maxidx;
00867         btAlignedObjectArray<bool>              chks;
00868         btAlignedObjectArray<btVector3> vtx;
00869         chks.resize(maxidx*maxidx,false);
00870         vtx.resize(maxidx);
00871         for(i=0,j=0,ni=maxidx*3;i<ni;++j,i+=3)
00872         {
00873                 vtx[j]=btVector3(vertices[i],vertices[i+1],vertices[i+2]);
00874         }
00875         btSoftBody*             psb=new btSoftBody(&worldInfo,vtx.size(),&vtx[0],0);
00876         for( i=0,ni=ntriangles*3;i<ni;i+=3)
00877         {
00878                 const int idx[]={triangles[i],triangles[i+1],triangles[i+2]};
00879 #define IDX(_x_,_y_) ((_y_)*maxidx+(_x_))
00880                 for(int j=2,k=0;k<3;j=k++)
00881                 {
00882                         if(!chks[IDX(idx[j],idx[k])])
00883                         {
00884                                 chks[IDX(idx[j],idx[k])]=true;
00885                                 chks[IDX(idx[k],idx[j])]=true;
00886                                 psb->appendLink(idx[j],idx[k]);
00887                         }
00888                 }
00889 #undef IDX
00890                 psb->appendFace(idx[0],idx[1],idx[2]);
00891         }
00892 
00893         if (randomizeConstraints)
00894         {
00895                 psb->randomizeConstraints();
00896         }
00897 
00898         return(psb);
00899 }
00900 
00901 //
00902 btSoftBody*             btSoftBodyHelpers::CreateFromConvexHull(btSoftBodyWorldInfo& worldInfo, const btVector3* vertices,
00903                                                                                                                 int nvertices, bool randomizeConstraints)
00904 {
00905         HullDesc                hdsc(QF_TRIANGLES,nvertices,vertices);
00906         HullResult              hres;
00907         HullLibrary             hlib;/*??*/ 
00908         hdsc.mMaxVertices=nvertices;
00909         hlib.CreateConvexHull(hdsc,hres);
00910         btSoftBody*             psb=new btSoftBody(&worldInfo,(int)hres.mNumOutputVertices,
00911                 &hres.m_OutputVertices[0],0);
00912         for(int i=0;i<(int)hres.mNumFaces;++i)
00913         {
00914                 const int idx[]={       hres.m_Indices[i*3+0],
00915                         hres.m_Indices[i*3+1],
00916                         hres.m_Indices[i*3+2]};
00917                 if(idx[0]<idx[1]) psb->appendLink(      idx[0],idx[1]);
00918                 if(idx[1]<idx[2]) psb->appendLink(      idx[1],idx[2]);
00919                 if(idx[2]<idx[0]) psb->appendLink(      idx[2],idx[0]);
00920                 psb->appendFace(idx[0],idx[1],idx[2]);
00921         }
00922         hlib.ReleaseResult(hres);
00923         if (randomizeConstraints)
00924         {
00925                 psb->randomizeConstraints();
00926         }
00927         return(psb);
00928 }
00929 
00930 
00931 
00932 
00933 static int nextLine(const char* buffer)
00934 {
00935         int numBytesRead=0;
00936 
00937         while (*buffer != '\n')
00938         {
00939                 buffer++;
00940                 numBytesRead++;
00941         }
00942 
00943         
00944         if (buffer[0]==0x0a)
00945         {
00946                 buffer++;
00947                 numBytesRead++;
00948         }
00949         return numBytesRead;
00950 }
00951 
00952 /* Create from TetGen .ele, .face, .node data                                                   */ 
00953 btSoftBody*     btSoftBodyHelpers::CreateFromTetGenData(btSoftBodyWorldInfo& worldInfo,
00954                                                                                                         const char* ele,
00955                                                                                                         const char* face,
00956                                                                                                         const char* node,
00957                                                                                                         bool bfacelinks,
00958                                                                                                         bool btetralinks,
00959                                                                                                         bool bfacesfromtetras)
00960 {
00961 btAlignedObjectArray<btVector3> pos;
00962 int                                                             nnode=0;
00963 int                                                             ndims=0;
00964 int                                                             nattrb=0;
00965 int                                                             hasbounds=0;
00966 int result = sscanf(node,"%d %d %d %d",&nnode,&ndims,&nattrb,&hasbounds);
00967 result = sscanf(node,"%d %d %d %d",&nnode,&ndims,&nattrb,&hasbounds);
00968 node += nextLine(node);
00969 
00970 pos.resize(nnode);
00971 for(int i=0;i<pos.size();++i)
00972         {
00973         int                     index=0;
00974         //int                   bound=0;
00975         float   x,y,z;
00976         sscanf(node,"%d %f %f %f",&index,&x,&y,&z);
00977 
00978 //      sn>>index;
00979 //      sn>>x;sn>>y;sn>>z;
00980         node += nextLine(node);
00981 
00982         //for(int j=0;j<nattrb;++j) 
00983         //      sn>>a;
00984 
00985         //if(hasbounds) 
00986         //      sn>>bound;
00987 
00988         pos[index].setX(btScalar(x));
00989         pos[index].setY(btScalar(y));
00990         pos[index].setZ(btScalar(z));
00991         }
00992 btSoftBody*                                             psb=new btSoftBody(&worldInfo,nnode,&pos[0],0);
00993 #if 0
00994 if(face&&face[0])
00995         {
00996         int                                                             nface=0;
00997         sf>>nface;sf>>hasbounds;
00998         for(int i=0;i<nface;++i)
00999                 {
01000                 int                     index=0;
01001                 int                     bound=0;
01002                 int                     ni[3];
01003                 sf>>index;
01004                 sf>>ni[0];sf>>ni[1];sf>>ni[2];
01005                 sf>>bound;
01006                 psb->appendFace(ni[0],ni[1],ni[2]);     
01007                 if(btetralinks)
01008                         {
01009                         psb->appendLink(ni[0],ni[1],0,true);
01010                         psb->appendLink(ni[1],ni[2],0,true);
01011                         psb->appendLink(ni[2],ni[0],0,true);
01012                         }
01013                 }
01014         }
01015 #endif
01016 
01017 if(ele&&ele[0])
01018         {
01019         int                                                             ntetra=0;
01020         int                                                             ncorner=0;
01021         int                                                             neattrb=0;
01022         sscanf(ele,"%d %d %d",&ntetra,&ncorner,&neattrb);
01023         ele += nextLine(ele);
01024         
01025         //se>>ntetra;se>>ncorner;se>>neattrb;
01026         for(int i=0;i<ntetra;++i)
01027                 {
01028                 int                     index=0;
01029                 int                     ni[4];
01030 
01031                 //se>>index;
01032                 //se>>ni[0];se>>ni[1];se>>ni[2];se>>ni[3];
01033                 sscanf(ele,"%d %d %d %d %d",&index,&ni[0],&ni[1],&ni[2],&ni[3]);
01034                 ele+=nextLine(ele);
01035                 //for(int j=0;j<neattrb;++j) 
01036                 //      se>>a;
01037                 psb->appendTetra(ni[0],ni[1],ni[2],ni[3]);
01038                 if(btetralinks)
01039                         {
01040                         psb->appendLink(ni[0],ni[1],0,true);
01041                         psb->appendLink(ni[1],ni[2],0,true);
01042                         psb->appendLink(ni[2],ni[0],0,true);
01043                         psb->appendLink(ni[0],ni[3],0,true);
01044                         psb->appendLink(ni[1],ni[3],0,true);
01045                         psb->appendLink(ni[2],ni[3],0,true);
01046                         }
01047                 }
01048         }
01049 printf("Nodes:  %u\r\n",psb->m_nodes.size());
01050 printf("Links:  %u\r\n",psb->m_links.size());
01051 printf("Faces:  %u\r\n",psb->m_faces.size());
01052 printf("Tetras: %u\r\n",psb->m_tetras.size());
01053 return(psb);
01054 }
01055