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