prepare and added constraint solver optimizations, not activated yet.

src/BulletCollision/CollisionDispatch/btCollisionObject.h

@@ -54,6 +54,8 @@ protected:
 	int				m_collisionFlags;
 
 	int				m_islandTag1;
+	int				m_companionId;
+
 	int				m_activationState1;
 	btScalar			m_deactivationTime;
 
@@ -251,6 +253,16 @@ public:
 		m_islandTag1 = tag;
 	}
 
+	const int getCompanionId() const
+	{
+		return	m_companionId;
+	}
+
+	void	setCompanionId(int id)
+	{
+		m_companionId = id;
+	}
+
 	const btScalar			getHitFraction() const
 	{
 		return m_hitFraction; 

src/BulletCollision/CollisionDispatch/btManifoldResult.cpp

@@ -63,15 +63,22 @@ void btManifoldResult::addContactPoint(const btVector3& normalOnBInWorld,const b
 
 	bool isSwapped = m_manifoldPtr->getBody0() != m_body0;
 
-	btTransform transAInv = isSwapped? m_rootTransB.inverse() : m_rootTransA.inverse();
-	btTransform transBInv = isSwapped? m_rootTransA.inverse() : m_rootTransB.inverse();
-
 	btVector3 pointA = pointInWorld + normalOnBInWorld * depth;
-	btVector3 localA = transAInv(pointA );
-	btVector3 localB = transBInv(pointInWorld);
-	btManifoldPoint newPt(localA,localB,normalOnBInWorld,depth);
 
+	btVector3 localA;
+	btVector3 localB;
 	
+	if (isSwapped)
+	{
+		localA = m_rootTransB.invXform(pointA );
+		localB = m_rootTransA.invXform(pointInWorld);
+	} else
+	{
+		localA = m_rootTransA.invXform(pointA );
+		localB = m_rootTransB.invXform(pointInWorld);
+	}
+
+	btManifoldPoint newPt(localA,localB,normalOnBInWorld,depth);
 
 	int insertIndex = m_manifoldPtr->getCacheEntry(newPt);
 

src/BulletCollision/CollisionDispatch/btSimulationIslandManager.cpp

@@ -8,7 +8,7 @@
 #include "BulletCollision/CollisionDispatch/btCollisionWorld.h"
 
 #include <stdio.h>
-
+#include "LinearMath/btQuickprof.h"
 
 btSimulationIslandManager::btSimulationIslandManager()
 {
@@ -63,6 +63,7 @@ void	btSimulationIslandManager::updateActivationState(btCollisionWorld* colWorld
 		{
 			btCollisionObject*	collisionObject= colWorld->getCollisionObjectArray()[i];
 			collisionObject->setIslandTag(index);
+			collisionObject->setCompanionId(-1);
 			collisionObject->setHitFraction(btScalar(1.));
 			index++;
 			
@@ -93,9 +94,11 @@ void	btSimulationIslandManager::storeIslandActivationState(btCollisionWorld* col
 			if (collisionObject->mergesSimulationIslands())
 			{
 				collisionObject->setIslandTag( m_unionFind.find(index) );
+				collisionObject->setCompanionId(-1);
 			} else
 			{
 				collisionObject->setIslandTag(-1);
+				collisionObject->setCompanionId(-2);
 			}
 			index++;
 		}
@@ -137,6 +140,11 @@ class btPersistentManifoldSortPredicate
 //
 void btSimulationIslandManager::buildAndProcessIslands(btDispatcher* dispatcher,btCollisionObjectArray& collisionObjects, IslandCallback* callback)
 {
+
+	
+	BEGIN_PROFILE("islandUnionFindAndHeapSort");
+
+	
 	//we are going to sort the unionfind array, and store the element id in the size
 	//afterwards, we clean unionfind, to make sure no-one uses it anymore
 	
@@ -278,6 +286,10 @@ void btSimulationIslandManager::buildAndProcessIslands(btDispatcher* dispatcher,
 
 	//int islandId;
 
+	END_PROFILE("islandUnionFindAndHeapSort");
+
+	btAlignedObjectArray<btCollisionObject*>	islandBodies;
+
 
 	//traverse the simulation islands, and call the solver, unless all objects are sleeping/deactivated
 	for (int startIslandIndex=0;startIslandIndex<numElem;startIslandIndex = endIslandIndex)
@@ -291,6 +303,7 @@ void btSimulationIslandManager::buildAndProcessIslands(btDispatcher* dispatcher,
                 {
                         int i = getUnionFind().getElement(endIslandIndex).m_sz;
                         btCollisionObject* colObj0 = collisionObjects[i];
+						islandBodies.push_back(colObj0);
                         if (!colObj0->isActive())
                                 islandSleeping = true;
                 }
@@ -319,12 +332,16 @@ void btSimulationIslandManager::buildAndProcessIslands(btDispatcher* dispatcher,
 
 		if (!islandSleeping)
 		{
-			callback->ProcessIsland(startManifold,numIslandManifolds, islandId);
+			callback->ProcessIsland(&islandBodies[0],islandBodies.size(),startManifold,numIslandManifolds, islandId);
 		}
 		
 		if (numIslandManifolds)
 		{
 			startManifoldIndex = endManifoldIndex;
 		}
+
+		islandBodies.resize(0);
 	}
+
+	
 }

src/BulletCollision/CollisionDispatch/btSimulationIslandManager.h

@@ -19,6 +19,7 @@ subject to the following restrictions:
 #include "../CollisionDispatch/btUnionFind.h"
 #include "btCollisionCreateFunc.h"
 
+class btCollisionObject;
 class btCollisionWorld;
 class btDispatcher;
 
@@ -49,7 +50,7 @@ public:
 	{
 		virtual ~IslandCallback() {};
 
-		virtual	void	ProcessIsland(class btPersistentManifold**	manifolds,int numManifolds, int islandId) = 0;
+		virtual	void	ProcessIsland(btCollisionObject** bodies,int numBodies,class btPersistentManifold**	manifolds,int numManifolds, int islandId) = 0;
 	};
 
 	void	buildAndProcessIslands(btDispatcher* dispatcher,btCollisionObjectArray& collisionObjects, IslandCallback* callback);

src/BulletCollision/NarrowPhaseCollision/btManifoldPoint.h

@@ -29,7 +29,8 @@ class btManifoldPoint
 	{
 		public:
 			btManifoldPoint()
-				:m_userPersistentData(0)
+				:m_userPersistentData(0),
+				m_lifeTime(0)
 			{
 			}
 
@@ -76,12 +77,12 @@ class btManifoldPoint
 				return m_lifeTime;
 			}
 
-			btVector3 getPositionWorldOnA() {
+			const btVector3& getPositionWorldOnA() const {
 				return m_positionWorldOnA;
 //				return m_positionWorldOnB + m_normalWorldOnB * m_distance1;
 			}
 
-			const btVector3& getPositionWorldOnB()
+			const btVector3& getPositionWorldOnB() const
 			{
 				return m_positionWorldOnB;
 			}

src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.h

@@ -115,11 +115,23 @@ public:
 	}
 	void replaceContactPoint(const btManifoldPoint& newPoint,int insertIndex)
 	{
-		assert(validContactDistance(newPoint));
+		btAssert(validContactDistance(newPoint));
 
-		clearUserCache(m_pointCache[insertIndex]);
+#define MAINTAIN_PERSISTENCY 1
+#ifdef MAINTAIN_PERSISTENCY
+		int	lifeTime = m_pointCache[insertIndex].getLifeTime();
+		btAssert(lifeTime>=0);
+		void* cache = m_pointCache[insertIndex].m_userPersistentData;
 		
 		m_pointCache[insertIndex] = newPoint;
+
+		m_pointCache[insertIndex].m_userPersistentData = cache;
+		m_pointCache[insertIndex].m_lifeTime = lifeTime;
+#else
+		clearUserCache(m_pointCache[insertIndex]);
+		m_pointCache[insertIndex] = newPoint;
+	
+#endif
 	}
 
 	bool validContactDistance(const btManifoldPoint& pt) const