Index: code/branches/questsystem5/src/bullet/BulletCollision/BroadphaseCollision/btDbvtBroadphase.cpp
===================================================================
--- code/branches/questsystem5/src/bullet/BulletCollision/BroadphaseCollision/btDbvtBroadphase.cpp	(revision 2777)
+++ code/branches/questsystem5/src/bullet/BulletCollision/BroadphaseCollision/btDbvtBroadphase.cpp	(revision 2907)
@@ -101,5 +101,6 @@
 			btDbvtProxy*	pb=(btDbvtProxy*)nb->data;
 #if DBVT_BP_SORTPAIRS
-			if(pa>pb) btSwap(pa,pb);
+			if(pa->m_uniqueId>pb->m_uniqueId) 
+				btSwap(pa,pb);
 #endif
 			pbp->m_paircache->addOverlappingPair(pa,pb);
@@ -133,7 +134,5 @@
 	m_updates_done		=	0;
 	m_updates_ratio		=	0;
-	m_paircache			=	paircache?
-paircache	:
-	new(btAlignedAlloc(sizeof(btHashedOverlappingPairCache),16)) btHashedOverlappingPairCache();
+	m_paircache			=	paircache? paircache	: new(btAlignedAlloc(sizeof(btHashedOverlappingPairCache),16)) btHashedOverlappingPairCache();
 	m_gid				=	0;
 	m_pid				=	0;
@@ -211,21 +210,20 @@
 }
 
+struct	BroadphaseRayTester : btDbvt::ICollide
+{
+	btBroadphaseRayCallback& m_rayCallback;
+	BroadphaseRayTester(btBroadphaseRayCallback& orgCallback)
+		:m_rayCallback(orgCallback)
+	{
+	}
+	void					Process(const btDbvtNode* leaf)
+	{
+		btDbvtProxy*	proxy=(btDbvtProxy*)leaf->data;
+		m_rayCallback.process(proxy);
+	}
+};	
+
 void	btDbvtBroadphase::rayTest(const btVector3& rayFrom,const btVector3& rayTo, btBroadphaseRayCallback& rayCallback,const btVector3& aabbMin,const btVector3& aabbMax)
 {
-
-	struct	BroadphaseRayTester : btDbvt::ICollide
-	{
-		btBroadphaseRayCallback& m_rayCallback;
-		BroadphaseRayTester(btBroadphaseRayCallback& orgCallback)
-			:m_rayCallback(orgCallback)
-		{
-		}
-		void					Process(const btDbvtNode* leaf)
-		{
-			btDbvtProxy*	proxy=(btDbvtProxy*)leaf->data;
-			m_rayCallback.process(proxy);
-		}
-	};	
-
 	BroadphaseRayTester callback(rayCallback);
 
@@ -342,4 +340,79 @@
 	}
 #endif
+
+	performDeferredRemoval(dispatcher);
+
+}
+
+void btDbvtBroadphase::performDeferredRemoval(btDispatcher* dispatcher)
+{
+
+	if (m_paircache->hasDeferredRemoval())
+	{
+
+		btBroadphasePairArray&	overlappingPairArray = m_paircache->getOverlappingPairArray();
+
+		//perform a sort, to find duplicates and to sort 'invalid' pairs to the end
+		overlappingPairArray.quickSort(btBroadphasePairSortPredicate());
+
+		int invalidPair = 0;
+
+		
+		int i;
+
+		btBroadphasePair previousPair;
+		previousPair.m_pProxy0 = 0;
+		previousPair.m_pProxy1 = 0;
+		previousPair.m_algorithm = 0;
+		
+		
+		for (i=0;i<overlappingPairArray.size();i++)
+		{
+		
+			btBroadphasePair& pair = overlappingPairArray[i];
+
+			bool isDuplicate = (pair == previousPair);
+
+			previousPair = pair;
+
+			bool needsRemoval = false;
+
+			if (!isDuplicate)
+			{
+				//important to perform AABB check that is consistent with the broadphase
+				btDbvtProxy*		pa=(btDbvtProxy*)pair.m_pProxy0;
+				btDbvtProxy*		pb=(btDbvtProxy*)pair.m_pProxy1;
+				bool hasOverlap = Intersect(pa->leaf->volume,pb->leaf->volume);
+
+				if (hasOverlap)
+				{
+					needsRemoval = false;
+				} else
+				{
+					needsRemoval = true;
+				}
+			} else
+			{
+				//remove duplicate
+				needsRemoval = true;
+				//should have no algorithm
+				btAssert(!pair.m_algorithm);
+			}
+			
+			if (needsRemoval)
+			{
+				m_paircache->cleanOverlappingPair(pair,dispatcher);
+
+				pair.m_pProxy0 = 0;
+				pair.m_pProxy1 = 0;
+				invalidPair++;
+			} 
+			
+		}
+
+		//perform a sort, to sort 'invalid' pairs to the end
+		overlappingPairArray.quickSort(btBroadphasePairSortPredicate());
+		overlappingPairArray.resize(overlappingPairArray.size() - invalidPair);
+	}
 }
 
@@ -347,4 +420,21 @@
 void							btDbvtBroadphase::collide(btDispatcher* dispatcher)
 {
+	/*printf("---------------------------------------------------------\n");
+	printf("m_sets[0].m_leaves=%d\n",m_sets[0].m_leaves);
+	printf("m_sets[1].m_leaves=%d\n",m_sets[1].m_leaves);
+	printf("numPairs = %d\n",getOverlappingPairCache()->getNumOverlappingPairs());
+	{
+		int i;
+		for (i=0;i<getOverlappingPairCache()->getNumOverlappingPairs();i++)
+		{
+			printf("pair[%d]=(%d,%d),",i,getOverlappingPairCache()->getOverlappingPairArray()[i].m_pProxy0->getUid(),
+				getOverlappingPairCache()->getOverlappingPairArray()[i].m_pProxy1->getUid());
+		}
+		printf("\n");
+	}
+*/
+
+
+
 	SPC(m_profiling.m_total);
 	/* optimize				*/ 
@@ -402,9 +492,9 @@
 		if(pairs.size()>0)
 		{
-			const int	ci=pairs.size();
-			int			ni=btMin(ci,btMax<int>(m_newpairs,(ci*m_cupdates)/100));
+
+			int			ni=btMin(pairs.size(),btMax<int>(m_newpairs,(pairs.size()*m_cupdates)/100));
 			for(int i=0;i<ni;++i)
 			{
-				btBroadphasePair&	p=pairs[(m_cid+i)%ci];
+				btBroadphasePair&	p=pairs[(m_cid+i)%pairs.size()];
 				btDbvtProxy*		pa=(btDbvtProxy*)p.m_pProxy0;
 				btDbvtProxy*		pb=(btDbvtProxy*)p.m_pProxy1;
@@ -412,5 +502,6 @@
 				{
 #if DBVT_BP_SORTPAIRS
-					if(pa>pb) btSwap(pa,pb);
+					if(pa->m_uniqueId>pb->m_uniqueId) 
+						btSwap(pa,pb);
 #endif
 					m_paircache->removeOverlappingPair(pa,pb,dispatcher);
@@ -467,4 +558,37 @@
 	aabbMin=bounds.Mins();
 	aabbMax=bounds.Maxs();
+}
+
+void btDbvtBroadphase::resetPool(btDispatcher* dispatcher)
+{
+	
+	int totalObjects = m_sets[0].m_leaves + m_sets[1].m_leaves;
+	if (!totalObjects)
+	{
+		//reset internal dynamic tree data structures
+		m_sets[0].clear();
+		m_sets[1].clear();
+		
+		m_deferedcollide	=	false;
+		m_needcleanup		=	true;
+		m_prediction		=	1/(btScalar)2;
+		m_stageCurrent		=	0;
+		m_fixedleft			=	0;
+		m_fupdates			=	1;
+		m_dupdates			=	0;
+		m_cupdates			=	10;
+		m_newpairs			=	1;
+		m_updates_call		=	0;
+		m_updates_done		=	0;
+		m_updates_ratio		=	0;
+		
+		m_gid				=	0;
+		m_pid				=	0;
+		m_cid				=	0;
+		for(int i=0;i<=STAGECOUNT;++i)
+		{
+			m_stageRoots[i]=0;
+		}
+	}
 }
 
@@ -601,2 +725,3 @@
 #undef	SPC
 #endif
+
