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