Reduce dynamic memory allocation, fixes Issue 573

Thanks to Anton Breusov for the patch

src/BulletCollision/CollisionDispatch/btSimulationIslandManager.cpp

@@ -365,7 +365,7 @@ void btSimulationIslandManager::buildAndProcessIslands(btDispatcher* dispatcher,
 	{
 		btPersistentManifold** manifold = dispatcher->getInternalManifoldPointer();
 		int maxNumManifolds = dispatcher->getNumManifolds();
-		callback->ProcessIsland(&collisionObjects[0],collisionObjects.size(),manifold,maxNumManifolds, -1);
+		callback->processIsland(&collisionObjects[0],collisionObjects.size(),manifold,maxNumManifolds, -1);
 	}
 	else
 	{
@@ -430,7 +430,7 @@ void btSimulationIslandManager::buildAndProcessIslands(btDispatcher* dispatcher,
 
 			if (!islandSleeping)
 			{
-				callback->ProcessIsland(&m_islandBodies[0],m_islandBodies.size(),startManifold,numIslandManifolds, islandId);
+				callback->processIsland(&m_islandBodies[0],m_islandBodies.size(),startManifold,numIslandManifolds, islandId);
 	//			printf("Island callback of size:%d bodies, %d manifolds\n",islandBodies.size(),numIslandManifolds);
 			}
 			

src/BulletCollision/CollisionDispatch/btSimulationIslandManager.h

@@ -59,7 +59,7 @@ public:
 	{
 		virtual ~IslandCallback() {};
 
-		virtual	void	ProcessIsland(btCollisionObject** bodies,int numBodies,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,btCollisionWorld* collisionWorld, IslandCallback* callback);

src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp

@@ -55,6 +55,160 @@ int startHit=2;
 int firstHit=startHit;
 #endif
 
+SIMD_FORCE_INLINE	int	btGetConstraintIslandId(const btTypedConstraint* lhs)
+{
+	int islandId;
+	
+	const btCollisionObject& rcolObj0 = lhs->getRigidBodyA();
+	const btCollisionObject& rcolObj1 = lhs->getRigidBodyB();
+	islandId= rcolObj0.getIslandTag()>=0?rcolObj0.getIslandTag():rcolObj1.getIslandTag();
+	return islandId;
+
+}
+
+
+class btSortConstraintOnIslandPredicate
+{
+	public:
+
+		bool operator() ( const btTypedConstraint* lhs, const btTypedConstraint* rhs )
+		{
+			int rIslandId0,lIslandId0;
+			rIslandId0 = btGetConstraintIslandId(rhs);
+			lIslandId0 = btGetConstraintIslandId(lhs);
+			return lIslandId0 < rIslandId0;
+		}
+};
+
+struct InplaceSolverIslandCallback : public btSimulationIslandManager::IslandCallback
+{
+	btContactSolverInfo*	m_solverInfo;
+	btConstraintSolver*		m_solver;
+	btTypedConstraint**		m_sortedConstraints;
+	int						m_numConstraints;
+	btIDebugDraw*			m_debugDrawer;
+	btStackAlloc*			m_stackAlloc;
+	btDispatcher*			m_dispatcher;
+	
+	btAlignedObjectArray<btCollisionObject*> m_bodies;
+	btAlignedObjectArray<btPersistentManifold*> m_manifolds;
+	btAlignedObjectArray<btTypedConstraint*> m_constraints;
+
+
+	InplaceSolverIslandCallback(
+		btConstraintSolver*	solver,
+		btStackAlloc* stackAlloc,
+		btDispatcher* dispatcher)
+		:m_solverInfo(NULL),
+		m_solver(solver),
+		m_sortedConstraints(NULL),
+		m_numConstraints(0),
+		m_debugDrawer(NULL),
+		m_stackAlloc(stackAlloc),
+		m_dispatcher(dispatcher)
+	{
+
+	}
+
+	InplaceSolverIslandCallback& operator=(InplaceSolverIslandCallback& other)
+	{
+		btAssert(0);
+		(void)other;
+		return *this;
+	}
+
+	SIMD_FORCE_INLINE void setup ( btContactSolverInfo* solverInfo, btTypedConstraint** sortedConstraints,	int	numConstraints,	btIDebugDraw* debugDrawer)
+	{
+		btAssert(solverInfo);
+		m_solverInfo = solverInfo;
+		m_sortedConstraints = sortedConstraints;
+		m_numConstraints = numConstraints;
+		m_debugDrawer = debugDrawer;
+		m_bodies.resize (0);
+		m_manifolds.resize (0);
+		m_constraints.resize (0);
+	}
+
+	
+	virtual	void	processIsland(btCollisionObject** bodies,int numBodies,btPersistentManifold**	manifolds,int numManifolds, int islandId)
+	{
+		if (islandId<0)
+		{
+			if (numManifolds + m_numConstraints)
+			{
+				///we don't split islands, so all constraints/contact manifolds/bodies are passed into the solver regardless the island id
+				m_solver->solveGroup( bodies,numBodies,manifolds, numManifolds,&m_sortedConstraints[0],m_numConstraints,*m_solverInfo,m_debugDrawer,m_stackAlloc,m_dispatcher);
+			}
+		} else
+		{
+				//also add all non-contact constraints/joints for this island
+			btTypedConstraint** startConstraint = 0;
+			int numCurConstraints = 0;
+			int i;
+			
+			//find the first constraint for this island
+			for (i=0;i<m_numConstraints;i++)
+			{
+				if (btGetConstraintIslandId(m_sortedConstraints[i]) == islandId)
+				{
+					startConstraint = &m_sortedConstraints[i];
+					break;
+				}
+			}
+			//count the number of constraints in this island
+			for (;i<m_numConstraints;i++)
+			{
+				if (btGetConstraintIslandId(m_sortedConstraints[i]) == islandId)
+				{
+					numCurConstraints++;
+				}
+			}
+
+			if (m_solverInfo->m_minimumSolverBatchSize<=1)
+			{
+				///only call solveGroup if there is some work: avoid virtual function call, its overhead can be excessive
+				if (numManifolds + numCurConstraints)
+				{
+					m_solver->solveGroup( bodies,numBodies,manifolds, numManifolds,startConstraint,numCurConstraints,*m_solverInfo,m_debugDrawer,m_stackAlloc,m_dispatcher);
+				}
+			} else
+			{
+				
+				for (i=0;i<numBodies;i++)
+					m_bodies.push_back(bodies[i]);
+				for (i=0;i<numManifolds;i++)
+					m_manifolds.push_back(manifolds[i]);
+				for (i=0;i<numCurConstraints;i++)
+					m_constraints.push_back(startConstraint[i]);
+				if ((m_constraints.size()+m_manifolds.size())>m_solverInfo->m_minimumSolverBatchSize)
+				{
+					processConstraints();
+				} else
+				{
+					//printf("deferred\n");
+				}
+			}
+		}
+	}
+	void	processConstraints()
+	{
+		if (m_manifolds.size() + m_constraints.size()>0)
+		{
+
+			btCollisionObject** bodies = m_bodies.size()? &m_bodies[0]:0;
+			btPersistentManifold** manifold = m_manifolds.size()?&m_manifolds[0]:0;
+			btTypedConstraint** constraints = m_constraints.size()?&m_constraints[0]:0;
+			
+			m_solver->solveGroup( bodies,m_bodies.size(),manifold, m_manifolds.size(),constraints, m_constraints.size() ,*m_solverInfo,m_debugDrawer,m_stackAlloc,m_dispatcher);
+		}
+		m_bodies.resize(0);
+		m_manifolds.resize(0);
+		m_constraints.resize(0);
+
+	}
+
+};
+
 
 
 btDiscreteDynamicsWorld::btDiscreteDynamicsWorld(btDispatcher* dispatcher,btBroadphaseInterface* pairCache,btConstraintSolver* constraintSolver, btCollisionConfiguration* collisionConfiguration)
@@ -63,7 +217,9 @@ m_constraintSolver(constraintSolver),
 m_gravity(0,-10,0),
 m_localTime(0),
 m_synchronizeAllMotionStates(false),
-m_profileTimings(0)
+m_profileTimings(0),
+m_sortedConstraints	(),
+m_solverIslandCallback ( NULL )
 {
 	if (!m_constraintSolver)
 	{
@@ -81,6 +237,11 @@ m_profileTimings(0)
 	}
 
 	m_ownsIslandManager = true;
+
+	{
+		void* mem = btAlignedAlloc(sizeof(InplaceSolverIslandCallback),16);
+		m_solverIslandCallback = new (mem) InplaceSolverIslandCallback (constraintSolver, m_stackAlloc, dispatcher);
+	}
 }
 
 
@@ -92,6 +253,11 @@ btDiscreteDynamicsWorld::~btDiscreteDynamicsWorld()
 		m_islandManager->~btSimulationIslandManager();
 		btAlignedFree( m_islandManager);
 	}
+	if (m_solverIslandCallback)
+	{
+		m_solverIslandCallback->~InplaceSolverIslandCallback();
+		btAlignedFree(m_solverIslandCallback);
+	}
 	if (m_ownsConstraintSolver)
 	{
 
@@ -535,192 +701,39 @@ void	btDiscreteDynamicsWorld::removeCharacter(btActionInterface* character)
 }
 
 
-SIMD_FORCE_INLINE	int	btGetConstraintIslandId(const btTypedConstraint* lhs)
-{
-	int islandId;
-	
-	const btCollisionObject& rcolObj0 = lhs->getRigidBodyA();
-	const btCollisionObject& rcolObj1 = lhs->getRigidBodyB();
-	islandId= rcolObj0.getIslandTag()>=0?rcolObj0.getIslandTag():rcolObj1.getIslandTag();
-	return islandId;
-
-}
-
-
-class btSortConstraintOnIslandPredicate
-{
-	public:
-
-		bool operator() ( const btTypedConstraint* lhs, const btTypedConstraint* rhs )
-		{
-			int rIslandId0,lIslandId0;
-			rIslandId0 = btGetConstraintIslandId(rhs);
-			lIslandId0 = btGetConstraintIslandId(lhs);
-			return lIslandId0 < rIslandId0;
-		}
-};
-
 
 
 void	btDiscreteDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo)
 {
 	BT_PROFILE("solveConstraints");
 	
-	struct InplaceSolverIslandCallback : public btSimulationIslandManager::IslandCallback
-	{
-
-		btContactSolverInfo&	m_solverInfo;
-		btConstraintSolver*		m_solver;
-		btTypedConstraint**		m_sortedConstraints;
-		int						m_numConstraints;
-		btIDebugDraw*			m_debugDrawer;
-		btStackAlloc*			m_stackAlloc;
-		btDispatcher*			m_dispatcher;
-		
-		btAlignedObjectArray<btCollisionObject*> m_bodies;
-		btAlignedObjectArray<btPersistentManifold*> m_manifolds;
-		btAlignedObjectArray<btTypedConstraint*> m_constraints;
-
-
-		InplaceSolverIslandCallback(
-			btContactSolverInfo& solverInfo,
-			btConstraintSolver*	solver,
-			btTypedConstraint** sortedConstraints,
-			int	numConstraints,
-			btIDebugDraw*	debugDrawer,
-			btStackAlloc*			stackAlloc,
-			btDispatcher* dispatcher)
-			:m_solverInfo(solverInfo),
-			m_solver(solver),
-			m_sortedConstraints(sortedConstraints),
-			m_numConstraints(numConstraints),
-			m_debugDrawer(debugDrawer),
-			m_stackAlloc(stackAlloc),
-			m_dispatcher(dispatcher)
-		{
-
-		}
-
-
-		InplaceSolverIslandCallback& operator=(InplaceSolverIslandCallback& other)
-		{
-			btAssert(0);
-			(void)other;
-			return *this;
-		}
-		virtual	void	ProcessIsland(btCollisionObject** bodies,int numBodies,btPersistentManifold**	manifolds,int numManifolds, int islandId)
-		{
-			if (islandId<0)
-			{
-				if (numManifolds + m_numConstraints)
-				{
-					///we don't split islands, so all constraints/contact manifolds/bodies are passed into the solver regardless the island id
-					m_solver->solveGroup( bodies,numBodies,manifolds, numManifolds,&m_sortedConstraints[0],m_numConstraints,m_solverInfo,m_debugDrawer,m_stackAlloc,m_dispatcher);
-				}
-			} else
-			{
-					//also add all non-contact constraints/joints for this island
-				btTypedConstraint** startConstraint = 0;
-				int numCurConstraints = 0;
-				int i;
-				
-				//find the first constraint for this island
-				for (i=0;i<m_numConstraints;i++)
-				{
-					if (btGetConstraintIslandId(m_sortedConstraints[i]) == islandId)
-					{
-						startConstraint = &m_sortedConstraints[i];
-						break;
-					}
-				}
-				//count the number of constraints in this island
-				for (;i<m_numConstraints;i++)
-				{
-					if (btGetConstraintIslandId(m_sortedConstraints[i]) == islandId)
-					{
-						numCurConstraints++;
-					}
-				}
-
-				if (m_solverInfo.m_minimumSolverBatchSize<=1)
-				{
-					///only call solveGroup if there is some work: avoid virtual function call, its overhead can be excessive
-					if (numManifolds + numCurConstraints)
-					{
-						m_solver->solveGroup( bodies,numBodies,manifolds, numManifolds,startConstraint,numCurConstraints,m_solverInfo,m_debugDrawer,m_stackAlloc,m_dispatcher);
-					}
-				} else
-				{
-					
-					for (i=0;i<numBodies;i++)
-						m_bodies.push_back(bodies[i]);
-					for (i=0;i<numManifolds;i++)
-						m_manifolds.push_back(manifolds[i]);
-					for (i=0;i<numCurConstraints;i++)
-						m_constraints.push_back(startConstraint[i]);
-					if ((m_constraints.size()+m_manifolds.size())>m_solverInfo.m_minimumSolverBatchSize)
-					{
-						processConstraints();
-					} else
-					{
-						//printf("deferred\n");
-					}
-				}
-			}
-		}
-		void	processConstraints()
-		{
-			if (m_manifolds.size() + m_constraints.size()>0)
-			{
-
-				btCollisionObject** bodies = m_bodies.size()? &m_bodies[0]:0;
-				btPersistentManifold** manifold = m_manifolds.size()?&m_manifolds[0]:0;
-				btTypedConstraint** constraints = m_constraints.size()?&m_constraints[0]:0;
-				
-				m_solver->solveGroup( bodies,m_bodies.size(),manifold, m_manifolds.size(),constraints, m_constraints.size() ,m_solverInfo,m_debugDrawer,m_stackAlloc,m_dispatcher);
-			}
-			m_bodies.resize(0);
-			m_manifolds.resize(0);
-			m_constraints.resize(0);
-
-		}
-
-	};
-
-	
-
-	//sorted version of all btTypedConstraint, based on islandId
-	btAlignedObjectArray<btTypedConstraint*>	sortedConstraints;
-	sortedConstraints.resize( m_constraints.size());
+	m_sortedConstraints.resize( m_constraints.size());
 	int i; 
 	for (i=0;i<getNumConstraints();i++)
 	{
-		sortedConstraints[i] = m_constraints[i];
+		m_sortedConstraints[i] = m_constraints[i];
 	}
 
 //	btAssert(0);
 		
 	
 
-	sortedConstraints.quickSort(btSortConstraintOnIslandPredicate());
+	m_sortedConstraints.quickSort(btSortConstraintOnIslandPredicate());
 	
-	btTypedConstraint** constraintsPtr = getNumConstraints() ? &sortedConstraints[0] : 0;
-	
-	InplaceSolverIslandCallback	solverCallback(	solverInfo,	m_constraintSolver, constraintsPtr,sortedConstraints.size(),	m_debugDrawer,m_stackAlloc,m_dispatcher1);
+	btTypedConstraint** constraintsPtr = getNumConstraints() ? &m_sortedConstraints[0] : 0;
 	
+	m_solverIslandCallback->setup(&solverInfo,constraintsPtr,m_sortedConstraints.size(),getDebugDrawer());
 	m_constraintSolver->prepareSolve(getCollisionWorld()->getNumCollisionObjects(), getCollisionWorld()->getDispatcher()->getNumManifolds());
 	
 	/// solve all the constraints for this island
-	m_islandManager->buildAndProcessIslands(getCollisionWorld()->getDispatcher(),getCollisionWorld(),&solverCallback);
+	m_islandManager->buildAndProcessIslands(getCollisionWorld()->getDispatcher(),getCollisionWorld(),m_solverIslandCallback);
 
-	solverCallback.processConstraints();
+	m_solverIslandCallback->processConstraints();
 
 	m_constraintSolver->allSolved(solverInfo, m_debugDrawer, m_stackAlloc);
 }
 
 
-
-
 void	btDiscreteDynamicsWorld::calculateSimulationIslands()
 {
 	BT_PROFILE("calculateSimulationIslands");

src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h

@@ -27,6 +27,8 @@ class btTypedConstraint;
 class btActionInterface;
 
 class btIDebugDraw;
+struct InplaceSolverIslandCallback;
+
 #include "LinearMath/btAlignedObjectArray.h"
 
 
@@ -35,6 +37,9 @@ class btIDebugDraw;
 class btDiscreteDynamicsWorld : public btDynamicsWorld
 {
 protected:
+	
+    btAlignedObjectArray<btTypedConstraint*>	m_sortedConstraints;
+	InplaceSolverIslandCallback* 	m_solverIslandCallback;
 
 	btConstraintSolver*	m_constraintSolver;