bullet3

/*
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>
#include <iostream>
#include <sstream>
#include <string.h>
#include <algorithm>
#include "btSoftBodyHelpers.h"
#include "LinearMath/btConvexHull.h"
#include "LinearMath/btConvexHullComputer.h"
#include <map>
#include <vector>

static void drawVertex(btIDebugDraw* idraw,
					   const btVector3& x, btScalar s, const btVector3& c)
{
	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,
										   btScalar perturbation)
{
#define IDX(_x_, _y_) ((_y_)*rx + (_x_))
	/* Create nodes	*/
	if ((resx < 2) || (resy < 2)) return (0);
	const int rx = resx;
	const int ry = resy;
	const int tot = rx * ry;
	btVector3* x = new btVector3[tot];
	btScalar* m = new btScalar[tot];
	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);
			btScalar pert = perturbation * btScalar(rand()) / RAND_MAX;
			btVector3 temp1 = py1;
			temp1.setY(py1.getY() + pert);
			btVector3 temp = py0;
			pert = perturbation * btScalar(rand()) / RAND_MAX;
			temp.setY(py0.getY() + pert);
			x[IDX(ix, iy)] = lerp(temp, temp1, tx);
			m[IDX(ix, iy)] = 1;
		}
	}
	btSoftBody* psb = new btSoftBody(&worldInfo, tot, x, m);
	if (fixeds & 1) psb->setMass(IDX(0, 0), 0);
	if (fixeds & 2) psb->setMass(IDX(rx - 1, 0), 0);
	if (fixeds & 4) psb->setMass(IDX(0, ry - 1), 0);
	if (fixeds & 8) psb->setMass(IDX(rx - 1, ry - 1), 0);
	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);
			}
		}
	}
	psb->initializeDmInverse();
	psb->m_tetraScratches.resize(psb->m_tetras.size());
	psb->m_tetraScratchesTn.resize(psb->m_tetras.size());
	printf("Nodes:  %u\r\n", psb->m_nodes.size());
	printf("Links:  %u\r\n", psb->m_links.size());
	printf("Faces:  %u\r\n", psb->m_faces.size());
	printf("Tetras: %u\r\n", psb->m_tetras.size());
	return (psb);
}

btSoftBody* btSoftBodyHelpers::CreateFromVtkFile(btSoftBodyWorldInfo& worldInfo, const char* vtk_file)
{
	std::ifstream fs;
	fs.open(vtk_file);
	btAssert(fs);

	typedef btAlignedObjectArray<int> Index;
	std::string line;
	btAlignedObjectArray<btVector3> X;
	btVector3 position;
	btAlignedObjectArray<Index> indices;
	bool reading_points = false;
	bool reading_tets = false;
	size_t n_points = 0;
	size_t n_tets = 0;
	size_t x_count = 0;
	size_t indices_count = 0;
	while (std::getline(fs, line))
	{
		std::stringstream ss(line);
		if (line.size() == (size_t)(0))
		{
		}
		else if (line.substr(0, 6) == "POINTS")
		{
			reading_points = true;
			reading_tets = false;
			ss.ignore(128, ' ');  // ignore "POINTS"
			ss >> n_points;
			X.resize(n_points);
		}
		else if (line.substr(0, 5) == "CELLS")
		{
			reading_points = false;
			reading_tets = true;
			ss.ignore(128, ' ');  // ignore "CELLS"
			ss >> n_tets;
			indices.resize(n_tets);
		}
		else if (line.substr(0, 10) == "CELL_TYPES")
		{
			reading_points = false;
			reading_tets = false;
		}
		else if (reading_points)
		{
			btScalar p;
			ss >> p;
			position.setX(p);
			ss >> p;
			position.setY(p);
			ss >> p;
			position.setZ(p);
			X[x_count++] = position;
		}
		else if (reading_tets)
		{
			int d;
			ss >> d;
			if (d != 4)
			{
				printf("Load deformable failed: Only Tetrahedra are supported in VTK file.\n");
				fs.close();
				return 0;
			}
			ss.ignore(128, ' ');  // ignore "4"
			Index tet;
			tet.resize(4);
			for (size_t i = 0; i < 4; i++)
			{
				ss >> tet[i];
				printf("%d ", tet[i]);
			}
			printf("\n");
			indices[indices_count++] = tet;
		}
	}
	btSoftBody* psb = new btSoftBody(&worldInfo, n_points, &X[0], 0);

	for (int i = 0; i < n_tets; ++i)
	{
		const Index& ni = indices[i];
		psb->appendTetra(ni[0], ni[1], ni[2], ni[3]);
		{
			psb->appendLink(ni[0], ni[1], 0, true);
			psb->appendLink(ni[1], ni[2], 0, true);
			psb->appendLink(ni[2], ni[0], 0, true);
			psb->appendLink(ni[0], ni[3], 0, true);
			psb->appendLink(ni[1], ni[3], 0, true);
			psb->appendLink(ni[2], ni[3], 0, true);
		}
	}

	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;
	}
}