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

src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp

@@ -420,6 +420,66 @@ void btMultiBodyDynamicsWorld::forwardKinematics()
 	}
 }
 void btMultiBodyDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo)
+{
+    solveExternalForces(solverInfo);
+
+	/// solve all the constraints for this island
+	m_islandManager->buildAndProcessIslands(getCollisionWorld()->getDispatcher(), getCollisionWorld(), m_solverMultiBodyIslandCallback);
+
+	m_solverMultiBodyIslandCallback->processConstraints();
+
+	m_constraintSolver->allSolved(solverInfo, m_debugDrawer);
+
+	{
+		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);
+
+				if (bod->internalNeedsJointFeedback())
+				{
+					if (!bod->isUsingRK4Integration())
+					{
+						if (bod->internalNeedsJointFeedback())
+						{
+							bool isConstraintPass = true;
+							bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(solverInfo.m_timeStep, m_scratch_r, m_scratch_v, m_scratch_m, isConstraintPass,
+								getSolverInfo().m_jointFeedbackInWorldSpace,
+								getSolverInfo().m_jointFeedbackInJointFrame);
+						}
+					}
+				}
+			}
+		}
+	}
+
+	for (int i = 0; i < this->m_multiBodies.size(); i++)
+	{
+		btMultiBody* bod = m_multiBodies[i];
+		bod->processDeltaVeeMultiDof2();
+	}
+}
+
+void btMultiBodyDynamicsWorld::solveExternalForces(btContactSolverInfo& solverInfo)
 {
     forwardKinematics();
     
@@ -514,10 +574,15 @@ void btMultiBodyDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo)
                 {
                     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
                     {
@@ -703,62 +768,8 @@ void btMultiBodyDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo)
             }  //if (!isSleeping)
         }
     }
-
-	/// solve all the constraints for this island
-	m_islandManager->buildAndProcessIslands(getCollisionWorld()->getDispatcher(), getCollisionWorld(), m_solverMultiBodyIslandCallback);
-
-	m_solverMultiBodyIslandCallback->processConstraints();
-
-	m_constraintSolver->allSolved(solverInfo, m_debugDrawer);
-
-	{
-		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);
-
-				if (bod->internalNeedsJointFeedback())
-				{
-					if (!bod->isUsingRK4Integration())
-					{
-						if (bod->internalNeedsJointFeedback())
-						{
-							bool isConstraintPass = true;
-							bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(solverInfo.m_timeStep, m_scratch_r, m_scratch_v, m_scratch_m, isConstraintPass,
-								getSolverInfo().m_jointFeedbackInWorldSpace,
-								getSolverInfo().m_jointFeedbackInJointFrame);
-						}
-					}
-				}
-			}
-		}
-	}
-
-	for (int i = 0; i < this->m_multiBodies.size(); i++)
-	{
-		btMultiBody* bod = m_multiBodies[i];
-		bod->processDeltaVeeMultiDof2();
-	}
-}
 
 void btMultiBodyDynamicsWorld::integrateTransforms(btScalar timeStep)
 {

src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h

@@ -112,5 +112,7 @@ public:
 	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