| /* | |
| 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); | |
| } | |