diff options
Diffstat (limited to 'extern/bullet2/src/BulletSoftBody/btSoftBodyHelpers.cpp')
| -rw-r--r-- | extern/bullet2/src/BulletSoftBody/btSoftBodyHelpers.cpp | 1719 |
1 files changed, 1081 insertions, 638 deletions
diff --git a/extern/bullet2/src/BulletSoftBody/btSoftBodyHelpers.cpp b/extern/bullet2/src/BulletSoftBody/btSoftBodyHelpers.cpp index d96f85ec630..c30b65e895a 100644 --- a/extern/bullet2/src/BulletSoftBody/btSoftBodyHelpers.cpp +++ b/extern/bullet2/src/BulletSoftBody/btSoftBodyHelpers.cpp @@ -16,115 +16,126 @@ subject to the following restrictions: #include "btSoftBodyInternals.h" #include <stdio.h> +#include <string> +#include <iostream> +#include <sstream> #include <string.h> +#include <algorithm> #include "btSoftBodyHelpers.h" #include "LinearMath/btConvexHull.h" #include "LinearMath/btConvexHullComputer.h" +#include <map> +#include <vector> - -// -static void drawVertex( btIDebugDraw* idraw, - const btVector3& x,btScalar s,const btVector3& c) +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); + 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) +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); + 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) +static void drawTree(btIDebugDraw* idraw, + const btDbvtNode* node, + int depth, + const btVector3& ncolor, + const btVector3& lcolor, + int mindepth, + int maxdepth) { - if(node) + if (node) { - if(node->isinternal()&&((depth<maxdepth)||(maxdepth<0))) + 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); + 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) + 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); + 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) +static inline T sum(const btAlignedObjectArray<T>& items) { - T v; - if(items.size()) + T v; + if (items.size()) { - v=items[0]; - for(int i=1,ni=items.size();i<ni;++i) + v = items[0]; + for (int i = 1, ni = items.size(); i < ni; ++i) { - v+=items[i]; + v += items[i]; } } - return(v); + return (v); } // -template <typename T,typename Q> -static inline void add(btAlignedObjectArray<T>& items,const Q& value) +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) + for (int i = 0, ni = items.size(); i < ni; ++i) { - items[i]+=value; + items[i] += value; } } // -template <typename T,typename Q> -static inline void mul(btAlignedObjectArray<T>& items,const Q& 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) + for (int i = 0, ni = items.size(); i < ni; ++i) { - items[i]*=value; + items[i] *= value; } } // template <typename T> -static inline T average(const btAlignedObjectArray<T>& items) +static inline T average(const btAlignedObjectArray<T>& items) { - const btScalar n=(btScalar)(items.size()>0?items.size():1); - return(sum(items)/n); + const btScalar n = (btScalar)(items.size() > 0 ? items.size() : 1); + return (sum(items) / n); } +#if 0 // -static inline btScalar tetravolume(const btVector3& x0, + inline static btScalar tetravolume(const btVector3& x0, const btVector3& x1, const btVector3& x2, const btVector3& x3) @@ -134,6 +145,7 @@ static inline btScalar tetravolume(const btVector3& x0, const btVector3 c=x3-x0; return(btDot(a,btCross(b,c))); } +#endif // #if 0 @@ -156,86 +168,84 @@ static btVector3 stresscolor(btScalar stress) #endif // -void btSoftBodyHelpers::Draw( btSoftBody* psb, - btIDebugDraw* idraw, - int drawflags) +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; - - /* Clusters */ - if(0!=(drawflags&fDrawFlags::Clusters)) + 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; + + /* Clusters */ + if (0 != (drawflags & fDrawFlags::Clusters)) { srand(1806); - for(i=0;i<psb->m_clusters.size();++i) + for (i = 0; i < psb->m_clusters.size(); ++i) { - if(psb->m_clusters[i]->m_collide) + 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; + 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; + for (j = 0, nj = vertices.size(); j < nj; ++j) + { + vertices[j] = psb->m_clusters[i]->m_nodes[j]->m_x; } #define USE_NEW_CONVEX_HULL_COMPUTER #ifdef USE_NEW_CONVEX_HULL_COMPUTER - btConvexHullComputer computer; + btConvexHullComputer computer; int stride = sizeof(btVector3); int count = vertices.size(); - btScalar shrink=0.f; - btScalar shrinkClamp=0.f; - computer.compute(&vertices[0].getX(),stride,count,shrink,shrinkClamp); - for (int i=0;i<computer.faces.size();i++) + btScalar shrink = 0.f; + btScalar shrinkClamp = 0.f; + computer.compute(&vertices[0].getX(), stride, count, shrink, shrinkClamp); + for (int i = 0; i < computer.faces.size(); i++) { - int face = computer.faces[i]; //printf("face=%d\n",face); - const btConvexHullComputer::Edge* firstEdge = &computer.edges[face]; - const btConvexHullComputer::Edge* edge = firstEdge->getNextEdgeOfFace(); + const btConvexHullComputer::Edge* firstEdge = &computer.edges[face]; + const btConvexHullComputer::Edge* edge = firstEdge->getNextEdgeOfFace(); int v0 = firstEdge->getSourceVertex(); int v1 = firstEdge->getTargetVertex(); - while (edge!=firstEdge) + while (edge != firstEdge) { int v2 = edge->getTargetVertex(); - idraw->drawTriangle(computer.vertices[v0],computer.vertices[v1],computer.vertices[v2],color,1); + idraw->drawTriangle(computer.vertices[v0], computer.vertices[v1], computer.vertices[v2], color, 1); edge = edge->getNextEdgeOfFace(); - v0=v1; - v1=v2; + v0 = v1; + v1 = v2; }; } #else - 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) + 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]}; + 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); + hres.m_OutputVertices[idx[1]], + hres.m_OutputVertices[idx[2]], + color, 1); } hlib.ReleaseResult(hres); #endif - } - /* Velocities */ + /* Velocities */ #if 0 for(int j=0;j<psb->m_clusters[i].m_nodes.size();++j) { @@ -245,273 +255,269 @@ void btSoftBodyHelpers::Draw( btSoftBody* psb, 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)); + /* 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)); } } else { - /* Nodes */ - if(0!=(drawflags&fDrawFlags::Nodes)) + /* Nodes */ + if (0 != (drawflags & fDrawFlags::Nodes)) { - for(i=0;i<psb->m_nodes.size();++i) + 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)); + 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)) + /* Links */ + if (0 != (drawflags & fDrawFlags::Links)) { - for(i=0;i<psb->m_links.size();++i) + 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); + 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)) + /* Normals */ + if (0 != (drawflags & fDrawFlags::Normals)) { - for(i=0;i<psb->m_nodes.size();++i) + 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); + 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)) + /* 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)); + 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)); } } - /* 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) + /* Faces */ + if (0 != (drawflags & fDrawFlags::Faces)) { - 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); - } - } - /* Tetras */ - if(0!=(drawflags&fDrawFlags::Tetras)) - { - const btScalar scl=(btScalar)0.8; - const btScalar alp=(btScalar)1; - const btVector3 col((btScalar)0.3,(btScalar)0.3,(btScalar)0.7); - for(int i=0;i<psb->m_tetras.size();++i) + 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); + } + } + /* Tetras */ + if (0 != (drawflags & fDrawFlags::Tetras)) { - 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); - } - } + const btScalar scl = (btScalar)0.8; + const btScalar alp = (btScalar)1; + const btVector3 col((btScalar)0.3, (btScalar)0.3, (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); + } + } } - /* Anchors */ - if(0!=(drawflags&fDrawFlags::Anchors)) + /* Anchors */ + if (0 != (drawflags & fDrawFlags::Anchors)) { - for(i=0;i<psb->m_anchors.size();++i) + 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)); + 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) + 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) + 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)); + drawVertex(idraw, n.m_x, 0.25, btVector3(1, 0, 0)); } } } - - /* Notes */ - if(0!=(drawflags&fDrawFlags::Notes)) + /* Notes */ + if (0 != (drawflags & fDrawFlags::Notes)) { - for(i=0;i<psb->m_notes.size();++i) + 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) + 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]; + p += n.m_nodes[j]->m_x * n.m_coords[j]; } - idraw->draw3dText(p,n.m_text); + 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)) + /* 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) + for (i = 0; i < psb->m_joints.size(); ++i) { - const btSoftBody::Joint* pj=psb->m_joints[i]; - switch(pj->Type()) + const btSoftBody::Joint* pj = psb->m_joints[i]; + switch (pj->Type()) { - case btSoftBody::Joint::eType::Linear: + 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)); + 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: + 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)); + 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)); break; } default: { } - - } + } } } } // -void btSoftBodyHelpers::DrawInfos( btSoftBody* psb, - btIDebugDraw* idraw, - bool masses, - bool areas, - bool /*stress*/) +void btSoftBodyHelpers::DrawInfos(btSoftBody* psb, + btIDebugDraw* idraw, + bool masses, + bool areas, + bool /*stress*/) { - for(int i=0;i<psb->m_nodes.size();++i) + 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) + 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); + sprintf(buff, " M(%.2f)", 1 / n.m_im); + strcat(text, buff); } - if(areas) + if (areas) { - sprintf(buff," A(%.2f)",n.m_area); - strcat(text,buff); + sprintf(buff, " A(%.2f)", n.m_area); + strcat(text, buff); } - if(text[0]) idraw->draw3dText(n.m_x,text); + if (text[0]) idraw->draw3dText(n.m_x, text); } } // -void btSoftBodyHelpers::DrawNodeTree( btSoftBody* psb, - btIDebugDraw* idraw, - int mindepth, - int maxdepth) +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); + 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) +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); + 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) +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); + drawTree(idraw, psb->m_cdbvt.m_root, 0, btVector3(0, 1, 1), btVector3(1, 0, 0), mindepth, maxdepth); } - //The btSoftBody object from the BulletSDK includes an array of Nodes and Links. These links appear -// to be first set up to connect a node to between 5 and 6 of its neighbors [480 links], -//and then to the rest of the nodes after the execution of the Floyd-Warshall graph algorithm -//[another 930 links]. +// to be first set up to connect a node to between 5 and 6 of its neighbors [480 links], +//and then to the rest of the nodes after the execution of the Floyd-Warshall graph algorithm +//[another 930 links]. //The way the links are stored by default, we have a number of cases where adjacent links share a node in common -// - this leads to the creation of a data dependency through memory. -//The PSolve_Links() function reads and writes nodes as it iterates over each link. -//So, we now have the possibility of a data dependency between iteration X -//that processes link L with iteration X+1 that processes link L+1 -//because L and L+1 have one node in common, and iteration X updates the positions of that node, +// - this leads to the creation of a data dependency through memory. +//The PSolve_Links() function reads and writes nodes as it iterates over each link. +//So, we now have the possibility of a data dependency between iteration X +//that processes link L with iteration X+1 that processes link L+1 +//because L and L+1 have one node in common, and iteration X updates the positions of that node, //and iteration X+1 reads in the position of that shared node. // -//Such a memory dependency limits the ability of a modern CPU to speculate beyond -//a certain point because it has to respect a possible dependency -//- this prevents the CPU from making full use of its out-of-order resources. -//If we re-order the links such that we minimize the cases where a link L and L+1 share a common node, -//we create a temporal gap between when the node position is written, -//and when it is subsequently read. This in turn allows the CPU to continue execution without -//risking a dependency violation. Such a reordering would result in significant speedups on -//modern CPUs with lots of execution resources. -//In our testing, we see it have a tremendous impact not only on the A7, -//but also on all x86 cores that ship with modern Macs. -//The attached source file includes a single function (ReoptimizeLinkOrder) which can be called on a -//btSoftBody object in the solveConstraints() function before the actual solver is invoked, +//Such a memory dependency limits the ability of a modern CPU to speculate beyond +//a certain point because it has to respect a possible dependency +//- this prevents the CPU from making full use of its out-of-order resources. +//If we re-order the links such that we minimize the cases where a link L and L+1 share a common node, +//we create a temporal gap between when the node position is written, +//and when it is subsequently read. This in turn allows the CPU to continue execution without +//risking a dependency violation. Such a reordering would result in significant speedups on +//modern CPUs with lots of execution resources. +//In our testing, we see it have a tremendous impact not only on the A7, +//but also on all x86 cores that ship with modern Macs. +//The attached source file includes a single function (ReoptimizeLinkOrder) which can be called on a +//btSoftBody object in the solveConstraints() function before the actual solver is invoked, //or right after generateBendingConstraints() once we have all 1410 links. - //=================================================================== // // -// This function takes in a list of interdependent Links and tries +// This function takes in a list of interdependent Links and tries // to maximize the distance between calculation // of dependent links. This increases the amount of parallelism that can // be exploited by out-of-order instruction processors with large but @@ -520,93 +526,103 @@ void btSoftBodyHelpers::DrawClusterTree( btSoftBody* psb, //=================================================================== // A small structure to track lists of dependent link calculations -class LinkDeps_t { - public: - int value; // A link calculation that is dependent on this one - // Positive values = "input A" while negative values = "input B" - LinkDeps_t *next; // Next dependence in the list +class LinkDeps_t +{ +public: + int value; // A link calculation that is dependent on this one + // Positive values = "input A" while negative values = "input B" + LinkDeps_t* next; // Next dependence in the list }; -typedef LinkDeps_t *LinkDepsPtr_t; +typedef LinkDeps_t* LinkDepsPtr_t; // Dependency list constants -#define REOP_NOT_DEPENDENT -1 -#define REOP_NODE_COMPLETE -2 // Must be less than REOP_NOT_DEPENDENT - +#define REOP_NOT_DEPENDENT -1 +#define REOP_NODE_COMPLETE -2 // Must be less than REOP_NOT_DEPENDENT -void btSoftBodyHelpers::ReoptimizeLinkOrder(btSoftBody *psb /* This can be replaced by a btSoftBody pointer */) +void btSoftBodyHelpers::ReoptimizeLinkOrder(btSoftBody* psb /* This can be replaced by a btSoftBody pointer */) { - int i, nLinks=psb->m_links.size(), nNodes=psb->m_nodes.size(); - btSoftBody::Link *lr; + int i, nLinks = psb->m_links.size(), nNodes = psb->m_nodes.size(); + btSoftBody::Link* lr; int ar, br; - btSoftBody::Node *node0 = &(psb->m_nodes[0]); - btSoftBody::Node *node1 = &(psb->m_nodes[1]); + btSoftBody::Node* node0 = &(psb->m_nodes[0]); + btSoftBody::Node* node1 = &(psb->m_nodes[1]); LinkDepsPtr_t linkDep; int readyListHead, readyListTail, linkNum, linkDepFrees, depLink; - + // Allocate temporary buffers - int *nodeWrittenAt = new int[nNodes+1]; // What link calculation produced this node's current values? - int *linkDepA = new int[nLinks]; // Link calculation input is dependent upon prior calculation #N - int *linkDepB = new int[nLinks]; - int *readyList = new int[nLinks]; // List of ready-to-process link calculations (# of links, maximum) - LinkDeps_t *linkDepFreeList = new LinkDeps_t[2*nLinks]; // Dependent-on-me list elements (2x# of links, maximum) - LinkDepsPtr_t *linkDepListStarts = new LinkDepsPtr_t[nLinks]; // Start nodes of dependent-on-me lists, one for each link - + int* nodeWrittenAt = new int[nNodes + 1]; // What link calculation produced this node's current values? + int* linkDepA = new int[nLinks]; // Link calculation input is dependent upon prior calculation #N + int* linkDepB = new int[nLinks]; + int* readyList = new int[nLinks]; // List of ready-to-process link calculations (# of links, maximum) + LinkDeps_t* linkDepFreeList = new LinkDeps_t[2 * nLinks]; // Dependent-on-me list elements (2x# of links, maximum) + LinkDepsPtr_t* linkDepListStarts = new LinkDepsPtr_t[nLinks]; // Start nodes of dependent-on-me lists, one for each link + // Copy the original, unsorted links to a side buffer - btSoftBody::Link *linkBuffer = new btSoftBody::Link[nLinks]; - memcpy(linkBuffer, &(psb->m_links[0]), sizeof(btSoftBody::Link)*nLinks); + btSoftBody::Link* linkBuffer = new btSoftBody::Link[nLinks]; + memcpy(linkBuffer, &(psb->m_links[0]), sizeof(btSoftBody::Link) * nLinks); // Clear out the node setup and ready list - for (i=0; i < nNodes+1; i++) { + for (i = 0; i < nNodes + 1; i++) + { nodeWrittenAt[i] = REOP_NOT_DEPENDENT; } - for (i=0; i < nLinks; i++) { + for (i = 0; i < nLinks; i++) + { linkDepListStarts[i] = NULL; } readyListHead = readyListTail = linkDepFrees = 0; // Initial link analysis to set up data structures - for (i=0; i < nLinks; i++) { - + for (i = 0; i < nLinks; i++) + { // Note which prior link calculations we are dependent upon & build up dependence lists lr = &(psb->m_links[i]); - ar = (lr->m_n[0] - node0)/(node1 - node0); - br = (lr->m_n[1] - node0)/(node1 - node0); - if (nodeWrittenAt[ar] > REOP_NOT_DEPENDENT) { + ar = (lr->m_n[0] - node0) / (node1 - node0); + br = (lr->m_n[1] - node0) / (node1 - node0); + if (nodeWrittenAt[ar] > REOP_NOT_DEPENDENT) + { linkDepA[i] = nodeWrittenAt[ar]; linkDep = &linkDepFreeList[linkDepFrees++]; linkDep->value = i; linkDep->next = linkDepListStarts[nodeWrittenAt[ar]]; linkDepListStarts[nodeWrittenAt[ar]] = linkDep; - } else { + } + else + { linkDepA[i] = REOP_NOT_DEPENDENT; } - if (nodeWrittenAt[br] > REOP_NOT_DEPENDENT) { + if (nodeWrittenAt[br] > REOP_NOT_DEPENDENT) + { linkDepB[i] = nodeWrittenAt[br]; linkDep = &linkDepFreeList[linkDepFrees++]; - linkDep->value = -(i+1); + linkDep->value = -(i + 1); linkDep->next = linkDepListStarts[nodeWrittenAt[br]]; linkDepListStarts[nodeWrittenAt[br]] = linkDep; - } else { + } + else + { linkDepB[i] = REOP_NOT_DEPENDENT; } - + // Add this link to the initial ready list, if it is not dependent on any other links - if ((linkDepA[i] == REOP_NOT_DEPENDENT) && (linkDepB[i] == REOP_NOT_DEPENDENT)) { + if ((linkDepA[i] == REOP_NOT_DEPENDENT) && (linkDepB[i] == REOP_NOT_DEPENDENT)) + { readyList[readyListTail++] = i; - linkDepA[i] = linkDepB[i] = REOP_NODE_COMPLETE; // Probably not needed now + linkDepA[i] = linkDepB[i] = REOP_NODE_COMPLETE; // Probably not needed now } - + // Update the nodes to mark which ones are calculated by this link nodeWrittenAt[ar] = nodeWrittenAt[br] = i; } - + // Process the ready list and create the sorted list of links // -- By treating the ready list as a queue, we maximize the distance between any // inter-dependent node calculations // -- All other (non-related) nodes in the ready list will automatically be inserted // in between each set of inter-dependent link calculations by this loop i = 0; - while (readyListHead != readyListTail) { + while (readyListHead != readyListTail) + { // Use ready list to select the next link to process linkNum = readyList[readyListHead++]; // Copy the next-to-calculate link back into the original link array @@ -614,180 +630,190 @@ void btSoftBodyHelpers::ReoptimizeLinkOrder(btSoftBody *psb /* This can be repla // Free up any link inputs that are dependent on this one linkDep = linkDepListStarts[linkNum]; - while (linkDep) { + while (linkDep) + { depLink = linkDep->value; - if (depLink >= 0) { + if (depLink >= 0) + { linkDepA[depLink] = REOP_NOT_DEPENDENT; - } else { + } + else + { depLink = -depLink - 1; linkDepB[depLink] = REOP_NOT_DEPENDENT; } // Add this dependent link calculation to the ready list if *both* inputs are clear - if ((linkDepA[depLink] == REOP_NOT_DEPENDENT) && (linkDepB[depLink] == REOP_NOT_DEPENDENT)) { + if ((linkDepA[depLink] == REOP_NOT_DEPENDENT) && (linkDepB[depLink] == REOP_NOT_DEPENDENT)) + { readyList[readyListTail++] = depLink; - linkDepA[depLink] = linkDepB[depLink] = REOP_NODE_COMPLETE; // Probably not needed now + linkDepA[depLink] = linkDepB[depLink] = REOP_NODE_COMPLETE; // Probably not needed now } linkDep = linkDep->next; } } // Delete the temporary buffers - delete [] nodeWrittenAt; - delete [] linkDepA; - delete [] linkDepB; - delete [] readyList; - delete [] linkDepFreeList; - delete [] linkDepListStarts; - delete [] linkBuffer; + delete[] nodeWrittenAt; + delete[] linkDepA; + delete[] linkDepB; + delete[] readyList; + delete[] linkDepFreeList; + delete[] linkDepListStarts; + delete[] linkBuffer; } - // -void btSoftBodyHelpers::DrawFrame( btSoftBody* psb, - btIDebugDraw* idraw) +void btSoftBodyHelpers::DrawFrame(btSoftBody* psb, + btIDebugDraw* idraw) { - if(psb->m_pose.m_bframe) + 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) + 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)); + 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) +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]; + /* 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) + for (i = 0; i < r; ++i) { - const btScalar t=i/(btScalar)(r-1); - x[i]=lerp(from,to,t); - m[i]=1; + 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); + 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) + /* Create links */ + for (i = 1; i < r; ++i) { - psb->appendLink(i-1,i); + psb->appendLink(i - 1, i); } - /* Finished */ - return(psb); + /* 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) +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, + btScalar perturbation) { -#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]; +#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) + 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 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; + const btScalar tx = ix / (btScalar)(rx - 1); + btScalar pert = perturbation * btScalar(rand()) / RAND_MAX; + btVector3 temp1 = py1; + temp1.setY(py1.getY() + pert); + btVector3 temp = py0; + pert = perturbation * btScalar(rand()) / RAND_MAX; + temp.setY(py0.getY() + pert); + x[IDX(ix, iy)] = lerp(temp, temp1, 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); + 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) + /* Create links and faces */ + for (iy = 0; iy < ry; ++iy) { - for(int ix=0;ix<rx;++ix) + 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) + 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) + 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->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)); + 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->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)); + psb->appendLink(IDX(ix + 1, iy), IDX(ix, iy + 1)); } } } } } - /* Finished */ + /* Finished */ #undef IDX - return(psb); + 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) +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: @@ -855,92 +881,92 @@ btSoftBody* btSoftBodyHelpers::CreatePatchUV(btSoftBodyWorldInfo& worldInfo, * */ -#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]; +#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) + 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 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; + 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); + 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) + /* Create links and faces */ + for (iy = 0; iy < ry; ++iy) { - for(int ix=0;ix<rx;++ix) + for (int ix = 0; ix < rx; ++ix) { - const bool mdx=(ix+1)<rx; - const bool mdy=(iy+1)<ry; + 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); + 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) + 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(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); + 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); + if (gendiags) psb->appendLink(node00, node11); z += 12; } } } - /* Finished */ + /* Finished */ #undef IDX - return(psb); + return (psb); } -float btSoftBodyHelpers::CalculateUV(int resx,int resy,int ix,int iy,int id) +float btSoftBodyHelpers::CalculateUV(int resx, int resy, int ix, int iy, int id) { - /* * * @@ -966,90 +992,93 @@ float btSoftBodyHelpers::CalculateUV(int resx,int resy,int ix,int iy,int id) * */ - float tc=0.0f; - if (id == 0) { - tc = (1.0f/((resx-1))*ix); + 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 == 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 == 2) + { + tc = (1.0f / ((resy - 1)) * (resy - 1 - iy - 1)); } - else if (id==3) { - tc = (1.0f/((resx-1))*(ix+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) +btSoftBody* btSoftBodyHelpers::CreateEllipsoid(btSoftBodyWorldInfo& worldInfo, const btVector3& center, + const btVector3& radius, + int res) { - struct Hammersley + struct Hammersley { - static void Generate(btVector3* x,int n) + static void Generate(btVector3* x, int n) { - for(int i=0;i<n;i++) + 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); + 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) + 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; + vtx[i] = vtx[i] * radius + center; } - return(CreateFromConvexHull(worldInfo,&vtx[0],vtx.size())); + return (CreateFromConvexHull(worldInfo, &vtx[0], vtx.size())); } - - // -btSoftBody* btSoftBodyHelpers::CreateFromTriMesh(btSoftBodyWorldInfo& worldInfo,const btScalar* vertices, - const int* triangles, - int ntriangles, bool randomizeConstraints) +btSoftBody* btSoftBodyHelpers::CreateFromTriMesh(btSoftBodyWorldInfo& worldInfo, const btScalar* vertices, + const int* triangles, + int ntriangles, bool randomizeConstraints) { - int maxidx=0; - int i,j,ni; + int maxidx = 0; + int i, j, ni; - for(i=0,ni=ntriangles*3;i<ni;++i) + for (i = 0, ni = ntriangles * 3; i < ni; ++i) { - maxidx=btMax(triangles[i],maxidx); + maxidx = btMax(triangles[i], maxidx); } ++maxidx; - btAlignedObjectArray<bool> chks; - btAlignedObjectArray<btVector3> vtx; - chks.resize(maxidx*maxidx,false); + 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) + for (i = 0, j = 0, ni = maxidx * 3; i < ni; ++j, i += 3) { - vtx[j]=btVector3(vertices[i],vertices[i+1],vertices[i+2]); + 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) + 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++) + 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])]) + 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]); + 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->appendFace(idx[0], idx[1], idx[2]); } if (randomizeConstraints) @@ -1057,44 +1086,41 @@ btSoftBody* btSoftBodyHelpers::CreateFromTriMesh(btSoftBodyWorldInfo& worldInfo psb->randomizeConstraints(); } - return(psb); + return (psb); } // -btSoftBody* btSoftBodyHelpers::CreateFromConvexHull(btSoftBodyWorldInfo& worldInfo, const btVector3* vertices, - int nvertices, bool randomizeConstraints) +btSoftBody* btSoftBodyHelpers::CreateFromConvexHull(btSoftBodyWorldInfo& worldInfo, const btVector3* vertices, + int nvertices, bool randomizeConstraints) { - 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) + 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[]={ static_cast<int>(hres.m_Indices[i*3+0]), - static_cast<int>(hres.m_Indices[i*3+1]), - static_cast<int>(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]); + const int idx[] = {static_cast<int>(hres.m_Indices[i * 3 + 0]), + static_cast<int>(hres.m_Indices[i * 3 + 1]), + static_cast<int>(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); if (randomizeConstraints) { psb->randomizeConstraints(); } - return(psb); + return (psb); } - - - static int nextLine(const char* buffer) { - int numBytesRead=0; + int numBytesRead = 0; while (*buffer != '\n') { @@ -1102,8 +1128,7 @@ static int nextLine(const char* buffer) numBytesRead++; } - - if (buffer[0]==0x0a) + if (buffer[0] == 0x0a) { buffer++; numBytesRead++; @@ -1111,8 +1136,8 @@ static int nextLine(const char* buffer) return numBytesRead; } -/* Create from TetGen .ele, .face, .node data */ -btSoftBody* btSoftBodyHelpers::CreateFromTetGenData(btSoftBodyWorldInfo& worldInfo, +/* Create from TetGen .ele, .face, .node data */ +btSoftBody* btSoftBodyHelpers::CreateFromTetGenData(btSoftBodyWorldInfo& worldInfo, const char* ele, const char* face, const char* node, @@ -1120,39 +1145,38 @@ btSoftBody* btSoftBodyHelpers::CreateFromTetGenData(btSoftBodyWorldInfo& worldIn 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); -(void)result; -node += nextLine(node); - -pos.resize(nnode); -for(int i=0;i<pos.size();++i) + 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); + 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); + // sn>>index; + // sn>>x;sn>>y;sn>>z; + node += nextLine(node); - //for(int j=0;j<nattrb;++j) - // sn>>a; + //for(int j=0;j<nattrb;++j) + // sn>>a; - //if(hasbounds) - // sn>>bound; + //if(hasbounds) + // sn>>bound; - pos[index].setX(btScalar(x)); - pos[index].setY(btScalar(y)); - pos[index].setZ(btScalar(z)); + pos[index].setX(btScalar(x)); + pos[index].setY(btScalar(y)); + pos[index].setZ(btScalar(z)); } -btSoftBody* psb=new btSoftBody(&worldInfo,nnode,&pos[0],0); + btSoftBody* psb = new btSoftBody(&worldInfo, nnode, &pos[0], 0); #if 0 if(face&&face[0]) { @@ -1177,42 +1201,461 @@ if(face&&face[0]) } #endif -if(ele&&ele[0]) + 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 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) + 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); + 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); + } + } + } + psb->initializeDmInverse(); + psb->m_tetraScratches.resize(psb->m_tetras.size()); + psb->m_tetraScratchesTn.resize(psb->m_tetras.size()); + 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); +} + +btSoftBody* btSoftBodyHelpers::CreateFromVtkFile(btSoftBodyWorldInfo& worldInfo, const char* vtk_file) +{ + std::ifstream fs; + fs.open(vtk_file); + btAssert(fs); + + typedef btAlignedObjectArray<int> Index; + std::string line; + btAlignedObjectArray<btVector3> X; + btVector3 position; + btAlignedObjectArray<Index> indices; + bool reading_points = false; + bool reading_tets = false; + size_t n_points = 0; + size_t n_tets = 0; + size_t x_count = 0; + size_t indices_count = 0; + while (std::getline(fs, line)) + { + std::stringstream ss(line); + if (line.size() == (size_t)(0)) + { + } + else if (line.substr(0, 6) == "POINTS") + { + reading_points = true; + reading_tets = false; + ss.ignore(128, ' '); // ignore "POINTS" + ss >> n_points; + X.resize(n_points); + } + else if (line.substr(0, 5) == "CELLS") + { + reading_points = false; + reading_tets = true; + ss.ignore(128, ' '); // ignore "CELLS" + ss >> n_tets; + indices.resize(n_tets); + } + else if (line.substr(0, 10) == "CELL_TYPES") + { + reading_points = false; + reading_tets = false; + } + else if (reading_points) + { + btScalar p; + ss >> p; + position.setX(p); + ss >> p; + position.setY(p); + ss >> p; + position.setZ(p); + X[x_count++] = position; + } + else if (reading_tets) + { + int d; + ss >> d; + if (d != 4) + { + printf("Load deformable failed: Only Tetrahedra are supported in VTK file.\n"); + fs.close(); + return 0; } + ss.ignore(128, ' '); // ignore "4" + Index tet; + tet.resize(4); + for (size_t i = 0; i < 4; i++) + { + ss >> tet[i]; + printf("%d ", tet[i]); + } + printf("\n"); + indices[indices_count++] = tet; + } + } + btSoftBody* psb = new btSoftBody(&worldInfo, n_points, &X[0], 0); + + for (int i = 0; i < n_tets; ++i) + { + const Index& ni = indices[i]; + psb->appendTetra(ni[0], ni[1], ni[2], ni[3]); + { + 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: %d\r\n",psb->m_nodes.size()); -printf("Links: %d\r\n",psb->m_links.size()); -printf("Faces: %d\r\n",psb->m_faces.size()); -printf("Tetras: %d\r\n",psb->m_tetras.size()); -return(psb); + + generateBoundaryFaces(psb); + psb->initializeDmInverse(); + psb->m_tetraScratches.resize(psb->m_tetras.size()); + psb->m_tetraScratchesTn.resize(psb->m_tetras.size()); + 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()); + + fs.close(); + return psb; } +void btSoftBodyHelpers::generateBoundaryFaces(btSoftBody* psb) +{ + int counter = 0; + for (int i = 0; i < psb->m_nodes.size(); ++i) + { + psb->m_nodes[i].index = counter++; + } + typedef btAlignedObjectArray<int> Index; + btAlignedObjectArray<Index> indices; + indices.resize(psb->m_tetras.size()); + for (int i = 0; i < indices.size(); ++i) + { + Index index; + index.push_back(psb->m_tetras[i].m_n[0]->index); + index.push_back(psb->m_tetras[i].m_n[1]->index); + index.push_back(psb->m_tetras[i].m_n[2]->index); + index.push_back(psb->m_tetras[i].m_n[3]->index); + indices[i] = index; + } + + std::map<std::vector<int>, std::vector<int> > dict; + for (int i = 0; i < indices.size(); ++i) + { + for (int j = 0; j < 4; ++j) + { + std::vector<int> f; + if (j == 0) + { + f.push_back(indices[i][1]); + f.push_back(indices[i][0]); + f.push_back(indices[i][2]); + } + if (j == 1) + { + f.push_back(indices[i][3]); + f.push_back(indices[i][0]); + f.push_back(indices[i][1]); + } + if (j == 2) + { + f.push_back(indices[i][3]); + f.push_back(indices[i][1]); + f.push_back(indices[i][2]); + } + if (j == 3) + { + f.push_back(indices[i][2]); + f.push_back(indices[i][0]); + f.push_back(indices[i][3]); + } + std::vector<int> f_sorted = f; + std::sort(f_sorted.begin(), f_sorted.end()); + if (dict.find(f_sorted) != dict.end()) + { + dict.erase(f_sorted); + } + else + { + dict.insert(std::make_pair(f_sorted, f)); + } + } + } + + for (std::map<std::vector<int>, std::vector<int> >::iterator it = dict.begin(); it != dict.end(); ++it) + { + std::vector<int> f = it->second; + psb->appendFace(f[0], f[1], f[2]); + } +} + +//Write the surface mesh to an obj file. +void btSoftBodyHelpers::writeObj(const char* filename, const btSoftBody* psb) +{ + std::ofstream fs; + fs.open(filename); + btAssert(fs); + + if (psb->m_tetras.size() > 0) + { + // For tetrahedron mesh, we need to re-index the surface mesh for it to be in obj file/ + std::map<int, int> dict; + for (int i = 0; i < psb->m_faces.size(); i++) + { + for (int d = 0; d < 3; d++) + { + int index = psb->m_faces[i].m_n[d]->index; + if (dict.find(index) == dict.end()) + { + int dict_size = dict.size(); + dict[index] = dict_size; + fs << "v"; + for (int k = 0; k < 3; k++) + { + fs << " " << psb->m_nodes[index].m_x[k]; + } + fs << "\n"; + } + } + } + // Write surface mesh. + for (int i = 0; i < psb->m_faces.size(); ++i) + { + fs << "f"; + for (int n = 0; n < 3; n++) + { + fs << " " << dict[psb->m_faces[i].m_n[n]->index] + 1; + } + fs << "\n"; + } + } + else + { + // For trimesh, directly write out all the nodes and faces.xs + for (int i = 0; i < psb->m_nodes.size(); ++i) + { + fs << "v"; + for (int d = 0; d < 3; d++) + { + fs << " " << psb->m_nodes[i].m_x[d]; + } + fs << "\n"; + } + + for (int i = 0; i < psb->m_faces.size(); ++i) + { + fs << "f"; + for (int n = 0; n < 3; n++) + { + fs << " " << psb->m_faces[i].m_n[n]->index + 1; + } + fs << "\n"; + } + } + fs.close(); +} + +void btSoftBodyHelpers::duplicateFaces(const char* filename, const btSoftBody* psb) +{ + std::ifstream fs_read; + fs_read.open(filename); + std::string line; + btVector3 pos; + btAlignedObjectArray<btAlignedObjectArray<int> > additional_faces; + while (std::getline(fs_read, line)) + { + std::stringstream ss(line); + if (line[0] == 'v') + { + } + else if (line[0] == 'f') + { + ss.ignore(); + int id0, id1, id2; + ss >> id0; + ss >> id1; + ss >> id2; + btAlignedObjectArray<int> new_face; + new_face.push_back(id1); + new_face.push_back(id0); + new_face.push_back(id2); + additional_faces.push_back(new_face); + } + } + fs_read.close(); + + std::ofstream fs_write; + fs_write.open(filename, std::ios_base::app); + for (int i = 0; i < additional_faces.size(); ++i) + { + fs_write << "f"; + for (int n = 0; n < 3; n++) + { + fs_write << " " << additional_faces[i][n]; + } + fs_write << "\n"; + } + fs_write.close(); +} + +// Given a simplex with vertices a,b,c,d, find the barycentric weights of p in this simplex +void btSoftBodyHelpers::getBarycentricWeights(const btVector3& a, const btVector3& b, const btVector3& c, const btVector3& d, const btVector3& p, btVector4& bary) +{ + btVector3 vap = p - a; + btVector3 vbp = p - b; + + btVector3 vab = b - a; + btVector3 vac = c - a; + btVector3 vad = d - a; + + btVector3 vbc = c - b; + btVector3 vbd = d - b; + btScalar va6 = (vbp.cross(vbd)).dot(vbc); + btScalar vb6 = (vap.cross(vac)).dot(vad); + btScalar vc6 = (vap.cross(vad)).dot(vab); + btScalar vd6 = (vap.cross(vab)).dot(vac); + btScalar v6 = btScalar(1) / (vab.cross(vac).dot(vad)); + bary = btVector4(va6 * v6, vb6 * v6, vc6 * v6, vd6 * v6); +} + +// Given a simplex with vertices a,b,c, find the barycentric weights of p in this simplex. bary[3] = 0. +void btSoftBodyHelpers::getBarycentricWeights(const btVector3& a, const btVector3& b, const btVector3& c, const btVector3& p, btVector4& bary) +{ + btVector3 v0 = b - a, v1 = c - a, v2 = p - a; + btScalar d00 = btDot(v0, v0); + btScalar d01 = btDot(v0, v1); + btScalar d11 = btDot(v1, v1); + btScalar d20 = btDot(v2, v0); + btScalar d21 = btDot(v2, v1); + btScalar invDenom = 1.0 / (d00 * d11 - d01 * d01); + bary[1] = (d11 * d20 - d01 * d21) * invDenom; + bary[2] = (d00 * d21 - d01 * d20) * invDenom; + bary[0] = 1.0 - bary[1] - bary[2]; + bary[3] = 0; +} + +// Iterate through all render nodes to find the simulation tetrahedron that contains the render node and record the barycentric weights +// If the node is not inside any tetrahedron, assign it to the tetrahedron in which the node has the least negative barycentric weight +void btSoftBodyHelpers::interpolateBarycentricWeights(btSoftBody* psb) +{ + psb->m_z.resize(0); + psb->m_renderNodesInterpolationWeights.resize(psb->m_renderNodes.size()); + psb->m_renderNodesParents.resize(psb->m_renderNodes.size()); + for (int i = 0; i < psb->m_renderNodes.size(); ++i) + { + const btVector3& p = psb->m_renderNodes[i].m_x; + btVector4 bary; + btVector4 optimal_bary; + btScalar min_bary_weight = -1e3; + btAlignedObjectArray<const btSoftBody::Node*> optimal_parents; + for (int j = 0; j < psb->m_tetras.size(); ++j) + { + const btSoftBody::Tetra& t = psb->m_tetras[j]; + getBarycentricWeights(t.m_n[0]->m_x, t.m_n[1]->m_x, t.m_n[2]->m_x, t.m_n[3]->m_x, p, bary); + btScalar new_min_bary_weight = bary[0]; + for (int k = 1; k < 4; ++k) + { + new_min_bary_weight = btMin(new_min_bary_weight, bary[k]); + } + if (new_min_bary_weight > min_bary_weight) + { + btAlignedObjectArray<const btSoftBody::Node*> parents; + parents.push_back(t.m_n[0]); + parents.push_back(t.m_n[1]); + parents.push_back(t.m_n[2]); + parents.push_back(t.m_n[3]); + optimal_parents = parents; + optimal_bary = bary; + min_bary_weight = new_min_bary_weight; + // stop searching if p is inside the tetrahedron at hand + if (bary[0] >= 0. && bary[1] >= 0. && bary[2] >= 0. && bary[3] >= 0.) + { + break; + } + } + } + psb->m_renderNodesInterpolationWeights[i] = optimal_bary; + psb->m_renderNodesParents[i] = optimal_parents; + } +} + +// Iterate through all render nodes to find the simulation triangle that's closest to the node in the barycentric sense. +void btSoftBodyHelpers::extrapolateBarycentricWeights(btSoftBody* psb) +{ + psb->m_renderNodesInterpolationWeights.resize(psb->m_renderNodes.size()); + psb->m_renderNodesParents.resize(psb->m_renderNodes.size()); + psb->m_z.resize(psb->m_renderNodes.size()); + for (int i = 0; i < psb->m_renderNodes.size(); ++i) + { + const btVector3& p = psb->m_renderNodes[i].m_x; + btVector4 bary; + btVector4 optimal_bary; + btScalar min_bary_weight = -SIMD_INFINITY; + btAlignedObjectArray<const btSoftBody::Node*> optimal_parents; + btScalar dist = 0, optimal_dist = 0; + for (int j = 0; j < psb->m_faces.size(); ++j) + { + const btSoftBody::Face& f = psb->m_faces[j]; + btVector3 n = btCross(f.m_n[1]->m_x - f.m_n[0]->m_x, f.m_n[2]->m_x - f.m_n[0]->m_x); + btVector3 unit_n = n.normalized(); + dist = (p - f.m_n[0]->m_x).dot(unit_n); + btVector3 proj_p = p - dist * unit_n; + getBarycentricWeights(f.m_n[0]->m_x, f.m_n[1]->m_x, f.m_n[2]->m_x, proj_p, bary); + btScalar new_min_bary_weight = bary[0]; + for (int k = 1; k < 3; ++k) + { + new_min_bary_weight = btMin(new_min_bary_weight, bary[k]); + } + + // p is out of the current best triangle, we found a traingle that's better + bool better_than_closest_outisde = (new_min_bary_weight > min_bary_weight && min_bary_weight < 0.); + // p is inside of the current best triangle, we found a triangle that's better + bool better_than_best_inside = (new_min_bary_weight >= 0 && min_bary_weight >= 0 && btFabs(dist) < btFabs(optimal_dist)); + + if (better_than_closest_outisde || better_than_best_inside) + { + btAlignedObjectArray<const btSoftBody::Node*> parents; + parents.push_back(f.m_n[0]); + parents.push_back(f.m_n[1]); + parents.push_back(f.m_n[2]); + optimal_parents = parents; + optimal_bary = bary; + optimal_dist = dist; + min_bary_weight = new_min_bary_weight; + } + } + psb->m_renderNodesInterpolationWeights[i] = optimal_bary; + psb->m_renderNodesParents[i] = optimal_parents; + psb->m_z[i] = optimal_dist; + } +} |