diff --git a/Demos/ConvexDecompositionDemo/ConvexDecompositionDemo.cpp b/Demos/ConvexDecompositionDemo/ConvexDecompositionDemo.cpp
index 817ab752e..6ec0f46aa 100644
--- a/Demos/ConvexDecompositionDemo/ConvexDecompositionDemo.cpp
+++ b/Demos/ConvexDecompositionDemo/ConvexDecompositionDemo.cpp
@@ -21,7 +21,7 @@ subject to the following restrictions:
 #include "LinearMath/btQuickprof.h"
 #include "LinearMath/btIDebugDraw.h"
 #include "LinearMath/btGeometryUtil.h"
-#include "btShapeHull.h"
+#include "BulletCollision/CollisionShapes/btShapeHull.h"
 
 //#define USE_PARALLEL_DISPATCHER 1
 #ifdef USE_PARALLEL_DISPATCHER
diff --git a/Demos/SoftDemo/SoftDemo.cpp b/Demos/SoftDemo/SoftDemo.cpp
index bc2223000..d4f1ce739 100644
--- a/Demos/SoftDemo/SoftDemo.cpp
+++ b/Demos/SoftDemo/SoftDemo.cpp
@@ -17,6 +17,8 @@ subject to the following restrictions:
 
 
 #include "btBulletDynamicsCommon.h"
+#include "BulletDynamics/SoftBody/btSoftRigidDynamicsWorld.h"
+
 #include "BulletCollision/CollisionDispatch/btSphereSphereCollisionAlgorithm.h"
 #include "BulletCollision/NarrowPhaseCollision/btGjkEpa2.h"
 #include "LinearMath/btQuickprof.h"
@@ -25,7 +27,7 @@ subject to the following restrictions:
 #include "../GimpactTestDemo/BunnyMesh.h"
 #include "../GimpactTestDemo/TorusMesh.h"
 #include <stdio.h> //printf debugging
-#include "../../Extras/ConvexHull/btConvexHull.h"
+#include "LinearMath/btConvexHull.h"
 #include "btSoftBodyRigidBodyCollisionConfiguration.h"
 #include "BulletDynamics/SoftBody/btSoftBodyHelpers.h"
 
@@ -42,34 +44,6 @@ extern int glutScreenHeight;
 const int maxProxies = 32766;
 const int maxOverlap = 65535;
 
-//
-btSoftBody*		btSoftBodyHelpers::CreateFromConvexHull(btSoftBody::btSoftBodyWorldInfo& worldInfo,	const btVector3* vertices,
-									 int nvertices)
-{
-	HullDesc		hdsc(QF_TRIANGLES,nvertices,vertices);
-	HullResult		hres;
-	HullLibrary		hlib;/*??*/ 
-	hdsc.mMaxVertices=nvertices;
-	hlib.CreateConvexHull(hdsc,hres);
-	btSoftBody*		psb=new btSoftBody(worldInfo,(int)hres.mNumOutputVertices,
-		hres.mOutputVertices,0);
-	for(int i=0;i<(int)hres.mNumFaces;++i)
-	{
-		const int idx[]={	hres.mIndices[i*3+0],
-			hres.mIndices[i*3+1],
-			hres.mIndices[i*3+2]};
-		if(idx[0]<idx[1]) psb->AppendLink(	idx[0],idx[1],
-			1,btSoftBody::eLType::Structural);
-		if(idx[1]<idx[2]) psb->AppendLink(	idx[1],idx[2],
-			1,btSoftBody::eLType::Structural);
-		if(idx[2]<idx[0]) psb->AppendLink(	idx[2],idx[0],
-			1,btSoftBody::eLType::Structural);
-		psb->AppendFace(idx[0],idx[1],idx[2]);
-	}
-	hlib.ReleaseResult(hres);
-	psb->RandomizeConstraints();
-	return(psb);
-}
 
 
 #ifdef _DEBUG
@@ -114,95 +88,6 @@ void SoftDemo::createStack( btCollisionShape* boxShape, float halfCubeSize, int
 
 ////////////////////////////////////
 
-
-
-#ifdef DHSJHDSAKJDHSJKADHSAKJDHSA
-//
-bool		SoftDemo::SoftBodyImpl::CheckContactPrecise(const btVector3& x,
-														btSoftBody::ISoftBody::sCti& cti)
-{
-	btScalar					maxdepth=0;
-	btGjkEpaSolver2::sResults	res;
-//	btDynamicsWorld*			pdw=pdemo->m_dynamicsWorld;
-
-	btCollisionObjectArray&		coa=pdw->getCollisionObjectArray();
-	for(int i=0,ni=coa.size();i<ni;++i)
-	{
-		btRigidBody*			prb=(btRigidBody*)(coa[i]);
-		btCollisionShape*		shp=prb->getCollisionShape();
-		if(shp->isConvex())
-		{
-			btConvexShape*		csh=static_cast<btConvexShape*>(shp);
-			const btTransform&	wtr=prb->getWorldTransform();
-			const btScalar		dst=btGjkEpaSolver2::SignedDistance(x,0.1,csh,wtr,res);
-			if(dst<maxdepth)
-			{
-				maxdepth		=	dst;
-				cti.m_body		=	prb;
-				cti.m_normal	=	res.normal;
-				cti.m_offset	=	-dot(cti.m_normal,res.witnesses[0]);
-			}
-		}
-	}
-	return(maxdepth<0);
-}
-
-//
-bool		SoftDemo::SoftBodyImpl::CheckContact(	const btVector3& x,
-												 btSoftBody::sCti& cti)
-{
-	btScalar					maxdepth=0;
-	btGjkEpaSolver2::sResults	res;
-	btDynamicsWorld*			pdw=pdemo->m_dynamicsWorld;
-	btCollisionObjectArray&		coa=pdw->getCollisionObjectArray();
-	for(int i=0,ni=coa.size();i<ni;++i)
-	{
-		btVector3			nrm;
-		btRigidBody*		prb=(btRigidBody*)(coa[i]);
-		btCollisionShape*	shp=prb->getCollisionShape();
-		btConvexShape*		csh=static_cast<btConvexShape*>(shp);
-		const btTransform&	wtr=prb->getWorldTransform();
-		btScalar			dst=pdemo->m_sparsesdf.Evaluate(wtr.invXform(x),csh,nrm);
-		nrm=wtr.getBasis()*nrm;
-		btVector3			wit=x-nrm*dst;
-		if(dst<maxdepth)
-		{
-			maxdepth		=	dst;
-			cti.m_body		=	prb;
-			cti.m_normal	=	nrm;
-			cti.m_offset	=	-dot(cti.m_normal,wit);
-		}
-	}
-	return(maxdepth<0);
-}
-
-//
-void		SoftDemo::SoftBodyImpl::EndCollide()
-{}
-
-//
-void		SoftDemo::SoftBodyImpl::EvaluateMedium(	const btVector3& x,
-												   btSoftBody::ISoftBody::sMedium& medium)
-{
-	medium.m_velocity	=	btVector3(0,0,0);
-	medium.m_pressure	=	0;
-	medium.m_density	=	air_density;
-	if(water_density>0)
-	{
-		const btScalar	depth=-(dot(x,water_normal)+water_offset);
-		if(depth>0)
-		{
-			medium.m_density	=	water_density;
-			medium.m_pressure	=	depth			*
-				water_density	*
-				pdemo->m_dynamicsWorld->getGravity().length();
-		}
-	}
-}
-
-#endif
-
-
 extern int gNumManifold;
 extern int gOverlappingPairs;
 extern int gTotalContactPoints;
@@ -253,19 +138,10 @@ void SoftDemo::clientMoveAndDisplay()
 #endif		
 
 
-		/* soft bodies simulation */ 
-		for(int ib=0;ib<m_softbodies.size();++ib)
-		{
-			btSoftBody*	psb=m_softbodies[ib];
-			psb->AddVelocity(m_dynamicsWorld->getGravity()*dt);
-			psb->Step(dt);
-		}
-
 		m_softBodyWorldInfo.m_sparsesdf.GarbageCollect();
 
 		//optional but useful: debug drawing
-
-		m_dynamicsWorld->debugDrawWorld();		
+		
 	}
 
 #ifdef USE_QUICKPROF 
@@ -406,9 +282,9 @@ static btSoftBody* Ctor_SoftBox(SoftDemo* pdemo,const btVector3& p,const btVecto
 		p+h*btVector3(-1,+1,+1),
 		p+h*btVector3(+1,+1,+1)};
 	btSoftBody*		psb=btSoftBodyHelpers::CreateFromConvexHull(pdemo->m_softBodyWorldInfo,c,8);
-	psb->GenerateBendingConstraints(2,1);
-	pdemo->m_softbodies.push_back(psb);
-
+	psb->generateBendingConstraints(2,1);
+	pdemo->getSoftDynamicsWorld()->addSoftBody(psb);
+	
 	return(psb);
 }
 
@@ -424,8 +300,8 @@ static btSoftBody* Ctor_SoftBoulder(SoftDemo* pdemo,const btVector3& p,const btV
 		pts.push_back(Vector3Rand()*s+p);
 	}
 	btSoftBody*		psb=btSoftBodyHelpers::CreateFromConvexHull(pdemo->m_softBodyWorldInfo,&pts[0],pts.size());
-	psb->GenerateBendingConstraints(2,1);
-	pdemo->m_softbodies.push_back(psb);
+	psb->generateBendingConstraints(2,1);
+	pdemo->getSoftDynamicsWorld()->addSoftBody(psb);
 
 	return(psb);
 }
@@ -447,8 +323,8 @@ static void	Init_Ropes(SoftDemo* pdemo)
 			1+2);
 		psb->m_cfg.iterations	=	4;
 		psb->m_cfg.kLST			=	0.1+(i/(btScalar)(n-1))*0.9;
-		psb->SetTotalMass(20);
-		pdemo->m_softbodies.push_back(psb);
+		psb->setTotalMass(20);
+		pdemo->getSoftDynamicsWorld()->addSoftBody(psb);
 	
 	}
 }
@@ -465,8 +341,9 @@ static void	Init_RopeAttach(SoftDemo* pdemo)
 		{
 			btSoftBody*	psb=btSoftBodyHelpers::CreateRope(pdemo->m_softBodyWorldInfo,p,p+btVector3(10,0,0),8,1);
 			psb->m_cfg.kDF			=	0;
-			psb->SetTotalMass(50);
-			pdemo->m_softbodies.push_back(psb);
+			psb->m_cfg.kDP			=	0.001;
+			psb->setTotalMass(50);
+			pdemo->getSoftDynamicsWorld()->addSoftBody(psb);
 			return(psb);
 		}
 	};
@@ -476,8 +353,8 @@ static void	Init_RopeAttach(SoftDemo* pdemo)
 	btRigidBody*		body=pdemo->localCreateRigidBody(50,startTransform,new btBoxShape(btVector3(2,6,2)));
 	btSoftBody*	psb0=Functors::CtorRope(pdemo,btVector3(0,8,-1));
 	btSoftBody*	psb1=Functors::CtorRope(pdemo,btVector3(0,8,+1));
-	psb0->AppendAnchor(psb0->getNodes().size()-1,body);
-	psb1->AppendAnchor(psb1->getNodes().size()-1,body);
+	psb0->appendAnchor(psb0->getNodes().size()-1,body);
+	psb1->appendAnchor(psb1->getNodes().size()-1,body);
 }
 
 //
@@ -493,14 +370,14 @@ static void	Init_ClothAttach(SoftDemo* pdemo)
 		btVector3(+s,h,-s),
 		btVector3(-s,h,+s),
 		btVector3(+s,h,+s),r,r,4+8,true);
-	pdemo->m_softbodies.push_back(psb);
+	pdemo->getSoftDynamicsWorld()->addSoftBody(psb);
 
 	btTransform startTransform;
 	startTransform.setIdentity();
 	startTransform.setOrigin(btVector3(0,h,-(s+3.5)));
 	btRigidBody*		body=pdemo->localCreateRigidBody(20,startTransform,new btBoxShape(btVector3(s,1,3)));
-	psb->AppendAnchor(0,body);
-	psb->AppendAnchor(r-1,body);
+	psb->appendAnchor(0,body);
+	psb->appendAnchor(r-1,body);
 }
 
 //
@@ -513,14 +390,144 @@ static void	Init_Impact(SoftDemo* pdemo)
 		btVector3(0,-1,0),
 		0,
 		1);
-	pdemo->m_softbodies.push_back(psb);
-
+	pdemo->getSoftDynamicsWorld()->addSoftBody(psb);
+	
 	btTransform startTransform;
 	startTransform.setIdentity();
 	startTransform.setOrigin(btVector3(0,20,0));
 	btRigidBody*		body=pdemo->localCreateRigidBody(10,startTransform,new btBoxShape(btVector3(2,2,2)));
 }
 
+//
+// Collide
+//
+static void	Init_Collide(SoftDemo* pdemo)
+{
+	//TRACEDEMO
+	struct Functor
+		{
+		static btSoftBody* Create(SoftDemo* pdemo,const btVector3& x,const btVector3& a)
+			{
+			btSoftBody*	psb=btSoftBodyHelpers::CreateFromTriMesh(pdemo->m_softBodyWorldInfo,gVertices,
+				&gIndices[0][0],
+				NUM_TRIANGLES);
+			psb->generateBendingConstraints(2,1);
+			psb->m_cfg.iterations=2;
+			psb->m_cfg.collisions|=btSoftBody::fCollision::VF_SS;
+			psb->randomizeConstraints();
+			btMatrix3x3	m;
+			m.setEulerZYX(a.x(),a.y(),a.z());
+			psb->transform(btTransform(m,x));
+			psb->scale(btVector3(2,2,2));
+			psb->setTotalMass(50,true);
+			pdemo->getSoftDynamicsWorld()->addSoftBody(psb);
+			return(psb);
+			}
+		};
+	for(int i=0;i<3;++i)
+		{
+		Functor::Create(pdemo,btVector3(3*i,2,0),btVector3(SIMD_PI/2*(1-(i&1)),SIMD_PI/2*(i&1),0));
+		}
+	#if 0
+	const btScalar	s=2;
+	const btScalar	s2=s*1.01;
+		{		
+		const btScalar	h=0;
+		const btVector3	p[]={	btVector3(-s,h,-s),btVector3(+s,h,-s),
+								btVector3(+s,h,+s),btVector3(-s,h,+s)};
+		btSoftBody*	psb=new btSoftBody(&pdemo->m_softBodyWorldInfo,4,p,0);
+		psb->appendLink(0,1,1,btSoftBody::eLType::Structural);
+		psb->appendLink(1,2,1,btSoftBody::eLType::Structural);
+		psb->appendLink(2,3,1,btSoftBody::eLType::Structural);
+		psb->appendLink(3,0,1,btSoftBody::eLType::Structural);
+		psb->appendLink(0,2,1,btSoftBody::eLType::Structural);
+		psb->appendFace(0,1,2);
+		psb->appendFace(2,3,0);
+		/*psb->setMass(0,0);
+		psb->setMass(1,0);
+		psb->setMass(2,0);
+		psb->setMass(3,0);*/
+		psb->m_cfg.collisions|=btSoftBody::fCollision::VF_SS;
+		pdemo->getSoftDynamicsWorld()->addSoftBody(psb);
+		}
+		{
+		btVector3	p[]={btVector3(0.5,5,-0.5)};
+		btSoftBody*	psb=new btSoftBody(&pdemo->m_softBodyWorldInfo,1,p,0);
+		//psb->setMass(0,0);
+		psb->m_cfg.collisions|=btSoftBody::fCollision::VF_SS;
+		pdemo->getSoftDynamicsWorld()->addSoftBody(psb);
+		}
+	#endif	
+}
+
+//
+// Collide2
+//
+static void	Init_Collide2(SoftDemo* pdemo)
+{
+	//TRACEDEMO
+	struct Functor
+		{
+		static btSoftBody* Create(SoftDemo* pdemo,const btVector3& x,const btVector3& a)
+			{
+			btSoftBody*	psb=btSoftBodyHelpers::CreateFromTriMesh(pdemo->m_softBodyWorldInfo,gVerticesBunny,
+				&gIndicesBunny[0][0],
+				BUNNY_NUM_TRIANGLES);
+			psb->generateBendingConstraints(2,0.5);
+			psb->m_cfg.iterations	=	2;
+			psb->m_cfg.kDF			=	0.5;
+			psb->m_cfg.collisions	|=	btSoftBody::fCollision::VF_SS;
+			psb->randomizeConstraints();
+			btMatrix3x3	m;
+			m.setEulerZYX(a.x(),a.y(),a.z());
+			psb->transform(btTransform(m,x));
+			psb->scale(btVector3(6,6,6));
+			psb->setTotalMass(100,true);
+			pdemo->getSoftDynamicsWorld()->addSoftBody(psb);
+			return(psb);
+			}
+		};
+	for(int i=0;i<3;++i)
+		{
+		Functor::Create(pdemo,btVector3(0,-1+4*i,0),btVector3(0,SIMD_PI/2*(i&1),0));
+		}	
+}
+
+//
+// Collide3
+//
+static void	Init_Collide3(SoftDemo* pdemo)
+{
+	//TRACEDEMO
+		{
+		const btScalar	s=8;
+		btSoftBody*		psb=btSoftBodyHelpers::CreatePatch(	pdemo->m_softBodyWorldInfo,btVector3(-s,0,-s),
+			btVector3(+s,0,-s),
+			btVector3(-s,0,+s),
+			btVector3(+s,0,+s),
+			15,15,1+2+4+8,true);
+		psb->m_cfg.kLST			=	0.4;
+		psb->m_cfg.collisions	|=	btSoftBody::fCollision::VF_SS;
+		psb->setTotalMass(150);
+		pdemo->getSoftDynamicsWorld()->addSoftBody(psb);
+		}
+		{
+		const btScalar	s=4;
+		const btVector3	o=btVector3(4,10,0);
+		btSoftBody*		psb=btSoftBodyHelpers::CreatePatch(	pdemo->m_softBodyWorldInfo,
+			btVector3(-s,0,-s)+o,
+			btVector3(+s,0,-s)+o,
+			btVector3(-s,0,+s)+o,
+			btVector3(+s,0,+s)+o,
+			15,15,0,true);
+		psb->generateBendingConstraints(2,0.5);
+		psb->m_cfg.kLST			=	0.4;
+		psb->m_cfg.collisions	|=	btSoftBody::fCollision::VF_SS;
+		psb->setTotalMass(150);
+		pdemo->getSoftDynamicsWorld()->addSoftBody(psb);
+		}
+}
+
 //
 // Aerodynamic forces, 50x1g flyers
 //
@@ -539,7 +546,7 @@ static void	Init_Aero(SoftDemo* pdemo)
 			btVector3(+s,h,+s),
 			segments,segments,
 			0,true);
-		psb->GenerateBendingConstraints(2,1);
+		psb->generateBendingConstraints(2,1);
 		psb->m_cfg.kLF			=	0.004;
 		psb->m_cfg.kDG			=	0.0003;
 		psb->m_cfg.aeromodel	=	btSoftBody::eAeroModel::V_TwoSided;
@@ -551,10 +558,10 @@ static void	Init_Aero(SoftDemo* pdemo)
 		trs.setIdentity();
 		trs.setOrigin(rp);
 		trs.setRotation(rot);
-		psb->Transform(trs);
-		psb->SetTotalMass(0.1);
-		psb->AddForce(btVector3(0,2,0),0);
-		pdemo->m_softbodies.push_back(psb);
+		psb->transform(trs);
+		psb->setTotalMass(0.1);
+		psb->addForce(btVector3(0,2,0),0);
+		pdemo->getSoftDynamicsWorld()->addSoftBody(psb);
 
 	}
 	pdemo->m_autocam=true;
@@ -573,7 +580,7 @@ static void	Init_Friction(SoftDemo* pdemo)
 		const btVector3	p(-ni*ts/2+i*ts,-10+bs,40);
 		btSoftBody*		psb=Ctor_SoftBox(pdemo,p,btVector3(bs,bs,bs));
 		psb->m_cfg.kDF	=	0.1 * ((i+1)/(btScalar)ni);
-		psb->AddVelocity(btVector3(0,0,-10));
+		psb->addVelocity(btVector3(0,0,-10));
 	}
 }
 
@@ -588,9 +595,10 @@ static void	Init_Pressure(SoftDemo* pdemo)
 		512);
 	psb->m_cfg.kLST		=	0.1;
 	psb->m_cfg.kDF		=	1;
+	psb->m_cfg.kDP		=	0.008; // fun factor...
 	psb->m_cfg.kPR		=	2500;
-	psb->SetTotalMass(30,true);
-	pdemo->m_softbodies.push_back(psb);
+	psb->setTotalMass(30,true);
+	pdemo->getSoftDynamicsWorld()->addSoftBody(psb);
 
 	Ctor_BigPlate(pdemo);
 	Ctor_LinearStair(pdemo,btVector3(0,0,0),btVector3(2,1,5),0,10);
@@ -608,9 +616,9 @@ static void	Init_Volume(SoftDemo* pdemo)
 		512);
 	psb->m_cfg.kLST		=	0.45;
 	psb->m_cfg.kVC		=	20;
-	psb->SetTotalMass(50,true);
-	psb->SetPose(true,false);
-	pdemo->m_softbodies.push_back(psb);
+	psb->setTotalMass(50,true);
+	psb->setPose(true,false);
+	pdemo->getSoftDynamicsWorld()->addSoftBody(psb);
 
 	Ctor_BigPlate(pdemo);
 	Ctor_LinearStair(pdemo,btVector3(0,0,0),btVector3(2,1,5),0,10);
@@ -644,11 +652,11 @@ static void	Init_Sticks(SoftDemo* pdemo)
 			psb->m_cfg.kCHR		=	0.1;
 			for(int i=0;i<3;++i)
 			{
-				psb->GenerateBendingConstraints(2+i,1);
+				psb->generateBendingConstraints(2+i,1);
 			}
-			psb->SetMass(1,0);
-			psb->SetTotalMass(0.01);
-			pdemo->m_softbodies.push_back(psb);
+			psb->setMass(1,0);
+			psb->setTotalMass(0.01);
+			pdemo->getSoftDynamicsWorld()->addSoftBody(psb);
 
 		}
 	}
@@ -670,10 +678,10 @@ static void	Init_Cloth(SoftDemo* pdemo)
 
 		//		31,31,
 		1+2+4+8,true);
-	psb->GenerateBendingConstraints(2,1);
+	psb->generateBendingConstraints(2,1);
 	psb->m_cfg.kLST			=	0.4;
-	psb->SetTotalMass(150);
-	pdemo->m_softbodies.push_back(psb);
+	psb->setTotalMass(150);
+	pdemo->getSoftDynamicsWorld()->addSoftBody(psb);
 
 	Ctor_RbUpStack(pdemo,10);
 }
@@ -687,13 +695,13 @@ static void	Init_Bunny(SoftDemo* pdemo)
 	btSoftBody*	psb=btSoftBodyHelpers::CreateFromTriMesh(pdemo->m_softBodyWorldInfo,gVerticesBunny,
 		&gIndicesBunny[0][0],
 		BUNNY_NUM_TRIANGLES);
-	psb->GenerateBendingConstraints(2,0.5);
+	psb->generateBendingConstraints(2,0.5);
 	psb->m_cfg.iterations	=	2;
 	psb->m_cfg.kDF			=	0.5;
-	psb->RandomizeConstraints();
-	psb->Scale(btVector3(6,6,6));
-	psb->SetTotalMass(100,true);
-	pdemo->m_softbodies.push_back(psb);	
+	psb->randomizeConstraints();
+	psb->scale(btVector3(6,6,6));
+	psb->setTotalMass(100,true);
+	pdemo->getSoftDynamicsWorld()->addSoftBody(psb);
 
 }
 
@@ -706,15 +714,14 @@ static void	Init_BunnyMatch(SoftDemo* pdemo)
 	btSoftBody*	psb=btSoftBodyHelpers::CreateFromTriMesh(pdemo->m_softBodyWorldInfo,	gVerticesBunny,
 		&gIndicesBunny[0][0],
 		BUNNY_NUM_TRIANGLES);
-	psb->GenerateBendingConstraints(2,0.5);
 	psb->m_cfg.kDF		=	0.5;
 	psb->m_cfg.kLST		=	0.1;
-	psb->m_cfg.kMT		=	0.01;
-	psb->RandomizeConstraints();
-	psb->Scale(btVector3(6,6,6));
-	psb->SetTotalMass(100,true);
-	psb->SetPose(true,true);
-	pdemo->m_softbodies.push_back(psb);	
+	psb->m_cfg.kMT		=	0.05;
+	psb->randomizeConstraints();
+	psb->scale(btVector3(6,6,6));
+	psb->setTotalMass(100,true);
+	psb->setPose(true,true);
+	pdemo->getSoftDynamicsWorld()->addSoftBody(psb);	
 
 }
 
@@ -727,15 +734,15 @@ static void	Init_Torus(SoftDemo* pdemo)
 	btSoftBody*	psb=btSoftBodyHelpers::CreateFromTriMesh(	pdemo->m_softBodyWorldInfo,	gVertices,
 		&gIndices[0][0],
 		NUM_TRIANGLES);
-	psb->GenerateBendingConstraints(2,1);
+	psb->generateBendingConstraints(2,1);
 	psb->m_cfg.iterations=2;
-	psb->RandomizeConstraints();
+	psb->randomizeConstraints();
 	btMatrix3x3	m;
 	m.setEulerZYX(SIMD_PI/2,0,0);
-	psb->Transform(btTransform(m,btVector3(0,4,0)));
-	psb->Scale(btVector3(2,2,2));
-	psb->SetTotalMass(50,true);
-	pdemo->m_softbodies.push_back(psb);
+	psb->transform(btTransform(m,btVector3(0,4,0)));
+	psb->scale(btVector3(2,2,2));
+	psb->setTotalMass(50,true);
+	pdemo->getSoftDynamicsWorld()->addSoftBody(psb);
 
 }
 
@@ -748,22 +755,21 @@ static void	Init_TorusMatch(SoftDemo* pdemo)
 	btSoftBody*	psb=btSoftBodyHelpers::CreateFromTriMesh(pdemo->m_softBodyWorldInfo,	gVertices,
 		&gIndices[0][0],
 		NUM_TRIANGLES);
-	psb->GenerateBendingConstraints(2,1);
 	psb->m_cfg.kLST=0.1;
 	psb->m_cfg.kMT=0.05;
-	psb->RandomizeConstraints();
+	psb->randomizeConstraints();
 	btMatrix3x3	m;
 	m.setEulerZYX(SIMD_PI/2,0,0);
-	psb->Transform(btTransform(m,btVector3(0,4,0)));
-	psb->Scale(btVector3(2,2,2));
-	psb->SetTotalMass(50,true);
-	psb->SetPose(true,true);
-	pdemo->m_softbodies.push_back(psb);
+	psb->transform(btTransform(m,btVector3(0,4,0)));
+	psb->scale(btVector3(2,2,2));
+	psb->setTotalMass(50,true);
+	psb->setPose(true,true);
+	pdemo->getSoftDynamicsWorld()->addSoftBody(psb);
 
 
 }
 
-static unsigned	current_demo=1;
+static unsigned	current_demo=14;
 
 void	SoftDemo::clientResetScene()
 {
@@ -777,15 +783,17 @@ void	SoftDemo::clientResetScene()
 		{
 			delete body->getMotionState();
 		}
-		m_dynamicsWorld->removeCollisionObject(obj);
+		btSoftBody* softBody = btSoftBody::upcast(obj);
+		if (softBody)
+		{
+			getSoftDynamicsWorld()->removeSoftBody(softBody);
+		} else
+		{
+			m_dynamicsWorld->removeCollisionObject(obj);
+		}
 		delete obj;
 	}
-	for(int i=0;i<m_softbodies.size();++i)
-	{
-		btSoftBody* softbody = m_softbodies[i];
-		delete softbody;
-	}
-	m_softbodies.clear();
+	
 	m_softBodyWorldInfo.m_sparsesdf.Reset();
 	/* Init		*/ 
 	void (*demofncs[])(SoftDemo*)=
@@ -797,6 +805,9 @@ void	SoftDemo::clientResetScene()
 		Init_RopeAttach,
 		Init_ClothAttach,
 		Init_Sticks,	
+		Init_Collide,
+		Init_Collide2,
+		Init_Collide3,
 		Init_Impact,
 		Init_Aero,
 		Init_Friction,			
@@ -816,6 +827,7 @@ void	SoftDemo::clientResetScene()
 
 
 	m_autocam						=	false;
+	m_showtrees						=	0;
 	demofncs[current_demo](this);
 }
 
@@ -823,12 +835,17 @@ void	SoftDemo::renderme()
 {
 	DemoApplication::renderme();
 	btIDebugDraw*	idraw=m_dynamicsWorld->getDebugDrawer();
+	
+	m_dynamicsWorld->debugDrawWorld();
+	
 	/* Bodies		*/ 
 	btVector3	ps(0,0,0);
 	int			nps=0;
-	for(int ib=0;ib<m_softbodies.size();++ib)
+	
+	btSoftBodyArray&	sbs=getSoftDynamicsWorld()->getSoftBodyArray();
+	for(int ib=0;ib<sbs.size();++ib)
 	{
-		btSoftBody*	psb=m_softbodies[ib];
+		btSoftBody*	psb=sbs[ib];
 		nps+=psb->getNodes().size();
 		for(int i=0;i<psb->getNodes().size();++i)
 		{
@@ -836,6 +853,8 @@ void	SoftDemo::renderme()
 		}
 		btSoftBodyHelpers::DrawFrame(psb,idraw);
 		btSoftBodyHelpers::Draw(psb,idraw,fDrawFlags::Std);
+		if(m_showtrees&1)	
+			btSoftBodyHelpers::DrawNodeTree(psb,idraw);
 	}
 	ps/=nps;
 	if(m_autocam)
@@ -867,6 +886,7 @@ void	SoftDemo::keyboardCallback(unsigned char key, int x, int y)
 	case	']':	++current_demo;clientResetScene();break;
 	case	'[':	--current_demo;clientResetScene();break;
 	case	';':	m_autocam=!m_autocam;break;
+	case	',':	m_showtrees+=1;break;
 	default:		DemoApplication::keyboardCallback(key,x,y);
 	}
 }
@@ -910,7 +930,7 @@ void	SoftDemo::initPhysics()
 
 	m_solver = solver;
 
-	btDiscreteDynamicsWorld* world = new btDiscreteDynamicsWorld(m_dispatcher,m_broadphase,m_solver,m_collisionConfiguration);
+	btDiscreteDynamicsWorld* world = new btSoftRigidDynamicsWorld(m_dispatcher,m_broadphase,m_solver,m_collisionConfiguration);
 	m_dynamicsWorld = world;
 
 
@@ -922,8 +942,6 @@ void	SoftDemo::initPhysics()
 
 
 
-	int i;
-
 	btTransform tr;
 	tr.setIdentity();
 	tr.setOrigin(btVector3(0,-12,0));
@@ -991,3 +1009,4 @@ void	SoftDemo::exitPhysics()
 }
 
 
+
diff --git a/Demos/SoftDemo/SoftDemo.h b/Demos/SoftDemo/SoftDemo.h
index 2628f3d04..6081711a7 100644
--- a/Demos/SoftDemo/SoftDemo.h
+++ b/Demos/SoftDemo/SoftDemo.h
@@ -36,6 +36,7 @@ class btSoftSoftCollisionAlgorithm;
 
 ///collisions between a btSoftBody and a btRigidBody
 class btSoftRididCollisionAlgorithm;
+class btSoftRigidDynamicsWorld;
 
 
 ///CcdPhysicsDemo shows basic stacking using Bullet physics, and allows toggle of Ccd (using key '1')
@@ -49,10 +50,9 @@ public:
 
 	btSoftBody::btSoftBodyWorldInfo	m_softBodyWorldInfo;
 
-	btAlignedObjectArray<btSoftBody*>	m_softbodies;
-
 	
 	bool								m_autocam;
+	int									m_showtrees;
 
 
 	//keep the collision shapes, for deletion/cleanup
@@ -95,6 +95,20 @@ public:
 		return demo;
 	}
 
+	const btSoftRigidDynamicsWorld*	getSoftDynamicsWorld() const
+	{
+		///just make it a btSoftRigidDynamicsWorld please
+		///or we will add type checking
+		return (btSoftRigidDynamicsWorld*) m_dynamicsWorld;
+	}
+
+	btSoftRigidDynamicsWorld*	getSoftDynamicsWorld()
+	{
+		///just make it a btSoftRigidDynamicsWorld please
+		///or we will add type checking
+		return (btSoftRigidDynamicsWorld*) m_dynamicsWorld;
+	}
+
 	//
 	void	clientResetScene();
 	void	renderme();
@@ -104,3 +118,4 @@ public:
 
 #endif //CCD_PHYSICS_DEMO_H
 
+
diff --git a/Demos/SoftDemo/btSoftRigidCollisionAlgorithm.cpp b/Demos/SoftDemo/btSoftRigidCollisionAlgorithm.cpp
index 9d4f880b2..c8c1b8ef3 100644
--- a/Demos/SoftDemo/btSoftRigidCollisionAlgorithm.cpp
+++ b/Demos/SoftDemo/btSoftRigidCollisionAlgorithm.cpp
@@ -31,11 +31,11 @@ btSoftRigidCollisionAlgorithm::btSoftRigidCollisionAlgorithm(btPersistentManifol
 m_isSwapped(isSwapped)
 {
 
-	m_softBody =  m_isSwapped? (btSoftBody*)col1 : (btSoftBody*)col0;
-	m_rigidCollisionObject = m_isSwapped? col0 : col1;
+	//m_softBody =  m_isSwapped? (btSoftBody*)col1 : (btSoftBody*)col0;
+	//m_rigidCollisionObject = m_isSwapped? col0 : col1;
 
 	//quick fix, add overlapping rigidbody to softbody, so it can be handled within btSoftBody::Step method
-	m_softBody->m_overlappingRigidBodies.push_back(m_rigidCollisionObject);
+	//m_softBody->m_overlappingRigidBodies.push_back(m_rigidCollisionObject);
 
 	///store the contacts straight into the btSoftBody for now?
 
@@ -54,7 +54,7 @@ m_isSwapped(isSwapped)
 btSoftRigidCollisionAlgorithm::~btSoftRigidCollisionAlgorithm()
 {
 	
-	m_softBody->m_overlappingRigidBodies.remove(m_rigidCollisionObject);
+	//m_softBody->m_overlappingRigidBodies.remove(m_rigidCollisionObject);
 
 	/*if (m_ownManifold)
 	{
@@ -76,7 +76,8 @@ void btSoftRigidCollisionAlgorithm::processCollision (btCollisionObject* body0,b
 
 	btSoftBody* softBody =  m_isSwapped? (btSoftBody*)body1 : (btSoftBody*)body0;
 	btCollisionObject* rigidCollisionObject = m_isSwapped? body0 : body1;
-	///do your stuff here
+	
+	softBody->defaultCollisionHandler(rigidCollisionObject);
 
 
 }
@@ -92,3 +93,4 @@ btScalar btSoftRigidCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject*
 	return btScalar(1.);
 }
 
+
diff --git a/Demos/SoftDemo/btSoftRigidCollisionAlgorithm.h b/Demos/SoftDemo/btSoftRigidCollisionAlgorithm.h
index 6800d9f4b..351952ffe 100644
--- a/Demos/SoftDemo/btSoftRigidCollisionAlgorithm.h
+++ b/Demos/SoftDemo/btSoftRigidCollisionAlgorithm.h
@@ -23,7 +23,7 @@ class btPersistentManifold;
 #include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h"
 
 #include "LinearMath/btVector3.h"
-struct  btSoftBody;
+class btSoftBody;
 
 /// btSoftRigidCollisionAlgorithm  provides collision detection between btSoftBody and btRigidBody
 class btSoftRigidCollisionAlgorithm : public btCollisionAlgorithm
@@ -67,3 +67,4 @@ public:
 
 #endif //SOFT_RIGID_COLLISION_ALGORITHM_H
 
+
diff --git a/Demos/SoftDemo/btSoftSoftCollisionAlgorithm.cpp b/Demos/SoftDemo/btSoftSoftCollisionAlgorithm.cpp
index e6fd73596..d03d82999 100644
--- a/Demos/SoftDemo/btSoftSoftCollisionAlgorithm.cpp
+++ b/Demos/SoftDemo/btSoftSoftCollisionAlgorithm.cpp
@@ -26,11 +26,11 @@ btSoftSoftCollisionAlgorithm::btSoftSoftCollisionAlgorithm(btPersistentManifold*
 //m_ownManifold(false),
 //m_manifoldPtr(mf)
 {
-	m_softBody0 = (btSoftBody*) obj0;
+	/*m_softBody0 = (btSoftBody*) obj0;
 	m_softBody1 = (btSoftBody*) obj1;
 
 	m_softBody0->m_overlappingSoftBodies.push_back(m_softBody1);
-	m_softBody1->m_overlappingSoftBodies.push_back(m_softBody0);
+	m_softBody1->m_overlappingSoftBodies.push_back(m_softBody0);*/
 
 	/*if (!m_manifoldPtr && m_dispatcher->needsCollision(obj0,obj1))
 	{
@@ -43,8 +43,8 @@ btSoftSoftCollisionAlgorithm::btSoftSoftCollisionAlgorithm(btPersistentManifold*
 
 btSoftSoftCollisionAlgorithm::~btSoftSoftCollisionAlgorithm()
 {
-	m_softBody0->m_overlappingSoftBodies.remove(m_softBody1);
-	m_softBody1->m_overlappingSoftBodies.remove(m_softBody0);
+	//m_softBody0->m_overlappingSoftBodies.remove(m_softBody1);
+	//m_softBody1->m_overlappingSoftBodies.remove(m_softBody0);
 
 	//this gets called when the overlap stops.
 
@@ -62,10 +62,11 @@ btSoftSoftCollisionAlgorithm::~btSoftSoftCollisionAlgorithm()
 
 void btSoftSoftCollisionAlgorithm::processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
 {
+	
+	btSoftBody* soft0 =	(btSoftBody*)body0;
+	btSoftBody* soft1 =	(btSoftBody*)body1;
 
-	btCollisionObject*	col0 = body0;
-	btCollisionObject*	col1 = body1;
-
+	soft0->defaultCollisionHandler(soft1);
 	/*
 	btBoxShape* box0 = (btBoxShape*)col0->getCollisionShape();
 	btBoxShape* box1 = (btBoxShape*)col1->getCollisionShape();
diff --git a/Demos/SoftDemo/btSoftSoftCollisionAlgorithm.h b/Demos/SoftDemo/btSoftSoftCollisionAlgorithm.h
index bcca080df..b1627603d 100644
--- a/Demos/SoftDemo/btSoftSoftCollisionAlgorithm.h
+++ b/Demos/SoftDemo/btSoftSoftCollisionAlgorithm.h
@@ -22,7 +22,7 @@ subject to the following restrictions:
 #include "BulletCollision/CollisionDispatch/btCollisionCreateFunc.h"
 
 class btPersistentManifold;
-struct btSoftBody;
+class btSoftBody;
 
 ///collision detection between two btSoftBody shapes
 class btSoftSoftCollisionAlgorithm : public btCollisionAlgorithm
@@ -60,3 +60,4 @@ public:
 
 #endif //SOFT_SOFT_COLLISION_ALGORITHM_H
 
+
diff --git a/Extras/BulletColladaConverter/ColladaConverter.cpp b/Extras/BulletColladaConverter/ColladaConverter.cpp
index 64214ba6f..48cdb66b1 100644
--- a/Extras/BulletColladaConverter/ColladaConverter.cpp
+++ b/Extras/BulletColladaConverter/ColladaConverter.cpp
@@ -26,7 +26,7 @@ subject to the following restrictions:
 #include "dom/domCOLLADA.h"
 #include "dae/domAny.h"
 #include "dom/domConstants.h"
-#include "btShapeHull.h"
+#include "BulletCollision/CollisionShapes/btShapeHull.h"
 
 #include "BulletCollision/CollisionShapes/btBoxShape.h"
 #include "BulletCollision/CollisionShapes/btSphereShape.h"
diff --git a/Extras/CMakeLists.txt b/Extras/CMakeLists.txt
index cfdbc4779..ae0d3cddc 100644
--- a/Extras/CMakeLists.txt
+++ b/Extras/CMakeLists.txt
@@ -1 +1 @@
-SUBDIRS( glui ConvexDecomposition ConvexHull BulletMultiThreaded BulletColladaConverter LibXML COLLADA_DOM GIMPACTUtils GIMPACT )
+SUBDIRS( glui ConvexDecomposition BulletMultiThreaded BulletColladaConverter LibXML COLLADA_DOM GIMPACTUtils GIMPACT )
diff --git a/Extras/ConvexHull/CMakeLists.txt b/Extras/ConvexHull/CMakeLists.txt
deleted file mode 100644
index 56a0517fa..000000000
--- a/Extras/ConvexHull/CMakeLists.txt
+++ /dev/null
@@ -1,10 +0,0 @@
-INCLUDE_DIRECTORIES(
-${BULLET_PHYSICS_SOURCE_DIR}/src ${BULLET_PHYSICS_SOURCE_DIR}/Extras/ConvexHull
-)
-	
-ADD_LIBRARY(LibConvexHull
-btConvexHull.h
-btConvexHull.cpp
-btShapeHull.cpp
-btShapeHull.h
-)
diff --git a/Extras/ConvexHull/Jamfile b/Extras/ConvexHull/Jamfile
deleted file mode 100644
index 2869eb9cf..000000000
--- a/Extras/ConvexHull/Jamfile
+++ /dev/null
@@ -1,17 +0,0 @@
-SubDir TOP Extras ConvexHull ;
-
-
-Library convexhull : [ Wildcard *.h *.cpp ] : noinstall ;
-#internal header path to compile convexhull
-CFlags convexhull : [ FIncludes $(TOP)/Extras/ConvexHull ] ;
-
-#expose header include path for apps that depend on ConvexHull
-convexhull.CFLAGS = [ FIncludes $(TOP)/Extras/ConvexHull ] ;
-#same for msvcgen
-MsvcGenConfig convexhull.INCDIRS : $(TOP)/Extras/ConvexHull ;
-
-#for the include paths
-LibDepends convexhull : bulletcollision  ;
-
-
-#InstallHeader [ Wildcard *.h ] : ConvexHull ;
diff --git a/Extras/ConvexHull/ibmsdk/Makefile b/Extras/ConvexHull/ibmsdk/Makefile
deleted file mode 100644
index a0fb47286..000000000
--- a/Extras/ConvexHull/ibmsdk/Makefile
+++ /dev/null
@@ -1,38 +0,0 @@
-#### Source code Dirs
-VPATH =	../
-
-ROOT = ../../..
-HOST = /usr/include
-
-#### Library
-LIBRARY_ppu = bulletconvexhull.a
-
-#### Compiler flags
-CPPFLAGS        = -DUSE_LIBSPE2		\
--I$(ROOT)/Extras/		\
--I$(ROOT)/src/			\
--I$(SDKINC)			\
--I$(HOST)
-
-#### Optimization level flags
-#CC_OPT_LEVEL =  $(CC_OPT_LEVEL_DEBUG)
-CC_OPT_LEVEL =  -O3
-
-##### Objects to be archived in lib
-OBJS = 				\
-btConvexHull.o			\
-btShapeHull.o
-
-#### Install directories
-INSTALL_DIR	= $(ROOT)/lib/ibmsdk/
-INSTALL_FILES	= $(LIBRARY)
-
-IBM_CELLSDK_VERSION := $(shell if [ -d /opt/cell ]; then echo "3.0"; fi)
-
-ifeq ("$(IBM_CELLSDK_VERSION)","3.0")
-        CELL_TOP ?= /opt/cell/sdk
-        include $(CELL_TOP)/buildutils/make.footer
-else
-        CELL_TOP ?= /opt/ibm/cell-sdk/prototype
-        include $(CELL_TOP)/make.footer
-endif
diff --git a/Extras/Jamfile b/Extras/Jamfile
index fd6266ee9..9a3e64dda 100644
--- a/Extras/Jamfile
+++ b/Extras/Jamfile
@@ -3,7 +3,6 @@ SubDir TOP Extras ;
 SubInclude TOP Extras ConvexDecomposition ;
 SubInclude TOP Extras COLLADA_DOM ;
 SubInclude TOP Extras glui ;
-SubInclude TOP Extras ConvexHull ;
 SubInclude TOP Extras LibXML ;
 SubInclude TOP Extras BulletColladaConverter ;
 SubInclude TOP Extras BulletMultiThreaded ;
diff --git a/src/BulletCollision/CollisionDispatch/btCollisionObject.cpp b/src/BulletCollision/CollisionDispatch/btCollisionObject.cpp
index 12267e2a9..4831efe72 100644
--- a/src/BulletCollision/CollisionDispatch/btCollisionObject.cpp
+++ b/src/BulletCollision/CollisionDispatch/btCollisionObject.cpp
@@ -27,7 +27,7 @@ btCollisionObject::btCollisionObject()
 		m_friction(btScalar(0.5)),
 		m_restitution(btScalar(0.)),
 		m_userObjectPointer(0),
-		m_internalOwner(0),
+		m_internalType(CO_COLLISION_OBJECT),
 		m_hitFraction(btScalar(1.)),
 		m_ccdSweptSphereRadius(btScalar(0.)),
 		m_ccdSquareMotionThreshold(btScalar(0.)),
diff --git a/src/BulletCollision/CollisionDispatch/btCollisionObject.h b/src/BulletCollision/CollisionDispatch/btCollisionObject.h
index bc7e68471..a58c4d3ec 100644
--- a/src/BulletCollision/CollisionDispatch/btCollisionObject.h
+++ b/src/BulletCollision/CollisionDispatch/btCollisionObject.h
@@ -66,8 +66,9 @@ protected:
 	///users can point to their objects, m_userPointer is not used by Bullet, see setUserPointer/getUserPointer
 	void*			m_userObjectPointer;
 
-	///m_internalOwner is reserved to point to Bullet's btRigidBody. Don't use this, use m_userObjectPointer instead.
-	void*			m_internalOwner;
+	///m_internalType is reserved to distinguish Bullet's btCollisionObject, btRigidBody, btSoftBody etc.
+	///do not assign your own m_internalType unless you write a new dynamics object class.
+	int				m_internalType;
 
 	///time of impact calculation
 	btScalar		m_hitFraction; 
@@ -100,6 +101,12 @@ public:
 		CF_CUSTOM_MATERIAL_CALLBACK = 8//this allows per-triangle material (friction/restitution)
 	};
 
+	enum	CollisionObjectTypes
+	{
+		CO_COLLISION_OBJECT =1,
+		CO_RIGID_BODY,
+		CO_SOFT_BODY
+	};
 
 	SIMD_FORCE_INLINE bool mergesSimulationIslands() const
 	{
@@ -189,14 +196,9 @@ public:
 	}
 
 	///reserved for Bullet internal usage
-	void*	getInternalOwner()
+	int	getInternalType() const
 	{
-		return m_internalOwner;
-	}
-
-	const void*	getInternalOwner() const
-	{
-		return m_internalOwner;
+		return m_internalType;
 	}
 
 	btTransform&	getWorldTransform()
diff --git a/Extras/ConvexHull/btShapeHull.cpp b/src/BulletCollision/CollisionShapes/btShapeHull.cpp
similarity index 100%
rename from Extras/ConvexHull/btShapeHull.cpp
rename to src/BulletCollision/CollisionShapes/btShapeHull.cpp
diff --git a/Extras/ConvexHull/btShapeHull.h b/src/BulletCollision/CollisionShapes/btShapeHull.h
similarity index 100%
rename from Extras/ConvexHull/btShapeHull.h
rename to src/BulletCollision/CollisionShapes/btShapeHull.h
diff --git a/src/BulletDynamics/Dynamics/btRigidBody.cpp b/src/BulletDynamics/Dynamics/btRigidBody.cpp
index 0eeea241e..54af82224 100644
--- a/src/BulletDynamics/Dynamics/btRigidBody.cpp
+++ b/src/BulletDynamics/Dynamics/btRigidBody.cpp
@@ -40,6 +40,8 @@ btRigidBody::btRigidBody(btScalar mass, btMotionState *motionState, btCollisionS
 void	btRigidBody::setupRigidBody(const btRigidBody::btRigidBodyConstructionInfo& constructionInfo)
 {
 
+	m_internalType=CO_RIGID_BODY;
+
 	m_linearVelocity.setValue(btScalar(0.0), btScalar(0.0), btScalar(0.0));
 	m_angularVelocity.setValue(btScalar(0.),btScalar(0.),btScalar(0.));
 	m_angularFactor = btScalar(1.);
@@ -78,9 +80,6 @@ void	btRigidBody::setupRigidBody(const btRigidBody::btRigidBodyConstructionInfo&
 	m_collisionShape = constructionInfo.m_collisionShape;
 	m_debugBodyId = uniqueId++;
 	
-	//m_internalOwner is to allow upcasting from collision object to rigid body
-	m_internalOwner = this;
-
 	setMassProps(constructionInfo.m_mass, constructionInfo.m_localInertia);
     setDamping(constructionInfo.m_linearDamping, constructionInfo.m_angularDamping);
 	updateInertiaTensor();
diff --git a/src/BulletDynamics/Dynamics/btRigidBody.h b/src/BulletDynamics/Dynamics/btRigidBody.h
index 4276c2302..fef5e4600 100644
--- a/src/BulletDynamics/Dynamics/btRigidBody.h
+++ b/src/BulletDynamics/Dynamics/btRigidBody.h
@@ -160,11 +160,15 @@ public:
 	///but a rigidbody is derived from btCollisionObject, so we can safely perform an upcast
 	static const btRigidBody*	upcast(const btCollisionObject* colObj)
 	{
-		return (const btRigidBody*)colObj->getInternalOwner();
+		if (colObj->getInternalType()==btCollisionObject::CO_RIGID_BODY)
+			return (const btRigidBody*)colObj;
+		return 0;
 	}
 	static btRigidBody*	upcast(btCollisionObject* colObj)
 	{
-		return (btRigidBody*)colObj->getInternalOwner();
+		if (colObj->getInternalType()==btCollisionObject::CO_RIGID_BODY)
+			return (btRigidBody*)colObj;
+		return 0;
 	}
 	
 	/// continuous collision detection needs prediction
diff --git a/src/BulletDynamics/SoftBody/btDbvt.cpp b/src/BulletDynamics/SoftBody/btDbvt.cpp
new file mode 100644
index 000000000..186693f2d
--- /dev/null
+++ b/src/BulletDynamics/SoftBody/btDbvt.cpp
@@ -0,0 +1,536 @@
+#include "btDbvt.h"
+
+#include <stdio.h>
+
+namespace btdbvt_internals
+{
+
+//
+typedef btAlignedObjectArray<btDbvt::Node*>	tNodeArray;
+
+//
+static inline int				indexof(const btDbvt::Node* node)
+{
+return(node->parent->childs[1]==node);
+}
+
+//
+static inline btDbvt::Aabb		merge(		const btDbvt::Aabb& a,
+											const btDbvt::Aabb& b)
+{
+btDbvt::Aabb	res;
+Merge(a,b,res);
+return(res);
+}
+
+
+// volume+edge lengths
+static inline btScalar			size(const btDbvt::Aabb& a)
+{
+const btVector3	edges=a.Lengths();
+return(	edges.x()*edges.y()*edges.z()+
+		edges.x()+edges.y()+edges.z());
+}
+
+//
+static inline void				deletenode(	btDbvt* pdbvt,
+											btDbvt::Node* node)
+{
+delete pdbvt->m_free;
+pdbvt->m_free=node;
+}
+	
+//
+static inline void				recursedeletenode(	btDbvt* pdbvt,
+													btDbvt::Node* node)
+{
+if(!node->isleaf())
+	{
+	recursedeletenode(pdbvt,node->childs[0]);
+	recursedeletenode(pdbvt,node->childs[1]);
+	}
+if(node==pdbvt->m_root) pdbvt->m_root=0;
+deletenode(pdbvt,node);
+}
+
+//
+static inline btDbvt::Node*		createnode(	btDbvt* pdbvt,
+											btDbvt::Node* parent,
+											const btDbvt::Aabb& box,
+											void* data)
+{
+btDbvt::Node*	node;
+if(pdbvt->m_free)
+	{ node=pdbvt->m_free;pdbvt->m_free=0; }
+	else
+	{ node=new btDbvt::Node(); }
+node->parent	=	parent;
+node->box		=	box;
+node->data		=	data;
+node->childs[1]	=	0;
+return(node);
+}
+
+//
+static inline void				insertleaf(	btDbvt* pdbvt,
+											btDbvt::Node* root,
+											btDbvt::Node* leaf)
+{
+if(!pdbvt->m_root)
+	{
+	pdbvt->m_root	=	leaf;
+	leaf->parent	=	0;
+	}
+	else
+	{
+	if(!root->isleaf())
+		{
+		do	{
+			if(	Proximity(root->childs[0]->box,leaf->box)<
+				Proximity(root->childs[1]->box,leaf->box))
+				root=root->childs[0];
+				else
+				root=root->childs[1];
+			} while(!root->isleaf());
+		}
+	btDbvt::Node*	prev=root->parent;
+	btDbvt::Node*	node=createnode(pdbvt,prev,merge(leaf->box,root->box),0);
+	if(prev)
+		{
+		prev->childs[indexof(root)]	=	node;
+		node->childs[0]				=	root;root->parent=node;
+		node->childs[1]				=	leaf;leaf->parent=node;
+		do	{
+			if(prev->box.Contain(node->box))
+				break;
+				else
+				Merge(prev->childs[0]->box,prev->childs[1]->box,prev->box);
+			node=prev;
+			} while(0!=(prev=node->parent));
+		}
+		else
+		{
+		node->childs[0]	=	root;root->parent=node;
+		node->childs[1]	=	leaf;leaf->parent=node;
+		pdbvt->m_root	=	node;
+		}
+	}
+}
+	
+//
+static inline btDbvt::Node*		removeleaf(	btDbvt* pdbvt,
+											btDbvt::Node* leaf)
+{
+if(leaf==pdbvt->m_root)
+	{
+	pdbvt->m_root=0;
+	return(0);
+	}
+	else
+	{
+	btDbvt::Node*	parent=leaf->parent;
+	btDbvt::Node*	prev=parent->parent;
+	btDbvt::Node*	sibling=parent->childs[1-indexof(leaf)];			
+	if(prev)
+		{
+		prev->childs[indexof(parent)]=sibling;
+		sibling->parent=prev;
+		deletenode(pdbvt,parent);
+		while(prev)
+			{
+			const btDbvt::Aabb	pb=prev->box;
+			Merge(prev->childs[0]->box,prev->childs[1]->box,prev->box);
+			if(NotEqual(pb,prev->box))
+				{
+				sibling		=	prev;
+				prev		=	prev->parent;
+				} else break;
+			}
+		return(prev?prev:pdbvt->m_root);
+		}
+		else
+		{								
+		pdbvt->m_root=sibling;
+		sibling->parent=0;
+		deletenode(pdbvt,parent);
+		return(pdbvt->m_root);
+		}			
+	}
+}
+
+//
+static void						fetchleafs(	btDbvt* pdbvt,
+											btDbvt::Node* root,
+											tNodeArray& leafs,
+											int depth=-1)
+{
+if(root->isinternal()&&depth)
+	{
+	fetchleafs(pdbvt,root->childs[0],leafs,depth-1);
+	fetchleafs(pdbvt,root->childs[1],leafs,depth-1);
+	deletenode(pdbvt,root);
+	}
+	else
+	{
+	leafs.push_back(root);
+	}
+}
+
+//
+static int						leafcount(btDbvt::Node* root)
+{
+if(root->isinternal())
+	{
+	return(	leafcount(root->childs[0])+
+			leafcount(root->childs[1]));
+	}
+return(1);
+}
+
+//
+static void						split(	const tNodeArray& leafs,
+										tNodeArray& left,
+										tNodeArray& right,
+										const btVector3& org,
+										const btVector3& axis)
+{
+left.resize(0);
+right.resize(0);
+for(int i=0,ni=leafs.size();i<ni;++i)
+	{
+	if(dot(axis,leafs[i]->box.Center()-org)<0)
+		left.push_back(leafs[i]);
+		else
+		right.push_back(leafs[i]);
+	}
+}
+
+//
+static btDbvt::Aabb				bounds(	const tNodeArray& leafs)
+{
+btDbvt::Aabb	box=leafs[0]->box;
+for(int i=1,ni=leafs.size();i<ni;++i)
+	{
+	box=merge(box,leafs[i]->box);
+	}
+return(box);
+}
+
+//
+static void						bottomup(	btDbvt* pdbvt,
+											tNodeArray& leafs)
+{
+while(leafs.size()>1)
+	{
+	btScalar	minsize=SIMD_INFINITY;
+	int			minidx[2]={-1,-1};
+	for(int i=0;i<leafs.size();++i)
+		{
+		for(int j=i+1;j<leafs.size();++j)
+			{
+			const btScalar	sz=size(merge(leafs[i]->box,leafs[j]->box));
+			if(sz<minsize)
+				{
+				minsize		=	sz;
+				minidx[0]	=	i;
+				minidx[1]	=	j;
+				}
+			}
+		}
+	btDbvt::Node*	n[]	=	{leafs[minidx[0]],leafs[minidx[1]]};
+	btDbvt::Node*	p	=	createnode(pdbvt,0,merge(n[0]->box,n[1]->box),0);
+	p->childs[0]		=	n[0];
+	p->childs[1]		=	n[1];
+	n[0]->parent		=	p;
+	n[1]->parent		=	p;
+	leafs[minidx[0]]	=	p;
+	leafs.swap(minidx[1],leafs.size()-1);
+	leafs.pop_back();
+	}
+}
+
+//
+static btDbvt::Node*			topdown(btDbvt* pdbvt,
+										tNodeArray& leafs,
+										int bu_treshold)
+{
+static const btVector3	axis[]={btVector3(1,0,0),
+								btVector3(0,1,0),
+								btVector3(0,0,1)};
+if(leafs.size()>1)
+	{
+	if(leafs.size()>bu_treshold)
+		{
+		const btDbvt::Aabb	box=bounds(leafs);
+		const btVector3		org=box.Center();
+		tNodeArray			sets[2];
+		int					bestaxis=-1;
+		int					bestmidp=leafs.size();
+		sets[0].reserve(leafs.size());
+		sets[1].reserve(leafs.size());
+		for(int i=0;i<3;++i)
+			{
+			split(leafs,sets[0],sets[1],org,axis[i]);
+			if((sets[0].size()>0)&&(sets[1].size()>0))
+				{
+				const int	midp=abs(sets[0].size()-sets[1].size());
+				if(midp<bestmidp)
+					{
+					bestaxis=i;
+					bestmidp=midp;
+					}
+				}
+			}
+		if(bestaxis>=0)
+			{
+			split(leafs,sets[0],sets[1],org,axis[bestaxis]);
+			}
+			else
+			{
+			sets[0].resize(0);
+			sets[1].resize(0);
+			for(int i=0,ni=leafs.size();i<ni;++i)
+				{
+				sets[i&1].push_back(leafs[i]);
+				}
+			}
+		btDbvt::Node*	node=createnode(pdbvt,0,box,0);
+		node->childs[0]=topdown(pdbvt,sets[0],bu_treshold);
+		node->childs[1]=topdown(pdbvt,sets[1],bu_treshold);
+		node->childs[0]->parent=node;
+		node->childs[1]->parent=node;
+		return(node);
+		}
+		else
+		{
+		bottomup(pdbvt,leafs);
+		return(leafs[0]);
+		}
+	}
+return(leafs[0]);
+}
+
+//
+static inline btDbvt::Node*		refit(	btDbvt* pdbvt,
+										btDbvt::Node* node)
+{
+btDbvt::Node*	parent=node->parent;
+if(parent)
+	{
+	const int		idx=indexof(node);
+	tNodeArray		leafs;
+	leafs.reserve(64);
+	fetchleafs(pdbvt,node,leafs,3);
+	if(leafs.size()>=2)
+		{
+		bottomup(pdbvt,leafs);
+		node=leafs[0];
+		node->parent=parent;
+		parent->childs[idx]=node;
+		}
+	}
+return(node);
+}
+										
+
+}
+
+using namespace btdbvt_internals;
+
+//
+// Api
+//
+
+//
+				btDbvt::btDbvt()
+{
+m_root	=	0;
+m_free	=	0;
+m_lkhd	=	2;
+}
+
+//
+				btDbvt::~btDbvt()
+{
+clear();
+}
+
+//
+void			btDbvt::clear()
+{
+if(m_root)	recursedeletenode(this,m_root);
+delete m_free;
+m_free=0;
+}
+
+//
+int				btDbvt::leafCount() const
+{
+if(m_root)	return(leafcount(m_root));
+			else
+			return(0);
+}
+
+//
+void			btDbvt::optimizeBottomUp()
+{
+if(m_root)
+	{
+	tNodeArray leafs;
+	leafs.reserve(leafCount());
+	fetchleafs(this,m_root,leafs);
+	bottomup(this,leafs);
+	m_root=leafs[0];
+	}
+}
+
+//
+void			btDbvt::optimizeTopDown(int bu_treshold)
+{
+if(m_root)
+	{
+	tNodeArray	leafs;
+	leafs.reserve(leafCount());
+	fetchleafs(this,m_root,leafs);
+	m_root=topdown(this,leafs,bu_treshold);
+	}
+}
+
+//
+btDbvt::Node*	btDbvt::insert(const Aabb& box,void* data)
+{
+Node*	leaf=createnode(this,0,box,data);
+insertleaf(this,m_root,leaf);
+return(leaf);
+}
+
+//
+void			btDbvt::update(Node* leaf,const Aabb& box)
+{
+Node*	root=removeleaf(this,leaf);
+if(root)
+	{
+	for(int i=0;(i<m_lkhd)&&root->parent;++i)
+		{
+		root=root->parent;
+		}
+	}
+leaf->box=box;
+insertleaf(this,root,leaf);
+}
+
+//
+bool			btDbvt::update(Node* leaf,Aabb box,const btVector3& velocity,btScalar margin)
+{
+if(leaf->box.Contain(box)) return(false);
+if(margin>0)
+	box.Expand(btVector3(margin,margin,margin));
+if(velocity.length2()>0)
+	box.SignedExpand(velocity);
+update(leaf,box);
+return(true);
+}
+
+//
+void			btDbvt::remove(Node* leaf)
+{
+removeleaf(this,leaf);
+deletenode(this,leaf);
+}
+
+//
+void			btDbvt::collide(btDbvt* tree,
+								ICollide* icollide) const
+{
+if(tree->m_root&&m_root)
+	{
+	struct	sStkElm
+		{
+		const Node*	a;
+		const Node*	b;
+		sStkElm(const Node* na,const Node* nb) : a(na),b(nb) {}
+		};
+	btAlignedObjectArray<sStkElm>	stack;
+	stack.reserve(128);
+	stack.push_back(sStkElm(m_root,tree->m_root));
+	do	{
+		sStkElm	p=stack[stack.size()-1];
+		stack.pop_back();
+		if(p.a==p.b)
+			{
+			if(p.a->isinternal())
+				{
+				stack.push_back(sStkElm(p.a->childs[0],p.a->childs[0]));
+				stack.push_back(sStkElm(p.a->childs[1],p.a->childs[1]));
+				stack.push_back(sStkElm(p.a->childs[0],p.a->childs[1]));
+				}
+			}
+		else if(Intersect(p.a->box,p.b->box))
+			{
+			if(p.a->isinternal())
+				{
+				if(p.b->isinternal())
+					{
+					stack.push_back(sStkElm(p.a->childs[0],p.b->childs[0]));
+					stack.push_back(sStkElm(p.a->childs[1],p.b->childs[0]));
+					stack.push_back(sStkElm(p.a->childs[0],p.b->childs[1]));
+					stack.push_back(sStkElm(p.a->childs[1],p.b->childs[1]));
+					}
+					else
+					{
+					stack.push_back(sStkElm(p.a->childs[0],p.b));
+					stack.push_back(sStkElm(p.a->childs[1],p.b));
+					}
+				}
+				else
+				{
+				if(p.b->isinternal())
+					{
+					stack.push_back(sStkElm(p.a,p.b->childs[0]));
+					stack.push_back(sStkElm(p.a,p.b->childs[1]));
+					}
+					else
+					{
+					icollide->Process(p.a,p.b);
+					}
+				}
+			}
+		} while(stack.size()>0);
+	}
+}
+							
+//
+void			btDbvt::collide(const Aabb& box,
+								ICollide* icollide) const
+{
+if(m_root)
+	{
+	btAlignedObjectArray<const Node*>	stack;
+	stack.reserve(64);
+	stack.push_back(m_root);
+	do	{
+		const Node*	n=stack[stack.size()-1];
+		stack.pop_back();
+		if(Intersect(n->box,box))
+			{
+			if(n->isinternal())
+				{
+				stack.push_back(n->childs[0]);
+				stack.push_back(n->childs[1]);
+				}
+				else
+				{
+				icollide->Process(n);
+				}
+			}
+		} while(stack.size()>0);
+	}
+}
+							
+//
+void			btDbvt::collide(const btVector3& org,
+								const btVector3& dir,
+								ICollide* icollide) const
+{
+/* not implemented	*/ 
+}
diff --git a/src/BulletDynamics/SoftBody/btDbvt.h b/src/BulletDynamics/SoftBody/btDbvt.h
new file mode 100644
index 000000000..c2d2c5630
--- /dev/null
+++ b/src/BulletDynamics/SoftBody/btDbvt.h
@@ -0,0 +1,224 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2007 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_DYNAMIC_BOUNDING_VOLUME_TREE_H
+#define BT_DYNAMIC_BOUNDING_VOLUME_TREE_H
+
+#include "LinearMath/btAlignedObjectArray.h"
+#include "LinearMath/btVector3.h"
+
+//
+// Dynamic bounding volume tree
+//
+struct	btDbvt
+	{
+	// Types
+
+	/* Aabb		*/ 
+	struct	Aabb
+		{
+		inline btVector3		Center() const	{ return((mi+mx)/2); }
+		inline btVector3		Extent() const	{ return((mx-mi)/2); }
+		inline const btVector3&	Mins() const	{ return(mi); }
+		inline const btVector3&	Maxs() const	{ return(mx); }
+		inline btVector3		Lengths() const	{ return(mx-mi); }
+		static inline Aabb		FromCE(const btVector3& c,const btVector3& e);
+		static inline Aabb		FromCR(const btVector3& c,btScalar r);
+		static inline Aabb		FromMM(const btVector3& mi,const btVector3& mx);
+		static inline Aabb		FromPoints(const btVector3* pts,int n);
+		static inline Aabb		FromPoints(const btVector3** ppts,int n);
+		inline void				Expand(const btVector3 e);
+		inline void				SignedExpand(const btVector3 e);
+		inline bool				Contain(const Aabb& a) const;
+		inline friend bool		Intersect(	const Aabb& a,
+											const Aabb& b);
+		inline friend btScalar	Proximity(	const Aabb& a,
+											const Aabb& b);
+		inline friend void		Merge(	const Aabb& a,
+										const Aabb& b,
+										Aabb& r);
+		inline friend bool		NotEqual(	const Aabb& a,
+											const Aabb& b);
+		btVector3	mi,mx;
+		};
+	/* Node			*/ 
+	struct	Node
+		{
+		Aabb	box;
+		Node*	parent;
+		bool	isleaf() const		{ return(childs[1]==0); }
+		bool	isinternal() const	{ return(!isleaf()); }
+		union	{
+				Node*	childs[2];
+				void*	data;
+				};
+		};
+	// Interfaces
+	
+	/* ICollide	*/ 
+	struct	ICollide
+		{
+		virtual void	Process(const Node* leaf0,const Node* leaf1) {};
+		virtual void	Process(const Node* leaf) {};
+		};
+		
+	// Fields
+	Node*			m_root;
+	Node*			m_free;
+	int				m_lkhd;
+	// Methods
+					btDbvt();
+					~btDbvt();
+	void			clear();
+	int				leafCount() const;
+	void			optimizeBottomUp();
+	void			optimizeTopDown(int bu_treshold=128);
+	Node*			insert(const Aabb& box,void* data);
+	void			update(Node* leaf,const Aabb& box);
+	bool			update(Node* leaf,Aabb box,const btVector3& velocity,btScalar margin);
+	void			remove(Node* leaf);
+	void			collide(btDbvt* tree,
+							ICollide* icollide) const;
+	void			collide(const Aabb& box,
+							ICollide* icollide) const;
+	void			collide(const btVector3& org,
+							const btVector3& dir,
+							ICollide* icollide) const;
+	//
+	private:
+	btDbvt(const btDbvt&)	{}
+	};
+
+//
+// Inline's
+//
+
+//
+inline btDbvt::Aabb		btDbvt::Aabb::FromCE(const btVector3& c,const btVector3& e)
+{
+Aabb box;
+box.mi=c-e;box.mx=c+e;
+return(box);
+}
+	
+//
+inline btDbvt::Aabb		btDbvt::Aabb::FromCR(const btVector3& c,btScalar r)
+{
+return(FromCE(c,btVector3(r,r,r)));
+}
+	
+//
+inline btDbvt::Aabb		btDbvt::Aabb::FromMM(const btVector3& mi,const btVector3& mx)
+{
+Aabb box;
+box.mi=mi;box.mx=mx;
+return(box);
+}
+	
+//
+inline btDbvt::Aabb		btDbvt::Aabb::FromPoints(const btVector3* pts,int n)
+{
+Aabb box;
+box.mi=box.mx=pts[0];
+for(int i=1;i<n;++i)
+	{
+	box.mi.setMin(pts[i]);
+	box.mx.setMax(pts[i]);
+	}
+return(box);
+}
+
+//
+inline btDbvt::Aabb		btDbvt::Aabb::FromPoints(const btVector3** ppts,int n)
+{
+Aabb box;
+box.mi=box.mx=*ppts[0];
+for(int i=1;i<n;++i)
+	{
+	box.mi.setMin(*ppts[i]);
+	box.mx.setMax(*ppts[i]);
+	}
+return(box);
+}
+
+//
+inline void				btDbvt::Aabb::Expand(const btVector3 e)
+{
+mi-=e;mx+=e;
+}
+	
+//
+inline void				btDbvt::Aabb::SignedExpand(const btVector3 e)
+{
+if(e.x()>0) mx.setX(mx.x()+e.x()); else mi.setX(mi.x()+e.x());
+if(e.y()>0) mx.setY(mx.y()+e.y()); else mi.setY(mi.y()+e.y());
+if(e.z()>0) mx.setZ(mx.z()+e.z()); else mi.setZ(mi.z()+e.z());
+}
+	
+//
+inline bool				btDbvt::Aabb::Contain(const Aabb& a) const
+{
+return(	(mi.x()<=a.mi.x())&&
+		(mi.y()<=a.mi.y())&&
+		(mi.z()<=a.mi.z())&&
+		(mx.x()>=a.mx.x())&&
+		(mx.y()>=a.mx.y())&&
+		(mx.z()>=a.mx.z()));
+}
+	
+//
+inline bool				Intersect(	const btDbvt::Aabb& a,
+									const btDbvt::Aabb& b)
+{
+return(	(a.mi.x()<=b.mx.x())&&
+		(a.mi.y()<=b.mx.y())&&
+		(a.mi.z()<=b.mx.z())&&
+		(a.mx.x()>=b.mi.x())&&
+		(a.mx.y()>=b.mi.y())&&
+		(a.mx.z()>=b.mi.z()));
+}
+	
+//
+inline btScalar			Proximity(	const btDbvt::Aabb& a,
+									const btDbvt::Aabb& b)
+{
+const btVector3	d=(a.mi+a.mx)-(b.mi+b.mx);
+return(btFabs(d.x())+btFabs(d.y())+btFabs(d.z()));
+}
+
+//
+inline void				Merge(	const btDbvt::Aabb& a,
+								const btDbvt::Aabb& b,
+								btDbvt::Aabb& r)
+{
+r=a;
+r.mi.setMin(b.mi);
+r.mx.setMax(b.mx);
+}
+
+//
+inline bool				NotEqual(	const btDbvt::Aabb& a,
+									const btDbvt::Aabb& b)
+{
+return(	(a.mi.x()!=b.mi.x())||
+		(a.mi.y()!=b.mi.y())||
+		(a.mi.z()!=b.mi.z())||
+		(a.mx.x()!=b.mx.x())||
+		(a.mx.y()!=b.mx.y())||
+		(a.mx.z()!=b.mx.z()));
+}
+
+#endif
diff --git a/src/BulletDynamics/SoftBody/btSoftBody.cpp b/src/BulletDynamics/SoftBody/btSoftBody.cpp
index e981d6d0f..db699ef77 100644
--- a/src/BulletDynamics/SoftBody/btSoftBody.cpp
+++ b/src/BulletDynamics/SoftBody/btSoftBody.cpp
@@ -17,20 +17,904 @@ subject to the following restrictions:
 #include "btSoftBody.h"
 #include <stdio.h>
 #include <string.h>
+#include "LinearMath/btQuickprof.h"
 #include "BulletCollision/NarrowPhaseCollision/btGjkEpa2.h"
 #include "BulletCollision/BroadphaseCollision/btBroadphaseInterface.h"
 #include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h"
 
+//
+// Collision shape
+//
 
-btSoftBody::btSoftBody(btSoftBody::btSoftBodyWorldInfo&	worldInfo,int node_count,  const btVector3* x,  const btScalar* m)
-:m_worldInfo(worldInfo)
+///btSoftBodyCollisionShape is work-in-progress collision shape for softbodies
+class btSoftBodyCollisionShape : public btConcaveShape
 {
-	///for now, create a collision shape internally
-	m_softBodyCollisionShape = new btSoftBodyCollisionShape();
+public:
+	btSoftBody*						m_body;
+	btSoftBody::tNodeArray			m_nodes;		// Nodes
+	btSoftBody::tLinkArray			m_links;		// Links
+	btSoftBody::tFaceArray			m_faces;		// Faces
 
-	setCollisionShape(m_softBodyCollisionShape);
+	btSoftBodyCollisionShape(btSoftBody* backptr);
 
-		/* Init		*/ 
+	virtual ~btSoftBodyCollisionShape();
+
+	virtual void	processAllTriangles(btTriangleCallback* callback,const btVector3& aabbMin,const btVector3& aabbMax) const;
+
+	///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 should be identity, but better be safe than...fast? */ 
+	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 int		getShapeType() const
+	{
+		return SOFTBODY_SHAPE_PROXYTYPE;
+	}
+	virtual void	setLocalScaling(const btVector3& scaling)
+	{		
+		///na
+		btAssert(0);
+	}
+	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";
+	}
+
+};
+
+btSoftBodyCollisionShape::btSoftBodyCollisionShape(btSoftBody* backptr)
+{
+m_body=backptr;
+}
+
+btSoftBodyCollisionShape::~btSoftBodyCollisionShape()
+{
+
+}
+
+void	btSoftBodyCollisionShape::processAllTriangles(btTriangleCallback* callback,const btVector3& aabbMin,const btVector3& aabbMax) const
+{
+	//not yet
+	btAssert(0);
+}
+
+
+//
+// Helpers
+//
+
+//
+static inline void			Trace(const btMatrix3x3& m,const char* name)
+{
+	printf("%s[0]: %.2f,\t%.2f,\t%.2f\r\n",name,m[0].x(),m[0].y(),m[0].z());
+	printf("%s[1]: %.2f,\t%.2f,\t%.2f\r\n",name,m[1].x(),m[1].y(),m[1].z());
+	printf("%s[2]: %.2f,\t%.2f,\t%.2f\r\n",name,m[2].x(),m[2].y(),m[2].z());
+	printf("\r\n");
+}
+
+//
+template <typename T>
+static inline T				Lerp(const T& a,const T& b,btScalar t)
+{ return(a+(b-a)*t); }
+//
+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);
+}
+//
+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				Sign(const T& x)
+{ return((T)(x<0?-1:+1)); }
+//
+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 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 void			PolarDecompose(	const btMatrix3x3& m,
+	btMatrix3x3& q,
+	btMatrix3x3& s)
+{
+	static const btScalar	half=(btScalar)0.5;
+	static const btScalar	accuracy=(btScalar)0.00001;
+	static const int		maxiterations=64;
+	btScalar				det=m.determinant();
+	if(!btFuzzyZero(det))
+	{
+		q=m;
+		for(int i=0;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	*/ 
+		q[0]=q[0].normalized();
+		q[1]=q[1].normalized();
+		q[2]=cross(q[0],q[1]).normalized();
+		/* Compute 'S'				*/ 
+		s=q.transpose()*m;
+	}
+	else
+	{
+		q.setIdentity();
+		s.setIdentity();
+	}
+}
+//
+static inline btVector3		ProjectOnAxis(	const btVector3& v,
+	const btVector3& a)
+{
+	return(a*dot(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>(-dot(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=cross(b-a,c-a);
+const btScalar		m2=q.length2();
+if(m2>SIMD_EPSILON)
+	{
+	const btVector3	n=q/btSqrt(m2);
+	const btScalar	k=dot(a,n);
+	const btScalar	k2=k*k;
+	if(k2<sqd)
+		{
+		const btVector3	p=n*k;
+		if(	(dot(cross(a-p,b-p),q)>0)&&
+			(dot(cross(b-p,c-p),q)>0)&&
+			(dot(cross(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[]={	cross(a-p,b-p).length(),
+						cross(b-p,c-p).length(),
+						cross(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 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 void					PointersToIndices(btSoftBody* psb)
+{
+#define	PTR2IDX(_p_,_b_)	reinterpret_cast<btSoftBody::Node*>((_p_)-(_b_))
+	btSoftBody::Node*	base=&psb->getNodes()[0];
+	for(int i=0,ni=psb->getLinks().size();i<ni;++i)
+	{
+		psb->getLinks()[i].m_n[0]=PTR2IDX(psb->getLinks()[i].m_n[0],base);
+		psb->getLinks()[i].m_n[1]=PTR2IDX(psb->getLinks()[i].m_n[1],base);
+	}
+	for(int i=0,ni=psb->getFaces().size();i<ni;++i)
+	{
+		psb->getFaces()[i].m_n[0]=PTR2IDX(psb->getFaces()[i].m_n[0],base);
+		psb->getFaces()[i].m_n[1]=PTR2IDX(psb->getFaces()[i].m_n[1],base);
+		psb->getFaces()[i].m_n[2]=PTR2IDX(psb->getFaces()[i].m_n[2],base);
+	}
+#undef	PTR2IDX
+}
+
+//
+static void					IndicesToPointers(btSoftBody* psb)
+{
+#define	IDX2PTR(_p_,_b_)	((_b_)+(((char*)_p_)-(char*)0))
+	btSoftBody::Node*	base=&psb->getNodes()[0];
+	for(int i=0,ni=psb->getLinks().size();i<ni;++i)
+	{
+		psb->getLinks()[i].m_n[0]=IDX2PTR(psb->getLinks()[i].m_n[0],base);
+		psb->getLinks()[i].m_n[1]=IDX2PTR(psb->getLinks()[i].m_n[1],base);
+	}
+	for(int i=0,ni=psb->getFaces().size();i<ni;++i)
+	{
+		psb->getFaces()[i].m_n[0]=IDX2PTR(psb->getFaces()[i].m_n[0],base);
+		psb->getFaces()[i].m_n[1]=IDX2PTR(psb->getFaces()[i].m_n[1],base);
+		psb->getFaces()[i].m_n[2]=IDX2PTR(psb->getFaces()[i].m_n[2],base);
+	}
+#undef	IDX2PTR
+}
+
+//
+static inline btDbvt::Aabb	BoxOf(	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};
+btDbvt::Aabb		aabb=btDbvt::Aabb::FromPoints(pts,3);
+aabb.Expand(btVector3(margin,margin,margin));
+return(aabb);
+}
+
+//
+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=cross(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(dot(a,cross(b,c)));
+}
+
+//
+static inline btScalar		RayTriangle(const btVector3& org,
+	const btVector3& dir,
+	const btVector3& a,
+	const btVector3& b,
+	const btVector3& c,
+	btScalar maxt=SIMD_INFINITY)
+{
+	static const btScalar	ceps=-SIMD_EPSILON*10;
+	static const btScalar	teps=SIMD_EPSILON*10;
+	const btVector3			n=cross(b-a,c-a).normalized();
+	const btScalar			d=dot(a,n);
+	const btScalar			den=dot(dir,n);
+	if(!btFuzzyZero(den))
+	{
+		const btScalar		num=dot(org,n)-d;
+		const btScalar		t=-num/den;
+		if((t>teps)&&(t<maxt))
+		{
+			const btVector3	hit=org+dir*t;
+			if(	(dot(n,cross(a-hit,b-hit))>ceps)	&&			
+				(dot(n,cross(b-hit,c-hit))>ceps)	&&
+				(dot(n,cross(c-hit,a-hit))>ceps))
+			{
+				return(t);
+			}
+		}
+	}
+	return(-1);
+}
+
+//
+// Private implementation
+//
+
+//
+static int		RaycastInternal(const btSoftBody* psb,
+	const btVector3& org,
+	const btVector3& dir,
+	btScalar& mint,
+	bool bcountonly)
+{
+	int	cnt=0;
+	mint=SIMD_INFINITY;
+	for(int i=0,ni=psb->getFaces().size();i<ni;++i)
+	{
+		const btSoftBody::Face&	f=psb->getFaces()[i];
+		const btScalar			t=RayTriangle(	org,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) mint=t;
+		}
+	}
+	return(cnt);
+}
+
+
+//
+static btVector3	EvaluateCom(btSoftBody* psb)
+{
+	btVector3	com(0,0,0);
+	if(psb->m_pose.m_bframe)
+	{
+		for(int i=0,ni=psb->getNodes().size();i<ni;++i)
+		{
+			com+=psb->getNodes()[i].m_x*psb->m_pose.m_wgh[i];
+		}
+	}
+	return(com);
+}
+
+//
+static void			EvaluateMedium(	const btSoftBody::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=-(dot(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 bool			CheckContact(	btSoftBody* psb,
+									btRigidBody* prb,
+									const btVector3& x,
+									btSoftBody::sCti& cti)
+{
+	btVector3			nrm;
+	btCollisionShape*	shp=prb->getCollisionShape();
+	btAssert(shp->isConvex());
+	btConvexShape*		csh=static_cast<btConvexShape*>(shp);
+	const btScalar		margin=psb->getCollisionShape()->getMargin();
+	const btTransform&	wtr=prb->getInterpolationWorldTransform();
+	btScalar			dst=psb->m_worldInfo->m_sparsesdf.Evaluate(	wtr.invXform(x),
+																	csh,
+																	nrm,
+																	margin);
+	if(dst<0)
+	{
+		cti.m_body		=	prb;
+		cti.m_normal	=	wtr.getBasis()*nrm;
+		cti.m_offset	=	-dot(	cti.m_normal,
+									x-cti.m_normal*dst);
+		return(true);
+	}
+	return(false);
+}
+
+//
+static void			UpdateNormals(btSoftBody* psb)
+{
+	const btVector3	zv(0,0,0);
+	for(int i=0,ni=psb->getNodes().size();i<ni;++i)
+	{
+		psb->getNodes()[i].m_n=zv;
+	}
+	for(int i=0,ni=psb->getFaces().size();i<ni;++i)
+	{
+		btSoftBody::Face&	f=psb->getFaces()[i];
+		const btVector3		n=cross(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(int i=0,ni=psb->getNodes().size();i<ni;++i)
+	{
+		psb->getNodes()[i].m_n.normalize();
+	}
+}
+
+//
+static void			UpdateBounds(btSoftBody* psb)
+{
+	if(psb->m_ndbvt.m_root)
+		{
+		const btVector3&	mins=psb->m_ndbvt.m_root->box.Mins();
+		const btVector3&	maxs=psb->m_ndbvt.m_root->box.Maxs();
+		const btScalar		csm=psb->getCollisionShape()->getMargin();
+		const btVector3		mrg=btVector3(	csm,
+											csm,
+											csm)*1; // ??? to investigate...
+		psb->m_bounds[0]=mins-mrg;
+		psb->m_bounds[1]=maxs+mrg;
+			if(0!=psb->getBroadphaseHandle())
+			{					
+				psb->m_worldInfo->m_broadphase->setAabb(psb->getBroadphaseHandle(),
+														psb->m_bounds[0],
+														psb->m_bounds[1],
+														psb->m_worldInfo->m_dispatcher);
+			}
+		}
+		else
+		{
+		psb->m_bounds[0]=
+		psb->m_bounds[1]=btVector3(0,0,0);
+		}		
+}
+
+
+//
+static void			UpdatePose(btSoftBody* psb)
+{
+	if(psb->m_pose.m_bframe)
+	{
+		btSoftBody::Pose&	pose=psb->m_pose;
+		const btVector3		com=EvaluateCom(psb);
+		/* Com			*/ 
+		pose.m_com	=	com;
+		/* Rotation		*/ 
+		btMatrix3x3		Apq;
+		const btScalar	eps=1/(btScalar)(100*psb->getNodes().size());
+		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=psb->getNodes().size();i<ni;++i)
+		{
+			const btVector3		a=pose.m_wgh[i]*(psb->getNodes()[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(psb->m_cfg.maxvolume>1)
+			{
+			const btScalar	idet=Clamp<btScalar>(	1/pose.m_scl.determinant(),
+													1,psb->m_cfg.maxvolume);
+			pose.m_scl=Mul(pose.m_scl,idet);
+			}
+		
+	}
+}
+
+//
+static void			UpdateConstants(btSoftBody* psb)
+{
+	/* Links		*/ 
+	for(int i=0,ni=psb->getLinks().size();i<ni;++i)
+	{
+		btSoftBody::Link&	l=psb->getLinks()[i];
+		l.m_rl	=	(l.m_n[0]->m_x-l.m_n[1]->m_x).length();
+		l.m_c0	=	l.m_n[0]->m_im+l.m_n[1]->m_im;
+		l.m_c1	=	l.m_rl*l.m_rl;
+	}
+	/* Faces		*/ 
+	for(int i=0,ni=psb->getFaces().size();i<ni;++i)
+	{
+		btSoftBody::Face&	f=psb->getFaces()[i];
+		f.m_ra	=	AreaOf(f.m_n[0]->m_x,f.m_n[1]->m_x,f.m_n[2]->m_x);
+	}
+	/* Area's		*/ 
+	btAlignedObjectArray<int>	counts;
+	counts.resize(psb->getNodes().size(),0);
+	for(int i=0,ni=psb->getNodes().size();i<ni;++i)
+	{
+		psb->getNodes()[i].m_area	=	0;
+	}
+	for(int i=0,ni=psb->getFaces().size();i<ni;++i)
+	{
+		btSoftBody::Face&	f=psb->getFaces()[i];
+		for(int j=0;j<3;++j)
+		{
+			const int index=(int)(f.m_n[j]-&psb->getNodes()[0]);
+			counts[index]++;
+			f.m_n[j]->m_area+=btFabs(f.m_ra);
+		}
+	}
+	for(int i=0,ni=psb->getNodes().size();i<ni;++i)
+	{
+		if(counts[i]>0)
+			psb->getNodes()[i].m_area/=(btScalar)counts[i];
+		else
+			psb->getNodes()[i].m_area=0;
+	}
+}
+
+//
+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 void			ApplyForces(btSoftBody* psb,btScalar dt)
+{
+	const btScalar					kLF=psb->m_cfg.kLF;
+	const btScalar					kDG=psb->m_cfg.kDG;
+	const btScalar					kPR=psb->m_cfg.kPR;
+	const btScalar					kVC=psb->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		&&
+		(psb->m_cfg.aeromodel<
+		btSoftBody::eAeroModel::F_TwoSided);
+	const bool						as_faero=	as_aero		&&
+		(psb->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		=	psb->getVolume();
+		ivolumetp	=	1/btFabs(volume)*kPR;
+		dvolumetv	=	(psb->m_pose.m_volume-volume)*kVC;
+	}
+	/* Per vertex forces			*/ 
+	for(int i=0,ni=psb->getNodes().size();i<ni;++i)
+	{
+		btSoftBody::Node&	n=psb->getNodes()[i];
+		if(n.m_im>0)
+		{
+			if(use_medium)
+			{
+				EvaluateMedium(psb->m_worldInfo,n.m_x,medium);
+				/* Aerodynamics			*/ 
+				if(as_vaero)
+				{				
+					const btVector3	rel_v=n.m_v-medium.m_velocity;
+					const btScalar	rel_v2=rel_v.length2();
+					if(rel_v2>SIMD_EPSILON)
+					{
+						btVector3	nrm=n.m_n;
+						/* Setup normal		*/ 
+						switch(psb->m_cfg.aeromodel)
+						{
+						case	btSoftBody::eAeroModel::V_Point:
+							nrm=NormalizeAny(rel_v);break;
+						case	btSoftBody::eAeroModel::V_TwoSided:
+							nrm*=(btScalar)(dot(nrm,rel_v)<0?-1:+1);break;
+						}
+						const btScalar	dvn=dot(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);
+						}
+					}
+				}
+			}
+			/* 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(int i=0,ni=psb->getFaces().size();i<ni;++i)
+	{
+		btSoftBody::Face&	f=psb->getFaces()[i];
+		if(as_faero)
+		{
+			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(psb->m_worldInfo,x,medium);
+			const btVector3	rel_v=v-medium.m_velocity;
+			const btScalar	rel_v2=rel_v.length2();
+			if(rel_v2>SIMD_EPSILON)
+			{
+				btVector3	nrm=f.m_normal;
+				/* Setup normal		*/ 
+				switch(psb->m_cfg.aeromodel)
+				{
+				case	btSoftBody::eAeroModel::F_TwoSided:
+					nrm*=(btScalar)(dot(nrm,rel_v)<0?-1:+1);break;
+				}
+				const btScalar	dvn=dot(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);
+				}
+			}
+		}
+	}
+}
+
+//
+static void			PSolve_Anchors(btSoftBody* psb,btScalar sdt)
+{
+	const btScalar	kAHR=psb->m_cfg.kAHR;
+	for(int i=0,ni=psb->m_anchors.size();i<ni;++i)
+	{
+		const btSoftBody::Anchor&	a=psb->m_anchors[i];
+		const btTransform&			t=a.m_body->getInterpolationWorldTransform();
+		btSoftBody::Node&			n=*a.m_node;
+		const btVector3				wa=t*a.m_local;
+		const btVector3				va=a.m_body->getVelocityInLocalPoint(a.m_c1)*sdt;
+		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;
+		n.m_x+=impulse*a.m_c2;
+		a.m_body->applyImpulse(-impulse,a.m_c1);
+	}
+}
+
+//
+static void			PSolve_RContacts(btSoftBody* psb,btScalar sdt)
+{
+	const btScalar	kCHR=psb->m_cfg.kCHR;
+	for(int i=0,ni=psb->m_rcontacts.size();i<ni;++i)
+	{
+		const btSoftBody::RContact&	c=psb->m_rcontacts[i];
+		const btSoftBody::sCti&		cti=c.m_cti;	
+		const btVector3		va=cti.m_body->getVelocityInLocalPoint(c.m_c1)*sdt;
+		const btVector3		vb=c.m_node->m_x-c.m_node->m_q;	
+		const btVector3		vr=vb-va;
+		const btScalar		dn=dot(vr,cti.m_normal);
+		if(dn<=SIMD_EPSILON)
+		{
+			const btScalar		dp=dot(c.m_node->m_x,cti.m_normal)+cti.m_offset;
+			const btVector3		fv=vr-cti.m_normal*dn;
+			const btVector3		impulse=c.m_c0*(vr-fv*c.m_c3+cti.m_normal*(dp*kCHR));
+			c.m_node->m_x-=impulse*c.m_c2;
+			c.m_cti.m_body->applyImpulse(impulse,c.m_c1);
+		}
+	}
+}
+
+//
+static void			PSolve_SContacts(btSoftBody* psb)
+{
+for(int i=0,ni=psb->m_scontacts.size();i<ni;++i)
+	{
+	const btSoftBody::SContact&	c=psb->m_scontacts[i];
+	const btVector3&	nr=c.m_normal;
+	btSoftBody::Node&	n=*c.m_node;
+	btSoftBody::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);
+	if(dot(vr,nr)<0)
+		{
+		const btScalar	j=c.m_margin-(dot(nr,n.m_x)-dot(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());
+	}
+}
+
+//
+static void			PSolve_Links(btSoftBody* psb,btScalar w)
+{
+	for(int i=0,ni=psb->getLinks().size();i<ni;++i)
+	{			
+		btSoftBody::Link&	l=psb->getLinks()[i];
+		if(l.m_c0>0)
+		{
+			btSoftBody::Node&	a=*l.m_n[0];
+			btSoftBody::Node&	b=*l.m_n[1];
+			const btVector3		del=b.m_x-a.m_x;
+			const btScalar		len=del.length2();
+			const btScalar		kst=l.m_kST*w;
+			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);
+		}
+	}
+}
+
+//
+// btSoftBody
+//
+
+//
+btSoftBody::btSoftBody(btSoftBody::btSoftBodyWorldInfo*	worldInfo,int node_count,  const btVector3* x,  const btScalar* m)
+:m_worldInfo(worldInfo)
+{	
+	m_internalType=CO_SOFT_BODY;
+
+	/* Init		*/ 
 	m_cfg.aeromodel		=	eAeroModel::V_Point;
 	m_cfg.kDG			=	0;
 	m_cfg.kLF			=	0;
@@ -42,24 +926,29 @@ btSoftBody::btSoftBody(btSoftBody::btSoftBodyWorldInfo&	worldInfo,int node_count
 	m_cfg.kMT			=	0;
 	m_cfg.kSOR			=	1;
 	m_cfg.kCHR			=	(btScalar)1.0;
-	m_cfg.kAHR			=	(btScalar)0.5;
+	m_cfg.kSHR			=	(btScalar)1.0;
+	m_cfg.kAHR			=	(btScalar)0.7;
+	m_cfg.maxvolume		=	(btScalar)1;
 	m_cfg.timescale		=	1;
-	m_cfg.timestep		=	(btScalar)(1/60.);
-	m_cfg.maxsteps		=	60;
 	m_cfg.iterations	=	1;
-	m_cfg.becollide		=	true;
-	m_cfg.bscollide		=	false;
+	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_trs.setIdentity();
+	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();
+	///for now, create a collision shape internally
+	setCollisionShape(new btSoftBodyCollisionShape(this));	
+	m_collisionShape->setMargin(0.25);
 	/* Nodes	*/ 
+	const btScalar		margin=getCollisionShape()->getMargin();
 	getNodes().resize(node_count);
 	for(int i=0,ni=node_count;i<ni;++i)
 	{	
@@ -69,692 +958,29 @@ btSoftBody::btSoftBody(btSoftBody::btSoftBodyWorldInfo&	worldInfo,int node_count
 		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(btDbvt::Aabb::FromCR(n.m_x,margin),&n);
 	}
-	updateTransform();
-	///register to the broadphase
-	setBroadphaseHandle( m_worldInfo.m_broadphase->createProxy(m_bounds[0],m_bounds[1],SOFTBODY_SHAPE_PROXYTYPE,this,btBroadphaseProxy::DefaultFilter,btBroadphaseProxy::AllFilter,m_worldInfo.m_dispatcher,0));
+	m_ndbvt.optimizeTopDown();
+	UpdateBounds(this);	
 
 
 }
 
-///destructor
+//
 btSoftBody::~btSoftBody()
 {
-	//remove from broadphase
-	m_worldInfo.m_broadphase->destroyProxy(getBroadphaseHandle(),m_worldInfo.m_dispatcher);
-
-	///for now, delete the internal shape
-	delete m_softBodyCollisionShape;
-}
-
-btVector3 btSoftBody::btSoftBodyCollisionShape::m_sScaling(0,0,0);
-
-btSoftBody::btSoftBodyCollisionShape::btSoftBodyCollisionShape()
-{
-}
-
-btSoftBody::btSoftBodyCollisionShape::~btSoftBodyCollisionShape()
-{
-
-}
-
-void	btSoftBody::btSoftBodyCollisionShape::processAllTriangles(btTriangleCallback* callback,const btVector3& aabbMin,const btVector3& aabbMax) const
-{
-	//not yet
-	btAssert(0);
-}
-
-
-
-//
-void btSoftBody::updateBounds()
-{
-	if(getNodes().size()>0)
-	{
-		m_bounds[0]=
-			m_bounds[1]=getNodes()[0].m_x;
-		for(int i=1,ni=getNodes().size();i<ni;++i)
-		{
-			const btSoftBody::Node&	n=getNodes()[i];
-			m_bounds[0].setMin(n.m_x);
-			m_bounds[1].setMax(n.m_x);
-			m_bounds[0].setMin(n.m_q);
-			m_bounds[1].setMax(n.m_q);
-		}
-
-		if (getBroadphaseHandle())
-		{
-			///todo: figure out how much larger the aabb needs to be, based on motion?
-			btVector3 aabbMin = m_bounds[0]-btVector3(1,1,1);
-			btVector3 aabbMax = m_bounds[1]+btVector3(1,1,1);
-			m_worldInfo.m_broadphase->setAabb(getBroadphaseHandle(),aabbMin,aabbMax,m_worldInfo.m_dispatcher);
-		}
-
-	}
-	else
-	{
-		m_bounds[0]=
-			m_bounds[1]=btVector3(0,0,0);
-	}
-}
-
-namespace btsoftbody_internals
-{
-
-	//
-	// Helpers
-	//
-
-	//
-	static inline void			Trace(const btMatrix3x3& m,const char* name)
-	{
-		printf("%s[0]: %.2f,\t%.2f,\t%.2f\r\n",name,m[0].x(),m[0].y(),m[0].z());
-		printf("%s[1]: %.2f,\t%.2f,\t%.2f\r\n",name,m[1].x(),m[1].y(),m[1].z());
-		printf("%s[2]: %.2f,\t%.2f,\t%.2f\r\n",name,m[2].x(),m[2].y(),m[2].z());
-	}
-
-	//
-	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				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				Sign(const T& x)
-	{ return((T)(x<0?-1:+1)); }
-	//
-	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 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 void			PolarDecompose(	const btMatrix3x3& m,
-		btMatrix3x3& q,
-		btMatrix3x3& s)
-	{
-		static const btScalar	half=(btScalar)0.5;
-		static const btScalar	accuracy=(btScalar)0.00001;
-		static const int		maxiterations=64;
-		btScalar				det=m.determinant();
-		if(!btFuzzyZero(det))
-		{
-			q=m;
-			for(int i=0;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;
-			}
-			s=q.transpose()*m;
-		}
-		else
-		{
-			q.setIdentity();
-			s.setIdentity();
-		}
-	}
-	//
-	static inline btVector3		ProjectOnAxis(	const btVector3& v,
-		const btVector3& a)
-	{
-		return(a*dot(v,a));
-	}
-	//
-	static inline btVector3		ProjectOnPlane(	const btVector3& v,
-		const btVector3& a)
-	{
-		return(v-ProjectOnAxis(v,a));
-	}
-	//
-	template <typename T>
-	static inline T				BaryEval(	const btVector3& coord,
-		const T& a,
-		const T& b,
-		const T& c)
-	{
-		return(a*coord.x()+b*coord.y()+c*coord.z());
-	}
-
-	//
-	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 void					PointersToIndices(btSoftBody* psb)
-	{
-#define	PTR2IDX(_p_,_b_)	reinterpret_cast<btSoftBody::Node*>((_p_)-(_b_))
-		btSoftBody::Node*	base=&psb->getNodes()[0];
-		for(int i=0,ni=psb->getLinks().size();i<ni;++i)
-		{
-			psb->getLinks()[i].m_n[0]=PTR2IDX(psb->getLinks()[i].m_n[0],base);
-			psb->getLinks()[i].m_n[1]=PTR2IDX(psb->getLinks()[i].m_n[1],base);
-		}
-		for(int i=0,ni=psb->getFaces().size();i<ni;++i)
-		{
-			psb->getFaces()[i].m_n[0]=PTR2IDX(psb->getFaces()[i].m_n[0],base);
-			psb->getFaces()[i].m_n[1]=PTR2IDX(psb->getFaces()[i].m_n[1],base);
-			psb->getFaces()[i].m_n[2]=PTR2IDX(psb->getFaces()[i].m_n[2],base);
-		}
-#undef	PTR2IDX
-	}
-
-	//
-	static void					IndicesToPointers(btSoftBody* psb)
-	{
-#define	IDX2PTR(_p_,_b_)	((_b_)+(((char*)_p_)-(char*)0))
-		btSoftBody::Node*	base=&psb->getNodes()[0];
-		for(int i=0,ni=psb->getLinks().size();i<ni;++i)
-		{
-			psb->getLinks()[i].m_n[0]=IDX2PTR(psb->getLinks()[i].m_n[0],base);
-			psb->getLinks()[i].m_n[1]=IDX2PTR(psb->getLinks()[i].m_n[1],base);
-		}
-		for(int i=0,ni=psb->getFaces().size();i<ni;++i)
-		{
-			psb->getFaces()[i].m_n[0]=IDX2PTR(psb->getFaces()[i].m_n[0],base);
-			psb->getFaces()[i].m_n[1]=IDX2PTR(psb->getFaces()[i].m_n[1],base);
-			psb->getFaces()[i].m_n[2]=IDX2PTR(psb->getFaces()[i].m_n[2],base);
-		}
-#undef	IDX2PTR
-	}
-
-	//
-	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=cross(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(dot(a,cross(b,c)));
-	}
-
-	//
-	static inline btScalar		RayTriangle(const btVector3& org,
-		const btVector3& dir,
-		const btVector3& a,
-		const btVector3& b,
-		const btVector3& c,
-		btScalar maxt=SIMD_INFINITY)
-	{
-		static const btScalar	ceps=-SIMD_EPSILON*10;
-		static const btScalar	teps=SIMD_EPSILON*10;
-		const btVector3			n=cross(b-a,c-a).normalized();
-		const btScalar			d=dot(a,n);
-		const btScalar			den=dot(dir,n);
-		if(!btFuzzyZero(den))
-		{
-			const btScalar		num=dot(org,n)-d;
-			const btScalar		t=-num/den;
-			if((t>teps)&&(t<maxt))
-			{
-				const btVector3	hit=org+dir*t;
-				if(	(dot(n,cross(a-hit,b-hit))>ceps)	&&			
-					(dot(n,cross(b-hit,c-hit))>ceps)	&&
-					(dot(n,cross(c-hit,a-hit))>ceps))
-				{
-					return(t);
-				}
-			}
-		}
-		return(-1);
-	}
-
-	//
-	// Private implementation
-	//
-
-	//
-	static int		RaycastInternal(const btSoftBody* psb,
-		const btVector3& org,
-		const btVector3& dir,
-		btScalar& mint,
-		bool bcountonly)
-	{
-		int	cnt=0;
-		mint=SIMD_INFINITY;
-		for(int i=0,ni=psb->getFaces().size();i<ni;++i)
-		{
-			const btSoftBody::Face&	f=psb->getFaces()[i];
-			const btScalar			t=RayTriangle(	org,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) mint=t;
-			}
-		}
-		return(cnt);
-	}
-
-
-	//
-	static btVector3	EvaluateCom(btSoftBody* psb)
-	{
-		btVector3	com(0,0,0);
-		if(psb->m_pose.m_bframe)
-		{
-			for(int i=0,ni=psb->getNodes().size();i<ni;++i)
-			{
-				com+=psb->getNodes()[i].m_x*psb->m_pose.m_wgh[i];
-			}
-		}
-		return(com);
-	}
-
-	//
-	static void			UpdateNormals(btSoftBody* psb)
-	{
-		const btVector3	zv(0,0,0);
-		for(int i=0,ni=psb->getNodes().size();i<ni;++i)
-		{
-			psb->getNodes()[i].m_n=zv;
-		}
-		for(int i=0,ni=psb->getFaces().size();i<ni;++i)
-		{
-			btSoftBody::Face&	f=psb->getFaces()[i];
-			const btVector3		n=cross(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(int i=0,ni=psb->getNodes().size();i<ni;++i)
-		{
-			psb->getNodes()[i].m_n.normalize();
-		}
-	}
-
-	
-
-	//
-	static void			UpdatePose(btSoftBody* psb)
-	{
-		if(psb->m_pose.m_bframe)
-		{
-			btSoftBody::Pose&	pose=psb->m_pose;
-			const btVector3		com=EvaluateCom(psb);
-			/* Com			*/ 
-			pose.m_com	=	com;
-			/* Rotation		*/ 
-			btMatrix3x3		Apq;
-			const btScalar	eps=1/(btScalar)(100*psb->getNodes().size());
-			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=psb->getNodes().size();i<ni;++i)
-			{
-				const btVector3		a=pose.m_wgh[i]*(psb->getNodes()[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);
-			psb->m_pose.m_trs=r;	
-		}
-	}
-
-	//
-	static void			UpdateConstants(btSoftBody* psb)
-	{
-		/* Links		*/ 
-		for(int i=0,ni=psb->getLinks().size();i<ni;++i)
-		{
-			btSoftBody::Link&	l=psb->getLinks()[i];
-			l.m_rl	=	(l.m_n[0]->m_x-l.m_n[1]->m_x).length();
-			l.m_c0	=	l.m_n[0]->m_im+l.m_n[1]->m_im;
-			l.m_c1	=	l.m_rl*l.m_rl;
-		}
-		/* Faces		*/ 
-		for(int i=0,ni=psb->getFaces().size();i<ni;++i)
-		{
-			btSoftBody::Face&	f=psb->getFaces()[i];
-			f.m_ra	=	AreaOf(f.m_n[0]->m_x,f.m_n[1]->m_x,f.m_n[2]->m_x);
-		}
-		/* Area's		*/ 
-		btAlignedObjectArray<int>	counts;
-		counts.resize(psb->getNodes().size(),0);
-		for(int i=0,ni=psb->getNodes().size();i<ni;++i)
-		{
-			psb->getNodes()[i].m_area	=	0;
-		}
-		for(int i=0,ni=psb->getFaces().size();i<ni;++i)
-		{
-			btSoftBody::Face&	f=psb->getFaces()[i];
-			for(int j=0;j<3;++j)
-			{
-				const int index=(int)(f.m_n[j]-&psb->getNodes()[0]);
-				counts[index]++;
-				f.m_n[j]->m_area+=btFabs(f.m_ra);
-			}
-		}
-		for(int i=0,ni=psb->getNodes().size();i<ni;++i)
-		{
-			if(counts[i]>0)
-				psb->getNodes()[i].m_area/=(btScalar)counts[i];
-			else
-				psb->getNodes()[i].m_area=0;
-		}
-	}
-
-	//
-	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 void			ApplyForces(btSoftBody* psb,btScalar dt)
-	{
-		const btScalar					kLF=psb->m_cfg.kLF;
-		const btScalar					kDG=psb->m_cfg.kDG;
-		const btScalar					kPR=psb->m_cfg.kPR;
-		const btScalar					kVC=psb->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		&&
-			(psb->m_cfg.aeromodel<
-			btSoftBody::eAeroModel::F_TwoSided);
-		const bool						as_faero=	as_aero		&&
-			(psb->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		=	psb->GetVolume();
-			ivolumetp	=	1/btFabs(volume)*kPR;
-			dvolumetv	=	(psb->m_pose.m_volume-volume)*kVC;
-		}
-		/* Per vertex forces			*/ 
-		for(int i=0,ni=psb->getNodes().size();i<ni;++i)
-		{
-			btSoftBody::Node&	n=psb->getNodes()[i];
-			if(n.m_im>0)
-			{
-				if(use_medium)
-				{
-					psb->EvaluateMedium(n.m_x,medium);
-					/* Aerodynamics			*/ 
-					if(as_vaero)
-					{				
-						const btVector3	rel_v=n.m_v-medium.m_velocity;
-						const btScalar	rel_v2=rel_v.length2();
-						if(rel_v2>SIMD_EPSILON)
-						{
-							btVector3	nrm=n.m_n;
-							/* Setup normal		*/ 
-							switch(psb->m_cfg.aeromodel)
-							{
-							case	btSoftBody::eAeroModel::V_Point:
-								nrm=NormalizeAny(rel_v);break;
-							case	btSoftBody::eAeroModel::V_TwoSided:
-								nrm*=(btScalar)(dot(nrm,rel_v)<0?-1:+1);break;
-							}
-							const btScalar	dvn=dot(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);
-							}
-						}
-					}
-				}
-				/* 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(int i=0,ni=psb->getFaces().size();i<ni;++i)
-		{
-			btSoftBody::Face&	f=psb->getFaces()[i];
-			if(as_faero)
-			{
-				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;
-				psb->EvaluateMedium(x,medium);
-				const btVector3	rel_v=v-medium.m_velocity;
-				const btScalar	rel_v2=rel_v.length2();
-				if(rel_v2>SIMD_EPSILON)
-				{
-					btVector3	nrm=f.m_normal;
-					/* Setup normal		*/ 
-					switch(psb->m_cfg.aeromodel)
-					{
-					case	btSoftBody::eAeroModel::F_TwoSided:
-						nrm*=(btScalar)(dot(nrm,rel_v)<0?-1:+1);break;
-					}
-					const btScalar	dvn=dot(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);
-					}
-				}
-			}
-		}
-	}
-
-	//
-	static void			PSolve_Anchors(btSoftBody* psb,btScalar sdt)
-	{
-		const btScalar	kAHR=psb->m_cfg.kAHR;
-		for(int i=0,ni=psb->m_anchors.size();i<ni;++i)
-		{
-			const btSoftBody::Anchor&	a=psb->m_anchors[i];
-			const btTransform&			t=a.m_body->getWorldTransform();
-			btSoftBody::Node&			n=*a.m_node;
-			const btVector3				wa=t*a.m_local;
-			const btVector3				va=a.m_body->getVelocityInLocalPoint(a.m_c1)*sdt;
-			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;
-			n.m_x+=impulse*a.m_c2;
-			a.m_body->applyImpulse(-impulse,a.m_c1);
-		}
-	}
-
-	//
-	static void			PSolve_Contacts(btSoftBody* psb,btScalar sdt)
-	{
-		const btScalar	kCHR=psb->m_cfg.kCHR;
-		for(int i=0,ni=psb->m_contacts.size();i<ni;++i)
-		{
-			const btSoftBody::Contact&			c=psb->m_contacts[i];
-			const btSoftBody::sCti&	cti=c.m_cti;	
-			const btVector3		va=cti.m_body->getVelocityInLocalPoint(c.m_c1)*sdt;
-			const btVector3		vb=c.m_node->m_x-c.m_node->m_q;	
-			const btVector3		vr=vb-va;
-			const btScalar		dn=dot(vr,cti.m_normal);
-			if(dn<=SIMD_EPSILON)
-			{
-				const btScalar		dp=dot(c.m_node->m_x,cti.m_normal)+cti.m_offset;
-				const btVector3		fv=vr-cti.m_normal*dn;
-				const btVector3		impulse=c.m_c0*(vr-fv*c.m_c3+cti.m_normal*(dp*kCHR));
-				c.m_node->m_x-=impulse*c.m_c2;
-				c.m_cti.m_body->applyImpulse(impulse,c.m_c1);
-			}
-		}
-	}
-
-	//
-	static void			PSolve_Links(btSoftBody* psb,btScalar w)
-	{
-		for(int i=0,ni=psb->getLinks().size();i<ni;++i)
-		{			
-			btSoftBody::Link&	l=psb->getLinks()[i];
-			if(l.m_c0>0)
-			{
-				btSoftBody::Node&	a=*l.m_n[0];
-				btSoftBody::Node&	b=*l.m_n[1];
-				const btVector3		del=b.m_x-a.m_x;
-				const btScalar		len=del.length2();
-				const btScalar		kst=l.m_kST*w;
-				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);
-			}
-		}
-	}
-
-}
-
-using namespace btsoftbody_internals;
-
-//
-// Api
-//
-
-
-//
-void			btSoftBody::Delete()
-{
-	delete this;
+	//for now, delete the internal shape
+	delete m_collisionShape;	
 }
 
 //
-bool			btSoftBody::CheckLink(int node0,int node1) const
+bool			btSoftBody::checkLink(int node0,int node1) const
 {
-	return(CheckLink(&getNodes()[node0],&getNodes()[node1]));
+	return(checkLink(&getNodes()[node0],&getNodes()[node1]));
 }
 
 //
-bool			btSoftBody::CheckLink(const Node* node0,const Node* node1) const
+bool			btSoftBody::checkLink(const Node* node0,const Node* node1) const
 {
 	const Node*	n[]={node0,node1};
 	for(int i=0,ni=getLinks().size();i<ni;++i)
@@ -770,7 +996,7 @@ bool			btSoftBody::CheckLink(const Node* node0,const Node* node1) const
 }
 
 //
-bool			btSoftBody::CheckFace(int node0,int node1,int node2) const
+bool			btSoftBody::checkFace(int node0,int node1,int node2) const
 {
 	const Node*	n[]={	&getNodes()[node0],
 		&getNodes()[node1],
@@ -791,23 +1017,23 @@ bool			btSoftBody::CheckFace(int node0,int node1,int node2) const
 }
 
 //
-void			btSoftBody::AppendLink(	int node0,
+void			btSoftBody::appendLink(	int node0,
 									   int node1,
 									   btScalar kST,
 									   eLType::_ type,
 									   bool bcheckexist)
 {
-	AppendLink(&getNodes()[node0],&getNodes()[node1],kST,type,bcheckexist);
+	appendLink(&getNodes()[node0],&getNodes()[node1],kST,type,bcheckexist);
 }
 
 //
-void			btSoftBody::AppendLink(	Node* node0,
+void			btSoftBody::appendLink(	Node* node0,
 									   Node* node1,
 									   btScalar kST,
 									   eLType::_ type,
 									   bool bcheckexist)
 {
-	if((!bcheckexist)||(!CheckLink(node0,node1)))
+	if((!bcheckexist)||(!checkLink(node0,node1)))
 	{
 		Link	l;
 		l.m_n[0]	=	node0;
@@ -824,7 +1050,7 @@ void			btSoftBody::AppendLink(	Node* node0,
 }
 
 //
-void			btSoftBody::AppendFace(int node0,int node1,int node2)
+void			btSoftBody::appendFace(int node0,int node1,int node2)
 {
 	Face	f;
 	f.m_n[0]	=	&getNodes()[node0];
@@ -839,24 +1065,24 @@ void			btSoftBody::AppendFace(int node0,int node1,int node2)
 }
 
 //
-void			btSoftBody::AppendAnchor(int node,btRigidBody* body)
+void			btSoftBody::appendAnchor(int node,btRigidBody* body)
 {
 	Anchor	a;
 	a.m_node			=	&getNodes()[node];
 	a.m_body			=	body;
-	a.m_local			=	body->getWorldTransform().inverse()*a.m_node->m_x;
+	a.m_local			=	body->getInterpolationWorldTransform().inverse()*a.m_node->m_x;
 	a.m_node->m_battach	=	1;
 	m_anchors.push_back(a);
 }
 
 //
-void			btSoftBody::AddForce(const btVector3& force)
+void			btSoftBody::addForce(const btVector3& force)
 {
-	for(int i=0,ni=getNodes().size();i<ni;++i) AddForce(force,i);
+	for(int i=0,ni=getNodes().size();i<ni;++i) addForce(force,i);
 }
 
 //
-void			btSoftBody::AddForce(const btVector3& force,int node)
+void			btSoftBody::addForce(const btVector3& force,int node)
 {
 	Node&	n=getNodes()[node];
 	if(n.m_im>0)
@@ -866,13 +1092,13 @@ void			btSoftBody::AddForce(const btVector3& force,int node)
 }
 
 //
-void			btSoftBody::AddVelocity(const btVector3& velocity)
+void			btSoftBody::addVelocity(const btVector3& velocity)
 {
-	for(int i=0,ni=getNodes().size();i<ni;++i) AddVelocity(velocity,i);
+	for(int i=0,ni=getNodes().size();i<ni;++i) addVelocity(velocity,i);
 }
 
 //
-void			btSoftBody::AddVelocity(const btVector3& velocity,int node)
+void			btSoftBody::addVelocity(const btVector3& velocity,int node)
 {
 	Node&	n=getNodes()[node];
 	if(n.m_im>0)
@@ -882,31 +1108,31 @@ void			btSoftBody::AddVelocity(const btVector3& velocity,int node)
 }
 
 //
-void			btSoftBody::SetMass(int node,btScalar mass)
+void			btSoftBody::setMass(int node,btScalar mass)
 {
 	getNodes()[node].m_im=mass>0?1/mass:0;
 	m_bUpdateRtCst=true;
 }
 
 //
-btScalar		btSoftBody::GetMass(int node) const
+btScalar		btSoftBody::getMass(int node) const
 {
 	return(getNodes()[node].m_im>0?1/getNodes()[node].m_im:0);
 }
 
 //
-btScalar		btSoftBody::GetTotalMass() const
+btScalar		btSoftBody::getTotalMass() const
 {
 	btScalar	mass=0;
 	for(int i=0;i<getNodes().size();++i)
 	{
-		mass+=GetMass(i);
+		mass+=getMass(i);
 	}
 	return(mass);
 }
 
 //
-void			btSoftBody::SetTotalMass(btScalar mass,bool fromfaces)
+void			btSoftBody::setTotalMass(btScalar mass,bool fromfaces)
 {
 	if(fromfaces)
 	{
@@ -930,7 +1156,7 @@ void			btSoftBody::SetTotalMass(btScalar mass,bool fromfaces)
 			getNodes()[i].m_im=1/getNodes()[i].m_im;
 		}
 	}
-	const btScalar	tm=GetTotalMass();
+	const btScalar	tm=getTotalMass();
 	const btScalar	itm=1/tm;
 	for(int i=0;i<getNodes().size();++i)
 	{
@@ -940,47 +1166,53 @@ void			btSoftBody::SetTotalMass(btScalar mass,bool fromfaces)
 }
 
 //
-void			btSoftBody::SetTotalDensity(btScalar density)
+void			btSoftBody::setTotalDensity(btScalar density)
 {
-	SetTotalMass(GetVolume()*density,true);
+	setTotalMass(getVolume()*density,true);
 }
 
 //
-void			btSoftBody::Transform(const btTransform& trs)
+void			btSoftBody::transform(const btTransform& trs)
 {
+	const btScalar	margin=getCollisionShape()->getMargin();
 	for(int i=0,ni=getNodes().size();i<ni;++i)
 	{
 		Node&	n=getNodes()[i];
 		n.m_x=trs*n.m_x;
 		n.m_q=trs*n.m_q;
 		n.m_n=trs.getBasis()*n.m_n;
+		m_ndbvt.update(n.m_leaf,btDbvt::Aabb::FromCR(n.m_x,margin));
 	}
+	m_ndbvt.optimizeTopDown();
 	UpdateNormals(this);
-	updateTransform();
+	UpdateBounds(this);
 	UpdateConstants(this);
 }
 
 //
-void			btSoftBody::Scale(const btVector3& scl)
+void			btSoftBody::scale(const btVector3& scl)
 {
+	const btScalar	margin=getCollisionShape()->getMargin();
 	for(int i=0,ni=getNodes().size();i<ni;++i)
 	{
 		Node&	n=getNodes()[i];
 		n.m_x*=scl;
 		n.m_q*=scl;
+		m_ndbvt.update(n.m_leaf,btDbvt::Aabb::FromCR(n.m_x,margin));
 	}
+	m_ndbvt.optimizeTopDown();
 	UpdateNormals(this);
-	updateTransform();
+	UpdateBounds(this);
 	UpdateConstants(this);
 }
 
 //
-void			btSoftBody::SetPose(bool bvolume,bool bframe)
+void			btSoftBody::setPose(bool bvolume,bool bframe)
 {
 	m_pose.m_bvolume	=	bvolume;
 	m_pose.m_bframe		=	bframe;
 	/* Weights		*/ 
-	const btScalar	omass=GetTotalMass();
+	const btScalar	omass=getTotalMass();
 	const btScalar	kmass=omass*getNodes().size()*1000;
 	btScalar		tmass=omass;
 	m_pose.m_wgh.resize(getNodes().size());
@@ -991,7 +1223,7 @@ void			btSoftBody::SetPose(bool bvolume,bool bframe)
 	for(int i=0,ni=getNodes().size();i<ni;++i)
 	{
 		Node&	n=getNodes()[i];
-		m_pose.m_wgh[i]=	n.m_im>0					?
+		m_pose.m_wgh[i]=	n.m_im>0		?
 			1/(getNodes()[i].m_im*tmass)	:
 		kmass/tmass;
 	}
@@ -1002,14 +1234,28 @@ void			btSoftBody::SetPose(bool bvolume,bool bframe)
 	{
 		m_pose.m_pos[i]=getNodes()[i].m_x-com;
 	}
-	m_pose.m_volume	=	bvolume?GetVolume():0;
+	m_pose.m_volume	=	bvolume?getVolume():0;
 	m_pose.m_com	=	com;
-	m_pose.m_trs.setIdentity();
+	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(int i=0,ni=getNodes().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(this);
 }
 
 //
-btScalar		btSoftBody::GetVolume() const
+btScalar		btSoftBody::getVolume() const
 {
 	btScalar	vol=0;
 	if(getNodes().size()>0)
@@ -1026,8 +1272,8 @@ btScalar		btSoftBody::GetVolume() const
 }
 
 //
-int				btSoftBody::GenerateBendingConstraints(	int distance,
-													   btScalar stiffness)
+int				btSoftBody::generateBendingConstraints(	int distance,
+														btScalar stiffness)
 {
 	if(distance>1)
 	{
@@ -1077,7 +1323,7 @@ int				btSoftBody::GenerateBendingConstraints(	int distance,
 			{
 				if(adj[IDX(i,j)]==(unsigned)distance)
 				{
-					AppendLink(i,j,stiffness,eLType::Bending);
+					appendLink(i,j,stiffness,eLType::Bending);
 					++nlinks;
 				}
 			}
@@ -1089,7 +1335,7 @@ int				btSoftBody::GenerateBendingConstraints(	int distance,
 }
 
 //
-void			btSoftBody::RandomizeConstraints()
+void			btSoftBody::randomizeConstraints()
 {
 	for(int i=0,ni=getLinks().size();i<ni;++i)
 	{
@@ -1102,7 +1348,7 @@ void			btSoftBody::RandomizeConstraints()
 }
 
 //
-btScalar		btSoftBody::Raycast(const btVector3& org,
+btScalar		btSoftBody::raycast(const btVector3& org,
 									const btVector3& dir) const
 {
 	btScalar	mint=0;
@@ -1113,171 +1359,306 @@ btScalar		btSoftBody::Raycast(const btVector3& org,
 }
 
 //
-void			btSoftBody::Step(btScalar dt)
+void			btSoftBody::predictMotion(btScalar dt)
 {
-	m_timeacc	+=	dt;
-	m_timeacc	=	btMin(m_cfg.timestep*m_cfg.maxsteps,m_timeacc);
-	while(m_timeacc>=m_cfg.timestep)
+	/* Update				*/ 
+	if(m_bUpdateRtCst)
 	{
-		/* Update				*/ 
-		m_timeacc-=m_cfg.timestep;
-		if(m_bUpdateRtCst)
-		{
-			m_bUpdateRtCst=false;
-			UpdateConstants(this);
-		}
-		/* Prepare				*/ 
-		const btScalar		iit		=	1/(btScalar)m_cfg.iterations;
-		const btScalar		sdt		=	m_cfg.timestep*m_cfg.timescale;
-		const btScalar		isdt	=	1/sdt;
-		Node*				pnodes	=	getNodes().size()>0?&getNodes()[0]:0;
-		/* Forces				*/ 
-		ApplyForces(this,sdt);	
-		/* Integrate			*/ 
-		for(int i=0,ni=getNodes().size();i<ni;++i)
-		{
-			Node&	n=pnodes[i];
-			n.m_q	=	n.m_x;
-			n.m_v	+=	n.m_f*n.m_im*sdt;
-			n.m_x	+=	n.m_v*sdt;
-		}	
-		/* Pose					*/ 
-		UpdatePose(this);
-		/* Match				*/ 
-		if(m_pose.m_bframe&&(m_cfg.kMT>0))
-		{
-			for(int i=0,ni=getNodes().size();i<ni;++i)
+		m_bUpdateRtCst=false;
+		UpdateConstants(this);
+		m_fdbvt.clear();
+		if(m_cfg.collisions&fCollision::VF_SS)
 			{
-				Node&	n=pnodes[i];
-				if(n.m_im>0)
+			for(int i=0;i<getFaces().size();++i)
 				{
-					const btVector3		x=	m_pose.m_trs*m_pose.m_pos[i]+
-						m_pose.m_com;
-					n.m_x=Lerp(n.m_x,x,m_cfg.kMT);
+				Face&	f=getFaces()[i];
+				f.m_leaf=m_fdbvt.insert(BoxOf(f,0),&f);
 				}
 			}
-		}
-		/* External collide		*/ 
-		m_contacts.resize(0);
-
-		if(m_cfg.becollide)
+	}
+	/* Prepare				*/ 
+	m_sst.iit		=	1/(btScalar)m_cfg.iterations;
+	m_sst.sdt		=	dt*m_cfg.timescale;
+	m_sst.isdt		=	1/m_sst.sdt;
+	m_sst.velmrg	=	m_sst.sdt*2;
+	/* Forces				*/ 
+	addVelocity(m_worldInfo->m_gravity*m_sst.sdt);
+	ApplyForces(this,m_sst.sdt);	
+	/* Integrate			*/ 
+	const btScalar	margin=getCollisionShape()->getMargin();
+	const btScalar	updmrg=margin*(btScalar)1.0;
+	for(int i=0,ni=getNodes().size();i<ni;++i)
+	{
+		Node&	n=getNodes()[i];
+		n.m_q	=	n.m_x;
+		n.m_v	+=	n.m_f*n.m_im*m_sst.sdt;
+		n.m_x	+=	n.m_v*m_sst.sdt;
+		m_ndbvt.update(	n.m_leaf,
+						btDbvt::Aabb::FromCR(n.m_x,margin),
+						n.m_v*m_sst.velmrg,updmrg);
+	}
+	UpdateBounds(this);	
+	/* Faces				*/ 
+	if(m_fdbvt.m_root)
 		{
-			StartCollide(m_bounds[0],m_bounds[1]);
-
-			for(int i=0,ni=getNodes().size();i<ni;++i)
+		for(int i=0;i<getFaces().size();++i)
 			{
-				Node&	n=pnodes[i];
-				Contact	c;
-				if((!n.m_battach)&&CheckContact(n.m_x,c.m_cti))
+			Face&	f=getFaces()[i];			
+			const btVector3	v=(	f.m_n[0]->m_v+
+								f.m_n[1]->m_v+
+								f.m_n[2]->m_v)/3;
+			m_fdbvt.update(f.m_leaf,BoxOf(f,margin),v*m_sst.velmrg,updmrg);
+			}
+		}
+	/* Pose					*/ 
+	UpdatePose(this);
+	/* Match				*/ 
+	if(m_pose.m_bframe&&(m_cfg.kMT>0))
+	{
+		for(int i=0,ni=getNodes().size();i<ni;++i)
+		{
+			Node&	n=getNodes()[i];
+			if(n.m_im>0)
+			{
+				const btVector3	x=m_pose.m_rot*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);
+}
+
+//
+void			btSoftBody::solveConstraints()
+{
+	/* Prepare anchors		*/ 
+	for(int 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				*/ 
+	for(int isolve=0;isolve<m_cfg.iterations;++isolve)
+	{
+		const btScalar	lw=	Lerp<btScalar>(1,m_cfg.kSOR,isolve*m_sst.iit)*m_cfg.kLST;
+		PSolve_Anchors(this,m_sst.sdt);
+		PSolve_RContacts(this,m_sst.sdt);
+		PSolve_SContacts(this);
+		PSolve_Links(this,lw);
+	}	
+}
+
+//
+void			btSoftBody::solveCommonConstraints(btSoftBody** bodies,int count,int iterations)
+{
+/// placeholder
+#if 0
+for(int isolve=0;isolve<iterations;++isolve)
+	{
+	for(int ibody=0;ibody<count;++ibody)
+		{
+		PSolve_SContacts(bodies[ibody]);
+		}
+	}
+#endif
+}
+
+//
+void			btSoftBody::integrateMotion()
+{
+	/* Velocities		*/ 
+	const btScalar	vc=m_sst.isdt*(1-m_cfg.kDP);
+	for(int i=0,ni=getNodes().size();i<ni;++i)
+	{
+		Node&	n=getNodes()[i];
+		n.m_v	=	(n.m_x-n.m_q)*vc;
+		n.m_f	=	btVector3(0,0,0);		
+	}
+	/* Update			*/ 
+	UpdateNormals(this);
+}
+
+//
+void			btSoftBody::defaultCollisionHandler(btCollisionObject* pco)
+{
+switch(m_cfg.collisions&fCollision::RVSmask)
+	{
+	case	fCollision::SDF_RS:
+		{
+		struct	DoCollide : btDbvt::ICollide
+			{
+			void		Process(const btDbvt::Node* leaf)
 				{
-					const btRigidBody*	prb=c.m_cti.m_body;
-					const btScalar		ima=n.m_im;
-					const btScalar		imb=prb->getInvMass();
-					const btScalar		ms=ima+imb;
-					if(ms>0)
+				Node*	node=(Node*)leaf->data;
+				DoNode(*node);
+				}
+			void		DoNode(Node& n) const
+				{				
+				RContact	c;
+				if(	(!n.m_battach)&&
+					CheckContact(psb,prb,n.m_x,c.m_cti))
 					{
-						const btTransform&	wtr=prb->getWorldTransform();
+					const btScalar	ima=n.m_im;
+					const btScalar	imb=prb->getInvMass();
+					const btScalar	ms=ima+imb;
+					if(ms>0)
+						{
+						const btTransform&	wtr=prb->getInterpolationWorldTransform();
 						const btMatrix3x3	iwi=prb->getInvInertiaTensorWorld();
 						const btVector3		ra=n.m_x-wtr.getOrigin();
-						const btVector3		va=prb->getVelocityInLocalPoint(ra)*sdt;
+						const btVector3		va=prb->getVelocityInLocalPoint(ra)*psb->m_sst.sdt;
 						const btVector3		vb=n.m_x-n.m_q;	
 						const btVector3		vr=vb-va;
 						const btScalar		dn=dot(vr,c.m_cti.m_normal);
 						const btVector3		fv=vr-c.m_cti.m_normal*dn;
-						const btScalar		fc=m_cfg.kDF*prb->getFriction();
+						const btScalar		fc=psb->m_cfg.kDF*prb->getFriction();
 						c.m_node	=	&n;
-						c.m_c0		=	ImpulseMatrix(sdt,ima,imb,iwi,ra);
+						c.m_c0		=	ImpulseMatrix(psb->m_sst.sdt,ima,imb,iwi,ra);
 						c.m_c1		=	ra;
-						c.m_c2		=	ima*sdt;
+						c.m_c2		=	ima*psb->m_sst.sdt;
 						c.m_c3		=	fv.length2()<(btFabs(dn)*fc)?0:1-fc;
-						m_contacts.push_back(c);
+						psb->m_rcontacts.push_back(c);
+						prb->activate();
+						}
 					}
 				}
-			}
-			EndCollide();
+			btSoftBody*		psb;
+			btRigidBody*	prb;
+			}			docollide;
+		const btScalar	margin=getCollisionShape()->getMargin();
+		btVector3		mins;
+		btVector3		maxs;
+		btDbvt::Aabb	aabb;
+		pco->getCollisionShape()->getAabb(	pco->getInterpolationWorldTransform(),
+											mins,
+											maxs);
+		aabb=btDbvt::Aabb::FromMM(mins,maxs);
+		aabb.Expand(btVector3(margin,margin,margin));		
+		docollide.psb=this;
+		docollide.prb=btRigidBody::upcast(pco);
+		m_ndbvt.collide(aabb,&docollide);
 		}
-		/* Prepare anchors	*/ 
-		for(int 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(	sdt,
-				a.m_node->m_im,
-				a.m_body->getInvMass(),
-				a.m_body->getInvInertiaTensorWorld(),
-				ra);
-			a.m_c1	=	ra;
-			a.m_c2	=	sdt*a.m_node->m_im;
-		}
-		/* Solve			*/ 
-		for(int isolve=0;isolve<m_cfg.iterations;++isolve)
-		{
-			const btScalar	lw=	Lerp<btScalar>(1,m_cfg.kSOR,isolve*iit)*m_cfg.kLST;
-			PSolve_Anchors(this,sdt);
-			PSolve_Contacts(this,sdt);
-			PSolve_Links(this,lw);
-		}
-		/* Velocities		*/ 
-		const btScalar	vc=isdt*(1-m_cfg.kDP);
-		for(int i=0,ni=getNodes().size();i<ni;++i)
-		{
-			pnodes[i].m_v	=	(pnodes[i].m_x-pnodes[i].m_q)*vc;
-			pnodes[i].m_f	=	btVector3(0,0,0);
-		}
-		/* Update			*/ 
-		updateTransform();
-		UpdateNormals(this);
+	break;
 	}
 }
 
-
 //
-bool		btSoftBody::CheckContact(	const btVector3& x,
-												 btSoftBody::sCti& cti)
+void			btSoftBody::defaultCollisionHandler(btSoftBody* psb)
 {
-	btScalar					maxdepth=0;
-	btGjkEpaSolver2::sResults	res;
-	for(int i=0,ni=m_overlappingRigidBodies.size();i<ni;++i)
+const int cf=m_cfg.collisions&psb->m_cfg.collisions;
+switch(cf&fCollision::SVSmask)
 	{
-		btVector3			nrm;
-		btRigidBody*		prb=btRigidBody::upcast(m_overlappingRigidBodies[i]);
-
-		btCollisionShape*	shp=prb->getCollisionShape();
-		btAssert(shp->isConvex());
-		btConvexShape*		csh=static_cast<btConvexShape*>(shp);
-
-		const btTransform&	wtr=prb->getWorldTransform();
-		btScalar			dst=m_worldInfo.m_sparsesdf.Evaluate(wtr.invXform(x),csh,nrm);
-		nrm=wtr.getBasis()*nrm;
-		btVector3			wit=x-nrm*dst;
-		if(dst<maxdepth)
+	case	fCollision::VF_SS:
 		{
-			maxdepth		=	dst;
-			cti.m_body		=	prb;
-			cti.m_normal	=	nrm;
-			cti.m_offset	=	-dot(cti.m_normal,wit);
+		struct	DoCollide : btDbvt::ICollide
+			{
+			void		Process(const btDbvt::Node* lnode,
+								const btDbvt::Node* lface)
+				{
+				Node*	node=(Node*)lnode->data;
+				Face*	face=(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 btVector3	w=BaryCoord(face->m_n[0]->m_x,
+												face->m_n[1]->m_x,
+												face->m_n[2]->m_x,
+												p+o);
+					const btScalar	ma=node->m_im;
+					const btScalar	mb=BaryEval(face->m_n[0]->m_im,
+												face->m_n[1]->m_im,
+												face->m_n[2]->m_im,
+												w);
+					const btScalar	ms=ma+mb;
+					if(ms>0)
+						{
+						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;
+			}	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.collide(	&docollide.psb[1]->m_fdbvt,
+											&docollide);
+		/* psb1 nodes vs psb0 faces	*/ 
+		docollide.psb[0]=psb;
+		docollide.psb[1]=this;
+		docollide.psb[0]->m_ndbvt.collide(	&docollide.psb[1]->m_fdbvt,
+											&docollide);
 		}
+	break;
 	}
-	return(maxdepth<0);
 }
 
-
 //
-void		btSoftBody::EvaluateMedium(	const btVector3& x,
-												   btSoftBody::sMedium& medium)
-{
-	medium.m_velocity	=	btVector3(0,0,0);
-	medium.m_pressure	=	0;
-	medium.m_density	=	m_worldInfo.air_density;
-	if(m_worldInfo.water_density>0)
+// Accessor's
+//
+
+//
+btSoftBody::tNodeArray&				btSoftBody::getNodes()
 	{
-		const btScalar	depth=-(dot(x,m_worldInfo.water_normal)+m_worldInfo.water_offset);
-		if(depth>0)
-		{
-			medium.m_density	=	m_worldInfo.water_density;
-			medium.m_pressure	=	depth			*
-				m_worldInfo.water_density	*
-				m_worldInfo.m_gravity.length();
-		}
+		return ((btSoftBodyCollisionShape*)m_collisionShape)->m_nodes;
+	}
+	
+//
+const btSoftBody::tNodeArray&		btSoftBody::getNodes() const
+	{
+		return ((btSoftBodyCollisionShape*)m_collisionShape)->m_nodes;
+	}
+
+//
+btSoftBody::tLinkArray&				btSoftBody::getLinks()
+	{
+		return ((btSoftBodyCollisionShape*)m_collisionShape)->m_links;
+	}
+	
+//
+const btSoftBody::tLinkArray&		btSoftBody::getLinks() const
+	{
+		return ((btSoftBodyCollisionShape*)m_collisionShape)->m_links;
+	}
+
+//
+btSoftBody::tFaceArray&				btSoftBody::getFaces()
+	{
+		return ((btSoftBodyCollisionShape*)m_collisionShape)->m_faces;
+	}
+
+//
+const btSoftBody::tFaceArray&		btSoftBody::getFaces() const
+	{
+		return ((btSoftBodyCollisionShape*)m_collisionShape)->m_faces;
 	}
-}
diff --git a/src/BulletDynamics/SoftBody/btSoftBody.h b/src/BulletDynamics/SoftBody/btSoftBody.h
index f36d969ca..67094b84c 100644
--- a/src/BulletDynamics/SoftBody/btSoftBody.h
+++ b/src/BulletDynamics/SoftBody/btSoftBody.h
@@ -26,14 +26,15 @@ subject to the following restrictions:
 #include "BulletCollision/CollisionShapes/btConcaveShape.h"
 #include "BulletCollision/CollisionDispatch/btCollisionCreateFunc.h"
 #include "BulletDynamics/SoftBody/btSparseSDF.h"
+#include "BulletDynamics/SoftBody/btDbvt.h"
 
 class btBroadphaseInterface;
 class btCollisionDispatcher;
 
 /// btSoftBody is work-in-progress
-struct	btSoftBody : public btCollisionObject
+class	btSoftBody : public btCollisionObject
 {
-
+public:
 	//
 	// Enumerations
 	//
@@ -44,7 +45,7 @@ struct	btSoftBody : public btCollisionObject
 		Bending,	///Secondary constraints
 	};};
 
-	////eAeroModel 
+	///eAeroModel 
 	struct eAeroModel { enum _ {
 		V_Point,	///Vertex normals are oriented toward velocity
 		V_TwoSided,	///Vertex normals are fliped to match velocity	
@@ -52,68 +53,22 @@ struct	btSoftBody : public btCollisionObject
 		F_TwoSided,	///Face normals are fliped to match velocity
 		F_OneSided,	///Face normals are taken as it is
 	};};
-
-	struct	btSoftBodyWorldInfo
-	{
-		btScalar	air_density;
-		btScalar	water_density;
-		btScalar	water_offset;
-		btVector3	water_normal;	
-		btBroadphaseInterface*	m_broadphase;
-		btCollisionDispatcher*	m_dispatcher;
-		btVector3	m_gravity;
-
-		btSparseSdf<3>						m_sparsesdf;
-	};
-
-	//reference or copy?
-	btSoftBodyWorldInfo&	m_worldInfo;
-
-	///constructor
-
-	btSoftBody(btSoftBody::btSoftBodyWorldInfo& worldInfo,int node_count,
-								   const btVector3* x,
-								   const btScalar* m);
-
-
-	///sCti is Softbody contact info
-	struct	sCti
-	{
-		btRigidBody*	m_body;		/* Rigid body			*/ 
-		btVector3		m_normal;	/* Outward normal		*/ 
-		btScalar		m_offset;	/* Offset from origin	*/ 
-	};
 	
-
-	void	StartCollide(const btVector3& aabbMin,const btVector3& aabbMax)
-	{
-		//??
-	}
-	void	EndCollide()
-	{
-		//??
-	}
-
 	//
-	bool		CheckContact(	const btVector3& x, btSoftBody::sCti& cti);
-	////
-
-	///destructor
-	virtual ~btSoftBody();
-
-
-
-	struct	sMedium
-	{
-		btVector3		m_velocity;	/* Velocity				*/ 
-		btScalar		m_pressure;	/* Pressure				*/ 
-		btScalar		m_density;	/* Density				*/ 
-	};
-
-	virtual void		EvaluateMedium(	const btVector3& /*position*/, 	sMedium& medium);
-
+	// Flags
+	//
+	
+	///fCollision
+	struct fCollision { enum _ {
+		RVSmask	=	0x000f,	///Rigid versus soft mask
+		SDF_RS	=	0x0001,	///SDF base rigid vs soft
+		
+		SVSmask	=	0x00f0,	///Rigid versus soft mask		
+		VF_SS	=	0x0010,	///Vertex vs face soft vs soft handling
+		/* presets	*/ 
+		Default	=	SDF_RS,
+	};};
 	
-
 	//
 	// Internal types
 	//
@@ -121,292 +76,289 @@ struct	btSoftBody : public btCollisionObject
 	typedef btAlignedObjectArray<btScalar>	tScalarArray;
 	typedef btAlignedObjectArray<btVector3>	tVector3Array;
 
+	/* btSoftBodyWorldInfo	*/ 
+	struct	btSoftBodyWorldInfo
+	{
+		btScalar				air_density;
+		btScalar				water_density;
+		btScalar				water_offset;
+		btVector3				water_normal;
+		btBroadphaseInterface*	m_broadphase;
+		btCollisionDispatcher*	m_dispatcher;
+		btVector3				m_gravity;
+		btSparseSdf<3>			m_sparsesdf;
+	};	
+
+	/* sCti is Softbody contact info	*/ 
+	struct	sCti
+	{
+		btRigidBody*	m_body;		/* Rigid body			*/ 
+		btVector3		m_normal;	/* Outward normal		*/ 
+		btScalar		m_offset;	/* Offset from origin	*/ 
+	};	
+
+	/* sMedium		*/ 
+	struct	sMedium
+	{
+		btVector3		m_velocity;	/* Velocity				*/ 
+		btScalar		m_pressure;	/* Pressure				*/ 
+		btScalar		m_density;	/* Density				*/ 
+	};
+
 	/* Base type	*/ 
 	struct	Element
 	{
 		void*			m_tag;			// User data
 	};
-	///Node
+	/* Node			*/ 
 	struct	Node : Element
 	{
-		btVector3		m_x;			// Position
-		btVector3		m_q;			// Previous step position
-		btVector3		m_v;			// Velocity
-		btVector3		m_f;			// Force accumulator
-		btVector3		m_n;			// Normal
-		btScalar		m_im;			// 1/mass
-		btScalar		m_area;			// Area
-		int				m_battach:1;	// Attached
+		btVector3				m_x;			// Position
+		btVector3				m_q;			// Previous step position
+		btVector3				m_v;			// Velocity
+		btVector3				m_f;			// Force accumulator
+		btVector3				m_n;			// Normal
+		btScalar				m_im;			// 1/mass
+		btScalar				m_area;			// Area
+		btDbvt::Node*			m_leaf;			// Leaf data
+		int						m_battach:1;	// Attached
 	};
 	/* Link			*/ 
 	struct	Link : Element
 	{
-		Node*			m_n[2];			// Node pointers
-		btScalar		m_rl;			// Rest length
-		btScalar		m_kST;			// Stiffness coefficient
-		btScalar		m_c0;			// (ima+imb)*kLST
-		btScalar		m_c1;			// rl^2
-		btSoftBody::eLType::_		m_type;			// Link type
+		Node*					m_n[2];			// Node pointers
+		btScalar				m_rl;			// Rest length
+		btScalar				m_kST;			// Stiffness coefficient
+		btScalar				m_c0;			// (ima+imb)*kLST
+		btScalar				m_c1;			// rl^2
+		btSoftBody::eLType::_	m_type;			// Link type
 	};
 	/* Face			*/ 
 	struct	Face : Element
 	{
-		Node*			m_n[3];			// Node pointers
-		btVector3		m_normal;		// Normal
-		btScalar		m_ra;			// Rest area
+		Node*					m_n[3];			// Node pointers
+		btVector3				m_normal;		// Normal
+		btScalar				m_ra;			// Rest area
+		btDbvt::Node*			m_leaf;			// Leaf data
 	};
-	/* Contact		*/ 
-	struct	Contact
+	/* RContact		*/ 
+	struct	RContact
 	{
-		btSoftBody::sCti	m_cti;			// Contact infos
-		Node*			m_node;			// Owner node
-		btMatrix3x3		m_c0;			// Impulse matrix
-		btVector3		m_c1;			// Relative anchor
-		btScalar		m_c2;			// ima*dt
-		btScalar		m_c3;			// Friction
+		btSoftBody::sCti		m_cti;			// Contact infos
+		Node*					m_node;			// Owner node
+		btMatrix3x3				m_c0;			// Impulse matrix
+		btVector3				m_c1;			// Relative anchor
+		btScalar				m_c2;			// ima*dt
+		btScalar				m_c3;			// Friction
+	};
+	/* SContact		*/ 
+	struct	SContact
+	{
+		Node*					m_node;			// Node
+		Face*					m_face;			// Face
+		btVector3				m_weights;		// Weigths
+		btVector3				m_normal;		// Normal
+		btScalar				m_margin;		// Margin
+		btScalar				m_friction;		// Friction
+		btScalar				m_cfm[2];		// Constraint force mixing
 	};
 	/* Anchor		*/ 
 	struct	Anchor
 	{
-		Node*			m_node;			// Node pointer
-		btVector3		m_local;		// Anchor position in body space
-		btRigidBody*	m_body;			// Body
-		btMatrix3x3		m_c0;			// Impulse matrix
-		btVector3		m_c1;			// Relative anchor
-		btScalar		m_c2;			// ima*dt
+		Node*					m_node;			// Node pointer
+		btVector3				m_local;		// Anchor position in body space
+		btRigidBody*			m_body;			// Body
+		btMatrix3x3				m_c0;			// Impulse matrix
+		btVector3				m_c1;			// Relative anchor
+		btScalar				m_c2;			// ima*dt
 	};
 	/* Pose			*/ 
 	struct	Pose
 	{
-		bool			m_bvolume;		// Is valid
-		bool			m_bframe;		// Is frame
-		btScalar		m_volume;		// Rest volume
-		tVector3Array	m_pos;			// Reference positions
-		tScalarArray	m_wgh;			// Weights
-		btVector3		m_com;			// COM
-		btMatrix3x3		m_trs;			// Transform
+		bool					m_bvolume;		// Is valid
+		bool					m_bframe;		// Is frame
+		btScalar				m_volume;		// Rest volume
+		tVector3Array			m_pos;			// Reference positions
+		tScalarArray			m_wgh;			// Weights
+		btVector3				m_com;			// COM
+		btMatrix3x3				m_rot;			// Rotation
+		btMatrix3x3				m_scl;			// Scale
+		btMatrix3x3				m_aqq;			// Base scaling
 	};	
 	/* Config		*/ 
 	struct	Config
 	{
 		btSoftBody::eAeroModel::_	aeromodel;		// Aerodynamic model (default: V_Point)
-		btScalar		kLST;			// Linear stiffness coefficient [0,1]
-		btScalar		kDP;			// Damping coefficient [0,1]
-		btScalar		kDG;			// Drag coefficient [0,+inf]
-		btScalar		kLF;			// Lift coefficient [0,+inf]
-		btScalar		kPR;			// Pressure coefficient [-inf,+inf]
-		btScalar		kVC;			// Volume conversation coefficient [0,+inf]
-		btScalar		kDF;			// Dynamic friction coefficient [0,1]
-		btScalar		kMT;			// Pose matching coefficient [0,1]		
-		btScalar		kSOR;			// SOR(w) [1,2] default 1, never use with solver!=Accurate
-		btScalar		kCHR;			// Contacts hardness [0,1]
-		btScalar		kAHR;			// Anchors hardness [0,1]
-		btScalar		timescale;		// Time scale
-		btScalar		timestep;		// Time step
-		int				maxsteps;		// Maximum time steps
-		int				iterations;		// Solver iterations
-		bool			becollide;		// Enable external collisions
-		bool			bscollide;		// Enable self collisions
+		btScalar				kLST;			// Linear stiffness coefficient [0,1]
+		btScalar				kDP;			// Damping coefficient [0,1]
+		btScalar				kDG;			// Drag coefficient [0,+inf]
+		btScalar				kLF;			// Lift coefficient [0,+inf]
+		btScalar				kPR;			// Pressure coefficient [-inf,+inf]
+		btScalar				kVC;			// Volume conversation coefficient [0,+inf]
+		btScalar				kDF;			// Dynamic friction coefficient [0,1]
+		btScalar				kMT;			// Pose matching coefficient [0,1]		
+		btScalar				kSOR;			// SOR(w) [1,2] default 1, never use with solver!=Accurate
+		btScalar				kCHR;			// Rigid contacts hardness [0,1]
+		btScalar				kSHR;			// Soft contacts hardness [0,1]
+		btScalar				kAHR;			// Anchors hardness [0,1]
+		btScalar				maxvolume;		// Maximum volume ratio for pose
+		btScalar				timescale;		// Time scale
+		int						iterations;		// Solver iterations
+		int						collisions;		// Collisions flags
+	};
+	/* SolverState	*/ 
+	struct	SolverState
+	{
+		btScalar				iit;			// 1/iterations
+		btScalar				sdt;			// dt*timescale
+		btScalar				isdt;			// 1/sdt
+		btScalar				velmrg;			// velocity margin
 	};
 
 	//
 	// Typedef's
 	//
 
-	typedef btAlignedObjectArray<Node>		tNodeArray;
-	typedef btAlignedObjectArray<Link>		tLinkArray;
-	typedef btAlignedObjectArray<Face>		tFaceArray;
-	typedef btAlignedObjectArray<Anchor>	tAnchorArray;
-	typedef btAlignedObjectArray<Contact>	tContactArray;
+	typedef btAlignedObjectArray<Node>			tNodeArray;
+	typedef btAlignedObjectArray<Link>			tLinkArray;
+	typedef btAlignedObjectArray<Face>			tFaceArray;
+	typedef btAlignedObjectArray<Anchor>		tAnchorArray;
+	typedef btAlignedObjectArray<RContact>		tRContactArray;
+	typedef btAlignedObjectArray<SContact>		tSContactArray;
+	typedef btAlignedObjectArray<btSoftBody*>	tSoftBodyArray;
 
 	//
 	// Fields
 	//
 
-	Config				m_cfg;			// Configuration
-	Pose				m_pose;			// Pose
-	void*				m_tag;			// User data
-
-	//////////////////////
-
-	///btSoftBodyCollisionShape is work-in-progress collision shape for softbodies
-	class btSoftBodyCollisionShape : public btConcaveShape
-	{
-		static btVector3 m_sScaling;
-	public:
-
-		tNodeArray			m_nodes;		// Nodes
-		tLinkArray			m_links;		// Links
-		tFaceArray			m_faces;		// Faces
-
-		btSoftBodyCollisionShape();
-
-		virtual ~btSoftBodyCollisionShape();
-
-		virtual void	processAllTriangles(btTriangleCallback* callback,const btVector3& aabbMin,const btVector3& aabbMax) const;
-
-		///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
-		{
-			///not yet
-			btAssert(0);
-		}
-
-		virtual int		getShapeType() const
-		{
-			return SOFTBODY_SHAPE_PROXYTYPE;
-		}
-		virtual void	setLocalScaling(const btVector3& scaling)
-		{
-			///not yet
-			btAssert(0);
-		}
-		virtual const btVector3& getLocalScaling() const
-		{
-			///not yet
-			btAssert(0);
-			return m_sScaling;
-		}
-		virtual void	calculateLocalInertia(btScalar mass,btVector3& inertia) const
-		{
-			///not yet
-			btAssert(0);
-		}
-		virtual const char*	getName()const
-		{
-			return "SoftBody";
-		}
-
-	};
-
-
-	btSoftBodyCollisionShape*	m_softBodyCollisionShape;
-
-	btCollisionObjectArray m_overlappingRigidBodies;
-
-	btAlignedObjectArray<btSoftBody*>		m_overlappingSoftBodies;
-
-
-	//////////////////////
-
-	inline tNodeArray&	getNodes()
-	{
-		return m_softBodyCollisionShape->m_nodes;
-	}
-	inline const tNodeArray&	getNodes() const
-	{
-		return m_softBodyCollisionShape->m_nodes;
-	}
-
-	inline tLinkArray&			getLinks()
-	{
-		return m_softBodyCollisionShape->m_links;
-	}
-	inline const	tLinkArray&			getLinks() const
-	{
-		return m_softBodyCollisionShape->m_links;
-	}
-
-	inline tFaceArray&			getFaces()
-	{
-		return m_softBodyCollisionShape->m_faces;
-	}
-
-	inline const	tFaceArray&			getFaces() const
-	{
-		return m_softBodyCollisionShape->m_faces;
-	}
-
-
-	tAnchorArray		m_anchors;		// Anchors
-	tContactArray		m_contacts;		// Contacts
-	btScalar			m_timeacc;		// Time accumulator
-	btVector3			m_bounds[2];	// Spatial bounds	
-	bool				m_bUpdateRtCst;	// Update runtime constants
-
+	Config					m_cfg;			// Configuration
+	SolverState				m_sst;			// Solver state
+	Pose					m_pose;			// Pose
+	void*					m_tag;			// User data
+	btSoftBodyWorldInfo*	m_worldInfo;	//
+	tAnchorArray			m_anchors;		// Anchors
+	tRContactArray			m_rcontacts;	// Rigid contacts
+	tSContactArray			m_scontacts;	// Soft contacts
+	btScalar				m_timeacc;		// Time accumulator
+	btVector3				m_bounds[2];	// Spatial bounds	
+	bool					m_bUpdateRtCst;	// Update runtime constants
+	btDbvt					m_ndbvt;		// Nodes tree
+	btDbvt					m_fdbvt;		// Faces tree
+	
 	//
 	// Api
 	//
-
-	/* Delete a body														*/ 
-	void				Delete();
+	
+	/* ctor																	*/ 
+	btSoftBody(	btSoftBody::btSoftBodyWorldInfo* worldInfo,int node_count,
+				const btVector3* x,
+				const btScalar* m);
+	/* dtor																	*/ 
+	virtual ~btSoftBody();
 	/* Check for existing link												*/ 
-	bool				CheckLink(	int node0,
+	bool				checkLink(	int node0,
 		int node1) const;
-	bool				CheckLink(	const btSoftBody::Node* node0,
+	bool				checkLink(	const btSoftBody::Node* node0,
 		const btSoftBody::Node* node1) const;
 	/* Check for existring face												*/ 
-	bool				CheckFace(	int node0,
+	bool				checkFace(	int node0,
 		int node1,
 		int node2) const;
 	/* Append link															*/ 
-	void				AppendLink(		int node0,
+	void				appendLink(		int node0,
 		int node1,
 		btScalar kST,
 		btSoftBody::eLType::_ type,
 		bool bcheckexist=false);
-	void				AppendLink(		btSoftBody::Node* node0,
+	void				appendLink(		btSoftBody::Node* node0,
 		btSoftBody::Node* node1,
 		btScalar kST,
 		btSoftBody::eLType::_ type,
 		bool bcheckexist=false);
 	/* Append face															*/ 
-	void				AppendFace(		int node0,
+	void				appendFace(		int node0,
 		int node1,
 		int node2);
 	/* Append anchor														*/ 
-	void				AppendAnchor(	int node,
+	void				appendAnchor(	int node,
 		btRigidBody* body);
 	/* Add force (or gravity) to the entire body							*/ 
-	void				AddForce(		const btVector3& force);
+	void				addForce(		const btVector3& force);
 	/* Add force (or gravity) to a node of the body							*/ 
-	void				AddForce(		const btVector3& force,
+	void				addForce(		const btVector3& force,
 		int node);
 	/* Add velocity to the entire body										*/ 
-	void				AddVelocity(	const btVector3& velocity);
+	void				addVelocity(	const btVector3& velocity);
 	/* Add velocity to a node of the body									*/ 
-	void				AddVelocity(	const btVector3& velocity,
+	void				addVelocity(	const btVector3& velocity,
 		int node);
 	/* Set mass																*/ 
-	void				SetMass(		int node,
+	void				setMass(		int node,
 		btScalar mass);
 	/* Get mass																*/ 
-	btScalar			GetMass(		int node) const;
+	btScalar			getMass(		int node) const;
 	/* Get total mass														*/ 
-	btScalar			GetTotalMass() const;
+	btScalar			getTotalMass() const;
 	/* Set total mass (weighted by previous masses)							*/ 
-	void				SetTotalMass(	btScalar mass,
+	void				setTotalMass(	btScalar mass,
 		bool fromfaces=false);
 	/* Set total density													*/ 
-	void				SetTotalDensity(btScalar density);
+	void				setTotalDensity(btScalar density);
 	/* Transform															*/ 
-	void				Transform(		const btTransform& trs);
+	void				transform(		const btTransform& trs);
 	/* Scale																*/ 
-	void				Scale(			const btVector3& scl);
+	void				scale(			const btVector3& scl);
 	/* Set current state as pose											*/ 
-	void				SetPose(		bool bvolume,
+	void				setPose(		bool bvolume,
 		bool bframe);
 	/* Return the volume													*/ 
-	btScalar			GetVolume() const;
+	btScalar			getVolume() const;
 	/* Generate bending constraints based on distance in the adjency graph	*/ 
-	int					GenerateBendingConstraints(	int distance,
+	int					generateBendingConstraints(	int distance,
 		btScalar stiffness);
 	/* Randomize constraints to reduce solver bias							*/ 
-	void				RandomizeConstraints();
+	void				randomizeConstraints();
 	/* Ray casting															*/ 
-	btScalar			Raycast(		const btVector3& org,
-		const btVector3& dir) const;
-	/* Step 																*/ 
-	void				Step(			btScalar dt);
-
-	void	updateBounds();
-
-	void			updateTransform()
+	btScalar			raycast(const btVector3& org,
+								const btVector3& dir) const;
+	/* predictMotion														*/ 
+	void				predictMotion(btScalar dt);
+	/* solveConstraints														*/ 
+	void				solveConstraints();
+	/* solveCommonConstraints												*/ 
+	static void			solveCommonConstraints(btSoftBody** bodies,int count,int iterations);
+	/* integrateMotion														*/ 
+	void				integrateMotion();
+	/* defaultCollisionHandlers												*/ 
+	void				defaultCollisionHandler(btCollisionObject* pco);
+	void				defaultCollisionHandler(btSoftBody* psb);
+	
+	///to keep collision detection and dynamics separate we don't store a rigidbody pointer
+	///but a rigidbody is derived from btCollisionObject, so we can safely perform an upcast
+	static const btSoftBody*	upcast(const btCollisionObject* colObj)
 	{
-		updateBounds();
+		if (colObj->getInternalType()==CO_SOFT_BODY)
+			return (const btSoftBody*)colObj;
+		return 0;
+	}
+	static btSoftBody*	upcast(btCollisionObject* colObj)
+	{
+		if (colObj->getInternalType()==CO_SOFT_BODY)
+			return (btSoftBody*)colObj;
+		return 0;
 	}
 
-
+	//
+	// ...
+	//
+	
+	tNodeArray&			getNodes();
+	const tNodeArray&	getNodes() const;
+	tLinkArray&			getLinks();
+	const tLinkArray&	getLinks() const;
+	tFaceArray&			getFaces();
+	const tFaceArray&	getFaces() const;
+	
 };
 
 
diff --git a/src/BulletDynamics/SoftBody/btSoftBodyHelpers.cpp b/src/BulletDynamics/SoftBody/btSoftBodyHelpers.cpp
index 131ddd320..7ffab5967 100644
--- a/src/BulletDynamics/SoftBody/btSoftBodyHelpers.cpp
+++ b/src/BulletDynamics/SoftBody/btSoftBodyHelpers.cpp
@@ -15,13 +15,15 @@ subject to the following restrictions:
 ///btSoftBodyHelpers.cpp by Nathanael Presson
 
 #include "btSoftBody.h"
+#include "btDbvt.h"
 #include <stdio.h>
 #include <string.h>
 #include "btSoftBodyHelpers.h"
+#include "LinearMath/btConvexHull.h"
 
 //
-void					btSoftBodyHelpers::drawVertex(	btIDebugDraw* idraw,
-		const btVector3& x,btScalar s,const btVector3& c)
+static void				drawVertex(	btIDebugDraw* idraw,
+									const btVector3& x,btScalar s,const btVector3& c)
 	{
 		idraw->drawLine(x-btVector3(s,0,0),x+btVector3(s,0,0),c);
 		idraw->drawLine(x-btVector3(0,s,0),x+btVector3(0,s,0),c);
@@ -29,26 +31,70 @@ void					btSoftBodyHelpers::drawVertex(	btIDebugDraw* idraw,
 	}
 
 //
- btVector3			btSoftBodyHelpers::stresscolor(btScalar stress)
+static void				drawBox(	btIDebugDraw* idraw,
+									const btVector3& mins,
+									const btVector3& maxs,
+									const btVector3& color)
 {
-	static const btVector3	spectrum[]=	{
-		btVector3(1,0,1),
-		btVector3(0,0,1),
-		btVector3(0,1,1),
-		btVector3(0,1,0),
-		btVector3(1,1,0),
-		btVector3(1,0,0),
-		btVector3(1,0,0),
-	};
+const btVector3	c[]={	btVector3(mins.x(),mins.y(),mins.z()),
+						btVector3(maxs.x(),mins.y(),mins.z()),
+						btVector3(maxs.x(),maxs.y(),mins.z()),
+						btVector3(mins.x(),maxs.y(),mins.z()),
+						btVector3(mins.x(),mins.y(),maxs.z()),
+						btVector3(maxs.x(),mins.y(),maxs.z()),
+						btVector3(maxs.x(),maxs.y(),maxs.z()),
+						btVector3(mins.x(),maxs.y(),maxs.z())};
+idraw->drawLine(c[0],c[1],color);idraw->drawLine(c[1],c[2],color);
+idraw->drawLine(c[2],c[3],color);idraw->drawLine(c[3],c[0],color);
+idraw->drawLine(c[4],c[5],color);idraw->drawLine(c[5],c[6],color);
+idraw->drawLine(c[6],c[7],color);idraw->drawLine(c[7],c[4],color);
+idraw->drawLine(c[0],c[4],color);idraw->drawLine(c[1],c[5],color);
+idraw->drawLine(c[2],c[6],color);idraw->drawLine(c[3],c[7],color);
+}
+
+//
+static void				drawTree(	btIDebugDraw* idraw,
+									const btDbvt::Node* node,
+									int depth,
+									const btVector3& ncolor,
+									const btVector3& lcolor,
+									int mindepth,
+									int maxdepth)
+{
+if(node)
+	{
+	if(node->isinternal()&&((depth<maxdepth)||(maxdepth<0)))
+		{
+		drawTree(idraw,node->childs[0],depth+1,ncolor,lcolor,mindepth,maxdepth);
+		drawTree(idraw,node->childs[1],depth+1,ncolor,lcolor,mindepth,maxdepth);
+		}
+	if(depth>=mindepth)
+		{
+		const btScalar	scl=(btScalar)(node->isinternal()?1:1);
+		const btVector3	mi=node->box.Center()-node->box.Extent()*scl;
+		const btVector3	mx=node->box.Center()+node->box.Extent()*scl;
+		drawBox(idraw,mi,mx,node->isleaf()?lcolor:ncolor);
+		}
+	}
+}
+
+//
+static btVector3		stresscolor(btScalar stress)
+	{
+	static const btVector3	spectrum[]=	{	btVector3(1,0,1),
+											btVector3(0,0,1),
+											btVector3(0,1,1),
+											btVector3(0,1,0),
+											btVector3(1,1,0),
+											btVector3(1,0,0),
+											btVector3(1,0,0)};
 	static const int		ncolors=sizeof(spectrum)/sizeof(spectrum[0])-1;
 	static const btScalar	one=1;
 	stress=btMax<btScalar>(0,btMin<btScalar>(1,stress))*ncolors;
 	const int				sel=(int)stress;
 	const btScalar			frc=stress-sel;
 	return(spectrum[sel]+(spectrum[sel+1]-spectrum[sel])*frc);
-}
-
-
+	}
 
 //
 void			btSoftBodyHelpers::Draw(	btSoftBody* psb,
@@ -105,9 +151,9 @@ void			btSoftBodyHelpers::Draw(	btSoftBody* psb,
 		static const btVector3		axis[]={btVector3(1,0,0),
 			btVector3(0,1,0),
 			btVector3(0,0,1)};
-		for(int i=0;i<psb->m_contacts.size();++i)
+		for(int i=0;i<psb->m_rcontacts.size();++i)
 		{		
-			const btSoftBody::Contact&	c=psb->m_contacts[i];
+			const btSoftBody::RContact&	c=psb->m_rcontacts[i];
 			const btVector3				o=	c.m_node->m_x-c.m_cti.m_normal*
 				(dot(c.m_node->m_x,c.m_cti.m_normal)+c.m_cti.m_offset);
 			const btVector3				x=cross(c.m_cti.m_normal,axis[c.m_cti.m_normal.minAxis()]).normalized();
@@ -183,6 +229,24 @@ void			btSoftBodyHelpers::DrawInfos(		btSoftBody* psb,
 	}
 }
 
+//
+void			btSoftBodyHelpers::DrawNodeTree(	btSoftBody* psb,
+													btIDebugDraw* idraw,
+													int mindepth,
+													int maxdepth)
+{
+drawTree(idraw,psb->m_ndbvt.m_root,0,btVector3(1,0,1),btVector3(1,1,1),mindepth,maxdepth);
+}
+
+//
+void			btSoftBodyHelpers::DrawFaceTree(	btSoftBody* psb,
+													btIDebugDraw* idraw,
+													int mindepth,
+													int maxdepth)
+{
+drawTree(idraw,psb->m_fdbvt.m_root,0,btVector3(0,1,0),btVector3(1,0,0),mindepth,maxdepth);
+}
+
 //
 void			btSoftBodyHelpers::DrawFrame(		btSoftBody* psb,
 						  btIDebugDraw* idraw)
@@ -192,7 +256,7 @@ void			btSoftBodyHelpers::DrawFrame(		btSoftBody* psb,
 		static const btScalar	ascl=10;
 		static const btScalar	nscl=(btScalar)0.1;
 		const btVector3			com=psb->m_pose.m_com;
-		const btMatrix3x3&		trs=psb->m_pose.m_trs;
+		const btMatrix3x3		trs=psb->m_pose.m_rot*psb->m_pose.m_scl;
 		const btVector3			Xaxis=(trs*btVector3(1,0,0)).normalized();
 		const btVector3			Yaxis=(trs*btVector3(0,1,0)).normalized();
 		const btVector3			Zaxis=(trs*btVector3(0,0,1)).normalized();
@@ -225,15 +289,15 @@ btSoftBody*		btSoftBodyHelpers::CreateRope(	btSoftBody::btSoftBodyWorldInfo& wor
 		x[i]=lerp(from,to,t);
 		m[i]=1;
 	}
-	btSoftBody*		psb= new btSoftBody(worldInfo,r,x,m);
-	if(fixeds&1) psb->SetMass(0,0);
-	if(fixeds&2) psb->SetMass(r-1,0);
+	btSoftBody*		psb= new btSoftBody(&worldInfo,r,x,m);
+	if(fixeds&1) psb->setMass(0,0);
+	if(fixeds&2) psb->setMass(r-1,0);
 	delete[] x;
 	delete[] m;
 	/* Create links	*/ 
 	for(int i=1;i<r;++i)
 	{
-		psb->AppendLink(i-1,i,1,btSoftBody::eLType::Structural);
+		psb->appendLink(i-1,i,1,btSoftBody::eLType::Structural);
 	}
 	/* Finished		*/ 
 	return(psb);
@@ -269,11 +333,11 @@ btSoftBody*		btSoftBodyHelpers::CreatePatch(btSoftBody::btSoftBodyWorldInfo& wor
 			m[IDX(ix,iy)]=1;
 		}
 	}
-	btSoftBody*		psb=new btSoftBody(worldInfo,tot,x,m);
-	if(fixeds&1)	psb->SetMass(IDX(0,0),0);
-	if(fixeds&2)	psb->SetMass(IDX(rx-1,0),0);
-	if(fixeds&4)	psb->SetMass(IDX(0,ry-1),0);
-	if(fixeds&8)	psb->SetMass(IDX(rx-1,ry-1),0);
+	btSoftBody*		psb=new btSoftBody(&worldInfo,tot,x,m);
+	if(fixeds&1)	psb->setMass(IDX(0,0),0);
+	if(fixeds&2)	psb->setMass(IDX(rx-1,0),0);
+	if(fixeds&4)	psb->setMass(IDX(0,ry-1),0);
+	if(fixeds&8)	psb->setMass(IDX(rx-1,ry-1),0);
 	delete[] x;
 	delete[] m;
 	/* Create links	and faces */ 
@@ -284,29 +348,29 @@ btSoftBody*		btSoftBodyHelpers::CreatePatch(btSoftBody::btSoftBodyWorldInfo& wor
 			const int	idx=IDX(ix,iy);
 			const bool	mdx=(ix+1)<rx;
 			const bool	mdy=(iy+1)<ry;
-			if(mdx) psb->AppendLink(idx,IDX(ix+1,iy),
+			if(mdx) psb->appendLink(idx,IDX(ix+1,iy),
 				1,btSoftBody::eLType::Structural);
-			if(mdy) psb->AppendLink(idx,IDX(ix,iy+1),
+			if(mdy) psb->appendLink(idx,IDX(ix,iy+1),
 				1,btSoftBody::eLType::Structural);			
 			if(mdx&&mdy)
 			{
 				if((ix+iy)&1)
 				{
-					psb->AppendFace(IDX(ix,iy),IDX(ix+1,iy),IDX(ix+1,iy+1));
-					psb->AppendFace(IDX(ix,iy),IDX(ix+1,iy+1),IDX(ix,iy+1));
+					psb->appendFace(IDX(ix,iy),IDX(ix+1,iy),IDX(ix+1,iy+1));
+					psb->appendFace(IDX(ix,iy),IDX(ix+1,iy+1),IDX(ix,iy+1));
 					if(gendiags)
 					{
-						psb->AppendLink(IDX(ix,iy),IDX(ix+1,iy+1),
+						psb->appendLink(IDX(ix,iy),IDX(ix+1,iy+1),
 							1,btSoftBody::eLType::Structural);
 					}
 				}
 				else
 				{
-					psb->AppendFace(IDX(ix,iy+1),IDX(ix,iy),IDX(ix+1,iy));
-					psb->AppendFace(IDX(ix,iy+1),IDX(ix+1,iy),IDX(ix+1,iy+1));
+					psb->appendFace(IDX(ix,iy+1),IDX(ix,iy),IDX(ix+1,iy));
+					psb->appendFace(IDX(ix,iy+1),IDX(ix+1,iy),IDX(ix+1,iy+1));
 					if(gendiags)
 					{
-						psb->AppendLink(IDX(ix+1,iy),IDX(ix,iy+1),
+						psb->appendLink(IDX(ix+1,iy),IDX(ix,iy+1),
 							1,btSoftBody::eLType::Structural);
 					}
 				}
@@ -369,7 +433,7 @@ btSoftBody*		btSoftBodyHelpers::CreateFromTriMesh(btSoftBody::btSoftBodyWorldInf
 	{
 		vtx[j]=btVector3(vertices[i],vertices[i+1],vertices[i+2]);
 	}
-	btSoftBody*		psb=new btSoftBody(worldInfo,vtx.size(),&vtx[0],0);
+	btSoftBody*		psb=new btSoftBody(&worldInfo,vtx.size(),&vtx[0],0);
 	for(int i=0,ni=ntriangles*3;i<ni;i+=3)
 	{
 		const int idx[]={triangles[i],triangles[i+1],triangles[i+2]};
@@ -380,12 +444,42 @@ btSoftBody*		btSoftBodyHelpers::CreateFromTriMesh(btSoftBody::btSoftBodyWorldInf
 			{
 				chks[IDX(idx[j],idx[k])]=true;
 				chks[IDX(idx[k],idx[k])]=true;
-				psb->AppendLink(idx[j],idx[k],1,btSoftBody::eLType::Structural);
+				psb->appendLink(idx[j],idx[k],1,btSoftBody::eLType::Structural);
 			}
 		}
 #undef IDX
-		psb->AppendFace(idx[0],idx[1],idx[2]);
+		psb->appendFace(idx[0],idx[1],idx[2]);
 	}
-	psb->RandomizeConstraints();
+	psb->randomizeConstraints();
 	return(psb);
 }
+
+//
+btSoftBody*		btSoftBodyHelpers::CreateFromConvexHull(btSoftBody::btSoftBodyWorldInfo& worldInfo,	const btVector3* vertices,
+									 int nvertices)
+{
+	HullDesc		hdsc(QF_TRIANGLES,nvertices,vertices);
+	HullResult		hres;
+	HullLibrary		hlib;/*??*/ 
+	hdsc.mMaxVertices=nvertices;
+	hlib.CreateConvexHull(hdsc,hres);
+	btSoftBody*		psb=new btSoftBody(&worldInfo,(int)hres.mNumOutputVertices,
+		&hres.m_OutputVertices[0],0);
+	for(int i=0;i<(int)hres.mNumFaces;++i)
+	{
+		const int idx[]={	hres.m_Indices[i*3+0],
+			hres.m_Indices[i*3+1],
+			hres.m_Indices[i*3+2]};
+		if(idx[0]<idx[1]) psb->appendLink(	idx[0],idx[1],
+			1,btSoftBody::eLType::Structural);
+		if(idx[1]<idx[2]) psb->appendLink(	idx[1],idx[2],
+			1,btSoftBody::eLType::Structural);
+		if(idx[2]<idx[0]) psb->appendLink(	idx[2],idx[0],
+			1,btSoftBody::eLType::Structural);
+		psb->appendFace(idx[0],idx[1],idx[2]);
+	}
+	hlib.ReleaseResult(hres);
+	psb->randomizeConstraints();
+	return(psb);
+}
+
diff --git a/src/BulletDynamics/SoftBody/btSoftBodyHelpers.h b/src/BulletDynamics/SoftBody/btSoftBodyHelpers.h
index 15c82e22f..a45967e2d 100644
--- a/src/BulletDynamics/SoftBody/btSoftBodyHelpers.h
+++ b/src/BulletDynamics/SoftBody/btSoftBodyHelpers.h
@@ -40,26 +40,29 @@ struct	fDrawFlags { enum _ {
 
 struct	btSoftBodyHelpers
 {
-
-	static btVector3			stresscolor(btScalar stress);
-
-	static void					drawVertex(	btIDebugDraw* idraw,
-		const btVector3& x,btScalar s,const btVector3& c);
-
-
 	/* Draw body															*/ 
-	static void			Draw(			btSoftBody* psb,
-						 btIDebugDraw* idraw,
-						 int drawflags=fDrawFlags::Std);
+	static void				Draw(		btSoftBody* psb,
+										btIDebugDraw* idraw,
+										int drawflags=fDrawFlags::Std);
 	/* Draw body infos														*/ 
-	static	void			DrawInfos(		btSoftBody* psb,
-							  btIDebugDraw* idraw,
-							  bool masses,
-							  bool areas,
-							  bool stress);
+	static	void			DrawInfos(	btSoftBody* psb,
+										btIDebugDraw* idraw,
+										bool masses,
+										bool areas,
+										bool stress);
+	/* Draw node tree														*/ 
+	static void				DrawNodeTree(	btSoftBody* psb,
+											btIDebugDraw* idraw,
+											int mindepth=0,
+											int maxdepth=-1);
+	/* Draw face tree														*/ 
+	static void				DrawFaceTree(	btSoftBody* psb,
+											btIDebugDraw* idraw,
+											int mindepth=0,
+											int maxdepth=-1);
 	/* Draw rigid frame														*/ 
 	static	void			DrawFrame(		btSoftBody* psb,
-							  btIDebugDraw* idraw);
+											btIDebugDraw* idraw);
 	/* Create a rope														*/ 
 	static	btSoftBody*		CreateRope(	 btSoftBody::btSoftBodyWorldInfo& worldInfo,const btVector3& from,
 							   const btVector3& to,
diff --git a/src/BulletDynamics/SoftBody/btSoftBodyRigidBodyCollisionConfiguration.cpp b/src/BulletDynamics/SoftBody/btSoftBodyRigidBodyCollisionConfiguration.cpp
new file mode 100644
index 000000000..5bd80f52f
--- /dev/null
+++ b/src/BulletDynamics/SoftBody/btSoftBodyRigidBodyCollisionConfiguration.cpp
@@ -0,0 +1,77 @@
+/*
+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.
+*/
+
+#include "btSoftBodyRigidBodyCollisionConfiguration.h"
+#include "btSoftRigidCollisionAlgorithm.h"
+#include "btSoftSoftCollisionAlgorithm.h"
+
+
+btSoftBodyRigidBodyCollisionConfiguration::btSoftBodyRigidBodyCollisionConfiguration(btStackAlloc*	stackAlloc,btPoolAllocator*	persistentManifoldPool,btPoolAllocator*	collisionAlgorithmPool)
+:btDefaultCollisionConfiguration(stackAlloc,persistentManifoldPool,collisionAlgorithmPool)
+{
+	void* mem;
+
+	mem = btAlignedAlloc(sizeof(btSoftSoftCollisionAlgorithm::CreateFunc),16);
+	m_softSoftCreateFunc = new(mem) btSoftSoftCollisionAlgorithm::CreateFunc;
+	
+	mem = btAlignedAlloc(sizeof(btSoftRigidCollisionAlgorithm::CreateFunc),16);
+	m_softRigidCreateFunc = new(mem) btSoftRigidCollisionAlgorithm::CreateFunc;
+	
+	mem = btAlignedAlloc(sizeof(btSoftRigidCollisionAlgorithm::CreateFunc),16);
+	m_swappedSoftRigidCreateFunc = new(mem) btSoftRigidCollisionAlgorithm::CreateFunc;
+	m_swappedSoftRigidCreateFunc->m_swapped=true;
+
+}
+
+btSoftBodyRigidBodyCollisionConfiguration::~btSoftBodyRigidBodyCollisionConfiguration()
+{
+	m_softSoftCreateFunc->~btCollisionAlgorithmCreateFunc();
+	btAlignedFree(	m_softSoftCreateFunc);
+
+	m_softRigidCreateFunc->~btCollisionAlgorithmCreateFunc();
+	btAlignedFree(	m_softRigidCreateFunc);
+
+	m_swappedSoftRigidCreateFunc->~btCollisionAlgorithmCreateFunc();
+	btAlignedFree(	m_swappedSoftRigidCreateFunc);
+
+
+}
+	
+///creation of soft-soft and soft-rigid, and otherwise fallback to base class implementation
+btCollisionAlgorithmCreateFunc* btSoftBodyRigidBodyCollisionConfiguration::getCollisionAlgorithmCreateFunc(int proxyType0,int proxyType1)
+{
+
+	///try to handle the softbody interactions first
+
+	if ((proxyType0 == SOFTBODY_SHAPE_PROXYTYPE  ) && (proxyType1==SOFTBODY_SHAPE_PROXYTYPE))
+	{
+		return	m_softSoftCreateFunc;
+	}
+
+	///other can't be also softbody, so assume rigid for now
+	if (proxyType0 == SOFTBODY_SHAPE_PROXYTYPE )
+	{
+		return	m_softRigidCreateFunc;
+	}
+
+	///other can't be also softbody, so assume rigid for now
+	if (proxyType1 == SOFTBODY_SHAPE_PROXYTYPE )
+	{
+		return	m_swappedSoftRigidCreateFunc;
+	}
+
+	///fallback to the regular rigid collision shape
+	return btDefaultCollisionConfiguration::getCollisionAlgorithmCreateFunc(proxyType0,proxyType1);
+}
\ No newline at end of file
diff --git a/src/BulletDynamics/SoftBody/btSoftBodyRigidBodyCollisionConfiguration.h b/src/BulletDynamics/SoftBody/btSoftBodyRigidBodyCollisionConfiguration.h
new file mode 100644
index 000000000..e38289b76
--- /dev/null
+++ b/src/BulletDynamics/SoftBody/btSoftBodyRigidBodyCollisionConfiguration.h
@@ -0,0 +1,46 @@
+/*
+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.
+*/
+
+#ifndef BT_SOFTBODY_RIGIDBODY_COLLISION_CONFIGURATION
+#define BT_SOFTBODY_RIGIDBODY_COLLISION_CONFIGURATION
+
+#include "BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.h"
+
+class btVoronoiSimplexSolver;
+class btGjkEpaPenetrationDepthSolver;
+
+
+///btSoftBodyRigidBodyCollisionConfiguration add softbody interaction on top of btDefaultCollisionConfiguration
+class	btSoftBodyRigidBodyCollisionConfiguration : public btDefaultCollisionConfiguration
+{
+
+	//default CreationFunctions, filling the m_doubleDispatch table
+	btCollisionAlgorithmCreateFunc*	m_softSoftCreateFunc;
+	btCollisionAlgorithmCreateFunc*	m_softRigidCreateFunc;
+	btCollisionAlgorithmCreateFunc*	m_swappedSoftRigidCreateFunc;
+	
+public:
+
+	btSoftBodyRigidBodyCollisionConfiguration(btStackAlloc*	stackAlloc=0,btPoolAllocator*	persistentManifoldPool=0,btPoolAllocator*	collisionAlgorithmPool=0);
+
+	virtual ~btSoftBodyRigidBodyCollisionConfiguration();
+
+	///creation of soft-soft and soft-rigid, and otherwise fallback to base class implementation
+	virtual btCollisionAlgorithmCreateFunc* getCollisionAlgorithmCreateFunc(int proxyType0,int proxyType1);
+
+};
+
+#endif //BT_SOFTBODY_RIGIDBODY_COLLISION_CONFIGURATION
+
diff --git a/src/BulletDynamics/SoftBody/btSoftRigidCollisionAlgorithm.cpp b/src/BulletDynamics/SoftBody/btSoftRigidCollisionAlgorithm.cpp
new file mode 100644
index 000000000..c8c1b8ef3
--- /dev/null
+++ b/src/BulletDynamics/SoftBody/btSoftRigidCollisionAlgorithm.cpp
@@ -0,0 +1,96 @@
+/*
+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.
+*/
+
+#include "btSoftRigidCollisionAlgorithm.h"
+#include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h"
+#include "BulletCollision/CollisionShapes/btSphereShape.h"
+#include "BulletCollision/CollisionShapes/btBoxShape.h"
+#include "BulletCollision/CollisionDispatch/btCollisionObject.h"
+#include "BulletDynamics/SoftBody/btSoftBody.h"
+///TODO: include all the shapes that the softbody can collide with
+///alternatively, implement special case collision algorithms (just like for rigid collision shapes)
+
+//#include <stdio.h>
+
+btSoftRigidCollisionAlgorithm::btSoftRigidCollisionAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* col0,btCollisionObject* col1, bool isSwapped)
+: btCollisionAlgorithm(ci),
+//m_ownManifold(false),
+//m_manifoldPtr(mf),
+m_isSwapped(isSwapped)
+{
+
+	//m_softBody =  m_isSwapped? (btSoftBody*)col1 : (btSoftBody*)col0;
+	//m_rigidCollisionObject = m_isSwapped? col0 : col1;
+
+	//quick fix, add overlapping rigidbody to softbody, so it can be handled within btSoftBody::Step method
+	//m_softBody->m_overlappingRigidBodies.push_back(m_rigidCollisionObject);
+
+	///store the contacts straight into the btSoftBody for now?
+
+	/*if (!m_manifoldPtr && m_dispatcher->needsCollision(sphereObj,boxObj))
+	{
+		m_manifoldPtr = m_dispatcher->getNewManifold(sphereObj,boxObj);
+		m_ownManifold = true;
+	}
+	*/
+
+
+
+}
+
+
+btSoftRigidCollisionAlgorithm::~btSoftRigidCollisionAlgorithm()
+{
+	
+	//m_softBody->m_overlappingRigidBodies.remove(m_rigidCollisionObject);
+
+	/*if (m_ownManifold)
+	{
+		if (m_manifoldPtr)
+			m_dispatcher->releaseManifold(m_manifoldPtr);
+	}
+	*/
+
+}
+
+
+#include <stdio.h>
+
+void btSoftRigidCollisionAlgorithm::processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
+{
+	(void)dispatchInfo;
+	(void)resultOut;
+	//printf("btSoftRigidCollisionAlgorithm\n");
+
+	btSoftBody* softBody =  m_isSwapped? (btSoftBody*)body1 : (btSoftBody*)body0;
+	btCollisionObject* rigidCollisionObject = m_isSwapped? body0 : body1;
+	
+	softBody->defaultCollisionHandler(rigidCollisionObject);
+
+
+}
+
+btScalar btSoftRigidCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* col0,btCollisionObject* col1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
+{
+	(void)resultOut;
+	(void)dispatchInfo;
+	(void)col0;
+	(void)col1;
+
+	//not yet
+	return btScalar(1.);
+}
+
+
diff --git a/src/BulletDynamics/SoftBody/btSoftRigidCollisionAlgorithm.h b/src/BulletDynamics/SoftBody/btSoftRigidCollisionAlgorithm.h
new file mode 100644
index 000000000..351952ffe
--- /dev/null
+++ b/src/BulletDynamics/SoftBody/btSoftRigidCollisionAlgorithm.h
@@ -0,0 +1,70 @@
+/*
+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.
+*/
+
+#ifndef SOFT_RIGID_COLLISION_ALGORITHM_H
+#define SOFT_RIGID_COLLISION_ALGORITHM_H
+
+#include "BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h"
+#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h"
+#include "BulletCollision/CollisionDispatch/btCollisionCreateFunc.h"
+class btPersistentManifold;
+#include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h"
+
+#include "LinearMath/btVector3.h"
+class btSoftBody;
+
+/// btSoftRigidCollisionAlgorithm  provides collision detection between btSoftBody and btRigidBody
+class btSoftRigidCollisionAlgorithm : public btCollisionAlgorithm
+{
+//	bool	m_ownManifold;
+//	btPersistentManifold*	m_manifoldPtr;
+
+	btSoftBody*				m_softBody;
+	btCollisionObject*		m_rigidCollisionObject;
+
+	///for rigid versus soft (instead of soft versus rigid), we use this swapped boolean
+	bool	m_isSwapped;
+	
+public:
+
+	btSoftRigidCollisionAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* col0,btCollisionObject* col1, bool isSwapped);
+
+	virtual ~btSoftRigidCollisionAlgorithm();
+
+	virtual void processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut);
+
+	virtual btScalar calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut);
+
+	
+	struct CreateFunc :public 	btCollisionAlgorithmCreateFunc
+	{
+		virtual	btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, btCollisionObject* body0,btCollisionObject* body1)
+		{
+			void* mem = ci.m_dispatcher1->allocateCollisionAlgorithm(sizeof(btSoftRigidCollisionAlgorithm));
+			if (!m_swapped)
+			{
+				return new(mem) btSoftRigidCollisionAlgorithm(0,ci,body0,body1,false);
+			} else
+			{
+				return new(mem) btSoftRigidCollisionAlgorithm(0,ci,body0,body1,true);
+			}
+		}
+	};
+
+};
+
+#endif //SOFT_RIGID_COLLISION_ALGORITHM_H
+
+
diff --git a/src/BulletDynamics/SoftBody/btSoftRigidDynamicsWorld.cpp b/src/BulletDynamics/SoftBody/btSoftRigidDynamicsWorld.cpp
new file mode 100644
index 000000000..e4541b6fd
--- /dev/null
+++ b/src/BulletDynamics/SoftBody/btSoftRigidDynamicsWorld.cpp
@@ -0,0 +1,132 @@
+/*
+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.
+*/
+
+
+#include "btSoftRigidDynamicsWorld.h"
+#include "LinearMath/btQuickprof.h"
+
+//softbody & helpers
+#include "BulletDynamics/SoftBody/btSoftBody.h"
+#include "BulletDynamics/SoftBody/btSoftBodyHelpers.h"
+
+btSoftRigidDynamicsWorld::btSoftRigidDynamicsWorld(btDispatcher* dispatcher,btBroadphaseInterface* pairCache,btConstraintSolver* constraintSolver,btCollisionConfiguration* collisionConfiguration)
+:btDiscreteDynamicsWorld(dispatcher,pairCache,constraintSolver,collisionConfiguration)
+{
+
+}
+		
+btSoftRigidDynamicsWorld::~btSoftRigidDynamicsWorld()
+{
+
+}
+
+void	btSoftRigidDynamicsWorld::predictUnconstraintMotion(btScalar timeStep)
+{
+	btDiscreteDynamicsWorld::predictUnconstraintMotion( timeStep);
+
+	for ( int i=0;i<m_softBodies.size();++i)
+	{
+		btSoftBody*	psb= m_softBodies[i];
+
+		psb->predictMotion(timeStep);		
+	}
+}
+		
+void	btSoftRigidDynamicsWorld::internalSingleStepSimulation( btScalar timeStep)
+{
+
+	btDiscreteDynamicsWorld::internalSingleStepSimulation( timeStep );
+
+	///solve soft bodies constraints
+	solveSoftBodiesConstraints();
+
+	///update soft bodies
+	updateSoftBodies();
+
+}
+
+void	btSoftRigidDynamicsWorld::updateSoftBodies()
+{
+	BT_PROFILE("updateSoftBodies");
+	
+	for ( int i=0;i<m_softBodies.size();i++)
+	{
+		btSoftBody*	psb=(btSoftBody*)m_softBodies[i];
+		psb->integrateMotion();	
+	}
+}
+
+void	btSoftRigidDynamicsWorld::solveSoftBodiesConstraints()
+{
+	BT_PROFILE("solveSoftConstraints");
+		
+	for(int i=0;i<m_softBodies.size();++i)
+	{
+		btSoftBody*	psb=(btSoftBody*)m_softBodies[i];
+		psb->solveConstraints();
+	}	
+}
+
+void	btSoftRigidDynamicsWorld::addSoftBody(btSoftBody* body)
+{
+	m_softBodies.push_back(body);
+
+	btCollisionWorld::addCollisionObject(body,
+					btBroadphaseProxy::DefaultFilter,
+					btBroadphaseProxy::AllFilter);
+
+}
+
+void	btSoftRigidDynamicsWorld::removeSoftBody(btSoftBody* body)
+{
+	m_softBodies.remove(body);
+
+	btCollisionWorld::removeCollisionObject(body);
+}
+
+void	btSoftRigidDynamicsWorld::debugDrawWorld()
+{
+	btDiscreteDynamicsWorld::debugDrawWorld();
+
+	if (getDebugDrawer() && getDebugDrawer()->getDebugMode() & (btIDebugDraw::DBG_DrawWireframe | btIDebugDraw::DBG_DrawAabb))
+	{
+		int i;
+		for (  i=0;i<this->m_softBodies.size();i++)
+		{
+			btSoftBody*	psb=(btSoftBody*)this->m_softBodies[i];
+			btSoftBodyHelpers::DrawFrame(psb,m_debugDrawer);
+			btSoftBodyHelpers::Draw(psb,m_debugDrawer,fDrawFlags::Std+fDrawFlags::Nodes);
+			if (m_debugDrawer && (m_debugDrawer->getDebugMode() & btIDebugDraw::DBG_DrawAabb))
+			{
+				btSoftBodyHelpers::DrawNodeTree(psb,m_debugDrawer);
+				//btSoftBodyHelpers::DrawFaceTree(psb,m_debugDrawer);
+			}
+		}
+		
+		if (getDebugDrawer())
+		{
+			for (int  i=0;i<this->m_softBodies.size();i++)
+			{
+				btSoftBody*	psb=(btSoftBody*)this->m_softBodies[i];
+				btSoftBodyHelpers::DrawFrame(psb,m_debugDrawer);
+				btSoftBodyHelpers::Draw(psb,m_debugDrawer,fDrawFlags::Std);
+				if (m_debugDrawer && (m_debugDrawer->getDebugMode() & btIDebugDraw::DBG_DrawAabb))
+				{
+					btSoftBodyHelpers::DrawNodeTree(psb,m_debugDrawer);
+				}
+			}
+		}
+	}
+}
\ No newline at end of file
diff --git a/src/BulletDynamics/SoftBody/btSoftRigidDynamicsWorld.h b/src/BulletDynamics/SoftBody/btSoftRigidDynamicsWorld.h
new file mode 100644
index 000000000..8857b1741
--- /dev/null
+++ b/src/BulletDynamics/SoftBody/btSoftRigidDynamicsWorld.h
@@ -0,0 +1,65 @@
+/*
+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.
+*/
+
+#include "BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h"
+
+#ifndef BT_SOFT_RIGID_DYNAMICS_WORLD_H
+#define BT_SOFT_RIGID_DYNAMICS_WORLD_H
+
+class btSoftBody;
+typedef	btAlignedObjectArray<btSoftBody*> btSoftBodyArray;
+
+class btSoftRigidDynamicsWorld : public btDiscreteDynamicsWorld
+{
+	
+	btSoftBodyArray	m_softBodies;
+		
+protected:
+	
+	virtual void	predictUnconstraintMotion(btScalar timeStep);
+	
+	virtual void	internalSingleStepSimulation( btScalar timeStep);
+
+	void	updateSoftBodies();
+
+	void	solveSoftBodiesConstraints();
+
+	virtual void	debugDrawWorld();
+
+public:
+	
+	btSoftRigidDynamicsWorld(btDispatcher* dispatcher,btBroadphaseInterface* pairCache,btConstraintSolver* constraintSolver,btCollisionConfiguration* collisionConfiguration);
+
+	virtual ~btSoftRigidDynamicsWorld();
+		
+			
+	void	addSoftBody(btSoftBody* body);
+
+	void	removeSoftBody(btSoftBody* body);
+
+			
+	btSoftBodyArray& getSoftBodyArray()
+	{
+		return m_softBodies;
+	}
+
+	const btSoftBodyArray& getSoftBodyArray() const
+	{
+		return m_softBodies;
+	}
+		
+};
+
+#endif //BT_SOFT_RIGID_DYNAMICS_WORLD_H
diff --git a/src/BulletDynamics/SoftBody/btSoftSoftCollisionAlgorithm.cpp b/src/BulletDynamics/SoftBody/btSoftSoftCollisionAlgorithm.cpp
new file mode 100644
index 000000000..d03d82999
--- /dev/null
+++ b/src/BulletDynamics/SoftBody/btSoftSoftCollisionAlgorithm.cpp
@@ -0,0 +1,105 @@
+/*
+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.
+*/
+
+#include "btSoftSoftCollisionAlgorithm.h"
+#include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h"
+#include "BulletCollision/CollisionShapes/btBoxShape.h"
+#include "BulletCollision/CollisionDispatch/btCollisionObject.h"
+#include "BulletDynamics/SoftBody/btSoftBody.h"
+
+#define USE_PERSISTENT_CONTACTS 1
+
+btSoftSoftCollisionAlgorithm::btSoftSoftCollisionAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* obj0,btCollisionObject* obj1)
+: btCollisionAlgorithm(ci)
+//m_ownManifold(false),
+//m_manifoldPtr(mf)
+{
+	/*m_softBody0 = (btSoftBody*) obj0;
+	m_softBody1 = (btSoftBody*) obj1;
+
+	m_softBody0->m_overlappingSoftBodies.push_back(m_softBody1);
+	m_softBody1->m_overlappingSoftBodies.push_back(m_softBody0);*/
+
+	/*if (!m_manifoldPtr && m_dispatcher->needsCollision(obj0,obj1))
+	{
+		m_manifoldPtr = m_dispatcher->getNewManifold(obj0,obj1);
+		m_ownManifold = true;
+	}
+	*/
+
+}
+
+btSoftSoftCollisionAlgorithm::~btSoftSoftCollisionAlgorithm()
+{
+	//m_softBody0->m_overlappingSoftBodies.remove(m_softBody1);
+	//m_softBody1->m_overlappingSoftBodies.remove(m_softBody0);
+
+	//this gets called when the overlap stops.
+
+	//here is where contacts (manifolds) should be removed
+
+	/*
+	if (m_ownManifold)
+	{
+		if (m_manifoldPtr)
+			m_dispatcher->releaseManifold(m_manifoldPtr);
+	}
+	*/
+
+}
+
+void btSoftSoftCollisionAlgorithm::processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
+{
+	
+	btSoftBody* soft0 =	(btSoftBody*)body0;
+	btSoftBody* soft1 =	(btSoftBody*)body1;
+
+	soft0->defaultCollisionHandler(soft1);
+	/*
+	btBoxShape* box0 = (btBoxShape*)col0->getCollisionShape();
+	btBoxShape* box1 = (btBoxShape*)col1->getCollisionShape();
+
+
+
+	/// report a contact. internally this will be kept persistent, and contact reduction is done
+	resultOut->setPersistentManifold(m_manifoldPtr);
+#ifndef USE_PERSISTENT_CONTACTS	
+	m_manifoldPtr->clearManifold();
+#endif //USE_PERSISTENT_CONTACTS
+
+	btDiscreteCollisionDetectorInterface::ClosestPointInput input;
+	input.m_maximumDistanceSquared = 1e30f;
+	input.m_transformA = body0->getWorldTransform();
+	input.m_transformB = body1->getWorldTransform();
+
+	btBoxBoxDetector detector(box0,box1);
+	detector.getClosestPoints(input,*resultOut,dispatchInfo.m_debugDraw);
+
+#ifdef USE_PERSISTENT_CONTACTS
+	//  refreshContactPoints is only necessary when using persistent contact points. otherwise all points are newly added
+	if (m_ownManifold)
+	{
+		resultOut->refreshContactPoints();
+	}
+#endif //USE_PERSISTENT_CONTACTS
+*/
+
+}
+
+btScalar btSoftSoftCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
+{
+	//not yet
+	return 1.f;
+}
diff --git a/src/BulletDynamics/SoftBody/btSoftSoftCollisionAlgorithm.h b/src/BulletDynamics/SoftBody/btSoftSoftCollisionAlgorithm.h
new file mode 100644
index 000000000..b1627603d
--- /dev/null
+++ b/src/BulletDynamics/SoftBody/btSoftSoftCollisionAlgorithm.h
@@ -0,0 +1,63 @@
+/*
+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.
+*/
+
+#ifndef SOFT_SOFT_COLLISION_ALGORITHM_H
+#define SOFT_SOFT_COLLISION_ALGORITHM_H
+
+#include "BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h"
+#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h"
+#include "BulletCollision/BroadphaseCollision/btDispatcher.h"
+#include "BulletCollision/CollisionDispatch/btCollisionCreateFunc.h"
+
+class btPersistentManifold;
+class btSoftBody;
+
+///collision detection between two btSoftBody shapes
+class btSoftSoftCollisionAlgorithm : public btCollisionAlgorithm
+{
+	bool	m_ownManifold;
+	btPersistentManifold*	m_manifoldPtr;
+
+	btSoftBody*	m_softBody0;
+	btSoftBody*	m_softBody1;
+
+	
+public:
+	btSoftSoftCollisionAlgorithm(const btCollisionAlgorithmConstructionInfo& ci)
+		: btCollisionAlgorithm(ci) {}
+
+	virtual void processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut);
+
+	virtual btScalar calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut);
+
+	btSoftSoftCollisionAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* body0,btCollisionObject* body1);
+
+	virtual ~btSoftSoftCollisionAlgorithm();
+
+	struct CreateFunc :public 	btCollisionAlgorithmCreateFunc
+	{
+		virtual	btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, btCollisionObject* body0,btCollisionObject* body1)
+		{
+			int bbsize = sizeof(btSoftSoftCollisionAlgorithm);
+			void* ptr = ci.m_dispatcher1->allocateCollisionAlgorithm(bbsize);
+			return new(ptr) btSoftSoftCollisionAlgorithm(0,ci,body0,body1);
+		}
+	};
+
+};
+
+#endif //SOFT_SOFT_COLLISION_ALGORITHM_H
+
+
diff --git a/src/BulletDynamics/SoftBody/btSparseSDF.h b/src/BulletDynamics/SoftBody/btSparseSDF.h
index ef2d35f2c..4f7c9656d 100644
--- a/src/BulletDynamics/SoftBody/btSparseSDF.h
+++ b/src/BulletDynamics/SoftBody/btSparseSDF.h
@@ -32,21 +32,13 @@ struct	btSparseSdf
 		int					i;
 		btScalar			f;
 		};
-	struct	Client
-		{
-		btCollisionShape*	shape;
-		btVector3			center;
-		btVector3			extent;
-		btScalar			vsize;
-		int					id;
-		};
 	struct	Cell
 		{
 		btScalar			d[CELLSIZE+1][CELLSIZE+1][CELLSIZE+1];
 		int					c[3];
 		int					puid;
 		unsigned			hash;
-		const Client*		pclient;
+		btCollisionShape*	pclient;
 		Cell*				next;
 		};
 	//
@@ -54,11 +46,11 @@ struct	btSparseSdf
 	//
 	
 	btAlignedObjectArray<Cell*>		cells;	
-	btAlignedObjectArray<Client*>	clients;
+	btScalar						voxelsz;
 	int								puid;
 	int								ncells;
 	int								nprobes;
-	int								nqueries;
+	int								nqueries;	
 	
 	//
 	// Methods
@@ -84,13 +76,14 @@ struct	btSparseSdf
 				pc=pn;
 				}
 			}
+		voxelsz		=0.25;
 		puid		=0;
 		ncells		=0;
 		nprobes		=1;
 		nqueries	=1;
 		}
 	//
-	void					GarbageCollect(int lifetime=64)
+	void					GarbageCollect(int lifetime=256)
 		{
 		const int life=puid-lifetime;
 		for(int i=0;i<cells.size();++i)
@@ -116,39 +109,17 @@ struct	btSparseSdf
 				/* else setup a priority list...						*/ 
 		}
 	//
-	Client*					GetClient(	btCollisionShape* shape)
-		{
-		Client*	pc=(Client*)shape->getUserPointer();
-		if(!pc)
-			{
-			pc=new Client();
-			clients.push_back(pc);
-			shape->setUserPointer(pc);
-			pc->shape	=	shape;
-			pc->id		=	clients.size();
-			pc->vsize	=	0.25;
-			SetShapeBounds(*pc);
-			}
-		return(pc);
-		}
-	//
 	btScalar				Evaluate(	const btVector3& x,
 										btCollisionShape* shape,
-										btVector3& normal)
+										btVector3& normal,
+										btScalar margin)
 		{
-		const Client*	pclient=GetClient(shape);
-		/* Bounds check			*/ 
-		const btVector3	offset=x-pclient->center;
-		const btVector3	sqoffset=offset*offset;
-		if(	(sqoffset.x()>pclient->extent.x())	||
-			(sqoffset.y()>pclient->extent.y())	||
-			(sqoffset.z()>pclient->extent.z()))	return(SIMD_INFINITY);
 		/* Lookup cell			*/ 
-		const btVector3	scx=x/pclient->vsize;
+		const btVector3	scx=x/voxelsz;
 		const IntFrac	ix=Decompose(scx.x());
 		const IntFrac	iy=Decompose(scx.y());
 		const IntFrac	iz=Decompose(scx.z());
-		const unsigned	h=Hash(ix.b,iy.b,iz.b,pclient->id);
+		const unsigned	h=Hash(ix.b,iy.b,iz.b,shape);
 		Cell*&			root=cells[h%cells.size()];
 		Cell*			c=root;
 		++nqueries;
@@ -159,7 +130,7 @@ struct	btSparseSdf
 				(c->c[0]==ix.b)	&&
 				(c->c[1]==iy.b)	&&
 				(c->c[2]==iz.b)	&&
-				(c->pclient==pclient))
+				(c->pclient==shape))
 				{ break; }
 				else
 				{ c=c->next; }
@@ -170,7 +141,8 @@ struct	btSparseSdf
 			++ncells;
 			c=new Cell();
 			c->next=root;root=c;
-			c->pclient=pclient;c->hash=h;
+			c->pclient=shape;
+			c->hash=h;
 			c->c[0]=ix.b;c->c[1]=iy.b;c->c[2]=iz.b;
 			BuildCell(*c);
 			}
@@ -208,33 +180,33 @@ struct	btSparseSdf
 								Lerp(d[3],d[2],ix.f),iy.f);
 		const btScalar	d1=Lerp(Lerp(d[4],d[5],ix.f),
 								Lerp(d[7],d[6],ix.f),iy.f);
-		return(Lerp(d0,d1,iz.f));
+		return(Lerp(d0,d1,iz.f)-margin);
 		}
 	//
 	void					BuildCell(Cell& c)
 		{
-		const Client*	client=c.pclient;
-		const btVector3	org=btVector3(c.c[0],c.c[1],c.c[2])*CELLSIZE*client->vsize;
+		const btVector3	org=btVector3(	(btScalar)c.c[0],
+										(btScalar)c.c[1],
+										(btScalar)c.c[2])	*
+										CELLSIZE*voxelsz;
 		for(int k=0;k<=CELLSIZE;++k)
 			{
-			const btScalar	z=client->vsize*k+org.z();
+			const btScalar	z=voxelsz*k+org.z();
 			for(int j=0;j<=CELLSIZE;++j)
 				{
-				const btScalar	y=client->vsize*j+org.y();
+				const btScalar	y=voxelsz*j+org.y();
 				for(int i=0;i<=CELLSIZE;++i)
 					{
-					const btScalar	x=client->vsize*i+org.x();
+					const btScalar	x=voxelsz*i+org.x();
 					c.d[i][j][k]=DistanceToShape(	btVector3(x,y,z),
-													client->shape,
-													client->vsize);
+													c.pclient);
 					}
 				}
 			}
 		}
 	//
 	static inline btScalar	DistanceToShape(const btVector3& x,
-											btCollisionShape* shape,
-											btScalar margin)
+											btCollisionShape* shape)
 		{
 		btTransform	unit;
 		unit.setIdentity();
@@ -242,33 +214,11 @@ struct	btSparseSdf
 			{
 			btGjkEpaSolver2::sResults	res;
 			btConvexShape*				csh=static_cast<btConvexShape*>(shape);
-			return(btGjkEpaSolver2::SignedDistance(x,margin,csh,unit,res));
+			return(btGjkEpaSolver2::SignedDistance(x,0,csh,unit,res));
 			}
 		return(0);
 		}
 	//
-	static inline void		SetShapeBounds(Client& c)
-		{
-		if(c.shape->isConvex())
-			{
-			btConvexShape*	csh=static_cast<btConvexShape*>(c.shape);
-			const btVector3	x[]={	csh->localGetSupportingVertex(btVector3(+1,0,0)),
-									csh->localGetSupportingVertex(btVector3(-1,0,0))};
-			const btVector3	y[]={	csh->localGetSupportingVertex(btVector3(0,+1,0)),
-									csh->localGetSupportingVertex(btVector3(0,-1,0))};
-			const btVector3	z[]={	csh->localGetSupportingVertex(btVector3(0,0,+1)),
-									csh->localGetSupportingVertex(btVector3(0,0,-1))};
-			c.center	=	btVector3(	x[0].x()+x[1].x(),
-										y[0].y()+y[1].y(),
-										z[0].z()+z[1].z())*0.5;
-			c.extent	=	btVector3(	x[0].x()-x[1].x(),
-										y[0].y()-y[1].y(),
-										z[0].z()-z[1].z())*0.5;
-			c.extent	+=	btVector3(c.vsize,c.vsize,c.vsize);
-			c.extent	*=	c.extent;
-			}
-		}
-	//
 	static inline IntFrac	Decompose(btScalar x)
 		{
 		/* That one need a lot of improvements...	*/
@@ -287,10 +237,11 @@ struct	btSparseSdf
 		return(a+(b-a)*t);
 		}
 	//
-	static inline unsigned	Hash(int x,int y,int z,int i)
+	static inline unsigned	Hash(int x,int y,int z,btCollisionShape* shape)
 		{
-		const int data[]={x,y,z,i};
-		return(HsiehHash<sizeof(data)/4>(data));
+		struct { int x,y,z;void* p; } set;
+		set.x=x;set.y=y;set.z=z;set.p=shape;
+		return(HsiehHash<sizeof(set)/4>(&set));
 		}
 	// Modified Paul Hsieh hash
 	template <const int DWORDLEN>
diff --git a/Extras/ConvexHull/btConvexHull.cpp b/src/LinearMath/btConvexHull.cpp
similarity index 100%
rename from Extras/ConvexHull/btConvexHull.cpp
rename to src/LinearMath/btConvexHull.cpp
diff --git a/Extras/ConvexHull/btConvexHull.h b/src/LinearMath/btConvexHull.h
similarity index 100%
rename from Extras/ConvexHull/btConvexHull.h
rename to src/LinearMath/btConvexHull.h