|
Blender
V2.59
|
00001 /* 00002 Bullet Continuous Collision Detection and Physics Library 00003 Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ 00004 00005 This software is provided 'as-is', without any express or implied warranty. 00006 In no event will the authors be held liable for any damages arising from the use of this software. 00007 Permission is granted to anyone to use this software for any purpose, 00008 including commercial applications, and to alter it and redistribute it freely, 00009 subject to the following restrictions: 00010 00011 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. 00012 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. 00013 3. This notice may not be removed or altered from any source distribution. 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 /* Clusters */ 00169 if(0!=(drawflags&fDrawFlags::Clusters)) 00170 { 00171 srand(1806); 00172 for(i=0;i<psb->m_clusters.size();++i) 00173 { 00174 if(psb->m_clusters[i]->m_collide) 00175 { 00176 btVector3 color( rand()/(btScalar)RAND_MAX, 00177 rand()/(btScalar)RAND_MAX, 00178 rand()/(btScalar)RAND_MAX); 00179 color=color.normalized()*0.75; 00180 btAlignedObjectArray<btVector3> vertices; 00181 vertices.resize(psb->m_clusters[i]->m_nodes.size()); 00182 for(j=0,nj=vertices.size();j<nj;++j) 00183 { 00184 vertices[j]=psb->m_clusters[i]->m_nodes[j]->m_x; 00185 } 00186 HullDesc hdsc(QF_TRIANGLES,vertices.size(),&vertices[0]); 00187 HullResult hres; 00188 HullLibrary hlib; 00189 hdsc.mMaxVertices=vertices.size(); 00190 hlib.CreateConvexHull(hdsc,hres); 00191 const btVector3 center=average(hres.m_OutputVertices); 00192 add(hres.m_OutputVertices,-center); 00193 mul(hres.m_OutputVertices,(btScalar)1); 00194 add(hres.m_OutputVertices,center); 00195 for(j=0;j<(int)hres.mNumFaces;++j) 00196 { 00197 const int idx[]={hres.m_Indices[j*3+0],hres.m_Indices[j*3+1],hres.m_Indices[j*3+2]}; 00198 idraw->drawTriangle(hres.m_OutputVertices[idx[0]], 00199 hres.m_OutputVertices[idx[1]], 00200 hres.m_OutputVertices[idx[2]], 00201 color,1); 00202 } 00203 hlib.ReleaseResult(hres); 00204 } 00205 /* Velocities */ 00206 #if 0 00207 for(int j=0;j<psb->m_clusters[i].m_nodes.size();++j) 00208 { 00209 const btSoftBody::Cluster& c=psb->m_clusters[i]; 00210 const btVector3 r=c.m_nodes[j]->m_x-c.m_com; 00211 const btVector3 v=c.m_lv+btCross(c.m_av,r); 00212 idraw->drawLine(c.m_nodes[j]->m_x,c.m_nodes[j]->m_x+v,btVector3(1,0,0)); 00213 } 00214 #endif 00215 /* Frame */ 00216 // btSoftBody::Cluster& c=*psb->m_clusters[i]; 00217 // idraw->drawLine(c.m_com,c.m_framexform*btVector3(10,0,0),btVector3(1,0,0)); 00218 // idraw->drawLine(c.m_com,c.m_framexform*btVector3(0,10,0),btVector3(0,1,0)); 00219 // idraw->drawLine(c.m_com,c.m_framexform*btVector3(0,0,10),btVector3(0,0,1)); 00220 } 00221 } 00222 else 00223 { 00224 /* Nodes */ 00225 if(0!=(drawflags&fDrawFlags::Nodes)) 00226 { 00227 for(i=0;i<psb->m_nodes.size();++i) 00228 { 00229 const btSoftBody::Node& n=psb->m_nodes[i]; 00230 if(0==(n.m_material->m_flags&btSoftBody::fMaterial::DebugDraw)) continue; 00231 idraw->drawLine(n.m_x-btVector3(scl,0,0),n.m_x+btVector3(scl,0,0),btVector3(1,0,0)); 00232 idraw->drawLine(n.m_x-btVector3(0,scl,0),n.m_x+btVector3(0,scl,0),btVector3(0,1,0)); 00233 idraw->drawLine(n.m_x-btVector3(0,0,scl),n.m_x+btVector3(0,0,scl),btVector3(0,0,1)); 00234 } 00235 } 00236 /* Links */ 00237 if(0!=(drawflags&fDrawFlags::Links)) 00238 { 00239 for(i=0;i<psb->m_links.size();++i) 00240 { 00241 const btSoftBody::Link& l=psb->m_links[i]; 00242 if(0==(l.m_material->m_flags&btSoftBody::fMaterial::DebugDraw)) continue; 00243 idraw->drawLine(l.m_n[0]->m_x,l.m_n[1]->m_x,lcolor); 00244 } 00245 } 00246 /* Normals */ 00247 if(0!=(drawflags&fDrawFlags::Normals)) 00248 { 00249 for(i=0;i<psb->m_nodes.size();++i) 00250 { 00251 const btSoftBody::Node& n=psb->m_nodes[i]; 00252 if(0==(n.m_material->m_flags&btSoftBody::fMaterial::DebugDraw)) continue; 00253 const btVector3 d=n.m_n*nscl; 00254 idraw->drawLine(n.m_x,n.m_x+d,ncolor); 00255 idraw->drawLine(n.m_x,n.m_x-d,ncolor*0.5); 00256 } 00257 } 00258 /* Contacts */ 00259 if(0!=(drawflags&fDrawFlags::Contacts)) 00260 { 00261 static const btVector3 axis[]={btVector3(1,0,0), 00262 btVector3(0,1,0), 00263 btVector3(0,0,1)}; 00264 for(i=0;i<psb->m_rcontacts.size();++i) 00265 { 00266 const btSoftBody::RContact& c=psb->m_rcontacts[i]; 00267 const btVector3 o= c.m_node->m_x-c.m_cti.m_normal* 00268 (btDot(c.m_node->m_x,c.m_cti.m_normal)+c.m_cti.m_offset); 00269 const btVector3 x=btCross(c.m_cti.m_normal,axis[c.m_cti.m_normal.minAxis()]).normalized(); 00270 const btVector3 y=btCross(x,c.m_cti.m_normal).normalized(); 00271 idraw->drawLine(o-x*nscl,o+x*nscl,ccolor); 00272 idraw->drawLine(o-y*nscl,o+y*nscl,ccolor); 00273 idraw->drawLine(o,o+c.m_cti.m_normal*nscl*3,btVector3(1,1,0)); 00274 } 00275 } 00276 /* Faces */ 00277 if(0!=(drawflags&fDrawFlags::Faces)) 00278 { 00279 const btScalar scl=(btScalar)0.8; 00280 const btScalar alp=(btScalar)1; 00281 const btVector3 col(0,(btScalar)0.7,0); 00282 for(i=0;i<psb->m_faces.size();++i) 00283 { 00284 const btSoftBody::Face& f=psb->m_faces[i]; 00285 if(0==(f.m_material->m_flags&btSoftBody::fMaterial::DebugDraw)) continue; 00286 const btVector3 x[]={f.m_n[0]->m_x,f.m_n[1]->m_x,f.m_n[2]->m_x}; 00287 const btVector3 c=(x[0]+x[1]+x[2])/3; 00288 idraw->drawTriangle((x[0]-c)*scl+c, 00289 (x[1]-c)*scl+c, 00290 (x[2]-c)*scl+c, 00291 col,alp); 00292 } 00293 } 00294 /* Tetras */ 00295 if(0!=(drawflags&fDrawFlags::Tetras)) 00296 { 00297 const btScalar scl=(btScalar)0.8; 00298 const btScalar alp=(btScalar)1; 00299 const btVector3 col((btScalar)0.7,(btScalar)0.7,(btScalar)0.7); 00300 for(int i=0;i<psb->m_tetras.size();++i) 00301 { 00302 const btSoftBody::Tetra& t=psb->m_tetras[i]; 00303 if(0==(t.m_material->m_flags&btSoftBody::fMaterial::DebugDraw)) continue; 00304 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}; 00305 const btVector3 c=(x[0]+x[1]+x[2]+x[3])/4; 00306 idraw->drawTriangle((x[0]-c)*scl+c,(x[1]-c)*scl+c,(x[2]-c)*scl+c,col,alp); 00307 idraw->drawTriangle((x[0]-c)*scl+c,(x[1]-c)*scl+c,(x[3]-c)*scl+c,col,alp); 00308 idraw->drawTriangle((x[1]-c)*scl+c,(x[2]-c)*scl+c,(x[3]-c)*scl+c,col,alp); 00309 idraw->drawTriangle((x[2]-c)*scl+c,(x[0]-c)*scl+c,(x[3]-c)*scl+c,col,alp); 00310 } 00311 } 00312 } 00313 /* Anchors */ 00314 if(0!=(drawflags&fDrawFlags::Anchors)) 00315 { 00316 for(i=0;i<psb->m_anchors.size();++i) 00317 { 00318 const btSoftBody::Anchor& a=psb->m_anchors[i]; 00319 const btVector3 q=a.m_body->getWorldTransform()*a.m_local; 00320 drawVertex(idraw,a.m_node->m_x,0.25,btVector3(1,0,0)); 00321 drawVertex(idraw,q,0.25,btVector3(0,1,0)); 00322 idraw->drawLine(a.m_node->m_x,q,btVector3(1,1,1)); 00323 } 00324 for(i=0;i<psb->m_nodes.size();++i) 00325 { 00326 const btSoftBody::Node& n=psb->m_nodes[i]; 00327 if(0==(n.m_material->m_flags&btSoftBody::fMaterial::DebugDraw)) continue; 00328 if(n.m_im<=0) 00329 { 00330 drawVertex(idraw,n.m_x,0.25,btVector3(1,0,0)); 00331 } 00332 } 00333 } 00334 00335 00336 /* Notes */ 00337 if(0!=(drawflags&fDrawFlags::Notes)) 00338 { 00339 for(i=0;i<psb->m_notes.size();++i) 00340 { 00341 const btSoftBody::Note& n=psb->m_notes[i]; 00342 btVector3 p=n.m_offset; 00343 for(int j=0;j<n.m_rank;++j) 00344 { 00345 p+=n.m_nodes[j]->m_x*n.m_coords[j]; 00346 } 00347 idraw->draw3dText(p,n.m_text); 00348 } 00349 } 00350 /* Node tree */ 00351 if(0!=(drawflags&fDrawFlags::NodeTree)) DrawNodeTree(psb,idraw); 00352 /* Face tree */ 00353 if(0!=(drawflags&fDrawFlags::FaceTree)) DrawFaceTree(psb,idraw); 00354 /* Cluster tree */ 00355 if(0!=(drawflags&fDrawFlags::ClusterTree)) DrawClusterTree(psb,idraw); 00356 /* Joints */ 00357 if(0!=(drawflags&fDrawFlags::Joints)) 00358 { 00359 for(i=0;i<psb->m_joints.size();++i) 00360 { 00361 const btSoftBody::Joint* pj=psb->m_joints[i]; 00362 switch(pj->Type()) 00363 { 00364 case btSoftBody::Joint::eType::Linear: 00365 { 00366 const btSoftBody::LJoint* pjl=(const btSoftBody::LJoint*)pj; 00367 const btVector3 a0=pj->m_bodies[0].xform()*pjl->m_refs[0]; 00368 const btVector3 a1=pj->m_bodies[1].xform()*pjl->m_refs[1]; 00369 idraw->drawLine(pj->m_bodies[0].xform().getOrigin(),a0,btVector3(1,1,0)); 00370 idraw->drawLine(pj->m_bodies[1].xform().getOrigin(),a1,btVector3(0,1,1)); 00371 drawVertex(idraw,a0,0.25,btVector3(1,1,0)); 00372 drawVertex(idraw,a1,0.25,btVector3(0,1,1)); 00373 } 00374 break; 00375 case btSoftBody::Joint::eType::Angular: 00376 { 00377 //const btSoftBody::AJoint* pja=(const btSoftBody::AJoint*)pj; 00378 const btVector3 o0=pj->m_bodies[0].xform().getOrigin(); 00379 const btVector3 o1=pj->m_bodies[1].xform().getOrigin(); 00380 const btVector3 a0=pj->m_bodies[0].xform().getBasis()*pj->m_refs[0]; 00381 const btVector3 a1=pj->m_bodies[1].xform().getBasis()*pj->m_refs[1]; 00382 idraw->drawLine(o0,o0+a0*10,btVector3(1,1,0)); 00383 idraw->drawLine(o0,o0+a1*10,btVector3(1,1,0)); 00384 idraw->drawLine(o1,o1+a0*10,btVector3(0,1,1)); 00385 idraw->drawLine(o1,o1+a1*10,btVector3(0,1,1)); 00386 break; 00387 } 00388 default: 00389 { 00390 } 00391 00392 } 00393 } 00394 } 00395 } 00396 00397 // 00398 void btSoftBodyHelpers::DrawInfos( btSoftBody* psb, 00399 btIDebugDraw* idraw, 00400 bool masses, 00401 bool areas, 00402 bool /*stress*/) 00403 { 00404 for(int i=0;i<psb->m_nodes.size();++i) 00405 { 00406 const btSoftBody::Node& n=psb->m_nodes[i]; 00407 char text[2048]={0}; 00408 char buff[1024]; 00409 if(masses) 00410 { 00411 sprintf(buff," M(%.2f)",1/n.m_im); 00412 strcat(text,buff); 00413 } 00414 if(areas) 00415 { 00416 sprintf(buff," A(%.2f)",n.m_area); 00417 strcat(text,buff); 00418 } 00419 if(text[0]) idraw->draw3dText(n.m_x,text); 00420 } 00421 } 00422 00423 // 00424 void btSoftBodyHelpers::DrawNodeTree( btSoftBody* psb, 00425 btIDebugDraw* idraw, 00426 int mindepth, 00427 int maxdepth) 00428 { 00429 drawTree(idraw,psb->m_ndbvt.m_root,0,btVector3(1,0,1),btVector3(1,1,1),mindepth,maxdepth); 00430 } 00431 00432 // 00433 void btSoftBodyHelpers::DrawFaceTree( btSoftBody* psb, 00434 btIDebugDraw* idraw, 00435 int mindepth, 00436 int maxdepth) 00437 { 00438 drawTree(idraw,psb->m_fdbvt.m_root,0,btVector3(0,1,0),btVector3(1,0,0),mindepth,maxdepth); 00439 } 00440 00441 // 00442 void btSoftBodyHelpers::DrawClusterTree( btSoftBody* psb, 00443 btIDebugDraw* idraw, 00444 int mindepth, 00445 int maxdepth) 00446 { 00447 drawTree(idraw,psb->m_cdbvt.m_root,0,btVector3(0,1,1),btVector3(1,0,0),mindepth,maxdepth); 00448 } 00449 00450 // 00451 void btSoftBodyHelpers::DrawFrame( btSoftBody* psb, 00452 btIDebugDraw* idraw) 00453 { 00454 if(psb->m_pose.m_bframe) 00455 { 00456 static const btScalar ascl=10; 00457 static const btScalar nscl=(btScalar)0.1; 00458 const btVector3 com=psb->m_pose.m_com; 00459 const btMatrix3x3 trs=psb->m_pose.m_rot*psb->m_pose.m_scl; 00460 const btVector3 Xaxis=(trs*btVector3(1,0,0)).normalized(); 00461 const btVector3 Yaxis=(trs*btVector3(0,1,0)).normalized(); 00462 const btVector3 Zaxis=(trs*btVector3(0,0,1)).normalized(); 00463 idraw->drawLine(com,com+Xaxis*ascl,btVector3(1,0,0)); 00464 idraw->drawLine(com,com+Yaxis*ascl,btVector3(0,1,0)); 00465 idraw->drawLine(com,com+Zaxis*ascl,btVector3(0,0,1)); 00466 for(int i=0;i<psb->m_pose.m_pos.size();++i) 00467 { 00468 const btVector3 x=com+trs*psb->m_pose.m_pos[i]; 00469 drawVertex(idraw,x,nscl,btVector3(1,0,1)); 00470 } 00471 } 00472 } 00473 00474 // 00475 btSoftBody* btSoftBodyHelpers::CreateRope( btSoftBodyWorldInfo& worldInfo, const btVector3& from, 00476 const btVector3& to, 00477 int res, 00478 int fixeds) 00479 { 00480 /* Create nodes */ 00481 const int r=res+2; 00482 btVector3* x=new btVector3[r]; 00483 btScalar* m=new btScalar[r]; 00484 int i; 00485 00486 for(i=0;i<r;++i) 00487 { 00488 const btScalar t=i/(btScalar)(r-1); 00489 x[i]=lerp(from,to,t); 00490 m[i]=1; 00491 } 00492 btSoftBody* psb= new btSoftBody(&worldInfo,r,x,m); 00493 if(fixeds&1) psb->setMass(0,0); 00494 if(fixeds&2) psb->setMass(r-1,0); 00495 delete[] x; 00496 delete[] m; 00497 /* Create links */ 00498 for(i=1;i<r;++i) 00499 { 00500 psb->appendLink(i-1,i); 00501 } 00502 /* Finished */ 00503 return(psb); 00504 } 00505 00506 // 00507 btSoftBody* btSoftBodyHelpers::CreatePatch(btSoftBodyWorldInfo& worldInfo,const btVector3& corner00, 00508 const btVector3& corner10, 00509 const btVector3& corner01, 00510 const btVector3& corner11, 00511 int resx, 00512 int resy, 00513 int fixeds, 00514 bool gendiags) 00515 { 00516 #define IDX(_x_,_y_) ((_y_)*rx+(_x_)) 00517 /* Create nodes */ 00518 if((resx<2)||(resy<2)) return(0); 00519 const int rx=resx; 00520 const int ry=resy; 00521 const int tot=rx*ry; 00522 btVector3* x=new btVector3[tot]; 00523 btScalar* m=new btScalar[tot]; 00524 int iy; 00525 00526 for(iy=0;iy<ry;++iy) 00527 { 00528 const btScalar ty=iy/(btScalar)(ry-1); 00529 const btVector3 py0=lerp(corner00,corner01,ty); 00530 const btVector3 py1=lerp(corner10,corner11,ty); 00531 for(int ix=0;ix<rx;++ix) 00532 { 00533 const btScalar tx=ix/(btScalar)(rx-1); 00534 x[IDX(ix,iy)]=lerp(py0,py1,tx); 00535 m[IDX(ix,iy)]=1; 00536 } 00537 } 00538 btSoftBody* psb=new btSoftBody(&worldInfo,tot,x,m); 00539 if(fixeds&1) psb->setMass(IDX(0,0),0); 00540 if(fixeds&2) psb->setMass(IDX(rx-1,0),0); 00541 if(fixeds&4) psb->setMass(IDX(0,ry-1),0); 00542 if(fixeds&8) psb->setMass(IDX(rx-1,ry-1),0); 00543 delete[] x; 00544 delete[] m; 00545 /* Create links and faces */ 00546 for(iy=0;iy<ry;++iy) 00547 { 00548 for(int ix=0;ix<rx;++ix) 00549 { 00550 const int idx=IDX(ix,iy); 00551 const bool mdx=(ix+1)<rx; 00552 const bool mdy=(iy+1)<ry; 00553 if(mdx) psb->appendLink(idx,IDX(ix+1,iy)); 00554 if(mdy) psb->appendLink(idx,IDX(ix,iy+1)); 00555 if(mdx&&mdy) 00556 { 00557 if((ix+iy)&1) 00558 { 00559 psb->appendFace(IDX(ix,iy),IDX(ix+1,iy),IDX(ix+1,iy+1)); 00560 psb->appendFace(IDX(ix,iy),IDX(ix+1,iy+1),IDX(ix,iy+1)); 00561 if(gendiags) 00562 { 00563 psb->appendLink(IDX(ix,iy),IDX(ix+1,iy+1)); 00564 } 00565 } 00566 else 00567 { 00568 psb->appendFace(IDX(ix,iy+1),IDX(ix,iy),IDX(ix+1,iy)); 00569 psb->appendFace(IDX(ix,iy+1),IDX(ix+1,iy),IDX(ix+1,iy+1)); 00570 if(gendiags) 00571 { 00572 psb->appendLink(IDX(ix+1,iy),IDX(ix,iy+1)); 00573 } 00574 } 00575 } 00576 } 00577 } 00578 /* Finished */ 00579 #undef IDX 00580 return(psb); 00581 } 00582 00583 // 00584 btSoftBody* btSoftBodyHelpers::CreatePatchUV(btSoftBodyWorldInfo& worldInfo, 00585 const btVector3& corner00, 00586 const btVector3& corner10, 00587 const btVector3& corner01, 00588 const btVector3& corner11, 00589 int resx, 00590 int resy, 00591 int fixeds, 00592 bool gendiags, 00593 float* tex_coords) 00594 { 00595 00596 /* 00597 * 00598 * corners: 00599 * 00600 * [0][0] corner00 ------- corner01 [resx][0] 00601 * | | 00602 * | | 00603 * [0][resy] corner10 -------- corner11 [resx][resy] 00604 * 00605 * 00606 * 00607 * 00608 * 00609 * 00610 * "fixedgs" map: 00611 * 00612 * corner00 --> +1 00613 * corner01 --> +2 00614 * corner10 --> +4 00615 * corner11 --> +8 00616 * upper middle --> +16 00617 * left middle --> +32 00618 * right middle --> +64 00619 * lower middle --> +128 00620 * center --> +256 00621 * 00622 * 00623 * tex_coords size (resx-1)*(resy-1)*12 00624 * 00625 * 00626 * 00627 * SINGLE QUAD INTERNALS 00628 * 00629 * 1) btSoftBody's nodes and links, 00630 * diagonal link is optional ("gendiags") 00631 * 00632 * 00633 * node00 ------ node01 00634 * | . 00635 * | . 00636 * | . 00637 * | . 00638 * | . 00639 * node10 node11 00640 * 00641 * 00642 * 00643 * 2) Faces: 00644 * two triangles, 00645 * UV Coordinates (hier example for single quad) 00646 * 00647 * (0,1) (0,1) (1,1) 00648 * 1 |\ 3 \-----| 2 00649 * | \ \ | 00650 * | \ \ | 00651 * | \ \ | 00652 * | \ \ | 00653 * 2 |-----\ 3 \| 1 00654 * (0,0) (1,0) (1,0) 00655 * 00656 * 00657 * 00658 * 00659 * 00660 * 00661 */ 00662 00663 #define IDX(_x_,_y_) ((_y_)*rx+(_x_)) 00664 /* Create nodes */ 00665 if((resx<2)||(resy<2)) return(0); 00666 const int rx=resx; 00667 const int ry=resy; 00668 const int tot=rx*ry; 00669 btVector3* x=new btVector3[tot]; 00670 btScalar* m=new btScalar[tot]; 00671 00672 int iy; 00673 00674 for(iy=0;iy<ry;++iy) 00675 { 00676 const btScalar ty=iy/(btScalar)(ry-1); 00677 const btVector3 py0=lerp(corner00,corner01,ty); 00678 const btVector3 py1=lerp(corner10,corner11,ty); 00679 for(int ix=0;ix<rx;++ix) 00680 { 00681 const btScalar tx=ix/(btScalar)(rx-1); 00682 x[IDX(ix,iy)]=lerp(py0,py1,tx); 00683 m[IDX(ix,iy)]=1; 00684 } 00685 } 00686 btSoftBody* psb=new btSoftBody(&worldInfo,tot,x,m); 00687 if(fixeds&1) psb->setMass(IDX(0,0),0); 00688 if(fixeds&2) psb->setMass(IDX(rx-1,0),0); 00689 if(fixeds&4) psb->setMass(IDX(0,ry-1),0); 00690 if(fixeds&8) psb->setMass(IDX(rx-1,ry-1),0); 00691 if(fixeds&16) psb->setMass(IDX((rx-1)/2,0),0); 00692 if(fixeds&32) psb->setMass(IDX(0,(ry-1)/2),0); 00693 if(fixeds&64) psb->setMass(IDX(rx-1,(ry-1)/2),0); 00694 if(fixeds&128) psb->setMass(IDX((rx-1)/2,ry-1),0); 00695 if(fixeds&256) psb->setMass(IDX((rx-1)/2,(ry-1)/2),0); 00696 delete[] x; 00697 delete[] m; 00698 00699 00700 int z = 0; 00701 /* Create links and faces */ 00702 for(iy=0;iy<ry;++iy) 00703 { 00704 for(int ix=0;ix<rx;++ix) 00705 { 00706 const bool mdx=(ix+1)<rx; 00707 const bool mdy=(iy+1)<ry; 00708 00709 int node00=IDX(ix,iy); 00710 int node01=IDX(ix+1,iy); 00711 int node10=IDX(ix,iy+1); 00712 int node11=IDX(ix+1,iy+1); 00713 00714 if(mdx) psb->appendLink(node00,node01); 00715 if(mdy) psb->appendLink(node00,node10); 00716 if(mdx&&mdy) 00717 { 00718 psb->appendFace(node00,node10,node11); 00719 if (tex_coords) { 00720 tex_coords[z+0]=CalculateUV(resx,resy,ix,iy,0); 00721 tex_coords[z+1]=CalculateUV(resx,resy,ix,iy,1); 00722 tex_coords[z+2]=CalculateUV(resx,resy,ix,iy,0); 00723 tex_coords[z+3]=CalculateUV(resx,resy,ix,iy,2); 00724 tex_coords[z+4]=CalculateUV(resx,resy,ix,iy,3); 00725 tex_coords[z+5]=CalculateUV(resx,resy,ix,iy,2); 00726 } 00727 psb->appendFace(node11,node01,node00); 00728 if (tex_coords) { 00729 tex_coords[z+6 ]=CalculateUV(resx,resy,ix,iy,3); 00730 tex_coords[z+7 ]=CalculateUV(resx,resy,ix,iy,2); 00731 tex_coords[z+8 ]=CalculateUV(resx,resy,ix,iy,3); 00732 tex_coords[z+9 ]=CalculateUV(resx,resy,ix,iy,1); 00733 tex_coords[z+10]=CalculateUV(resx,resy,ix,iy,0); 00734 tex_coords[z+11]=CalculateUV(resx,resy,ix,iy,1); 00735 } 00736 if (gendiags) psb->appendLink(node00,node11); 00737 z += 12; 00738 } 00739 } 00740 } 00741 /* Finished */ 00742 #undef IDX 00743 return(psb); 00744 } 00745 00746 float btSoftBodyHelpers::CalculateUV(int resx,int resy,int ix,int iy,int id) 00747 { 00748 00749 /* 00750 * 00751 * 00752 * node00 --- node01 00753 * | | 00754 * node10 --- node11 00755 * 00756 * 00757 * ID map: 00758 * 00759 * node00 s --> 0 00760 * node00 t --> 1 00761 * 00762 * node01 s --> 3 00763 * node01 t --> 1 00764 * 00765 * node10 s --> 0 00766 * node10 t --> 2 00767 * 00768 * node11 s --> 3 00769 * node11 t --> 2 00770 * 00771 * 00772 */ 00773 00774 float tc=0.0f; 00775 if (id == 0) { 00776 tc = (1.0f/((resx-1))*ix); 00777 } 00778 else if (id==1) { 00779 tc = (1.0f/((resy-1))*(resy-1-iy)); 00780 } 00781 else if (id==2) { 00782 tc = (1.0f/((resy-1))*(resy-1-iy-1)); 00783 } 00784 else if (id==3) { 00785 tc = (1.0f/((resx-1))*(ix+1)); 00786 } 00787 return tc; 00788 } 00789 // 00790 btSoftBody* btSoftBodyHelpers::CreateEllipsoid(btSoftBodyWorldInfo& worldInfo,const btVector3& center, 00791 const btVector3& radius, 00792 int res) 00793 { 00794 struct Hammersley 00795 { 00796 static void Generate(btVector3* x,int n) 00797 { 00798 for(int i=0;i<n;i++) 00799 { 00800 btScalar p=0.5,t=0; 00801 for(int j=i;j;p*=0.5,j>>=1) if(j&1) t+=p; 00802 btScalar w=2*t-1; 00803 btScalar a=(SIMD_PI+2*i*SIMD_PI)/n; 00804 btScalar s=btSqrt(1-w*w); 00805 *x++=btVector3(s*btCos(a),s*btSin(a),w); 00806 } 00807 } 00808 }; 00809 btAlignedObjectArray<btVector3> vtx; 00810 vtx.resize(3+res); 00811 Hammersley::Generate(&vtx[0],vtx.size()); 00812 for(int i=0;i<vtx.size();++i) 00813 { 00814 vtx[i]=vtx[i]*radius+center; 00815 } 00816 return(CreateFromConvexHull(worldInfo,&vtx[0],vtx.size())); 00817 } 00818 00819 00820 00821 // 00822 btSoftBody* btSoftBodyHelpers::CreateFromTriMesh(btSoftBodyWorldInfo& worldInfo,const btScalar* vertices, 00823 const int* triangles, 00824 int ntriangles, bool randomizeConstraints) 00825 { 00826 int maxidx=0; 00827 int i,j,ni; 00828 00829 for(i=0,ni=ntriangles*3;i<ni;++i) 00830 { 00831 maxidx=btMax(triangles[i],maxidx); 00832 } 00833 ++maxidx; 00834 btAlignedObjectArray<bool> chks; 00835 btAlignedObjectArray<btVector3> vtx; 00836 chks.resize(maxidx*maxidx,false); 00837 vtx.resize(maxidx); 00838 for(i=0,j=0,ni=maxidx*3;i<ni;++j,i+=3) 00839 { 00840 vtx[j]=btVector3(vertices[i],vertices[i+1],vertices[i+2]); 00841 } 00842 btSoftBody* psb=new btSoftBody(&worldInfo,vtx.size(),&vtx[0],0); 00843 for( i=0,ni=ntriangles*3;i<ni;i+=3) 00844 { 00845 const int idx[]={triangles[i],triangles[i+1],triangles[i+2]}; 00846 #define IDX(_x_,_y_) ((_y_)*maxidx+(_x_)) 00847 for(int j=2,k=0;k<3;j=k++) 00848 { 00849 if(!chks[IDX(idx[j],idx[k])]) 00850 { 00851 chks[IDX(idx[j],idx[k])]=true; 00852 chks[IDX(idx[k],idx[j])]=true; 00853 psb->appendLink(idx[j],idx[k]); 00854 } 00855 } 00856 #undef IDX 00857 psb->appendFace(idx[0],idx[1],idx[2]); 00858 } 00859 00860 if (randomizeConstraints) 00861 { 00862 psb->randomizeConstraints(); 00863 } 00864 00865 return(psb); 00866 } 00867 00868 // 00869 btSoftBody* btSoftBodyHelpers::CreateFromConvexHull(btSoftBodyWorldInfo& worldInfo, const btVector3* vertices, 00870 int nvertices, bool randomizeConstraints) 00871 { 00872 HullDesc hdsc(QF_TRIANGLES,nvertices,vertices); 00873 HullResult hres; 00874 HullLibrary hlib;/*??*/ 00875 hdsc.mMaxVertices=nvertices; 00876 hlib.CreateConvexHull(hdsc,hres); 00877 btSoftBody* psb=new btSoftBody(&worldInfo,(int)hres.mNumOutputVertices, 00878 &hres.m_OutputVertices[0],0); 00879 for(int i=0;i<(int)hres.mNumFaces;++i) 00880 { 00881 const int idx[]={ hres.m_Indices[i*3+0], 00882 hres.m_Indices[i*3+1], 00883 hres.m_Indices[i*3+2]}; 00884 if(idx[0]<idx[1]) psb->appendLink( idx[0],idx[1]); 00885 if(idx[1]<idx[2]) psb->appendLink( idx[1],idx[2]); 00886 if(idx[2]<idx[0]) psb->appendLink( idx[2],idx[0]); 00887 psb->appendFace(idx[0],idx[1],idx[2]); 00888 } 00889 hlib.ReleaseResult(hres); 00890 if (randomizeConstraints) 00891 { 00892 psb->randomizeConstraints(); 00893 } 00894 return(psb); 00895 } 00896 00897 00898 00899 00900 static int nextLine(const char* buffer) 00901 { 00902 int numBytesRead=0; 00903 00904 while (*buffer != '\n') 00905 { 00906 buffer++; 00907 numBytesRead++; 00908 } 00909 00910 00911 if (buffer[0]==0x0a) 00912 { 00913 buffer++; 00914 numBytesRead++; 00915 } 00916 return numBytesRead; 00917 } 00918 00919 /* Create from TetGen .ele, .face, .node data */ 00920 btSoftBody* btSoftBodyHelpers::CreateFromTetGenData(btSoftBodyWorldInfo& worldInfo, 00921 const char* ele, 00922 const char* face, 00923 const char* node, 00924 bool bfacelinks, 00925 bool btetralinks, 00926 bool bfacesfromtetras) 00927 { 00928 btAlignedObjectArray<btVector3> pos; 00929 int nnode=0; 00930 int ndims=0; 00931 int nattrb=0; 00932 int hasbounds=0; 00933 int result = sscanf(node,"%d %d %d %d",&nnode,&ndims,&nattrb,&hasbounds); 00934 result = sscanf(node,"%d %d %d %d",&nnode,&ndims,&nattrb,&hasbounds); 00935 node += nextLine(node); 00936 00937 pos.resize(nnode); 00938 for(int i=0;i<pos.size();++i) 00939 { 00940 int index=0; 00941 //int bound=0; 00942 float x,y,z; 00943 sscanf(node,"%d %f %f %f",&index,&x,&y,&z); 00944 00945 // sn>>index; 00946 // sn>>x;sn>>y;sn>>z; 00947 node += nextLine(node); 00948 00949 //for(int j=0;j<nattrb;++j) 00950 // sn>>a; 00951 00952 //if(hasbounds) 00953 // sn>>bound; 00954 00955 pos[index].setX(btScalar(x)); 00956 pos[index].setY(btScalar(y)); 00957 pos[index].setZ(btScalar(z)); 00958 } 00959 btSoftBody* psb=new btSoftBody(&worldInfo,nnode,&pos[0],0); 00960 #if 0 00961 if(face&&face[0]) 00962 { 00963 int nface=0; 00964 sf>>nface;sf>>hasbounds; 00965 for(int i=0;i<nface;++i) 00966 { 00967 int index=0; 00968 int bound=0; 00969 int ni[3]; 00970 sf>>index; 00971 sf>>ni[0];sf>>ni[1];sf>>ni[2]; 00972 sf>>bound; 00973 psb->appendFace(ni[0],ni[1],ni[2]); 00974 if(btetralinks) 00975 { 00976 psb->appendLink(ni[0],ni[1],0,true); 00977 psb->appendLink(ni[1],ni[2],0,true); 00978 psb->appendLink(ni[2],ni[0],0,true); 00979 } 00980 } 00981 } 00982 #endif 00983 00984 if(ele&&ele[0]) 00985 { 00986 int ntetra=0; 00987 int ncorner=0; 00988 int neattrb=0; 00989 sscanf(ele,"%d %d %d",&ntetra,&ncorner,&neattrb); 00990 ele += nextLine(ele); 00991 00992 //se>>ntetra;se>>ncorner;se>>neattrb; 00993 for(int i=0;i<ntetra;++i) 00994 { 00995 int index=0; 00996 int ni[4]; 00997 00998 //se>>index; 00999 //se>>ni[0];se>>ni[1];se>>ni[2];se>>ni[3]; 01000 sscanf(ele,"%d %d %d %d %d",&index,&ni[0],&ni[1],&ni[2],&ni[3]); 01001 ele+=nextLine(ele); 01002 //for(int j=0;j<neattrb;++j) 01003 // se>>a; 01004 psb->appendTetra(ni[0],ni[1],ni[2],ni[3]); 01005 if(btetralinks) 01006 { 01007 psb->appendLink(ni[0],ni[1],0,true); 01008 psb->appendLink(ni[1],ni[2],0,true); 01009 psb->appendLink(ni[2],ni[0],0,true); 01010 psb->appendLink(ni[0],ni[3],0,true); 01011 psb->appendLink(ni[1],ni[3],0,true); 01012 psb->appendLink(ni[2],ni[3],0,true); 01013 } 01014 } 01015 } 01016 printf("Nodes: %u\r\n",psb->m_nodes.size()); 01017 printf("Links: %u\r\n",psb->m_links.size()); 01018 printf("Faces: %u\r\n",psb->m_faces.size()); 01019 printf("Tetras: %u\r\n",psb->m_tetras.size()); 01020 return(psb); 01021 } 01022