| /* |
| Bullet Continuous Collision Detection and Physics Library |
| Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ |
| |
| This software is provided 'as-is', without any express or implied warranty. |
| In no event will the authors be held liable for any damages arising from the use of this software. |
| Permission is granted to anyone to use this software for any purpose, |
| including commercial applications, and to alter it and redistribute it freely, |
| subject to the following restrictions: |
| |
| 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. |
| 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. |
| 3. This notice may not be removed or altered from any source distribution. |
| */ |
| ///btSoftBodyHelpers.cpp by Nathanael Presson |
| |
| #include "BulletSoftBody/btSoftBodyInternals.h" |
| #include <stdio.h> |
| #include <string.h> |
| #include "BulletSoftBody/btSoftBodyHelpers.h" |
| #include "LinearMath/btConvexHull.h" |
| |
| // |
| static void drawVertex( btIDebugDraw* idraw, |
| const btVector3& x,btScalar s,const btVector3& c) |
| { |
| idraw->drawLine(x-btVector3(s,0,0),x+btVector3(s,0,0),c); |
| idraw->drawLine(x-btVector3(0,s,0),x+btVector3(0,s,0),c); |
| idraw->drawLine(x-btVector3(0,0,s),x+btVector3(0,0,s),c); |
| } |
| |
| // |
| static void drawBox( btIDebugDraw* idraw, |
| const btVector3& mins, |
| const btVector3& maxs, |
| const btVector3& color) |
| { |
| const btVector3 c[]={ btVector3(mins.x(),mins.y(),mins.z()), |
| btVector3(maxs.x(),mins.y(),mins.z()), |
| btVector3(maxs.x(),maxs.y(),mins.z()), |
| btVector3(mins.x(),maxs.y(),mins.z()), |
| btVector3(mins.x(),mins.y(),maxs.z()), |
| btVector3(maxs.x(),mins.y(),maxs.z()), |
| btVector3(maxs.x(),maxs.y(),maxs.z()), |
| btVector3(mins.x(),maxs.y(),maxs.z())}; |
| idraw->drawLine(c[0],c[1],color);idraw->drawLine(c[1],c[2],color); |
| idraw->drawLine(c[2],c[3],color);idraw->drawLine(c[3],c[0],color); |
| idraw->drawLine(c[4],c[5],color);idraw->drawLine(c[5],c[6],color); |
| idraw->drawLine(c[6],c[7],color);idraw->drawLine(c[7],c[4],color); |
| idraw->drawLine(c[0],c[4],color);idraw->drawLine(c[1],c[5],color); |
| idraw->drawLine(c[2],c[6],color);idraw->drawLine(c[3],c[7],color); |
| } |
| |
| // |
| static void drawTree( btIDebugDraw* idraw, |
| const btDbvtNode* node, |
| int depth, |
| const btVector3& ncolor, |
| const btVector3& lcolor, |
| int mindepth, |
| int maxdepth) |
| { |
| if(node) |
| { |
| if(node->isinternal()&&((depth<maxdepth)||(maxdepth<0))) |
| { |
| drawTree(idraw,node->childs[0],depth+1,ncolor,lcolor,mindepth,maxdepth); |
| drawTree(idraw,node->childs[1],depth+1,ncolor,lcolor,mindepth,maxdepth); |
| } |
| if(depth>=mindepth) |
| { |
| const btScalar scl=(btScalar)(node->isinternal()?1:1); |
| const btVector3 mi=node->volume.Center()-node->volume.Extents()*scl; |
| const btVector3 mx=node->volume.Center()+node->volume.Extents()*scl; |
| drawBox(idraw,mi,mx,node->isleaf()?lcolor:ncolor); |
| } |
| } |
| } |
| |
| // |
| template <typename T> |
| static inline T sum(const btAlignedObjectArray<T>& items) |
| { |
| T v; |
| if(items.size()) |
| { |
| v=items[0]; |
| for(int i=1,ni=items.size();i<ni;++i) |
| { |
| v+=items[i]; |
| } |
| } |
| return(v); |
| } |
| |
| // |
| template <typename T,typename Q> |
| static inline void add(btAlignedObjectArray<T>& items,const Q& value) |
| { |
| for(int i=0,ni=items.size();i<ni;++i) |
| { |
| items[i]+=value; |
| } |
| } |
| |
| // |
| template <typename T,typename Q> |
| static inline void mul(btAlignedObjectArray<T>& items,const Q& value) |
| { |
| for(int i=0,ni=items.size();i<ni;++i) |
| { |
| items[i]*=value; |
| } |
| } |
| |
| // |
| template <typename T> |
| static inline T average(const btAlignedObjectArray<T>& items) |
| { |
| const btScalar n=(btScalar)(items.size()>0?items.size():1); |
| return(sum(items)/n); |
| } |
| |
| // |
| static inline btScalar tetravolume(const btVector3& x0, |
| const btVector3& x1, |
| const btVector3& x2, |
| const btVector3& x3) |
| { |
| const btVector3 a=x1-x0; |
| const btVector3 b=x2-x0; |
| const btVector3 c=x3-x0; |
| return(btDot(a,btCross(b,c))); |
| } |
| |
| // |
| #if 0 |
| static btVector3 stresscolor(btScalar stress) |
| { |
| static const btVector3 spectrum[]= { btVector3(1,0,1), |
| btVector3(0,0,1), |
| btVector3(0,1,1), |
| btVector3(0,1,0), |
| btVector3(1,1,0), |
| btVector3(1,0,0), |
| btVector3(1,0,0)}; |
| static const int ncolors=sizeof(spectrum)/sizeof(spectrum[0])-1; |
| static const btScalar one=1; |
| stress=btMax<btScalar>(0,btMin<btScalar>(1,stress))*ncolors; |
| const int sel=(int)stress; |
| const btScalar frc=stress-sel; |
| return(spectrum[sel]+(spectrum[sel+1]-spectrum[sel])*frc); |
| } |
| #endif |
| |
| // |
| void btSoftBodyHelpers::Draw( btSoftBody* psb, |
| btIDebugDraw* idraw, |
| int drawflags) |
| { |
| const btScalar scl=(btScalar)0.1; |
| const btScalar nscl=scl*5; |
| const btVector3 lcolor=btVector3(0,0,0); |
| const btVector3 ncolor=btVector3(1,1,1); |
| const btVector3 ccolor=btVector3(1,0,0); |
| int i,j,nj; |
| |
| /* Nodes */ |
| if(0!=(drawflags&fDrawFlags::Nodes)) |
| { |
| for(i=0;i<psb->m_nodes.size();++i) |
| { |
| const btSoftBody::Node& n=psb->m_nodes[i]; |
| if(0==(n.m_material->m_flags&btSoftBody::fMaterial::DebugDraw)) continue; |
| idraw->drawLine(n.m_x-btVector3(scl,0,0),n.m_x+btVector3(scl,0,0),btVector3(1,0,0)); |
| idraw->drawLine(n.m_x-btVector3(0,scl,0),n.m_x+btVector3(0,scl,0),btVector3(0,1,0)); |
| idraw->drawLine(n.m_x-btVector3(0,0,scl),n.m_x+btVector3(0,0,scl),btVector3(0,0,1)); |
| } |
| } |
| /* Links */ |
| if(0!=(drawflags&fDrawFlags::Links)) |
| { |
| for(i=0;i<psb->m_links.size();++i) |
| { |
| const btSoftBody::Link& l=psb->m_links[i]; |
| if(0==(l.m_material->m_flags&btSoftBody::fMaterial::DebugDraw)) continue; |
| idraw->drawLine(l.m_n[0]->m_x,l.m_n[1]->m_x,lcolor); |
| } |
| } |
| /* Normals */ |
| if(0!=(drawflags&fDrawFlags::Normals)) |
| { |
| for(i=0;i<psb->m_nodes.size();++i) |
| { |
| const btSoftBody::Node& n=psb->m_nodes[i]; |
| if(0==(n.m_material->m_flags&btSoftBody::fMaterial::DebugDraw)) continue; |
| const btVector3 d=n.m_n*nscl; |
| idraw->drawLine(n.m_x,n.m_x+d,ncolor); |
| idraw->drawLine(n.m_x,n.m_x-d,ncolor*0.5); |
| } |
| } |
| /* Contacts */ |
| if(0!=(drawflags&fDrawFlags::Contacts)) |
| { |
| static const btVector3 axis[]={btVector3(1,0,0), |
| btVector3(0,1,0), |
| btVector3(0,0,1)}; |
| for(i=0;i<psb->m_rcontacts.size();++i) |
| { |
| const btSoftBody::RContact& c=psb->m_rcontacts[i]; |
| const btVector3 o= c.m_node->m_x-c.m_cti.m_normal* |
| (btDot(c.m_node->m_x,c.m_cti.m_normal)+c.m_cti.m_offset); |
| const btVector3 x=btCross(c.m_cti.m_normal,axis[c.m_cti.m_normal.minAxis()]).normalized(); |
| const btVector3 y=btCross(x,c.m_cti.m_normal).normalized(); |
| idraw->drawLine(o-x*nscl,o+x*nscl,ccolor); |
| idraw->drawLine(o-y*nscl,o+y*nscl,ccolor); |
| idraw->drawLine(o,o+c.m_cti.m_normal*nscl*3,btVector3(1,1,0)); |
| } |
| } |
| /* Anchors */ |
| if(0!=(drawflags&fDrawFlags::Anchors)) |
| { |
| for(i=0;i<psb->m_anchors.size();++i) |
| { |
| const btSoftBody::Anchor& a=psb->m_anchors[i]; |
| const btVector3 q=a.m_body->getWorldTransform()*a.m_local; |
| drawVertex(idraw,a.m_node->m_x,0.25,btVector3(1,0,0)); |
| drawVertex(idraw,q,0.25,btVector3(0,1,0)); |
| idraw->drawLine(a.m_node->m_x,q,btVector3(1,1,1)); |
| } |
| for(i=0;i<psb->m_nodes.size();++i) |
| { |
| const btSoftBody::Node& n=psb->m_nodes[i]; |
| if(0==(n.m_material->m_flags&btSoftBody::fMaterial::DebugDraw)) continue; |
| if(n.m_im<=0) |
| { |
| drawVertex(idraw,n.m_x,0.25,btVector3(1,0,0)); |
| } |
| } |
| } |
| /* Faces */ |
| if(0!=(drawflags&fDrawFlags::Faces)) |
| { |
| const btScalar scl=(btScalar)0.8; |
| const btScalar alp=(btScalar)1; |
| const btVector3 col(0,(btScalar)0.7,0); |
| for(i=0;i<psb->m_faces.size();++i) |
| { |
| const btSoftBody::Face& f=psb->m_faces[i]; |
| if(0==(f.m_material->m_flags&btSoftBody::fMaterial::DebugDraw)) continue; |
| const btVector3 x[]={f.m_n[0]->m_x,f.m_n[1]->m_x,f.m_n[2]->m_x}; |
| const btVector3 c=(x[0]+x[1]+x[2])/3; |
| idraw->drawTriangle((x[0]-c)*scl+c, |
| (x[1]-c)*scl+c, |
| (x[2]-c)*scl+c, |
| col,alp); |
| } |
| } |
| /* Clusters */ |
| if(0!=(drawflags&fDrawFlags::Clusters)) |
| { |
| srand(1806); |
| for(i=0;i<psb->m_clusters.size();++i) |
| { |
| if(psb->m_clusters[i]->m_collide) |
| { |
| btVector3 color( rand()/(btScalar)RAND_MAX, |
| rand()/(btScalar)RAND_MAX, |
| rand()/(btScalar)RAND_MAX); |
| color=color.normalized()*0.75; |
| btAlignedObjectArray<btVector3> vertices; |
| vertices.resize(psb->m_clusters[i]->m_nodes.size()); |
| for(j=0,nj=vertices.size();j<nj;++j) |
| { |
| vertices[j]=psb->m_clusters[i]->m_nodes[j]->m_x; |
| } |
| HullDesc hdsc(QF_TRIANGLES,vertices.size(),&vertices[0]); |
| HullResult hres; |
| HullLibrary hlib; |
| hdsc.mMaxVertices=vertices.size(); |
| hlib.CreateConvexHull(hdsc,hres); |
| const btVector3 center=average(hres.m_OutputVertices); |
| add(hres.m_OutputVertices,-center); |
| mul(hres.m_OutputVertices,(btScalar)1); |
| add(hres.m_OutputVertices,center); |
| for(j=0;j<(int)hres.mNumFaces;++j) |
| { |
| const int idx[]={hres.m_Indices[j*3+0],hres.m_Indices[j*3+1],hres.m_Indices[j*3+2]}; |
| idraw->drawTriangle(hres.m_OutputVertices[idx[0]], |
| hres.m_OutputVertices[idx[1]], |
| hres.m_OutputVertices[idx[2]], |
| color,1); |
| } |
| hlib.ReleaseResult(hres); |
| } |
| /* Velocities */ |
| #if 0 |
| for(int j=0;j<psb->m_clusters[i].m_nodes.size();++j) |
| { |
| const btSoftBody::Cluster& c=psb->m_clusters[i]; |
| const btVector3 r=c.m_nodes[j]->m_x-c.m_com; |
| const btVector3 v=c.m_lv+btCross(c.m_av,r); |
| idraw->drawLine(c.m_nodes[j]->m_x,c.m_nodes[j]->m_x+v,btVector3(1,0,0)); |
| } |
| #endif |
| /* Frame */ |
| btSoftBody::Cluster& c=*psb->m_clusters[i]; |
| idraw->drawLine(c.m_com,c.m_framexform*btVector3(10,0,0),btVector3(1,0,0)); |
| idraw->drawLine(c.m_com,c.m_framexform*btVector3(0,10,0),btVector3(0,1,0)); |
| idraw->drawLine(c.m_com,c.m_framexform*btVector3(0,0,10),btVector3(0,0,1)); |
| } |
| } |
| |
| /* Tetras */ |
| if(0!=(drawflags&fDrawFlags::Tetras)) |
| { |
| const btScalar scl=(btScalar)0.8; |
| const btScalar alp=(btScalar)1; |
| const btVector3 col((btScalar)0.7,(btScalar)0.7,(btScalar)0.7); |
| for(int i=0;i<psb->m_tetras.size();++i) |
| { |
| const btSoftBody::Tetra& t=psb->m_tetras[i]; |
| if(0==(t.m_material->m_flags&btSoftBody::fMaterial::DebugDraw)) continue; |
| 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}; |
| const btVector3 c=(x[0]+x[1]+x[2]+x[3])/4; |
| idraw->drawTriangle((x[0]-c)*scl+c,(x[1]-c)*scl+c,(x[2]-c)*scl+c,col,alp); |
| idraw->drawTriangle((x[0]-c)*scl+c,(x[1]-c)*scl+c,(x[3]-c)*scl+c,col,alp); |
| idraw->drawTriangle((x[1]-c)*scl+c,(x[2]-c)*scl+c,(x[3]-c)*scl+c,col,alp); |
| idraw->drawTriangle((x[2]-c)*scl+c,(x[0]-c)*scl+c,(x[3]-c)*scl+c,col,alp); |
| } |
| } |
| |
| /* Notes */ |
| if(0!=(drawflags&fDrawFlags::Notes)) |
| { |
| for(i=0;i<psb->m_notes.size();++i) |
| { |
| const btSoftBody::Note& n=psb->m_notes[i]; |
| btVector3 p=n.m_offset; |
| for(int j=0;j<n.m_rank;++j) |
| { |
| p+=n.m_nodes[j]->m_x*n.m_coords[j]; |
| } |
| idraw->draw3dText(p,n.m_text); |
| } |
| } |
| /* Node tree */ |
| if(0!=(drawflags&fDrawFlags::NodeTree)) DrawNodeTree(psb,idraw); |
| /* Face tree */ |
| if(0!=(drawflags&fDrawFlags::FaceTree)) DrawFaceTree(psb,idraw); |
| /* Cluster tree */ |
| if(0!=(drawflags&fDrawFlags::ClusterTree)) DrawClusterTree(psb,idraw); |
| /* Joints */ |
| if(0!=(drawflags&fDrawFlags::Joints)) |
| { |
| for(i=0;i<psb->m_joints.size();++i) |
| { |
| const btSoftBody::Joint* pj=psb->m_joints[i]; |
| switch(pj->Type()) |
| { |
| case btSoftBody::Joint::eType::Linear: |
| { |
| const btSoftBody::LJoint* pjl=(const btSoftBody::LJoint*)pj; |
| const btVector3 a0=pj->m_bodies[0].xform()*pjl->m_refs[0]; |
| const btVector3 a1=pj->m_bodies[1].xform()*pjl->m_refs[1]; |
| idraw->drawLine(pj->m_bodies[0].xform().getOrigin(),a0,btVector3(1,1,0)); |
| idraw->drawLine(pj->m_bodies[1].xform().getOrigin(),a1,btVector3(0,1,1)); |
| drawVertex(idraw,a0,0.25,btVector3(1,1,0)); |
| drawVertex(idraw,a1,0.25,btVector3(0,1,1)); |
| } |
| break; |
| case btSoftBody::Joint::eType::Angular: |
| { |
| //const btSoftBody::AJoint* pja=(const btSoftBody::AJoint*)pj; |
| const btVector3 o0=pj->m_bodies[0].xform().getOrigin(); |
| const btVector3 o1=pj->m_bodies[1].xform().getOrigin(); |
| const btVector3 a0=pj->m_bodies[0].xform().getBasis()*pj->m_refs[0]; |
| const btVector3 a1=pj->m_bodies[1].xform().getBasis()*pj->m_refs[1]; |
| idraw->drawLine(o0,o0+a0*10,btVector3(1,1,0)); |
| idraw->drawLine(o0,o0+a1*10,btVector3(1,1,0)); |
| idraw->drawLine(o1,o1+a0*10,btVector3(0,1,1)); |
| idraw->drawLine(o1,o1+a1*10,btVector3(0,1,1)); |
| } |
| } |
| } |
| } |
| } |
| |
| // |
| void btSoftBodyHelpers::DrawInfos( btSoftBody* psb, |
| btIDebugDraw* idraw, |
| bool masses, |
| bool areas, |
| bool /*stress*/) |
| { |
| for(int i=0;i<psb->m_nodes.size();++i) |
| { |
| const btSoftBody::Node& n=psb->m_nodes[i]; |
| char text[2048]={0}; |
| char buff[1024]; |
| if(masses) |
| { |
| sprintf(buff," M(%.2f)",1/n.m_im); |
| strcat(text,buff); |
| } |
| if(areas) |
| { |
| sprintf(buff," A(%.2f)",n.m_area); |
| strcat(text,buff); |
| } |
| if(text[0]) idraw->draw3dText(n.m_x,text); |
| } |
| } |
| |
| // |
| void btSoftBodyHelpers::DrawNodeTree( btSoftBody* psb, |
| btIDebugDraw* idraw, |
| int mindepth, |
| int maxdepth) |
| { |
| drawTree(idraw,psb->m_ndbvt.m_root,0,btVector3(1,0,1),btVector3(1,1,1),mindepth,maxdepth); |
| } |
| |
| // |
| void btSoftBodyHelpers::DrawFaceTree( btSoftBody* psb, |
| btIDebugDraw* idraw, |
| int mindepth, |
| int maxdepth) |
| { |
| drawTree(idraw,psb->m_fdbvt.m_root,0,btVector3(0,1,0),btVector3(1,0,0),mindepth,maxdepth); |
| } |
| |
| // |
| void btSoftBodyHelpers::DrawClusterTree( btSoftBody* psb, |
| btIDebugDraw* idraw, |
| int mindepth, |
| int maxdepth) |
| { |
| drawTree(idraw,psb->m_cdbvt.m_root,0,btVector3(0,1,1),btVector3(1,0,0),mindepth,maxdepth); |
| } |
| |
| // |
| void btSoftBodyHelpers::DrawFrame( btSoftBody* psb, |
| btIDebugDraw* idraw) |
| { |
| if(psb->m_pose.m_bframe) |
| { |
| static const btScalar ascl=10; |
| static const btScalar nscl=(btScalar)0.1; |
| const btVector3 com=psb->m_pose.m_com; |
| const btMatrix3x3 trs=psb->m_pose.m_rot*psb->m_pose.m_scl; |
| const btVector3 Xaxis=(trs*btVector3(1,0,0)).normalized(); |
| const btVector3 Yaxis=(trs*btVector3(0,1,0)).normalized(); |
| const btVector3 Zaxis=(trs*btVector3(0,0,1)).normalized(); |
| idraw->drawLine(com,com+Xaxis*ascl,btVector3(1,0,0)); |
| idraw->drawLine(com,com+Yaxis*ascl,btVector3(0,1,0)); |
| idraw->drawLine(com,com+Zaxis*ascl,btVector3(0,0,1)); |
| for(int i=0;i<psb->m_pose.m_pos.size();++i) |
| { |
| const btVector3 x=com+trs*psb->m_pose.m_pos[i]; |
| drawVertex(idraw,x,nscl,btVector3(1,0,1)); |
| } |
| } |
| } |
| |
| // |
| btSoftBody* btSoftBodyHelpers::CreateRope( btSoftBodyWorldInfo& worldInfo, const btVector3& from, |
| const btVector3& to, |
| int res, |
| int fixeds) |
| { |
| /* Create nodes */ |
| const int r=res+2; |
| btVector3* x=new btVector3[r]; |
| btScalar* m=new btScalar[r]; |
| int i; |
| |
| for(i=0;i<r;++i) |
| { |
| const btScalar t=i/(btScalar)(r-1); |
| x[i]=lerp(from,to,t); |
| m[i]=1; |
| } |
| btSoftBody* psb= new btSoftBody(&worldInfo,r,x,m); |
| if(fixeds&1) psb->setMass(0,0); |
| if(fixeds&2) psb->setMass(r-1,0); |
| delete[] x; |
| delete[] m; |
| /* Create links */ |
| for(i=1;i<r;++i) |
| { |
| psb->appendLink(i-1,i); |
| } |
| /* Finished */ |
| return(psb); |
| } |
| |
| // |
| btSoftBody* btSoftBodyHelpers::CreatePatch(btSoftBodyWorldInfo& worldInfo,const btVector3& corner00, |
| const btVector3& corner10, |
| const btVector3& corner01, |
| const btVector3& corner11, |
| int resx, |
| int resy, |
| int fixeds, |
| bool gendiags) |
| { |
| #define IDX(_x_,_y_) ((_y_)*rx+(_x_)) |
| /* Create nodes */ |
| if((resx<2)||(resy<2)) return(0); |
| const int rx=resx; |
| const int ry=resy; |
| const int tot=rx*ry; |
| btVector3* x=new btVector3[tot]; |
| btScalar* m=new btScalar[tot]; |
| int iy; |
| |
| for(iy=0;iy<ry;++iy) |
| { |
| const btScalar ty=iy/(btScalar)(ry-1); |
| const btVector3 py0=lerp(corner00,corner01,ty); |
| const btVector3 py1=lerp(corner10,corner11,ty); |
| for(int ix=0;ix<rx;++ix) |
| { |
| const btScalar tx=ix/(btScalar)(rx-1); |
| x[IDX(ix,iy)]=lerp(py0,py1,tx); |
| m[IDX(ix,iy)]=1; |
| } |
| } |
| btSoftBody* psb=new btSoftBody(&worldInfo,tot,x,m); |
| if(fixeds&1) psb->setMass(IDX(0,0),0); |
| if(fixeds&2) psb->setMass(IDX(rx-1,0),0); |
| if(fixeds&4) psb->setMass(IDX(0,ry-1),0); |
| if(fixeds&8) psb->setMass(IDX(rx-1,ry-1),0); |
| delete[] x; |
| delete[] m; |
| /* Create links and faces */ |
| for(iy=0;iy<ry;++iy) |
| { |
| for(int ix=0;ix<rx;++ix) |
| { |
| const int idx=IDX(ix,iy); |
| const bool mdx=(ix+1)<rx; |
| const bool mdy=(iy+1)<ry; |
| if(mdx) psb->appendLink(idx,IDX(ix+1,iy)); |
| if(mdy) psb->appendLink(idx,IDX(ix,iy+1)); |
| if(mdx&&mdy) |
| { |
| if((ix+iy)&1) |
| { |
| psb->appendFace(IDX(ix,iy),IDX(ix+1,iy),IDX(ix+1,iy+1)); |
| psb->appendFace(IDX(ix,iy),IDX(ix+1,iy+1),IDX(ix,iy+1)); |
| if(gendiags) |
| { |
| psb->appendLink(IDX(ix,iy),IDX(ix+1,iy+1)); |
| } |
| } |
| else |
| { |
| psb->appendFace(IDX(ix,iy+1),IDX(ix,iy),IDX(ix+1,iy)); |
| psb->appendFace(IDX(ix,iy+1),IDX(ix+1,iy),IDX(ix+1,iy+1)); |
| if(gendiags) |
| { |
| psb->appendLink(IDX(ix+1,iy),IDX(ix,iy+1)); |
| } |
| } |
| } |
| } |
| } |
| /* Finished */ |
| #undef IDX |
| return(psb); |
| } |
| |
| // |
| btSoftBody* btSoftBodyHelpers::CreatePatchUV(btSoftBodyWorldInfo& worldInfo, |
| const btVector3& corner00, |
| const btVector3& corner10, |
| const btVector3& corner01, |
| const btVector3& corner11, |
| int resx, |
| int resy, |
| int fixeds, |
| bool gendiags, |
| float* tex_coords) |
| { |
| |
| /* |
| * |
| * corners: |
| * |
| * [0][0] corner00 ------- corner01 [resx][0] |
| * | | |
| * | | |
| * [0][resy] corner10 -------- corner11 [resx][resy] |
| * |
| * |
| * |
| * |
| * |
| * |
| * "fixedgs" map: |
| * |
| * corner00 --> +1 |
| * corner01 --> +2 |
| * corner10 --> +4 |
| * corner11 --> +8 |
| * upper middle --> +16 |
| * left middle --> +32 |
| * right middle --> +64 |
| * lower middle --> +128 |
| * center --> +256 |
| * |
| * |
| * tex_coords size (resx-1)*(resy-1)*12 |
| * |
| * |
| * |
| * SINGLE QUAD INTERNALS |
| * |
| * 1) btSoftBody's nodes and links, |
| * diagonal link is optional ("gendiags") |
| * |
| * |
| * node00 ------ node01 |
| * | . |
| * | . |
| * | . |
| * | . |
| * | . |
| * node10 node11 |
| * |
| * |
| * |
| * 2) Faces: |
| * two triangles, |
| * UV Coordinates (hier example for single quad) |
| * |
| * (0,1) (0,1) (1,1) |
| * 1 |\ 3 \-----| 2 |
| * | \ \ | |
| * | \ \ | |
| * | \ \ | |
| * | \ \ | |
| * 2 |-----\ 3 \| 1 |
| * (0,0) (1,0) (1,0) |
| * |
| * |
| * |
| * |
| * |
| * |
| */ |
| |
| #define IDX(_x_,_y_) ((_y_)*rx+(_x_)) |
| /* Create nodes */ |
| if((resx<2)||(resy<2)) return(0); |
| const int rx=resx; |
| const int ry=resy; |
| const int tot=rx*ry; |
| btVector3* x=new btVector3[tot]; |
| btScalar* m=new btScalar[tot]; |
| |
| int iy; |
| |
| for(iy=0;iy<ry;++iy) |
| { |
| const btScalar ty=iy/(btScalar)(ry-1); |
| const btVector3 py0=lerp(corner00,corner01,ty); |
| const btVector3 py1=lerp(corner10,corner11,ty); |
| for(int ix=0;ix<rx;++ix) |
| { |
| const btScalar tx=ix/(btScalar)(rx-1); |
| x[IDX(ix,iy)]=lerp(py0,py1,tx); |
| m[IDX(ix,iy)]=1; |
| } |
| } |
| btSoftBody* psb=new btSoftBody(&worldInfo,tot,x,m); |
| if(fixeds&1) psb->setMass(IDX(0,0),0); |
| if(fixeds&2) psb->setMass(IDX(rx-1,0),0); |
| if(fixeds&4) psb->setMass(IDX(0,ry-1),0); |
| if(fixeds&8) psb->setMass(IDX(rx-1,ry-1),0); |
| if(fixeds&16) psb->setMass(IDX((rx-1)/2,0),0); |
| if(fixeds&32) psb->setMass(IDX(0,(ry-1)/2),0); |
| if(fixeds&64) psb->setMass(IDX(rx-1,(ry-1)/2),0); |
| if(fixeds&128) psb->setMass(IDX((rx-1)/2,ry-1),0); |
| if(fixeds&256) psb->setMass(IDX((rx-1)/2,(ry-1)/2),0); |
| delete[] x; |
| delete[] m; |
| |
| |
| int z = 0; |
| /* Create links and faces */ |
| for(iy=0;iy<ry;++iy) |
| { |
| for(int ix=0;ix<rx;++ix) |
| { |
| const bool mdx=(ix+1)<rx; |
| const bool mdy=(iy+1)<ry; |
| |
| int node00=IDX(ix,iy); |
| int node01=IDX(ix+1,iy); |
| int node10=IDX(ix,iy+1); |
| int node11=IDX(ix+1,iy+1); |
| |
| if(mdx) psb->appendLink(node00,node01); |
| if(mdy) psb->appendLink(node00,node10); |
| if(mdx&&mdy) |
| { |
| psb->appendFace(node00,node10,node11); |
| if (tex_coords) { |
| tex_coords[z+0]=CalculateUV(resx,resy,ix,iy,0); |
| tex_coords[z+1]=CalculateUV(resx,resy,ix,iy,1); |
| tex_coords[z+2]=CalculateUV(resx,resy,ix,iy,0); |
| tex_coords[z+3]=CalculateUV(resx,resy,ix,iy,2); |
| tex_coords[z+4]=CalculateUV(resx,resy,ix,iy,3); |
| tex_coords[z+5]=CalculateUV(resx,resy,ix,iy,2); |
| } |
| psb->appendFace(node11,node01,node00); |
| if (tex_coords) { |
| tex_coords[z+6 ]=CalculateUV(resx,resy,ix,iy,3); |
| tex_coords[z+7 ]=CalculateUV(resx,resy,ix,iy,2); |
| tex_coords[z+8 ]=CalculateUV(resx,resy,ix,iy,3); |
| tex_coords[z+9 ]=CalculateUV(resx,resy,ix,iy,1); |
| tex_coords[z+10]=CalculateUV(resx,resy,ix,iy,0); |
| tex_coords[z+11]=CalculateUV(resx,resy,ix,iy,1); |
| } |
| if (gendiags) psb->appendLink(node00,node11); |
| z += 12; |
| } |
| } |
| } |
| /* Finished */ |
| #undef IDX |
| return(psb); |
| } |
| |
| float btSoftBodyHelpers::CalculateUV(int resx,int resy,int ix,int iy,int id) |
| { |
| |
| /* |
| * |
| * |
| * node00 --- node01 |
| * | | |
| * node10 --- node11 |
| * |
| * |
| * ID map: |
| * |
| * node00 s --> 0 |
| * node00 t --> 1 |
| * |
| * node01 s --> 3 |
| * node01 t --> 1 |
| * |
| * node10 s --> 0 |
| * node10 t --> 2 |
| * |
| * node11 s --> 3 |
| * node11 t --> 2 |
| * |
| * |
| */ |
| |
| float tc=0.0f; |
| if (id == 0) { |
| tc = (1.0f/((resx-1))*ix); |
| } |
| else if (id==1) { |
| tc = (1.0f/((resy-1))*(resy-1-iy)); |
| } |
| else if (id==2) { |
| tc = (1.0f/((resy-1))*(resy-1-iy-1)); |
| } |
| else if (id==3) { |
| tc = (1.0f/((resx-1))*(ix+1)); |
| } |
| return tc; |
| } |
| // |
| btSoftBody* btSoftBodyHelpers::CreateEllipsoid(btSoftBodyWorldInfo& worldInfo,const btVector3& center, |
| const btVector3& radius, |
| int res) |
| { |
| struct Hammersley |
| { |
| static void Generate(btVector3* x,int n) |
| { |
| for(int i=0;i<n;i++) |
| { |
| btScalar p=0.5,t=0; |
| for(int j=i;j;p*=0.5,j>>=1) if(j&1) t+=p; |
| btScalar w=2*t-1; |
| btScalar a=(SIMD_PI+2*i*SIMD_PI)/n; |
| btScalar s=btSqrt(1-w*w); |
| *x++=btVector3(s*btCos(a),s*btSin(a),w); |
| } |
| } |
| }; |
| btAlignedObjectArray<btVector3> vtx; |
| vtx.resize(3+res); |
| Hammersley::Generate(&vtx[0],vtx.size()); |
| for(int i=0;i<vtx.size();++i) |
| { |
| vtx[i]=vtx[i]*radius+center; |
| } |
| return(CreateFromConvexHull(worldInfo,&vtx[0],vtx.size())); |
| } |
| |
| |
| |
| // |
| btSoftBody* btSoftBodyHelpers::CreateFromTriMesh(btSoftBodyWorldInfo& worldInfo,const btScalar* vertices, |
| const int* triangles, |
| int ntriangles) |
| { |
| int maxidx=0; |
| int i,j,ni; |
| |
| for(i=0,ni=ntriangles*3;i<ni;++i) |
| { |
| maxidx=btMax(triangles[i],maxidx); |
| } |
| ++maxidx; |
| btAlignedObjectArray<bool> chks; |
| btAlignedObjectArray<btVector3> vtx; |
| chks.resize(maxidx*maxidx,false); |
| vtx.resize(maxidx); |
| for(i=0,j=0,ni=maxidx*3;i<ni;++j,i+=3) |
| { |
| vtx[j]=btVector3(vertices[i],vertices[i+1],vertices[i+2]); |
| } |
| btSoftBody* psb=new btSoftBody(&worldInfo,vtx.size(),&vtx[0],0); |
| for( i=0,ni=ntriangles*3;i<ni;i+=3) |
| { |
| const int idx[]={triangles[i],triangles[i+1],triangles[i+2]}; |
| #define IDX(_x_,_y_) ((_y_)*maxidx+(_x_)) |
| for(int j=2,k=0;k<3;j=k++) |
| { |
| if(!chks[IDX(idx[j],idx[k])]) |
| { |
| chks[IDX(idx[j],idx[k])]=true; |
| chks[IDX(idx[k],idx[j])]=true; |
| psb->appendLink(idx[j],idx[k]); |
| } |
| } |
| #undef IDX |
| psb->appendFace(idx[0],idx[1],idx[2]); |
| } |
| psb->randomizeConstraints(); |
| return(psb); |
| } |
| |
| // |
| btSoftBody* btSoftBodyHelpers::CreateFromConvexHull(btSoftBodyWorldInfo& worldInfo, const btVector3* vertices, |
| int nvertices) |
| { |
| HullDesc hdsc(QF_TRIANGLES,nvertices,vertices); |
| HullResult hres; |
| HullLibrary hlib;/*??*/ |
| hdsc.mMaxVertices=nvertices; |
| hlib.CreateConvexHull(hdsc,hres); |
| btSoftBody* psb=new btSoftBody(&worldInfo,(int)hres.mNumOutputVertices, |
| &hres.m_OutputVertices[0],0); |
| for(int i=0;i<(int)hres.mNumFaces;++i) |
| { |
| const int idx[]={ hres.m_Indices[i*3+0], |
| hres.m_Indices[i*3+1], |
| hres.m_Indices[i*3+2]}; |
| if(idx[0]<idx[1]) psb->appendLink( idx[0],idx[1]); |
| if(idx[1]<idx[2]) psb->appendLink( idx[1],idx[2]); |
| if(idx[2]<idx[0]) psb->appendLink( idx[2],idx[0]); |
| psb->appendFace(idx[0],idx[1],idx[2]); |
| } |
| hlib.ReleaseResult(hres); |
| psb->randomizeConstraints(); |
| return(psb); |
| } |
| |
| |
| |
| |
| static int nextLine(const char* buffer) |
| { |
| int numBytesRead=0; |
| |
| while (*buffer != '\n') |
| { |
| buffer++; |
| numBytesRead++; |
| } |
| |
| |
| if (buffer[0]==0x0a) |
| { |
| buffer++; |
| numBytesRead++; |
| } |
| return numBytesRead; |
| } |
| |
| /* Create from TetGen .ele, .face, .node data */ |
| btSoftBody* btSoftBodyHelpers::CreateFromTetGenData(btSoftBodyWorldInfo& worldInfo, |
| const char* ele, |
| const char* face, |
| const char* node, |
| bool bfacelinks, |
| bool btetralinks, |
| bool bfacesfromtetras) |
| { |
| btAlignedObjectArray<btVector3> pos; |
| int nnode=0; |
| int ndims=0; |
| int nattrb=0; |
| int hasbounds=0; |
| int result = sscanf(node,"%d %d %d %d",&nnode,&ndims,&nattrb,&hasbounds); |
| result = sscanf(node,"%d %d %d %d",&nnode,&ndims,&nattrb,&hasbounds); |
| node += nextLine(node); |
| |
| pos.resize(nnode); |
| for(int i=0;i<pos.size();++i) |
| { |
| int index=0; |
| //int bound=0; |
| float x,y,z; |
| sscanf(node,"%d %f %f %f",&index,&x,&y,&z); |
| |
| // sn>>index; |
| // sn>>x;sn>>y;sn>>z; |
| node += nextLine(node); |
| |
| //for(int j=0;j<nattrb;++j) |
| // sn>>a; |
| |
| //if(hasbounds) |
| // sn>>bound; |
| |
| pos[index].setX(btScalar(x)); |
| pos[index].setY(btScalar(y)); |
| pos[index].setZ(btScalar(z)); |
| } |
| btSoftBody* psb=new btSoftBody(&worldInfo,nnode,&pos[0],0); |
| #if 0 |
| if(face&&face[0]) |
| { |
| int nface=0; |
| sf>>nface;sf>>hasbounds; |
| for(int i=0;i<nface;++i) |
| { |
| int index=0; |
| int bound=0; |
| int ni[3]; |
| sf>>index; |
| sf>>ni[0];sf>>ni[1];sf>>ni[2]; |
| sf>>bound; |
| psb->appendFace(ni[0],ni[1],ni[2]); |
| if(btetralinks) |
| { |
| psb->appendLink(ni[0],ni[1],0,true); |
| psb->appendLink(ni[1],ni[2],0,true); |
| psb->appendLink(ni[2],ni[0],0,true); |
| } |
| } |
| } |
| #endif |
| |
| if(ele&&ele[0]) |
| { |
| int ntetra=0; |
| int ncorner=0; |
| int neattrb=0; |
| sscanf(ele,"%d %d %d",&ntetra,&ncorner,&neattrb); |
| ele += nextLine(ele); |
| |
| //se>>ntetra;se>>ncorner;se>>neattrb; |
| for(int i=0;i<ntetra;++i) |
| { |
| int index=0; |
| int ni[4]; |
| |
| //se>>index; |
| //se>>ni[0];se>>ni[1];se>>ni[2];se>>ni[3]; |
| sscanf(ele,"%d %d %d %d %d",&index,&ni[0],&ni[1],&ni[2],&ni[3]); |
| ele+=nextLine(ele); |
| //for(int j=0;j<neattrb;++j) |
| // se>>a; |
| psb->appendTetra(ni[0],ni[1],ni[2],ni[3]); |
| if(btetralinks) |
| { |
| psb->appendLink(ni[0],ni[1],0,true); |
| psb->appendLink(ni[1],ni[2],0,true); |
| psb->appendLink(ni[2],ni[0],0,true); |
| psb->appendLink(ni[0],ni[3],0,true); |
| psb->appendLink(ni[1],ni[3],0,true); |
| psb->appendLink(ni[2],ni[3],0,true); |
| } |
| } |
| } |
| printf("Nodes: %u\r\n",psb->m_nodes.size()); |
| printf("Links: %u\r\n",psb->m_links.size()); |
| printf("Faces: %u\r\n",psb->m_faces.size()); |
| printf("Tetras: %u\r\n",psb->m_tetras.size()); |
| return(psb); |
| } |
| |