more work on hashed pairmanager. growing doesn't work yet, so need to allocate enough room for the overlapping pairs in advance.

boxbox reports contact point in B, rather then average point box, cylinder use halfextents corrected for scaling and margin. made the margin in this halfextents explicit in the 'getHalfExtentsWithMargin' and 'getHalfExtentsWithoutMargin' integrated changed for ODE quickstep solver replaced inline with SIMD_FORCE_INLINE some minor optimizations in the btSequentialImpulseConstraintSolver added cone drawing (for X,Y,Z cones)
2007-10-12 02:52:28 +00:00
parent 1baa61bc8d
commit eff4fe8ec8
39 changed files with 1882 additions and 1336 deletions
--- a/src/BulletCollision/BroadphaseCollision/btAxisSweep3.h
+++ b/src/BulletCollision/BroadphaseCollision/btAxisSweep3.h
@@ -63,8 +63,8 @@ public:
 		
 		//void* m_pOwner; this is now in btBroadphaseProxy.m_clientObject
 	
-		inline void SetNextFree(BP_FP_INT_TYPE next) {m_minEdges[0] = next;}
-		inline BP_FP_INT_TYPE GetNextFree() const {return m_minEdges[0];}
+		SIMD_FORCE_INLINE void SetNextFree(BP_FP_INT_TYPE next) {m_minEdges[0] = next;}
+		SIMD_FORCE_INLINE BP_FP_INT_TYPE GetNextFree() const {return m_minEdges[0];}
 	};		// 24 bytes + 24 for Edge structures = 44 bytes total per entry

 	
@@ -119,7 +119,7 @@ public:
 	BP_FP_INT_TYPE addHandle(const btPoint3& aabbMin,const btPoint3& aabbMax, void* pOwner,short int collisionFilterGroup,short int collisionFilterMask,btDispatcher* dispatcher);
 	void removeHandle(BP_FP_INT_TYPE handle,btDispatcher* dispatcher);
 	void updateHandle(BP_FP_INT_TYPE handle, const btPoint3& aabbMin,const btPoint3& aabbMax,btDispatcher* dispatcher);
-	inline Handle* getHandle(BP_FP_INT_TYPE index) const {return m_pHandles + index;}
+	SIMD_FORCE_INLINE Handle* getHandle(BP_FP_INT_TYPE index) const {return m_pHandles + index;}

 	void	processAllOverlappingPairs(btOverlapCallback* callback);

@@ -344,7 +344,7 @@ BP_FP_INT_TYPE btAxisSweep3Internal<BP_FP_INT_TYPE>::addHandle(const btPoint3& a

 	// allocate a handle
 	BP_FP_INT_TYPE handle = allocHandle();
-	assert(handle!= 0xcdcd);
+	

 	Handle* pHandle = getHandle(handle);
 	
--- a/src/BulletCollision/BroadphaseCollision/btBroadphaseProxy.h
+++ b/src/BulletCollision/BroadphaseCollision/btBroadphaseProxy.h
@@ -98,7 +98,7 @@ BT_DECLARE_ALIGNED_ALLOCATOR();
 	};

 	int			m_uniqueId;//m_uniqueId is introduced for paircache. could get rid of this, by calculating the address offset etc.
-	inline int getUid()
+	SIMD_FORCE_INLINE int getUid()
 	{
 		return m_uniqueId;//(int)this;
 	}
@@ -115,26 +115,26 @@ BT_DECLARE_ALIGNED_ALLOCATOR();

 	

-	static inline bool isPolyhedral(int proxyType)
+	static SIMD_FORCE_INLINE bool isPolyhedral(int proxyType)
 	{
 		return (proxyType  < IMPLICIT_CONVEX_SHAPES_START_HERE);
 	}

-	static inline bool	isConvex(int proxyType)
+	static SIMD_FORCE_INLINE bool	isConvex(int proxyType)
 	{
 		return (proxyType < CONCAVE_SHAPES_START_HERE);
 	}

-	static inline bool	isConcave(int proxyType)
+	static SIMD_FORCE_INLINE bool	isConcave(int proxyType)
 	{
 		return ((proxyType > CONCAVE_SHAPES_START_HERE) &&
 			(proxyType < CONCAVE_SHAPES_END_HERE));
 	}
-	static inline bool	isCompound(int proxyType)
+	static SIMD_FORCE_INLINE bool	isCompound(int proxyType)
 	{
 		return (proxyType == COMPOUND_SHAPE_PROXYTYPE);
 	}
-	static inline bool isInfinite(int proxyType)
+	static SIMD_FORCE_INLINE bool isInfinite(int proxyType)
 	{
 		return (proxyType == STATIC_PLANE_PROXYTYPE);
 	}
--- a/src/BulletCollision/BroadphaseCollision/btMultiSapBroadphase.cpp
+++ b/src/BulletCollision/BroadphaseCollision/btMultiSapBroadphase.cpp
@@ -23,10 +23,17 @@ subject to the following restrictions:
 ///	btOverlappingPairCache*	m_overlappingPairs;
 extern int gOverlappingPairs;

-btMultiSapBroadphase::btMultiSapBroadphase(int maxProxies)
-:m_invalidPair(0)
+btMultiSapBroadphase::btMultiSapBroadphase(int maxProxies,btOverlappingPairCache* pairCache)
+:m_invalidPair(0),
+m_ownsPairCache(false),
+m_overlappingPairs(pairCache)
 {
-	m_overlappingPairs = new btOverlappingPairCache();
+	if (!m_overlappingPairs)
+	{
+		m_ownsPairCache = true;
+		void* mem = btAlignedAlloc(sizeof(btOverlappingPairCache),16);
+		m_overlappingPairs = new (mem)btOverlappingPairCache();
+	}

 	struct btMultiSapOverlapFilterCallback : public btOverlapFilterCallback
 	{
@@ -54,6 +61,10 @@ btMultiSapBroadphase::btMultiSapBroadphase(int maxProxies)

 btMultiSapBroadphase::~btMultiSapBroadphase()
 {
+	if (m_ownsPairCache)
+	{
+		btAlignedFree(m_overlappingPairs);
+	}
 }

 btBroadphaseProxy*	btMultiSapBroadphase::createProxy(  const btVector3& aabbMin,  const btVector3& aabbMax,int shapeType,void* userPtr, short int collisionFilterGroup,short int collisionFilterMask, btDispatcher* dispatcher)
--- a/src/BulletCollision/BroadphaseCollision/btMultiSapBroadphase.h
+++ b/src/BulletCollision/BroadphaseCollision/btMultiSapBroadphase.h
@@ -35,6 +35,8 @@ class btMultiSapBroadphase :public btBroadphaseInterface
 	btSimpleBroadphase*		m_simpleBroadphase;

 	btOverlappingPairCache*	m_overlappingPairs;
+
+	bool					m_ownsPairCache;
 	
 	btOverlapFilterCallback*	m_filterCallback;

@@ -81,7 +83,7 @@ protected:

 public:

-	btMultiSapBroadphase(int maxProxies = 16384);
+	btMultiSapBroadphase(int maxProxies = 16384,btOverlappingPairCache* pairCache=0);

 	btSapBroadphaseArray	getBroadphaseArray()
 	{
--- a/src/BulletCollision/BroadphaseCollision/btOverlappingPairCache.cpp
+++ b/src/BulletCollision/BroadphaseCollision/btOverlappingPairCache.cpp
@@ -23,24 +23,18 @@ subject to the following restrictions:

 int	gOverlappingPairs = 0;

+int gRemovePairs =0;
+int gAddedPairs =0;
+int gFindPairs =0;
+
 btOverlappingPairCache::btOverlappingPairCache():
-m_blockedForChanges(false),
-m_overlapFilterCallback(0)
-//m_NumOverlapBroadphasePair(0)
+	m_blockedForChanges(false),
+	m_overlapFilterCallback(0)
 {
+	int initialAllocatedSize= 65536*256;//2;//this needs to be a power of 2!
+	m_overlappingPairArray.reserve(initialAllocatedSize);

-#ifdef USE_HASH_PAIRCACHE
-	m_overlappingPairArray.reserve(b2_maxPairs);
-
-	for (int32 i = 0; i < b2_tableCapacity; ++i)
-	{
-		m_hashTable[i] = b2_nullPair;
-	}
-	for (int32 i = 0; i < b2_maxPairs; ++i)
-	{
-		m_next[i] = b2_nullPair;
-	}
-#endif //USE_HASH_PAIRCACHE
+	growTables();
 }


@@ -125,66 +119,31 @@ void	btOverlappingPairCache::removeOverlappingPairsContainingProxy(btBroadphaseP

 #ifdef USE_HASH_PAIRCACHE

-// Thomas Wang's hash, see: http://www.concentric.net/~Ttwang/tech/inthash.htm
-// This assumes proxyId1 and proxyId2 are 16-bit.
-inline uint32 Hash(uint32 proxyId1, uint32 proxyId2)
+
+
+
+
+
+
+btBroadphasePair* btOverlappingPairCache::findPair(btBroadphaseProxy* proxy0, btBroadphaseProxy* proxy1)
 {
-	uint32 key = (proxyId2 << 16) | proxyId1;
-	key = ~key + (key << 15);
-	key = key ^ (key >> 12);
-	key = key + (key << 2);
-	key = key ^ (key >> 4);
-	key = key * 2057;
-	key = key ^ (key >> 16);
-	return key;
-}
+	gFindPairs++;

-inline bool Equals(const btBroadphasePair& pair, int32 proxyId1, int32 proxyId2)
-{	
-	return pair.m_pProxy0->getUid() == proxyId1 && pair.m_pProxy1->getUid() == proxyId2;
-}
-
-inline btBroadphasePair* btOverlappingPairCache::Find(btBroadphaseProxy* proxy0, btBroadphaseProxy* proxy1, uint32 hash)
-{
-	int32 proxyId1 = proxy0->getUid();
-	int32 proxyId2 = proxy1->getUid();
-	if (proxyId1 > proxyId2) 
-		btSwap(proxyId1, proxyId2);
-
-	int32 index = m_hashTable[hash];
-	
-	while( index != b2_nullPair && Equals(m_overlappingPairArray[index], proxyId1, proxyId2) == false)
-	{
-		index = m_next[index];
-	}
-
-	if ( index == b2_nullPair )
-	{
-		return NULL;
-	}
-
-	btAssert(index < m_overlappingPairArray.size());
-
-	return &m_overlappingPairArray[index];
-}
-
-btBroadphasePair* btOverlappingPairCache::Find(btBroadphaseProxy* proxy0, btBroadphaseProxy* proxy1)
-{
-	int32 proxyId1 = proxy0->getUid();
-	int32 proxyId2 = proxy1->getUid();
+	int proxyId1 = proxy0->getUid();
+	int proxyId2 = proxy1->getUid();

 	if (proxyId1 > proxyId2) 
 		btSwap(proxyId1, proxyId2);

-	int32 hash = Hash(proxyId1, proxyId2) & b2_tableMask;
+	int hash = getHash(proxyId1, proxyId2) & (m_overlappingPairArray.capacity()-1);

-	int32 index = m_hashTable[hash];
-	while (index != b2_nullPair && Equals(m_overlappingPairArray[index], proxyId1, proxyId2) == false)
+	int index = m_hashTable[hash];
+	while (index != BT_NULL_PAIR && equalsPair(m_overlappingPairArray[index], proxyId1, proxyId2) == false)
 	{
 		index = m_next[index];
 	}

-	if (index == b2_nullPair)
+	if (index == BT_NULL_PAIR)
 	{
 		return NULL;
 	}
@@ -196,32 +155,79 @@ btBroadphasePair* btOverlappingPairCache::Find(btBroadphaseProxy* proxy0, btBroa

 #include <stdio.h>

-btBroadphasePair* btOverlappingPairCache::Add(btBroadphaseProxy* proxy0, btBroadphaseProxy* proxy1)
+void	btOverlappingPairCache::growTables()
 {
-	int32 proxyId1 = proxy0->getUid();
-	int32 proxyId2 = proxy1->getUid();
+	//or put an assert here instead?
+
+	int newCapacity = m_overlappingPairArray.capacity();
+
+	if (m_hashTable.size() < newCapacity)
+	{
+		//grow hashtable and next table
+		int curHashtableSize = m_hashTable.size();
+		int curNextTableSize = m_next.size();
+
+		m_hashTable.resize(newCapacity);
+		m_next.resize(newCapacity);
+
+		for (int i= curHashtableSize; i < newCapacity; ++i)
+		{
+			m_hashTable[i] = BT_NULL_PAIR;
+		}
+		for (int i = curNextTableSize; i < newCapacity; ++i)
+		{
+			m_next[i] = BT_NULL_PAIR;
+		}
+
+		for(int	i=0;i<curHashtableSize;i++)
+		{
+			btAssert(0);
+			//this is not working yet, work in progress... please allocate enough room up front to avoid growing for now.
+	
+
+			const btBroadphasePair& pair = m_overlappingPairArray[i];
+			int proxyId1 = pair.m_pProxy0->getUid();
+			int proxyId2 = pair.m_pProxy1->getUid();
+			if (proxyId1 > proxyId2) 
+				btSwap(proxyId1, proxyId2);
+			int	hashValue = getHash(proxyId1,proxyId2) & (m_overlappingPairArray.capacity()-1);	// New hash value with new mask
+			m_next[i] = m_hashTable[hashValue];
+			m_hashTable[hashValue] = i;
+		}
+
+
+	}
+}
+
+btBroadphasePair* btOverlappingPairCache::internalAddPair(btBroadphaseProxy* proxy0, btBroadphaseProxy* proxy1)
+{
+	int proxyId1 = proxy0->getUid();
+	int proxyId2 = proxy1->getUid();

 	if (proxyId1 > proxyId2) 
 		btSwap(proxyId1, proxyId2);

-	int32 hash = Hash(proxyId1, proxyId2) & b2_tableMask;
+	int hash = getHash(proxyId1, proxyId2) & (m_overlappingPairArray.capacity()-1);

 	

-	btBroadphasePair* pair = Find(proxy0, proxy1, hash);
+	btBroadphasePair* pair = internalFindPair(proxy0, proxy1, hash);
 	if (pair != NULL)
 	{
 		return pair;
 	}

-	if (m_overlappingPairArray.size() == b2_maxPairs)
-	{
-		btAssert(false);
-		return NULL;
-	}
-	
 	int count = m_overlappingPairArray.size();
+	int oldCapacity = m_overlappingPairArray.capacity();
 	void* mem = &m_overlappingPairArray.expand();
+	int newCapacity = m_overlappingPairArray.capacity();
+
+	if (oldCapacity < newCapacity)
+	{
+		growTables();
+		//hash with new capacity
+		hash = getHash(proxyId1, proxyId2) & (m_overlappingPairArray.capacity()-1);
+	}
 	
 	pair = new (mem) btBroadphasePair(*proxy0,*proxy1);
 //	pair->m_pProxy0 = proxy0;
@@ -231,26 +237,29 @@ btBroadphasePair* btOverlappingPairCache::Add(btBroadphaseProxy* proxy0, btBroad
 	

 	m_next[count] = m_hashTable[hash];
-	m_hashTable[hash] = (uint16)count;
+	m_hashTable[hash] = count;

 	return pair;
 }


-void* btOverlappingPairCache::Remove(btBroadphaseProxy* proxy0, btBroadphaseProxy* proxy1,btDispatcher* dispatcher)
+
+void* btOverlappingPairCache::removeOverlappingPair(btBroadphaseProxy* proxy0, btBroadphaseProxy* proxy1,btDispatcher* dispatcher)
 {
-	int32 proxyId1 = proxy0->getUid();
-	int32 proxyId2 = proxy1->getUid();
+	gRemovePairs++;
+
+	int proxyId1 = proxy0->getUid();
+	int proxyId2 = proxy1->getUid();

 	if (proxyId1 > proxyId2) 
 		btSwap(proxyId1, proxyId2);

-	int32 hash = Hash(proxyId1, proxyId2) & b2_tableMask;
+	int hash = getHash(proxyId1, proxyId2) & (m_overlappingPairArray.capacity()-1);

-	btBroadphasePair* pair = Find(proxy0, proxy1, hash);
+	btBroadphasePair* pair = internalFindPair(proxy0, proxy1, hash);
 	if (pair == NULL)
 	{
-		return NULL;
+		return 0;
 	}

 	cleanOverlappingPair(*pair,dispatcher);
@@ -260,21 +269,21 @@ void* btOverlappingPairCache::Remove(btBroadphaseProxy* proxy0, btBroadphaseProx
 	btAssert(pair->m_pProxy0->getUid() == proxyId1);
 	btAssert(pair->m_pProxy1->getUid() == proxyId2);

-	int32 pairIndex = int32(pair - &m_overlappingPairArray[0]);
+	int pairIndex = int(pair - &m_overlappingPairArray[0]);
 	btAssert(pairIndex < m_overlappingPairArray.size());

 	// Remove the pair from the hash table.
-	int32 index = m_hashTable[hash];
-	btAssert(index != b2_nullPair);
+	int index = m_hashTable[hash];
+	btAssert(index != BT_NULL_PAIR);

-	int32 previous = b2_nullPair;
+	int previous = BT_NULL_PAIR;
 	while (index != pairIndex)
 	{
 		previous = index;
 		index = m_next[index];
 	}

-	if (previous != b2_nullPair)
+	if (previous != BT_NULL_PAIR)
 	{
 		btAssert(m_next[previous] == pairIndex);
 		m_next[previous] = m_next[pairIndex];
@@ -288,7 +297,7 @@ void* btOverlappingPairCache::Remove(btBroadphaseProxy* proxy0, btBroadphaseProx
 	// pair being removed. We need to fix the hash
 	// table indices to support the move.

-	int32 lastPairIndex = m_overlappingPairArray.size() - 1;
+	int lastPairIndex = m_overlappingPairArray.size() - 1;

 	// If the removed pair is the last pair, we are done.
 	if (lastPairIndex == pairIndex)
@@ -299,19 +308,19 @@ void* btOverlappingPairCache::Remove(btBroadphaseProxy* proxy0, btBroadphaseProx

 	// Remove the last pair from the hash table.
 	const btBroadphasePair* last = &m_overlappingPairArray[lastPairIndex];
-	int32 lastHash = Hash(last->m_pProxy0->getUid(), last->m_pProxy1->getUid()) & b2_tableMask;
+	int lastHash = getHash(last->m_pProxy0->getUid(), last->m_pProxy1->getUid()) & (m_overlappingPairArray.capacity()-1);

 	index = m_hashTable[lastHash];
-	btAssert(index != b2_nullPair);
+	btAssert(index != BT_NULL_PAIR);

-	previous = b2_nullPair;
+	previous = BT_NULL_PAIR;
 	while (index != lastPairIndex)
 	{
 		previous = index;
 		index = m_next[index];
 	}

-	if (previous != b2_nullPair)
+	if (previous != BT_NULL_PAIR)
 	{
 		btAssert(m_next[previous] == lastPairIndex);
 		m_next[previous] = m_next[lastPairIndex];
@@ -326,7 +335,7 @@ void* btOverlappingPairCache::Remove(btBroadphaseProxy* proxy0, btBroadphaseProx

 	// Insert the last pair into the hash table
 	m_next[pairIndex] = m_hashTable[lastHash];
-	m_hashTable[lastHash] = (uint16)pairIndex;
+	m_hashTable[lastHash] = pairIndex;

 	m_overlappingPairArray.pop_back();

@@ -361,7 +370,7 @@ void	btOverlappingPairCache::processAllOverlappingPairs(btOverlapCallback* callb



-void	btOverlappingPairCache::removeOverlappingPair(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1, btDispatcher* dispatcher )
+void*	btOverlappingPairCache::removeOverlappingPair(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1, btDispatcher* dispatcher )
 {
 #ifndef USE_LAZY_REMOVAL

@@ -372,13 +381,16 @@ void	btOverlappingPairCache::removeOverlappingPair(btBroadphaseProxy* proxy0,btB
 	{
 		gOverlappingPairs--;
 		btBroadphasePair& pair = m_overlappingPairArray[findIndex];
+		void* userData = pair.m_userInfo;
 		cleanOverlappingPair(pair,dispatcher);
 		
-		m_overlappingPairArray.swap(findIndex,m_overlappingPairArray.size()-1);
+		m_overlappingPairArray.swap(findIndex,m_overlappingPairArray.capacity()-1);
 		m_overlappingPairArray.pop_back();
+		return userData;
 	}
 #endif //USE_LAZY_REMOVAL

+	return 0;
 }


@@ -388,18 +400,18 @@ void	btOverlappingPairCache::removeOverlappingPair(btBroadphaseProxy* proxy0,btB



-void	btOverlappingPairCache::addOverlappingPair(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1)
+btBroadphasePair*	btOverlappingPairCache::addOverlappingPair(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1)
 {
 	//don't add overlap with own
 	assert(proxy0 != proxy1);

 	if (!needsBroadphaseCollision(proxy0,proxy1))
-		return;
-
-
-	btBroadphasePair pair(*proxy0,*proxy1);
-	m_overlappingPairArray.push_back(pair);
+		return 0;
+	
+	void* mem = &m_overlappingPairArray.expand();
+	btBroadphasePair* pair = new (mem) btBroadphasePair(*proxy0,*proxy1);
 	gOverlappingPairs++;
+	return pair;
 	
 }

@@ -448,7 +460,7 @@ void	btOverlappingPairCache::processAllOverlappingPairs(btOverlapCallback* callb
 		{
 			cleanOverlappingPair(*pair,dispatcher);

-			m_overlappingPairArray.swap(i,m_overlappingPairArray.size()-1);
+			m_overlappingPairArray.swap(i,m_overlappingPairArray.capacity()-1);
 			m_overlappingPairArray.pop_back();
 			gOverlappingPairs--;
 		} else
--- a/src/BulletCollision/BroadphaseCollision/btOverlappingPairCache.h
+++ b/src/BulletCollision/BroadphaseCollision/btOverlappingPairCache.h
@@ -24,6 +24,7 @@ subject to the following restrictions:
 #include "LinearMath/btAlignedObjectArray.h"
 class btDispatcher;

+///disable the USE_HASH_PAIRCACHE define to use a pair manager that sorts the pairs to find duplicates/non-overlap
 #define USE_HASH_PAIRCACHE 1


@@ -48,18 +49,13 @@ typedef btAlignedObjectArray<btBroadphasePair>	btBroadphasePairArray;
 #ifdef USE_HASH_PAIRCACHE


-
-const int b2_maxPairs = 65536;//32768;
-typedef unsigned short int uint16;
-typedef int int32;
-typedef unsigned int uint32;
-
 /// Hash-space based Pair Cache, thanks to Erin Catto, Box2D, http://www.box2d.org, and Pierre Terdiman, Codercorner, http://codercorner.com
-const uint16 b2_nullPair = 0xffff;
-const uint16 b2_nullProxy = 0xffff;
-const int32 b2_tableCapacity = b2_maxPairs;	// must be a power of two
-const int32 b2_tableMask = b2_tableCapacity - 1;

+extern int gRemovePairs;
+extern int gAddedPairs;
+extern int gFindPairs;
+
+#define BT_NULL_PAIR 0xffffffff

 class btOverlappingPairCache
 {
@@ -67,6 +63,8 @@ class btOverlappingPairCache
 	btOverlapFilterCallback* m_overlapFilterCallback;
 	bool		m_blockedForChanges;

+	
+
 public:
 	btOverlappingPairCache();
 	~btOverlappingPairCache();
@@ -74,12 +72,9 @@ public:
 	
 	void	removeOverlappingPairsContainingProxy(btBroadphaseProxy* proxy,btDispatcher* dispatcher);

-	void	removeOverlappingPair(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1,btDispatcher* dispatcher)
-	{
-		Remove(proxy0,proxy1,dispatcher);
-	}
-
-	inline bool needsBroadphaseCollision(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1) const
+	void*	removeOverlappingPair(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1,btDispatcher* dispatcher);
+	
+	SIMD_FORCE_INLINE bool needsBroadphaseCollision(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1) const
 	{
 		if (m_overlapFilterCallback)
 			return m_overlapFilterCallback->needBroadphaseCollision(proxy0,proxy1);
@@ -90,18 +85,19 @@ public:
 		return collides;
 	}

-	void	addOverlappingPair(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1)
+	// Add a pair and return the new pair. If the pair already exists,
+	// no new pair is created and the old one is returned.
+	SIMD_FORCE_INLINE	btBroadphasePair* 	addOverlappingPair(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1)
 	{
+		gAddedPairs++;
+
 		if (!needsBroadphaseCollision(proxy0,proxy1))
-			return;
+			return 0;

-		Add(proxy0,proxy1);
+		return internalAddPair(proxy0,proxy1);
 	}

-	btBroadphasePair*	findPair(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1)
-	{
-		return Find(proxy0,proxy1);
-	}
+	

 	void	cleanProxyFromPairs(btBroadphaseProxy* proxy,btDispatcher* dispatcher);

@@ -130,16 +126,11 @@ public:

 	void	cleanOverlappingPair(btBroadphasePair& pair,btDispatcher* dispatcher);

-	// Add a pair and return the new pair. If the pair already exists,
-	// no new pair is created and the old one is returned.
-	btBroadphasePair* Add(btBroadphaseProxy* proxy0, btBroadphaseProxy* proxy1);

-	// Remove a pair, return the pair's userData.
-	void* Remove(btBroadphaseProxy* proxy0, btBroadphaseProxy* proxy1,btDispatcher* dispatcher);

-	btBroadphasePair* Find(btBroadphaseProxy* proxy0, btBroadphaseProxy* proxy1);
+	btBroadphasePair* findPair(btBroadphaseProxy* proxy0, btBroadphaseProxy* proxy1);

-	int32 GetCount() const { return m_overlappingPairArray.size(); }
+	int GetCount() const { return m_overlappingPairArray.size(); }
 //	btBroadphasePair* GetPairs() { return m_pairs; }

 	btOverlapFilterCallback* getOverlapFilterCallback()
@@ -157,12 +148,82 @@ public:
 		return m_overlappingPairArray.size();
 	}
 private:
-	btBroadphasePair* Find(btBroadphaseProxy* proxy0, btBroadphaseProxy* proxy1, uint32 hashValue);
+	
+	btBroadphasePair* 	internalAddPair(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1);
+
+	void	growTables();
+
+	SIMD_FORCE_INLINE bool equalsPair(const btBroadphasePair& pair, int proxyId1, int proxyId2)
+	{	
+		return pair.m_pProxy0->getUid() == proxyId1 && pair.m_pProxy1->getUid() == proxyId2;
+	}
+
+	/*
+	// Thomas Wang's hash, see: http://www.concentric.net/~Ttwang/tech/inthash.htm
+	// This assumes proxyId1 and proxyId2 are 16-bit.
+	SIMD_FORCE_INLINE int getHash(int proxyId1, int proxyId2)
+	{
+		int key = (proxyId2 << 16) | proxyId1;
+		key = ~key + (key << 15);
+		key = key ^ (key >> 12);
+		key = key + (key << 2);
+		key = key ^ (key >> 4);
+		key = key * 2057;
+		key = key ^ (key >> 16);
+		return key;
+	}
+	*/
+
+
+	
+	SIMD_FORCE_INLINE	unsigned int getHash(unsigned int proxyId1, unsigned int proxyId2)
+	{
+		int key = ((unsigned int)proxyId1) | (((unsigned int)proxyId1) <<16);
+		// Thomas Wang's hash
+
+		key += ~(key << 15);
+		key ^=  (key >> 10);
+		key +=  (key << 3);
+		key ^=  (key >> 6);
+		key += ~(key << 11);
+		key ^=  (key >> 16);
+		return key;
+	}
+	
+
+
+
+
+	SIMD_FORCE_INLINE btBroadphasePair* internalFindPair(btBroadphaseProxy* proxy0, btBroadphaseProxy* proxy1, int hash)
+	{
+		int proxyId1 = proxy0->getUid();
+		int proxyId2 = proxy1->getUid();
+		if (proxyId1 > proxyId2) 
+			btSwap(proxyId1, proxyId2);
+
+		int index = m_hashTable[hash];
+		
+		while( index != BT_NULL_PAIR && equalsPair(m_overlappingPairArray[index], proxyId1, proxyId2) == false)
+		{
+			index = m_next[index];
+		}
+
+		if ( index == BT_NULL_PAIR )
+		{
+			return NULL;
+		}
+
+		btAssert(index < m_overlappingPairArray.size());
+
+		return &m_overlappingPairArray[index];
+	}
+

 public:
 	
-	uint16 m_hashTable[b2_tableCapacity];
-	uint16 m_next[b2_maxPairs];
+	btAlignedObjectArray<int>	m_hashTable;
+	btAlignedObjectArray<int>	m_next;
+	
 };


@@ -192,11 +253,11 @@ class	btOverlappingPairCache

 		virtual void	processAllOverlappingPairs(btOverlapCallback*,btDispatcher* dispatcher);

-		void	removeOverlappingPair(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1,btDispatcher* dispatcher);
+		void*	removeOverlappingPair(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1,btDispatcher* dispatcher);

 		void	cleanOverlappingPair(btBroadphasePair& pair,btDispatcher* dispatcher);
 		
-		void	addOverlappingPair(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1);
+		btBroadphasePair*	addOverlappingPair(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1);

 		btBroadphasePair*	findPair(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1);
 			
--- a/src/BulletCollision/BroadphaseCollision/btSimpleBroadphase.cpp
+++ b/src/BulletCollision/BroadphaseCollision/btSimpleBroadphase.cpp
@@ -44,7 +44,8 @@ btSimpleBroadphase::btSimpleBroadphase(int maxProxies, btOverlappingPairCache* o

 	if (!overlappingPairCache)
 	{
-		m_pairCache = new btOverlappingPairCache();
+		void* mem = btAlignedAlloc(sizeof(btOverlappingPairCache),16);
+		m_pairCache = new (mem)btOverlappingPairCache();
 		m_ownsPairCache = true;
 	}

@@ -76,7 +77,7 @@ btSimpleBroadphase::~btSimpleBroadphase()

 	if (m_ownsPairCache)
 	{
-		delete m_pairCache;
+		btAlignedFree(m_pairCache);
 	}
 }

--- a/src/BulletCollision/BroadphaseCollision/btSimpleBroadphase.h
+++ b/src/BulletCollision/BroadphaseCollision/btSimpleBroadphase.h
@@ -39,11 +39,11 @@ struct btSimpleBroadphaseProxy : public btBroadphaseProxy
 	}
 	
 	
-	inline void SetNextFree(int next) {m_nextFree = next;}
-	inline int GetNextFree() const {return m_nextFree;}
+	SIMD_FORCE_INLINE void SetNextFree(int next) {m_nextFree = next;}
+	SIMD_FORCE_INLINE int GetNextFree() const {return m_nextFree;}

-	inline void SetNextAllocated(int next) {m_nextAllocated = next;}
-	inline int GetNextAllocated() const {return m_nextAllocated;}
+	SIMD_FORCE_INLINE void SetNextAllocated(int next) {m_nextAllocated = next;}
+	SIMD_FORCE_INLINE int GetNextAllocated() const {return m_nextAllocated;}


 };