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;}


 };
--- a/src/BulletCollision/CollisionDispatch/btCollisionObject.h
+++ b/src/BulletCollision/CollisionDispatch/btCollisionObject.h
@@ -101,28 +101,28 @@ public:
 	};


-	inline bool mergesSimulationIslands() const
+	SIMD_FORCE_INLINE bool mergesSimulationIslands() const
 	{
 		///static objects, kinematic and object without contact response don't merge islands
 		return  ((m_collisionFlags & (CF_STATIC_OBJECT | CF_KINEMATIC_OBJECT | CF_NO_CONTACT_RESPONSE) )==0);
 	}


-	inline bool		isStaticObject() const {
+	SIMD_FORCE_INLINE bool		isStaticObject() const {
 		return (m_collisionFlags & CF_STATIC_OBJECT) != 0;
 	}

-	inline bool		isKinematicObject() const
+	SIMD_FORCE_INLINE bool		isKinematicObject() const
 	{
 		return (m_collisionFlags & CF_KINEMATIC_OBJECT) != 0;
 	}

-	inline bool		isStaticOrKinematicObject() const
+	SIMD_FORCE_INLINE bool		isStaticOrKinematicObject() const
 	{
 		return (m_collisionFlags & (CF_KINEMATIC_OBJECT | CF_STATIC_OBJECT)) != 0 ;
 	}

-	inline bool		hasContactResponse() const {
+	SIMD_FORCE_INLINE bool		hasContactResponse() const {
 		return (m_collisionFlags & CF_NO_CONTACT_RESPONSE)==0;
 	}

@@ -136,12 +136,12 @@ public:
 		m_collisionShape = collisionShape;
 	}

-	inline const btCollisionShape*	getCollisionShape() const
+	SIMD_FORCE_INLINE const btCollisionShape*	getCollisionShape() const
 	{
 		return m_collisionShape;
 	}

-	inline btCollisionShape*	getCollisionShape()
+	SIMD_FORCE_INLINE btCollisionShape*	getCollisionShape()
 	{
 		return m_collisionShape;
 	}
--- a/src/BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.h
+++ b/src/BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.h
@@ -55,11 +55,11 @@ int	m_triangleCount;
 	
 	void clearCache();

-	inline const btVector3& getAabbMin() const
+	SIMD_FORCE_INLINE const btVector3& getAabbMin() const
 	{
 		return m_aabbMin;
 	}
-	inline const btVector3& getAabbMax() const
+	SIMD_FORCE_INLINE const btVector3& getAabbMax() const
 	{
 		return m_aabbMax;
 	}
--- a/src/BulletCollision/CollisionDispatch/btSphereBoxCollisionAlgorithm.cpp
+++ b/src/BulletCollision/CollisionDispatch/btSphereBoxCollisionAlgorithm.cpp
@@ -109,8 +109,8 @@ btScalar btSphereBoxCollisionAlgorithm::getSphereDistance(btCollisionObject* box
 	btVector3 bounds[2];
 	btBoxShape* boxShape= (btBoxShape*)boxObj->getCollisionShape();
 	
-	bounds[0] = -boxShape->getHalfExtents();
-	bounds[1] = boxShape->getHalfExtents();
+	bounds[0] = -boxShape->getHalfExtentsWithoutMargin();
+	bounds[1] = boxShape->getHalfExtentsWithoutMargin();

 	margins = boxShape->getMargin();//also add sphereShape margin?

--- a/src/BulletCollision/CollisionDispatch/btUnionFind.h
+++ b/src/BulletCollision/CollisionDispatch/btUnionFind.h
@@ -46,11 +46,11 @@ class btUnionFind

 	  void	reset(int N);

-	  inline int	getNumElements() const
+	  SIMD_FORCE_INLINE int	getNumElements() const
 	  {
 		  return int(m_elements.size());
 	  }
-	  inline bool  isRoot(int x) const
+	  SIMD_FORCE_INLINE bool  isRoot(int x) const
 	  {
 		  return (x == m_elements[x].m_id);
 	  }
--- a/src/BulletCollision/CollisionShapes/btBoxShape.cpp
+++ b/src/BulletCollision/CollisionShapes/btBoxShape.cpp
@@ -15,16 +15,13 @@ subject to the following restrictions:

 #include "btBoxShape.h"

-btVector3 btBoxShape::getHalfExtents() const
-{
-	return m_implicitShapeDimensions * m_localScaling;
-}
+
 //{ 


 void btBoxShape::getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const
 {
-	btVector3 halfExtents = getHalfExtents();
+	const btVector3& halfExtents = getHalfExtentsWithoutMargin();

 	btMatrix3x3 abs_b = t.getBasis().absolute();  
 	btPoint3 center = t.getOrigin();
@@ -43,7 +40,7 @@ void btBoxShape::getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabb
 void	btBoxShape::calculateLocalInertia(btScalar mass,btVector3& inertia)
 {
 	//btScalar margin = btScalar(0.);
-	btVector3 halfExtents = getHalfExtents();
+	btVector3 halfExtents = getHalfExtentsWithMargin();

 	btScalar lx=btScalar(2.)*(halfExtents.x());
 	btScalar ly=btScalar(2.)*(halfExtents.y());
--- a/src/BulletCollision/CollisionShapes/btBoxShape.h
+++ b/src/BulletCollision/CollisionShapes/btBoxShape.h
@@ -31,25 +31,37 @@ class btBoxShape: public btPolyhedralConvexShape

 public:

-	btVector3 getHalfExtents() const;
-		
+	btVector3 getHalfExtentsWithMargin() const
+	{
+		btVector3 halfExtents = getHalfExtentsWithoutMargin();
+		btVector3 margin(getMargin(),getMargin(),getMargin());
+		halfExtents += margin;
+		return halfExtents;
+	}
+	
+	const btVector3& getHalfExtentsWithoutMargin() const
+	{
+		return m_implicitShapeDimensions;//changed in Bullet 2.63: assume the scaling and margin are included
+	}
+	
+
 	virtual int	getShapeType() const { return BOX_SHAPE_PROXYTYPE;}

 	virtual btVector3	localGetSupportingVertex(const btVector3& vec) const
 	{
-		btVector3 halfExtents = getHalfExtents();
+		btVector3 halfExtents = getHalfExtentsWithoutMargin();
+		btVector3 margin(getMargin(),getMargin(),getMargin());
+		halfExtents += margin;
 		
 		return btVector3(btFsels(vec.x(), halfExtents.x(), -halfExtents.x()),
 			btFsels(vec.y(), halfExtents.y(), -halfExtents.y()),
 			btFsels(vec.z(), halfExtents.z(), -halfExtents.z()));
 	}

-	virtual inline btVector3	localGetSupportingVertexWithoutMargin(const btVector3& vec)const
+	SIMD_FORCE_INLINE  btVector3	localGetSupportingVertexWithoutMargin(const btVector3& vec)const
 	{
-		btVector3 halfExtents = getHalfExtents();
-		btVector3 margin(getMargin(),getMargin(),getMargin());
-		halfExtents -= margin;
-
+		const btVector3& halfExtents = getHalfExtentsWithoutMargin();
+		
 		return btVector3(btFsels(vec.x(), halfExtents.x(), -halfExtents.x()),
 			btFsels(vec.y(), halfExtents.y(), -halfExtents.y()),
 			btFsels(vec.z(), halfExtents.z(), -halfExtents.z()));
@@ -57,11 +69,8 @@ public:

 	virtual void	batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const
 	{
-		btVector3 halfExtents = getHalfExtents();
-		btVector3 margin(getMargin(),getMargin(),getMargin());
-		halfExtents -= margin;
-
-
+		const btVector3& halfExtents = getHalfExtentsWithoutMargin();
+	
 		for (int i=0;i<numVectors;i++)
 		{
 			const btVector3& vec = vectors[i];
@@ -75,9 +84,33 @@ public:

 	btBoxShape( const btVector3& boxHalfExtents)
 	{
-		m_implicitShapeDimensions = boxHalfExtents;
+		btVector3 margin(getMargin(),getMargin(),getMargin());
+		m_implicitShapeDimensions = (boxHalfExtents * m_localScaling) - margin;
 	};
-	
+
+	virtual void setMargin(btScalar collisionMargin)
+	{
+		//correct the m_implicitShapeDimensions for the margin
+		btVector3 oldMargin(getMargin(),getMargin(),getMargin());
+		btVector3 implicitShapeDimensionsWithMargin = m_implicitShapeDimensions+oldMargin;
+		
+		btConvexInternalShape::setMargin(collisionMargin);
+		btVector3 newMargin(getMargin(),getMargin(),getMargin());
+		m_implicitShapeDimensions = implicitShapeDimensionsWithMargin - newMargin;
+
+	}
+	virtual void	setLocalScaling(const btVector3& scaling)
+	{
+		btVector3 oldMargin(getMargin(),getMargin(),getMargin());
+		btVector3 implicitShapeDimensionsWithMargin = m_implicitShapeDimensions+oldMargin;
+		btVector3 unScaledImplicitShapeDimensionsWithMargin = implicitShapeDimensionsWithMargin / m_localScaling;
+
+		btConvexInternalShape::setLocalScaling(scaling);
+
+		m_implicitShapeDimensions = (unScaledImplicitShapeDimensionsWithMargin * m_localScaling) - oldMargin;
+
+	}
+
 	virtual void getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const;

 	
@@ -112,7 +145,7 @@ public:

 	virtual void getVertex(int i,btVector3& vtx) const
 	{
-		btVector3 halfExtents = getHalfExtents();
+		btVector3 halfExtents = getHalfExtentsWithoutMargin();

 		vtx = btVector3(
 				halfExtents.x() * (1-(i&1)) - halfExtents.x() * (i&1),
@@ -123,7 +156,7 @@ public:

 	virtual void	getPlaneEquation(btVector4& plane,int i) const
 	{
-		btVector3 halfExtents = getHalfExtents();
+		btVector3 halfExtents = getHalfExtentsWithoutMargin();

 		switch (i)
 		{
@@ -230,7 +263,7 @@ public:
 	
 	virtual	bool isInside(const btPoint3& pt,btScalar tolerance) const
 	{
-		btVector3 halfExtents = getHalfExtents();
+		btVector3 halfExtents = getHalfExtentsWithoutMargin();

 		//btScalar minDist = 2*tolerance;
 		
--- a/src/BulletCollision/CollisionShapes/btCollisionShape.h
+++ b/src/BulletCollision/CollisionShapes/btCollisionShape.h
@@ -49,26 +49,26 @@ public:

 #ifndef __SPU__

-	inline bool	isPolyhedral() const
+	SIMD_FORCE_INLINE bool	isPolyhedral() const
 	{
 		return btBroadphaseProxy::isPolyhedral(getShapeType());
 	}

-	inline bool	isConvex() const
+	SIMD_FORCE_INLINE bool	isConvex() const
 	{
 		return btBroadphaseProxy::isConvex(getShapeType());
 	}
-	inline bool	isConcave() const
+	SIMD_FORCE_INLINE bool	isConcave() const
 	{
 		return btBroadphaseProxy::isConcave(getShapeType());
 	}
-	inline bool	isCompound() const
+	SIMD_FORCE_INLINE bool	isCompound() const
 	{
 		return btBroadphaseProxy::isCompound(getShapeType());
 	}

 	///isInfinite is used to catch simulation error (aabb check)
-	inline bool isInfinite() const
+	SIMD_FORCE_INLINE bool isInfinite() const
 	{
 		return btBroadphaseProxy::isInfinite(getShapeType());
 	}
--- a/src/BulletCollision/CollisionShapes/btCylinderShape.cpp
+++ b/src/BulletCollision/CollisionShapes/btCylinderShape.cpp
@@ -45,7 +45,7 @@ void btCylinderShape::getAabb(const btTransform& t,btVector3& aabbMin,btVector3&
 }


-inline btVector3 CylinderLocalSupportX(const btVector3& halfExtents,const btVector3& v) 
+SIMD_FORCE_INLINE btVector3 CylinderLocalSupportX(const btVector3& halfExtents,const btVector3& v) 
 {
 const int cylinderUpAxis = 0;
 const int XX = 1;
@@ -163,24 +163,24 @@ const int ZZ = 1;

 btVector3	btCylinderShapeX::localGetSupportingVertexWithoutMargin(const btVector3& vec)const
 {
-	return CylinderLocalSupportX(getHalfExtents(),vec);
+	return CylinderLocalSupportX(getHalfExtentsWithoutMargin(),vec);
 }


 btVector3	btCylinderShapeZ::localGetSupportingVertexWithoutMargin(const btVector3& vec)const
 {
-	return CylinderLocalSupportZ(getHalfExtents(),vec);
+	return CylinderLocalSupportZ(getHalfExtentsWithoutMargin(),vec);
 }
 btVector3	btCylinderShape::localGetSupportingVertexWithoutMargin(const btVector3& vec)const
 {
-	return CylinderLocalSupportY(getHalfExtents(),vec);
+	return CylinderLocalSupportY(getHalfExtentsWithoutMargin(),vec);
 }

 void	btCylinderShape::batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const
 {
 	for (int i=0;i<numVectors;i++)
 	{
-		supportVerticesOut[i] = CylinderLocalSupportY(getHalfExtents(),vectors[i]);
+		supportVerticesOut[i] = CylinderLocalSupportY(getHalfExtentsWithoutMargin(),vectors[i]);
 	}
 }

@@ -188,7 +188,7 @@ void	btCylinderShapeZ::batchedUnitVectorGetSupportingVertexWithoutMargin(const b
 {
 	for (int i=0;i<numVectors;i++)
 	{
-		supportVerticesOut[i] = CylinderLocalSupportZ(getHalfExtents(),vectors[i]);
+		supportVerticesOut[i] = CylinderLocalSupportZ(getHalfExtentsWithoutMargin(),vectors[i]);
 	}
 }

@@ -199,7 +199,7 @@ void	btCylinderShapeX::batchedUnitVectorGetSupportingVertexWithoutMargin(const b
 {
 	for (int i=0;i<numVectors;i++)
 	{
-		supportVerticesOut[i] = CylinderLocalSupportX(getHalfExtents(),vectors[i]);
+		supportVerticesOut[i] = CylinderLocalSupportX(getHalfExtentsWithoutMargin(),vectors[i]);
 	}
 }

--- a/src/BulletCollision/CollisionShapes/btCylinderShape.h
+++ b/src/BulletCollision/CollisionShapes/btCylinderShape.h
@@ -74,7 +74,7 @@ public:

 	virtual btScalar getRadius() const
 	{
-		return getHalfExtents().getX();
+		return getHalfExtentsWithMargin().getX();
 	}

 	//debugging
@@ -103,7 +103,7 @@ public:

 	virtual btScalar getRadius() const
 	{
-		return getHalfExtents().getY();
+		return getHalfExtentsWithMargin().getY();
 	}

 };
@@ -128,7 +128,7 @@ public:

 	virtual btScalar getRadius() const
 	{
-		return getHalfExtents().getX();
+		return getHalfExtentsWithMargin().getX();
 	}

 };
--- a/src/BulletCollision/CollisionShapes/btOptimizedBvh.h
+++ b/src/BulletCollision/CollisionShapes/btOptimizedBvh.h
@@ -285,7 +285,7 @@ protected:
 #define USE_BANCHLESS 1
 #ifdef USE_BANCHLESS
 	//This block replaces the block below and uses no branches, and replaces the 8 bit return with a 32 bit return for improved performance (~3x on XBox 360)
-	inline unsigned testQuantizedAabbAgainstQuantizedAabb(unsigned short int* aabbMin1,unsigned short int* aabbMax1,const unsigned short int* aabbMin2,const unsigned short int* aabbMax2) const
+	SIMD_FORCE_INLINE unsigned testQuantizedAabbAgainstQuantizedAabb(unsigned short int* aabbMin1,unsigned short int* aabbMax1,const unsigned short int* aabbMin2,const unsigned short int* aabbMax2) const
 	{		
 		return btSelect((unsigned)((aabbMin1[0] <= aabbMax2[0]) & (aabbMax1[0] >= aabbMin2[0])
 			& (aabbMin1[2] <= aabbMax2[2]) & (aabbMax1[2] >= aabbMin2[2])
@@ -293,7 +293,7 @@ protected:
 			1, 0);
 	}
 #else
-	inline bool testQuantizedAabbAgainstQuantizedAabb(unsigned short int* aabbMin1,unsigned short int* aabbMax1,const unsigned short int* aabbMin2,const unsigned short int* aabbMax2) const
+	SIMD_FORCE_INLINE bool testQuantizedAabbAgainstQuantizedAabb(unsigned short int* aabbMin1,unsigned short int* aabbMax1,const unsigned short int* aabbMin2,const unsigned short int* aabbMax2) const
 	{
 		bool overlap = true;
 		overlap = (aabbMin1[0] > aabbMax2[0] || aabbMax1[0] < aabbMin2[0]) ? false : overlap;
@@ -316,7 +316,7 @@ public:

 	void	reportSphereOverlappingNodex(btNodeOverlapCallback* nodeCallback,const btVector3& aabbMin,const btVector3& aabbMax) const;
 	
-	inline void quantizeWithClamp(unsigned short* out, const btVector3& point) const
+	SIMD_FORCE_INLINE void quantizeWithClamp(unsigned short* out, const btVector3& point) const
 	{

 		btAssert(m_useQuantization);
@@ -347,12 +347,12 @@ public:
 	void	updateBvhNodes(btStridingMeshInterface* meshInterface,int firstNode,int endNode,int index);


-	inline QuantizedNodeArray&	getQuantizedNodeArray()
+	SIMD_FORCE_INLINE QuantizedNodeArray&	getQuantizedNodeArray()
 	{	
 		return	m_quantizedContiguousNodes;
 	}

-	inline BvhSubtreeInfoArray&	getSubtreeInfoArray()
+	SIMD_FORCE_INLINE BvhSubtreeInfoArray&	getSubtreeInfoArray()
 	{
 		return m_SubtreeHeaders;
 	}
@@ -368,7 +368,7 @@ public:

 	static unsigned int getAlignmentSerializationPadding();

-	inline bool isQuantized()
+	SIMD_FORCE_INLINE bool isQuantized()
 	{
 		return m_useQuantization;
 	}
--- a/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp
+++ b/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp
@@ -158,64 +158,62 @@ btScalar restitutionCurve(btScalar rel_vel, btScalar restitution)

 //velocity + friction
 //response  between two dynamic objects with friction
-SIMD_FORCE_INLINE btScalar resolveSingleCollisionCombinedCacheFriendly(
+//SIMD_FORCE_INLINE 
+btScalar resolveSingleCollisionCombinedCacheFriendly(
 	btSolverBody& body1,
 	btSolverBody& body2,
-	btSolverConstraint& contactConstraint,
+	const btSolverConstraint& contactConstraint,
 	const btContactSolverInfo& solverInfo)
 {
 	(void)solverInfo;

-	btScalar normalImpulse(0.f);
-	{
-		if (contactConstraint.m_penetration < 0.f)
-			return 0.f;
-
+	btScalar normalImpulse;
+	
 	//  Optimized version of projected relative velocity, use precomputed cross products with normal
 	//	body1.getVelocityInLocalPoint(contactConstraint.m_rel_posA,vel1);
 	//	body2.getVelocityInLocalPoint(contactConstraint.m_rel_posB,vel2);
 	//	btVector3 vel = vel1 - vel2;
 	//	btScalar  rel_vel = contactConstraint.m_contactNormal.dot(vel);

-		btScalar rel_vel;
-		btScalar vel1Dotn = contactConstraint.m_contactNormal.dot(body1.m_linearVelocity) 
-					+ contactConstraint.m_relpos1CrossNormal.dot(body1.m_angularVelocity);
-		btScalar vel2Dotn = contactConstraint.m_contactNormal.dot(body2.m_linearVelocity) 
-					+ contactConstraint.m_relpos2CrossNormal.dot(body2.m_angularVelocity);
+	btScalar rel_vel;
+	btScalar vel1Dotn = contactConstraint.m_contactNormal.dot(body1.m_linearVelocity) 
+				+ contactConstraint.m_relpos1CrossNormal.dot(body1.m_angularVelocity);
+	btScalar vel2Dotn = contactConstraint.m_contactNormal.dot(body2.m_linearVelocity) 
+				+ contactConstraint.m_relpos2CrossNormal.dot(body2.m_angularVelocity);

-		rel_vel = vel1Dotn-vel2Dotn;
+	rel_vel = vel1Dotn-vel2Dotn;


-		btScalar positionalError = contactConstraint.m_penetration;
-		btScalar velocityError = contactConstraint.m_restitution - rel_vel;// * damping;
+	btScalar positionalError = contactConstraint.m_penetration;
+	btScalar velocityError = contactConstraint.m_restitution - rel_vel;// * damping;

-		btScalar penetrationImpulse = positionalError * contactConstraint.m_jacDiagABInv;
-		btScalar	velocityImpulse = velocityError * contactConstraint.m_jacDiagABInv;
-		btScalar normalImpulse = penetrationImpulse+velocityImpulse;
-		
-		// See Erin Catto's GDC 2006 paper: Clamp the accumulated impulse
-		btScalar oldNormalImpulse = contactConstraint.m_appliedImpulse;
-		btScalar sum = oldNormalImpulse + normalImpulse;
-		contactConstraint.m_appliedImpulse = btScalar(0.) > sum ? btScalar(0.): sum;
+	btScalar penetrationImpulse = positionalError * contactConstraint.m_jacDiagABInv;
+	btScalar	velocityImpulse = velocityError * contactConstraint.m_jacDiagABInv;
+	normalImpulse = penetrationImpulse+velocityImpulse;
+	
+	// See Erin Catto's GDC 2006 paper: Clamp the accumulated impulse
+	btScalar oldNormalImpulse = contactConstraint.m_appliedImpulse;
+	btScalar sum = oldNormalImpulse + normalImpulse;
+	contactConstraint.m_appliedImpulse = btScalar(0.) > sum ? btScalar(0.): sum;

-		btScalar oldVelocityImpulse = contactConstraint.m_appliedVelocityImpulse;
-		btScalar velocitySum = oldVelocityImpulse + velocityImpulse;
-		contactConstraint.m_appliedVelocityImpulse = btScalar(0.) > velocitySum ? btScalar(0.): velocitySum;
+	btScalar oldVelocityImpulse = contactConstraint.m_appliedVelocityImpulse;
+	btScalar velocitySum = oldVelocityImpulse + velocityImpulse;
+	contactConstraint.m_appliedVelocityImpulse = btScalar(0.) > velocitySum ? btScalar(0.): velocitySum;

-		normalImpulse = contactConstraint.m_appliedImpulse - oldNormalImpulse;
-
-		if (body1.m_invMass)
-		{
-			body1.internalApplyImpulse(contactConstraint.m_contactNormal*body1.m_invMass,
-				contactConstraint.m_angularComponentA,normalImpulse);
-		}
-		if (body2.m_invMass)
-		{
-			body2.internalApplyImpulse(contactConstraint.m_contactNormal*body2.m_invMass,
-				contactConstraint.m_angularComponentB,-normalImpulse);
-		}
+	normalImpulse = contactConstraint.m_appliedImpulse - oldNormalImpulse;

+	if (body1.m_invMass)
+	{
+		body1.internalApplyImpulse(contactConstraint.m_contactNormal*body1.m_invMass,
+			contactConstraint.m_angularComponentA,normalImpulse);
 	}
+	if (body2.m_invMass)
+	{
+		body2.internalApplyImpulse(contactConstraint.m_contactNormal*body2.m_invMass,
+			contactConstraint.m_angularComponentB,-normalImpulse);
+	}
+
+

 	

@@ -225,10 +223,11 @@ SIMD_FORCE_INLINE btScalar resolveSingleCollisionCombinedCacheFriendly(

 #ifndef NO_FRICTION_TANGENTIALS

-SIMD_FORCE_INLINE btScalar resolveSingleFrictionCacheFriendly(
+//SIMD_FORCE_INLINE 
+btScalar resolveSingleFrictionCacheFriendly(
 	btSolverBody& body1,
 	btSolverBody& body2,
-	btSolverConstraint& contactConstraint,
+	const btSolverConstraint& contactConstraint,
 	const btContactSolverInfo& solverInfo,
 	btScalar appliedNormalImpulse)
 {
@@ -255,11 +254,36 @@ SIMD_FORCE_INLINE btScalar resolveSingleFrictionCacheFriendly(

 			// calculate j that moves us to zero relative velocity
 			j1 = -rel_vel * contactConstraint.m_jacDiagABInv;
+#define CLAMP_ACCUMULATED_FRICTION_IMPULSE 1
+#ifdef CLAMP_ACCUMULATED_FRICTION_IMPULSE
 			btScalar oldTangentImpulse = contactConstraint.m_appliedImpulse;
 			contactConstraint.m_appliedImpulse = oldTangentImpulse + j1;
-			GEN_set_min(contactConstraint.m_appliedImpulse, limit);
-			GEN_set_max(contactConstraint.m_appliedImpulse, -limit);
+			
+			if (limit < contactConstraint.m_appliedImpulse)
+			{
+				contactConstraint.m_appliedImpulse = limit;
+			} else
+			{
+				if (contactConstraint.m_appliedImpulse < -limit)
+					contactConstraint.m_appliedImpulse = -limit;
+			}
 			j1 = contactConstraint.m_appliedImpulse - oldTangentImpulse;
+#else
+			if (limit < j1)
+			{
+				j1 = limit;
+			} else
+			{
+				if (j1 < -limit)
+					j1 = -limit;
+			}
+
+#endif //CLAMP_ACCUMULATED_FRICTION_IMPULSE
+
+			//GEN_set_min(contactConstraint.m_appliedImpulse, limit);
+			//GEN_set_max(contactConstraint.m_appliedImpulse, -limit);
+
+			

 		}
 	
@@ -446,6 +470,8 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol

 		tmpSolverBodyPool.reserve(minReservation);

+		//don't convert all bodies, only the one we need so solver the constraints
+/*
 		{
 			for (int i=0;i<numBodies;i++)
 			{
@@ -460,7 +486,7 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol
 				} 
 			}
 		}
-
+*/
 		
 		tmpSolverConstraintPool.reserve(minReservation);
 		tmpSolverFrictionConstraintPool.reserve(minReservation);
@@ -485,7 +511,17 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol

 					if (rb0->getIslandTag() >= 0)
 					{
-						solverBodyIdA = rb0->getCompanionId();
+						if (rb0->getCompanionId() >= 0)
+						{
+							//body has already been converted
+							solverBodyIdA = rb0->getCompanionId();
+						} else
+						{
+							solverBodyIdA = tmpSolverBodyPool.size();
+							btSolverBody& solverBody = tmpSolverBodyPool.expand();
+							initSolverBody(&solverBody,rb0);
+							rb0->setCompanionId(solverBodyIdA);
+						}
 					} else
 					{
 						//create a static body
@@ -496,7 +532,16 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol

 					if (rb1->getIslandTag() >= 0)
 					{
-						solverBodyIdB = rb1->getCompanionId();
+						if (rb1->getCompanionId() >= 0)
+						{
+							solverBodyIdB = rb1->getCompanionId();
+						} else
+						{
+							solverBodyIdB = tmpSolverBodyPool.size();
+							btSolverBody& solverBody = tmpSolverBodyPool.expand();
+							initSolverBody(&solverBody,rb1);
+							rb1->setCompanionId(solverBodyIdB);
+						}
 					} else
 					{
 						//create a static body
@@ -729,7 +774,7 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyIterations(
 				int numPoolConstraints = tmpSolverConstraintPool.size();
 				for (j=0;j<numPoolConstraints;j++)
 				{
-					btSolverConstraint& solveManifold = tmpSolverConstraintPool[gOrderTmpConstraintPool[j]];
+					const btSolverConstraint& solveManifold = tmpSolverConstraintPool[gOrderTmpConstraintPool[j]];
 					resolveSingleCollisionCombinedCacheFriendly(tmpSolverBodyPool[solveManifold.m_solverBodyIdA],
 						tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold,infoGlobal);
 				}
@@ -737,13 +782,15 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyIterations(

 			{
 				 int numFrictionPoolConstraints = tmpSolverFrictionConstraintPool.size();
-				for (j=0;j<numFrictionPoolConstraints;j++)
+				
+				 for (j=0;j<numFrictionPoolConstraints;j++)
 				{
-					btSolverConstraint& solveManifold = tmpSolverFrictionConstraintPool[gOrderFrictionConstraintPool[j]];
-						btScalar appliedNormalImpulse = tmpSolverConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse;
+					const btSolverConstraint& solveManifold = tmpSolverFrictionConstraintPool[gOrderFrictionConstraintPool[j]];
+						

 						resolveSingleFrictionCacheFriendly(tmpSolverBodyPool[solveManifold.m_solverBodyIdA],
-							tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold,infoGlobal,appliedNormalImpulse);
+							tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold,infoGlobal,
+							tmpSolverConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse);
 				}
 			}
 			
--- a/src/BulletDynamics/ConstraintSolver/btSolverBody.h
+++ b/src/BulletDynamics/ConstraintSolver/btSolverBody.h
@@ -27,22 +27,22 @@ class	btRigidBody;
 ATTRIBUTE_ALIGNED16 (struct)	btSolverBody
 {
 	BT_DECLARE_ALIGNED_ALLOCATOR();
-
-	btVector3		m_centerOfMassPosition;
-	btVector3		m_linearVelocity;
+	
 	btVector3		m_angularVelocity;
-	btRigidBody*	m_originalBody;
+	float			m_angularFactor;
 	float			m_invMass;
 	float			m_friction;
-	float			m_angularFactor;
-
-	inline void	getVelocityInLocalPoint(const btVector3& rel_pos, btVector3& velocity ) const
+	btRigidBody*	m_originalBody;
+	btVector3		m_linearVelocity;
+	btVector3		m_centerOfMassPosition;
+	
+	SIMD_FORCE_INLINE void	getVelocityInLocalPoint(const btVector3& rel_pos, btVector3& velocity ) const
 	{
 		velocity = m_linearVelocity + m_angularVelocity.cross(rel_pos);
 	}

 	//Optimization for the iterative solver: avoid calculating constant terms involving inertia, normal, relative position
-	inline void internalApplyImpulse(const btVector3& linearComponent, const btVector3& angularComponent,btScalar impulseMagnitude)
+	SIMD_FORCE_INLINE void internalApplyImpulse(const btVector3& linearComponent, const btVector3& angularComponent,btScalar impulseMagnitude)
 	{
 		m_linearVelocity += linearComponent*impulseMagnitude;
 		m_angularVelocity += angularComponent*(impulseMagnitude*m_angularFactor);
@@ -54,6 +54,7 @@ ATTRIBUTE_ALIGNED16 (struct)	btSolverBody
 		{
 			m_originalBody->setLinearVelocity(m_linearVelocity);
 			m_originalBody->setAngularVelocity(m_angularVelocity);
+			//m_originalBody->setCompanionId(-1);
 		}
 	}

--- a/src/BulletDynamics/ConstraintSolver/btSolverConstraint.h
+++ b/src/BulletDynamics/ConstraintSolver/btSolverConstraint.h
@@ -30,20 +30,24 @@ ATTRIBUTE_ALIGNED16 (struct)	btSolverConstraint
 	BT_DECLARE_ALIGNED_ALLOCATOR();

 	btVector3	m_relpos1CrossNormal;
-	btVector3	m_relpos2CrossNormal;
 	btVector3	m_contactNormal;
-	btVector3	m_angularComponentA;
-	btVector3	m_angularComponentB;

-	btScalar	m_appliedVelocityImpulse;
+	btVector3	m_relpos2CrossNormal;
+	btVector3	m_angularComponentA;
+
+	btVector3	m_angularComponentB;
+	mutable btScalar	m_appliedVelocityImpulse;
+	mutable btScalar	m_appliedImpulse;
 	int			m_solverBodyIdA;
 	int			m_solverBodyIdB;
+	
 	btScalar	m_friction;
 	btScalar	m_restitution;
 	btScalar	m_jacDiagABInv;
 	btScalar	m_penetration;
-	btScalar	m_appliedImpulse;
+	

+	
 	int			m_constraintType;
 	int			m_frictionIndex;
 	int			m_unusedPadding[2];
--- a/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp
+++ b/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp
@@ -412,7 +412,7 @@ void	btDiscreteDynamicsWorld::removeVehicle(btRaycastVehicle* vehicle)
 	m_vehicles.remove(vehicle);
 }

-inline	int	btGetConstraintIslandId(const btTypedConstraint* lhs)
+SIMD_FORCE_INLINE	int	btGetConstraintIslandId(const btTypedConstraint* lhs)
 {
 	int islandId;
 	
@@ -866,10 +866,24 @@ void btDiscreteDynamicsWorld::debugDrawObject(const btTransform& worldTransform,
 				btScalar radius = coneShape->getRadius();//+coneShape->getMargin();
 				btScalar height = coneShape->getHeight();//+coneShape->getMargin();
 				btVector3 start = worldTransform.getOrigin();
-				getDebugDrawer()->drawLine(start+worldTransform.getBasis() * btVector3(btScalar(0.),btScalar(0.),btScalar(0.5)*height),start+worldTransform.getBasis() * btVector3(radius,btScalar(0.),btScalar(-0.5)*height),color);
-				getDebugDrawer()->drawLine(start+worldTransform.getBasis() * btVector3(btScalar(0.),btScalar(0.),btScalar(0.5)*height),start+worldTransform.getBasis() * btVector3(-radius,btScalar(0.),btScalar(-0.5)*height),color);
-				getDebugDrawer()->drawLine(start+worldTransform.getBasis() * btVector3(btScalar(0.),btScalar(0.),btScalar(0.5)*height),start+worldTransform.getBasis() * btVector3(btScalar(0.),radius,btScalar(-0.5)*height),color);
-				getDebugDrawer()->drawLine(start+worldTransform.getBasis() * btVector3(btScalar(0.),btScalar(0.),btScalar(0.5)*height),start+worldTransform.getBasis() * btVector3(btScalar(0.),-radius,btScalar(-0.5)*height),color);
+
+				int upAxis= coneShape->getConeUpIndex();
+				
+
+				btVector3	offsetHeight(0,0,0);
+				offsetHeight[upAxis] = height * btScalar(0.5);
+				btVector3	offsetRadius(0,0,0);
+				offsetRadius[(upAxis+1)%3] = radius;
+				btVector3	offset2Radius(0,0,0);
+				offset2Radius[(upAxis+2)%3] = radius;
+
+				getDebugDrawer()->drawLine(start+worldTransform.getBasis() * (offsetHeight),start+worldTransform.getBasis() * (-offsetHeight+offsetRadius),color);
+				getDebugDrawer()->drawLine(start+worldTransform.getBasis() * (offsetHeight),start+worldTransform.getBasis() * (-offsetHeight-offsetRadius),color);
+				getDebugDrawer()->drawLine(start+worldTransform.getBasis() * (offsetHeight),start+worldTransform.getBasis() * (-offsetHeight+offset2Radius),color);
+				getDebugDrawer()->drawLine(start+worldTransform.getBasis() * (offsetHeight),start+worldTransform.getBasis() * (-offsetHeight-offset2Radius),color);
+
+
+
 				break;

 			}
@@ -878,7 +892,7 @@ void btDiscreteDynamicsWorld::debugDrawObject(const btTransform& worldTransform,
 				const btCylinderShape* cylinder = static_cast<const btCylinderShape*>(shape);
 				int upAxis = cylinder->getUpAxis();
 				btScalar radius = cylinder->getRadius();
-				btScalar halfHeight = cylinder->getHalfExtents()[upAxis];
+				btScalar halfHeight = cylinder->getHalfExtentsWithMargin()[upAxis];
 				btVector3 start = worldTransform.getOrigin();
 				btVector3	offsetHeight(0,0,0);
 				offsetHeight[upAxis] = halfHeight;
--- a/src/BulletDynamics/Dynamics/btRigidBody.h
+++ b/src/BulletDynamics/Dynamics/btRigidBody.h
@@ -111,11 +111,11 @@ public:

 	void			setDamping(btScalar lin_damping, btScalar ang_damping);
 	
-	inline const btCollisionShape*	getCollisionShape() const {
+	SIMD_FORCE_INLINE const btCollisionShape*	getCollisionShape() const {
 		return m_collisionShape;
 	}

-	inline btCollisionShape*	getCollisionShape() {
+	SIMD_FORCE_INLINE btCollisionShape*	getCollisionShape() {
 			return m_collisionShape;
 	}
 	
@@ -185,7 +185,7 @@ public:
 	}

 	//Optimization for the iterative solver: avoid calculating constant terms involving inertia, normal, relative position
-	inline void internalApplyImpulse(const btVector3& linearComponent, const btVector3& angularComponent,btScalar impulseMagnitude)
+	SIMD_FORCE_INLINE void internalApplyImpulse(const btVector3& linearComponent, const btVector3& angularComponent,btScalar impulseMagnitude)
 	{
 		if (m_inverseMass != btScalar(0.))
 		{
@@ -255,7 +255,7 @@ public:


 	
-	inline btScalar computeImpulseDenominator(const btPoint3& pos, const btVector3& normal) const
+	SIMD_FORCE_INLINE btScalar computeImpulseDenominator(const btPoint3& pos, const btVector3& normal) const
 	{
 		btVector3 r0 = pos - getCenterOfMassPosition();

@@ -267,13 +267,13 @@ public:

 	}

-	inline btScalar computeAngularImpulseDenominator(const btVector3& axis) const
+	SIMD_FORCE_INLINE btScalar computeAngularImpulseDenominator(const btVector3& axis) const
 	{
 		btVector3 vec = axis * getInvInertiaTensorWorld();
 		return axis.dot(vec);
 	}

-	inline void	updateDeactivation(btScalar timeStep)
+	SIMD_FORCE_INLINE void	updateDeactivation(btScalar timeStep)
 	{
 		if ( (getActivationState() == ISLAND_SLEEPING) || (getActivationState() == DISABLE_DEACTIVATION))
 			return;
@@ -290,7 +290,7 @@ public:

 	}

-	inline bool	wantsSleeping()
+	SIMD_FORCE_INLINE bool	wantsSleeping()
 	{

 		if (getActivationState() == DISABLE_DEACTIVATION)
--- a/src/LinearMath/btAlignedAllocator.cpp
+++ b/src/LinearMath/btAlignedAllocator.cpp
@@ -16,7 +16,7 @@ subject to the following restrictions:
 #include "btAlignedAllocator.h"


-#if defined (BT_HAS_ALIGNED_ALOCATOR)
+#if defined (BT_HAS_ALIGNED_ALLOCATOR)

 #include <malloc.h>
 void*	btAlignedAlloc	(size_t size, int alignment)
--- a/src/LinearMath/btScalar.h
+++ b/src/LinearMath/btScalar.h
@@ -33,7 +33,7 @@ subject to the following restrictions:
 			#define ATTRIBUTE_ALIGNED16(a) a
 			#define ATTRIBUTE_ALIGNED128(a) a
 		#else
-			#define BT_HAS_ALIGNED_ALOCATOR
+			#define BT_HAS_ALIGNED_ALLOCATOR
 			#pragma warning(disable:4530)
 			#pragma warning(disable:4996)
 			#pragma warning(disable:4786)
@@ -288,7 +288,7 @@ SIMD_FORCE_INLINE float btSelect(unsigned condition, float valueIfConditionNonZe
 #endif
 }

-template<typename T> inline void btSwap(T& a, T& b)
+template<typename T> SIMD_FORCE_INLINE void btSwap(T& a, T& b)
 {
 	T tmp = a;
 	a = b;
--- a/src/LinearMath/btStackAlloc.h
+++ b/src/LinearMath/btStackAlloc.h
@@ -77,7 +77,7 @@ public:
 		
 		return(0);
 	}
-	inline btBlock*		beginBlock()
+	SIMD_FORCE_INLINE btBlock*		beginBlock()
 	{
 		btBlock*	pb = (btBlock*)allocate(sizeof(btBlock));
 		pb->previous	=	current;
@@ -85,7 +85,7 @@ public:
 		current			=	pb;
 		return(pb);
 	}
-	inline void		endBlock(btBlock* block)
+	SIMD_FORCE_INLINE void		endBlock(btBlock* block)
 	{
 		btAssert(block==current);
 		//Raise(L"Unmatched blocks");
--- a/src/LinearMath/btTransformUtil.h
+++ b/src/LinearMath/btTransformUtil.h
@@ -25,7 +25,7 @@ subject to the following restrictions:

 #define btRecipSqrt(x) ((btScalar)(btScalar(1.0)/btSqrt(btScalar(x))))		/* reciprocal square root */

-inline btVector3 btAabbSupport(const btVector3& halfExtents,const btVector3& supportDir)
+SIMD_FORCE_INLINE btVector3 btAabbSupport(const btVector3& halfExtents,const btVector3& supportDir)
 {
 	return btVector3(supportDir.x() < btScalar(0.0) ? -halfExtents.x() : halfExtents.x(),
      supportDir.y() < btScalar(0.0) ? -halfExtents.y() : halfExtents.y(),
@@ -33,7 +33,7 @@ inline btVector3 btAabbSupport(const btVector3& halfExtents,const btVector3& sup
 }


-inline void btPlaneSpace1 (const btVector3& n, btVector3& p, btVector3& q)
+SIMD_FORCE_INLINE void btPlaneSpace1 (const btVector3& n, btVector3& p, btVector3& q)
 {
  if (btFabs(n.z()) > SIMDSQRT12) {
    // choose p in y-z plane