Applied some fixes from Bullet: issues with btDbvtBroadphase, and btSoftBody, and better fix for 64-bit issue in btCompoundCollisionAlgorithm.

6 files changed, 285 insertions, 60 deletions

diff --git a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btDbvt.h b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btDbvt.h
index da296445e81..21d69acf151 100644
--- a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btDbvt.h
+++ b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btDbvt.h
@@ -188,6 +188,7 @@ struct	btDbvtNode
 	union	{
 			btDbvtNode*	childs[2];
 			void*	data;
+			int		dataAsInt;
 			};
 };
 
diff --git a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btDbvtBroadphase.cpp b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btDbvtBroadphase.cpp
index c6086f28f19..fd82fd7cae3 100644
--- a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btDbvtBroadphase.cpp
+++ b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btDbvtBroadphase.cpp
@@ -20,17 +20,18 @@ subject to the following restrictions:
 // Profiling
 //
 
-#if DBVT_BP_PROFILE
+#if DBVT_BP_PROFILE||DBVT_BP_ENABLE_BENCHMARK
 #include <stdio.h>
+#endif
+
+#if DBVT_BP_PROFILE
 struct	ProfileScope
 	{
-	ProfileScope(btClock& clock,unsigned long& value)
+	__forceinline ProfileScope(btClock& clock,unsigned long& value) :
+		m_clock(&clock),m_value(&value),m_base(clock.getTimeMicroseconds())
 		{
-		m_clock=&clock;
-		m_value=&value;
-		m_base=clock.getTimeMicroseconds();
 		}
-	~ProfileScope()
+	__forceinline ~ProfileScope()
 		{
 		(*m_value)+=m_clock->getTimeMicroseconds()-m_base;
 		}
@@ -90,19 +91,25 @@ value=zerodummy;
 struct	btDbvtTreeCollider : btDbvt::ICollide
 {
 btDbvtBroadphase*	pbp;
+btDbvtProxy*		proxy;
 		btDbvtTreeCollider(btDbvtBroadphase* p) : pbp(p) {}
 void	Process(const btDbvtNode* na,const btDbvtNode* nb)
 	{
-	btDbvtProxy*	pa=(btDbvtProxy*)na->data;
-	btDbvtProxy*	pb=(btDbvtProxy*)nb->data;
-	#if DBVT_BP_DISCRETPAIRS
-	if(Intersect(pa->aabb,pb->aabb))
-	#endif
+	if(na!=nb)
 		{
+		btDbvtProxy*	pa=(btDbvtProxy*)na->data;
+		btDbvtProxy*	pb=(btDbvtProxy*)nb->data;
+		#if DBVT_BP_SORTPAIRS
 		if(pa>pb) btSwap(pa,pb);
+		#endif
 		pbp->m_paircache->addOverlappingPair(pa,pb);
+		++pbp->m_newpairs;
 		}
 	}
+void	Process(const btDbvtNode* n)
+	{
+	Process(n,proxy->leaf);
+	}
 };
 
 //
@@ -112,16 +119,25 @@ void	Process(const btDbvtNode* na,const btDbvtNode* nb)
 //
 btDbvtBroadphase::btDbvtBroadphase(btOverlappingPairCache* paircache)
 {
+m_deferedcollide	=	false;
+m_needcleanup		=	true;
 m_releasepaircache	=	(paircache!=0)?false:true;
-m_predictedframes	=	2;
+m_prediction		=	1/(btScalar)2;
 m_stageCurrent		=	0;
+m_fixedleft			=	0;
 m_fupdates			=	1;
-m_dupdates			=	1;
+m_dupdates			=	0;
+m_cupdates			=	10;
+m_newpairs			=	1;
+m_updates_call		=	0;
+m_updates_done		=	0;
+m_updates_ratio		=	0;
 m_paircache			=	paircache?
-							paircache	:
-							new(btAlignedAlloc(sizeof(btHashedOverlappingPairCache),16)) btHashedOverlappingPairCache();
+						paircache	:
+						new(btAlignedAlloc(sizeof(btHashedOverlappingPairCache),16)) btHashedOverlappingPairCache();
 m_gid				=	0;
 m_pid				=	0;
+m_cid				=	0;
 for(int i=0;i<=STAGECOUNT;++i)
 	{
 	m_stageRoots[i]=0;
@@ -148,17 +164,23 @@ btBroadphaseProxy*				btDbvtBroadphase::createProxy(	const btVector3& aabbMin,
 																void* userPtr,
 																short int collisionFilterGroup,
 																short int collisionFilterMask,
-																btDispatcher* /*dispatcher*/,
+																btDispatcher* dispatcher,
 																void* /*multiSapProxy*/)
 {
-btDbvtProxy*	proxy=new(btAlignedAlloc(sizeof(btDbvtProxy),16)) btDbvtProxy(	userPtr,
-																				collisionFilterGroup,
-																				collisionFilterMask);
+btDbvtProxy*		proxy=new(btAlignedAlloc(sizeof(btDbvtProxy),16)) btDbvtProxy(	userPtr,
+																					collisionFilterGroup,
+																					collisionFilterMask);
 proxy->aabb			=	btDbvtVolume::FromMM(aabbMin,aabbMax);
-proxy->leaf			=	m_sets[0].insert(proxy->aabb,proxy);
 proxy->stage		=	m_stageCurrent;
 proxy->m_uniqueId	=	++m_gid;
+proxy->leaf			=	m_sets[0].insert(proxy->aabb,proxy);
 listappend(proxy,m_stageRoots[m_stageCurrent]);
+if(!m_deferedcollide)
+	{
+	btDbvtTreeCollider	collider(this);
+	collider.proxy=proxy;
+	btDbvt::collideTV(m_sets[0].m_root,proxy->aabb,collider);
+	}
 return(proxy);
 }
 
@@ -174,6 +196,7 @@ if(proxy->stage==STAGECOUNT)
 listremove(proxy,m_stageRoots[proxy->stage]);
 m_paircache->removeOverlappingPairsContainingProxy(proxy,dispatcher);
 btAlignedFree(proxy);
+m_needcleanup=true;
 }
 
 //
@@ -182,35 +205,62 @@ void							btDbvtBroadphase::setAabb(		btBroadphaseProxy* absproxy,
 																const btVector3& aabbMax,
 																btDispatcher* /*dispatcher*/)
 {
-btDbvtProxy*	proxy=(btDbvtProxy*)absproxy;
-btDbvtVolume	aabb=btDbvtVolume::FromMM(aabbMin,aabbMax);
+btDbvtProxy*						proxy=(btDbvtProxy*)absproxy;
+ATTRIBUTE_ALIGNED16(btDbvtVolume)	aabb=btDbvtVolume::FromMM(aabbMin,aabbMax);
+#if DBVT_BP_PREVENTFALSEUPDATE
 if(NotEqual(aabb,proxy->leaf->volume))
+#endif
 	{
+	bool	docollide=false;
 	if(proxy->stage==STAGECOUNT)
 		{/* fixed -> dynamic set	*/ 
 		m_sets[1].remove(proxy->leaf);
 		proxy->leaf=m_sets[0].insert(aabb,proxy);
+		docollide=true;
 		}
 		else
 		{/* dynamic set				*/ 
+		++m_updates_call;
 		if(Intersect(proxy->leaf->volume,aabb))
 			{/* Moving				*/ 
-			const btVector3	delta=(aabbMin+aabbMax)/2-proxy->aabb.Center();
-			#ifdef DBVT_BP_MARGIN
-			m_sets[0].update(proxy->leaf,aabb,delta*m_predictedframes,DBVT_BP_MARGIN);
-			#else
-			m_sets[0].update(proxy->leaf,aabb,delta*m_predictedframes);
-			#endif
+			const btVector3	delta=aabbMin-proxy->aabb.Mins();
+			btVector3		velocity(aabb.Extents()*m_prediction);
+			if(delta[0]<0) velocity[0]=-velocity[0];
+			if(delta[1]<0) velocity[1]=-velocity[1];
+			if(delta[2]<0) velocity[2]=-velocity[2];
+			if	(
+				#ifdef DBVT_BP_MARGIN				
+				m_sets[0].update(proxy->leaf,aabb,velocity,DBVT_BP_MARGIN)
+				#else
+				m_sets[0].update(proxy->leaf,aabb,velocity)
+				#endif
+				)
+				{
+				++m_updates_done;
+				docollide=true;
+				}
 			}
 			else
 			{/* Teleporting			*/ 
-			m_sets[0].update(proxy->leaf,aabb);		
+			m_sets[0].update(proxy->leaf,aabb);
+			++m_updates_done;
+			docollide=true;
 			}	
 		}
 	listremove(proxy,m_stageRoots[proxy->stage]);
 	proxy->aabb		=	aabb;
 	proxy->stage	=	m_stageCurrent;
 	listappend(proxy,m_stageRoots[m_stageCurrent]);
+	if(docollide)
+		{
+		m_needcleanup=true;
+		if(!m_deferedcollide)
+			{
+			btDbvtTreeCollider	collider(this);
+			btDbvt::collideTT(m_sets[1].m_root,proxy->leaf,collider);
+			btDbvt::collideTT(m_sets[0].m_root,proxy->leaf,collider);
+			}
+		}	
 	}
 }
 
@@ -245,7 +295,12 @@ void							btDbvtBroadphase::collide(btDispatcher* dispatcher)
 SPC(m_profiling.m_total);
 /* optimize				*/ 
 m_sets[0].optimizeIncremental(1+(m_sets[0].m_leaves*m_dupdates)/100);
-m_sets[1].optimizeIncremental(1+(m_sets[1].m_leaves*m_fupdates)/100);
+if(m_fixedleft)
+	{
+	const int count=1+(m_sets[1].m_leaves*m_fupdates)/100;
+	m_sets[1].optimizeIncremental(1+(m_sets[1].m_leaves*m_fupdates)/100);
+	m_fixedleft=btMax<int>(0,m_fixedleft-count);
+	}
 /* dynamic -> fixed set	*/ 
 m_stageCurrent=(m_stageCurrent+1)%STAGECOUNT;
 btDbvtProxy*	current=m_stageRoots[m_stageCurrent];
@@ -256,46 +311,69 @@ if(current)
 		btDbvtProxy*	next=current->links[1];
 		listremove(current,m_stageRoots[current->stage]);
 		listappend(current,m_stageRoots[STAGECOUNT]);
-		btDbvt::collideTT(m_sets[1].m_root,current->leaf,collider);
+		#if DBVT_BP_ACCURATESLEEPING
+		m_paircache->removeOverlappingPairsContainingProxy(current,dispatcher);
+		collider.proxy=current;
+		btDbvt::collideTV(m_sets[0].m_root,current->aabb,collider);
+		btDbvt::collideTV(m_sets[1].m_root,current->aabb,collider);
+		#endif
 		m_sets[0].remove(current->leaf);
 		current->leaf	=	m_sets[1].insert(current->aabb,current);
 		current->stage	=	STAGECOUNT;	
 		current			=	next;
 		} while(current);
+	m_fixedleft=m_sets[1].m_leaves;
+	m_needcleanup=true;
 	}
 /* collide dynamics		*/ 
 	{
 	btDbvtTreeCollider	collider(this);
+	if(m_deferedcollide)
 		{
 		SPC(m_profiling.m_fdcollide);
 		btDbvt::collideTT(m_sets[0].m_root,m_sets[1].m_root,collider);
 		}
+	if(m_deferedcollide)
 		{
 		SPC(m_profiling.m_ddcollide);
 		btDbvt::collideTT(m_sets[0].m_root,m_sets[0].m_root,collider);
 		}
 	}
 /* clean up				*/ 
+if(m_needcleanup)
 	{
 	SPC(m_profiling.m_cleanup);
 	btBroadphasePairArray&	pairs=m_paircache->getOverlappingPairArray();
 	if(pairs.size()>0)
 		{
-		for(int i=0,ni=pairs.size();i<ni;++i)
+		const int	ci=pairs.size();
+		int			ni=btMin(ci,btMax<int>(m_newpairs,(ci*m_cupdates)/100));
+		for(int i=0;i<ni;++i)
 			{
-			btBroadphasePair&	p=pairs[i];
-			btDbvtProxy*	pa=(btDbvtProxy*)p.m_pProxy0;
-			btDbvtProxy*	pb=(btDbvtProxy*)p.m_pProxy1;
-			if(!Intersect(pa->aabb,pb->aabb))
+			btBroadphasePair&	p=pairs[(m_cid+i)%ci];
+			btDbvtProxy*		pa=(btDbvtProxy*)p.m_pProxy0;
+			btDbvtProxy*		pb=(btDbvtProxy*)p.m_pProxy1;
+			if(!Intersect(pa->leaf->volume,pb->leaf->volume))
 				{
+				#if DBVT_BP_SORTPAIRS
 				if(pa>pb) btSwap(pa,pb);
+				#endif
 				m_paircache->removeOverlappingPair(pa,pb,dispatcher);
 				--ni;--i;
 				}
 			}
+		if(pairs.size()>0) m_cid=(m_cid+ni)%pairs.size(); else m_cid=0;
 		}
 	}
 ++m_pid;
+m_newpairs=1;
+m_needcleanup=false;
+if(m_updates_call>0)
+	{ m_updates_ratio=m_updates_done/(btScalar)m_updates_call; }
+	else
+	{ m_updates_ratio=0; }
+m_updates_done/=2;
+m_updates_call/=2;
 }
 
 //
@@ -339,6 +417,131 @@ aabbMax=bounds.Maxs();
 void							btDbvtBroadphase::printStats()
 {}
 
+//
+#if DBVT_BP_ENABLE_BENCHMARK
+
+struct	btBroadphaseBenchmark
+	{
+	struct	Experiment
+		{
+		const char*			name;
+		int					object_count;
+		int					update_count;
+		int					spawn_count;
+		int					iterations;
+		btScalar			speed;
+		btScalar			amplitude;
+		};
+	struct	Object
+		{
+		btVector3			center;
+		btVector3			extents;
+		btBroadphaseProxy*	proxy;
+		btScalar			time;
+		void				update(btScalar speed,btScalar amplitude,btBroadphaseInterface* pbi)
+			{
+			time		+=	speed;
+			center[0]	=	btCos(time*(btScalar)2.17)*amplitude+
+							btSin(time)*amplitude/2;
+			center[1]	=	btCos(time*(btScalar)1.38)*amplitude+
+							btSin(time)*amplitude;
+			center[2]	=	btSin(time*(btScalar)0.777)*amplitude;
+			pbi->setAabb(proxy,center-extents,center+extents,0);
+			}
+		};
+	static int		UnsignedRand(int range=RAND_MAX-1)	{ return(rand()%(range+1)); }
+	static btScalar	UnitRand()							{ return(UnsignedRand(16384)/(btScalar)16384); }
+	static void		OutputTime(const char* name,btClock& c,unsigned count=0)
+		{
+		const unsigned long	us=c.getTimeMicroseconds();
+		const unsigned long	ms=(us+500)/1000;
+		const btScalar		sec=us/(btScalar)(1000*1000);
+		if(count>0)
+			printf("%s : %u us (%u ms), %.2f/s\r\n",name,us,ms,count/sec);
+			else
+			printf("%s : %u us (%u ms)\r\n",name,us,ms);
+		}
+	};
+
+void							btDbvtBroadphase::benchmark(btBroadphaseInterface* pbi)
+{
+static const btBroadphaseBenchmark::Experiment		experiments[]=
+	{
+	{"1024o.10%",1024,10,0,8192,(btScalar)0.005,(btScalar)100},
+	/*{"4096o.10%",4096,10,0,8192,(btScalar)0.005,(btScalar)100},
+	{"8192o.10%",8192,10,0,8192,(btScalar)0.005,(btScalar)100},*/
+	};
+static const int										nexperiments=sizeof(experiments)/sizeof(experiments[0]);
+btAlignedObjectArray<btBroadphaseBenchmark::Object*>	objects;
+btClock													wallclock;
+/* Begin			*/ 
+for(int iexp=0;iexp<nexperiments;++iexp)
+	{
+	const btBroadphaseBenchmark::Experiment&	experiment=experiments[iexp];
+	const int									object_count=experiment.object_count;
+	const int									update_count=(object_count*experiment.update_count)/100;
+	const int									spawn_count=(object_count*experiment.spawn_count)/100;
+	const btScalar								speed=experiment.speed;	
+	const btScalar								amplitude=experiment.amplitude;
+	printf("Experiment #%u '%s':\r\n",iexp,experiment.name);
+	printf("\tObjects: %u\r\n",object_count);
+	printf("\tUpdate: %u\r\n",update_count);
+	printf("\tSpawn: %u\r\n",spawn_count);
+	printf("\tSpeed: %f\r\n",speed);
+	printf("\tAmplitude: %f\r\n",amplitude);
+	srand(180673);
+	/* Create objects	*/ 
+	wallclock.reset();
+	objects.reserve(object_count);
+	for(int i=0;i<object_count;++i)
+		{
+		btBroadphaseBenchmark::Object*	po=new btBroadphaseBenchmark::Object();
+		po->center[0]=btBroadphaseBenchmark::UnitRand()*50;
+		po->center[1]=btBroadphaseBenchmark::UnitRand()*50;
+		po->center[2]=btBroadphaseBenchmark::UnitRand()*50;
+		po->extents[0]=btBroadphaseBenchmark::UnitRand()*2+2;
+		po->extents[1]=btBroadphaseBenchmark::UnitRand()*2+2;
+		po->extents[2]=btBroadphaseBenchmark::UnitRand()*2+2;
+		po->time=btBroadphaseBenchmark::UnitRand()*2000;
+		po->proxy=pbi->createProxy(po->center-po->extents,po->center+po->extents,0,po,1,1,0,0);
+		objects.push_back(po);
+		}
+	btBroadphaseBenchmark::OutputTime("\tInitialization",wallclock);
+	/* First update		*/ 
+	wallclock.reset();
+	for(int i=0;i<objects.size();++i)
+		{
+		objects[i]->update(speed,amplitude,pbi);
+		}
+	btBroadphaseBenchmark::OutputTime("\tFirst update",wallclock);
+	/* Updates			*/ 
+	wallclock.reset();
+	for(int i=0;i<experiment.iterations;++i)
+		{
+		for(int j=0;j<update_count;++j)
+			{				
+			objects[j]->update(speed,amplitude,pbi);
+			}
+		pbi->calculateOverlappingPairs(0);
+		}
+	btBroadphaseBenchmark::OutputTime("\tUpdate",wallclock,experiment.iterations);
+	/* Clean up			*/ 
+	wallclock.reset();
+	for(int i=0;i<objects.size();++i)
+		{
+		pbi->destroyProxy(objects[i]->proxy,0);
+		delete objects[i];
+		}
+	objects.resize(0);
+	btBroadphaseBenchmark::OutputTime("\tRelease",wallclock);
+	}
+
+}
+#else
+void							btDbvtBroadphase::benchmark(btBroadphaseInterface*)
+{}
+#endif
+
 #if DBVT_BP_PROFILE
 #undef	SPC
 #endif
diff --git a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btDbvtBroadphase.h b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btDbvtBroadphase.h
index 3f19075552b..4576e48f3f4 100644
--- a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btDbvtBroadphase.h
+++ b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btDbvtBroadphase.h
@@ -23,9 +23,12 @@ subject to the following restrictions:
 // Compile time config
 //
 
-#define	DBVT_BP_PROFILE			0
-#define DBVT_BP_DISCRETPAIRS	1
-#define DBVT_BP_MARGIN			(btScalar)0.05
+#define	DBVT_BP_PROFILE					0
+#define DBVT_BP_SORTPAIRS				1
+#define DBVT_BP_PREVENTFALSEUPDATE		0
+#define DBVT_BP_ACCURATESLEEPING		0
+#define DBVT_BP_ENABLE_BENCHMARK		0
+#define DBVT_BP_MARGIN					(btScalar)0.05
 
 #if DBVT_BP_PROFILE
 	#define	DBVT_BP_PROFILING_RATE	256
@@ -38,10 +41,10 @@ subject to the following restrictions:
 struct btDbvtProxy : btBroadphaseProxy
 {
 /* Fields		*/ 
-btDbvtAabbMm		aabb;
+btDbvtAabbMm	aabb;
 btDbvtNode*		leaf;
-btDbvtProxy*		links[2];
-int					stage;
+btDbvtProxy*	links[2];
+int				stage;
 /* ctor			*/ 
 btDbvtProxy(void* userPtr,short int collisionFilterGroup, short int collisionFilterMask) :
 	btBroadphaseProxy(userPtr,collisionFilterGroup,collisionFilterMask)
@@ -67,13 +70,23 @@ enum	{
 btDbvt					m_sets[2];					// Dbvt sets
 btDbvtProxy*			m_stageRoots[STAGECOUNT+1];	// Stages list
 btOverlappingPairCache*	m_paircache;				// Pair cache
-btScalar				m_predictedframes;			// Frames predicted
+btScalar				m_prediction;				// Velocity prediction
 int						m_stageCurrent;				// Current stage
 int						m_fupdates;					// % of fixed updates per frame
 int						m_dupdates;					// % of dynamic updates per frame
+int						m_cupdates;					// % of cleanup updates per frame
+int						m_newpairs;					// Number of pairs created
+int						m_fixedleft;				// Fixed optimization left
+unsigned				m_updates_call;				// Number of updates call
+unsigned				m_updates_done;				// Number of updates done
+btScalar				m_updates_ratio;			// m_updates_done/m_updates_call
 int						m_pid;						// Parse id
+int						m_cid;						// Cleanup index
 int						m_gid;						// Gen id
 bool					m_releasepaircache;			// Release pair cache on delete
+bool					m_deferedcollide;			// Defere dynamic/static collision to collide call
+bool					m_needcleanup;				// Need to run cleanup?
+bool					m_initialize;				// Initialization
 #if DBVT_BP_PROFILE
 btClock					m_clock;
 struct	{
@@ -98,6 +111,7 @@ btOverlappingPairCache*			getOverlappingPairCache();
 const btOverlappingPairCache*	getOverlappingPairCache() const;
 void							getBroadphaseAabb(btVector3& aabbMin,btVector3& aabbMax) const;
 void							printStats();
+static void						benchmark(btBroadphaseInterface*);
 };
 
 #endif
diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.cpp b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.cpp
index 57812818007..535b61992b0 100644
--- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.cpp
+++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.cpp
@@ -141,7 +141,7 @@ public:
 	}
 	void		Process(const btDbvtNode* leaf)
 	{
-		int index = int(long(leaf->data));
+		int index = leaf->dataAsInt;
 
 		btCompoundShape* compoundShape = static_cast<btCompoundShape*>(m_compoundColObj->getCollisionShape());
 		btCollisionShape* childShape = compoundShape->getChildShape(index);
diff --git a/extern/bullet2/src/BulletSoftBody/btSoftBody.cpp b/extern/bullet2/src/BulletSoftBody/btSoftBody.cpp
index 2553009fec2..91b9d6f1bbd 100644
--- a/extern/bullet2/src/BulletSoftBody/btSoftBody.cpp
+++ b/extern/bullet2/src/BulletSoftBody/btSoftBody.cpp
@@ -97,8 +97,7 @@ btSoftBody::~btSoftBody()
 	delete m_collisionShape;	
 	int i;
 
-	for(i=0;i<m_clusters.size();++i)
-		btAlignedFree(m_clusters[i]);
+	releaseClusters();
 	for(i=0;i<m_materials.size();++i) 
 		btAlignedFree(m_materials[i]);
 	for(i=0;i<m_joints.size();++i) 
@@ -749,19 +748,27 @@ int i,ni;
 }
 
 //
-int				btSoftBody::generateClusters(int k,int maxiterations)
+void			btSoftBody::releaseCluster(int index)
 {
-	int i;
-
-for(i=0;i<m_clusters.size();++i)
-	{
-	if(m_clusters[i]->m_leaf) m_cdbvt.remove(m_clusters[i]->m_leaf);
-	btAlignedFree(m_clusters[i]);
-	}
-m_clusters.resize(btMin(k,m_nodes.size()));
-
+Cluster*	c=m_clusters[index];
+if(c->m_leaf) m_cdbvt.remove(c->m_leaf);
+c->~Cluster();
+btAlignedFree(c);
+m_clusters.remove(c);
+}
 
+//
+void			btSoftBody::releaseClusters()
+{
+while(m_clusters.size()>0) releaseCluster(0);
+}
 
+//
+int				btSoftBody::generateClusters(int k,int maxiterations)
+{
+int i;
+releaseClusters();
+m_clusters.resize(btMin(k,m_nodes.size()));
 for(i=0;i<m_clusters.size();++i)
 	{
 	m_clusters[i]			=	new(btAlignedAlloc(sizeof(Cluster),16)) Cluster();
@@ -870,10 +877,7 @@ if(k>0)
 		{
 		if(m_clusters[i]->m_nodes.size()==0)
 			{
-			btAlignedFree(m_clusters[i]);
-			btSwap(m_clusters[i],m_clusters[m_clusters.size()-1]);
-			m_clusters.pop_back();
-			--i;
+			releaseCluster(i--);
 			}
 		}
 		
diff --git a/extern/bullet2/src/BulletSoftBody/btSoftBody.h b/extern/bullet2/src/BulletSoftBody/btSoftBody.h
index 834199c668a..91d732b4e9a 100644
--- a/extern/bullet2/src/BulletSoftBody/btSoftBody.h
+++ b/extern/bullet2/src/BulletSoftBody/btSoftBody.h
@@ -717,6 +717,9 @@ public:
 													Material* mat=0);
 	/* Randomize constraints to reduce solver bias							*/ 
 	void				randomizeConstraints();
+	/* Release clusters														*/ 
+	void				releaseCluster(int index);
+	void				releaseClusters();
 	/* Generate clusters (K-mean)											*/ 
 	int					generateClusters(int k,int maxiterations=8192);
 	/* Refine																*/