update bullet test to compile from source

1 files changed, 3409 insertions, 0 deletions

diff --git a/tests/bullet/src/BulletSoftBody/btSoftBody.cpp b/tests/bullet/src/BulletSoftBody/btSoftBody.cpp
new file mode 100644
index 00000000..d440c94a
--- /dev/null
+++ b/tests/bullet/src/BulletSoftBody/btSoftBody.cpp
@@ -0,0 +1,3409 @@
+/*
+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.
+*/
+///btSoftBody implementation by Nathanael Presson
+
+#include "btSoftBodyInternals.h"
+#include "BulletSoftBody/btSoftBodySolvers.h"
+#include "btSoftBodyData.h"
+#include "LinearMath/btSerializer.h"
+
+//
+btSoftBody::btSoftBody(btSoftBodyWorldInfo*	worldInfo,int node_count,  const btVector3* x,  const btScalar* m)
+:m_worldInfo(worldInfo),m_softBodySolver(0)
+{	
+	/* Init		*/ 
+	initDefaults();
+
+	/* Default material	*/ 
+	Material*	pm=appendMaterial();
+	pm->m_kLST	=	1;
+	pm->m_kAST	=	1;
+	pm->m_kVST	=	1;
+	pm->m_flags	=	fMaterial::Default;
+
+	/* Nodes			*/ 
+	const btScalar		margin=getCollisionShape()->getMargin();
+	m_nodes.resize(node_count);
+	for(int i=0,ni=node_count;i<ni;++i)
+	{	
+		Node&	n=m_nodes[i];
+		ZeroInitialize(n);
+		n.m_x		=	x?*x++:btVector3(0,0,0);
+		n.m_q		=	n.m_x;
+		n.m_im		=	m?*m++:1;
+		n.m_im		=	n.m_im>0?1/n.m_im:0;
+		n.m_leaf	=	m_ndbvt.insert(btDbvtVolume::FromCR(n.m_x,margin),&n);
+		n.m_material=	pm;
+	}
+	updateBounds();	
+
+}
+
+btSoftBody::btSoftBody(btSoftBodyWorldInfo*	worldInfo)
+:m_worldInfo(worldInfo)
+{
+	initDefaults();
+}
+
+
+void	btSoftBody::initDefaults()
+{
+	m_internalType		=	CO_SOFT_BODY;
+	m_cfg.aeromodel		=	eAeroModel::V_Point;
+	m_cfg.kVCF			=	1;
+	m_cfg.kDG			=	0;
+	m_cfg.kLF			=	0;
+	m_cfg.kDP			=	0;
+	m_cfg.kPR			=	0;
+	m_cfg.kVC			=	0;
+	m_cfg.kDF			=	(btScalar)0.2;
+	m_cfg.kMT			=	0;
+	m_cfg.kCHR			=	(btScalar)1.0;
+	m_cfg.kKHR			=	(btScalar)0.1;
+	m_cfg.kSHR			=	(btScalar)1.0;
+	m_cfg.kAHR			=	(btScalar)0.7;
+	m_cfg.kSRHR_CL		=	(btScalar)0.1;
+	m_cfg.kSKHR_CL		=	(btScalar)1;
+	m_cfg.kSSHR_CL		=	(btScalar)0.5;
+	m_cfg.kSR_SPLT_CL	=	(btScalar)0.5;
+	m_cfg.kSK_SPLT_CL	=	(btScalar)0.5;
+	m_cfg.kSS_SPLT_CL	=	(btScalar)0.5;
+	m_cfg.maxvolume		=	(btScalar)1;
+	m_cfg.timescale		=	1;
+	m_cfg.viterations	=	0;
+	m_cfg.piterations	=	1;	
+	m_cfg.diterations	=	0;
+	m_cfg.citerations	=	4;
+	m_cfg.collisions	=	fCollision::Default;
+	m_pose.m_bvolume	=	false;
+	m_pose.m_bframe		=	false;
+	m_pose.m_volume		=	0;
+	m_pose.m_com		=	btVector3(0,0,0);
+	m_pose.m_rot.setIdentity();
+	m_pose.m_scl.setIdentity();
+	m_tag				=	0;
+	m_timeacc			=	0;
+	m_bUpdateRtCst		=	true;
+	m_bounds[0]			=	btVector3(0,0,0);
+	m_bounds[1]			=	btVector3(0,0,0);
+	m_worldTransform.setIdentity();
+	setSolver(eSolverPresets::Positions);
+	
+	/* Collision shape	*/ 
+	///for now, create a collision shape internally
+	m_collisionShape = new btSoftBodyCollisionShape(this);
+	m_collisionShape->setMargin(0.25);
+	
+	m_initialWorldTransform.setIdentity();
+
+	m_windVelocity = btVector3(0,0,0);
+
+}
+
+//
+btSoftBody::~btSoftBody()
+{
+	//for now, delete the internal shape
+	delete m_collisionShape;	
+	int i;
+
+	releaseClusters();
+	for(i=0;i<m_materials.size();++i) 
+		btAlignedFree(m_materials[i]);
+	for(i=0;i<m_joints.size();++i) 
+		btAlignedFree(m_joints[i]);
+}
+
+//
+bool			btSoftBody::checkLink(int node0,int node1) const
+{
+	return(checkLink(&m_nodes[node0],&m_nodes[node1]));
+}
+
+//
+bool			btSoftBody::checkLink(const Node* node0,const Node* node1) const
+{
+	const Node*	n[]={node0,node1};
+	for(int i=0,ni=m_links.size();i<ni;++i)
+	{
+		const Link&	l=m_links[i];
+		if(	(l.m_n[0]==n[0]&&l.m_n[1]==n[1])||
+			(l.m_n[0]==n[1]&&l.m_n[1]==n[0]))
+		{
+			return(true);
+		}
+	}
+	return(false);
+}
+
+//
+bool			btSoftBody::checkFace(int node0,int node1,int node2) const
+{
+	const Node*	n[]={	&m_nodes[node0],
+		&m_nodes[node1],
+		&m_nodes[node2]};
+	for(int i=0,ni=m_faces.size();i<ni;++i)
+	{
+		const Face&	f=m_faces[i];
+		int			c=0;
+		for(int j=0;j<3;++j)
+		{
+			if(	(f.m_n[j]==n[0])||
+				(f.m_n[j]==n[1])||
+				(f.m_n[j]==n[2])) c|=1<<j; else break;
+		}
+		if(c==7) return(true);
+	}
+	return(false);
+}
+
+//
+btSoftBody::Material*		btSoftBody::appendMaterial()
+{
+	Material*	pm=new(btAlignedAlloc(sizeof(Material),16)) Material();
+	if(m_materials.size()>0)
+		*pm=*m_materials[0];
+	else
+		ZeroInitialize(*pm);
+	m_materials.push_back(pm);
+	return(pm);
+}
+
+//
+void			btSoftBody::appendNote(	const char* text,
+									   const btVector3& o,
+									   const btVector4& c,
+									   Node* n0,
+									   Node* n1,
+									   Node* n2,
+									   Node* n3)
+{
+	Note	n;
+	ZeroInitialize(n);
+	n.m_rank		=	0;
+	n.m_text		=	text;
+	n.m_offset		=	o;
+	n.m_coords[0]	=	c.x();
+	n.m_coords[1]	=	c.y();
+	n.m_coords[2]	=	c.z();
+	n.m_coords[3]	=	c.w();
+	n.m_nodes[0]	=	n0;n.m_rank+=n0?1:0;
+	n.m_nodes[1]	=	n1;n.m_rank+=n1?1:0;
+	n.m_nodes[2]	=	n2;n.m_rank+=n2?1:0;
+	n.m_nodes[3]	=	n3;n.m_rank+=n3?1:0;
+	m_notes.push_back(n);
+}
+
+//
+void			btSoftBody::appendNote(	const char* text,
+									   const btVector3& o,
+									   Node* feature)
+{
+	appendNote(text,o,btVector4(1,0,0,0),feature);
+}
+
+//
+void			btSoftBody::appendNote(	const char* text,
+									   const btVector3& o,
+									   Link* feature)
+{
+	static const btScalar	w=1/(btScalar)2;
+	appendNote(text,o,btVector4(w,w,0,0),	feature->m_n[0],
+		feature->m_n[1]);
+}
+
+//
+void			btSoftBody::appendNote(	const char* text,
+									   const btVector3& o,
+									   Face* feature)
+{
+	static const btScalar	w=1/(btScalar)3;
+	appendNote(text,o,btVector4(w,w,w,0),	feature->m_n[0],
+		feature->m_n[1],
+		feature->m_n[2]);
+}
+
+//
+void			btSoftBody::appendNode(	const btVector3& x,btScalar m)
+{
+	if(m_nodes.capacity()==m_nodes.size())
+	{
+		pointersToIndices();
+		m_nodes.reserve(m_nodes.size()*2+1);
+		indicesToPointers();
+	}
+	const btScalar	margin=getCollisionShape()->getMargin();
+	m_nodes.push_back(Node());
+	Node&			n=m_nodes[m_nodes.size()-1];
+	ZeroInitialize(n);
+	n.m_x			=	x;
+	n.m_q			=	n.m_x;
+	n.m_im			=	m>0?1/m:0;
+	n.m_material	=	m_materials[0];
+	n.m_leaf		=	m_ndbvt.insert(btDbvtVolume::FromCR(n.m_x,margin),&n);
+}
+
+//
+void			btSoftBody::appendLink(int model,Material* mat)
+{
+	Link	l;
+	if(model>=0)
+		l=m_links[model];
+	else
+	{ ZeroInitialize(l);l.m_material=mat?mat:m_materials[0]; }
+	m_links.push_back(l);
+}
+
+//
+void			btSoftBody::appendLink(	int node0,
+									   int node1,
+									   Material* mat,
+									   bool bcheckexist)
+{
+	appendLink(&m_nodes[node0],&m_nodes[node1],mat,bcheckexist);
+}
+
+//
+void			btSoftBody::appendLink(	Node* node0,
+									   Node* node1,
+									   Material* mat,
+									   bool bcheckexist)
+{
+	if((!bcheckexist)||(!checkLink(node0,node1)))
+	{
+		appendLink(-1,mat);
+		Link&	l=m_links[m_links.size()-1];
+		l.m_n[0]		=	node0;
+		l.m_n[1]		=	node1;
+		l.m_rl			=	(l.m_n[0]->m_x-l.m_n[1]->m_x).length();
+		m_bUpdateRtCst=true;
+	}
+}
+
+//
+void			btSoftBody::appendFace(int model,Material* mat)
+{
+	Face	f;
+	if(model>=0)
+	{ f=m_faces[model]; }
+	else
+	{ ZeroInitialize(f);f.m_material=mat?mat:m_materials[0]; }
+	m_faces.push_back(f);
+}
+
+//
+void			btSoftBody::appendFace(int node0,int node1,int node2,Material* mat)
+{
+	if (node0==node1)
+		return;
+	if (node1==node2)
+		return;
+	if (node2==node0)
+		return;
+
+	appendFace(-1,mat);
+	Face&	f=m_faces[m_faces.size()-1];
+	btAssert(node0!=node1);
+	btAssert(node1!=node2);
+	btAssert(node2!=node0);
+	f.m_n[0]	=	&m_nodes[node0];
+	f.m_n[1]	=	&m_nodes[node1];
+	f.m_n[2]	=	&m_nodes[node2];
+	f.m_ra		=	AreaOf(	f.m_n[0]->m_x,
+		f.m_n[1]->m_x,
+		f.m_n[2]->m_x);	
+	m_bUpdateRtCst=true;
+}
+
+//
+void			btSoftBody::appendTetra(int model,Material* mat)
+{
+Tetra	t;
+if(model>=0)
+	t=m_tetras[model];
+	else
+	{ ZeroInitialize(t);t.m_material=mat?mat:m_materials[0]; }
+m_tetras.push_back(t);
+}
+
+//
+void			btSoftBody::appendTetra(int node0,
+										int node1,
+										int node2,
+										int node3,
+										Material* mat)
+{
+	appendTetra(-1,mat);
+	Tetra&	t=m_tetras[m_tetras.size()-1];
+	t.m_n[0]	=	&m_nodes[node0];
+	t.m_n[1]	=	&m_nodes[node1];
+	t.m_n[2]	=	&m_nodes[node2];
+	t.m_n[3]	=	&m_nodes[node3];
+	t.m_rv		=	VolumeOf(t.m_n[0]->m_x,t.m_n[1]->m_x,t.m_n[2]->m_x,t.m_n[3]->m_x);
+	m_bUpdateRtCst=true;
+}
+
+//
+
+void			btSoftBody::appendAnchor(int node,btRigidBody* body, bool disableCollisionBetweenLinkedBodies,btScalar influence)
+{
+	btVector3 local = body->getWorldTransform().inverse()*m_nodes[node].m_x;
+	appendAnchor(node,body,local,disableCollisionBetweenLinkedBodies,influence);
+}
+
+//
+void			btSoftBody::appendAnchor(int node,btRigidBody* body, const btVector3& localPivot,bool disableCollisionBetweenLinkedBodies,btScalar influence)
+{
+	if (disableCollisionBetweenLinkedBodies)
+	{
+		if (m_collisionDisabledObjects.findLinearSearch(body)==m_collisionDisabledObjects.size())
+		{
+			m_collisionDisabledObjects.push_back(body);
+		}
+	}
+
+	Anchor	a;
+	a.m_node			=	&m_nodes[node];
+	a.m_body			=	body;
+	a.m_local			=	localPivot;
+	a.m_node->m_battach	=	1;
+	a.m_influence = influence;
+	m_anchors.push_back(a);
+}
+
+//
+void			btSoftBody::appendLinearJoint(const LJoint::Specs& specs,Cluster* body0,Body body1)
+{
+	LJoint*		pj	=	new(btAlignedAlloc(sizeof(LJoint),16)) LJoint();
+	pj->m_bodies[0]	=	body0;
+	pj->m_bodies[1]	=	body1;
+	pj->m_refs[0]	=	pj->m_bodies[0].xform().inverse()*specs.position;
+	pj->m_refs[1]	=	pj->m_bodies[1].xform().inverse()*specs.position;
+	pj->m_cfm		=	specs.cfm;
+	pj->m_erp		=	specs.erp;
+	pj->m_split		=	specs.split;
+	m_joints.push_back(pj);
+}
+
+//
+void			btSoftBody::appendLinearJoint(const LJoint::Specs& specs,Body body)
+{
+	appendLinearJoint(specs,m_clusters[0],body);
+}
+
+//
+void			btSoftBody::appendLinearJoint(const LJoint::Specs& specs,btSoftBody* body)
+{
+	appendLinearJoint(specs,m_clusters[0],body->m_clusters[0]);
+}
+
+//
+void			btSoftBody::appendAngularJoint(const AJoint::Specs& specs,Cluster* body0,Body body1)
+{
+	AJoint*		pj	=	new(btAlignedAlloc(sizeof(AJoint),16)) AJoint();
+	pj->m_bodies[0]	=	body0;
+	pj->m_bodies[1]	=	body1;
+	pj->m_refs[0]	=	pj->m_bodies[0].xform().inverse().getBasis()*specs.axis;
+	pj->m_refs[1]	=	pj->m_bodies[1].xform().inverse().getBasis()*specs.axis;
+	pj->m_cfm		=	specs.cfm;
+	pj->m_erp		=	specs.erp;
+	pj->m_split		=	specs.split;
+	pj->m_icontrol	=	specs.icontrol;
+	m_joints.push_back(pj);
+}
+
+//
+void			btSoftBody::appendAngularJoint(const AJoint::Specs& specs,Body body)
+{
+	appendAngularJoint(specs,m_clusters[0],body);
+}
+
+//
+void			btSoftBody::appendAngularJoint(const AJoint::Specs& specs,btSoftBody* body)
+{
+	appendAngularJoint(specs,m_clusters[0],body->m_clusters[0]);
+}
+
+//
+void			btSoftBody::addForce(const btVector3& force)
+{
+	for(int i=0,ni=m_nodes.size();i<ni;++i) addForce(force,i);
+}
+
+//
+void			btSoftBody::addForce(const btVector3& force,int node)
+{
+	Node&	n=m_nodes[node];
+	if(n.m_im>0)
+	{
+		n.m_f	+=	force;
+	}
+}
+
+//
+void			btSoftBody::addVelocity(const btVector3& velocity)
+{
+	for(int i=0,ni=m_nodes.size();i<ni;++i) addVelocity(velocity,i);
+}
+
+/* Set velocity for the entire body										*/ 
+void				btSoftBody::setVelocity(	const btVector3& velocity)
+{
+	for(int i=0,ni=m_nodes.size();i<ni;++i) 
+	{
+		Node&	n=m_nodes[i];
+		if(n.m_im>0)
+		{
+			n.m_v	=	velocity;
+		}
+	}
+}
+
+
+//
+void			btSoftBody::addVelocity(const btVector3& velocity,int node)
+{
+	Node&	n=m_nodes[node];
+	if(n.m_im>0)
+	{
+		n.m_v	+=	velocity;
+	}
+}
+
+//
+void			btSoftBody::setMass(int node,btScalar mass)
+{
+	m_nodes[node].m_im=mass>0?1/mass:0;
+	m_bUpdateRtCst=true;
+}
+
+//
+btScalar		btSoftBody::getMass(int node) const
+{
+	return(m_nodes[node].m_im>0?1/m_nodes[node].m_im:0);
+}
+
+//
+btScalar		btSoftBody::getTotalMass() const
+{
+	btScalar	mass=0;
+	for(int i=0;i<m_nodes.size();++i)
+	{
+		mass+=getMass(i);
+	}
+	return(mass);
+}
+
+//
+void			btSoftBody::setTotalMass(btScalar mass,bool fromfaces)
+{
+	int i;
+
+	if(fromfaces)
+	{
+
+		for(i=0;i<m_nodes.size();++i)
+		{
+			m_nodes[i].m_im=0;
+		}
+		for(i=0;i<m_faces.size();++i)
+		{
+			const Face&		f=m_faces[i];
+			const btScalar	twicearea=AreaOf(	f.m_n[0]->m_x,
+				f.m_n[1]->m_x,
+				f.m_n[2]->m_x);
+			for(int j=0;j<3;++j)
+			{
+				f.m_n[j]->m_im+=twicearea;
+			}
+		}
+		for( i=0;i<m_nodes.size();++i)
+		{
+			m_nodes[i].m_im=1/m_nodes[i].m_im;
+		}
+	}
+	const btScalar	tm=getTotalMass();
+	const btScalar	itm=1/tm;
+	for( i=0;i<m_nodes.size();++i)
+	{
+		m_nodes[i].m_im/=itm*mass;
+	}
+	m_bUpdateRtCst=true;
+}
+
+//
+void			btSoftBody::setTotalDensity(btScalar density)
+{
+	setTotalMass(getVolume()*density,true);
+}
+
+//
+void			btSoftBody::setVolumeMass(btScalar mass)
+{
+btAlignedObjectArray<btScalar>	ranks;
+ranks.resize(m_nodes.size(),0);
+int i;
+
+for(i=0;i<m_nodes.size();++i)
+	{
+	m_nodes[i].m_im=0;
+	}
+for(i=0;i<m_tetras.size();++i)
+	{
+	const Tetra& t=m_tetras[i];
+	for(int j=0;j<4;++j)
+		{
+		t.m_n[j]->m_im+=btFabs(t.m_rv);
+		ranks[int(t.m_n[j]-&m_nodes[0])]+=1;
+		}
+	}
+for( i=0;i<m_nodes.size();++i)
+	{
+	if(m_nodes[i].m_im>0)
+		{
+		m_nodes[i].m_im=ranks[i]/m_nodes[i].m_im;
+		}
+	}
+setTotalMass(mass,false);
+}
+
+//
+void			btSoftBody::setVolumeDensity(btScalar density)
+{
+btScalar	volume=0;
+for(int i=0;i<m_tetras.size();++i)
+	{
+	const Tetra& t=m_tetras[i];
+	for(int j=0;j<4;++j)
+		{
+		volume+=btFabs(t.m_rv);
+		}
+	}
+setVolumeMass(volume*density/6);
+}
+
+//
+void			btSoftBody::transform(const btTransform& trs)
+{
+	const btScalar	margin=getCollisionShape()->getMargin();
+	ATTRIBUTE_ALIGNED16(btDbvtVolume)	vol;
+	
+	for(int i=0,ni=m_nodes.size();i<ni;++i)
+	{
+		Node&	n=m_nodes[i];
+		n.m_x=trs*n.m_x;
+		n.m_q=trs*n.m_q;
+		n.m_n=trs.getBasis()*n.m_n;
+		vol = btDbvtVolume::FromCR(n.m_x,margin);
+		
+		m_ndbvt.update(n.m_leaf,vol);
+	}
+	updateNormals();
+	updateBounds();
+	updateConstants();
+	m_initialWorldTransform = trs;
+}
+
+//
+void			btSoftBody::translate(const btVector3& trs)
+{
+	btTransform	t;
+	t.setIdentity();
+	t.setOrigin(trs);
+	transform(t);
+}
+
+//
+void			btSoftBody::rotate(	const btQuaternion& rot)
+{
+	btTransform	t;
+	t.setIdentity();
+	t.setRotation(rot);
+	transform(t);
+}
+
+//
+void			btSoftBody::scale(const btVector3& scl)
+{
+
+	const btScalar	margin=getCollisionShape()->getMargin();
+	ATTRIBUTE_ALIGNED16(btDbvtVolume)	vol;
+	
+	for(int i=0,ni=m_nodes.size();i<ni;++i)
+	{
+		Node&	n=m_nodes[i];
+		n.m_x*=scl;
+		n.m_q*=scl;
+		vol = btDbvtVolume::FromCR(n.m_x,margin);
+		m_ndbvt.update(n.m_leaf,vol);
+	}
+	updateNormals();
+	updateBounds();
+	updateConstants();
+}
+
+//
+void			btSoftBody::setPose(bool bvolume,bool bframe)
+{
+	m_pose.m_bvolume	=	bvolume;
+	m_pose.m_bframe		=	bframe;
+	int i,ni;
+
+	/* Weights		*/ 
+	const btScalar	omass=getTotalMass();
+	const btScalar	kmass=omass*m_nodes.size()*1000;
+	btScalar		tmass=omass;
+	m_pose.m_wgh.resize(m_nodes.size());
+	for(i=0,ni=m_nodes.size();i<ni;++i)
+	{
+		if(m_nodes[i].m_im<=0) tmass+=kmass;
+	}
+	for( i=0,ni=m_nodes.size();i<ni;++i)
+	{
+		Node&	n=m_nodes[i];
+		m_pose.m_wgh[i]=	n.m_im>0					?
+			1/(m_nodes[i].m_im*tmass)	:
+		kmass/tmass;
+	}
+	/* Pos		*/ 
+	const btVector3	com=evaluateCom();
+	m_pose.m_pos.resize(m_nodes.size());
+	for( i=0,ni=m_nodes.size();i<ni;++i)
+	{
+		m_pose.m_pos[i]=m_nodes[i].m_x-com;
+	}
+	m_pose.m_volume	=	bvolume?getVolume():0;
+	m_pose.m_com	=	com;
+	m_pose.m_rot.setIdentity();
+	m_pose.m_scl.setIdentity();
+	/* Aqq		*/ 
+	m_pose.m_aqq[0]	=
+		m_pose.m_aqq[1]	=
+		m_pose.m_aqq[2]	=	btVector3(0,0,0);
+	for( i=0,ni=m_nodes.size();i<ni;++i)
+	{
+		const btVector3&	q=m_pose.m_pos[i];
+		const btVector3		mq=m_pose.m_wgh[i]*q;
+		m_pose.m_aqq[0]+=mq.x()*q;
+		m_pose.m_aqq[1]+=mq.y()*q;
+		m_pose.m_aqq[2]+=mq.z()*q;
+	}
+	m_pose.m_aqq=m_pose.m_aqq.inverse();
+	updateConstants();
+}
+
+//
+btScalar		btSoftBody::getVolume() const
+{
+	btScalar	vol=0;
+	if(m_nodes.size()>0)
+	{
+		int i,ni;
+
+		const btVector3	org=m_nodes[0].m_x;
+		for(i=0,ni=m_faces.size();i<ni;++i)
+		{
+			const Face&	f=m_faces[i];
+			vol+=btDot(f.m_n[0]->m_x-org,btCross(f.m_n[1]->m_x-org,f.m_n[2]->m_x-org));
+		}
+		vol/=(btScalar)6;
+	}
+	return(vol);
+}
+
+//
+int				btSoftBody::clusterCount() const
+{
+	return(m_clusters.size());
+}
+
+//
+btVector3		btSoftBody::clusterCom(const Cluster* cluster)
+{
+	btVector3		com(0,0,0);
+	for(int i=0,ni=cluster->m_nodes.size();i<ni;++i)
+	{
+		com+=cluster->m_nodes[i]->m_x*cluster->m_masses[i];
+	}
+	return(com*cluster->m_imass);
+}
+
+//
+btVector3		btSoftBody::clusterCom(int cluster) const
+{
+	return(clusterCom(m_clusters[cluster]));
+}
+
+//
+btVector3		btSoftBody::clusterVelocity(const Cluster* cluster,const btVector3& rpos)
+{
+	return(cluster->m_lv+btCross(cluster->m_av,rpos));
+}
+
+//
+void			btSoftBody::clusterVImpulse(Cluster* cluster,const btVector3& rpos,const btVector3& impulse)
+{
+	const btVector3	li=cluster->m_imass*impulse;
+	const btVector3	ai=cluster->m_invwi*btCross(rpos,impulse);
+	cluster->m_vimpulses[0]+=li;cluster->m_lv+=li;
+	cluster->m_vimpulses[1]+=ai;cluster->m_av+=ai;
+	cluster->m_nvimpulses++;
+}
+
+//
+void			btSoftBody::clusterDImpulse(Cluster* cluster,const btVector3& rpos,const btVector3& impulse)
+{
+	const btVector3	li=cluster->m_imass*impulse;
+	const btVector3	ai=cluster->m_invwi*btCross(rpos,impulse);
+	cluster->m_dimpulses[0]+=li;
+	cluster->m_dimpulses[1]+=ai;
+	cluster->m_ndimpulses++;
+}
+
+//
+void			btSoftBody::clusterImpulse(Cluster* cluster,const btVector3& rpos,const Impulse& impulse)
+{
+	if(impulse.m_asVelocity)	clusterVImpulse(cluster,rpos,impulse.m_velocity);
+	if(impulse.m_asDrift)		clusterDImpulse(cluster,rpos,impulse.m_drift);
+}
+
+//
+void			btSoftBody::clusterVAImpulse(Cluster* cluster,const btVector3& impulse)
+{
+	const btVector3	ai=cluster->m_invwi*impulse;
+	cluster->m_vimpulses[1]+=ai;cluster->m_av+=ai;
+	cluster->m_nvimpulses++;
+}
+
+//
+void			btSoftBody::clusterDAImpulse(Cluster* cluster,const btVector3& impulse)
+{
+	const btVector3	ai=cluster->m_invwi*impulse;
+	cluster->m_dimpulses[1]+=ai;
+	cluster->m_ndimpulses++;
+}
+
+//
+void			btSoftBody::clusterAImpulse(Cluster* cluster,const Impulse& impulse)
+{
+	if(impulse.m_asVelocity)	clusterVAImpulse(cluster,impulse.m_velocity);
+	if(impulse.m_asDrift)		clusterDAImpulse(cluster,impulse.m_drift);
+}
+
+//
+void			btSoftBody::clusterDCImpulse(Cluster* cluster,const btVector3& impulse)
+{
+	cluster->m_dimpulses[0]+=impulse*cluster->m_imass;
+	cluster->m_ndimpulses++;
+}
+
+struct NodeLinks
+{
+    btAlignedObjectArray<int> m_links;
+};
+
+
+
+//
+int				btSoftBody::generateBendingConstraints(int distance,Material* mat)
+{
+	int i,j;
+
+	if(distance>1)
+	{
+		/* Build graph	*/ 
+		const int		n=m_nodes.size();
+		const unsigned	inf=(~(unsigned)0)>>1;
+		unsigned*		adj=new unsigned[n*n];
+		
+
+#define IDX(_x_,_y_)	((_y_)*n+(_x_))
+		for(j=0;j<n;++j)
+		{
+			for(i=0;i<n;++i)
+			{
+				if(i!=j)
+				{
+					adj[IDX(i,j)]=adj[IDX(j,i)]=inf;
+				}
+				else
+				{
+					adj[IDX(i,j)]=adj[IDX(j,i)]=0;
+				}
+			}
+		}
+		for( i=0;i<m_links.size();++i)
+		{
+			const int	ia=(int)(m_links[i].m_n[0]-&m_nodes[0]);
+			const int	ib=(int)(m_links[i].m_n[1]-&m_nodes[0]);
+			adj[IDX(ia,ib)]=1;
+			adj[IDX(ib,ia)]=1;
+		}
+
+
+		//special optimized case for distance == 2
+		if (distance == 2)
+		{
+
+			btAlignedObjectArray<NodeLinks> nodeLinks;
+
+
+			/* Build node links */
+			nodeLinks.resize(m_nodes.size());
+
+			for( i=0;i<m_links.size();++i)
+			{
+				const int	ia=(int)(m_links[i].m_n[0]-&m_nodes[0]);
+				const int	ib=(int)(m_links[i].m_n[1]-&m_nodes[0]);
+				if (nodeLinks[ia].m_links.findLinearSearch(ib)==nodeLinks[ia].m_links.size())
+					nodeLinks[ia].m_links.push_back(ib);
+
+				if (nodeLinks[ib].m_links.findLinearSearch(ia)==nodeLinks[ib].m_links.size())
+					nodeLinks[ib].m_links.push_back(ia);
+			}
+			for (int ii=0;ii<nodeLinks.size();ii++)
+			{
+				int i=ii;
+
+				for (int jj=0;jj<nodeLinks[ii].m_links.size();jj++)
+				{
+					int k = nodeLinks[ii].m_links[jj];
+					for (int kk=0;kk<nodeLinks[k].m_links.size();kk++)
+					{
+						int j = nodeLinks[k].m_links[kk];
+						if (i!=j)
+						{
+							const unsigned	sum=adj[IDX(i,k)]+adj[IDX(k,j)];
+							btAssert(sum==2);
+							if(adj[IDX(i,j)]>sum)
+							{
+								adj[IDX(i,j)]=adj[IDX(j,i)]=sum;
+							}
+						}
+
+					}
+				}
+			}
+		} else
+		{
+			///generic Floyd's algorithm
+			for(int k=0;k<n;++k)
+			{
+				for(j=0;j<n;++j)
+				{
+					for(i=j+1;i<n;++i)
+					{
+						const unsigned	sum=adj[IDX(i,k)]+adj[IDX(k,j)];
+						if(adj[IDX(i,j)]>sum)
+						{
+							adj[IDX(i,j)]=adj[IDX(j,i)]=sum;
+						}
+					}
+				}
+			}
+		}
+
+
+		/* Build links	*/ 
+		int	nlinks=0;
+		for(j=0;j<n;++j)
+		{
+			for(i=j+1;i<n;++i)
+			{
+				if(adj[IDX(i,j)]==(unsigned)distance)
+				{
+					appendLink(i,j,mat);
+					m_links[m_links.size()-1].m_bbending=1;
+					++nlinks;
+				}
+			}
+		}
+		delete[] adj;		
+		return(nlinks);
+	}
+	return(0);
+}
+
+//
+void			btSoftBody::randomizeConstraints()
+{
+	unsigned long	seed=243703;
+#define NEXTRAND (seed=(1664525L*seed+1013904223L)&0xffffffff)
+	int i,ni;
+
+	for(i=0,ni=m_links.size();i<ni;++i)
+	{
+		btSwap(m_links[i],m_links[NEXTRAND%ni]);
+	}
+	for(i=0,ni=m_faces.size();i<ni;++i)
+	{
+		btSwap(m_faces[i],m_faces[NEXTRAND%ni]);
+	}
+#undef NEXTRAND
+}
+
+//
+void			btSoftBody::releaseCluster(int index)
+{
+	Cluster*	c=m_clusters[index];
+	if(c->m_leaf) m_cdbvt.remove(c->m_leaf);
+	c->~Cluster();
+	btAlignedFree(c);
+	m_clusters.remove(c);
+}
+
+//
+void			btSoftBody::releaseClusters()
+{
+	while(m_clusters.size()>0) releaseCluster(0);
+}
+
+//
+int				btSoftBody::generateClusters(int k,int maxiterations)
+{
+	int i;
+	releaseClusters();
+	m_clusters.resize(btMin(k,m_nodes.size()));
+	for(i=0;i<m_clusters.size();++i)
+	{
+		m_clusters[i]			=	new(btAlignedAlloc(sizeof(Cluster),16)) Cluster();
+		m_clusters[i]->m_collide=	true;
+	}
+	k=m_clusters.size();
+	if(k>0)
+	{
+		/* Initialize		*/ 
+		btAlignedObjectArray<btVector3>	centers;
+		btVector3						cog(0,0,0);
+		int								i;
+		for(i=0;i<m_nodes.size();++i)
+		{
+			cog+=m_nodes[i].m_x;
+			m_clusters[(i*29873)%m_clusters.size()]->m_nodes.push_back(&m_nodes[i]);
+		}
+		cog/=(btScalar)m_nodes.size();
+		centers.resize(k,cog);
+		/* Iterate			*/ 
+		const btScalar	slope=16;
+		bool			changed;
+		int				iterations=0;
+		do	{
+			const btScalar	w=2-btMin<btScalar>(1,iterations/slope);
+			changed=false;
+			iterations++;	
+			int i;
+
+			for(i=0;i<k;++i)
+			{
+				btVector3	c(0,0,0);
+				for(int j=0;j<m_clusters[i]->m_nodes.size();++j)
+				{
+					c+=m_clusters[i]->m_nodes[j]->m_x;
+				}
+				if(m_clusters[i]->m_nodes.size())
+				{
+					c			/=	(btScalar)m_clusters[i]->m_nodes.size();
+					c			=	centers[i]+(c-centers[i])*w;
+					changed		|=	((c-centers[i]).length2()>SIMD_EPSILON);
+					centers[i]	=	c;
+					m_clusters[i]->m_nodes.resize(0);
+				}			
+			}
+			for(i=0;i<m_nodes.size();++i)
+			{
+				const btVector3	nx=m_nodes[i].m_x;
+				int				kbest=0;
+				btScalar		kdist=ClusterMetric(centers[0],nx);
+				for(int j=1;j<k;++j)
+				{
+					const btScalar	d=ClusterMetric(centers[j],nx);
+					if(d<kdist)
+					{
+						kbest=j;
+						kdist=d;
+					}
+				}
+				m_clusters[kbest]->m_nodes.push_back(&m_nodes[i]);
+			}		
+		} while(changed&&(iterations<maxiterations));
+		/* Merge		*/ 
+		btAlignedObjectArray<int>	cids;
+		cids.resize(m_nodes.size(),-1);
+		for(i=0;i<m_clusters.size();++i)
+		{
+			for(int j=0;j<m_clusters[i]->m_nodes.size();++j)
+			{
+				cids[int(m_clusters[i]->m_nodes[j]-&m_nodes[0])]=i;
+			}
+		}
+		for(i=0;i<m_faces.size();++i)
+		{
+			const int idx[]={	int(m_faces[i].m_n[0]-&m_nodes[0]),
+				int(m_faces[i].m_n[1]-&m_nodes[0]),
+				int(m_faces[i].m_n[2]-&m_nodes[0])};
+			for(int j=0;j<3;++j)
+			{
+				const int cid=cids[idx[j]];
+				for(int q=1;q<3;++q)
+				{
+					const int kid=idx[(j+q)%3];
+					if(cids[kid]!=cid)
+					{
+						if(m_clusters[cid]->m_nodes.findLinearSearch(&m_nodes[kid])==m_clusters[cid]->m_nodes.size())
+						{
+							m_clusters[cid]->m_nodes.push_back(&m_nodes[kid]);
+						}
+					}
+				}
+			}
+		}
+		/* Master		*/ 
+		if(m_clusters.size()>1)
+		{
+			Cluster*	pmaster=new(btAlignedAlloc(sizeof(Cluster),16)) Cluster();
+			pmaster->m_collide	=	false;
+			pmaster->m_nodes.reserve(m_nodes.size());
+			for(int i=0;i<m_nodes.size();++i) pmaster->m_nodes.push_back(&m_nodes[i]);
+			m_clusters.push_back(pmaster);
+			btSwap(m_clusters[0],m_clusters[m_clusters.size()-1]);
+		}
+		/* Terminate	*/ 
+		for(i=0;i<m_clusters.size();++i)
+		{
+			if(m_clusters[i]->m_nodes.size()==0)
+			{
+				releaseCluster(i--);
+			}
+		}
+	} else
+	{
+		//create a cluster for each tetrahedron (if tetrahedra exist) or each face
+		if (m_tetras.size())
+		{
+			m_clusters.resize(m_tetras.size());
+			for(i=0;i<m_clusters.size();++i)
+			{
+				m_clusters[i]			=	new(btAlignedAlloc(sizeof(Cluster),16)) Cluster();
+				m_clusters[i]->m_collide=	true;
+			}
+			for (i=0;i<m_tetras.size();i++)
+			{
+				for (int j=0;j<4;j++)
+				{
+					m_clusters[i]->m_nodes.push_back(m_tetras[i].m_n[j]);
+				}
+			}
+
+		} else
+		{
+			m_clusters.resize(m_faces.size());
+			for(i=0;i<m_clusters.size();++i)
+			{
+				m_clusters[i]			=	new(btAlignedAlloc(sizeof(Cluster),16)) Cluster();
+				m_clusters[i]->m_collide=	true;
+			}
+
+			for(i=0;i<m_faces.size();++i)
+			{
+				for(int j=0;j<3;++j)
+				{
+					m_clusters[i]->m_nodes.push_back(m_faces[i].m_n[j]);
+				}
+			}
+		}
+	}
+
+	if (m_clusters.size())
+	{
+		initializeClusters();
+		updateClusters();
+
+
+		//for self-collision
+		m_clusterConnectivity.resize(m_clusters.size()*m_clusters.size());
+		{
+			for (int c0=0;c0<m_clusters.size();c0++)
+			{
+				m_clusters[c0]->m_clusterIndex=c0;
+				for (int c1=0;c1<m_clusters.size();c1++)
+				{
+					
+					bool connected=false;
+					Cluster* cla = m_clusters[c0];
+					Cluster* clb = m_clusters[c1];
+					for (int i=0;!connected&&i<cla->m_nodes.size();i++)
+					{
+						for (int j=0;j<clb->m_nodes.size();j++)
+						{
+							if (cla->m_nodes[i] == clb->m_nodes[j])
+							{
+								connected=true;
+								break;
+							}
+						}
+					}
+					m_clusterConnectivity[c0+c1*m_clusters.size()]=connected;
+				}
+			}
+		}
+	}
+
+	return(m_clusters.size());
+}
+
+//
+void			btSoftBody::refine(ImplicitFn* ifn,btScalar accurary,bool cut)
+{
+	const Node*			nbase = &m_nodes[0];
+	int					ncount = m_nodes.size();
+	btSymMatrix<int>	edges(ncount,-2);
+	int					newnodes=0;
+	int i,j,k,ni;
+
+	/* Filter out		*/ 
+	for(i=0;i<m_links.size();++i)
+	{
+		Link&	l=m_links[i];
+		if(l.m_bbending)
+		{
+			if(!SameSign(ifn->Eval(l.m_n[0]->m_x),ifn->Eval(l.m_n[1]->m_x)))
+			{
+				btSwap(m_links[i],m_links[m_links.size()-1]);
+				m_links.pop_back();--i;
+			}
+		}	
+	}
+	/* Fill edges		*/ 
+	for(i=0;i<m_links.size();++i)
+	{
+		Link&	l=m_links[i];
+		edges(int(l.m_n[0]-nbase),int(l.m_n[1]-nbase))=-1;
+	}
+	for(i=0;i<m_faces.size();++i)
+	{	
+		Face&	f=m_faces[i];
+		edges(int(f.m_n[0]-nbase),int(f.m_n[1]-nbase))=-1;
+		edges(int(f.m_n[1]-nbase),int(f.m_n[2]-nbase))=-1;
+		edges(int(f.m_n[2]-nbase),int(f.m_n[0]-nbase))=-1;
+	}
+	/* Intersect		*/ 
+	for(i=0;i<ncount;++i)
+	{
+		for(j=i+1;j<ncount;++j)
+		{
+			if(edges(i,j)==-1)
+			{
+				Node&			a=m_nodes[i];
+				Node&			b=m_nodes[j];
+				const btScalar	t=ImplicitSolve(ifn,a.m_x,b.m_x,accurary);
+				if(t>0)
+				{
+					const btVector3	x=Lerp(a.m_x,b.m_x,t);
+					const btVector3	v=Lerp(a.m_v,b.m_v,t);
+					btScalar		m=0;
+					if(a.m_im>0)
+					{
+						if(b.m_im>0)
+						{
+							const btScalar	ma=1/a.m_im;
+							const btScalar	mb=1/b.m_im;
+							const btScalar	mc=Lerp(ma,mb,t);
+							const btScalar	f=(ma+mb)/(ma+mb+mc);
+							a.m_im=1/(ma*f);
+							b.m_im=1/(mb*f);
+							m=mc*f;
+						}
+						else
+						{ a.m_im/=0.5;m=1/a.m_im; }
+					}
+					else
+					{
+						if(b.m_im>0)
+						{ b.m_im/=0.5;m=1/b.m_im; }
+						else
+							m=0;
+					}
+					appendNode(x,m);
+					edges(i,j)=m_nodes.size()-1;
+					m_nodes[edges(i,j)].m_v=v;
+					++newnodes;
+				}
+			}
+		}
+	}
+	nbase=&m_nodes[0];
+	/* Refine links		*/ 
+	for(i=0,ni=m_links.size();i<ni;++i)
+	{
+		Link&		feat=m_links[i];
+		const int	idx[]={	int(feat.m_n[0]-nbase),
+			int(feat.m_n[1]-nbase)};
+		if((idx[0]<ncount)&&(idx[1]<ncount)