Get rid of btSolverBody and use btRigidBody directly. btSolverBody didn't improve performance after all, due to random-access

Tweak the BenchmarkDemo a bit: 

1) disable deactivation in graphical mode
2) add some settings that increase performance in the BenchmarkDemo2 (1000 stack) from 35ms to 15ms on this quad core (at the cost of a bit of quality)

src/BulletDynamics/Dynamics/btRigidBody.cpp

@@ -88,6 +88,15 @@ void	btRigidBody::setupRigidBody(const btRigidBody::btRigidBodyConstructionInfo&
 
 	m_rigidbodyFlags = 0;
 
+
+	m_deltaLinearVelocity.setZero();
+	m_deltaAngularVelocity.setZero();
+	m_invMass = m_inverseMass*m_linearFactor;
+	m_pushVelocity.setZero();
+	m_turnVelocity.setZero();
+
+	
+
 }
 
 
@@ -234,6 +243,7 @@ void btRigidBody::setMassProps(btScalar mass, const btVector3& inertia)
 				   inertia.y() != btScalar(0.0) ? btScalar(1.0) / inertia.y(): btScalar(0.0),
 				   inertia.z() != btScalar(0.0) ? btScalar(1.0) / inertia.z(): btScalar(0.0));
 
+	m_invMass = m_linearFactor*m_inverseMass;
 }
 
 	
@@ -303,6 +313,28 @@ bool btRigidBody::checkCollideWithOverride(btCollisionObject* co)
 	return true;
 }
 
+void	btRigidBody::internalWritebackVelocity(btScalar timeStep)
+{
+    (void) timeStep;
+	if (m_inverseMass)
+	{
+		setLinearVelocity(getLinearVelocity()+ m_deltaLinearVelocity);
+		setAngularVelocity(getAngularVelocity()+m_deltaAngularVelocity);
+		
+		//correct the position/orientation based on push/turn recovery
+		btTransform newTransform;
+		btTransformUtil::integrateTransform(getWorldTransform(),m_pushVelocity,m_turnVelocity,timeStep,newTransform);
+		setWorldTransform(newTransform);
+		//m_originalBody->setCompanionId(-1);
+	}
+	m_deltaLinearVelocity.setZero();
+	m_deltaAngularVelocity .setZero();
+	m_pushVelocity.setZero();
+	m_turnVelocity.setZero();
+}
+
+
+
 void btRigidBody::addConstraintRef(btTypedConstraint* c)
 {
 	int index = m_constraintRefs.findLinearSearch(c);
@@ -355,3 +387,4 @@ const char*	btRigidBody::serialize(void* dataBuffer, class btSerializer* seriali
 	return btRigidBodyDataName;
 }
 
+

src/BulletDynamics/Dynamics/btRigidBody.h

@@ -59,7 +59,6 @@ class btRigidBody  : public btCollisionObject
 	btVector3		m_linearVelocity;
 	btVector3		m_angularVelocity;
 	btScalar		m_inverseMass;
-	btVector3		m_angularFactor;
 	btVector3		m_linearFactor;
 
 	btVector3		m_gravity;	
@@ -91,8 +90,20 @@ class btRigidBody  : public btCollisionObject
 	
 	int				m_debugBodyId;
 
+
+protected:
+
+	ATTRIBUTE_ALIGNED64(btVector3		m_deltaLinearVelocity);
+	btVector3		m_deltaAngularVelocity;
+	btVector3		m_angularFactor;
+	btVector3		m_invMass;
+	btVector3		m_pushVelocity;
+	btVector3		m_turnVelocity;
+
+
 public:
 
+
 	///The btRigidBodyConstructionInfo structure provides information to create a rigid body. Setting mass to zero creates a fixed (non-dynamic) rigid body.
 	///For dynamic objects, you can use the collision shape to approximate the local inertia tensor, otherwise use the zero vector (default argument)
 	///You can use the motion state to synchronize the world transform between physics and graphics objects. 
@@ -128,7 +139,6 @@ public:
 		btScalar			m_additionalAngularDampingThresholdSqr;
 		btScalar			m_additionalAngularDampingFactor;
 
-		
 		btRigidBodyConstructionInfo(	btScalar mass, btMotionState* motionState, btCollisionShape* collisionShape, const btVector3& localInertia=btVector3(0,0,0)):
 		m_mass(mass),
 			m_motionState(motionState),
@@ -244,6 +254,7 @@ public:
 	void setLinearFactor(const btVector3& linearFactor)
 	{
 		m_linearFactor = linearFactor;
+		m_invMass = m_linearFactor*m_inverseMass;
 	}
 	btScalar		getInvMass() const { return m_inverseMass; }
 	const btMatrix3x3& getInvInertiaTensorWorld() const { 
@@ -318,19 +329,6 @@ public:
 		}
 	}
 
-	//Optimization for the iterative solver: avoid calculating constant terms involving inertia, normal, relative position
-	SIMD_FORCE_INLINE void internalApplyImpulse(const btVector3& linearComponent, const btVector3& angularComponent,btScalar impulseMagnitude)
-	{
-		if (m_inverseMass != btScalar(0.))
-		{
-			m_linearVelocity += linearComponent*m_linearFactor*impulseMagnitude;
-			if (m_angularFactor)
-			{
-				m_angularVelocity += angularComponent*m_angularFactor*impulseMagnitude;
-			}
-		}
-	}
-	
 	void clearForces() 
 	{
 		m_totalForce.setValue(btScalar(0.0), btScalar(0.0), btScalar(0.0));
@@ -521,6 +519,88 @@ public:
 		return m_rigidbodyFlags;
 	}
 
+
+	////////////////////////////////////////////////
+	///some internal methods, don't use them
+		
+	btVector3& internalGetDeltaLinearVelocity()
+	{
+		return m_deltaLinearVelocity;
+	}
+
+	btVector3& internalGetDeltaAngularVelocity()
+	{
+		return m_deltaAngularVelocity;
+	}
+
+	const btVector3& internalGetAngularFactor() const
+	{
+		return m_angularFactor;
+	}
+
+	const btVector3& internalGetInvMass() const
+	{
+		return m_invMass;
+	}
+	
+	btVector3& internalGetPushVelocity()
+	{
+		return m_pushVelocity;
+	}
+
+	btVector3& internalGetTurnVelocity()
+	{
+		return m_turnVelocity;
+	}
+
+	SIMD_FORCE_INLINE void	internalGetVelocityInLocalPointObsolete(const btVector3& rel_pos, btVector3& velocity ) const
+	{
+		velocity = getLinearVelocity()+m_deltaLinearVelocity + (getAngularVelocity()+m_deltaAngularVelocity).cross(rel_pos);
+	}
+
+	SIMD_FORCE_INLINE void	internalGetAngularVelocity(btVector3& angVel) const
+	{
+		angVel = getAngularVelocity()+m_deltaAngularVelocity;
+	}
+
+
+	//Optimization for the iterative solver: avoid calculating constant terms involving inertia, normal, relative position
+	SIMD_FORCE_INLINE void internalApplyImpulse(const btVector3& linearComponent, const btVector3& angularComponent,const btScalar impulseMagnitude)
+	{
+		if (m_inverseMass)
+		{
+			m_deltaLinearVelocity += linearComponent*impulseMagnitude;
+			m_deltaAngularVelocity += angularComponent*(impulseMagnitude*m_angularFactor);
+		}
+	}
+
+	SIMD_FORCE_INLINE void internalApplyPushImpulse(const btVector3& linearComponent, const btVector3& angularComponent,btScalar impulseMagnitude)
+	{
+		if (m_inverseMass)
+		{
+			m_pushVelocity += linearComponent*impulseMagnitude;
+			m_turnVelocity += angularComponent*(impulseMagnitude*m_angularFactor);
+		}
+	}
+	
+	void	internalWritebackVelocity()
+	{
+		if (m_inverseMass)
+		{
+			setLinearVelocity(getLinearVelocity()+ m_deltaLinearVelocity);
+			setAngularVelocity(getAngularVelocity()+m_deltaAngularVelocity);
+			m_deltaLinearVelocity.setZero();
+			m_deltaAngularVelocity .setZero();
+			//m_originalBody->setCompanionId(-1);
+		}
+	}
+
+
+	void	internalWritebackVelocity(btScalar timeStep);
+	
+
+	///////////////////////////////////////////////
+
 	virtual	int	calculateSerializeBufferSize()	const;
 
 	///fills the dataBuffer and returns the struct name (and 0 on failure)