ruby-bullet 0.1.0-x86-linux → 0.1.1-x86-linux

data/deps/include/bullet/BulletSoftBody/btSoftBodyInternals.h

@@ -1,930 +0,0 @@
-/*
-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
-
-#ifndef _BT_SOFT_BODY_INTERNALS_H
-#define _BT_SOFT_BODY_INTERNALS_H
-
-#include "btSoftBody.h"
-
-
-#include "LinearMath/btQuickprof.h"
-#include "BulletCollision/BroadphaseCollision/btBroadphaseInterface.h"
-#include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h"
-#include "BulletCollision/CollisionShapes/btConvexInternalShape.h"
-#include "BulletCollision/NarrowPhaseCollision/btGjkEpa2.h"
-#include <string.h> //for memset
-//
-// btSymMatrix
-//
-template <typename T>
-struct btSymMatrix
-{
-	btSymMatrix() : dim(0)					{}
-	btSymMatrix(int n,const T& init=T())	{ resize(n,init); }
-	void					resize(int n,const T& init=T())			{ dim=n;store.resize((n*(n+1))/2,init); }
-	int						index(int c,int r) const				{ if(c>r) btSwap(c,r);btAssert(r<dim);return((r*(r+1))/2+c); }
-	T&						operator()(int c,int r)					{ return(store[index(c,r)]); }
-	const T&				operator()(int c,int r) const			{ return(store[index(c,r)]); }
-	btAlignedObjectArray<T>	store;
-	int						dim;
-};	
-
-//
-// btSoftBodyCollisionShape
-//
-class btSoftBodyCollisionShape : public btConcaveShape
-{
-public:
-	btSoftBody*						m_body;
-
-	btSoftBodyCollisionShape(btSoftBody* backptr)
-	{
-		m_shapeType = SOFTBODY_SHAPE_PROXYTYPE;
-		m_body=backptr;
-	}
-
-	virtual ~btSoftBodyCollisionShape()
-	{
-
-	}
-
-	void	processAllTriangles(btTriangleCallback* /*callback*/,const btVector3& /*aabbMin*/,const btVector3& /*aabbMax*/) const
-	{
-		//not yet
-		btAssert(0);
-	}
-
-	///getAabb returns the axis aligned bounding box in the coordinate frame of the given transform t.
-	virtual void getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const
-	{
-		/* t is usually identity, except when colliding against btCompoundShape. See Issue 512 */
-		const btVector3	mins=m_body->m_bounds[0];
-		const btVector3	maxs=m_body->m_bounds[1];
-		const btVector3	crns[]={t*btVector3(mins.x(),mins.y(),mins.z()),
-			t*btVector3(maxs.x(),mins.y(),mins.z()),
-			t*btVector3(maxs.x(),maxs.y(),mins.z()),
-			t*btVector3(mins.x(),maxs.y(),mins.z()),
-			t*btVector3(mins.x(),mins.y(),maxs.z()),
-			t*btVector3(maxs.x(),mins.y(),maxs.z()),
-			t*btVector3(maxs.x(),maxs.y(),maxs.z()),
-			t*btVector3(mins.x(),maxs.y(),maxs.z())};
-		aabbMin=aabbMax=crns[0];
-		for(int i=1;i<8;++i)
-		{
-			aabbMin.setMin(crns[i]);
-			aabbMax.setMax(crns[i]);
-		}
-	}
-
-
-	virtual void	setLocalScaling(const btVector3& /*scaling*/)
-	{		
-		///na
-	}
-	virtual const btVector3& getLocalScaling() const
-	{
-		static const btVector3 dummy(1,1,1);
-		return dummy;
-	}
-	virtual void	calculateLocalInertia(btScalar /*mass*/,btVector3& /*inertia*/) const
-	{
-		///not yet
-		btAssert(0);
-	}
-	virtual const char*	getName()const
-	{
-		return "SoftBody";
-	}
-
-};
-
-//
-// btSoftClusterCollisionShape
-//
-class btSoftClusterCollisionShape : public btConvexInternalShape
-{
-public:
-	const btSoftBody::Cluster*	m_cluster;
-
-	btSoftClusterCollisionShape (const btSoftBody::Cluster* cluster) : m_cluster(cluster) { setMargin(0); }
-
-
-	virtual btVector3	localGetSupportingVertex(const btVector3& vec) const
-	{
-		btSoftBody::Node* const *						n=&m_cluster->m_nodes[0];
-		btScalar										d=btDot(vec,n[0]->m_x);
-		int												j=0;
-		for(int i=1,ni=m_cluster->m_nodes.size();i<ni;++i)
-		{
-			const btScalar	k=btDot(vec,n[i]->m_x);
-			if(k>d) { d=k;j=i; }
-		}
-		return(n[j]->m_x);
-	}
-	virtual btVector3	localGetSupportingVertexWithoutMargin(const btVector3& vec)const
-	{
-		return(localGetSupportingVertex(vec));
-	}
-	//notice that the vectors should be unit length
-	virtual void	batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const
-	{}
-
-
-	virtual void	calculateLocalInertia(btScalar mass,btVector3& inertia) const
-	{}
-
-	virtual void getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const
-	{}
-
-	virtual int	getShapeType() const { return SOFTBODY_SHAPE_PROXYTYPE; }
-
-	//debugging
-	virtual const char*	getName()const {return "SOFTCLUSTER";}
-
-	virtual void	setMargin(btScalar margin)
-	{
-		btConvexInternalShape::setMargin(margin);
-	}
-	virtual btScalar	getMargin() const
-	{
-		return getMargin();
-	}
-};
-
-//
-// Inline's
-//
-
-//
-template <typename T>
-static inline void			ZeroInitialize(T& value)
-{
-	memset(&value,0,sizeof(T));
-}
-//
-template <typename T>
-static inline bool			CompLess(const T& a,const T& b)
-{ return(a<b); }
-//
-template <typename T>
-static inline bool			CompGreater(const T& a,const T& b)
-{ return(a>b); }
-//
-template <typename T>
-static inline T				Lerp(const T& a,const T& b,btScalar t)
-{ return(a+(b-a)*t); }
-//
-template <typename T>
-static inline T				InvLerp(const T& a,const T& b,btScalar t)
-{ return((b+a*t-b*t)/(a*b)); }
-//
-static inline btMatrix3x3	Lerp(	const btMatrix3x3& a,
-								 const btMatrix3x3& b,
-								 btScalar t)
-{
-	btMatrix3x3	r;
-	r[0]=Lerp(a[0],b[0],t);
-	r[1]=Lerp(a[1],b[1],t);
-	r[2]=Lerp(a[2],b[2],t);
-	return(r);
-}
-//
-static inline btVector3		Clamp(const btVector3& v,btScalar maxlength)
-{
-	const btScalar sql=v.length2();
-	if(sql>(maxlength*maxlength))
-		return((v*maxlength)/btSqrt(sql));
-	else
-		return(v);
-}
-//
-template <typename T>
-static inline T				Clamp(const T& x,const T& l,const T& h)
-{ return(x<l?l:x>h?h:x); }
-//
-template <typename T>
-static inline T				Sq(const T& x)
-{ return(x*x); }
-//
-template <typename T>
-static inline T				Cube(const T& x)
-{ return(x*x*x); }
-//
-template <typename T>
-static inline T				Sign(const T& x)
-{ return((T)(x<0?-1:+1)); }
-//
-template <typename T>
-static inline bool			SameSign(const T& x,const T& y)
-{ return((x*y)>0); }
-//
-static inline btScalar		ClusterMetric(const btVector3& x,const btVector3& y)
-{
-	const btVector3	d=x-y;
-	return(btFabs(d[0])+btFabs(d[1])+btFabs(d[2]));
-}
-//
-static inline btMatrix3x3	ScaleAlongAxis(const btVector3& a,btScalar s)
-{
-	const btScalar	xx=a.x()*a.x();
-	const btScalar	yy=a.y()*a.y();
-	const btScalar	zz=a.z()*a.z();
-	const btScalar	xy=a.x()*a.y();
-	const btScalar	yz=a.y()*a.z();
-	const btScalar	zx=a.z()*a.x();
-	btMatrix3x3		m;
-	m[0]=btVector3(1-xx+xx*s,xy*s-xy,zx*s-zx);
-	m[1]=btVector3(xy*s-xy,1-yy+yy*s,yz*s-yz);
-	m[2]=btVector3(zx*s-zx,yz*s-yz,1-zz+zz*s);
-	return(m);
-}
-//
-static inline btMatrix3x3	Cross(const btVector3& v)
-{
-	btMatrix3x3	m;
-	m[0]=btVector3(0,-v.z(),+v.y());
-	m[1]=btVector3(+v.z(),0,-v.x());
-	m[2]=btVector3(-v.y(),+v.x(),0);
-	return(m);
-}
-//
-static inline btMatrix3x3	Diagonal(btScalar x)
-{
-	btMatrix3x3	m;
-	m[0]=btVector3(x,0,0);
-	m[1]=btVector3(0,x,0);
-	m[2]=btVector3(0,0,x);
-	return(m);
-}
-//
-static inline btMatrix3x3	Add(const btMatrix3x3& a,
-								const btMatrix3x3& b)
-{
-	btMatrix3x3	r;
-	for(int i=0;i<3;++i) r[i]=a[i]+b[i];
-	return(r);
-}
-//
-static inline btMatrix3x3	Sub(const btMatrix3x3& a,
-								const btMatrix3x3& b)
-{
-	btMatrix3x3	r;
-	for(int i=0;i<3;++i) r[i]=a[i]-b[i];
-	return(r);
-}
-//
-static inline btMatrix3x3	Mul(const btMatrix3x3& a,
-								btScalar b)
-{
-	btMatrix3x3	r;
-	for(int i=0;i<3;++i) r[i]=a[i]*b;
-	return(r);
-}
-//
-static inline void			Orthogonalize(btMatrix3x3& m)
-{
-	m[2]=btCross(m[0],m[1]).normalized();
-	m[1]=btCross(m[2],m[0]).normalized();
-	m[0]=btCross(m[1],m[2]).normalized();
-}
-//
-static inline btMatrix3x3	MassMatrix(btScalar im,const btMatrix3x3& iwi,const btVector3& r)
-{
-	const btMatrix3x3	cr=Cross(r);
-	return(Sub(Diagonal(im),cr*iwi*cr));
-}
-
-//
-static inline btMatrix3x3	ImpulseMatrix(	btScalar dt,
-										  btScalar ima,
-										  btScalar imb,
-										  const btMatrix3x3& iwi,
-										  const btVector3& r)
-{
-	return(Diagonal(1/dt)*Add(Diagonal(ima),MassMatrix(imb,iwi,r)).inverse());
-}
-
-//
-static inline btMatrix3x3	ImpulseMatrix(	btScalar ima,const btMatrix3x3& iia,const btVector3& ra,
-										  btScalar imb,const btMatrix3x3& iib,const btVector3& rb)	
-{
-	return(Add(MassMatrix(ima,iia,ra),MassMatrix(imb,iib,rb)).inverse());
-}
-
-//
-static inline btMatrix3x3	AngularImpulseMatrix(	const btMatrix3x3& iia,
-												 const btMatrix3x3& iib)
-{
-	return(Add(iia,iib).inverse());
-}
-
-//
-static inline btVector3		ProjectOnAxis(	const btVector3& v,
-										  const btVector3& a)
-{
-	return(a*btDot(v,a));
-}
-//
-static inline btVector3		ProjectOnPlane(	const btVector3& v,
-										   const btVector3& a)
-{
-	return(v-ProjectOnAxis(v,a));
-}
-
-//
-static inline void			ProjectOrigin(	const btVector3& a,
-										  const btVector3& b,
-										  btVector3& prj,
-										  btScalar& sqd)
-{
-	const btVector3	d=b-a;
-	const btScalar	m2=d.length2();
-	if(m2>SIMD_EPSILON)
-	{	
-		const btScalar	t=Clamp<btScalar>(-btDot(a,d)/m2,0,1);
-		const btVector3	p=a+d*t;
-		const btScalar	l2=p.length2();
-		if(l2<sqd)
-		{
-			prj=p;
-			sqd=l2;
-		}
-	}
-}
-//
-static inline void			ProjectOrigin(	const btVector3& a,
-										  const btVector3& b,
-										  const btVector3& c,
-										  btVector3& prj,
-										  btScalar& sqd)
-{
-	const btVector3&	q=btCross(b-a,c-a);
-	const btScalar		m2=q.length2();
-	if(m2>SIMD_EPSILON)
-	{
-		const btVector3	n=q/btSqrt(m2);
-		const btScalar	k=btDot(a,n);
-		const btScalar	k2=k*k;
-		if(k2<sqd)
-		{
-			const btVector3	p=n*k;
-			if(	(btDot(btCross(a-p,b-p),q)>0)&&
-				(btDot(btCross(b-p,c-p),q)>0)&&
-				(btDot(btCross(c-p,a-p),q)>0))
-			{			
-				prj=p;
-				sqd=k2;
-			}
-			else
-			{
-				ProjectOrigin(a,b,prj,sqd);
-				ProjectOrigin(b,c,prj,sqd);
-				ProjectOrigin(c,a,prj,sqd);
-			}
-		}
-	}
-}
-
-//
-template <typename T>
-static inline T				BaryEval(		const T& a,
-									 const T& b,
-									 const T& c,
-									 const btVector3& coord)
-{
-	return(a*coord.x()+b*coord.y()+c*coord.z());
-}
-//
-static inline btVector3		BaryCoord(	const btVector3& a,
-									  const btVector3& b,
-									  const btVector3& c,
-									  const btVector3& p)
-{
-	const btScalar	w[]={	btCross(a-p,b-p).length(),
-		btCross(b-p,c-p).length(),
-		btCross(c-p,a-p).length()};
-	const btScalar	isum=1/(w[0]+w[1]+w[2]);
-	return(btVector3(w[1]*isum,w[2]*isum,w[0]*isum));
-}
-
-//
-static btScalar				ImplicitSolve(	btSoftBody::ImplicitFn* fn,
-										  const btVector3& a,
-										  const btVector3& b,
-										  const btScalar accuracy,
-										  const int maxiterations=256)
-{
-	btScalar	span[2]={0,1};
-	btScalar	values[2]={fn->Eval(a),fn->Eval(b)};
-	if(values[0]>values[1])
-	{
-		btSwap(span[0],span[1]);
-		btSwap(values[0],values[1]);
-	}
-	if(values[0]>-accuracy) return(-1);
-	if(values[1]<+accuracy) return(-1);
-	for(int i=0;i<maxiterations;++i)
-	{
-		const btScalar	t=Lerp(span[0],span[1],values[0]/(values[0]-values[1]));
-		const btScalar	v=fn->Eval(Lerp(a,b,t));
-		if((t<=0)||(t>=1))		break;
-		if(btFabs(v)<accuracy)	return(t);
-		if(v<0)
-		{ span[0]=t;values[0]=v; }
-		else
-		{ span[1]=t;values[1]=v; }
-	}
-	return(-1);
-}
-
-//
-static inline btVector3		NormalizeAny(const btVector3& v)
-{
-	const btScalar l=v.length();
-	if(l>SIMD_EPSILON)
-		return(v/l);
-	else
-		return(btVector3(0,0,0));
-}
-
-//
-static inline btDbvtVolume	VolumeOf(	const btSoftBody::Face& f,
-									 btScalar margin)
-{
-	const btVector3*	pts[]={	&f.m_n[0]->m_x,
-		&f.m_n[1]->m_x,
-		&f.m_n[2]->m_x};
-	btDbvtVolume		vol=btDbvtVolume::FromPoints(pts,3);
-	vol.Expand(btVector3(margin,margin,margin));
-	return(vol);
-}
-
-//
-static inline btVector3			CenterOf(	const btSoftBody::Face& f)
-{
-	return((f.m_n[0]->m_x+f.m_n[1]->m_x+f.m_n[2]->m_x)/3);
-}
-
-//
-static inline btScalar			AreaOf(		const btVector3& x0,
-									   const btVector3& x1,
-									   const btVector3& x2)
-{
-	const btVector3	a=x1-x0;
-	const btVector3	b=x2-x0;
-	const btVector3	cr=btCross(a,b);
-	const btScalar	area=cr.length();
-	return(area);
-}
-
-//
-static inline btScalar		VolumeOf(	const btVector3& x0,
-									 const btVector3& x1,
-									 const btVector3& x2,
-									 const btVector3& x3)
-{
-	const btVector3	a=x1-x0;
-	const btVector3	b=x2-x0;
-	const btVector3	c=x3-x0;
-	return(btDot(a,btCross(b,c)));
-}
-
-//
-static void					EvaluateMedium(	const btSoftBodyWorldInfo* wfi,
-										   const btVector3& x,
-										   btSoftBody::sMedium& medium)
-{
-	medium.m_velocity	=	btVector3(0,0,0);
-	medium.m_pressure	=	0;
-	medium.m_density	=	wfi->air_density;
-	if(wfi->water_density>0)
-	{
-		const btScalar	depth=-(btDot(x,wfi->water_normal)+wfi->water_offset);
-		if(depth>0)
-		{
-			medium.m_density	=	wfi->water_density;
-			medium.m_pressure	=	depth*wfi->water_density*wfi->m_gravity.length();
-		}
-	}
-}
-
-//
-static inline void			ApplyClampedForce(	btSoftBody::Node& n,
-											  const btVector3& f,
-											  btScalar dt)
-{
-	const btScalar	dtim=dt*n.m_im;
-	if((f*dtim).length2()>n.m_v.length2())
-	{/* Clamp	*/ 
-		n.m_f-=ProjectOnAxis(n.m_v,f.normalized())/dtim;						
-	}
-	else
-	{/* Apply	*/ 
-		n.m_f+=f;
-	}
-}
-
-//
-static inline int		MatchEdge(	const btSoftBody::Node* a,
-								  const btSoftBody::Node* b,
-								  const btSoftBody::Node* ma,
-								  const btSoftBody::Node* mb)
-{
-	if((a==ma)&&(b==mb)) return(0);
-	if((a==mb)&&(b==ma)) return(1);
-	return(-1);
-}
-
-//
-// btEigen : Extract eigen system,
-// straitforward implementation of http://math.fullerton.edu/mathews/n2003/JacobiMethodMod.html
-// outputs are NOT sorted.
-//
-struct	btEigen
-{
-	static int			system(btMatrix3x3& a,btMatrix3x3* vectors,btVector3* values=0)
-	{
-		static const int		maxiterations=16;
-		static const btScalar	accuracy=(btScalar)0.0001;
-		btMatrix3x3&			v=*vectors;
-		int						iterations=0;
-		vectors->setIdentity();
-		do	{
-			int				p=0,q=1;
-			if(btFabs(a[p][q])<btFabs(a[0][2])) { p=0;q=2; }
-			if(btFabs(a[p][q])<btFabs(a[1][2])) { p=1;q=2; }
-			if(btFabs(a[p][q])>accuracy)
-			{
-				const btScalar	w=(a[q][q]-a[p][p])/(2*a[p][q]);
-				const btScalar	z=btFabs(w);
-				const btScalar	t=w/(z*(btSqrt(1+w*w)+z));
-				if(t==t)/* [WARNING] let hope that one does not get thrown aways by some compilers... */ 
-				{
-					const btScalar	c=1/btSqrt(t*t+1);
-					const btScalar	s=c*t;
-					mulPQ(a,c,s,p,q);
-					mulTPQ(a,c,s,p,q);
-					mulPQ(v,c,s,p,q);
-				} else break;
-			} else break;
-		} while((++iterations)<maxiterations);
-		if(values)
-		{
-			*values=btVector3(a[0][0],a[1][1],a[2][2]);
-		}
-		return(iterations);
-	}
-private:
-	static inline void	mulTPQ(btMatrix3x3& a,btScalar c,btScalar s,int p,int q)
-	{
-		const btScalar	m[2][3]={	{a[p][0],a[p][1],a[p][2]},
-		{a[q][0],a[q][1],a[q][2]}};
-		int i;
-
-		for(i=0;i<3;++i) a[p][i]=c*m[0][i]-s*m[1][i];
-		for(i=0;i<3;++i) a[q][i]=c*m[1][i]+s*m[0][i];
-	}
-	static inline void	mulPQ(btMatrix3x3& a,btScalar c,btScalar s,int p,int q)
-	{
-		const btScalar	m[2][3]={	{a[0][p],a[1][p],a[2][p]},
-		{a[0][q],a[1][q],a[2][q]}};
-		int i;
-
-		for(i=0;i<3;++i) a[i][p]=c*m[0][i]-s*m[1][i];
-		for(i=0;i<3;++i) a[i][q]=c*m[1][i]+s*m[0][i];
-	}
-};
-
-//
-// Polar decomposition,
-// "Computing the Polar Decomposition with Applications", Nicholas J. Higham, 1986.
-//
-static inline int			PolarDecompose(	const btMatrix3x3& m,btMatrix3x3& q,btMatrix3x3& s)
-{
-	static const btScalar	half=(btScalar)0.5;
-	static const btScalar	accuracy=(btScalar)0.0001;
-	static const int		maxiterations=16;
-	int						i=0;
-	btScalar				det=0;
-	q	=	Mul(m,1/btVector3(m[0][0],m[1][1],m[2][2]).length());
-	det	=	q.determinant();
-	if(!btFuzzyZero(det))
-	{
-		for(;i<maxiterations;++i)
-		{
-			q=Mul(Add(q,Mul(q.adjoint(),1/det).transpose()),half);
-			const btScalar	ndet=q.determinant();
-			if(Sq(ndet-det)>accuracy) det=ndet; else break;
-		}
-		/* Final orthogonalization	*/ 
-		Orthogonalize(q);
-		/* Compute 'S'				*/ 
-		s=q.transpose()*m;
-	}
-	else
-	{
-		q.setIdentity();
-		s.setIdentity();
-	}
-	return(i);
-}
-
-//
-// btSoftColliders
-//
-struct btSoftColliders
-{
-	//
-	// ClusterBase
-	//
-	struct	ClusterBase : btDbvt::ICollide
-	{
-		btScalar			erp;
-		btScalar			idt;
-		btScalar			m_margin;
-		btScalar			friction;
-		btScalar			threshold;
-		ClusterBase()
-		{
-			erp			=(btScalar)1;
-			idt			=0;
-			m_margin		=0;
-			friction	=0;
-			threshold	=(btScalar)0;
-		}
-		bool				SolveContact(	const btGjkEpaSolver2::sResults& res,
-			btSoftBody::Body ba,btSoftBody::Body bb,
-			btSoftBody::CJoint& joint)
-		{
-			if(res.distance<m_margin)
-			{
-				btVector3 norm = res.normal;
-				norm.normalize();//is it necessary?
-
-				const btVector3		ra=res.witnesses[0]-ba.xform().getOrigin();
-				const btVector3		rb=res.witnesses[1]-bb.xform().getOrigin();
-				const btVector3		va=ba.velocity(ra);
-				const btVector3		vb=bb.velocity(rb);
-				const btVector3		vrel=va-vb;
-				const btScalar		rvac=btDot(vrel,norm);
-				 btScalar		depth=res.distance-m_margin;
-				
-//				printf("depth=%f\n",depth);
-				const btVector3		iv=norm*rvac;
-				const btVector3		fv=vrel-iv;
-				joint.m_bodies[0]	=	ba;
-				joint.m_bodies[1]	=	bb;
-				joint.m_refs[0]		=	ra*ba.xform().getBasis();
-				joint.m_refs[1]		=	rb*bb.xform().getBasis();
-				joint.m_rpos[0]		=	ra;
-				joint.m_rpos[1]		=	rb;
-				joint.m_cfm			=	1;
-				joint.m_erp			=	1;
-				joint.m_life		=	0;
-				joint.m_maxlife		=	0;
-				joint.m_split		=	1;
-				
-				joint.m_drift		=	depth*norm;
-
-				joint.m_normal		=	norm;
-//				printf("normal=%f,%f,%f\n",res.normal.getX(),res.normal.getY(),res.normal.getZ());
-				joint.m_delete		=	false;
-				joint.m_friction	=	fv.length2()<(-rvac*friction)?1:friction;
-				joint.m_massmatrix	=	ImpulseMatrix(	ba.invMass(),ba.invWorldInertia(),joint.m_rpos[0],
-					bb.invMass(),bb.invWorldInertia(),joint.m_rpos[1]);
-
-				return(true);
-			}
-			return(false);
-		}
-	};
-	//
-	// CollideCL_RS
-	//
-	struct	CollideCL_RS : ClusterBase
-	{
-		btSoftBody*		psb;
-		
-		btCollisionObject*	m_colObj;
-		void		Process(const btDbvtNode* leaf)
-		{
-			btSoftBody::Cluster*		cluster=(btSoftBody::Cluster*)leaf->data;
-			btSoftClusterCollisionShape	cshape(cluster);
-			
-			const btConvexShape*		rshape=(const btConvexShape*)m_colObj->getCollisionShape();
-
-			///don't collide an anchored cluster with a static/kinematic object
-			if(m_colObj->isStaticOrKinematicObject() && cluster->m_containsAnchor)
-				return;
-
-			btGjkEpaSolver2::sResults	res;		
-			if(btGjkEpaSolver2::SignedDistance(	&cshape,btTransform::getIdentity(),
-				rshape,m_colObj->getWorldTransform(),
-				btVector3(1,0,0),res))
-			{
-				btSoftBody::CJoint	joint;
-				if(SolveContact(res,cluster,m_colObj,joint))//prb,joint))
-				{
-					btSoftBody::CJoint*	pj=new(btAlignedAlloc(sizeof(btSoftBody::CJoint),16)) btSoftBody::CJoint();
-					*pj=joint;psb->m_joints.push_back(pj);
-					if(m_colObj->isStaticOrKinematicObject())
-					{
-						pj->m_erp	*=	psb->m_cfg.kSKHR_CL;
-						pj->m_split	*=	psb->m_cfg.kSK_SPLT_CL;
-					}
-					else
-					{
-						pj->m_erp	*=	psb->m_cfg.kSRHR_CL;
-						pj->m_split	*=	psb->m_cfg.kSR_SPLT_CL;
-					}
-				}
-			}
-		}
-		void		Process(btSoftBody* ps,btCollisionObject* colOb)
-		{
-			psb			=	ps;
-			m_colObj			=	colOb;
-			idt			=	ps->m_sst.isdt;
-			m_margin		=	m_colObj->getCollisionShape()->getMargin()+psb->getCollisionShape()->getMargin();
-			///Bullet rigid body uses multiply instead of minimum to determine combined friction. Some customization would be useful.
-			friction	=	btMin(psb->m_cfg.kDF,m_colObj->getFriction());
-			btVector3			mins;
-			btVector3			maxs;
-
-			ATTRIBUTE_ALIGNED16(btDbvtVolume)		volume;
-			colOb->getCollisionShape()->getAabb(colOb->getWorldTransform(),mins,maxs);
-			volume=btDbvtVolume::FromMM(mins,maxs);
-			volume.Expand(btVector3(1,1,1)*m_margin);
-			ps->m_cdbvt.collideTV(ps->m_cdbvt.m_root,volume,*this);
-		}	
-	};
-	//
-	// CollideCL_SS
-	//
-	struct	CollideCL_SS : ClusterBase
-	{
-		btSoftBody*	bodies[2];
-		void		Process(const btDbvtNode* la,const btDbvtNode* lb)
-		{
-			btSoftBody::Cluster*		cla=(btSoftBody::Cluster*)la->data;
-			btSoftBody::Cluster*		clb=(btSoftBody::Cluster*)lb->data;
-
-
-			bool connected=false;
-			if ((bodies[0]==bodies[1])&&(bodies[0]->m_clusterConnectivity.size()))
-			{
-				connected = bodies[0]->m_clusterConnectivity[cla->m_clusterIndex+bodies[0]->m_clusters.size()*clb->m_clusterIndex];
-			}
-
-			if (!connected)
-			{
-				btSoftClusterCollisionShape	csa(cla);
-				btSoftClusterCollisionShape	csb(clb);
-				btGjkEpaSolver2::sResults	res;		
-				if(btGjkEpaSolver2::SignedDistance(	&csa,btTransform::getIdentity(),
-					&csb,btTransform::getIdentity(),
-					cla->m_com-clb->m_com,res))
-				{
-					btSoftBody::CJoint	joint;
-					if(SolveContact(res,cla,clb,joint))
-					{
-						btSoftBody::CJoint*	pj=new(btAlignedAlloc(sizeof(btSoftBody::CJoint),16)) btSoftBody::CJoint();
-						*pj=joint;bodies[0]->m_joints.push_back(pj);
-						pj->m_erp	*=	btMax(bodies[0]->m_cfg.kSSHR_CL,bodies[1]->m_cfg.kSSHR_CL);
-						pj->m_split	*=	(bodies[0]->m_cfg.kSS_SPLT_CL+bodies[1]->m_cfg.kSS_SPLT_CL)/2;
-					}
-				}
-			} else
-			{
-				static int count=0;
-				count++;
-				//printf("count=%d\n",count);
-				
-			}
-		}
-		void		Process(btSoftBody* psa,btSoftBody* psb)
-		{
-			idt			=	psa->m_sst.isdt;
-			//m_margin		=	(psa->getCollisionShape()->getMargin()+psb->getCollisionShape()->getMargin())/2;
-			m_margin		=	(psa->getCollisionShape()->getMargin()+psb->getCollisionShape()->getMargin());
-			friction	=	btMin(psa->m_cfg.kDF,psb->m_cfg.kDF);
-			bodies[0]	=	psa;
-			bodies[1]	=	psb;
-			psa->m_cdbvt.collideTT(psa->m_cdbvt.m_root,psb->m_cdbvt.m_root,*this);
-		}	
-	};
-	//
-	// CollideSDF_RS
-	//
-	struct	CollideSDF_RS : btDbvt::ICollide
-	{
-		void		Process(const btDbvtNode* leaf)
-		{
-			btSoftBody::Node*	node=(btSoftBody::Node*)leaf->data;
-			DoNode(*node);
-		}
-		void		DoNode(btSoftBody::Node& n) const
-		{
-			const btScalar			m=n.m_im>0?dynmargin:stamargin;
-			btSoftBody::RContact	c;
-			if(	(!n.m_battach)&&
-				psb->checkContact(m_colObj1,n.m_x,m,c.m_cti))
-			{
-				const btScalar	ima=n.m_im;
-				const btScalar	imb= m_rigidBody? m_rigidBody->getInvMass() : 0.f;
-				const btScalar	ms=ima+imb;
-				if(ms>0)
-				{
-					const btTransform&	wtr=m_rigidBody?m_rigidBody->getWorldTransform() : m_colObj1->getWorldTransform();
-					static const btMatrix3x3	iwiStatic(0,0,0,0,0,0,0,0,0);
-					const btMatrix3x3&	iwi=m_rigidBody?m_rigidBody->getInvInertiaTensorWorld() : iwiStatic;
-					const btVector3		ra=n.m_x-wtr.getOrigin();
-					const btVector3		va=m_rigidBody ? m_rigidBody->getVelocityInLocalPoint(ra)*psb->m_sst.sdt : btVector3(0,0,0);
-					const btVector3		vb=n.m_x-n.m_q;	
-					const btVector3		vr=vb-va;
-					const btScalar		dn=btDot(vr,c.m_cti.m_normal);
-					const btVector3		fv=vr-c.m_cti.m_normal*dn;
-					const btScalar		fc=psb->m_cfg.kDF*m_colObj1->getFriction();
-					c.m_node	=	&n;
-					c.m_c0		=	ImpulseMatrix(psb->m_sst.sdt,ima,imb,iwi,ra);
-					c.m_c1		=	ra;
-					c.m_c2		=	ima*psb->m_sst.sdt;
-					c.m_c3		=	fv.length2()<(btFabs(dn)*fc)?0:1-fc;
-					c.m_c4		=	m_colObj1->isStaticOrKinematicObject()?psb->m_cfg.kKHR:psb->m_cfg.kCHR;
-					psb->m_rcontacts.push_back(c);
-					if (m_rigidBody)
-						m_rigidBody->activate();
-				}
-			}
-		}
-		btSoftBody*		psb;
-		btCollisionObject*	m_colObj1;
-		btRigidBody*	m_rigidBody;
-		btScalar		dynmargin;
-		btScalar		stamargin;
-	};
-	//
-	// CollideVF_SS
-	//
-	struct	CollideVF_SS : btDbvt::ICollide
-	{
-		void		Process(const btDbvtNode* lnode,
-			const btDbvtNode* lface)
-		{
-			btSoftBody::Node*	node=(btSoftBody::Node*)lnode->data;
-			btSoftBody::Face*	face=(btSoftBody::Face*)lface->data;
-			btVector3			o=node->m_x;
-			btVector3			p;
-			btScalar			d=SIMD_INFINITY;
-			ProjectOrigin(	face->m_n[0]->m_x-o,
-				face->m_n[1]->m_x-o,
-				face->m_n[2]->m_x-o,
-				p,d);
-			const btScalar	m=mrg+(o-node->m_q).length()*2;
-			if(d<(m*m))
-			{
-				const btSoftBody::Node*	n[]={face->m_n[0],face->m_n[1],face->m_n[2]};
-				const btVector3			w=BaryCoord(n[0]->m_x,n[1]->m_x,n[2]->m_x,p+o);
-				const btScalar			ma=node->m_im;
-				btScalar				mb=BaryEval(n[0]->m_im,n[1]->m_im,n[2]->m_im,w);
-				if(	(n[0]->m_im<=0)||
-					(n[1]->m_im<=0)||
-					(n[2]->m_im<=0))
-				{
-					mb=0;
-				}
-				const btScalar	ms=ma+mb;
-				if(ms>0)
-				{
-					btSoftBody::SContact	c;
-					c.m_normal		=	p/-btSqrt(d);
-					c.m_margin		=	m;
-					c.m_node		=	node;
-					c.m_face		=	face;
-					c.m_weights		=	w;
-					c.m_friction	=	btMax(psb[0]->m_cfg.kDF,psb[1]->m_cfg.kDF);
-					c.m_cfm[0]		=	ma/ms*psb[0]->m_cfg.kSHR;
-					c.m_cfm[1]		=	mb/ms*psb[1]->m_cfg.kSHR;
-					psb[0]->m_scontacts.push_back(c);
-				}
-			}	
-		}
-		btSoftBody*		psb[2];
-		btScalar		mrg;
-	};
-};
-
-#endif //_BT_SOFT_BODY_INTERNALS_H