extern/bullet2/src/BulletSoftBody/btSoftBodyHelpers.cpp

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