separate external force solve from constraint solve and eliminate damping in external force solve

2 files changed, 295 insertions, 282 deletions

diff --git a/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp b/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp
index 1131e5378..718106e77 100644
--- a/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp
+++ b/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp
@@ -421,288 +421,7 @@ void btMultiBodyDynamicsWorld::forwardKinematics()
 }
 void btMultiBodyDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo)
 {
-	forwardKinematics();
-
-	BT_PROFILE("solveConstraints");
-
-	clearMultiBodyConstraintForces();
-
-	m_sortedConstraints.resize(m_constraints.size());
-	int i;
-	for (i = 0; i < getNumConstraints(); i++)
-	{
-		m_sortedConstraints[i] = m_constraints[i];
-	}
-	m_sortedConstraints.quickSort(btSortConstraintOnIslandPredicate2());
-	btTypedConstraint** constraintsPtr = getNumConstraints() ? &m_sortedConstraints[0] : 0;
-
-	m_sortedMultiBodyConstraints.resize(m_multiBodyConstraints.size());
-	for (i = 0; i < m_multiBodyConstraints.size(); i++)
-	{
-		m_sortedMultiBodyConstraints[i] = m_multiBodyConstraints[i];
-	}
-	m_sortedMultiBodyConstraints.quickSort(btSortMultiBodyConstraintOnIslandPredicate());
-
-	btMultiBodyConstraint** sortedMultiBodyConstraints = m_sortedMultiBodyConstraints.size() ? &m_sortedMultiBodyConstraints[0] : 0;
-
-	m_solverMultiBodyIslandCallback->setup(&solverInfo, constraintsPtr, m_sortedConstraints.size(), sortedMultiBodyConstraints, m_sortedMultiBodyConstraints.size(), getDebugDrawer());
-	m_constraintSolver->prepareSolve(getCollisionWorld()->getNumCollisionObjects(), getCollisionWorld()->getDispatcher()->getNumManifolds());
-
-#ifndef BT_USE_VIRTUAL_CLEARFORCES_AND_GRAVITY
-	{
-		BT_PROFILE("btMultiBody addForce");
-		for (int i = 0; i < this->m_multiBodies.size(); i++)
-		{
-			btMultiBody* bod = m_multiBodies[i];
-
-			bool isSleeping = false;
-
-			if (bod->getBaseCollider() && bod->getBaseCollider()->getActivationState() == ISLAND_SLEEPING)
-			{
-				isSleeping = true;
-			}
-			for (int b = 0; b < bod->getNumLinks(); b++)
-			{
-				if (bod->getLink(b).m_collider && bod->getLink(b).m_collider->getActivationState() == ISLAND_SLEEPING)
-					isSleeping = true;
-			}
-
-			if (!isSleeping)
-			{
-				//useless? they get resized in stepVelocities once again (AND DIFFERENTLY)
-				m_scratch_r.resize(bod->getNumLinks() + 1);  //multidof? ("Y"s use it and it is used to store qdd)
-				m_scratch_v.resize(bod->getNumLinks() + 1);
-				m_scratch_m.resize(bod->getNumLinks() + 1);
-
-				bod->addBaseForce(m_gravity * bod->getBaseMass());
-
-				for (int j = 0; j < bod->getNumLinks(); ++j)
-				{
-					bod->addLinkForce(j, m_gravity * bod->getLinkMass(j));
-				}
-			}  //if (!isSleeping)
-		}
-	}
-#endif  //BT_USE_VIRTUAL_CLEARFORCES_AND_GRAVITY
-
-	{
-		BT_PROFILE("btMultiBody stepVelocities");
-		for (int i = 0; i < this->m_multiBodies.size(); i++)
-		{
-			btMultiBody* bod = m_multiBodies[i];
-
-			bool isSleeping = false;
-
-			if (bod->getBaseCollider() && bod->getBaseCollider()->getActivationState() == ISLAND_SLEEPING)
-			{
-				isSleeping = true;
-			}
-			for (int b = 0; b < bod->getNumLinks(); b++)
-			{
-				if (bod->getLink(b).m_collider && bod->getLink(b).m_collider->getActivationState() == ISLAND_SLEEPING)
-					isSleeping = true;
-			}
-
-			if (!isSleeping)
-			{
-				//useless? they get resized in stepVelocities once again (AND DIFFERENTLY)
-				m_scratch_r.resize(bod->getNumLinks() + 1);  //multidof? ("Y"s use it and it is used to store qdd)
-				m_scratch_v.resize(bod->getNumLinks() + 1);
-				m_scratch_m.resize(bod->getNumLinks() + 1);
-				bool doNotUpdatePos = false;
-                bool isConstraintPass = false;
-				{
-					if (!bod->isUsingRK4Integration())
-					{
-						bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(solverInfo.m_timeStep,
-						m_scratch_r, m_scratch_v, m_scratch_m,isConstraintPass,
-						getSolverInfo().m_jointFeedbackInWorldSpace,
-						getSolverInfo().m_jointFeedbackInJointFrame);
-					}
-					else
-					{
-						//
-						int numDofs = bod->getNumDofs() + 6;
-						int numPosVars = bod->getNumPosVars() + 7;
-						btAlignedObjectArray<btScalar> scratch_r2;
-						scratch_r2.resize(2 * numPosVars + 8 * numDofs);
-						//convenience
-						btScalar* pMem = &scratch_r2[0];
-						btScalar* scratch_q0 = pMem;
-						pMem += numPosVars;
-						btScalar* scratch_qx = pMem;
-						pMem += numPosVars;
-						btScalar* scratch_qd0 = pMem;
-						pMem += numDofs;
-						btScalar* scratch_qd1 = pMem;
-						pMem += numDofs;
-						btScalar* scratch_qd2 = pMem;
-						pMem += numDofs;
-						btScalar* scratch_qd3 = pMem;
-						pMem += numDofs;
-						btScalar* scratch_qdd0 = pMem;
-						pMem += numDofs;
-						btScalar* scratch_qdd1 = pMem;
-						pMem += numDofs;
-						btScalar* scratch_qdd2 = pMem;
-						pMem += numDofs;
-						btScalar* scratch_qdd3 = pMem;
-						pMem += numDofs;
-						btAssert((pMem - (2 * numPosVars + 8 * numDofs)) == &scratch_r2[0]);
-
-						/////
-						//copy q0 to scratch_q0 and qd0 to scratch_qd0
-						scratch_q0[0] = bod->getWorldToBaseRot().x();
-						scratch_q0[1] = bod->getWorldToBaseRot().y();
-						scratch_q0[2] = bod->getWorldToBaseRot().z();
-						scratch_q0[3] = bod->getWorldToBaseRot().w();
-						scratch_q0[4] = bod->getBasePos().x();
-						scratch_q0[5] = bod->getBasePos().y();
-						scratch_q0[6] = bod->getBasePos().z();
-						//
-						for (int link = 0; link < bod->getNumLinks(); ++link)
-						{
-							for (int dof = 0; dof < bod->getLink(link).m_posVarCount; ++dof)
-								scratch_q0[7 + bod->getLink(link).m_cfgOffset + dof] = bod->getLink(link).m_jointPos[dof];
-						}
-						//
-						for (int dof = 0; dof < numDofs; ++dof)
-							scratch_qd0[dof] = bod->getVelocityVector()[dof];
-						////
-						struct
-						{
-							btMultiBody* bod;
-							btScalar *scratch_qx, *scratch_q0;
-
-							void operator()()
-							{
-								for (int dof = 0; dof < bod->getNumPosVars() + 7; ++dof)
-									scratch_qx[dof] = scratch_q0[dof];
-							}
-						} pResetQx = {bod, scratch_qx, scratch_q0};
-						//
-						struct
-						{
-							void operator()(btScalar dt, const btScalar* pDer, const btScalar* pCurVal, btScalar* pVal, int size)
-							{
-								for (int i = 0; i < size; ++i)
-									pVal[i] = pCurVal[i] + dt * pDer[i];
-							}
-
-						} pEulerIntegrate;
-						//
-						struct
-						{
-							void operator()(btMultiBody* pBody, const btScalar* pData)
-							{
-								btScalar* pVel = const_cast<btScalar*>(pBody->getVelocityVector());
-
-								for (int i = 0; i < pBody->getNumDofs() + 6; ++i)
-									pVel[i] = pData[i];
-							}
-						} pCopyToVelocityVector;
-						//
-						struct
-						{
-							void operator()(const btScalar* pSrc, btScalar* pDst, int start, int size)
-							{
-								for (int i = 0; i < size; ++i)
-									pDst[i] = pSrc[start + i];
-							}
-						} pCopy;
-						//
-
-						btScalar h = solverInfo.m_timeStep;
-#define output &m_scratch_r[bod->getNumDofs()]
-						//calc qdd0 from: q0 & qd0
-						bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(0., m_scratch_r, m_scratch_v, m_scratch_m,
-						isConstraintPass,getSolverInfo().m_jointFeedbackInWorldSpace,
-						getSolverInfo().m_jointFeedbackInJointFrame);
-						pCopy(output, scratch_qdd0, 0, numDofs);
-						//calc q1 = q0 + h/2 * qd0
-						pResetQx();
-						bod->stepPositionsMultiDof(btScalar(.5) * h, scratch_qx, scratch_qd0);
-						//calc qd1 = qd0 + h/2 * qdd0
-						pEulerIntegrate(btScalar(.5) * h, scratch_qdd0, scratch_qd0, scratch_qd1, numDofs);
-						//
-						//calc qdd1 from: q1 & qd1
-						pCopyToVelocityVector(bod, scratch_qd1);
-						bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(0., m_scratch_r, m_scratch_v, m_scratch_m,
-						isConstraintPass,getSolverInfo().m_jointFeedbackInWorldSpace,
-						getSolverInfo().m_jointFeedbackInJointFrame);
-						pCopy(output, scratch_qdd1, 0, numDofs);
-						//calc q2 = q0 + h/2 * qd1
-						pResetQx();
-						bod->stepPositionsMultiDof(btScalar(.5) * h, scratch_qx, scratch_qd1);
-						//calc qd2 = qd0 + h/2 * qdd1
-						pEulerIntegrate(btScalar(.5) * h, scratch_qdd1, scratch_qd0, scratch_qd2, numDofs);
-						//
-						//calc qdd2 from: q2 & qd2
-						pCopyToVelocityVector(bod, scratch_qd2);
-						bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(0., m_scratch_r, m_scratch_v, m_scratch_m,
-						isConstraintPass,getSolverInfo().m_jointFeedbackInWorldSpace,
-						getSolverInfo().m_jointFeedbackInJointFrame);
-						pCopy(output, scratch_qdd2, 0, numDofs);
-						//calc q3 = q0 + h * qd2
-						pResetQx();
-						bod->stepPositionsMultiDof(h, scratch_qx, scratch_qd2);
-						//calc qd3 = qd0 + h * qdd2
-						pEulerIntegrate(h, scratch_qdd2, scratch_qd0, scratch_qd3, numDofs);
-						//
-						//calc qdd3 from: q3 & qd3
-						pCopyToVelocityVector(bod, scratch_qd3);
-						bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(0., m_scratch_r, m_scratch_v, m_scratch_m,
-						isConstraintPass,getSolverInfo().m_jointFeedbackInWorldSpace,
-						getSolverInfo().m_jointFeedbackInJointFrame);
-						pCopy(output, scratch_qdd3, 0, numDofs);
-
-						//
-						//calc q = q0 + h/6(qd0 + 2*(qd1 + qd2) + qd3)
-						//calc qd = qd0 + h/6(qdd0 + 2*(qdd1 + qdd2) + qdd3)
-						btAlignedObjectArray<btScalar> delta_q;
-						delta_q.resize(numDofs);
-						btAlignedObjectArray<btScalar> delta_qd;
-						delta_qd.resize(numDofs);
-						for (int i = 0; i < numDofs; ++i)
-						{
-							delta_q[i] = h / btScalar(6.) * (scratch_qd0[i] + 2 * scratch_qd1[i] + 2 * scratch_qd2[i] + scratch_qd3[i]);
-							delta_qd[i] = h / btScalar(6.) * (scratch_qdd0[i] + 2 * scratch_qdd1[i] + 2 * scratch_qdd2[i] + scratch_qdd3[i]);
-							//delta_q[i] = h*scratch_qd0[i];
-							//delta_qd[i] = h*scratch_qdd0[i];
-						}
-						//
-						pCopyToVelocityVector(bod, scratch_qd0);
-						bod->applyDeltaVeeMultiDof(&delta_qd[0], 1);
-						//
-						if (!doNotUpdatePos)
-						{
-							btScalar* pRealBuf = const_cast<btScalar*>(bod->getVelocityVector());
-							pRealBuf += 6 + bod->getNumDofs() + bod->getNumDofs() * bod->getNumDofs();
-
-							for (int i = 0; i < numDofs; ++i)
-								pRealBuf[i] = delta_q[i];
-
-							//bod->stepPositionsMultiDof(1, 0, &delta_q[0]);
-							bod->setPosUpdated(true);
-						}
-
-						//ugly hack which resets the cached data to t0 (needed for constraint solver)
-						{
-							for (int link = 0; link < bod->getNumLinks(); ++link)
-								bod->getLink(link).updateCacheMultiDof();
-							bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(0, m_scratch_r, m_scratch_v, m_scratch_m,
-							isConstraintPass,getSolverInfo().m_jointFeedbackInWorldSpace,
-							getSolverInfo().m_jointFeedbackInJointFrame);
-						}
-					}
-				}
-
-#ifndef BT_USE_VIRTUAL_CLEARFORCES_AND_GRAVITY
-				bod->clearForcesAndTorques();
-#endif         //BT_USE_VIRTUAL_CLEARFORCES_AND_GRAVITY
-			}  //if (!isSleeping)
-		}
-	}
+    solveExternalForces(solverInfo);
 
 	/// solve all the constraints for this island
 	m_islandManager->buildAndProcessIslands(getCollisionWorld()->getDispatcher(), getCollisionWorld(), m_solverMultiBodyIslandCallback);
@@ -760,6 +479,298 @@ void btMultiBodyDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo)
 	}
 }
 
+void btMultiBodyDynamicsWorld::solveExternalForces(btContactSolverInfo& solverInfo)
+{
+    forwardKinematics();
+    
+    BT_PROFILE("solveConstraints");
+    
+    clearMultiBodyConstraintForces();
+    
+    m_sortedConstraints.resize(m_constraints.size());
+    int i;
+    for (i = 0; i < getNumConstraints(); i++)
+    {
+        m_sortedConstraints[i] = m_constraints[i];
+    }
+    m_sortedConstraints.quickSort(btSortConstraintOnIslandPredicate2());
+    btTypedConstraint** constraintsPtr = getNumConstraints() ? &m_sortedConstraints[0] : 0;
+    
+    m_sortedMultiBodyConstraints.resize(m_multiBodyConstraints.size());
+    for (i = 0; i < m_multiBodyConstraints.size(); i++)
+    {
+        m_sortedMultiBodyConstraints[i] = m_multiBodyConstraints[i];
+    }
+    m_sortedMultiBodyConstraints.quickSort(btSortMultiBodyConstraintOnIslandPredicate());
+    
+    btMultiBodyConstraint** sortedMultiBodyConstraints = m_sortedMultiBodyConstraints.size() ? &m_sortedMultiBodyConstraints[0] : 0;
+    
+    m_solverMultiBodyIslandCallback->setup(&solverInfo, constraintsPtr, m_sortedConstraints.size(), sortedMultiBodyConstraints, m_sortedMultiBodyConstraints.size(), getDebugDrawer());
+    m_constraintSolver->prepareSolve(getCollisionWorld()->getNumCollisionObjects(), getCollisionWorld()->getDispatcher()->getNumManifolds());
+    
+#ifndef BT_USE_VIRTUAL_CLEARFORCES_AND_GRAVITY
+    {
+        BT_PROFILE("btMultiBody addForce");
+        for (int i = 0; i < this->m_multiBodies.size(); i++)
+        {
+            btMultiBody* bod = m_multiBodies[i];
+            
+            bool isSleeping = false;
+            
+            if (bod->getBaseCollider() && bod->getBaseCollider()->getActivationState() == ISLAND_SLEEPING)
+            {
+                isSleeping = true;
+            }
+            for (int b = 0; b < bod->getNumLinks(); b++)
+            {
+                if (bod->getLink(b).m_collider && bod->getLink(b).m_collider->getActivationState() == ISLAND_SLEEPING)
+                    isSleeping = true;
+            }
+            
+            if (!isSleeping)
+            {
+                //useless? they get resized in stepVelocities once again (AND DIFFERENTLY)
+                m_scratch_r.resize(bod->getNumLinks() + 1);  //multidof? ("Y"s use it and it is used to store qdd)
+                m_scratch_v.resize(bod->getNumLinks() + 1);
+                m_scratch_m.resize(bod->getNumLinks() + 1);
+                
+                bod->addBaseForce(m_gravity * bod->getBaseMass());
+                
+                for (int j = 0; j < bod->getNumLinks(); ++j)
+                {
+                    bod->addLinkForce(j, m_gravity * bod->getLinkMass(j));
+                }
+            }  //if (!isSleeping)
+        }
+    }
+#endif  //BT_USE_VIRTUAL_CLEARFORCES_AND_GRAVITY
+    
+    {
+        BT_PROFILE("btMultiBody stepVelocities");
+        for (int i = 0; i < this->m_multiBodies.size(); i++)
+        {
+            btMultiBody* bod = m_multiBodies[i];
+            
+            bool isSleeping = false;
+            
+            if (bod->getBaseCollider() && bod->getBaseCollider()->getActivationState() == ISLAND_SLEEPING)
+            {
+                isSleeping = true;
+            }
+            for (int b = 0; b < bod->getNumLinks(); b++)
+            {
+                if (bod->getLink(b).m_collider && bod->getLink(b).m_collider->getActivationState() == ISLAND_SLEEPING)
+                    isSleeping = true;
+            }
+            
+            if (!isSleeping)
+            {
+                //useless? they get resized in stepVelocities once again (AND DIFFERENTLY)
+                m_scratch_r.resize(bod->getNumLinks() + 1);  //multidof? ("Y"s use it and it is used to store qdd)
+                m_scratch_v.resize(bod->getNumLinks() + 1);
+                m_scratch_m.resize(bod->getNumLinks() + 1);
+                bool doNotUpdatePos = false;
+                bool isConstraintPass = false;
+                {
+                    if (!bod->isUsingRK4Integration())
+                    {
+                        const btScalar linearDamp = bod->getLinearDamping();
+//                        const btScalar angularDamp = bod->getAngularDamping();
+                        bod->setLinearDamping(0);
+                        bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(solverInfo.m_timeStep,
+                                                                                  m_scratch_r, m_scratch_v, m_scratch_m,isConstraintPass,
+                                                                                  getSolverInfo().m_jointFeedbackInWorldSpace,
+                                                                                  getSolverInfo().m_jointFeedbackInJointFrame);
+                        bod->setLinearDamping(linearDamp);
+//                        bod->setAngularDamping(angularDamp);
+                    }
+                    else
+                    {
+                        //
+                        int numDofs = bod->getNumDofs() + 6;
+                        int numPosVars = bod->getNumPosVars() + 7;
+                        btAlignedObjectArray<btScalar> scratch_r2;
+                        scratch_r2.resize(2 * numPosVars + 8 * numDofs);
+                        //convenience
+                        btScalar* pMem = &scratch_r2[0];
+                        btScalar* scratch_q0 = pMem;
+                        pMem += numPosVars;
+                        btScalar* scratch_qx = pMem;
+                        pMem += numPosVars;
+                        btScalar* scratch_qd0 = pMem;
+                        pMem += numDofs;
+                        btScalar* scratch_qd1 = pMem;
+                        pMem += numDofs;
+                        btScalar* scratch_qd2 = pMem;
+                        pMem += numDofs;
+                        btScalar* scratch_qd3 = pMem;
+                        pMem += numDofs;
+                        btScalar* scratch_qdd0 = pMem;
+                        pMem += numDofs;
+                        btScalar* scratch_qdd1 = pMem;
+                        pMem += numDofs;
+                        btScalar* scratch_qdd2 = pMem;
+                        pMem += numDofs;
+                        btScalar* scratch_qdd3 = pMem;
+                        pMem += numDofs;
+                        btAssert((pMem - (2 * numPosVars + 8 * numDofs)) == &scratch_r2[0]);
+                        
+                        /////
+                        //copy q0 to scratch_q0 and qd0 to scratch_qd0
+                        scratch_q0[0] = bod->getWorldToBaseRot().x();
+                        scratch_q0[1] = bod->getWorldToBaseRot().y();
+                        scratch_q0[2] = bod->getWorldToBaseRot().z();
+                        scratch_q0[3] = bod->getWorldToBaseRot().w();
+                        scratch_q0[4] = bod->getBasePos().x();
+                        scratch_q0[5] = bod->getBasePos().y();
+                        scratch_q0[6] = bod->getBasePos().z();
+                        //
+                        for (int link = 0; link < bod->getNumLinks(); ++link)
+                        {
+                            for (int dof = 0; dof < bod->getLink(link).m_posVarCount; ++dof)
+                                scratch_q0[7 + bod->getLink(link).m_cfgOffset + dof] = bod->getLink(link).m_jointPos[dof];
+                        }
+                        //
+                        for (int dof = 0; dof < numDofs; ++dof)
+                            scratch_qd0[dof] = bod->getVelocityVector()[dof];
+                        ////
+                        struct
+                        {
+                            btMultiBody* bod;
+                            btScalar *scratch_qx, *scratch_q0;
+                            
+                            void operator()()
+                            {
+                                for (int dof = 0; dof < bod->getNumPosVars() + 7; ++dof)
+                                    scratch_qx[dof] = scratch_q0[dof];
+                            }
+                        } pResetQx = {bod, scratch_qx, scratch_q0};
+                        //
+                        struct
+                        {
+                            void operator()(btScalar dt, const btScalar* pDer, const btScalar* pCurVal, btScalar* pVal, int size)
+                            {
+                                for (int i = 0; i < size; ++i)
+                                    pVal[i] = pCurVal[i] + dt * pDer[i];
+                            }
+                            
+                        } pEulerIntegrate;
+                        //
+                        struct
+                        {
+                            void operator()(btMultiBody* pBody, const btScalar* pData)
+                            {
+                                btScalar* pVel = const_cast<btScalar*>(pBody->getVelocityVector());
+                                
+                                for (int i = 0; i < pBody->getNumDofs() + 6; ++i)
+                                    pVel[i] = pData[i];
+                            }
+                        } pCopyToVelocityVector;
+                        //
+                        struct
+                        {
+                            void operator()(const btScalar* pSrc, btScalar* pDst, int start, int size)
+                            {
+                                for (int i = 0; i < size; ++i)
+                                    pDst[i] = pSrc[start + i];
+                            }
+                        } pCopy;
+                        //
+                        
+                        btScalar h = solverInfo.m_timeStep;
+#define output &m_scratch_r[bod->getNumDofs()]
+                        //calc qdd0 from: q0 & qd0
+                        bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(0., m_scratch_r, m_scratch_v, m_scratch_m,
+                                                                                  isConstraintPass,getSolverInfo().m_jointFeedbackInWorldSpace,
+                                                                                  getSolverInfo().m_jointFeedbackInJointFrame);
+                        pCopy(output, scratch_qdd0, 0, numDofs);
+                        //calc q1 = q0 + h/2 * qd0
+                        pResetQx();
+                        bod->stepPositionsMultiDof(btScalar(.5) * h, scratch_qx, scratch_qd0);
+                        //calc qd1 = qd0 + h/2 * qdd0
+                        pEulerIntegrate(btScalar(.5) * h, scratch_qdd0, scratch_qd0, scratch_qd1, numDofs);
+                        //
+                        //calc qdd1 from: q1 & qd1
+                        pCopyToVelocityVector(bod, scratch_qd1);
+                        bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(0., m_scratch_r, m_scratch_v, m_scratch_m,
+                                                                                  isConstraintPass,getSolverInfo().m_jointFeedbackInWorldSpace,
+                                                                                  getSolverInfo().m_jointFeedbackInJointFrame);
+                        pCopy(output, scratch_qdd1, 0, numDofs);
+                        //calc q2 = q0 + h/2 * qd1
+                        pResetQx();
+                        bod->stepPositionsMultiDof(btScalar(.5) * h, scratch_qx, scratch_qd1);
+                        //calc qd2 = qd0 + h/2 * qdd1
+                        pEulerIntegrate(btScalar(.5) * h, scratch_qdd1, scratch_qd0, scratch_qd2, numDofs);
+                        //
+                        //calc qdd2 from: q2 & qd2
+                        pCopyToVelocityVector(bod, scratch_qd2);
+                        bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(0., m_scratch_r, m_scratch_v, m_scratch_m,
+                                                                                  isConstraintPass,getSolverInfo().m_jointFeedbackInWorldSpace,
+                                                                                  getSolverInfo().m_jointFeedbackInJointFrame);
+                        pCopy(output, scratch_qdd2, 0, numDofs);
+                        //calc q3 = q0 + h * qd2
+                        pResetQx();
+                        bod->stepPositionsMultiDof(h, scratch_qx, scratch_qd2);
+                        //calc qd3 = qd0 + h * qdd2
+                        pEulerIntegrate(h, scratch_qdd2, scratch_qd0, scratch_qd3, numDofs);
+                        //
+                        //calc qdd3 from: q3 & qd3
+                        pCopyToVelocityVector(bod, scratch_qd3);
+                        bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(0., m_scratch_r, m_scratch_v, m_scratch_m,
+                                                                                  isConstraintPass,getSolverInfo().m_jointFeedbackInWorldSpace,
+                                                                                  getSolverInfo().m_jointFeedbackInJointFrame);
+                        pCopy(output, scratch_qdd3, 0, numDofs);
+                        
+                        //
+                        //calc q = q0 + h/6(qd0 + 2*(qd1 + qd2) + qd3)
+                        //calc qd = qd0 + h/6(qdd0 + 2*(qdd1 + qdd2) + qdd3)
+                        btAlignedObjectArray<btScalar> delta_q;
+                        delta_q.resize(numDofs);
+                        btAlignedObjectArray<btScalar> delta_qd;
+                        delta_qd.resize(numDofs);
+                        for (int i = 0; i < numDofs; ++i)
+                        {
+                            delta_q[i] = h / btScalar(6.) * (scratch_qd0[i] + 2 * scratch_qd1[i] + 2 * scratch_qd2[i] + scratch_qd3[i]);
+                            delta_qd[i] = h / btScalar(6.) * (scratch_qdd0[i] + 2 * scratch_qdd1[i] + 2 * scratch_qdd2[i] + scratch_qdd3[i]);
+                            //delta_q[i] = h*scratch_qd0[i];
+                            //delta_qd[i] = h*scratch_qdd0[i];
+                        }
+                        //
+                        pCopyToVelocityVector(bod, scratch_qd0);
+                        bod->applyDeltaVeeMultiDof(&delta_qd[0], 1);
+                        //
+                        if (!doNotUpdatePos)
+                        {
+                            btScalar* pRealBuf = const_cast<btScalar*>(bod->getVelocityVector());
+                            pRealBuf += 6 + bod->getNumDofs() + bod->getNumDofs() * bod->getNumDofs();
+                            
+                            for (int i = 0; i < numDofs; ++i)
+                                pRealBuf[i] = delta_q[i];
+                            
+                            //bod->stepPositionsMultiDof(1, 0, &delta_q[0]);
+                            bod->setPosUpdated(true);
+                        }
+                        
+                        //ugly hack which resets the cached data to t0 (needed for constraint solver)
+                        {
+                            for (int link = 0; link < bod->getNumLinks(); ++link)
+                                bod->getLink(link).updateCacheMultiDof();
+                            bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(0, m_scratch_r, m_scratch_v, m_scratch_m,
+                                                                                      isConstraintPass,getSolverInfo().m_jointFeedbackInWorldSpace,
+                                                                                      getSolverInfo().m_jointFeedbackInJointFrame);
+                        }
+                    }
+                }
+                
+#ifndef BT_USE_VIRTUAL_CLEARFORCES_AND_GRAVITY
+                bod->clearForcesAndTorques();
+#endif         //BT_USE_VIRTUAL_CLEARFORCES_AND_GRAVITY
+            }  //if (!isSleeping)
+        }
+    }
+}
+
+
 void btMultiBodyDynamicsWorld::integrateTransforms(btScalar timeStep)
 {
 	btDiscreteDynamicsWorld::integrateTransforms(timeStep);
diff --git a/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h b/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h
index 23641a37b..019d1bd87 100644
--- a/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h
+++ b/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h
@@ -111,6 +111,8 @@ public:
 	virtual void setMultiBodyConstraintSolver(btMultiBodyConstraintSolver* solver);
 	virtual void setConstraintSolver(btConstraintSolver* solver);
 	virtual void getAnalyticsData(btAlignedObjectArray<struct btSolverAnalyticsData>& m_islandAnalyticsData) const;
+    
+    virtual void solveExternalForces(btContactSolverInfo& solverInfo);
 
 };
 #endif  //BT_MULTIBODY_DYNAMICS_WORLD_H