Page MenuHome
Differential D8762 Diff 28333 extern/bullet2/src/BulletSoftBody/btSoftBodyHelpers.cpp

Changeset View

Standalone View

View Options

extern/bullet2/src/BulletSoftBody/btSoftBodyHelpers.cpp

Show All 10 Lines
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. 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. 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. 3. This notice may not be removed or altered from any source distribution.
*/ */
///btSoftBodyHelpers.cpp by Nathanael Presson ///btSoftBodyHelpers.cpp by Nathanael Presson
#include "btSoftBodyInternals.h" #include "btSoftBodyInternals.h"
#include <stdio.h> #include <stdio.h>
#include <string>
#include <iostream>
#include <sstream>
#include <string.h> #include <string.h>
#include <algorithm>
#include "btSoftBodyHelpers.h" #include "btSoftBodyHelpers.h"
#include "LinearMath/btConvexHull.h" #include "LinearMath/btConvexHull.h"
#include "LinearMath/btConvexHullComputer.h" #include "LinearMath/btConvexHullComputer.h"
#include <map>
#include <vector>
//
static void drawVertex( btIDebugDraw* idraw, static void drawVertex(btIDebugDraw* idraw,
const btVector3& x,btScalar s,const btVector3& c) 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(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, 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(0, 0, s), x + btVector3(0, 0, s), c);
} }
// //
static void drawBox( btIDebugDraw* idraw, static void drawBox(btIDebugDraw* idraw,
const btVector3& mins, const btVector3& mins,
const btVector3& maxs, const btVector3& maxs,
const btVector3& color) const btVector3& color)
{ {
const btVector3 c[]={ btVector3(mins.x(),mins.y(),mins.z()), const btVector3 c[] = {btVector3(mins.x(), mins.y(), mins.z()),
btVector3(maxs.x(),mins.y(),mins.z()), btVector3(maxs.x(), mins.y(), mins.z()),
btVector3(maxs.x(),maxs.y(),mins.z()), btVector3(maxs.x(), maxs.y(), mins.z()),
btVector3(mins.x(),maxs.y(),mins.z()), btVector3(mins.x(), maxs.y(), mins.z()),
btVector3(mins.x(),mins.y(),maxs.z()), btVector3(mins.x(), mins.y(), maxs.z()),
btVector3(maxs.x(),mins.y(),maxs.z()), btVector3(maxs.x(), mins.y(), maxs.z()),
btVector3(maxs.x(),maxs.y(),maxs.z()), btVector3(maxs.x(), maxs.y(), maxs.z()),
btVector3(mins.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[0], c[1], color);
idraw->drawLine(c[2],c[3],color);idraw->drawLine(c[3],c[0],color); idraw->drawLine(c[1], c[2], color);
idraw->drawLine(c[4],c[5],color);idraw->drawLine(c[5],c[6],color); idraw->drawLine(c[2], c[3], color);
idraw->drawLine(c[6],c[7],color);idraw->drawLine(c[7],c[4],color); idraw->drawLine(c[3], c[0], color);
idraw->drawLine(c[0],c[4],color);idraw->drawLine(c[1],c[5],color); idraw->drawLine(c[4], c[5], color);
idraw->drawLine(c[2],c[6],color);idraw->drawLine(c[3],c[7],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, static void drawTree(btIDebugDraw* idraw,
const btDbvtNode* node, const btDbvtNode* node,
int depth, int depth,
const btVector3& ncolor, const btVector3& ncolor,
const btVector3& lcolor, const btVector3& lcolor,
int mindepth, int mindepth,
int maxdepth) 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[0], depth + 1, ncolor, lcolor, mindepth, maxdepth);
drawTree(idraw,node->childs[1],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 btScalar scl = (btScalar)(node->isinternal() ? 1 : 1);
const btVector3 mi=node->volume.Center()-node->volume.Extents()*scl; const btVector3 mi = node->volume.Center() - node->volume.Extents() * scl;
const btVector3 mx=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); drawBox(idraw, mi, mx, node->isleaf() ? lcolor : ncolor);
} }
} }
} }
// //
template <typename T> template <typename T>
static inline T sum(const btAlignedObjectArray<T>& items) static inline T sum(const btAlignedObjectArray<T>& items)
{ {
T v; T v;
if(items.size()) if (items.size())
{ {
v=items[0]; v = items[0];
for(int i=1,ni=items.size();i<ni;++i) 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> template <typename T, typename Q>
static inline void add(btAlignedObjectArray<T>& items,const Q& value) 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> template <typename T, typename Q>
static inline void mul(btAlignedObjectArray<T>& items,const Q& value) 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> 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); const btScalar n = (btScalar)(items.size() > 0 ? items.size() : 1);
return(sum(items)/n); 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& x1,
const btVector3& x2, const btVector3& x2,
const btVector3& x3) const btVector3& x3)
{ {
const btVector3 a=x1-x0; const btVector3 a=x1-x0;
const btVector3 b=x2-x0; const btVector3 b=x2-x0;
const btVector3 c=x3-x0; const btVector3 c=x3-x0;
return(btDot(a,btCross(b,c))); return(btDot(a,btCross(b,c)));
} }
#endif
// //
#if 0 #if 0
static btVector3 stresscolor(btScalar stress) static btVector3 stresscolor(btScalar stress)
{ {
static const btVector3 spectrum[]= { btVector3(1,0,1), static const btVector3 spectrum[]= { btVector3(1,0,1),
btVector3(0,0,1), btVector3(0,0,1),
btVector3(0,1,1), btVector3(0,1,1),
btVector3(0,1,0), btVector3(0,1,0),
btVector3(1,1,0), btVector3(1,1,0),
btVector3(1,0,0), btVector3(1,0,0),
btVector3(1,0,0)}; btVector3(1,0,0)};
static const int ncolors=sizeof(spectrum)/sizeof(spectrum[0])-1; static const int ncolors=sizeof(spectrum)/sizeof(spectrum[0])-1;
static const btScalar one=1; static const btScalar one=1;
stress=btMax<btScalar>(0,btMin<btScalar>(1,stress))*ncolors; stress=btMax<btScalar>(0,btMin<btScalar>(1,stress))*ncolors;
const int sel=(int)stress; const int sel=(int)stress;
const btScalar frc=stress-sel; const btScalar frc=stress-sel;
return(spectrum[sel]+(spectrum[sel+1]-spectrum[sel])*frc); return(spectrum[sel]+(spectrum[sel+1]-spectrum[sel])*frc);
} }
#endif #endif
// //
void btSoftBodyHelpers::Draw( btSoftBody* psb, void btSoftBodyHelpers::Draw(btSoftBody* psb,
btIDebugDraw* idraw, btIDebugDraw* idraw,
int drawflags) int drawflags)
{ {
const btScalar scl=(btScalar)0.1; const btScalar scl = (btScalar)0.1;
const btScalar nscl=scl*5; const btScalar nscl = scl * 5;
const btVector3 lcolor=btVector3(0,0,0); const btVector3 lcolor = btVector3(0, 0, 0);
const btVector3 ncolor=btVector3(1,1,1); const btVector3 ncolor = btVector3(1, 1, 1);
const btVector3 ccolor=btVector3(1,0,0); const btVector3 ccolor = btVector3(1, 0, 0);
int i,j,nj; int i, j, nj;
/* Clusters */ /* Clusters */
if(0!=(drawflags&fDrawFlags::Clusters)) if (0 != (drawflags & fDrawFlags::Clusters))
{ {
srand(1806); 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, btVector3 color(rand() / (btScalar)RAND_MAX,
rand()/(btScalar)RAND_MAX, rand() / (btScalar)RAND_MAX,
rand()/(btScalar)RAND_MAX); rand() / (btScalar)RAND_MAX);
color=color.normalized()*0.75; color = color.normalized() * 0.75;
btAlignedObjectArray<btVector3> vertices; btAlignedObjectArray<btVector3> vertices;
vertices.resize(psb->m_clusters[i]->m_nodes.size()); vertices.resize(psb->m_clusters[i]->m_nodes.size());
for(j=0,nj=vertices.size();j<nj;++j) for (j = 0, nj = vertices.size(); j < nj; ++j)
{ {
vertices[j]=psb->m_clusters[i]->m_nodes[j]->m_x; vertices[j] = psb->m_clusters[i]->m_nodes[j]->m_x;
} }
#define USE_NEW_CONVEX_HULL_COMPUTER #define USE_NEW_CONVEX_HULL_COMPUTER
#ifdef USE_NEW_CONVEX_HULL_COMPUTER #ifdef USE_NEW_CONVEX_HULL_COMPUTER
btConvexHullComputer computer; btConvexHullComputer computer;
int stride = sizeof(btVector3); int stride = sizeof(btVector3);
int count = vertices.size(); int count = vertices.size();
btScalar shrink=0.f; btScalar shrink = 0.f;
btScalar shrinkClamp=0.f; btScalar shrinkClamp = 0.f;
computer.compute(&vertices[0].getX(),stride,count,shrink,shrinkClamp); computer.compute(&vertices[0].getX(), stride, count, shrink, shrinkClamp);
for (int i=0;i<computer.faces.size();i++) for (int i = 0; i < computer.faces.size(); i++)
{ {
int face = computer.faces[i]; int face = computer.faces[i];
//printf("face=%d\n",face); //printf("face=%d\n",face);
const btConvexHullComputer::Edge* firstEdge = &computer.edges[face]; const btConvexHullComputer::Edge* firstEdge = &computer.edges[face];
const btConvexHullComputer::Edge* edge = firstEdge->getNextEdgeOfFace(); const btConvexHullComputer::Edge* edge = firstEdge->getNextEdgeOfFace();
int v0 = firstEdge->getSourceVertex(); int v0 = firstEdge->getSourceVertex();
int v1 = firstEdge->getTargetVertex(); int v1 = firstEdge->getTargetVertex();
while (edge!=firstEdge) while (edge != firstEdge)
{ {
int v2 = edge->getTargetVertex(); 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(); edge = edge->getNextEdgeOfFace();
v0=v1; v0 = v1;
v1=v2; v1 = v2;
}; };
} }
#else #else
HullDesc hdsc(QF_TRIANGLES,vertices.size(),&vertices[0]); HullDesc hdsc(QF_TRIANGLES, vertices.size(), &vertices[0]);
HullResult hres; HullResult hres;
HullLibrary hlib; HullLibrary hlib;
hdsc.mMaxVertices=vertices.size(); hdsc.mMaxVertices = vertices.size();
hlib.CreateConvexHull(hdsc,hres); hlib.CreateConvexHull(hdsc, hres);
const btVector3 center=average(hres.m_OutputVertices); const btVector3 center = average(hres.m_OutputVertices);
add(hres.m_OutputVertices,-center); add(hres.m_OutputVertices, -center);
mul(hres.m_OutputVertices,(btScalar)1); mul(hres.m_OutputVertices, (btScalar)1);
add(hres.m_OutputVertices,center); add(hres.m_OutputVertices, center);
for(j=0;j<(int)hres.mNumFaces;++j) 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]], idraw->drawTriangle(hres.m_OutputVertices[idx[0]],
hres.m_OutputVertices[idx[1]], hres.m_OutputVertices[idx[1]],
hres.m_OutputVertices[idx[2]], hres.m_OutputVertices[idx[2]],
color,1); color, 1);
} }
hlib.ReleaseResult(hres); hlib.ReleaseResult(hres);
#endif #endif
} }
/* Velocities */ /* Velocities */
#if 0 #if 0
for(int j=0;j<psb->m_clusters[i].m_nodes.size();++j) for(int j=0;j<psb->m_clusters[i].m_nodes.size();++j)
{ {
const btSoftBody::Cluster& c=psb->m_clusters[i]; const btSoftBody::Cluster& c=psb->m_clusters[i];
const btVector3 r=c.m_nodes[j]->m_x-c.m_com; const btVector3 r=c.m_nodes[j]->m_x-c.m_com;
const btVector3 v=c.m_lv+btCross(c.m_av,r); 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)); idraw->drawLine(c.m_nodes[j]->m_x,c.m_nodes[j]->m_x+v,btVector3(1,0,0));
} }
#endif #endif
/* Frame */ /* Frame */
// btSoftBody::Cluster& c=*psb->m_clusters[i]; // 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(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,10,0),btVector3(0,1,0));
// idraw->drawLine(c.m_com,c.m_framexform*btVector3(0,0,10),btVector3(0,0,1)); // idraw->drawLine(c.m_com,c.m_framexform*btVector3(0,0,10),btVector3(0,0,1));
} }
} }
else else
{ {
/* Nodes */ /* Nodes */
if(0!=(drawflags&fDrawFlags::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]; const btSoftBody::Node& n = psb->m_nodes[i];
if(0==(n.m_material->m_flags&btSoftBody::fMaterial::DebugDraw)) continue; 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(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, 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)); idraw->drawLine(n.m_x - btVector3(0, 0, scl), n.m_x + btVector3(0, 0, scl), btVector3(0, 0, 1));
} }
} }
/* Links */ /* Links */
if(0!=(drawflags&fDrawFlags::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]; const btSoftBody::Link& l = psb->m_links[i];
if(0==(l.m_material->m_flags&btSoftBody::fMaterial::DebugDraw)) continue; 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); idraw->drawLine(l.m_n[0]->m_x, l.m_n[1]->m_x, lcolor);
} }
} }
/* Normals */ /* Normals */
if(0!=(drawflags&fDrawFlags::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]; const btSoftBody::Node& n = psb->m_nodes[i];
if(0==(n.m_material->m_flags&btSoftBody::fMaterial::DebugDraw)) continue; if (0 == (n.m_material->m_flags & btSoftBody::fMaterial::DebugDraw)) continue;
const btVector3 d=n.m_n*nscl; 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);
idraw->drawLine(n.m_x,n.m_x-d,ncolor*0.5); idraw->drawLine(n.m_x, n.m_x - d, ncolor * 0.5);
} }
} }
/* Contacts */ /* Contacts */
if(0!=(drawflags&fDrawFlags::Contacts)) if (0 != (drawflags & fDrawFlags::Contacts))
{ {
static const btVector3 axis[]={btVector3(1,0,0), static const btVector3 axis[] = {btVector3(1, 0, 0),
btVector3(0,1,0), btVector3(0, 1, 0),
btVector3(0,0,1)}; btVector3(0, 0, 1)};
for(i=0;i<psb->m_rcontacts.size();++i) for (i = 0; i < psb->m_rcontacts.size(); ++i)
{ {
const btSoftBody::RContact& c=psb->m_rcontacts[i]; const btSoftBody::RContact& c = psb->m_rcontacts[i];
const btVector3 o= c.m_node->m_x-c.m_cti.m_normal* 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); (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 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(); const btVector3 y = btCross(x, c.m_cti.m_normal).normalized();
idraw->drawLine(o-x*nscl,o+x*nscl,ccolor); idraw->drawLine(o - x * nscl, o + x * nscl, ccolor);
idraw->drawLine(o-y*nscl,o+y*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)); idraw->drawLine(o, o + c.m_cti.m_normal * nscl * 3, btVector3(1, 1, 0));
} }
} }
/* Faces */ /* Faces */
if(0!=(drawflags&fDrawFlags::Faces)) if (0 != (drawflags & fDrawFlags::Faces))
{ {
const btScalar scl=(btScalar)0.8; const btScalar scl = (btScalar)0.8;
const btScalar alp=(btScalar)1; const btScalar alp = (btScalar)1;
const btVector3 col(0,(btScalar)0.7,0); const btVector3 col(0, (btScalar)0.7, 0);
for(i=0;i<psb->m_faces.size();++i) for (i = 0; i < psb->m_faces.size(); ++i)
{ {
const btSoftBody::Face& f=psb->m_faces[i]; const btSoftBody::Face& f = psb->m_faces[i];
if(0==(f.m_material->m_flags&btSoftBody::fMaterial::DebugDraw)) continue; 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 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; const btVector3 c = (x[0] + x[1] + x[2]) / 3;
idraw->drawTriangle((x[0]-c)*scl+c, idraw->drawTriangle((x[0] - c) * scl + c,
(x[1]-c)*scl+c, (x[1] - c) * scl + c,
(x[2]-c)*scl+c, (x[2] - c) * scl + c,
col,alp); col, alp);
} }
} }
/* Tetras */ /* Tetras */
if(0!=(drawflags&fDrawFlags::Tetras)) if (0 != (drawflags & fDrawFlags::Tetras))
{ {
const btScalar scl=(btScalar)0.8; const btScalar scl = (btScalar)0.8;
const btScalar alp=(btScalar)1; const btScalar alp = (btScalar)1;
const btVector3 col((btScalar)0.3,(btScalar)0.3,(btScalar)0.7); const btVector3 col((btScalar)0.3, (btScalar)0.3, (btScalar)0.7);
for(int i=0;i<psb->m_tetras.size();++i) for (int i = 0; i < psb->m_tetras.size(); ++i)
{ {
const btSoftBody::Tetra& t=psb->m_tetras[i]; const btSoftBody::Tetra& t = psb->m_tetras[i];
if(0==(t.m_material->m_flags&btSoftBody::fMaterial::DebugDraw)) continue; 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 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; 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[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[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[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); idraw->drawTriangle((x[2] - c) * scl + c, (x[0] - c) * scl + c, (x[3] - c) * scl + c, col, alp);
} }
} }
} }
/* Anchors */ /* Anchors */
if(0!=(drawflags&fDrawFlags::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 btSoftBody::Anchor& a = psb->m_anchors[i];
const btVector3 q=a.m_body->getWorldTransform()*a.m_local; 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, a.m_node->m_x, 0.25, btVector3(1, 0, 0));
drawVertex(idraw,q,0.25,btVector3(0,1,0)); drawVertex(idraw, q, 0.25, btVector3(0, 1, 0));
idraw->drawLine(a.m_node->m_x,q,btVector3(1,1,1)); 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]; const btSoftBody::Node& n = psb->m_nodes[i];
if(0==(n.m_material->m_flags&btSoftBody::fMaterial::DebugDraw)) continue; if (0 == (n.m_material->m_flags & btSoftBody::fMaterial::DebugDraw)) continue;
if(n.m_im<=0) 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 */ /* Notes */
if(0!=(drawflags&fDrawFlags::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]; const btSoftBody::Note& n = psb->m_notes[i];
btVector3 p=n.m_offset; btVector3 p = n.m_offset;
for(int j=0;j<n.m_rank;++j) 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 */ /* Node tree */
if(0!=(drawflags&fDrawFlags::NodeTree)) DrawNodeTree(psb,idraw); if (0 != (drawflags & fDrawFlags::NodeTree)) DrawNodeTree(psb, idraw);
/* Face tree */ /* Face tree */
if(0!=(drawflags&fDrawFlags::FaceTree)) DrawFaceTree(psb,idraw); if (0 != (drawflags & fDrawFlags::FaceTree)) DrawFaceTree(psb, idraw);
/* Cluster tree */ /* Cluster tree */
if(0!=(drawflags&fDrawFlags::ClusterTree)) DrawClusterTree(psb,idraw); if (0 != (drawflags & fDrawFlags::ClusterTree)) DrawClusterTree(psb, idraw);
/* Joints */ /* Joints */
if(0!=(drawflags&fDrawFlags::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]; const btSoftBody::Joint* pj = psb->m_joints[i];
switch(pj->Type()) switch (pj->Type())
{ {
case btSoftBody::Joint::eType::Linear: case btSoftBody::Joint::eType::Linear:
{ {
const btSoftBody::LJoint* pjl=(const btSoftBody::LJoint*)pj; const btSoftBody::LJoint* pjl = (const btSoftBody::LJoint*)pj;
const btVector3 a0=pj->m_bodies[0].xform()*pjl->m_refs[0]; const btVector3 a0 = pj->m_bodies[0].xform() * pjl->m_refs[0];
const btVector3 a1=pj->m_bodies[1].xform()*pjl->m_refs[1]; 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[0].xform().getOrigin(), a0, btVector3(1, 1, 0));
idraw->drawLine(pj->m_bodies[1].xform().getOrigin(),a1,btVector3(0,1,1)); idraw->drawLine(pj->m_bodies[1].xform().getOrigin(), a1, btVector3(0, 1, 1));
drawVertex(idraw,a0,0.25,btVector3(1,1,0)); drawVertex(idraw, a0, 0.25, btVector3(1, 1, 0));
drawVertex(idraw,a1,0.25,btVector3(0,1,1)); drawVertex(idraw, a1, 0.25, btVector3(0, 1, 1));
} }
break; break;
case btSoftBody::Joint::eType::Angular: case btSoftBody::Joint::eType::Angular:
{ {
//const btSoftBody::AJoint* pja=(const btSoftBody::AJoint*)pj; //const btSoftBody::AJoint* pja=(const btSoftBody::AJoint*)pj;
const btVector3 o0=pj->m_bodies[0].xform().getOrigin(); const btVector3 o0 = pj->m_bodies[0].xform().getOrigin();
const btVector3 o1=pj->m_bodies[1].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 a0 = pj->m_bodies[0].xform().getBasis() * pj->m_refs[0];
const btVector3 a1=pj->m_bodies[1].xform().getBasis()*pj->m_refs[1]; 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 + a0 * 10, btVector3(1, 1, 0));
idraw->drawLine(o0,o0+a1*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 + a0 * 10, btVector3(0, 1, 1));
idraw->drawLine(o1,o1+a1*10,btVector3(0,1,1)); idraw->drawLine(o1, o1 + a1 * 10, btVector3(0, 1, 1));
break; break;
} }
default: default:
{ {
} }
} }
} }
} }
} }
// //
void btSoftBodyHelpers::DrawInfos( btSoftBody* psb, void btSoftBodyHelpers::DrawInfos(btSoftBody* psb,
btIDebugDraw* idraw, btIDebugDraw* idraw,
bool masses, bool masses,
bool areas, bool areas,
bool /*stress*/) 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]; const btSoftBody::Node& n = psb->m_nodes[i];
char text[2048]={0}; char text[2048] = {0};
char buff[1024]; char buff[1024];
if(masses) if (masses)
{ {
sprintf(buff," M(%.2f)",1/n.m_im); sprintf(buff, " M(%.2f)", 1 / n.m_im);
strcat(text,buff); strcat(text, buff);
} }
if(areas) if (areas)
{ {
sprintf(buff," A(%.2f)",n.m_area); sprintf(buff, " A(%.2f)", n.m_area);
strcat(text,buff); 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, void btSoftBodyHelpers::DrawNodeTree(btSoftBody* psb,
btIDebugDraw* idraw, btIDebugDraw* idraw,
int mindepth, int mindepth,
int maxdepth) 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, void btSoftBodyHelpers::DrawFaceTree(btSoftBody* psb,
btIDebugDraw* idraw, btIDebugDraw* idraw,
int mindepth, int mindepth,
int maxdepth) 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, void btSoftBodyHelpers::DrawClusterTree(btSoftBody* psb,
btIDebugDraw* idraw, btIDebugDraw* idraw,
int mindepth, int mindepth,
int maxdepth) 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 //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], // 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 //and then to the rest of the nodes after the execution of the Floyd-Warshall graph algorithm
//[another 930 links]. //[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 //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. // - 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. //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 //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 //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, //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. //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 //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 //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. //- 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, //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, //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 //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 //risking a dependency violation. Such a reordering would result in significant speedups on
//modern CPUs with lots of execution resources. //modern CPUs with lots of execution resources.
//In our testing, we see it have a tremendous impact not only on the A7, //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. //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 //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, //btSoftBody object in the solveConstraints() function before the actual solver is invoked,
//or right after generateBendingConstraints() once we have all 1410 links. //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 // to maximize the distance between calculation
// of dependent links. This increases the amount of parallelism that can // of dependent links. This increases the amount of parallelism that can
// be exploited by out-of-order instruction processors with large but // be exploited by out-of-order instruction processors with large but
// (inevitably) finite instruction windows. // (inevitably) finite instruction windows.
// //
//=================================================================== //===================================================================
// A small structure to track lists of dependent link calculations // A small structure to track lists of dependent link calculations
class LinkDeps_t { class LinkDeps_t
{
public: public:
int value; // A link calculation that is dependent on this one int value; // A link calculation that is dependent on this one
// Positive values = "input A" while negative values = "input B" // Positive values = "input A" while negative values = "input B"
LinkDeps_t *next; // Next dependence in the list LinkDeps_t* next; // Next dependence in the list
}; };
typedef LinkDeps_t *LinkDepsPtr_t; typedef LinkDeps_t* LinkDepsPtr_t;
// Dependency list constants // Dependency list constants
#define REOP_NOT_DEPENDENT -1 #define REOP_NOT_DEPENDENT -1
#define REOP_NODE_COMPLETE -2 // Must be less than REOP_NOT_DEPENDENT #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(); int i, nLinks = psb->m_links.size(), nNodes = psb->m_nodes.size();
btSoftBody::Link *lr; btSoftBody::Link* lr;
int ar, br; int ar, br;
btSoftBody::Node *node0 = &(psb->m_nodes[0]); btSoftBody::Node* node0 = &(psb->m_nodes[0]);
btSoftBody::Node *node1 = &(psb->m_nodes[1]); btSoftBody::Node* node1 = &(psb->m_nodes[1]);
LinkDepsPtr_t linkDep; LinkDepsPtr_t linkDep;
int readyListHead, readyListTail, linkNum, linkDepFrees, depLink; int readyListHead, readyListTail, linkNum, linkDepFrees, depLink;
// Allocate temporary buffers // Allocate temporary buffers
int *nodeWrittenAt = new int[nNodes+1]; // What link calculation produced this node's current values? 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* linkDepA = new int[nLinks]; // Link calculation input is dependent upon prior calculation #N
int *linkDepB = new int[nLinks]; int* linkDepB = new int[nLinks];
int *readyList = new int[nLinks]; // List of ready-to-process link calculations (# of links, maximum) 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) 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 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 // Copy the original, unsorted links to a side buffer
btSoftBody::Link *linkBuffer = new btSoftBody::Link[nLinks]; btSoftBody::Link* linkBuffer = new btSoftBody::Link[nLinks];
memcpy(linkBuffer, &(psb->m_links[0]), sizeof(btSoftBody::Link)*nLinks); memcpy(linkBuffer, &(psb->m_links[0]), sizeof(btSoftBody::Link) * nLinks);
// Clear out the node setup and ready list // 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; nodeWrittenAt[i] = REOP_NOT_DEPENDENT;
} }
for (i=0; i < nLinks; i++) { for (i = 0; i < nLinks; i++)
{
linkDepListStarts[i] = NULL; linkDepListStarts[i] = NULL;
} }
readyListHead = readyListTail = linkDepFrees = 0; readyListHead = readyListTail = linkDepFrees = 0;
// Initial link analysis to set up data structures // 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 // Note which prior link calculations we are dependent upon & build up dependence lists
lr = &(psb->m_links[i]); lr = &(psb->m_links[i]);
ar = (lr->m_n[0] - node0)/(node1 - node0); ar = (lr->m_n[0] - node0) / (node1 - node0);
br = (lr->m_n[1] - node0)/(node1 - node0); br = (lr->m_n[1] - node0) / (node1 - node0);
if (nodeWrittenAt[ar] > REOP_NOT_DEPENDENT) { if (nodeWrittenAt[ar] > REOP_NOT_DEPENDENT)
{
linkDepA[i] = nodeWrittenAt[ar]; linkDepA[i] = nodeWrittenAt[ar];
linkDep = &linkDepFreeList[linkDepFrees++]; linkDep = &linkDepFreeList[linkDepFrees++];
linkDep->value = i; linkDep->value = i;
linkDep->next = linkDepListStarts[nodeWrittenAt[ar]]; linkDep->next = linkDepListStarts[nodeWrittenAt[ar]];
linkDepListStarts[nodeWrittenAt[ar]] = linkDep; linkDepListStarts[nodeWrittenAt[ar]] = linkDep;
} else { }
else
{
linkDepA[i] = REOP_NOT_DEPENDENT; linkDepA[i] = REOP_NOT_DEPENDENT;
} }
if (nodeWrittenAt[br] > REOP_NOT_DEPENDENT) { if (nodeWrittenAt[br] > REOP_NOT_DEPENDENT)
{
linkDepB[i] = nodeWrittenAt[br]; linkDepB[i] = nodeWrittenAt[br];
linkDep = &linkDepFreeList[linkDepFrees++]; linkDep = &linkDepFreeList[linkDepFrees++];
linkDep->value = -(i+1); linkDep->value = -(i + 1);
linkDep->next = linkDepListStarts[nodeWrittenAt[br]]; linkDep->next = linkDepListStarts[nodeWrittenAt[br]];
linkDepListStarts[nodeWrittenAt[br]] = linkDep; linkDepListStarts[nodeWrittenAt[br]] = linkDep;
} else { }
else
{
linkDepB[i] = REOP_NOT_DEPENDENT; linkDepB[i] = REOP_NOT_DEPENDENT;
} }
// Add this link to the initial ready list, if it is not dependent on any other links // 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; 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 // Update the nodes to mark which ones are calculated by this link
nodeWrittenAt[ar] = nodeWrittenAt[br] = i; nodeWrittenAt[ar] = nodeWrittenAt[br] = i;
} }
// Process the ready list and create the sorted list of links // 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 // -- By treating the ready list as a queue, we maximize the distance between any
// inter-dependent node calculations // inter-dependent node calculations
// -- All other (non-related) nodes in the ready list will automatically be inserted // -- 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 // in between each set of inter-dependent link calculations by this loop
i = 0; i = 0;
while (readyListHead != readyListTail) { while (readyListHead != readyListTail)
{
// Use ready list to select the next link to process // Use ready list to select the next link to process
linkNum = readyList[readyListHead++]; linkNum = readyList[readyListHead++];
// Copy the next-to-calculate link back into the original link array // Copy the next-to-calculate link back into the original link array
psb->m_links[i++] = linkBuffer[linkNum]; psb->m_links[i++] = linkBuffer[linkNum];
// Free up any link inputs that are dependent on this one // Free up any link inputs that are dependent on this one
linkDep = linkDepListStarts[linkNum]; linkDep = linkDepListStarts[linkNum];
while (linkDep) { while (linkDep)
{
depLink = linkDep->value; depLink = linkDep->value;
if (depLink >= 0) { if (depLink >= 0)
{
linkDepA[depLink] = REOP_NOT_DEPENDENT; linkDepA[depLink] = REOP_NOT_DEPENDENT;
} else { }
else
{
depLink = -depLink - 1; depLink = -depLink - 1;
linkDepB[depLink] = REOP_NOT_DEPENDENT; linkDepB[depLink] = REOP_NOT_DEPENDENT;
} }
// Add this dependent link calculation to the ready list if *both* inputs are clear // 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; 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; linkDep = linkDep->next;
} }
} }
// Delete the temporary buffers // Delete the temporary buffers
delete [] nodeWrittenAt; delete[] nodeWrittenAt;
delete [] linkDepA; delete[] linkDepA;
delete [] linkDepB; delete[] linkDepB;
delete [] readyList; delete[] readyList;
delete [] linkDepFreeList; delete[] linkDepFreeList;
delete [] linkDepListStarts; delete[] linkDepListStarts;
delete [] linkBuffer; delete[] linkBuffer;
} }
// //
void btSoftBodyHelpers::DrawFrame( btSoftBody* psb, void btSoftBodyHelpers::DrawFrame(btSoftBody* psb,
btIDebugDraw* idraw) btIDebugDraw* idraw)
{ {
if(psb->m_pose.m_bframe) if (psb->m_pose.m_bframe)
{ {
static const btScalar ascl=10; static const btScalar ascl = 10;
static const btScalar nscl=(btScalar)0.1; static const btScalar nscl = (btScalar)0.1;
const btVector3 com=psb->m_pose.m_com; const btVector3 com = psb->m_pose.m_com;
const btMatrix3x3 trs=psb->m_pose.m_rot*psb->m_pose.m_scl; const btMatrix3x3 trs = psb->m_pose.m_rot * psb->m_pose.m_scl;
const btVector3 Xaxis=(trs*btVector3(1,0,0)).normalized(); const btVector3 Xaxis = (trs * btVector3(1, 0, 0)).normalized();
const btVector3 Yaxis=(trs*btVector3(0,1,0)).normalized(); const btVector3 Yaxis = (trs * btVector3(0, 1, 0)).normalized();
const btVector3 Zaxis=(trs*btVector3(0,0,1)).normalized(); const btVector3 Zaxis = (trs * btVector3(0, 0, 1)).normalized();
idraw->drawLine(com,com+Xaxis*ascl,btVector3(1,0,0)); idraw->drawLine(com, com + Xaxis * ascl, btVector3(1, 0, 0));
idraw->drawLine(com,com+Yaxis*ascl,btVector3(0,1,0)); idraw->drawLine(com, com + Yaxis * ascl, btVector3(0, 1, 0));
idraw->drawLine(com,com+Zaxis*ascl,btVector3(0,0,1)); idraw->drawLine(com, com + Zaxis * ascl, btVector3(0, 0, 1));
for(int i=0;i<psb->m_pose.m_pos.size();++i) for (int i = 0; i < psb->m_pose.m_pos.size(); ++i)
{ {
const btVector3 x=com+trs*psb->m_pose.m_pos[i]; const btVector3 x = com + trs * psb->m_pose.m_pos[i];
drawVertex(idraw,x,nscl,btVector3(1,0,1)); drawVertex(idraw, x, nscl, btVector3(1, 0, 1));
} }
} }
} }
// //
btSoftBody* btSoftBodyHelpers::CreateRope( btSoftBodyWorldInfo& worldInfo, const btVector3& from, btSoftBody* btSoftBodyHelpers::CreateRope(btSoftBodyWorldInfo& worldInfo, const btVector3& from,
const btVector3& to, const btVector3& to,
int res, int res,
int fixeds) int fixeds)
{ {
/* Create nodes */ /* Create nodes */
const int r=res+2; const int r = res + 2;
btVector3* x=new btVector3[r]; btVector3* x = new btVector3[r];
btScalar* m=new btScalar[r]; btScalar* m = new btScalar[r];
int i; int i;
for(i=0;i<r;++i) for (i = 0; i < r; ++i)
{ {
const btScalar t=i/(btScalar)(r-1); const btScalar t = i / (btScalar)(r - 1);
x[i]=lerp(from,to,t); x[i] = lerp(from, to, t);
m[i]=1; m[i] = 1;
} }
btSoftBody* psb= new btSoftBody(&worldInfo,r,x,m); btSoftBody* psb = new btSoftBody(&worldInfo, r, x, m);
if(fixeds&1) psb->setMass(0,0); if (fixeds & 1) psb->setMass(0, 0);
if(fixeds&2) psb->setMass(r-1,0); if (fixeds & 2) psb->setMass(r - 1, 0);
delete[] x; delete[] x;
delete[] m; delete[] m;
/* Create links */ /* Create links */
for(i=1;i<r;++i) for (i = 1; i < r; ++i)
{ {
psb->appendLink(i-1,i); psb->appendLink(i - 1, i);
} }
/* Finished */ /* Finished */
return(psb); return (psb);
} }
// //
btSoftBody* btSoftBodyHelpers::CreatePatch(btSoftBodyWorldInfo& worldInfo,const btVector3& corner00, btSoftBody* btSoftBodyHelpers::CreatePatch(btSoftBodyWorldInfo& worldInfo, const btVector3& corner00,
const btVector3& corner10, const btVector3& corner10,
const btVector3& corner01, const btVector3& corner01,
const btVector3& corner11, const btVector3& corner11,
int resx, int resx,
int resy, int resy,
int fixeds, int fixeds,
bool gendiags) bool gendiags,
btScalar perturbation)
{ {
#define IDX(_x_,_y_) ((_y_)*rx+(_x_)) #define IDX(_x_, _y_) ((_y_)*rx + (_x_))
/* Create nodes */ /* Create nodes */
if((resx<2)||(resy<2)) return(0); if ((resx < 2) || (resy < 2)) return (0);
const int rx=resx; const int rx = resx;
const int ry=resy; const int ry = resy;
const int tot=rx*ry; const int tot = rx * ry;
btVector3* x=new btVector3[tot]; btVector3* x = new btVector3[tot];
btScalar* m=new btScalar[tot]; btScalar* m = new btScalar[tot];
int iy; int iy;
for(iy=0;iy<ry;++iy) for (iy = 0; iy < ry; ++iy)
{ {
const btScalar ty=iy/(btScalar)(ry-1); const btScalar ty = iy / (btScalar)(ry - 1);
const btVector3 py0=lerp(corner00,corner01,ty); const btVector3 py0 = lerp(corner00, corner01, ty);
const btVector3 py1=lerp(corner10,corner11,ty); const btVector3 py1 = lerp(corner10, corner11, ty);
for(int ix=0;ix<rx;++ix) for (int ix = 0; ix < rx; ++ix)
{ {
const btScalar tx=ix/(btScalar)(rx-1); const btScalar tx = ix / (btScalar)(rx - 1);
x[IDX(ix,iy)]=lerp(py0,py1,tx); 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; m[IDX(ix, iy)] = 1;
} }
} }
btSoftBody* psb=new btSoftBody(&worldInfo,tot,x,m); btSoftBody* psb = new btSoftBody(&worldInfo, tot, x, m);
if(fixeds&1) psb->setMass(IDX(0,0),0); if (fixeds & 1) psb->setMass(IDX(0, 0), 0);
if(fixeds&2) psb->setMass(IDX(rx-1,0),0); if (fixeds & 2) psb->setMass(IDX(rx - 1, 0), 0);
if(fixeds&4) psb->setMass(IDX(0,ry-1),0); if (fixeds & 4) psb->setMass(IDX(0, ry - 1), 0);
if(fixeds&8) psb->setMass(IDX(rx-1,ry-1),0); if (fixeds & 8) psb->setMass(IDX(rx - 1, ry - 1), 0);
delete[] x; delete[] x;
delete[] m; delete[] m;
/* Create links and faces */ /* Create links and faces */
for(iy=0;iy<ry;++iy) 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 int idx = IDX(ix, iy);
const bool mdx=(ix+1)<rx; const bool mdx = (ix + 1) < rx;
const bool mdy=(iy+1)<ry; const bool mdy = (iy + 1) < ry;
if(mdx) psb->appendLink(idx,IDX(ix+1,iy)); if (mdx) psb->appendLink(idx, IDX(ix + 1, iy));
if(mdy) psb->appendLink(idx,IDX(ix,iy+1)); if (mdy) psb->appendLink(idx, IDX(ix, iy + 1));
if(mdx&&mdy) 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), IDX(ix + 1, iy + 1));
psb->appendFace(IDX(ix,iy),IDX(ix+1,iy+1),IDX(ix,iy+1)); psb->appendFace(IDX(ix, iy), IDX(ix + 1, iy + 1), IDX(ix, iy + 1));
if(gendiags) if (gendiags)
{ {
psb->appendLink(IDX(ix,iy),IDX(ix+1,iy+1)); psb->appendLink(IDX(ix, iy), IDX(ix + 1, iy + 1));
} }
} }
else else
{ {
psb->appendFace(IDX(ix,iy+1),IDX(ix,iy),IDX(ix+1,iy)); 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)); psb->appendFace(IDX(ix, iy + 1), IDX(ix + 1, iy), IDX(ix + 1, iy + 1));
if(gendiags) 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 #undef IDX
return(psb); return (psb);
} }
// //
btSoftBody* btSoftBodyHelpers::CreatePatchUV(btSoftBodyWorldInfo& worldInfo, btSoftBody* btSoftBodyHelpers::CreatePatchUV(btSoftBodyWorldInfo& worldInfo,
const btVector3& corner00, const btVector3& corner00,
const btVector3& corner10, const btVector3& corner10,
const btVector3& corner01, const btVector3& corner01,
const btVector3& corner11, const btVector3& corner11,
int resx, int resx,
int resy, int resy,
int fixeds, int fixeds,
bool gendiags, bool gendiags,
float* tex_coords) float* tex_coords)
{ {
/* /*
* *
* corners: * corners:
* *
* [0][0] corner00 ------- corner01 [resx][0] * [0][0] corner00 ------- corner01 [resx][0]
* | | * | |
* | | * | |
* [0][resy] corner10 -------- corner11 [resx][resy] * [0][resy] corner10 -------- corner11 [resx][resy]
▲ Show 20 Lines • Show All 51 Lines • ▼ Show 20 LinesbtSoftBody* btSoftBodyHelpers::CreatePatchUV(btSoftBodyWorldInfo& worldInfo,
* *
* *
* *
* *
* *
* *
*/ */
#define IDX(_x_,_y_) ((_y_)*rx+(_x_)) #define IDX(_x_, _y_) ((_y_)*rx + (_x_))
/* Create nodes */ /* Create nodes */
if((resx<2)||(resy<2)) return(0); if ((resx < 2) || (resy < 2)) return (0);
const int rx=resx; const int rx = resx;
const int ry=resy; const int ry = resy;
const int tot=rx*ry; const int tot = rx * ry;
btVector3* x=new btVector3[tot]; btVector3* x = new btVector3[tot];
btScalar* m=new btScalar[tot]; btScalar* m = new btScalar[tot];
int iy; int iy;
for(iy=0;iy<ry;++iy) for (iy = 0; iy < ry; ++iy)
{ {
const btScalar ty=iy/(btScalar)(ry-1); const btScalar ty = iy / (btScalar)(ry - 1);
const btVector3 py0=lerp(corner00,corner01,ty); const btVector3 py0 = lerp(corner00, corner01, ty);
const btVector3 py1=lerp(corner10,corner11,ty); const btVector3 py1 = lerp(corner10, corner11, ty);
for(int ix=0;ix<rx;++ix) for (int ix = 0; ix < rx; ++ix)
{ {
const btScalar tx=ix/(btScalar)(rx-1); const btScalar tx = ix / (btScalar)(rx - 1);
x[IDX(ix,iy)]=lerp(py0,py1,tx); x[IDX(ix, iy)] = lerp(py0, py1, tx);
m[IDX(ix,iy)]=1; m[IDX(ix, iy)] = 1;
} }
} }
btSoftBody* psb=new btSoftBody(&worldInfo,tot,x,m); btSoftBody* psb = new btSoftBody(&worldInfo, tot, x, m);
if(fixeds&1) psb->setMass(IDX(0,0),0); if (fixeds & 1) psb->setMass(IDX(0, 0), 0);
if(fixeds&2) psb->setMass(IDX(rx-1,0),0); if (fixeds & 2) psb->setMass(IDX(rx - 1, 0), 0);
if(fixeds&4) psb->setMass(IDX(0,ry-1),0); if (fixeds & 4) psb->setMass(IDX(0, ry - 1), 0);
if(fixeds&8) psb->setMass(IDX(rx-1,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 & 16) psb->setMass(IDX((rx - 1) / 2, 0), 0);
if(fixeds&32) psb->setMass(IDX(0,(ry-1)/2),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 & 64) psb->setMass(IDX(rx - 1, (ry - 1) / 2), 0);
if(fixeds&128) psb->setMass(IDX((rx-1)/2,ry-1),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); if (fixeds & 256) psb->setMass(IDX((rx - 1) / 2, (ry - 1) / 2), 0);
delete[] x; delete[] x;
delete[] m; delete[] m;
int z = 0; int z = 0;
/* Create links and faces */ /* Create links and faces */
for(iy=0;iy<ry;++iy) 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 mdx = (ix + 1) < rx;
const bool mdy=(iy+1)<ry; const bool mdy = (iy + 1) < ry;
int node00=IDX(ix,iy); int node00 = IDX(ix, iy);
int node01=IDX(ix+1,iy); int node01 = IDX(ix + 1, iy);
int node10=IDX(ix,iy+1); int node10 = IDX(ix, iy + 1);
int node11=IDX(ix+1,iy+1); int node11 = IDX(ix + 1, iy + 1);
if(mdx) psb->appendLink(node00,node01); if (mdx) psb->appendLink(node00, node01);
if(mdy) psb->appendLink(node00,node10); if (mdy) psb->appendLink(node00, node10);
if(mdx&&mdy) if (mdx && mdy)
{ {
psb->appendFace(node00,node10,node11); psb->appendFace(node00, node10, node11);
if (tex_coords) { if (tex_coords)
{
tex_coords[z+0]=CalculateUV(resx,resy,ix,iy,0); tex_coords[z + 0] = CalculateUV(resx, resy, ix, iy, 0);
tex_coords[z+1]=CalculateUV(resx,resy,ix,iy,1); tex_coords[z + 1] = CalculateUV(resx, resy, ix, iy, 1);
tex_coords[z+2]=CalculateUV(resx,resy,ix,iy,0); tex_coords[z + 2] = CalculateUV(resx, resy, ix, iy, 0);
tex_coords[z+3]=CalculateUV(resx,resy,ix,iy,2); tex_coords[z + 3] = CalculateUV(resx, resy, ix, iy, 2);
tex_coords[z+4]=CalculateUV(resx,resy,ix,iy,3); tex_coords[z + 4] = CalculateUV(resx, resy, ix, iy, 3);
tex_coords[z+5]=CalculateUV(resx,resy,ix,iy,2); tex_coords[z + 5] = CalculateUV(resx, resy, ix, iy, 2);
} }
psb->appendFace(node11,node01,node00); psb->appendFace(node11, node01, node00);
if (tex_coords) { if (tex_coords)
{
tex_coords[z+6 ]=CalculateUV(resx,resy,ix,iy,3); tex_coords[z + 6] = CalculateUV(resx, resy, ix, iy, 3);
tex_coords[z+7 ]=CalculateUV(resx,resy,ix,iy,2); tex_coords[z + 7] = CalculateUV(resx, resy, ix, iy, 2);
tex_coords[z+8 ]=CalculateUV(resx,resy,ix,iy,3); tex_coords[z + 8] = CalculateUV(resx, resy, ix, iy, 3);
tex_coords[z+9 ]=CalculateUV(resx,resy,ix,iy,1); tex_coords[z + 9] = CalculateUV(resx, resy, ix, iy, 1);
tex_coords[z+10]=CalculateUV(resx,resy,ix,iy,0); tex_coords[z + 10] = CalculateUV(resx, resy, ix, iy, 0);
tex_coords[z+11]=CalculateUV(resx,resy,ix,iy,1); tex_coords[z + 11] = CalculateUV(resx, resy, ix, iy, 1);
} }
if (gendiags) psb->appendLink(node00,node11); if (gendiags) psb->appendLink(node00, node11);
z += 12; z += 12;
} }
} }
} }
/* Finished */ /* Finished */
#undef IDX #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)
{ {
/* /*
* *
* *
* node00 --- node01 * node00 --- node01
* | | * | |
* node10 --- node11 * node10 --- node11
* *
* *
Show All 9 Linesfloat btSoftBodyHelpers::CalculateUV(int resx, int resy, int ix, int iy, int id)
* node10 t --> 2 * node10 t --> 2
* *
* node11 s --> 3 * node11 s --> 3
* node11 t --> 2 * node11 t --> 2
* *
* *
*/ */
float tc=0.0f; float tc = 0.0f;
if (id == 0) { if (id == 0)
{
tc = (1.0f/((resx-1))*ix); tc = (1.0f / ((resx - 1)) * ix);
} }
else if (id==1) { else if (id == 1)
{
tc = (1.0f/((resy-1))*(resy-1-iy)); tc = (1.0f / ((resy - 1)) * (resy - 1 - iy));
} }
else if (id==2) { else if (id == 2)
{
tc = (1.0f/((resy-1))*(resy-1-iy-1)); tc = (1.0f / ((resy - 1)) * (resy - 1 - iy - 1));
} }
else if (id==3) { else if (id == 3)
{
tc = (1.0f/((resx-1))*(ix+1)); tc = (1.0f / ((resx - 1)) * (ix + 1));
} }
return tc; return tc;
} }
// //
btSoftBody* btSoftBodyHelpers::CreateEllipsoid(btSoftBodyWorldInfo& worldInfo,const btVector3& center, btSoftBody* btSoftBodyHelpers::CreateEllipsoid(btSoftBodyWorldInfo& worldInfo, const btVector3& center,
const btVector3& radius, const btVector3& radius,
int res) 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; btScalar p = 0.5, t = 0;
for(int j=i;j;p*=0.5,j>>=1) if(j&1) t+=p; for (int j = i; j; p *= 0.5, j >>= 1)
if (j & 1) t += p;
btScalar w=2*t-1; btScalar w = 2 * t - 1;
btScalar a=(SIMD_PI+2*i*SIMD_PI)/n; btScalar a = (SIMD_PI + 2 * i * SIMD_PI) / n;
btScalar s=btSqrt(1-w*w); btScalar s = btSqrt(1 - w * w);
*x++=btVector3(s*btCos(a),s*btSin(a),w); *x++ = btVector3(s * btCos(a), s * btSin(a), w);
} }
} }
}; };
btAlignedObjectArray<btVector3> vtx; btAlignedObjectArray<btVector3> vtx;
vtx.resize(3+res); vtx.resize(3 + res);
Hammersley::Generate(&vtx[0],vtx.size()); Hammersley::Generate(&vtx[0], vtx.size());
for(int i=0;i<vtx.size();++i) 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, btSoftBody* btSoftBodyHelpers::CreateFromTriMesh(btSoftBodyWorldInfo& worldInfo, const btScalar* vertices,
const int* triangles, const int* triangles,
int ntriangles, bool randomizeConstraints) int ntriangles, bool randomizeConstraints)
{ {
int maxidx=0; int maxidx = 0;
int i,j,ni; 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; ++maxidx;
btAlignedObjectArray<bool> chks; btAlignedObjectArray<bool> chks;
btAlignedObjectArray<btVector3> vtx; btAlignedObjectArray<btVector3> vtx;
chks.resize(maxidx*maxidx,false); chks.resize(maxidx * maxidx, false);
vtx.resize(maxidx); 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); btSoftBody* psb = new btSoftBody(&worldInfo, vtx.size(), &vtx[0], 0);
for( i=0,ni=ntriangles*3;i<ni;i+=3) for (i = 0, ni = ntriangles * 3; i < ni; i += 3)
{ {
const int idx[]={triangles[i],triangles[i+1],triangles[i+2]}; const int idx[] = {triangles[i], triangles[i + 1], triangles[i + 2]};
#define IDX(_x_,_y_) ((_y_)*maxidx+(_x_)) #define IDX(_x_, _y_) ((_y_)*maxidx + (_x_))
for(int j=2,k=0;k<3;j=k++) 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[j], idx[k])] = true;
chks[IDX(idx[k],idx[j])]=true; chks[IDX(idx[k], idx[j])] = true;
psb->appendLink(idx[j],idx[k]); psb->appendLink(idx[j], idx[k]);
} }
} }
#undef IDX #undef IDX
psb->appendFace(idx[0],idx[1],idx[2]); psb->appendFace(idx[0], idx[1], idx[2]);
} }
if (randomizeConstraints) if (randomizeConstraints)
{ {
psb->randomizeConstraints(); psb->randomizeConstraints();
} }
return(psb); return (psb);
} }
// //
btSoftBody* btSoftBodyHelpers::CreateFromConvexHull(btSoftBodyWorldInfo& worldInfo, const btVector3* vertices, btSoftBody* btSoftBodyHelpers::CreateFromConvexHull(btSoftBodyWorldInfo& worldInfo, const btVector3* vertices,
int nvertices, bool randomizeConstraints) int nvertices, bool randomizeConstraints)
{ {
HullDesc hdsc(QF_TRIANGLES,nvertices,vertices); HullDesc hdsc(QF_TRIANGLES, nvertices, vertices);
HullResult hres; HullResult hres;
HullLibrary hlib;/*??*/ HullLibrary hlib; /*??*/
hdsc.mMaxVertices=nvertices; hdsc.mMaxVertices = nvertices;
hlib.CreateConvexHull(hdsc,hres); hlib.CreateConvexHull(hdsc, hres);
btSoftBody* psb=new btSoftBody(&worldInfo,(int)hres.mNumOutputVertices, btSoftBody* psb = new btSoftBody(&worldInfo, (int)hres.mNumOutputVertices,
&hres.m_OutputVertices[0],0); &hres.m_OutputVertices[0], 0);
for(int i=0;i<(int)hres.mNumFaces;++i) for (int i = 0; i < (int)hres.mNumFaces; ++i)
{ {
const int idx[]={ static_cast<int>(hres.m_Indices[i*3+0]), 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 + 1]),
static_cast<int>(hres.m_Indices[i*3+2])}; static_cast<int>(hres.m_Indices[i * 3 + 2])};
if(idx[0]<idx[1]) psb->appendLink( idx[0],idx[1]); if (idx[0] < idx[1]) psb->appendLink(idx[0], idx[1]);
if(idx[1]<idx[2]) psb->appendLink( idx[1],idx[2]); if (idx[1] < idx[2]) psb->appendLink(idx[1], idx[2]);
if(idx[2]<idx[0]) psb->appendLink( idx[2],idx[0]); if (idx[2] < idx[0]) psb->appendLink(idx[2], idx[0]);
psb->appendFace(idx[0],idx[1],idx[2]); psb->appendFace(idx[0], idx[1], idx[2]);
} }
hlib.ReleaseResult(hres); hlib.ReleaseResult(hres);
if (randomizeConstraints) if (randomizeConstraints)
{ {
psb->randomizeConstraints(); psb->randomizeConstraints();
} }
return(psb); return (psb);
} }
static int nextLine(const char* buffer) static int nextLine(const char* buffer)
{ {
int numBytesRead=0; int numBytesRead = 0;
while (*buffer != '\n') while (*buffer != '\n')
{ {
buffer++; buffer++;
numBytesRead++; numBytesRead++;
} }
if (buffer[0]==0x0a) if (buffer[0] == 0x0a)
{ {
buffer++; buffer++;
numBytesRead++; numBytesRead++;
} }
return numBytesRead; return numBytesRead;
} }
/* Create from TetGen .ele, .face, .node data */ /* Create from TetGen .ele, .face, .node data */
btSoftBody* btSoftBodyHelpers::CreateFromTetGenData(btSoftBodyWorldInfo& worldInfo, btSoftBody* btSoftBodyHelpers::CreateFromTetGenData(btSoftBodyWorldInfo& worldInfo,
const char* ele, const char* ele,
const char* face, const char* face,
const char* node, const char* node,
bool bfacelinks, bool bfacelinks,
bool btetralinks, bool btetralinks,
bool bfacesfromtetras) bool bfacesfromtetras)
{ {
btAlignedObjectArray<btVector3> pos; btAlignedObjectArray<btVector3> pos;
int nnode=0; int nnode = 0;
int ndims=0; int ndims = 0;
int nattrb=0; int nattrb = 0;
int hasbounds=0; int hasbounds = 0;
int result = sscanf(node,"%d %d %d %d",&nnode,&ndims,&nattrb,&hasbounds); int result = sscanf(node, "%d %d %d %d", &nnode, &ndims, &nattrb, &hasbounds);
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); node += nextLine(node);
pos.resize(nnode); pos.resize(nnode);
for(int i=0;i<pos.size();++i) for (int i = 0; i < pos.size(); ++i)
{ {
int index=0; int index = 0;
//int bound=0; //int bound=0;
float x,y,z; float x, y, z;
sscanf(node,"%d %f %f %f",&index,&x,&y,&z); sscanf(node, "%d %f %f %f", &index, &x, &y, &z);
// sn>>index; // sn>>index;
// sn>>x;sn>>y;sn>>z; // sn>>x;sn>>y;sn>>z;
node += nextLine(node); node += nextLine(node);
//for(int j=0;j<nattrb;++j) //for(int j=0;j<nattrb;++j)
// sn>>a; // sn>>a;
//if(hasbounds) //if(hasbounds)
// sn>>bound; // sn>>bound;
pos[index].setX(btScalar(x)); pos[index].setX(btScalar(x));
pos[index].setY(btScalar(y)); pos[index].setY(btScalar(y));
pos[index].setZ(btScalar(z)); 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 0
if(face&&face[0]) if(face&&face[0])
{ {
int nface=0; int nface=0;
sf>>nface;sf>>hasbounds; sf>>nface;sf>>hasbounds;
for(int i=0;i<nface;++i) for(int i=0;i<nface;++i)
{ {
int index=0; int index=0;
int bound=0; int bound=0;
int ni[3]; int ni[3];
sf>>index; sf>>index;
sf>>ni[0];sf>>ni[1];sf>>ni[2]; sf>>ni[0];sf>>ni[1];sf>>ni[2];
sf>>bound; sf>>bound;
psb->appendFace(ni[0],ni[1],ni[2]); psb->appendFace(ni[0],ni[1],ni[2]);
if(btetralinks) if(btetralinks)
{ {
psb->appendLink(ni[0],ni[1],0,true); psb->appendLink(ni[0],ni[1],0,true);
psb->appendLink(ni[1],ni[2],0,true); psb->appendLink(ni[1],ni[2],0,true);
psb->appendLink(ni[2],ni[0],0,true); psb->appendLink(ni[2],ni[0],0,true);
} }
} }
} }
#endif #endif
if(ele&&ele[0]) if (ele && ele[0])
{ {
int ntetra=0; int ntetra = 0;
int ncorner=0; int ncorner = 0;
int neattrb=0; int neattrb = 0;
sscanf(ele,"%d %d %d",&ntetra,&ncorner,&neattrb); sscanf(ele, "%d %d %d", &ntetra, &ncorner, &neattrb);
ele += nextLine(ele); ele += nextLine(ele);
//se>>ntetra;se>>ncorner;se>>neattrb; //se>>ntetra;se>>ncorner;se>>neattrb;
for(int i=0;i<ntetra;++i) for (int i = 0; i < ntetra; ++i)
{ {
int index=0; int index = 0;
int ni[4]; int ni[4];
//se>>index; //se>>index;
//se>>ni[0];se>>ni[1];se>>ni[2];se>>ni[3]; //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]); sscanf(ele, "%d %d %d %d %d", &index, &ni[0], &ni[1], &ni[2], &ni[3]);
ele+=nextLine(ele); ele += nextLine(ele);
//for(int j=0;j<neattrb;++j) //for(int j=0;j<neattrb;++j)
// se>>a; // se>>a;
psb->appendTetra(ni[0],ni[1],ni[2],ni[3]); psb->appendTetra(ni[0], ni[1], ni[2], ni[3]);
if(btetralinks) if (btetralinks)
{ {
psb->appendLink(ni[0],ni[1],0,true); psb->appendLink(ni[0], ni[1], 0, true);
psb->appendLink(ni[1],ni[2],0,true); psb->appendLink(ni[1], ni[2], 0, true);
psb->appendLink(ni[2],ni[0],0,true); psb->appendLink(ni[2], ni[0], 0, true);
psb->appendLink(ni[0],ni[3],0,true); psb->appendLink(ni[0], ni[3], 0, true);
psb->appendLink(ni[1],ni[3],0,true); psb->appendLink(ni[1], ni[3], 0, true);
psb->appendLink(ni[2],ni[3],0,true); psb->appendLink(ni[2], ni[3], 0, true);
} }
} }
} }
printf("Nodes: %d\r\n",psb->m_nodes.size()); psb->initializeDmInverse();
printf("Links: %d\r\n",psb->m_links.size()); psb->m_tetraScratches.resize(psb->m_tetras.size());
printf("Faces: %d\r\n",psb->m_faces.size()); psb->m_tetraScratchesTn.resize(psb->m_tetras.size());
printf("Tetras: %d\r\n",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); 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);
}
}
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;
}
}