diff options
Diffstat (limited to 'thirdparty/bullet/BulletSoftBody/btSoftBodyHelpers.cpp')
| -rw-r--r-- | thirdparty/bullet/BulletSoftBody/btSoftBodyHelpers.cpp | 1219 |
1 files changed, 1219 insertions, 0 deletions
diff --git a/thirdparty/bullet/BulletSoftBody/btSoftBodyHelpers.cpp b/thirdparty/bullet/BulletSoftBody/btSoftBodyHelpers.cpp new file mode 100644 index 0000000000..51fcd16da4 --- /dev/null +++ b/thirdparty/bullet/BulletSoftBody/btSoftBodyHelpers.cpp @@ -0,0 +1,1219 @@ +/* +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 "btSoftBodyInternals.h" +#include <stdio.h> +#include <string.h> +#include "btSoftBodyHelpers.h" +#include "LinearMath/btConvexHull.h" +#include "LinearMath/btConvexHullComputer.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); +} + +#if 0 +// + inline static 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))); +} +#endif + +// +#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; + + /* 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; + } +#define USE_NEW_CONVEX_HULL_COMPUTER +#ifdef USE_NEW_CONVEX_HULL_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++) + { + + int face = computer.faces[i]; + //printf("face=%d\n",face); + const btConvexHullComputer::Edge* firstEdge = &computer.edges[face]; + const btConvexHullComputer::Edge* edge = firstEdge->getNextEdgeOfFace(); + + int v0 = firstEdge->getSourceVertex(); + int v1 = firstEdge->getTargetVertex(); + while (edge!=firstEdge) + { + int v2 = edge->getTargetVertex(); + idraw->drawTriangle(computer.vertices[v0],computer.vertices[v1],computer.vertices[v2],color,1); + edge = edge->getNextEdgeOfFace(); + 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) + { + 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); +#endif + + } + /* 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)); + } + } + else + { + /* 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)); + } + } + /* 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); + } + } + /* 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 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)) + { + 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)); + } + } + } + + + /* 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)); + break; + } + default: + { + } + + } + } + } +} + +// +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); +} + + +//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]. +//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, +//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, +//or right after generateBendingConstraints() once we have all 1410 links. + + +//=================================================================== +// +// +// 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 +// (inevitably) finite instruction windows. +// +//=================================================================== + +// 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 +}; +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 + + +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 ar, br; + 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 + + // 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); + + // Clear out the node setup and ready list + for (i=0; i < nNodes+1; i++) { + nodeWrittenAt[i] = REOP_NOT_DEPENDENT; + } + 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++) { + + // 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) { + linkDepA[i] = nodeWrittenAt[ar]; + linkDep = &linkDepFreeList[linkDepFrees++]; + linkDep->value = i; + linkDep->next = linkDepListStarts[nodeWrittenAt[ar]]; + linkDepListStarts[nodeWrittenAt[ar]] = linkDep; + } else { + linkDepA[i] = REOP_NOT_DEPENDENT; + } + if (nodeWrittenAt[br] > REOP_NOT_DEPENDENT) { + linkDepB[i] = nodeWrittenAt[br]; + linkDep = &linkDepFreeList[linkDepFrees++]; + linkDep->value = -(i+1); + linkDep->next = linkDepListStarts[nodeWrittenAt[br]]; + linkDepListStarts[nodeWrittenAt[br]] = linkDep; + } 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)) { + readyList[readyListTail++] = i; + 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) { + // 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 + psb->m_links[i++] = linkBuffer[linkNum]; + + // Free up any link inputs that are dependent on this one + linkDep = linkDepListStarts[linkNum]; + while (linkDep) { + depLink = linkDep->value; + if (depLink >= 0) { + linkDepA[depLink] = REOP_NOT_DEPENDENT; + } 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)) { + readyList[readyListTail++] = depLink; + 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; +} + + +// +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, bool randomizeConstraints) +{ + 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]); + } + + if (randomizeConstraints) + { + psb->randomizeConstraints(); + } + + return(psb); +} + +// +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) + { + 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); +} + + + + +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); +} + |