Fixes for broadphase/paircache determinism.

Revert definition for ATTRIBUTE_ALIGNED16, and try to force sizeof(btSolverConstraint) by using unions with btScalar, for non-btScalar data types.
Use btAssert and not assert.
Don't access btAlignedObjectArray elements, for zero sets

src/BulletCollision/BroadphaseCollision/btAxisSweep3.cpp

@@ -19,7 +19,6 @@
 // 3. This notice may not be removed or altered from any source distribution.
 #include "btAxisSweep3.h"
 
-#include <assert.h>
 
 btAxisSweep3::btAxisSweep3(const btVector3& worldAabbMin,const btVector3& worldAabbMax, unsigned short int maxHandles, btOverlappingPairCache* pairCache, bool disableRaycastAccelerator)
 :btAxisSweep3Internal<unsigned short int>(worldAabbMin,worldAabbMax,0xfffe,0xffff,maxHandles,pairCache,disableRaycastAccelerator)

src/BulletCollision/BroadphaseCollision/btAxisSweep3.h

@@ -221,7 +221,7 @@ void btAxisSweep3<BP_FP_INT_TYPE>::debugPrintAxis(int axis, bool checkCardinalit
 	}
 
 	if (checkCardinality)
-		assert(numEdges == m_numHandles*2+1);
+		btAssert(numEdges == m_numHandles*2+1);
 }
 #endif //DEBUG_BROADPHASE
 
@@ -347,7 +347,7 @@ m_raycastAccelerator(0)
 		m_raycastAccelerator->m_deferedcollide = true;//don't add/remove pairs
 	}
 
-	//assert(bounds.HasVolume());
+	//btAssert(bounds.HasVolume());
 
 	// init bounds
 	m_worldAabbMin = worldAabbMin;
@@ -453,7 +453,7 @@ void btAxisSweep3Internal<BP_FP_INT_TYPE>::quantize(BP_FP_INT_TYPE* out, const b
 template <typename BP_FP_INT_TYPE>
 BP_FP_INT_TYPE btAxisSweep3Internal<BP_FP_INT_TYPE>::allocHandle()
 {
-	assert(m_firstFreeHandle);
+	btAssert(m_firstFreeHandle);
 
 	BP_FP_INT_TYPE handle = m_firstFreeHandle;
 	m_firstFreeHandle = getHandle(handle)->GetNextFree();
@@ -465,7 +465,7 @@ BP_FP_INT_TYPE btAxisSweep3Internal<BP_FP_INT_TYPE>::allocHandle()
 template <typename BP_FP_INT_TYPE>
 void btAxisSweep3Internal<BP_FP_INT_TYPE>::freeHandle(BP_FP_INT_TYPE handle)
 {
-	assert(handle > 0 && handle < m_maxHandles);
+	btAssert(handle > 0 && handle < m_maxHandles);
 
 	getHandle(handle)->SetNextFree(m_firstFreeHandle);
 	m_firstFreeHandle = handle;
@@ -611,8 +611,83 @@ void	btAxisSweep3Internal<BP_FP_INT_TYPE>::calculateOverlappingPairs(btDispatche
 
 	if (m_pairCache->hasDeferredRemoval())
 	{
-		m_pairCache->performDeferredRemoval(dispatcher);
+	
+		btBroadphasePairArray&	overlappingPairArray = m_pairCache->getOverlappingPairArray();
+
+		//perform a sort, to find duplicates and to sort 'invalid' pairs to the end
+		overlappingPairArray.quickSort(btBroadphasePairSortPredicate());
+
+		overlappingPairArray.resize(overlappingPairArray.size() - m_invalidPair);
+		m_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 use an AABB test that is consistent with the broadphase
+				bool hasOverlap = testAabbOverlap(pair.m_pProxy0,pair.m_pProxy1);
+
+				if (hasOverlap)
+				{
+					needsRemoval = false;//callback->processOverlap(pair);
+				} else
+				{
+					needsRemoval = true;
+				}
+			} else
+			{
+				//remove duplicate
+				needsRemoval = true;
+				//should have no algorithm
+				btAssert(!pair.m_algorithm);
+			}
+			
+			if (needsRemoval)
+			{
+				m_pairCache->cleanOverlappingPair(pair,dispatcher);
+
+		//		m_overlappingPairArray.swap(i,m_overlappingPairArray.size()-1);
+		//		m_overlappingPairArray.pop_back();
+				pair.m_pProxy0 = 0;
+				pair.m_pProxy1 = 0;
+				m_invalidPair++;
+				gOverlappingPairs--;
+			} 
+			
+		}
+
+	///if you don't like to skip the invalid pairs in the array, execute following code:
+	#define CLEAN_INVALID_PAIRS 1
+	#ifdef CLEAN_INVALID_PAIRS
+
+		//perform a sort, to sort 'invalid' pairs to the end
+		overlappingPairArray.quickSort(btBroadphasePairSortPredicate());
+
+		overlappingPairArray.resize(overlappingPairArray.size() - m_invalidPair);
+		m_invalidPair = 0;
+	#endif//CLEAN_INVALID_PAIRS
+		
+		//printf("overlappingPairArray.size()=%d\n",overlappingPairArray.size());
 	}
+
 }
 
 
@@ -653,8 +728,8 @@ bool btAxisSweep3Internal<BP_FP_INT_TYPE>::testOverlap2D(const Handle* pHandleA,
 template <typename BP_FP_INT_TYPE>
 void btAxisSweep3Internal<BP_FP_INT_TYPE>::updateHandle(BP_FP_INT_TYPE handle, const btVector3& aabbMin,const btVector3& aabbMax,btDispatcher* dispatcher)
 {
-//	assert(bounds.IsFinite());
-	//assert(bounds.HasVolume());
+//	btAssert(bounds.IsFinite());
+	//btAssert(bounds.HasVolume());
 
 	Handle* pHandle = getHandle(handle);
 

src/BulletCollision/BroadphaseCollision/btDbvt.cpp

@@ -424,9 +424,14 @@ btDbvt::~btDbvt()
 //
 void			btDbvt::clear()
 {
-	if(m_root)	recursedeletenode(this,m_root);
+	if(m_root)	
+		recursedeletenode(this,m_root);
 	btAlignedFree(m_free);
 	m_free=0;
+	m_lkhd		=	-1;
+	m_stkStack.clear();
+	m_opath		=	0;
+	
 }
 
 //

src/BulletCollision/BroadphaseCollision/btDbvtBroadphase.cpp

@@ -133,9 +133,7 @@ btDbvtBroadphase::btDbvtBroadphase(btOverlappingPairCache* paircache)
 	m_updates_call		=	0;
 	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;
 	m_cid				=	0;
@@ -342,16 +340,102 @@ void							btDbvtBroadphase::calculateOverlappingPairs(btDispatcher* dispatcher)
 	}
 #endif
 
+	performDeferredRemoval(dispatcher);
+
+}
+
+void btDbvtBroadphase::performDeferredRemoval(btDispatcher* dispatcher)
+{
+
 	if (m_paircache->hasDeferredRemoval())
 	{
-		m_paircache->performDeferredRemoval(dispatcher);
+
+		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);
 	}
-	
 }
 
 //
 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				*/ 
 	m_sets[0].optimizeIncremental(1+(m_sets[0].m_leaves*m_dupdates)/100);
@@ -407,11 +491,11 @@ void							btDbvtBroadphase::collide(btDispatcher* dispatcher)
 		btBroadphasePairArray&	pairs=m_paircache->getOverlappingPairArray();
 		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;
 				if(!Intersect(pa->leaf->volume,pb->leaf->volume))
@@ -477,26 +561,35 @@ void							btDbvtBroadphase::getBroadphaseAabb(btVector3& aabbMin,btVector3& aab
 
 void btDbvtBroadphase::resetPool(btDispatcher* dispatcher)
 {
-	return;
-	m_sets[0].optimizeBottomUp();
-	m_sets[1].optimizeBottomUp();
-
-
-	btDbvtProxy*	current=m_stageRoots[m_stageCurrent];
-	if(current)
-	{
-		btDbvtTreeCollider	collider(this);
-		do	{
-			btDbvtProxy*	next=current->links[1];
-			listremove(current,m_stageRoots[current->stage]);
-			listappend(current,m_stageRoots[m_stageCurrent]);
-			current->stage	=	m_stageCurrent;	
-			current			=	next;
-		} while(current);
-	}
-
 	
-
+	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;
+		}
+	}
 }
 
 //

src/BulletCollision/BroadphaseCollision/btDbvtBroadphase.h

@@ -115,6 +115,9 @@ struct	btDbvtBroadphase : btBroadphaseInterface
 	void							printStats();
 	static void						benchmark(btBroadphaseInterface*);
 
+
+	void	performDeferredRemoval(btDispatcher* dispatcher);
+
 	///reset broadphase internal structures, to ensure determinism/reproducability
 	virtual void resetPool(btDispatcher* dispatcher);
 

src/BulletCollision/BroadphaseCollision/btOverlappingPairCache.cpp

@@ -462,7 +462,7 @@ void*	btSortedOverlappingPairCache::removeOverlappingPair(btBroadphaseProxy* pro
 btBroadphasePair*	btSortedOverlappingPairCache::addOverlappingPair(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1)
 {
 	//don't add overlap with own
-	assert(proxy0 != proxy1);
+	btAssert(proxy0 != proxy1);
 
 	if (!needsBroadphaseCollision(proxy0,proxy1))
 		return 0;
@@ -493,7 +493,7 @@ btBroadphasePair*	btSortedOverlappingPairCache::addOverlappingPair(btBroadphaseP
 
 	if (findIndex < m_overlappingPairArray.size())
 	{
-		//assert(it != m_overlappingPairSet.end());
+		//btAssert(it != m_overlappingPairSet.end());
 		 btBroadphasePair* pair = &m_overlappingPairArray[findIndex];
 		return pair;
 	}
@@ -631,70 +631,3 @@ void	btSortedOverlappingPairCache::sortOverlappingPairs(btDispatcher* dispatcher
 	//should already be sorted
 }
 
-
-
-void	btSortedOverlappingPairCache::performDeferredRemoval(btDispatcher* dispatcher)
-{
-
-	btBroadphasePairArray&	overlappingPairArray = 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)
-			{
-				
-				bool hasOverlap = TestAabbAgainstAabb2(pair.m_pProxy0->m_aabbMin,pair.m_pProxy0->m_aabbMax,pair.m_pProxy1->m_aabbMin,pair.m_pProxy1->m_aabbMax);
-
-				if (hasOverlap)
-				{
-					needsRemoval = false;
-				} else
-				{
-					needsRemoval = true;
-				}
-			} else
-			{
-				//remove duplicate
-				needsRemoval = true;
-				//should have no algorithm
-				btAssert(!pair.m_algorithm);
-			}
-			
-			if (needsRemoval)
-			{
-				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);
-	}

src/BulletCollision/BroadphaseCollision/btOverlappingPairCache.h

@@ -86,7 +86,6 @@ public:
 
 	virtual void	sortOverlappingPairs(btDispatcher* dispatcher) = 0;
 
-	virtual void	performDeferredRemoval(btDispatcher* dispatcher) = 0;
 
 };
 
@@ -265,10 +264,6 @@ private:
 
 	virtual void	sortOverlappingPairs(btDispatcher* dispatcher);
 	
-	virtual void	performDeferredRemoval(btDispatcher* dispatcher)
-	{
-
-	}
 
 protected:
 	
@@ -382,8 +377,6 @@ class	btSortedOverlappingPairCache : public btOverlappingPairCache
 
 		virtual void	sortOverlappingPairs(btDispatcher* dispatcher);
 		
-		void	performDeferredRemoval(btDispatcher* dispatcher);
-
 
 };
 
@@ -466,9 +459,6 @@ public:
 	{
 	}
 
-	virtual void	performDeferredRemoval(btDispatcher* dispatcher)
-	{
-	}
 
 };