src/BulletSoftBody/btSoftBody.cpp

/*
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"
#include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h"
#include "BulletDynamics/Featherstone/btMultiBodyConstraint.h"


//
btSoftBody::btSoftBody(btSoftBodyWorldInfo*	worldInfo,int node_count,  const btVector3* x,  const btScalar* m)
:m_softBodySolver(0),m_worldInfo(worldInfo)
{
	/* 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.25f);

	m_initialWorldTransform.setIdentity();

	m_windVelocity = btVector3(0,0,0);
	m_restLengthScale = btScalar(1.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::addAeroForceToNode(const btVector3& windVelocity,int nodeIndex)
{
	btAssert(nodeIndex >= 0 && nodeIndex < m_nodes.size());

	const btScalar dt = m_sst.sdt;
	const btScalar kLF = m_cfg.kLF;
	const btScalar kDG = m_cfg.kDG;
	//const btScalar kPR = m_cfg.kPR;
	//const btScalar kVC = m_cfg.kVC;
	const bool as_lift = kLF>0;
	const bool as_drag = kDG>0;
	const bool as_aero = as_lift || as_drag;
	const bool as_vaero = as_aero && (m_cfg.aeromodel < btSoftBody::eAeroModel::F_TwoSided);

	Node& n = m_nodes[nodeIndex];

	if( n.m_im>0 )
	{
		btSoftBody::sMedium	medium;

		EvaluateMedium(m_worldInfo, n.m_x, medium);
		medium.m_velocity = windVelocity;
		medium.m_density = m_worldInfo->air_density;

		/* Aerodynamics			*/
		if(as_vaero)
		{
			const btVector3	rel_v = n.m_v - medium.m_velocity;
			const btScalar rel_v_len = rel_v.length();
			const btScalar	rel_v2 = rel_v.length2();

			if(rel_v2>SIMD_EPSILON)
			{
				const btVector3 rel_v_nrm = rel_v.normalized();
				btVector3	nrm = n.m_n;

				if (m_cfg.aeromodel == btSoftBody::eAeroModel::V_TwoSidedLiftDrag)
				{
					nrm *= (btScalar)( (btDot(nrm,rel_v) < 0) ? -1 : +1);
					btVector3 fDrag(0, 0, 0);
					btVector3 fLift(0, 0, 0);

					btScalar n_dot_v = nrm.dot(rel_v_nrm);
					btScalar tri_area = 0.5f * n.m_area;

					fDrag = 0.5f * kDG * medium.m_density * rel_v2 * tri_area * n_dot_v * (-rel_v_nrm);

					// Check angle of attack
					// cos(10�) = 0.98480
					if ( 0 < n_dot_v && n_dot_v < 0.98480f)
						fLift = 0.5f * kLF * medium.m_density * rel_v_len * tri_area * btSqrt(1.0f-n_dot_v*n_dot_v) * (nrm.cross(rel_v_nrm).cross(rel_v_nrm));

					// Check if the velocity change resulted by aero drag force exceeds the current velocity of the node.
					btVector3 del_v_by_fDrag = fDrag*n.m_im*m_sst.sdt;
					btScalar del_v_by_fDrag_len2 = del_v_by_fDrag.length2();
					btScalar v_len2 = n.m_v.length2();

					if (del_v_by_fDrag_len2 >= v_len2 && del_v_by_fDrag_len2 > 0)
					{
						btScalar del_v_by_fDrag_len = del_v_by_fDrag.length();
						btScalar v_len = n.m_v.length();
						fDrag *= btScalar(0.8)*(v_len / del_v_by_fDrag_len);
					}

					n.m_f += fDrag;
					n.m_f += fLift;
				}
				else if (m_cfg.aeromodel == btSoftBody::eAeroModel::V_Point || m_cfg.aeromodel == btSoftBody::eAeroModel::V_OneSided || m_cfg.aeromodel == btSoftBody::eAeroModel::V_TwoSided)
				{
					if (btSoftBody::eAeroModel::V_TwoSided)
						nrm *= (btScalar)( (btDot(nrm,rel_v) < 0) ? -1 : +1);

					const btScalar dvn = btDot(rel_v,nrm);
					/* Compute forces	*/
					if(dvn>0)
					{
						btVector3		force(0,0,0);
						const btScalar	c0	=	n.m_area * dvn * rel_v2/2;
						const btScalar	c1	=	c0 * medium.m_density;
						force	+=	nrm*(-c1*kLF);
						force	+=	rel_v.normalized() * (-c1 * kDG);
						ApplyClampedForce(n, force, dt);
					}
				}
			}
		}
	}
}

void			btSoftBody::addAeroForceToFace(const btVector3& windVelocity,int faceIndex)
{
	const btScalar dt = m_sst.sdt;
	const btScalar kLF = m_cfg.kLF;
	const btScalar kDG = m_cfg.kDG;
//	const btScalar kPR = m_cfg.kPR;
//	const btScalar kVC = m_cfg.kVC;
	const bool as_lift = kLF>0;
	const bool as_drag = kDG>0;
	const bool as_aero = as_lift || as_drag;
	const bool as_faero = as_aero && (m_cfg.aeromodel >= btSoftBody::eAeroModel::F_TwoSided);

	if(as_faero)
	{
		btSoftBody::Face&	f=m_faces[faceIndex];

		btSoftBody::sMedium	medium;

		const btVector3	v=(f.m_n[0]->m_v+f.m_n[1]->m_v+f.m_n[2]->m_v)/3;
		const btVector3	x=(f.m_n[0]->m_x+f.m_n[1]->m_x+f.m_n[2]->m_x)/3;
		EvaluateMedium(m_worldInfo,x,medium);
		medium.m_velocity = windVelocity;
		medium.m_density = m_worldInfo->air_density;
		const btVector3	rel_v=v-medium.m_velocity;
		const btScalar rel_v_len = rel_v.length();
		const btScalar	rel_v2=rel_v.length2();

		if(rel_v2>SIMD_EPSILON)
		{
			const btVector3 rel_v_nrm = rel_v.normalized();
			btVector3	nrm = f.m_normal;

			if (m_cfg.aeromodel == btSoftBody::eAeroModel::F_TwoSidedLiftDrag)
			{
				nrm *= (btScalar)( (btDot(nrm,rel_v) < 0) ? -1 : +1);

				btVector3 fDrag(0, 0, 0);
				btVector3 fLift(0, 0, 0);

				btScalar n_dot_v = nrm.dot(rel_v_nrm);
				btScalar tri_area = 0.5f * f.m_ra;

				fDrag = 0.5f * kDG * medium.m_density * rel_v2 * tri_area * n_dot_v * (-rel_v_nrm);

				// Check angle of attack
				// cos(10�) = 0.98480
				if ( 0 < n_dot_v && n_dot_v < 0.98480f)
					fLift = 0.5f * kLF * medium.m_density * rel_v_len * tri_area * btSqrt(1.0f-n_dot_v*n_dot_v) * (nrm.cross(rel_v_nrm).cross(rel_v_nrm));

				fDrag /= 3;
				fLift /= 3;

				for(int j=0;j<3;++j)
				{
					if (f.m_n[j]->m_im>0)
					{
						// Check if the velocity change resulted by aero drag force exceeds the current velocity of the node.
						btVector3 del_v_by_fDrag = fDrag*f.m_n[j]->m_im*m_sst.sdt;
						btScalar del_v_by_fDrag_len2 = del_v_by_fDrag.length2();
						btScalar v_len2 = f.m_n[j]->m_v.length2();

						if (del_v_by_fDrag_len2 >= v_len2 && del_v_by_fDrag_len2 > 0)
						{
							btScalar del_v_by_fDrag_len = del_v_by_fDrag.length();
							btScalar v_len = f.m_n[j]->m_v.length();
							fDrag *= btScalar(0.8)*(v_len / del_v_by_fDrag_len);
						}

						f.m_n[j]->m_f += fDrag;
						f.m_n[j]->m_f += fLift;
					}
				}
			}
			else if (m_cfg.aeromodel == btSoftBody::eAeroModel::F_OneSided || m_cfg.aeromodel == btSoftBody::eAeroModel::F_TwoSided)
			{
				if (btSoftBody::eAeroModel::F_TwoSided)
					nrm *= (btScalar)( (btDot(nrm,rel_v) < 0) ? -1 : +1);

				const btScalar	dvn=btDot(rel_v,nrm);
				/* Compute forces	*/
				if(dvn>0)
				{
					btVector3		force(0,0,0);
					const btScalar	c0	=	f.m_ra*dvn*rel_v2;
					const btScalar	c1	=	c0*medium.m_density;
					force	+=	nrm*(-c1*kLF);
					force	+=	rel_v.normalized()*(-c1*kDG);
					force	/=	3;
					for(int j=0;j<3;++j) ApplyClampedForce(*f.m_n[j],force,dt);
				}
			}
		}
	}

}

//
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();
}

//
btScalar btSoftBody::getRestLengthScale()
{
	return m_restLengthScale;
}

//
void btSoftBody::setRestLengthScale(btScalar restLengthScale)
{
	for(int i=0, ni=m_links.size(); i<ni; ++i)
	{
		Link&		l=m_links[i];
		l.m_rl	=	l.m_rl / m_restLengthScale * restLengthScale;
		l.m_c1	=	l.m_rl*l.m_rl;
	}
	m_restLengthScale = restLengthScale;

	if (getActivationState() == ISLAND_SLEEPING)
		activate();
}

//
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();
}

void				btSoftBody::resetLinkRestLengths()
{
	for(int i=0, ni=m_links.size();i<ni;++i)
	{
		Link& l =	m_links[i];
		l.m_rl	=	(l.m_n[0]->m_x-l.m_n[1]->m_x).length();
		l.m_c1	=	l.m_rl*l.m_rl;
	}
}

//
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.5f;m=1/a.m_im; }
					}
					else
					{
						if(b.m_im>0)
						{ b.m_im/=0.5f;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))
		{
			const int ni=edges(idx[0],idx[1]);
			if(ni>0)
			{
				appendLink(i);
				Link*		pft[]={	&m_links[i],
					&m_links[m_links.size()-1]};
				pft[0]->m_n[0]=&m_nodes[idx[0]];
				pft[0]->m_n[1]=&m_nodes[ni];
				pft[1]->m_n[0]=&m_nodes[ni];
				pft[1]->m_n[1]=&m_nodes[idx[1]];
			}
		}
	}
	/* Refine faces		*/
	for(i=0;i<m_faces.size();++i)
	{
		const Face&	feat=m_faces[i];
		const int	idx[]={	int(feat.m_n[0]-nbase),
			int(feat.m_n[1]-nbase),
			int(feat.m_n[2]-nbase)};
		for(j=2,k=0;k<3;j=k++)
		{
			if((idx[j]<ncount)&&(idx[k]<ncount))
			{
				const int ni=edges(idx[j],idx[k]);
				if(ni>0)
				{
					appendFace(i);
					const int	l=(k+1)%3;
					Face*		pft[]={	&m_faces[i],
						&m_faces[m_faces.size()-1]};
					pft[0]->m_n[0]=&m_nodes[idx[l]];
					pft[0]->m_n[1]=&m_nodes[idx[j]];
					pft[0]->m_n[2]=&m_nodes[ni];
					pft[1]->m_n[0]=&m_nodes[ni];
					pft[1]->m_n[1]=&m_nodes[idx[k]];
					pft[1]->m_n[2]=&m_nodes[idx[l]];
					appendLink(ni,idx[l],pft[0]->m_material);
					--i;break;
				}
			}
		}
	}
	/* Cut				*/
	if(cut)
	{
		btAlignedObjectArray<int>	cnodes;
		const int					pcount=ncount;
		int							i;
		ncount=m_nodes.size();
		cnodes.resize(ncount,0);
		/* Nodes		*/
		for(i=0;i<ncount;++i)
		{
			const btVector3	x=m_nodes[i].m_x;
			if((i>=pcount)||(btFabs(ifn->Eval(x))<accurary))
			{
				const btVector3	v=m_nodes[i].m_v;
				btScalar		m=getMass(i);
				if(m>0) { m*=0.5f;m_nodes[i].m_im/=0.5f; }
				appendNode(x,m);
				cnodes[i]=m_nodes.size()-1;
				m_nodes[cnodes[i]].m_v=v;
			}
		}
		nbase=&m_nodes[0];
		/* Links		*/
		for(i=0,ni=m_links.size();i<ni;++i)
		{
			const int		id[]={	int(m_links[i].m_n[0]-nbase),
				int(m_links[i].m_n[1]-nbase)};
			int				todetach=0;
			if(cnodes[id[0]]&&cnodes[id[1]])
			{
				appendLink(i);
				todetach=m_links.size()-1;
			}
			else
			{
				if((	(ifn->Eval(m_nodes[id[0]].m_x)<accurary)&&
					(ifn->Eval(m_nodes[id[1]].m_x)<accurary)))
					todetach=i;
			}
			if(todetach)
			{
				Link&	l=m_links[todetach];
				for(int j=0;j<2;++j)
				{
					int cn=cnodes[int(l.m_n[j]-nbase)];
					if(cn) l.m_n[j]=&m_nodes[cn];
				}
			}
		}
		/* Faces		*/
		for(i=0,ni=m_faces.size();i<ni;++i)
		{
			Node**			n=	m_faces[i].m_n;
			if(	(ifn->Eval(n[0]->m_x)<accurary)&&
				(ifn->Eval(n[1]->m_x)<accurary)&&
				(ifn->Eval(n[2]->m_x)<accurary))
			{
				for(int j=0;j<3;++j)
				{
					int cn=cnodes[int(n[j]-nbase)];
					if(cn) n[j]=&m_nodes[cn];
				}
			}
		}
		/* Clean orphans	*/
		int							nnodes=m_nodes.size();
		btAlignedObjectArray<int>	ranks;
		btAlignedObjectArray<int>	todelete;
		ranks.resize(nnodes,0);
		for(i=0,ni=m_links.size();i<ni;++i)
		{
			for(int j=0;j<2;++j) ranks[int(m_links[i].m_n[j]-nbase)]++;
		}
		for(i=0,ni=m_faces.size();i<ni;++i)
		{
			for(int j=0;j<3;++j) ranks[int(m_faces[i].m_n[j]-nbase)]++;
		}
		for(i=0;i<m_links.size();++i)
		{
			const int	id[]={	int(m_links[i].m_n[0]-nbase),
				int(m_links[i].m_n[1]-nbase)};
			const bool	sg[]={	ranks[id[0]]==1,
				ranks[id[1]]==1};
			if(sg[0]||sg[1])
			{
				--ranks[id[0]];
				--ranks[id[1]];
				btSwap(m_links[i],m_links[m_links.size()-1]);
				m_links.pop_back();--i;
			}
		}
#if 0
		for(i=nnodes-1;i>=0;--i)
		{
			if(!ranks[i]) todelete.push_back(i);
		}
		if(todelete.size())
		{
			btAlignedObjectArray<int>&	map=ranks;
			for(int i=0;i<nnodes;++i) map[i]=i;
			PointersToIndices(this);
			for(int i=0,ni=todelete.size();i<ni;++i)
			{
				int		j=todelete[i];
				int&	a=map[j];
				int&	b=map[--nnodes];
				m_ndbvt.remove(m_nodes[a].m_leaf);m_nodes[a].m_leaf=0;
				btSwap(m_nodes[a],m_nodes[b]);
				j=a;a=b;b=j;
			}
			IndicesToPointers(this,&map[0]);
			m_nodes.resize(nnodes);
		}
#endif
	}
	m_bUpdateRtCst=true;
}

//
bool			btSoftBody::cutLink(const Node* node0,const Node* node1,btScalar position)
{
	return(cutLink(int(node0-&m_nodes[0]),int(node1-&m_nodes[0]),position));
}

//
bool			btSoftBody::cutLink(int node0,int node1,btScalar position)
{
	bool			done=false;
	int i,ni;
//	const btVector3	d=m_nodes[node0].m_x-m_nodes[node1].m_x;
	const btVector3	x=Lerp(m_nodes[node0].m_x,m_nodes[node1].m_x,position);
	const btVector3	v=Lerp(m_nodes[node0].m_v,m_nodes[node1].m_v,position);
	const btScalar	m=1;
	appendNode(x,m);
	appendNode(x,m);
	Node*			pa=&m_nodes[node0];
	Node*			pb=&m_nodes[node1];
	Node*			pn[2]={	&m_nodes[m_nodes.size()-2],
		&m_nodes[m_nodes.size()-1]};
	pn[0]->m_v=v;
	pn[1]->m_v=v;
	for(i=0,ni=m_links.size();i<ni;++i)
	{
		const int mtch=MatchEdge(m_links[i].m_n[0],m_links[i].m_n[1],pa,pb);
		if(mtch!=-1)
		{
			appendLink(i);
			Link*	pft[]={&m_links[i],&m_links[m_links.size()-1]};
			pft[0]->m_n[1]=pn[mtch];
			pft[1]->m_n[0]=pn[1-mtch];
			done=true;
		}
	}
	for(i=0,ni=m_faces.size();i<ni;++i)
	{
		for(int k=2,l=0;l<3;k=l++)
		{
			const int mtch=MatchEdge(m_faces[i].m_n[k],m_faces[i].m_n[l],pa,pb);
			if(mtch!=-1)
			{
				appendFace(i);
				Face*	pft[]={&m_faces[i],&m_faces[m_faces.size()-1]};
				pft[0]->m_n[l]=pn[mtch];
				pft[1]->m_n[k]=pn[1-mtch];
				appendLink(pn[0],pft[0]->m_n[(l+1)%3],pft[0]->m_material,true);
				appendLink(pn[1],pft[0]->m_n[(l+1)%3],pft[0]->m_material,true);
			}
		}
	}
	if(!done)
	{
		m_ndbvt.remove(pn[0]->m_leaf);
		m_ndbvt.remove(pn[1]->m_leaf);
		m_nodes.pop_back();
		m_nodes.pop_back();
	}
	return(done);
}

//
bool			btSoftBody::rayTest(const btVector3& rayFrom,
									const btVector3& rayTo,
									sRayCast& results)
{
	if(m_faces.size()&&m_fdbvt.empty())
		initializeFaceTree();

	results.body	=	this;
	results.fraction = 1.f;
	results.feature	=	eFeature::None;
	results.index	=	-1;

	return(rayTest(rayFrom,rayTo,results.fraction,results.feature,results.index,false)!=0);
}

//
void			btSoftBody::setSolver(eSolverPresets::_ preset)
{
	m_cfg.m_vsequence.clear();
	m_cfg.m_psequence.clear();
	m_cfg.m_dsequence.clear();
	switch(preset)
	{
	case	eSolverPresets::Positions:
		m_cfg.m_psequence.push_back(ePSolver::Anchors);
		m_cfg.m_psequence.push_back(ePSolver::RContacts);
		m_cfg.m_psequence.push_back(ePSolver::SContacts);
		m_cfg.m_psequence.push_back(ePSolver::Linear);
		break;
	case	eSolverPresets::Velocities:
		m_cfg.m_vsequence.push_back(eVSolver::Linear);

		m_cfg.m_psequence.push_back(ePSolver::Anchors);
		m_cfg.m_psequence.push_back(ePSolver::RContacts);
		m_cfg.m_psequence.push_back(ePSolver::SContacts);

		m_cfg.m_dsequence.push_back(ePSolver::Linear);
		break;
	}
}

//
void			btSoftBody::predictMotion(btScalar dt)
{

	int i,ni;

	/* Update				*/
	if(m_bUpdateRtCst)
	{
		m_bUpdateRtCst=false;
		updateConstants();
		m_fdbvt.clear();
		if(m_cfg.collisions&fCollision::VF_SS)
		{
			initializeFaceTree();
		}
	}

	/* Prepare				*/
	m_sst.sdt		=	dt*m_cfg.timescale;
	m_sst.isdt		=	1/m_sst.sdt;
	m_sst.velmrg	=	m_sst.sdt*3;
	m_sst.radmrg	=	getCollisionShape()->getMargin();
	m_sst.updmrg	=	m_sst.radmrg*(btScalar)0.25;
	/* Forces				*/
	addVelocity(m_worldInfo->m_gravity*m_sst.sdt);
	applyForces();
	/* Integrate			*/
	for(i=0,ni=m_nodes.size();i<ni;++i)
	{
		Node&	n=m_nodes[i];
		n.m_q	=	n.m_x;
		btVector3 deltaV = n.m_f*n.m_im*m_sst.sdt;
		{
			btScalar maxDisplacement = m_worldInfo->m_maxDisplacement;
			btScalar clampDeltaV = maxDisplacement/m_sst.sdt;
			for (int c=0;c<3;c++)
			{
				if (deltaV[c]>clampDeltaV)
				{
					deltaV[c] = clampDeltaV;
				}
				if (deltaV[c]<-clampDeltaV)
				{
					deltaV[c]=-clampDeltaV;
				}
			}
		}
		n.m_v	+=	deltaV;
		n.m_x	+=	n.m_v*m_sst.sdt;
		n.m_f	=	btVector3(0,0,0);
	}
	/* Clusters				*/
	updateClusters();
	/* Bounds				*/
	updateBounds();
	/* Nodes				*/
	ATTRIBUTE_ALIGNED16(btDbvtVolume)	vol;
	for(i=0,ni=m_nodes.size();i<ni;++i)
	{
		Node&	n=m_nodes[i];
		vol = btDbvtVolume::FromCR(n.m_x,m_sst.radmrg);
		m_ndbvt.update(	n.m_leaf,
			vol,
			n.m_v*m_sst.velmrg,
			m_sst.updmrg);
	}
	/* Faces				*/
	if(!m_fdbvt.empty())
	{
		for(int i=0;i<m_faces.size();++i)
		{
			Face&			f=m_faces[i];
			const btVector3	v=(	f.m_n[0]->m_v+
				f.m_n[1]->m_v+
				f.m_n[2]->m_v)/3;
			vol = VolumeOf(f,m_sst.radmrg);
			m_fdbvt.update(	f.m_leaf,
				vol,
				v*m_sst.velmrg,
				m_sst.updmrg);
		}
	}
	/* Pose					*/
	updatePose();
	/* Match				*/
	if(m_pose.m_bframe&&(m_cfg.kMT>0))
	{
		const btMatrix3x3	posetrs=m_pose.m_rot;
		for(int i=0,ni=m_nodes.size();i<ni;++i)
		{
			Node&	n=m_nodes[i];
			if(n.m_im>0)
			{
				const btVector3	x=posetrs*m_pose.m_pos[i]+m_pose.m_com;
				n.m_x=Lerp(n.m_x,x,m_cfg.kMT);
			}
		}
	}
	/* Clear contacts		*/
	m_rcontacts.resize(0);
	m_scontacts.resize(0);
	/* Optimize dbvt's		*/
	m_ndbvt.optimizeIncremental(1);
	m_fdbvt.optimizeIncremental(1);
	m_cdbvt.optimizeIncremental(1);
}

//
void			btSoftBody::solveConstraints()
{

	/* Apply clusters		*/
	applyClusters(false);
	/* Prepare links		*/

	int i,ni;

	for(i=0,ni=m_links.size();i<ni;++i)
	{
		Link&	l=m_links[i];
		l.m_c3		=	l.m_n[1]->m_q-l.m_n[0]->m_q;
		l.m_c2		=	1/(l.m_c3.length2()*l.m_c0);
	}
	/* Prepare anchors		*/
	for(i=0,ni=m_anchors.size();i<ni;++i)
	{
		Anchor&			a=m_anchors[i];
		const btVector3	ra=a.m_body->getWorldTransform().getBasis()*a.m_local;
		a.m_c0	=	ImpulseMatrix(	m_sst.sdt,
			a.m_node->m_im,
			a.m_body->getInvMass(),
			a.m_body->getInvInertiaTensorWorld(),
			ra);
		a.m_c1	=	ra;
		a.m_c2	=	m_sst.sdt*a.m_node->m_im;
		a.m_body->activate();
	}
	/* Solve velocities		*/
	if(m_cfg.viterations>0)
	{
		/* Solve			*/
		for(int isolve=0;isolve<m_cfg.viterations;++isolve)
		{
			for(int iseq=0;iseq<m_cfg.m_vsequence.size();++iseq)
			{
				getSolver(m_cfg.m_vsequence[iseq])(this,1);
			}
		}
		/* Update			*/
		for(i=0,ni=m_nodes.size();i<ni;++i)
		{
			Node&	n=m_nodes[i];
			n.m_x	=	n.m_q+n.m_v*m_sst.sdt;
		}
	}
	/* Solve positions		*/
	if(m_cfg.piterations>0)
	{
		for(int isolve=0;isolve<m_cfg.piterations;++isolve)
		{
			const btScalar ti=isolve/(btScalar)m_cfg.piterations;
			for(int iseq=0;iseq<m_cfg.m_psequence.size();++iseq)
			{
				getSolver(m_cfg.m_psequence[iseq])(this,1,ti);
			}
		}
		const btScalar	vc=m_sst.isdt*(1-m_cfg.kDP);
		for(i=0,ni=m_nodes.size();i<ni;++i)
		{
			Node&	n=m_nodes[i];
			n.m_v	=	(n.m_x-n.m_q)*vc;
			n.m_f	=	btVector3(0,0,0);
		}
	}
	/* Solve drift			*/
	if(m_cfg.diterations>0)
	{
		const btScalar	vcf=m_cfg.kVCF*m_sst.isdt;
		for(i=0,ni=m_nodes.size();i<ni;++i)
		{
			Node&	n=m_nodes[i];
			n.m_q	=	n.m_x;
		}
		for(int idrift=0;idrift<m_cfg.diterations;++idrift)
		{
			for(int iseq=0;iseq<m_cfg.m_dsequence.size();++iseq)
			{
				getSolver(m_cfg.m_dsequence[iseq])(this,1,0);
			}
		}
		for(int i=0,ni=m_nodes.size();i<ni;++i)
		{
			Node&	n=m_nodes[i];
			n.m_v	+=	(n.m_x-n.m_q)*vcf;
		}
	}
	/* Apply clusters		*/
	dampClusters();
	applyClusters(true);
}

//
void			btSoftBody::staticSolve(int iterations)
{
	for(int isolve=0;isolve<iterations;++isolve)
	{
		for(int iseq=0;iseq<m_cfg.m_psequence.size();++iseq)
		{
			getSolver(m_cfg.m_psequence[iseq])(this,1,0);
		}
	}
}

//
void			btSoftBody::solveCommonConstraints(btSoftBody** /*bodies*/,int /*count*/,int /*iterations*/)
{
	/// placeholder
}

//
void			btSoftBody::solveClusters(const btAlignedObjectArray<btSoftBody*>& bodies)
{
	const int	nb=bodies.size();
	int			iterations=0;
	int i;

	for(i=0;i<nb;++i)
	{
		iterations=btMax(iterations,bodies[i]->m_cfg.citerations);
	}
	for(i=0;i<nb;++i)
	{
		bodies[i]->prepareClusters(iterations);
	}
	for(i=0;i<iterations;++i)
	{
		const btScalar sor=1;
		for(int j=0;j<nb;++j)
		{
			bodies[j]->solveClusters(sor);
		}
	}
	for(i=0;i<nb;++i)
	{
		bodies[i]->cleanupClusters();
	}
}

//
void			btSoftBody::integrateMotion()
{
	/* Update			*/
	updateNormals();
}

//
btSoftBody::RayFromToCaster::RayFromToCaster(const btVector3& rayFrom,const btVector3& rayTo,btScalar mxt)
{
	m_rayFrom = rayFrom;
	m_rayNormalizedDirection = (rayTo-rayFrom);
	m_rayTo = rayTo;
	m_mint	=	mxt;
	m_face	=	0;
	m_tests	=	0;
}

//
void				btSoftBody::RayFromToCaster::Process(const btDbvtNode* leaf)
{
	btSoftBody::Face&	f=*(btSoftBody::Face*)leaf->data;
	const btScalar		t=rayFromToTriangle(	m_rayFrom,m_rayTo,m_rayNormalizedDirection,
		f.m_n[0]->m_x,
		f.m_n[1]->m_x,
		f.m_n[2]->m_x,
		m_mint);
	if((t>0)&&(t<m_mint))
	{
		m_mint=t;m_face=&f;
	}
	++m_tests;
}

//
btScalar			btSoftBody::RayFromToCaster::rayFromToTriangle(	const btVector3& rayFrom,
																   const btVector3& rayTo,
																   const btVector3& rayNormalizedDirection,
																   const btVector3& a,
																   const btVector3& b,
																   const btVector3& c,
																   btScalar maxt)
{
	static const btScalar	ceps=-SIMD_EPSILON*10;
	static const btScalar	teps=SIMD_EPSILON*10;

	const btVector3			n=btCross(b-a,c-a);
	const btScalar			d=btDot(a,n);
	const btScalar			den=btDot(rayNormalizedDirection,n);
	if(!btFuzzyZero(den))
	{
		const btScalar		num=btDot(rayFrom,n)-d;
		const btScalar		t=-num/den;
		if((t>teps)&&(t<maxt))
		{
			const btVector3	hit=rayFrom+rayNormalizedDirection*t;
			if(	(btDot(n,btCross(a-hit,b-hit))>ceps)	&&
				(btDot(n,btCross(b-hit,c-hit))>ceps)	&&
				(btDot(n,btCross(c-hit,a-hit))>ceps))
			{
				return(t);
			}
		}
	}
	return(-1);
}

//
void				btSoftBody::pointersToIndices()
{
#define	PTR2IDX(_p_,_b_)	reinterpret_cast<btSoftBody::Node*>((_p_)-(_b_))
	btSoftBody::Node*	base=m_nodes.size() ? &m_nodes[0] : 0;
	int i,ni;

	for(i=0,ni=m_nodes.size();i<ni;++i)
	{
		if(m_nodes[i].m_leaf)
		{
			m_nodes[i].m_leaf->data=*(void**)&i;
		}
	}
	for(i=0,ni=m_links.size();i<ni;++i)
	{
		m_links[i].m_n[0]=PTR2IDX(m_links[i].m_n[0],base);
		m_links[i].m_n[1]=PTR2IDX(m_links[i].m_n[1],base);
	}
	for(i=0,ni=m_faces.size();i<ni;++i)
	{
		m_faces[i].m_n[0]=PTR2IDX(m_faces[i].m_n[0],base);
		m_faces[i].m_n[1]=PTR2IDX(m_faces[i].m_n[1],base);
		m_faces[i].m_n[2]=PTR2IDX(m_faces[i].m_n[2],base);
		if(m_faces[i].m_leaf)
		{
			m_faces[i].m_leaf->data=*(void**)&i;
		}
	}
	for(i=0,ni=m_anchors.size();i<ni;++i)
	{
		m_anchors[i].m_node=PTR2IDX(m_anchors[i].m_node,base);
	}
	for(i=0,ni=m_notes.size();i<ni;++i)
	{
		for(int j=0;j<m_notes[i].m_rank;++j)
		{
			m_notes[i].m_nodes[j]=PTR2IDX(m_notes[i].m_nodes[j],base);
		}
	}
#undef	PTR2IDX
}

//
void				btSoftBody::indicesToPointers(const int* map)
{
#define	IDX2PTR(_p_,_b_)	map?(&(_b_)[map[(((char*)_p_)-(char*)0)]]):	\
	(&(_b_)[(((char*)_p_)-(char*)0)])
	btSoftBody::Node*	base=m_nodes.size() ? &m_nodes[0]:0;
	int i,ni;

	for(i=0,ni=m_nodes.size();i<ni;++i)
	{
		if(m_nodes[i].m_leaf)
		{
			m_nodes[i].m_leaf->data=&m_nodes[i];
		}
	}
	for(i=0,ni=m_links.size();i<ni;++i)
	{
		m_links[i].m_n[0]=IDX2PTR(m_links[i].m_n[0],base);
		m_links[i].m_n[1]=IDX2PTR(m_links[i].m_n[1],base);
	}
	for(i=0,ni=m_faces.size();i<ni;++i)
	{
		m_faces[i].m_n[0]=IDX2PTR(m_faces[i].m_n[0],base);
		m_faces[i].m_n[1]=IDX2PTR(m_faces[i].m_n[1],base);
		m_faces[i].m_n[2]=IDX2PTR(m_faces[i].m_n[2],base);
		if(m_faces[i].m_leaf)
		{
			m_faces[i].m_leaf->data=&m_faces[i];
		}
	}
	for(i=0,ni=m_anchors.size();i<ni;++i)
	{
		m_anchors[i].m_node=IDX2PTR(m_anchors[i].m_node,base);
	}
	for(i=0,ni=m_notes.size();i<ni;++i)
	{
		for(int j=0;j<m_notes[i].m_rank;++j)
		{
			m_notes[i].m_nodes[j]=IDX2PTR(m_notes[i].m_nodes[j],base);
		}
	}
#undef	IDX2PTR
}

//
int					btSoftBody::rayTest(const btVector3& rayFrom,const btVector3& rayTo,
										btScalar& mint,eFeature::_& feature,int& index,bool bcountonly) const
{
	int	cnt=0;
	btVector3 dir = rayTo-rayFrom;


	if(bcountonly||m_fdbvt.empty())
	{/* Full search	*/

		for(int i=0,ni=m_faces.size();i<ni;++i)
		{
			const btSoftBody::Face&	f=m_faces[i];

			const btScalar			t=RayFromToCaster::rayFromToTriangle(	rayFrom,rayTo,dir,
				f.m_n[0]->m_x,
				f.m_n[1]->m_x,
				f.m_n[2]->m_x,
				mint);
			if(t>0)
			{
				++cnt;
				if(!bcountonly)
				{
					feature=btSoftBody::eFeature::Face;
					index=i;
					mint=t;
				}
			}
		}
	}
	else
	{/* Use dbvt	*/
		RayFromToCaster	collider(rayFrom,rayTo,mint);

		btDbvt::rayTest(m_fdbvt.m_root,rayFrom,rayTo,collider);
		if(collider.m_face)
		{
			mint=collider.m_mint;
			feature=btSoftBody::eFeature::Face;
			index=(int)(collider.m_face-&m_faces[0]);
			cnt=1;
		}
	}

	for (int i=0;i<m_tetras.size();i++)
	{
		const btSoftBody::Tetra& tet = m_tetras[i];
		int tetfaces[4][3] = {{0,1,2},{0,1,3},{1,2,3},{0,2,3}};
		for (int f=0;f<4;f++)
		{

			int index0=tetfaces[f][0];
			int index1=tetfaces[f][1];
			int index2=tetfaces[f][2];
			btVector3 v0=tet.m_n[index0]->m_x;
			btVector3 v1=tet.m_n[index1]->m_x;
			btVector3 v2=tet.m_n[index2]->m_x;


		const btScalar			t=RayFromToCaster::rayFromToTriangle(	rayFrom,rayTo,dir,
			v0,v1,v2,
				mint);
		if(t>0)
			{
				++cnt;
				if(!bcountonly)
				{
					feature=btSoftBody::eFeature::Tetra;
					index=i;
					mint=t;
				}
			}
		}
	}
	return(cnt);
}

//
void			btSoftBody::initializeFaceTree()
{
	m_fdbvt.clear();
	for(int i=0;i<m_faces.size();++i)
	{
		Face&	f=m_faces[i];
		f.m_leaf=m_fdbvt.insert(VolumeOf(f,0),&f);
	}
}

//
btVector3		btSoftBody::evaluateCom() const
{
	btVector3	com(0,0,0);
	if(m_pose.m_bframe)
	{
		for(int i=0,ni=m_nodes.size();i<ni;++i)
		{
			com+=m_nodes[i].m_x*m_pose.m_wgh[i];
		}
	}
	return(com);
}

//
bool				btSoftBody::checkContact(	const btCollisionObjectWrapper* colObjWrap,
											 const btVector3& x,
											 btScalar margin,
											 btSoftBody::sCti& cti) const
{
	btVector3 nrm;
	const btCollisionShape *shp = colObjWrap->getCollisionShape();
//	const btRigidBody *tmpRigid = btRigidBody::upcast(colObjWrap->getCollisionObject());
	//const btTransform &wtr = tmpRigid ? tmpRigid->getWorldTransform() : colObjWrap->getWorldTransform();
	const btTransform &wtr = colObjWrap->getWorldTransform();
	//todo: check which transform is needed here

	btScalar dst =
		m_worldInfo->m_sparsesdf.Evaluate(
			wtr.invXform(x),
			shp,
			nrm,
			margin);
	if(dst<0)
	{
		cti.m_colObj = colObjWrap->getCollisionObject();
		cti.m_normal = wtr.getBasis()*nrm;
		cti.m_offset = -btDot( cti.m_normal, x - cti.m_normal * dst );
		return(true);
	}
	return(false);
}

//
void					btSoftBody::updateNormals()
{

	const btVector3	zv(0,0,0);
	int i,ni;

	for(i=0,ni=m_nodes.size();i<ni;++i)
	{
		m_nodes[i].m_n=zv;
	}
	for(i=0,ni=m_faces.size();i<ni;++i)
	{
		btSoftBody::Face&	f=m_faces[i];
		const 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);
		f.m_normal=n.normalized();
		f.m_n[0]->m_n+=n;
		f.m_n[1]->m_n+=n;
		f.m_n[2]->m_n+=n;
	}
	for(i=0,ni=m_nodes.size();i<ni;++i)
	{
		btScalar len = m_nodes[i].m_n.length();
		if (len>SIMD_EPSILON)
			m_nodes[i].m_n /= len;
	}
}

//
void					btSoftBody::updateBounds()
{
	/*if( m_acceleratedSoftBody )
	{
		// If we have an accelerated softbody we need to obtain the bounds correctly
		// For now (slightly hackily) just have a very large AABB
		// TODO: Write get bounds kernel
		// If that is updating in place, atomic collisions might be low (when the cloth isn't perfectly aligned to an axis) and we could
		// probably do a test and exchange reasonably efficiently.

		m_bounds[0] = btVector3(-1000, -1000, -1000);
		m_bounds[1] = btVector3(1000, 1000, 1000);

	} else {*/
		if(m_ndbvt.m_root)
		{
			const btVector3&	mins=m_ndbvt.m_root->volume.Mins();
			const btVector3&	maxs=m_ndbvt.m_root->volume.Maxs();
			const btScalar		csm=getCollisionShape()->getMargin();
			const btVector3		mrg=btVector3(	csm,
				csm,
				csm)*1; // ??? to investigate...
			m_bounds[0]=mins-mrg;
			m_bounds[1]=maxs+mrg;
			if(0!=getBroadphaseHandle())
			{
				m_worldInfo->m_broadphase->setAabb(	getBroadphaseHandle(),
					m_bounds[0],
					m_bounds[1],
					m_worldInfo->m_dispatcher);
			}
		}
		else
		{
			m_bounds[0]=
				m_bounds[1]=btVector3(0,0,0);
		}
	//}
}


//
void					btSoftBody::updatePose()
{
	if(m_pose.m_bframe)
	{
		btSoftBody::Pose&	pose=m_pose;
		const btVector3		com=evaluateCom();
		/* Com			*/
		pose.m_com	=	com;
		/* Rotation		*/
		btMatrix3x3		Apq;
		const btScalar	eps=SIMD_EPSILON;
		Apq[0]=Apq[1]=Apq[2]=btVector3(0,0,0);
		Apq[0].setX(eps);Apq[1].setY(eps*2);Apq[2].setZ(eps*3);
		for(int i=0,ni=m_nodes.size();i<ni;++i)
		{
			const btVector3		a=pose.m_wgh[i]*(m_nodes[i].m_x-com);
			const btVector3&	b=pose.m_pos[i];
			Apq[0]+=a.x()*b;
			Apq[1]+=a.y()*b;
			Apq[2]+=a.z()*b;
		}
		btMatrix3x3		r,s;
		PolarDecompose(Apq,r,s);
		pose.m_rot=r;
		pose.m_scl=pose.m_aqq*r.transpose()*Apq;
		if(m_cfg.maxvolume>1)
		{
			const btScalar	idet=Clamp<btScalar>(	1/pose.m_scl.determinant(),
				1,m_cfg.maxvolume);
			pose.m_scl=Mul(pose.m_scl,idet);
		}

	}
}

//
void				btSoftBody::updateArea(bool averageArea)
{
	int i,ni;

	/* Face area		*/
	for(i=0,ni=m_faces.size();i<ni;++i)
	{
		Face&		f=m_faces[i];
		f.m_ra	=	AreaOf(f.m_n[0]->m_x,f.m_n[1]->m_x,f.m_n[2]->m_x);
	}

	/* Node area		*/

	if (averageArea)
	{
		btAlignedObjectArray<int>	counts;
		counts.resize(m_nodes.size(),0);
		for(i=0,ni=m_nodes.size();i<ni;++i)
		{
			m_nodes[i].m_area	=	0;
		}
		for(i=0,ni=m_faces.size();i<ni;++i)
		{
			btSoftBody::Face&	f=m_faces[i];
			for(int j=0;j<3;++j)
			{
				const int index=(int)(f.m_n[j]-&m_nodes[0]);
				counts[index]++;
				f.m_n[j]->m_area+=btFabs(f.m_ra);
			}
		}
		for(i=0,ni=m_nodes.size();i<ni;++i)
		{
			if(counts[i]>0)
				m_nodes[i].m_area/=(btScalar)counts[i];
			else
				m_nodes[i].m_area=0;
		}
	}
	else
	{
		// initialize node area as zero
		for(i=0,ni=m_nodes.size();i<ni;++i)
		{
			m_nodes[i].m_area=0;
		}

		for(i=0,ni=m_faces.size();i<ni;++i)
		{
			btSoftBody::Face&	f=m_faces[i];

			for(int j=0;j<3;++j)
			{
				f.m_n[j]->m_area += f.m_ra;
			}
		}

		for(i=0,ni=m_nodes.size();i<ni;++i)
		{
			m_nodes[i].m_area *= 0.3333333f;
		}
	}
}


void				btSoftBody::updateLinkConstants()
{
	int i,ni;

	/* Links		*/
	for(i=0,ni=m_links.size();i<ni;++i)
	{
		Link&		l=m_links[i];
		Material&	m=*l.m_material;
		l.m_c0	=	(l.m_n[0]->m_im+l.m_n[1]->m_im)/m.m_kLST;
	}
}

void				btSoftBody::updateConstants()
{
	resetLinkRestLengths();
	updateLinkConstants();
	updateArea();
}


//
void					btSoftBody::initializeClusters()
{
	int i;

	for( i=0;i<m_clusters.size();++i)
	{
		Cluster&	c=*m_clusters[i];
		c.m_imass=0;
		c.m_masses.resize(c.m_nodes.size());
		for(int j=0;j<c.m_nodes.size();++j)
		{
			if (c.m_nodes[j]->m_im==0)
			{
				c.m_containsAnchor = true;
				c.m_masses[j]	=	BT_LARGE_FLOAT;
			} else
			{
				c.m_masses[j]	=	btScalar(1.)/c.m_nodes[j]->m_im;
			}
			c.m_imass		+=	c.m_masses[j];
		}
		c.m_imass		=	btScalar(1.)/c.m_imass;
		c.m_com			=	btSoftBody::clusterCom(&c);
		c.m_lv			=	btVector3(0,0,0);
		c.m_av			=	btVector3(0,0,0);
		c.m_leaf		=	0;
		/* Inertia	*/
		btMatrix3x3&	ii=c.m_locii;
		ii[0]=ii[1]=ii[2]=btVector3(0,0,0);
		{
			int i,ni;

			for(i=0,ni=c.m_nodes.size();i<ni;++i)
			{
				const btVector3	k=c.m_nodes[i]->m_x-c.m_com;
				const btVector3	q=k*k;
				const btScalar	m=c.m_masses[i];
				ii[0][0]	+=	m*(q[1]+q[2]);
				ii[1][1]	+=	m*(q[0]+q[2]);
				ii[2][2]	+=	m*(q[0]+q[1]);
				ii[0][1]	-=	m*k[0]*k[1];
				ii[0][2]	-=	m*k[0]*k[2];
				ii[1][2]	-=	m*k[1]*k[2];
			}
		}
		ii[1][0]=ii[0][1];
		ii[2][0]=ii[0][2];
		ii[2][1]=ii[1][2];

		ii = ii.inverse();

		/* Frame	*/
		c.m_framexform.setIdentity();
		c.m_framexform.setOrigin(c.m_com);
		c.m_framerefs.resize(c.m_nodes.size());
		{
			int i;
			for(i=0;i<c.m_framerefs.size();++i)
			{
				c.m_framerefs[i]=c.m_nodes[i]->m_x-c.m_com;
			}
		}
	}
}

//
void					btSoftBody::updateClusters()
{
	BT_PROFILE("UpdateClusters");
	int i;

	for(i=0;i<m_clusters.size();++i)
	{
		btSoftBody::Cluster&	c=*m_clusters[i];
		const int				n=c.m_nodes.size();
		//const btScalar			invn=1/(btScalar)n;
		if(n)
		{
			/* Frame				*/
			const btScalar	eps=btScalar(0.0001);
			btMatrix3x3		m,r,s;
			m[0]=m[1]=m[2]=btVector3(0,0,0);
			m[0][0]=eps*1;
			m[1][1]=eps*2;
			m[2][2]=eps*3;
			c.m_com=clusterCom(&c);
			for(int i=0;i<c.m_nodes.size();++i)
			{
				const btVector3		a=c.m_nodes[i]->m_x-c.m_com;
				const btVector3&	b=c.m_framerefs[i];
				m[0]+=a[0]*b;m[1]+=a[1]*b;m[2]+=a[2]*b;
			}
			PolarDecompose(m,r,s);
			c.m_framexform.setOrigin(c.m_com);
			c.m_framexform.setBasis(r);
			/* Inertia			*/
#if 1/* Constant	*/
			c.m_invwi=c.m_framexform.getBasis()*c.m_locii*c.m_framexform.getBasis().transpose();
#else
#if 0/* Sphere	*/
			const btScalar	rk=(2*c.m_extents.length2())/(5*c.m_imass);
			const btVector3	inertia(rk,rk,rk);
			const btVector3	iin(btFabs(inertia[0])>SIMD_EPSILON?1/inertia[0]:0,
				btFabs(inertia[1])>SIMD_EPSILON?1/inertia[1]:0,
				btFabs(inertia[2])>SIMD_EPSILON?1/inertia[2]:0);

			c.m_invwi=c.m_xform.getBasis().scaled(iin)*c.m_xform.getBasis().transpose();
#else/* Actual	*/
			c.m_invwi[0]=c.m_invwi[1]=c.m_invwi[2]=btVector3(0,0,0);
			for(int i=0;i<n;++i)
			{
				const btVector3	k=c.m_nodes[i]->m_x-c.m_com;
				const btVector3		q=k*k;
				const btScalar		m=1/c.m_nodes[i]->m_im;
				c.m_invwi[0][0]	+=	m*(q[1]+q[2]);
				c.m_invwi[1][1]	+=	m*(q[0]+q[2]);
				c.m_invwi[2][2]	+=	m*(q[0]+q[1]);
				c.m_invwi[0][1]	-=	m*k[0]*k[1];
				c.m_invwi[0][2]	-=	m*k[0]*k[2];
				c.m_invwi[1][2]	-=	m*k[1]*k[2];
			}
			c.m_invwi[1][0]=c.m_invwi[0][1];
			c.m_invwi[2][0]=c.m_invwi[0][2];
			c.m_invwi[2][1]=c.m_invwi[1][2];
			c.m_invwi=c.m_invwi.inverse();
#endif
#endif
			/* Velocities			*/
			c.m_lv=btVector3(0,0,0);
			c.m_av=btVector3(0,0,0);
			{
				int i;

				for(i=0;i<n;++i)
				{
					const btVector3	v=c.m_nodes[i]->m_v*c.m_masses[i];
					c.m_lv	+=	v;
					c.m_av	+=	btCross(c.m_nodes[i]->m_x-c.m_com,v);
				}
			}
			c.m_lv=c.m_imass*c.m_lv*(1-c.m_ldamping);
			c.m_av=c.m_invwi*c.m_av*(1-c.m_adamping);
			c.m_vimpulses[0]	=
				c.m_vimpulses[1]	= btVector3(0,0,0);
			c.m_dimpulses[0]	=
				c.m_dimpulses[1]	= btVector3(0,0,0);
			c.m_nvimpulses		= 0;
			c.m_ndimpulses		= 0;
			/* Matching				*/
			if(c.m_matching>0)
			{
				for(int j=0;j<c.m_nodes.size();++j)
				{
					Node&			n=*c.m_nodes[j];
					const btVector3	x=c.m_framexform*c.m_framerefs[j];
					n.m_x=Lerp(n.m_x,x,c.m_matching);
				}
			}
			/* Dbvt					*/
			if(c.m_collide)
			{
				btVector3	mi=c.m_nodes[0]->m_x;
				btVector3	mx=mi;
				for(int j=1;j<n;++j)
				{
					mi.setMin(c.m_nodes[j]->m_x);
					mx.setMax(c.m_nodes[j]->m_x);
				}
				ATTRIBUTE_ALIGNED16(btDbvtVolume)	bounds=btDbvtVolume::FromMM(mi,mx);
				if(c.m_leaf)
					m_cdbvt.update(c.m_leaf,bounds,c.m_lv*m_sst.sdt*3,m_sst.radmrg);
				else
					c.m_leaf=m_cdbvt.insert(bounds,&c);
			}
		}
	}


}


//
void					btSoftBody::cleanupClusters()
{
	for(int i=0;i<m_joints.size();++i)
	{
		m_joints[i]->Terminate(m_sst.sdt);
		if(m_joints[i]->m_delete)
		{
			btAlignedFree(m_joints[i]);
			m_joints.remove(m_joints[i--]);
		}
	}
}

//
void					btSoftBody::prepareClusters(int iterations)
{
	for(int i=0;i<m_joints.size();++i)
	{
		m_joints[i]->Prepare(m_sst.sdt,iterations);
	}
}


//
void					btSoftBody::solveClusters(btScalar sor)
{
	for(int i=0,ni=m_joints.size();i<ni;++i)
	{
		m_joints[i]->Solve(m_sst.sdt,sor);
	}
}

//
void					btSoftBody::applyClusters(bool drift)
{
	BT_PROFILE("ApplyClusters");
//	const btScalar					f0=m_sst.sdt;
	//const btScalar					f1=f0/2;
	btAlignedObjectArray<btVector3> deltas;
	btAlignedObjectArray<btScalar> weights;
	deltas.resize(m_nodes.size(),btVector3(0,0,0));
	weights.resize(m_nodes.size(),0);
	int i;

	if(drift)
	{
		for(i=0;i<m_clusters.size();++i)
		{
			Cluster&	c=*m_clusters[i];
			if(c.m_ndimpulses)
			{
				c.m_dimpulses[0]/=(btScalar)c.m_ndimpulses;
				c.m_dimpulses[1]/=(btScalar)c.m_ndimpulses;
			}
		}
	}

	for(i=0;i<m_clusters.size();++i)
	{
		Cluster&	c=*m_clusters[i];
		if(0<(drift?c.m_ndimpulses:c.m_nvimpulses))
		{
			const btVector3		v=(drift?c.m_dimpulses[0]:c.m_vimpulses[0])*m_sst.sdt;
			const btVector3		w=(drift?c.m_dimpulses[1]:c.m_vimpulses[1])*m_sst.sdt;
			for(int j=0;j<c.m_nodes.size();++j)
			{
				const int			idx=int(c.m_nodes[j]-&m_nodes[0]);
				const btVector3&	x=c.m_nodes[j]->m_x;
				const btScalar		q=c.m_masses[j];
				deltas[idx]		+=	(v+btCross(w,x-c.m_com))*q;
				weights[idx]	+=	q;
			}
		}
	}
	for(i=0;i<deltas.size();++i)
	{
		if(weights[i]>0)
		{
			m_nodes[i].m_x+=deltas[i]/weights[i];
		}
	}
}

//
void					btSoftBody::dampClusters()
{
	int i;

	for(i=0;i<m_clusters.size();++i)
	{
		Cluster&	c=*m_clusters[i];
		if(c.m_ndamping>0)
		{
			for(int j=0;j<c.m_nodes.size();++j)
			{
				Node&			n=*c.m_nodes[j];
				if(n.m_im>0)
				{
					const btVector3	vx=c.m_lv+btCross(c.m_av,c.m_nodes[j]->m_q-c.m_com);
					if(vx.length2()<=n.m_v.length2())
						{
						n.m_v	+=	c.m_ndamping*(vx-n.m_v);
						}
				}
			}
		}
	}
}

//
void				btSoftBody::Joint::Prepare(btScalar dt,int)
{
	m_bodies[0].activate();
	m_bodies[1].activate();
}

//
void				btSoftBody::LJoint::Prepare(btScalar dt,int iterations)
{
	static const btScalar	maxdrift=4;
	Joint::Prepare(dt,iterations);
	m_rpos[0]		=	m_bodies[0].xform()*m_refs[0];
	m_rpos[1]		=	m_bodies[1].xform()*m_refs[1];
	m_drift			=	Clamp(m_rpos[0]-m_rpos[1],maxdrift)*m_erp/dt;
	m_rpos[0]		-=	m_bodies[0].xform().getOrigin();
	m_rpos[1]		-=	m_bodies[1].xform().getOrigin();
	m_massmatrix	=	ImpulseMatrix(	m_bodies[0].invMass(),m_bodies[0].invWorldInertia(),m_rpos[0],
		m_bodies[1].invMass(),m_bodies[1].invWorldInertia(),m_rpos[1]);
	if(m_split>0)
	{
		m_sdrift	=	m_massmatrix*(m_drift*m_split);
		m_drift		*=	1-m_split;
	}
	m_drift	/=(btScalar)iterations;
}

//
void				btSoftBody::LJoint::Solve(btScalar dt,btScalar sor)
{
	const btVector3		va=m_bodies[0].velocity(m_rpos[0]);
	const btVector3		vb=m_bodies[1].velocity(m_rpos[1]);
	const btVector3		vr=va-vb;
	btSoftBody::Impulse	impulse;
	impulse.m_asVelocity	=	1;
	impulse.m_velocity		=	m_massmatrix*(m_drift+vr*m_cfm)*sor;
	m_bodies[0].applyImpulse(-impulse,m_rpos[0]);
	m_bodies[1].applyImpulse( impulse,m_rpos[1]);
}

//
void				btSoftBody::LJoint::Terminate(btScalar dt)
{
	if(m_split>0)
	{
		m_bodies[0].applyDImpulse(-m_sdrift,m_rpos[0]);
		m_bodies[1].applyDImpulse( m_sdrift,m_rpos[1]);
	}
}

//
void				btSoftBody::AJoint::Prepare(btScalar dt,int iterations)
{
	static const btScalar	maxdrift=SIMD_PI/16;
	m_icontrol->Prepare(this);
	Joint::Prepare(dt,iterations);
	m_axis[0]	=	m_bodies[0].xform().getBasis()*m_refs[0];
	m_axis[1]	=	m_bodies[1].xform().getBasis()*m_refs[1];
	m_drift		=	NormalizeAny(btCross(m_axis[1],m_axis[0]));
	m_drift		*=	btMin(maxdrift,btAcos(Clamp<btScalar>(btDot(m_axis[0],m_axis[1]),-1,+1)));
	m_drift		*=	m_erp/dt;
	m_massmatrix=	AngularImpulseMatrix(m_bodies[0].invWorldInertia(),m_bodies[1].invWorldInertia());
	if(m_split>0)
	{
		m_sdrift	=	m_massmatrix*(m_drift*m_split);
		m_drift		*=	1-m_split;
	}
	m_drift	/=(btScalar)iterations;
}

//
void				btSoftBody::AJoint::Solve(btScalar dt,btScalar sor)
{
	const btVector3		va=m_bodies[0].angularVelocity();
	const btVector3		vb=m_bodies[1].angularVelocity();
	const btVector3		vr=va-vb;
	const btScalar		sp=btDot(vr,m_axis[0]);
	const btVector3		vc=vr-m_axis[0]*m_icontrol->Speed(this,sp);
	btSoftBody::Impulse	impulse;
	impulse.m_asVelocity	=	1;
	impulse.m_velocity		=	m_massmatrix*(m_drift+vc*m_cfm)*sor;
	m_bodies[0].applyAImpulse(-impulse);
	m_bodies[1].applyAImpulse( impulse);
}

//
void				btSoftBody::AJoint::Terminate(btScalar dt)
{
	if(m_split>0)
	{
		m_bodies[0].applyDAImpulse(-m_sdrift);
		m_bodies[1].applyDAImpulse( m_sdrift);
	}
}

//
void				btSoftBody::CJoint::Prepare(btScalar dt,int iterations)
{
	Joint::Prepare(dt,iterations);
	const bool	dodrift=(m_life==0);
	m_delete=(++m_life)>m_maxlife;
	if(dodrift)
	{
		m_drift=m_drift*m_erp/dt;
		if(m_split>0)
		{
			m_sdrift	=	m_massmatrix*(m_drift*m_split);
			m_drift		*=	1-m_split;
		}
		m_drift/=(btScalar)iterations;
	}
	else
	{
		m_drift=m_sdrift=btVector3(0,0,0);
	}
}

//
void				btSoftBody::CJoint::Solve(btScalar dt,btScalar sor)
{
	const btVector3		va=m_bodies[0].velocity(m_rpos[0]);
	const btVector3		vb=m_bodies[1].velocity(m_rpos[1]);
	const btVector3		vrel=va-vb;
	const btScalar		rvac=btDot(vrel,m_normal);
	btSoftBody::Impulse	impulse;
	impulse.m_asVelocity	=	1;
	impulse.m_velocity		=	m_drift;
	if(rvac<0)
	{
		const btVector3	iv=m_normal*rvac;
		const btVector3	fv=vrel-iv;
		impulse.m_velocity	+=	iv+fv*m_friction;
	}
	impulse.m_velocity=m_massmatrix*impulse.m_velocity*sor;

	if (m_bodies[0].m_soft==m_bodies[1].m_soft)
	{
		if ((impulse.m_velocity.getX() ==impulse.m_velocity.getX())&&(impulse.m_velocity.getY() ==impulse.m_velocity.getY())&&
			(impulse.m_velocity.getZ() ==impulse.m_velocity.getZ()))
		{
			if (impulse.m_asVelocity)
			{
				if (impulse.m_velocity.length() <m_bodies[0].m_soft->m_maxSelfCollisionImpulse)
				{

				} else
				{
					m_bodies[0].applyImpulse(-impulse*m_bodies[0].m_soft->m_selfCollisionImpulseFactor,m_rpos[0]);
					m_bodies[1].applyImpulse( impulse*m_bodies[0].m_soft->m_selfCollisionImpulseFactor,m_rpos[1]);
				}
			}
		}
	} else
	{
		m_bodies[0].applyImpulse(-impulse,m_rpos[0]);
		m_bodies[1].applyImpulse( impulse,m_rpos[1]);
	}
}

//
void				btSoftBody::CJoint::Terminate(btScalar dt)
{
	if(m_split>0)
	{
		m_bodies[0].applyDImpulse(-m_sdrift,m_rpos[0]);
		m_bodies[1].applyDImpulse( m_sdrift,m_rpos[1]);
	}
}

//
void				btSoftBody::applyForces()
{

	BT_PROFILE("SoftBody applyForces");
//	const btScalar					dt =			m_sst.sdt;
	const btScalar					kLF =			m_cfg.kLF;
	const btScalar					kDG =			m_cfg.kDG;
	const btScalar					kPR =			m_cfg.kPR;
	const btScalar					kVC =			m_cfg.kVC;
	const bool						as_lift =		kLF>0;
	const bool						as_drag =		kDG>0;
	const bool						as_pressure =	kPR!=0;
	const bool						as_volume =		kVC>0;
	const bool						as_aero =		as_lift	||
													as_drag		;
	//const bool						as_vaero =		as_aero	&&
	//												(m_cfg.aeromodel < btSoftBody::eAeroModel::F_TwoSided);
	//const bool						as_faero =		as_aero	&&
	//												(m_cfg.aeromodel >= btSoftBody::eAeroModel::F_TwoSided);
	const bool						use_medium =	as_aero;
	const bool						use_volume =	as_pressure	||
		as_volume	;
	btScalar						volume =		0;
	btScalar						ivolumetp =		0;
	btScalar						dvolumetv =		0;
	btSoftBody::sMedium	medium;
	if(use_volume)
	{
		volume		=	getVolume();
		ivolumetp	=	1/btFabs(volume)*kPR;
		dvolumetv	=	(m_pose.m_volume-volume)*kVC;
	}
	/* Per vertex forces			*/
	int i,ni;

	for(i=0,ni=m_nodes.size();i<ni;++i)
	{
		btSoftBody::Node&	n=m_nodes[i];
		if(n.m_im>0)
		{
			if(use_medium)
			{
				/* Aerodynamics			*/
				addAeroForceToNode(m_windVelocity, i);
			}
			/* Pressure				*/
			if(as_pressure)
			{
				n.m_f	+=	n.m_n*(n.m_area*ivolumetp);
			}
			/* Volume				*/
			if(as_volume)
			{
				n.m_f	+=	n.m_n*(n.m_area*dvolumetv);
			}
		}
	}

	/* Per face forces				*/
	for(i=0,ni=m_faces.size();i<ni;++i)
	{
	//	btSoftBody::Face&	f=m_faces[i];

		/* Aerodynamics			*/
		addAeroForceToFace(m_windVelocity, i);
	}
}

//
void				btSoftBody::PSolve_Anchors(btSoftBody* psb,btScalar kst,btScalar ti)
{
	const btScalar	kAHR=psb->m_cfg.kAHR*kst;
	const btScalar	dt=psb->m_sst.sdt;
	for(int i=0,ni=psb->m_anchors.size();i<ni;++i)
	{
		const Anchor&		a=psb->m_anchors[i];
		const btTransform&	t=a.m_body->getWorldTransform();
		Node&				n=*a.m_node;
		const btVector3		wa=t*a.m_local;
		const btVector3		va=a.m_body->getVelocityInLocalPoint(a.m_c1)*dt;
		const btVector3		vb=n.m_x-n.m_q;
		const btVector3		vr=(va-vb)+(wa-n.m_x)*kAHR;
		const btVector3		impulse=a.m_c0*vr*a.m_influence;
		n.m_x+=impulse*a.m_c2;
		a.m_body->applyImpulse(-impulse,a.m_c1);
	}
}


//
void btSoftBody::PSolve_RContacts(btSoftBody* psb, btScalar kst, btScalar ti)
{
	const btScalar	dt = psb->m_sst.sdt;
	const btScalar	mrg = psb->getCollisionShape()->getMargin();
	for(int i=0,ni=psb->m_rcontacts.size();i<ni;++i)
	{
		const RContact&		c = psb->m_rcontacts[i];
		const sCti&			cti = c.m_cti;
		if (cti.m_colObj->hasContactResponse())
		{
            btVector3 va(0,0,0);
            btRigidBody* rigidCol;
            btMultiBodyLinkCollider* multibodyLinkCol;
            btScalar* deltaV;
            btMultiBodyJacobianData jacobianData;
            if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY)
            {
                rigidCol = (btRigidBody*)btRigidBody::upcast(cti.m_colObj);
                va = rigidCol ? rigidCol->getVelocityInLocalPoint(c.m_c1)*dt : btVector3(0,0,0);
            }
			else if (cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK)
            {
                multibodyLinkCol = (btMultiBodyLinkCollider*)btMultiBodyLinkCollider::upcast(cti.m_colObj);
                if (multibodyLinkCol)
                {
                    const int ndof  = multibodyLinkCol->m_multiBody->getNumDofs() + 6;
                    jacobianData.m_jacobians.resize(ndof);
                    jacobianData.m_deltaVelocitiesUnitImpulse.resize(ndof);
                    btScalar* jac=&jacobianData.m_jacobians[0];

                    multibodyLinkCol->m_multiBody->fillContactJacobianMultiDof(multibodyLinkCol->m_link, c.m_node->m_x, cti.m_normal, jac, jacobianData.scratch_r, jacobianData.scratch_v, jacobianData.scratch_m);
                    deltaV = &jacobianData.m_deltaVelocitiesUnitImpulse[0];
                    multibodyLinkCol->m_multiBody->calcAccelerationDeltasMultiDof(&jacobianData.m_jacobians[0],deltaV,jacobianData.scratch_r, jacobianData.scratch_v);

                    btScalar vel = 0.0;
                    for (int j = 0; j < ndof ; ++j) {
                        vel += multibodyLinkCol->m_multiBody->getVelocityVector()[j] * jac[j];
                    }
                    va = cti.m_normal*vel*dt;
                }
            }

			const btVector3		vb = c.m_node->m_x-c.m_node->m_q;
			const btVector3		vr = vb-va;
			const btScalar		dn = btDot(vr, cti.m_normal);
			if(dn<=SIMD_EPSILON)
			{
				const btScalar		dp = btMin( (btDot(c.m_node->m_x, cti.m_normal) + cti.m_offset), mrg );
				const btVector3		fv = vr - (cti.m_normal * dn);
				// c0 is the impulse matrix, c3 is 1 - the friction coefficient or 0, c4 is the contact hardness coefficient
				const btVector3		impulse = c.m_c0 * ( (vr - (fv * c.m_c3) + (cti.m_normal * (dp * c.m_c4))) * kst );
				c.m_node->m_x -= impulse * c.m_c2;

                if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY)
                {
                    if (rigidCol)
                        rigidCol->applyImpulse(impulse,c.m_c1);
                }
                else if (cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK)
                {
                    if (multibodyLinkCol)
                    {
                        double multiplier = 0.5;
                        multibodyLinkCol->m_multiBody->applyDeltaVeeMultiDof(deltaV,-impulse.length()*multiplier);
                    }
                }
			}
		}
	}
}

//
void				btSoftBody::PSolve_SContacts(btSoftBody* psb,btScalar,btScalar ti)
{
	for(int i=0,ni=psb->m_scontacts.size();i<ni;++i)
	{
		const SContact&		c=psb->m_scontacts[i];
		const btVector3&	nr=c.m_normal;
		Node&				n=*c.m_node;
		Face&				f=*c.m_face;
		const btVector3		p=BaryEval(	f.m_n[0]->m_x,
			f.m_n[1]->m_x,
			f.m_n[2]->m_x,
			c.m_weights);
		const btVector3		q=BaryEval(	f.m_n[0]->m_q,
			f.m_n[1]->m_q,
			f.m_n[2]->m_q,
			c.m_weights);
		const btVector3		vr=(n.m_x-n.m_q)-(p-q);
		btVector3			corr(0,0,0);
		btScalar dot = btDot(vr,nr);
		if(dot<0)
		{
			const btScalar	j=c.m_margin-(btDot(nr,n.m_x)-btDot(nr,p));
			corr+=c.m_normal*j;
		}
		corr			-=	ProjectOnPlane(vr,nr)*c.m_friction;
		n.m_x			+=	corr*c.m_cfm[0];
		f.m_n[0]->m_x	-=	corr*(c.m_cfm[1]*c.m_weights.x());
		f.m_n[1]->m_x	-=	corr*(c.m_cfm[1]*c.m_weights.y());
		f.m_n[2]->m_x	-=	corr*(c.m_cfm[1]*c.m_weights.z());
	}
}

//
void				btSoftBody::PSolve_Links(btSoftBody* psb,btScalar kst,btScalar ti)
{
	for(int i=0,ni=psb->m_links.size();i<ni;++i)
	{
		Link&	l=psb->m_links[i];
		if(l.m_c0>0)
		{
			Node&			a=*l.m_n[0];
			Node&			b=*l.m_n[1];
			const btVector3	del=b.m_x-a.m_x;
			const btScalar	len=del.length2();
			if (l.m_c1+len > SIMD_EPSILON)
			{
				const btScalar	k=((l.m_c1-len)/(l.m_c0*(l.m_c1+len)))*kst;
				a.m_x-=del*(k*a.m_im);
				b.m_x+=del*(k*b.m_im);
			}
		}
	}
}

//
void				btSoftBody::VSolve_Links(btSoftBody* psb,btScalar kst)
{
	for(int i=0,ni=psb->m_links.size();i<ni;++i)
	{
		Link&			l=psb->m_links[i];
		Node**			n=l.m_n;
		const btScalar	j=-btDot(l.m_c3,n[0]->m_v-n[1]->m_v)*l.m_c2*kst;
		n[0]->m_v+=	l.m_c3*(j*n[0]->m_im);
		n[1]->m_v-=	l.m_c3*(j*n[1]->m_im);
	}
}

//
btSoftBody::psolver_t	btSoftBody::getSolver(ePSolver::_ solver)
{
	switch(solver)
	{
	case	ePSolver::Anchors:
		return(&btSoftBody::PSolve_Anchors);
	case	ePSolver::Linear:
		return(&btSoftBody::PSolve_Links);
	case	ePSolver::RContacts:
		return(&btSoftBody::PSolve_RContacts);
	case	ePSolver::SContacts:
		return(&btSoftBody::PSolve_SContacts);
		default:
		{
		}
	}
	return(0);
}

//
btSoftBody::vsolver_t	btSoftBody::getSolver(eVSolver::_ solver)
{
	switch(solver)
	{
	case	eVSolver::Linear:		return(&btSoftBody::VSolve_Links);
		default:
		{
		}
	}
	return(0);
}

//
void			btSoftBody::defaultCollisionHandler(const btCollisionObjectWrapper* pcoWrap)
{

	switch(m_cfg.collisions&fCollision::RVSmask)
	{
	case	fCollision::SDF_RS:
		{
			btSoftColliders::CollideSDF_RS	docollide;
			btRigidBody*		prb1=(btRigidBody*) btRigidBody::upcast(pcoWrap->getCollisionObject());
			btTransform	wtr=pcoWrap->getWorldTransform();

			const btTransform	ctr=pcoWrap->getWorldTransform();
			const btScalar		timemargin=(wtr.getOrigin()-ctr.getOrigin()).length();
			const btScalar		basemargin=getCollisionShape()->getMargin();
			btVector3			mins;
			btVector3			maxs;
			ATTRIBUTE_ALIGNED16(btDbvtVolume)		volume;
			pcoWrap->getCollisionShape()->getAabb(	pcoWrap->getWorldTransform(),
				mins,
				maxs);
			volume=btDbvtVolume::FromMM(mins,maxs);
			volume.Expand(btVector3(basemargin,basemargin,basemargin));
			docollide.psb		=	this;
			docollide.m_colObj1Wrap = pcoWrap;
			docollide.m_rigidBody = prb1;

			docollide.dynmargin	=	basemargin+timemargin;
			docollide.stamargin	=	basemargin;
			m_ndbvt.collideTV(m_ndbvt.m_root,volume,docollide);
		}
		break;
	case	fCollision::CL_RS:
		{
			btSoftColliders::CollideCL_RS	collider;
			collider.ProcessColObj(this,pcoWrap);
		}
		break;
	}
}

//
void			btSoftBody::defaultCollisionHandler(btSoftBody* psb)
{
	const int cf=m_cfg.collisions&psb->m_cfg.collisions;
	switch(cf&fCollision::SVSmask)
	{
	case	fCollision::CL_SS:
		{

			//support self-collision if CL_SELF flag set
			if (this!=psb || psb->m_cfg.collisions&fCollision::CL_SELF)
			{
				btSoftColliders::CollideCL_SS	docollide;
				docollide.ProcessSoftSoft(this,psb);
			}

		}
		break;
	case	fCollision::VF_SS:
		{
			//only self-collision for Cluster, not Vertex-Face yet
			if (this!=psb)
			{
				btSoftColliders::CollideVF_SS	docollide;
				/* common					*/
				docollide.mrg=	getCollisionShape()->getMargin()+
					psb->getCollisionShape()->getMargin();
				/* psb0 nodes vs psb1 faces	*/
				docollide.psb[0]=this;
				docollide.psb[1]=psb;
				docollide.psb[0]->m_ndbvt.collideTT(	docollide.psb[0]->m_ndbvt.m_root,
					docollide.psb[1]->m_fdbvt.m_root,
					docollide);
				/* psb1 nodes vs psb0 faces	*/
				docollide.psb[0]=psb;
				docollide.psb[1]=this;
				docollide.psb[0]->m_ndbvt.collideTT(	docollide.psb[0]->m_ndbvt.m_root,
					docollide.psb[1]->m_fdbvt.m_root,
					docollide);
			}
		}
		break;
	default:
		{

		}
	}
}


void btSoftBody::setWindVelocity( const btVector3 &velocity )
{
	m_windVelocity = velocity;
}


const btVector3& btSoftBody::getWindVelocity()
{
	return m_windVelocity;
}


int	btSoftBody::calculateSerializeBufferSize()	const
{
	int sz = sizeof(btSoftBodyData);
	return sz;
}

	///fills the dataBuffer and returns the struct name (and 0 on failure)
const char*	btSoftBody::serialize(void* dataBuffer, class btSerializer* serializer) const
{
	btSoftBodyData* sbd = (btSoftBodyData*) dataBuffer;

	btCollisionObject::serialize(&sbd->m_collisionObjectData, serializer);

	btHashMap<btHashPtr,int>	m_nodeIndexMap;

	sbd->m_numMaterials = m_materials.size();
	sbd->m_materials = sbd->m_numMaterials? (SoftBodyMaterialData**) serializer->getUniquePointer((void*)&m_materials): 0;

	if (sbd->m_materials)
	{
		int sz = sizeof(SoftBodyMaterialData*);
		int numElem = sbd->m_numMaterials;
		btChunk* chunk = serializer->allocate(sz,numElem);
		//SoftBodyMaterialData** memPtr = chunk->m_oldPtr;
		SoftBodyMaterialData** memPtr = (SoftBodyMaterialData**)chunk->m_oldPtr;
		for (int i=0;i<numElem;i++,memPtr++)
		{
			btSoftBody::Material* mat = m_materials[i];
			*memPtr = mat ? (SoftBodyMaterialData*)serializer->getUniquePointer((void*)mat) : 0;
			if (!serializer->findPointer(mat))
			{
				//serialize it here
				btChunk* chunk = serializer->allocate(sizeof(SoftBodyMaterialData),1);
				SoftBodyMaterialData* memPtr = (SoftBodyMaterialData*)chunk->m_oldPtr;
				memPtr->m_flags = mat->m_flags;
				memPtr->m_angularStiffness = mat->m_kAST;
				memPtr->m_linearStiffness = mat->m_kLST;
				memPtr->m_volumeStiffness = mat->m_kVST;
				serializer->finalizeChunk(chunk,"SoftBodyMaterialData",BT_SBMATERIAL_CODE,mat);
			}
		}
		serializer->finalizeChunk(chunk,"SoftBodyMaterialData",BT_ARRAY_CODE,(void*) &m_materials);
	}


	sbd->m_numNodes = m_nodes.size();
	sbd->m_nodes = sbd->m_numNodes ? (SoftBodyNodeData*)serializer->getUniquePointer((void*)&m_nodes): 0;
	if (sbd->m_nodes)
	{
		int sz = sizeof(SoftBodyNodeData);
		int numElem = sbd->m_numNodes;
		btChunk* chunk = serializer->allocate(sz,numElem);
		SoftBodyNodeData* memPtr = (SoftBodyNodeData*)chunk->m_oldPtr;
		for (int i=0;i<numElem;i++,memPtr++)
		{
			m_nodes[i].m_f.serializeFloat( memPtr->m_accumulatedForce);
			memPtr->m_area = m_nodes[i].m_area;
			memPtr->m_attach = m_nodes[i].m_battach;
			memPtr->m_inverseMass = m_nodes[i].m_im;
			memPtr->m_material = m_nodes[i].m_material? (SoftBodyMaterialData*)serializer->getUniquePointer((void*) m_nodes[i].m_material):0;
			m_nodes[i].m_n.serializeFloat(memPtr->m_normal);
			m_nodes[i].m_x.serializeFloat(memPtr->m_position);
			m_nodes[i].m_q.serializeFloat(memPtr->m_previousPosition);
			m_nodes[i].m_v.serializeFloat(memPtr->m_velocity);
			m_nodeIndexMap.insert(&m_nodes[i],i);
		}
		serializer->finalizeChunk(chunk,"SoftBodyNodeData",BT_SBNODE_CODE,(void*) &m_nodes);
	}

	sbd->m_numLinks = m_links.size();
	sbd->m_links = sbd->m_numLinks? (SoftBodyLinkData*) serializer->getUniquePointer((void*)&m_links[0]):0;
	if (sbd->m_links)
	{
		int sz = sizeof(SoftBodyLinkData);
		int numElem = sbd->m_numLinks;
		btChunk* chunk = serializer->allocate(sz,numElem);
		SoftBodyLinkData* memPtr = (SoftBodyLinkData*)chunk->m_oldPtr;
		for (int i=0;i<numElem;i++,memPtr++)
		{
			memPtr->m_bbending = m_links[i].m_bbending;
			memPtr->m_material = m_links[i].m_material? (SoftBodyMaterialData*)serializer->getUniquePointer((void*) m_links[i].m_material):0;
			memPtr->m_nodeIndices[0] = m_links[i].m_n[0] ? m_links[i].m_n[0] - &m_nodes[0]: -1;
			memPtr->m_nodeIndices[1] = m_links[i].m_n[1] ? m_links[i].m_n[1] - &m_nodes[0]: -1;
			btAssert(memPtr->m_nodeIndices[0]<m_nodes.size());
			btAssert(memPtr->m_nodeIndices[1]<m_nodes.size());
			memPtr->m_restLength = m_links[i].m_rl;
		}
		serializer->finalizeChunk(chunk,"SoftBodyLinkData",BT_ARRAY_CODE,(void*) &m_links[0]);

	}


	sbd->m_numFaces = m_faces.size();
	sbd->m_faces = sbd->m_numFaces? (SoftBodyFaceData*) serializer->getUniquePointer((void*)&m_faces[0]):0;
	if (sbd->m_faces)
	{
		int sz = sizeof(SoftBodyFaceData);
		int numElem = sbd->m_numFaces;
		btChunk* chunk = serializer->allocate(sz,numElem);
		SoftBodyFaceData* memPtr = (SoftBodyFaceData*)chunk->m_oldPtr;
		for (int i=0;i<numElem;i++,memPtr++)
		{
			memPtr->m_material = m_faces[i].m_material ?  (SoftBodyMaterialData*) serializer->getUniquePointer((void*)m_faces[i].m_material): 0;
			m_faces[i].m_normal.serializeFloat(	memPtr->m_normal);
			for (int j=0;j<3;j++)
			{
				memPtr->m_nodeIndices[j] = m_faces[i].m_n[j]? m_faces[i].m_n[j] - &m_nodes[0]: -1;
			}
			memPtr->m_restArea = m_faces[i].m_ra;
		}
		serializer->finalizeChunk(chunk,"SoftBodyFaceData",BT_ARRAY_CODE,(void*) &m_faces[0]);
	}


	sbd->m_numTetrahedra = m_tetras.size();
	sbd->m_tetrahedra = sbd->m_numTetrahedra ? (SoftBodyTetraData*) serializer->getUniquePointer((void*)&m_tetras[0]):0;
	if (sbd->m_tetrahedra)
	{
		int sz = sizeof(SoftBodyTetraData);
		int numElem = sbd->m_numTetrahedra;
		btChunk* chunk = serializer->allocate(sz,numElem);
		SoftBodyTetraData* memPtr = (SoftBodyTetraData*)chunk->m_oldPtr;
		for (int i=0;i<numElem;i++,memPtr++)
		{
			for (int j=0;j<4;j++)
			{
				m_tetras[i].m_c0[j].serializeFloat(	memPtr->m_c0[j] );
				memPtr->m_nodeIndices[j] = m_tetras[j].m_n[j]? m_tetras[j].m_n[j]-&m_nodes[0] : -1;
			}
			memPtr->m_c1 = m_tetras[i].m_c1;
			memPtr->m_c2 = m_tetras[i].m_c2;
			memPtr->m_material = m_tetras[i].m_material ? (SoftBodyMaterialData*)serializer->getUniquePointer((void*) m_tetras[i].m_material): 0;
			memPtr->m_restVolume = m_tetras[i].m_rv;
		}
		serializer->finalizeChunk(chunk,"SoftBodyTetraData",BT_ARRAY_CODE,(void*) &m_tetras[0]);
	}

	sbd->m_numAnchors = m_anchors.size();
	sbd->m_anchors = sbd->m_numAnchors ? (SoftRigidAnchorData*) serializer->getUniquePointer((void*)&m_anchors[0]):0;
	if (sbd->m_anchors)
	{
		int sz = sizeof(SoftRigidAnchorData);
		int numElem = sbd->m_numAnchors;
		btChunk* chunk = serializer->allocate(sz,numElem);
		SoftRigidAnchorData* memPtr = (SoftRigidAnchorData*)chunk->m_oldPtr;
		for (int i=0;i<numElem;i++,memPtr++)
		{
			m_anchors[i].m_c0.serializeFloat(memPtr->m_c0);
			m_anchors[i].m_c1.serializeFloat(memPtr->m_c1);
			memPtr->m_c2 = m_anchors[i].m_c2;
			m_anchors[i].m_local.serializeFloat(memPtr->m_localFrame);
			memPtr->m_nodeIndex = m_anchors[i].m_node? m_anchors[i].m_node-&m_nodes[0]: -1;

			memPtr->m_rigidBody = m_anchors[i].m_body? (btRigidBodyData*)  serializer->getUniquePointer((void*)m_anchors[i].m_body): 0;
			btAssert(memPtr->m_nodeIndex < m_nodes.size());
		}
		serializer->finalizeChunk(chunk,"SoftRigidAnchorData",BT_ARRAY_CODE,(void*) &m_anchors[0]);
	}


	sbd->m_config.m_dynamicFriction = m_cfg.kDF;
	sbd->m_config.m_baumgarte = m_cfg.kVCF;
	sbd->m_config.m_pressure = m_cfg.kPR;
	sbd->m_config.m_aeroModel = this->m_cfg.aeromodel;
	sbd->m_config.m_lift = m_cfg.kLF;
	sbd->m_config.m_drag = m_cfg.kDG;
	sbd->m_config.m_positionIterations = m_cfg.piterations;
	sbd->m_config.m_driftIterations = m_cfg.diterations;
	sbd->m_config.m_clusterIterations = m_cfg.citerations;
	sbd->m_config.m_velocityIterations = m_cfg.viterations;
	sbd->m_config.m_maxVolume = m_cfg.maxvolume;
	sbd->m_config.m_damping = m_cfg.kDP;
	sbd->m_config.m_poseMatch = m_cfg.kMT;
	sbd->m_config.m_collisionFlags = m_cfg.collisions;
	sbd->m_config.m_volume = m_cfg.kVC;
	sbd->m_config.m_rigidContactHardness = m_cfg.kCHR;
	sbd->m_config.m_kineticContactHardness = m_cfg.kKHR;
	sbd->m_config.m_softContactHardness = m_cfg.kSHR;
	sbd->m_config.m_anchorHardness = m_cfg.kAHR;
	sbd->m_config.m_timeScale = m_cfg.timescale;
	sbd->m_config.m_maxVolume = m_cfg.maxvolume;
	sbd->m_config.m_softRigidClusterHardness = m_cfg.kSRHR_CL;
	sbd->m_config.m_softKineticClusterHardness = m_cfg.kSKHR_CL;
	sbd->m_config.m_softSoftClusterHardness = m_cfg.kSSHR_CL;
	sbd->m_config.m_softRigidClusterImpulseSplit = m_cfg.kSR_SPLT_CL;
	sbd->m_config.m_softKineticClusterImpulseSplit = m_cfg.kSK_SPLT_CL;
	sbd->m_config.m_softSoftClusterImpulseSplit = m_cfg.kSS_SPLT_CL;

	//pose for shape matching
	{
		sbd->m_pose = (SoftBodyPoseData*)serializer->getUniquePointer((void*)&m_pose);

		int sz = sizeof(SoftBodyPoseData);
		btChunk* chunk = serializer->allocate(sz,1);
		SoftBodyPoseData* memPtr = (SoftBodyPoseData*)chunk->m_oldPtr;

		m_pose.m_aqq.serializeFloat(memPtr->m_aqq);
		memPtr->m_bframe = m_pose.m_bframe;
		memPtr->m_bvolume = m_pose.m_bvolume;
		m_pose.m_com.serializeFloat(memPtr->m_com);

		memPtr->m_numPositions = m_pose.m_pos.size();
		memPtr->m_positions = memPtr->m_numPositions ? (btVector3FloatData*)serializer->getUniquePointer((void*)&m_pose.m_pos[0]): 0;
		if (memPtr->m_numPositions)
		{
			int numElem = memPtr->m_numPositions;
			int sz = sizeof(btVector3Data);
			btChunk* chunk = serializer->allocate(sz,numElem);
			btVector3FloatData* memPtr = (btVector3FloatData*)chunk->m_oldPtr;
			for (int i=0;i<numElem;i++,memPtr++)
			{
				m_pose.m_pos[i].serializeFloat(*memPtr);
			}
			serializer->finalizeChunk(chunk,"btVector3FloatData",BT_ARRAY_CODE,(void*)&m_pose.m_pos[0]);
		}
		memPtr->m_restVolume = m_pose.m_volume;
		m_pose.m_rot.serializeFloat(memPtr->m_rot);
		m_pose.m_scl.serializeFloat(memPtr->m_scale);

		memPtr->m_numWeigts = m_pose.m_wgh.size();
		memPtr->m_weights = memPtr->m_numWeigts? (float*) serializer->getUniquePointer((void*) &m_pose.m_wgh[0]) : 0;
		if (memPtr->m_numWeigts)
		{

			int numElem = memPtr->m_numWeigts;
			int sz = sizeof(float);
			btChunk* chunk = serializer->allocate(sz,numElem);
			float* memPtr = (float*) chunk->m_oldPtr;
			for (int i=0;i<numElem;i++,memPtr++)
			{
				*memPtr = m_pose.m_wgh[i];
			}
			serializer->finalizeChunk(chunk,"float",BT_ARRAY_CODE,(void*)&m_pose.m_wgh[0]);
		}

		serializer->finalizeChunk(chunk,"SoftBodyPoseData",BT_ARRAY_CODE,(void*)&m_pose);
	}

	//clusters for convex-cluster collision detection

	sbd->m_numClusters = m_clusters.size();
	sbd->m_clusters = sbd->m_numClusters? (SoftBodyClusterData*) serializer->getUniquePointer((void*)m_clusters[0]) : 0;
	if (sbd->m_numClusters)
	{
		int numElem = sbd->m_numClusters;
		int sz = sizeof(SoftBodyClusterData);
		btChunk* chunk = serializer->allocate(sz,numElem);
		SoftBodyClusterData* memPtr = (SoftBodyClusterData*) chunk->m_oldPtr;
		for (int i=0;i<numElem;i++,memPtr++)
		{
			memPtr->m_adamping= m_clusters[i]->m_adamping;
			m_clusters[i]->m_av.serializeFloat(memPtr->m_av);
			memPtr->m_clusterIndex = m_clusters[i]->m_clusterIndex;
			memPtr->m_collide = m_clusters[i]->m_collide;
			m_clusters[i]->m_com.serializeFloat(memPtr->m_com);
			memPtr->m_containsAnchor = m_clusters[i]->m_containsAnchor;
			m_clusters[i]->m_dimpulses[0].serializeFloat(memPtr->m_dimpulses[0]);
			m_clusters[i]->m_dimpulses[1].serializeFloat(memPtr->m_dimpulses[1]);
			m_clusters[i]->m_framexform.serializeFloat(memPtr->m_framexform);
			memPtr->m_idmass = m_clusters[i]->m_idmass;
			memPtr->m_imass = m_clusters[i]->m_imass;
			m_clusters[i]->m_invwi.serializeFloat(memPtr->m_invwi);
			memPtr->m_ldamping = m_clusters[i]->m_ldamping;
			m_clusters[i]->m_locii.serializeFloat(memPtr->m_locii);
			m_clusters[i]->m_lv.serializeFloat(memPtr->m_lv);
			memPtr->m_matching = m_clusters[i]->m_matching;
			memPtr->m_maxSelfCollisionImpulse = m_clusters[i]->m_maxSelfCollisionImpulse;
			memPtr->m_ndamping = m_clusters[i]->m_ndamping;
			memPtr->m_ldamping = m_clusters[i]->m_ldamping;
			memPtr->m_adamping = m_clusters[i]->m_adamping;
			memPtr->m_selfCollisionImpulseFactor = m_clusters[i]->m_selfCollisionImpulseFactor;

			memPtr->m_numFrameRefs = m_clusters[i]->m_framerefs.size();
			memPtr->m_numMasses = m_clusters[i]->m_masses.size();
			memPtr->m_numNodes = m_clusters[i]->m_nodes.size();

			memPtr->m_nvimpulses = m_clusters[i]->m_nvimpulses;
			m_clusters[i]->m_vimpulses[0].serializeFloat(memPtr->m_vimpulses[0]);
			m_clusters[i]->m_vimpulses[1].serializeFloat(memPtr->m_vimpulses[1]);
			memPtr->m_ndimpulses = m_clusters[i]->m_ndimpulses;


			memPtr->m_framerefs = memPtr->m_numFrameRefs? (btVector3FloatData*)serializer->getUniquePointer((void*)&m_clusters[i]->m_framerefs[0]) : 0;
			if (memPtr->m_framerefs)
			{
				int numElem = memPtr->m_numFrameRefs;
				int sz = sizeof(btVector3FloatData);
				btChunk* chunk = serializer->allocate(sz,numElem);
				btVector3FloatData* memPtr = (btVector3FloatData*) chunk->m_oldPtr;
				for (int j=0;j<numElem;j++,memPtr++)
				{
					m_clusters[i]->m_framerefs[j].serializeFloat(*memPtr);
				}
				serializer->finalizeChunk(chunk,"btVector3FloatData",BT_ARRAY_CODE,(void*)&m_clusters[i]->m_framerefs[0]);
			}

			memPtr->m_masses = memPtr->m_numMasses ? (float*) serializer->getUniquePointer((void*)&m_clusters[i]->m_masses[0]): 0;
			if (memPtr->m_masses)
			{
				int numElem = memPtr->m_numMasses;
				int sz = sizeof(float);
				btChunk* chunk = serializer->allocate(sz,numElem);
				float* memPtr = (float*) chunk->m_oldPtr;
				for (int j=0;j<numElem;j++,memPtr++)
				{
					*memPtr = m_clusters[i]->m_masses[j];
				}
				serializer->finalizeChunk(chunk,"float",BT_ARRAY_CODE,(void*)&m_clusters[i]->m_masses[0]);
			}

			memPtr->m_nodeIndices  = memPtr->m_numNodes ? (int*) serializer->getUniquePointer((void*) &m_clusters[i]->m_nodes) : 0;
			if (memPtr->m_nodeIndices )
			{
				int numElem = memPtr->m_numMasses;
				int sz = sizeof(int);
				btChunk* chunk = serializer->allocate(sz,numElem);
				int* memPtr = (int*) chunk->m_oldPtr;
				for (int j=0;j<numElem;j++,memPtr++)
				{
					int* indexPtr = m_nodeIndexMap.find(m_clusters[i]->m_nodes[j]);
					btAssert(indexPtr);
					*memPtr = *indexPtr;
				}
				serializer->finalizeChunk(chunk,"int",BT_ARRAY_CODE,(void*)&m_clusters[i]->m_nodes);
			}
		}
		serializer->finalizeChunk(chunk,"SoftBodyClusterData",BT_ARRAY_CODE,(void*)m_clusters[0]);

	}


	sbd->m_numJoints = m_joints.size();
	sbd->m_joints = m_joints.size()? (btSoftBodyJointData*) serializer->getUniquePointer((void*)&m_joints[0]) : 0;

	if (sbd->m_joints)
	{
		int sz = sizeof(btSoftBodyJointData);
		int numElem = m_joints.size();
		btChunk* chunk = serializer->allocate(sz,numElem);
		btSoftBodyJointData* memPtr = (btSoftBodyJointData*)chunk->m_oldPtr;

		for (int i=0;i<numElem;i++,memPtr++)
		{
			memPtr->m_jointType = (int)m_joints[i]->Type();
			m_joints[i]->m_refs[0].serializeFloat(memPtr->m_refs[0]);
			m_joints[i]->m_refs[1].serializeFloat(memPtr->m_refs[1]);
			memPtr->m_cfm = m_joints[i]->m_cfm;
			memPtr->m_erp = float(m_joints[i]->m_erp);
			memPtr->m_split = float(m_joints[i]->m_split);
			memPtr->m_delete = m_joints[i]->m_delete;

			for (int j=0;j<4;j++)
			{
				memPtr->m_relPosition[0].m_floats[j] = 0.f;
				memPtr->m_relPosition[1].m_floats[j] = 0.f;
			}
			memPtr->m_bodyA = 0;
			memPtr->m_bodyB = 0;
			if (m_joints[i]->m_bodies[0].m_soft)
			{
				memPtr->m_bodyAtype = BT_JOINT_SOFT_BODY_CLUSTER;
				memPtr->m_bodyA = serializer->getUniquePointer((void*)m_joints[i]->m_bodies[0].m_soft);
			}
			if (m_joints[i]->m_bodies[0].m_collisionObject)
			{
				memPtr->m_bodyAtype = BT_JOINT_COLLISION_OBJECT;
				memPtr->m_bodyA = serializer->getUniquePointer((void*)m_joints[i]->m_bodies[0].m_collisionObject);
			}
			if (m_joints[i]->m_bodies[0].m_rigid)
			{
				memPtr->m_bodyAtype = BT_JOINT_RIGID_BODY;
				memPtr->m_bodyA = serializer->getUniquePointer((void*)m_joints[i]->m_bodies[0].m_rigid);
			}

			if (m_joints[i]->m_bodies[1].m_soft)
			{
				memPtr->m_bodyBtype = BT_JOINT_SOFT_BODY_CLUSTER;
				memPtr->m_bodyB = serializer->getUniquePointer((void*)m_joints[i]->m_bodies[1].m_soft);
			}
			if (m_joints[i]->m_bodies[1].m_collisionObject)
			{
				memPtr->m_bodyBtype = BT_JOINT_COLLISION_OBJECT;
				memPtr->m_bodyB = serializer->getUniquePointer((void*)m_joints[i]->m_bodies[1].m_collisionObject);
			}
			if (m_joints[i]->m_bodies[1].m_rigid)
			{
				memPtr->m_bodyBtype = BT_JOINT_RIGID_BODY;
				memPtr->m_bodyB = serializer->getUniquePointer((void*)m_joints[i]->m_bodies[1].m_rigid);
			}
		}
		serializer->finalizeChunk(chunk,"btSoftBodyJointData",BT_ARRAY_CODE,(void*) &m_joints[0]);
	}


	return btSoftBodyDataName;
}