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