Iterate only over non-static rigid bodies, instead of all collision objects

http://bulletphysics.com/Bullet/phpBB3/viewtopic.php?f=18&t=3625
http://code.google.com/p/bullet/issues/detail?id=128

Attempt to fix issue in mesh striding, multiple-mesh-parts were broken.

src/BulletCollision/CollisionShapes/btStridingMeshInterface.cpp

@@ -44,15 +44,13 @@ void	btStridingMeshInterface::InternalProcessAllTriangles(btInternalTriangleInde
 		getLockedReadOnlyVertexIndexBase(&vertexbase,numverts,type,stride,&indexbase,indexstride,numtriangles,gfxindextype,part);
 		numtotalphysicsverts+=numtriangles*3; //upper bound
 
-///unlike that developers want to pass in double-precision meshes in single-precision Bullet build
+		///unlike that developers want to pass in double-precision meshes in single-precision Bullet build
-///so disable this feature by default
+		///so disable this feature by default
-///see patch http://code.google.com/p/bullet/issues/detail?id=213
+		///see patch http://code.google.com/p/bullet/issues/detail?id=213
 
-#ifdef BT_USE_DOUBLE_PRECISION
 		switch (type)
 		{
 		case PHY_FLOAT:
-#endif
 		 {
 
 			 float* graphicsbase;
@@ -95,7 +93,6 @@ void	btStridingMeshInterface::InternalProcessAllTriangles(btInternalTriangleInde
 			 break;
 		 }
 
-#ifdef BT_USE_DOUBLE_PRECISION
 		case PHY_DOUBLE:
 			{
 				double* graphicsbase;
@@ -140,7 +137,6 @@ void	btStridingMeshInterface::InternalProcessAllTriangles(btInternalTriangleInde
 		default:
 			btAssert((type == PHY_FLOAT) || (type == PHY_DOUBLE));
 		}
-#endif //BT_USE_DOUBLE_PRECISION
 
 		unLockReadOnlyVertexBase(part);
 	}

src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp

@@ -103,14 +103,14 @@ btDiscreteDynamicsWorld::~btDiscreteDynamicsWorld()
 
 void	btDiscreteDynamicsWorld::saveKinematicState(btScalar timeStep)
 {
-
+///would like to iterate over m_nonStaticRigidBodies, but unfortunately old API allows
+///to switch status _after_ adding kinematic objects to the world
+///fix it for Bullet 3.x release
 	for (int i=0;i<m_collisionObjects.size();i++)
 	{
 		btCollisionObject* colObj = m_collisionObjects[i];
 		btRigidBody* body = btRigidBody::upcast(colObj);
-		if (body)
+		if (body && body->getActivationState() != ISLAND_SLEEPING)
-		{
-				if (body->getActivationState() != ISLAND_SLEEPING)
 		{
 			if (body->isKinematicObject())
 			{
@@ -119,7 +119,7 @@ void	btDiscreteDynamicsWorld::saveKinematicState(btScalar timeStep)
 			}
 		}
 	}
-	}
+
 }
 
 void	btDiscreteDynamicsWorld::debugDrawWorld()
@@ -217,28 +217,23 @@ void	btDiscreteDynamicsWorld::debugDrawWorld()
 void	btDiscreteDynamicsWorld::clearForces()
 {
 	///@todo: iterate over awake simulation islands!
-	for ( int i=0;i<m_collisionObjects.size();i++)
+	for ( int i=0;i<m_nonStaticRigidBodies.size();i++)
-	{
-		btCollisionObject* colObj = m_collisionObjects[i];
-		
-		btRigidBody* body = btRigidBody::upcast(colObj);
-		if (body)
 	{
+		btRigidBody* body = m_nonStaticRigidBodies[i];
+		//need to check if next line is ok
+		//it might break backward compatibility (people applying forces on sleeping objects get never cleared and accumulate on wake-up
 		body->clearForces();
 	}
-	}
 }	
 
 ///apply gravity, call this once per timestep
 void	btDiscreteDynamicsWorld::applyGravity()
 {
 	///@todo: iterate over awake simulation islands!
-	for ( int i=0;i<m_collisionObjects.size();i++)
+	for ( int i=0;i<m_nonStaticRigidBodies.size();i++)
 	{
-		btCollisionObject* colObj = m_collisionObjects[i];
+		btRigidBody* body = m_nonStaticRigidBodies[i];
-		
+		if (body->isActive())
-		btRigidBody* body = btRigidBody::upcast(colObj);
-		if (body && body->isActive())
 		{
 			body->applyGravity();
 		}
@@ -271,12 +266,10 @@ void	btDiscreteDynamicsWorld::synchronizeMotionStates()
 	BT_PROFILE("synchronizeMotionStates");
 	{
 		//todo: iterate over awake simulation islands!
-		for ( int i=0;i<m_collisionObjects.size();i++)
+		for ( int i=0;i<m_nonStaticRigidBodies.size();i++)
 		{
-			btCollisionObject* colObj = m_collisionObjects[i];
+			btRigidBody* body = m_nonStaticRigidBodies[i];
-			
+			if (body->isActive())
-			btRigidBody* body = btRigidBody::upcast(colObj);
-			if (body)
 				synchronizeSingleMotionState(body);
 		}
 	}
@@ -411,11 +404,10 @@ void	btDiscreteDynamicsWorld::internalSingleStepSimulation(btScalar timeStep)
 void	btDiscreteDynamicsWorld::setGravity(const btVector3& gravity)
 {
 	m_gravity = gravity;
-	for ( int i=0;i<m_collisionObjects.size();i++)
+	for ( int i=0;i<m_nonStaticRigidBodies.size();i++)
 	{
-		btCollisionObject* colObj = m_collisionObjects[i];
+		btRigidBody* body = m_nonStaticRigidBodies[i];
-		btRigidBody* body = btRigidBody::upcast(colObj);
+		if (body->isActive())
-		if (body)
 		{
 			body->setGravity(gravity);
 		}
@@ -430,6 +422,7 @@ btVector3 btDiscreteDynamicsWorld::getGravity () const
 
 void	btDiscreteDynamicsWorld::removeRigidBody(btRigidBody* body)
 {
+	m_nonStaticRigidBodies.remove(body);
 	removeCollisionObject(body);
 }
 
@@ -442,6 +435,11 @@ void	btDiscreteDynamicsWorld::addRigidBody(btRigidBody* body)
 
 	if (body->getCollisionShape())
 	{
+		if (!body->isStaticObject())
+		{
+			m_nonStaticRigidBodies.push_back(body);
+		}
+
 		bool isDynamic = !(body->isStaticObject() || body->isKinematicObject());
 		short collisionFilterGroup = isDynamic? short(btBroadphaseProxy::DefaultFilter) : short(btBroadphaseProxy::StaticFilter);
 		short collisionFilterMask = isDynamic? 	short(btBroadphaseProxy::AllFilter) : 	short(btBroadphaseProxy::AllFilter ^ btBroadphaseProxy::StaticFilter);
@@ -459,6 +457,10 @@ void	btDiscreteDynamicsWorld::addRigidBody(btRigidBody* body, short group, short
 
 	if (body->getCollisionShape())
 	{
+		if (!body->isStaticObject())
+		{
+			m_nonStaticRigidBodies.push_back(body);
+		}
 		addCollisionObject(body,group,mask);
 	}
 }
@@ -479,10 +481,9 @@ void	btDiscreteDynamicsWorld::updateActivationState(btScalar timeStep)
 {
 	BT_PROFILE("updateActivationState");
 
-	for ( int i=0;i<m_collisionObjects.size();i++)
+	for ( int i=0;i<m_nonStaticRigidBodies.size();i++)
 	{
-		btCollisionObject* colObj = m_collisionObjects[i];
+		btRigidBody* body = m_nonStaticRigidBodies[i];
-		btRigidBody* body = btRigidBody::upcast(colObj);
 		if (body)
 		{
 			body->updateDeactivation(timeStep);
@@ -828,12 +829,9 @@ void	btDiscreteDynamicsWorld::integrateTransforms(btScalar timeStep)
 {
 	BT_PROFILE("integrateTransforms");
 	btTransform predictedTrans;
-	for ( int i=0;i<m_collisionObjects.size();i++)
+	for ( int i=0;i<m_nonStaticRigidBodies.size();i++)
-	{
-		btCollisionObject* colObj = m_collisionObjects[i];
-		btRigidBody* body = btRigidBody::upcast(colObj);
-		if (body)
 	{
+		btRigidBody* body = m_nonStaticRigidBodies[i];
 		body->setHitFraction(1.f);
 
 		if (body->isActive() && (!body->isStaticOrKinematicObject()))
@@ -869,7 +867,6 @@ void	btDiscreteDynamicsWorld::integrateTransforms(btScalar timeStep)
 			body->proceedToTransform( predictedTrans);
 		}
 	}
-	}
 }
 
 
@@ -879,15 +876,11 @@ void	btDiscreteDynamicsWorld::integrateTransforms(btScalar timeStep)
 void	btDiscreteDynamicsWorld::predictUnconstraintMotion(btScalar timeStep)
 {
 	BT_PROFILE("predictUnconstraintMotion");
-	for ( int i=0;i<m_collisionObjects.size();i++)
+	for ( int i=0;i<m_nonStaticRigidBodies.size();i++)
-	{
-		btCollisionObject* colObj = m_collisionObjects[i];
-		btRigidBody* body = btRigidBody::upcast(colObj);
-		if (body)
 	{
+		btRigidBody* body = m_nonStaticRigidBodies[i];
 		if (!body->isStaticOrKinematicObject())
 		{
-				
 			body->integrateVelocities( timeStep);
 			//damping
 			body->applyDamping(timeStep);
@@ -895,7 +888,6 @@ void	btDiscreteDynamicsWorld::predictUnconstraintMotion(btScalar timeStep)
 			body->predictIntegratedTransform(timeStep,body->getInterpolationWorldTransform());
 		}
 	}
-	}
 }
 
 

src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h

@@ -42,6 +42,8 @@ protected:
 
 	btAlignedObjectArray<btTypedConstraint*> m_constraints;
 
+	btAlignedObjectArray<btRigidBody*> m_nonStaticRigidBodies;
+
 	btVector3	m_gravity;
 
 	//for variable timesteps