From c5594a58263f55cd0e7d76c477eb13a0c1f471e4 Mon Sep 17 00:00:00 2001
From: kubas <jakub.stepien@polsl.pl>
Date: Thu, 9 Jan 2014 00:59:48 +0100
Subject: [PATCH] a bit of rk4 clean-up

---
 .../Featherstone/btMultiBody.cpp              |  4 ++--
 .../Featherstone/btMultiBodyDynamicsWorld.cpp | 21 +++++++++++++++----
 .../Featherstone/btMultiBodyDynamicsWorld.h   |  4 ++++
 3 files changed, 23 insertions(+), 6 deletions(-)

diff --git a/src/BulletDynamics/Featherstone/btMultiBody.cpp b/src/BulletDynamics/Featherstone/btMultiBody.cpp
index d4cafe36d..4b06aec7b 100644
--- a/src/BulletDynamics/Featherstone/btMultiBody.cpp
+++ b/src/BulletDynamics/Featherstone/btMultiBody.cpp
@@ -306,8 +306,8 @@ void btMultiBody::finalizeMultiDof()
 {
 	btAssert(m_isMultiDof);
 
-	m_realBuf.resize(6 + m_dofCount + m_dofCount*m_dofCount);				//m_dofCount for joint-space vels + m_dofCount^2 for "D" matrices
-	m_vectorBuf.resize(2 * m_dofCount);										//two 3-vectors (i.e. one six-vector) for each system dof	("h" matrices)
+	m_realBuf.resize(6 + m_dofCount + m_dofCount*m_dofCount + m_posVarCnt);			//m_dofCount for joint-space vels + m_dofCount^2 for "D" matrices + delta-pos vector
+	m_vectorBuf.resize(2 * m_dofCount);												//two 3-vectors (i.e. one six-vector) for each system dof	("h" matrices)
 
 	updateLinksDofOffsets();
 }
diff --git a/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp b/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp
index 1e6d7a7fe..cf5c5edc5 100644
--- a/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp
+++ b/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp
@@ -381,7 +381,7 @@ struct MultiBodyInplaceSolverIslandCallback : public btSimulationIslandManager::
 
 btMultiBodyDynamicsWorld::btMultiBodyDynamicsWorld(btDispatcher* dispatcher,btBroadphaseInterface* pairCache,btMultiBodyConstraintSolver* constraintSolver,btCollisionConfiguration* collisionConfiguration)
 	:btDiscreteDynamicsWorld(dispatcher,pairCache,constraintSolver,collisionConfiguration),
-	m_multiBodyConstraintSolver(constraintSolver)
+	m_multiBodyConstraintSolver(constraintSolver), m_useRK4ForMultiBodies(false)
 {
 	//split impulse is not yet supported for Featherstone hierarchies
 	getSolverInfo().m_splitImpulse = false;
@@ -463,12 +463,11 @@ void	btMultiBodyDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo)
 					bod->addLinkForce(j, m_gravity * bod->getLinkMass(j));
 				}
 
-				bool rk = false;	
 				bool doNotUpdatePos = false;
 
 				if(bod->isMultiDof())
 				{
-					if(!rk)
+					if(!m_useRK4ForMultiBodies)
 						bod->stepVelocitiesMultiDof(solverInfo.m_timeStep, scratch_r, scratch_v, scratch_m);
 					else
 					{						
@@ -577,7 +576,13 @@ void	btMultiBodyDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo)
 						//
 						if(!doNotUpdatePos)
 						{
-							bod->stepPositionsMultiDof(1, 0, &delta_q[0]);
+							btScalar *pRealBuf = const_cast<btScalar *>(bod->getVelocityVector());
+							pRealBuf += 6 + bod->getNumDofs() + bod->getNumDofs()*bod->getNumDofs();
+
+							for(int i = 0; i < bod->getNumPosVars(); ++i)
+								pRealBuf[i] = delta_q[i];
+
+							//bod->stepPositionsMultiDof(1, 0, &delta_q[0]);
 							bod->__posUpdated = true;
 						}
 						//
@@ -636,6 +641,14 @@ void	btMultiBodyDynamicsWorld::integrateTransforms(btScalar timeStep)
 				{
 					if(!bod->__posUpdated)
 						bod->stepPositionsMultiDof(timeStep);
+					else
+					{
+						btScalar *pRealBuf = const_cast<btScalar *>(bod->getVelocityVector());
+						pRealBuf += 6 + bod->getNumDofs() + bod->getNumDofs()*bod->getNumDofs();
+
+						bod->stepPositionsMultiDof(1, 0, pRealBuf);
+						bod->__posUpdated = false;
+					}
 				}
 				else
 					bod->stepPositions(timeStep);			
diff --git a/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h b/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h
index a4d1a1161..590906e16 100644
--- a/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h
+++ b/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h
@@ -34,6 +34,7 @@ protected:
 	btAlignedObjectArray<btMultiBodyConstraint*> m_sortedMultiBodyConstraints;
 	btMultiBodyConstraintSolver*	m_multiBodyConstraintSolver;
 	MultiBodyInplaceSolverIslandCallback*	m_solverMultiBodyIslandCallback;
+	bool m_useRK4ForMultiBodies;
 
 	virtual void	calculateSimulationIslands();
 	virtual void	updateActivationState(btScalar timeStep);
@@ -52,5 +53,8 @@ public:
 	virtual void	addMultiBodyConstraint( btMultiBodyConstraint* constraint);
 
 	virtual void	removeMultiBodyConstraint( btMultiBodyConstraint* constraint);
+
+	void useRK4IntegrationForMultiBodies(bool use) { m_useRK4ForMultiBodies = use; }
+	bool isUsingRK4IntegrationForMultiBodies() const { return m_useRK4ForMultiBodies; }
 };
 #endif //BT_MULTIBODY_DYNAMICS_WORLD_H