more solver optimizations, can be disabled using solver->setSolverMode(SOLVER_RANDMIZE_ORDER)

ChangeLog.txt

@@ -1,6 +1,12 @@
 Bullet Continuous Collision Detection and Physics Library
 Erwin Coumans
 
+2007 March 17
+	- Added constraint solver optimizations, avoiding cross products during iterations, and gather rigidbody/constraint info in contiguous memory (btSolverBody/btSolverConstraint)
+	- These optimizations don't give large benefit yet, but it has good potential. Turned on by default. Can be switched off using solver->setSolverMode(SOLVER_RANDMIZE_ORDER).
+	- Enabled anti-jitter for rigid bodies. This is experimental, and can be switched off by setting a global (it is experimental so no proper interface) gJitterVelocityDampingFactor = 1.0;
+	- Fixed bug in islandmanifold.heapSort(btPersistentManifoldSortPredicate()); , thanks Noehrgel for reporting this (affected Sun Solaris)
+	
 2007 March 12
 	- Added compile-time toggle between on 16-bit and 32-bit fixed-point SAP broadphase. 
 		This allows the number of bodies to exceed 32767

src/BulletCollision/BroadphaseCollision/btAxisSweep3.cpp

@@ -252,6 +252,11 @@ void btAxisSweep3::removeHandle(BP_FP_INT_TYPE handle)
 
 extern int gOverlappingPairs;
 
+
+void	btAxisSweep3::refreshOverlappingPairs()
+{
+
+}
 void	btAxisSweep3::processAllOverlappingPairs(btOverlapCallback* callback)
 {
 
@@ -269,6 +274,8 @@ void	btAxisSweep3::processAllOverlappingPairs(btOverlapCallback* callback)
 	m_overlappingPairArray.resize(m_overlappingPairArray.size() - m_invalidPair);
 	m_invalidPair = 0;
 #endif
+
+	
 	int i;
 
 	btBroadphasePair previousPair;

src/BulletCollision/BroadphaseCollision/btAxisSweep3.h

@@ -112,10 +112,7 @@ public:
 	btAxisSweep3(const btPoint3& worldAabbMin,const btPoint3& worldAabbMax, int maxHandles = 16384);
 	virtual	~btAxisSweep3();
 
-	virtual void	refreshOverlappingPairs()
+	virtual void	refreshOverlappingPairs();
-	{
-		//this is performed incrementally by sweep and prune (add pair), and during pair traversal (remove pair)
-	}
 	
 	BP_FP_INT_TYPE addHandle(const btPoint3& aabbMin,const btPoint3& aabbMax, void* pOwner,short int collisionFilterGroup,short int collisionFilterMask);
 	void removeHandle(BP_FP_INT_TYPE handle);

src/BulletCollision/CollisionDispatch/btCollisionWorld.cpp

@@ -119,7 +119,7 @@ void	btCollisionWorld::performDiscreteCollisionDetection()
 {
 	btDispatcherInfo& dispatchInfo = getDispatchInfo();
 
-	BEGIN_PROFILE("performDiscreteCollisionDetection");
+	BEGIN_PROFILE("perform Broadphase Collision Detection");
 
 
 	//update aabb (of all moved objects)
@@ -133,6 +133,11 @@ void	btCollisionWorld::performDiscreteCollisionDetection()
 
 	m_broadphasePairCache->refreshOverlappingPairs();
 
+	
+	END_PROFILE("perform Broadphase Collision Detection");
+
+	BEGIN_PROFILE("performDiscreteCollisionDetection");
+
 	btDispatcher* dispatcher = getDispatcher();
 	if (dispatcher)
 		dispatcher->dispatchAllCollisionPairs(m_broadphasePairCache,dispatchInfo);

src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp

@@ -46,8 +46,12 @@ struct	btOrderIndex
 	short int	m_pointIndex;
 };
 
+
+
 #define SEQUENTIAL_IMPULSE_MAX_SOLVER_POINTS 16384
 static btOrderIndex	gOrder[SEQUENTIAL_IMPULSE_MAX_SOLVER_POINTS];
+
+
 static unsigned long btSeed2 = 0;
 unsigned long btRand2()
 {
@@ -111,7 +115,7 @@ bool  MyContactDestroyedCallback(void* userPersistentData)
 
 
 btSequentialImpulseConstraintSolver::btSequentialImpulseConstraintSolver()
-:m_solverMode(SOLVER_RANDMIZE_ORDER)//SOLVER_USE_WARMSTARTING)//SOLVER_RANDMIZE_ORDER) //not using SOLVER_USE_WARMSTARTING
+:m_solverMode(SOLVER_RANDMIZE_ORDER | SOLVER_CACHE_FRIENDLY) //not using SOLVER_USE_WARMSTARTING
 {
 	gContactDestroyedCallback = &MyContactDestroyedCallback;
 
@@ -138,7 +142,7 @@ void	initSolverBody(btSolverBody* solverBody, btRigidBody* rigidbody)
 	solverBody->m_angularVelocity = rigidbody->getAngularVelocity();
 	solverBody->m_centerOfMassPosition = rigidbody->getCenterOfMassPosition();
 	solverBody->m_friction = rigidbody->getFriction();
-	solverBody->m_invInertiaWorld = rigidbody->getInvInertiaTensorWorld();
+//	solverBody->m_invInertiaWorld = rigidbody->getInvInertiaTensorWorld();
 	solverBody->m_invMass = rigidbody->getInvMass();
 	solverBody->m_linearVelocity = rigidbody->getLinearVelocity();
 	solverBody->m_originalBody = rigidbody;
@@ -342,12 +346,29 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendly(btCollisio
 {
 
 	if (!(numConstraints + numManifolds))
+	{
+//		printf("empty\n");
 		return 0.f;
+	}
+
+	BEGIN_PROFILE("refreshManifolds");
+
+	int i;
+	for (i=0;i<numManifolds;i++)
+	{
+		btPersistentManifold* manifold = manifoldPtr[i];
+		btRigidBody* rb0 = (btRigidBody*)manifold->getBody0();
+		btRigidBody* rb1 = (btRigidBody*)manifold->getBody1();
+
+		manifold->refreshContactPoints(rb0->getCenterOfMassTransform(),rb1->getCenterOfMassTransform());
+
+	}
+	
+	END_PROFILE("refreshManifolds");
+
 
 	BEGIN_PROFILE("gatherSolverData");
 
-	
-
 	int sizeofSB = sizeof(btSolverBody);
 	int sizeofSC = sizeof(btSolverConstraint);
 
@@ -362,7 +383,10 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendly(btCollisio
 //		unsigned char* stackMemory = stackAlloc->allocate(memsize);
 
 		
-		tmpSolverBodyPool.reserve(numManifolds*2);
+		//todo: use stack allocator for this temp memory
+		int minReservation = numManifolds*2;
+
+		tmpSolverBodyPool.reserve(minReservation);
 
 		{
 			for (int i=0;i<numBodies;i++)
@@ -380,18 +404,17 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendly(btCollisio
 		}
 
 		
-		tmpSolverConstraintPool.reserve(numManifolds*4+numConstraints);
+		tmpSolverConstraintPool.reserve(minReservation);
-		tmpSolverFrictionConstraintPool.reserve(numManifolds*4);
+		tmpSolverFrictionConstraintPool.reserve(minReservation);
-
 		{
 			int i;
+
 			for (i=0;i<numManifolds;i++)
 			{
 				btPersistentManifold* manifold = manifoldPtr[i];
 				btRigidBody* rb0 = (btRigidBody*)manifold->getBody0();
 				btRigidBody* rb1 = (btRigidBody*)manifold->getBody1();
 
-				manifold->refreshContactPoints(rb0->getCenterOfMassTransform(),rb1->getCenterOfMassTransform());
 			
 				int solverBodyIdA;
 				int solverBodyIdB;
@@ -591,6 +614,30 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendly(btCollisio
 		}
 	}
 	
+	btAlignedObjectArray<int>	gOrderTmpConstraintPool;
+	btAlignedObjectArray<int>	gOrderFrictionConstraintPool;
+
+	int numConstraintPool = tmpSolverConstraintPool.size();
+	int numFrictionPool = tmpSolverFrictionConstraintPool.size();
+
+	///todo: use stack allocator for such temporarily memory, same for solver bodies/constraints
+	gOrderTmpConstraintPool.resize(numConstraintPool);
+	gOrderFrictionConstraintPool.resize(numFrictionPool);
+	{
+		int i;
+		for (i=0;i<numConstraintPool;i++)
+		{
+			gOrderTmpConstraintPool[i] = i;
+		}
+		for (i=0;i<numFrictionPool;i++)
+		{
+			gOrderFrictionConstraintPool[i] = i;
+		}
+	}
+
+
+
+
 	END_PROFILE("prepareConstraints");
 
 
@@ -598,41 +645,64 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendly(btCollisio
 
 	//should traverse the contacts random order...
 	int iteration;
-	int j;
 	{
 		for ( iteration = 0;iteration<info.m_numIterations;iteration++)
 		{			
+
+			int j;
+			if (m_solverMode & SOLVER_RANDMIZE_ORDER)
+			{
+				if ((iteration & 7) == 0) {
+					for (j=0; j<numConstraintPool; ++j) {
+						int tmp = gOrderTmpConstraintPool[j];
+						int swapi = btRandInt2(j+1);
+						gOrderTmpConstraintPool[j] = gOrderTmpConstraintPool[swapi];
+						gOrderTmpConstraintPool[swapi] = tmp;
+					}
+
+					for (j=0; j<numFrictionPool; ++j) {
+						int tmp = gOrderFrictionConstraintPool[j];
+						int swapi = btRandInt2(j+1);
+						gOrderFrictionConstraintPool[j] = gOrderFrictionConstraintPool[swapi];
+						gOrderFrictionConstraintPool[swapi] = tmp;
+					}
+				}
+			}
+
 			for (j=0;j<numConstraints;j++)
 			{
 				btTypedConstraint* constraint = constraints[j];
+				///todo: use solver bodies, so we don't need to copy from/to btRigidBody
+
+				if (constraint->getRigidBodyA().getIslandTag() >= 0)
+				{
+					tmpSolverBodyPool[constraint->getRigidBodyA().getCompanionId()].writebackVelocity();
+				}
+				if (constraint->getRigidBodyB().getIslandTag() >= 0)
+				{
+					tmpSolverBodyPool[constraint->getRigidBodyB().getCompanionId()].writebackVelocity();
+				}
+
 				constraint->solveConstraint(info.m_timeStep);
+
+				if (constraint->getRigidBodyA().getIslandTag() >= 0)
+				{
+					tmpSolverBodyPool[constraint->getRigidBodyA().getCompanionId()].readVelocity();
+				}
+				if (constraint->getRigidBodyB().getIslandTag() >= 0)
+				{
+					tmpSolverBodyPool[constraint->getRigidBodyB().getCompanionId()].readVelocity();
+				}
+
 			}
 
 			{
 				int numPoolConstraints = tmpSolverConstraintPool.size();
 				for (j=0;j<numPoolConstraints;j++)
 				{
-					btSolverConstraint& solveManifold = tmpSolverConstraintPool[j];
+					btSolverConstraint& solveManifold = tmpSolverConstraintPool[gOrderTmpConstraintPool[j]];
-					switch (solveManifold.m_constraintType)
-					{
-					case btSolverConstraint::BT_SOLVER_CONTACT_1D:
-						{
 					resolveSingleCollisionCombinedCacheFriendly(tmpSolverBodyPool[solveManifold.m_solverBodyIdA],
 						tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold,info);
-							break;
-						}
-					case btSolverConstraint::BT_SOLVER_FRICTION_1D:
-						{
-
-							break;
-						}
-
-					default:
-						{
-							//undefined constraint type?
-							btAssert(0);
-						}
-					};
 				}
 			}
 
@@ -640,29 +710,11 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendly(btCollisio
 				 int numFrictionPoolConstraints = tmpSolverFrictionConstraintPool.size();
 				for (j=0;j<numFrictionPoolConstraints;j++)
 				{
-					btSolverConstraint& solveManifold = tmpSolverFrictionConstraintPool[j];
+					btSolverConstraint& solveManifold = tmpSolverFrictionConstraintPool[gOrderFrictionConstraintPool[j]];
-					switch (solveManifold.m_constraintType)
-					{
-					case btSolverConstraint::BT_SOLVER_CONTACT_1D:
-						{
-							break;
-						}
-					case btSolverConstraint::BT_SOLVER_FRICTION_1D:
-						{
-
 						btScalar appliedNormalImpulse = tmpSolverConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse;
 
 						resolveSingleFrictionCacheFriendly(tmpSolverBodyPool[solveManifold.m_solverBodyIdA],
 							tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold,info,appliedNormalImpulse);
-							break;
-						}
-
-					default:
-						{
-							//undefined constraint type?
-							btAssert(0);
-						}
-					};
 				}
 			}
 			
@@ -678,6 +730,18 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendly(btCollisio
 
 	END_PROFILE("solveConstraints");
 
+//	printf("tmpSolverConstraintPool.size() = %i\n",tmpSolverConstraintPool.size());
+
+/*
+	printf("tmpSolverBodyPool.size() = %i\n",tmpSolverBodyPool.size());
+	printf("tmpSolverConstraintPool.size() = %i\n",tmpSolverConstraintPool.size());
+	printf("tmpSolverFrictionConstraintPool.size() = %i\n",tmpSolverFrictionConstraintPool.size());
+
+	
+	printf("tmpSolverBodyPool.capacity() = %i\n",tmpSolverBodyPool.capacity());
+	printf("tmpSolverConstraintPool.capacity() = %i\n",tmpSolverConstraintPool.capacity());
+	printf("tmpSolverFrictionConstraintPool.capacity() = %i\n",tmpSolverFrictionConstraintPool.capacity());
+*/
 
 	tmpSolverBodyPool.resize(0);
 	tmpSolverConstraintPool.resize(0);
@@ -691,10 +755,10 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendly(btCollisio
 btScalar btSequentialImpulseConstraintSolver::solveGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc)
 {
 
-
+	if (getSolverMode() & SOLVER_CACHE_FRIENDLY)
-
+	{
-	//return solveGroupCacheFriendly(bodies,numBodies,manifoldPtr, numManifolds,constraints,numConstraints,infoGlobal,debugDrawer,stackAlloc);
+		return solveGroupCacheFriendly(bodies,numBodies,manifoldPtr, numManifolds,constraints,numConstraints,infoGlobal,debugDrawer,stackAlloc);
-
+	}
 
 
 	BEGIN_PROFILE("prepareConstraints");

src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h

@@ -46,7 +46,8 @@ public:
 	{
 		SOLVER_RANDMIZE_ORDER = 1,
 		SOLVER_FRICTION_SEPARATE = 2,
-		SOLVER_USE_WARMSTARTING = 4
+		SOLVER_USE_WARMSTARTING = 4,
+		SOLVER_CACHE_FRIENDLY = 8
 	};
 
 	btSequentialImpulseConstraintSolver();

src/BulletDynamics/ConstraintSolver/btsolverbody.h

@@ -25,7 +25,6 @@ class	btRigidBody;
 
 ATTRIBUTE_ALIGNED16 (struct	btSolverBody)
 {
-	btMatrix3x3		m_invInertiaWorld;
 	btVector3		m_centerOfMassPosition;
 	btVector3		m_linearVelocity;
 	btVector3		m_angularVelocity;
@@ -33,7 +32,6 @@ ATTRIBUTE_ALIGNED16 (struct	btSolverBody)
 	float			m_invMass;
 	float			m_friction;
 	float			m_unused;
-	btVector3		m_unused2;
 
 	inline void	getVelocityInLocalPoint(const btVector3& rel_pos, btVector3& velocity ) const
 	{
@@ -56,13 +54,24 @@ ATTRIBUTE_ALIGNED16 (struct	btSolverBody)
 		}
 	}
 
+	void	readVelocity()
+	{
+		if (m_invMass)
+		{
+			m_linearVelocity = m_originalBody->getLinearVelocity();
+			m_angularVelocity = m_originalBody->getAngularVelocity();
+		}
+	}
+
+	
+
 	inline void	applyImpulse(const btVector3& impulse,const btVector3& rel_pos)
 	{
 		if (m_invMass)
 		{
 			m_linearVelocity += impulse * m_invMass;
 			btVector3 torqueImpulse = rel_pos.cross(impulse);
-			m_angularVelocity += m_invInertiaWorld * torqueImpulse;
+//			m_angularVelocity += m_invInertiaWorld * torqueImpulse;
 		}
 	}
 

src/BulletDynamics/ConstraintSolver/btsolverconstraint.h

@@ -44,7 +44,7 @@ ATTRIBUTE_ALIGNED16 (struct	btSolverConstraint)
 
 	int			m_constraintType;
 	int			m_frictionIndex;
-	int			m_unusedPadding[2];
+//	int			m_unusedPadding[2];
 
 	enum		btSolverConstraintType
 	{

src/BulletDynamics/Dynamics/btRigidBody.cpp

@@ -112,7 +112,7 @@ btRigidBody::btRigidBody( btScalar mass,const btTransform& worldTransform,btColl
 
 
 
-//#define EXPERIMENTAL_JITTER_REMOVAL 1
+#define EXPERIMENTAL_JITTER_REMOVAL 1
 #ifdef EXPERIMENTAL_JITTER_REMOVAL
 //Bullet 2.20b has experimental damping code to reduce jitter just before objects fall asleep/deactivate
 //doesn't work very well yet (value 0 disabled this damping)
@@ -121,7 +121,7 @@ btScalar gClippedAngvelThresholdSqr = btScalar(0.01);
 btScalar	gClippedLinearThresholdSqr = btScalar(0.01);
 #endif //EXPERIMENTAL_JITTER_REMOVAL
 
-btScalar	gJitterVelocityDampingFactor = btScalar(0.9);
+btScalar	gJitterVelocityDampingFactor = btScalar(0.7);
 
 void btRigidBody::predictIntegratedTransform(btScalar timeStep,btTransform& predictedTransform) 
 {