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::CreateEllipsoid(btSoftBodyWorldInfo& worldInfo,const btVector3& center,
 557                                                                 const btVector3& radius,
 558                                                                 int res)
 559 {
 560         struct  Hammersley
 561         {
 562                 static void     Generate(btVector3* x,int n)
 563                 {
 564                         for(int i=0;i<n;i++)
 565                         {
 566                                 btScalar        p=0.5,t=0;
 567                                 for(int j=i;j;p*=0.5,j>>=1) if(j&1) t+=p;
 568                                 btScalar        w=2*t-1;
 569                                 btScalar        a=(SIMD_PI+2*i*SIMD_PI)/n;
 570                                 btScalar        s=btSqrt(1-w*w);
 571                                 *x++=btVector3(s*btCos(a),s*btSin(a),w);
 572                         }
 573                 }
 574         };
 575         btAlignedObjectArray<btVector3> vtx;
 576         vtx.resize(3+res);
 577         Hammersley::Generate(&vtx[0],vtx.size());
 578         for(int i=0;i<vtx.size();++i)
 579         {
 580                 vtx[i]=vtx[i]*radius+center;
 581         }
 582         return(CreateFromConvexHull(worldInfo,&vtx[0],vtx.size()));
 583 }
 584
 585
 586
 587 //
 588 btSoftBody*             btSoftBodyHelpers::CreateFromTriMesh(btSoftBodyWorldInfo& worldInfo,const btScalar*     vertices,
 589                                                                   const int* triangles,
 590                                                                   int ntriangles)
 591 {
 592         int             maxidx=0;
 593         int i,j,ni;
 594
 595         for(i=0,ni=ntriangles*3;i<ni;++i)
 596         {
 597                 maxidx=btMax(triangles[i],maxidx);
 598         }
 599         ++maxidx;
 600         btAlignedObjectArray<bool>              chks;
 601         btAlignedObjectArray<btVector3> vtx;
 602         chks.resize(maxidx*maxidx,false);
 603         vtx.resize(maxidx);
 604         for(i=0,j=0,ni=maxidx*3;i<ni;++j,i+=3)
 605         {
 606                 vtx[j]=btVector3(vertices[i],vertices[i+1],vertices[i+2]);
 607         }
 608         btSoftBody*             psb=new btSoftBody(&worldInfo,vtx.size(),&vtx[0],0);
 609         for( i=0,ni=ntriangles*3;i<ni;i+=3)
 610         {
 611                 const int idx[]={triangles[i],triangles[i+1],triangles[i+2]};
 612 #define IDX(_x_,_y_) ((_y_)*maxidx+(_x_))
 613                 for(int j=2,k=0;k<3;j=k++)
 614                 {
 615                         if(!chks[IDX(idx[j],idx[k])])
 616                         {
 617                                 chks[IDX(idx[j],idx[k])]=true;
 618                                 chks[IDX(idx[k],idx[k])]=true;
 619                                 psb->appendLink(idx[j],idx[k]);
 620                         }
 621                 }
 622 #undef IDX
 623                 psb->appendFace(idx[0],idx[1],idx[2]);
 624         }
 625         psb->randomizeConstraints();
 626         return(psb);
 627 }
 628
 629 //
 630 btSoftBody*             btSoftBodyHelpers::CreateFromConvexHull(btSoftBodyWorldInfo& worldInfo, const btVector3* vertices,
 631                                                                          int nvertices)
 632 {
 633         HullDesc                hdsc(QF_TRIANGLES,nvertices,vertices);
 634         HullResult              hres;
 635         HullLibrary             hlib;/*??*/
 636         hdsc.mMaxVertices=nvertices;
 637         hlib.CreateConvexHull(hdsc,hres);
 638         btSoftBody*             psb=new btSoftBody(&worldInfo,(int)hres.mNumOutputVertices,
 639                 &hres.m_OutputVertices[0],0);
 640         for(int i=0;i<(int)hres.mNumFaces;++i)
 641         {
 642                 const int idx[]={       hres.m_Indices[i*3+0],
 643                         hres.m_Indices[i*3+1],
 644                         hres.m_Indices[i*3+2]};
 645                 if(idx[0]<idx[1]) psb->appendLink(      idx[0],idx[1]);
 646                 if(idx[1]<idx[2]) psb->appendLink(      idx[1],idx[2]);
 647                 if(idx[2]<idx[0]) psb->appendLink(      idx[2],idx[0]);
 648                 psb->appendFace(idx[0],idx[1],idx[2]);
 649         }
 650         hlib.ReleaseResult(hres);
 651         psb->randomizeConstraints();
 652         return(psb);
 653 }