From d6b3f735b6da5eb8918c2067848ea2f0f811ca04 Mon Sep 17 00:00:00 2001
From: erwincoumans <erwin.coumans@gmail.com>
Date: Thu, 9 Nov 2017 10:46:13 -0800
Subject: [PATCH] fix a bug related to joint reaction forces
 (clearMultiBodyConstraintForces was clearing forces after they were computed
 in some cases)

---
 src/BulletDynamics/Featherstone/btMultiBody.cpp              | 3 +++
 src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp | 4 +++-
 src/BulletDynamics/Featherstone/btMultiBodyLink.h            | 2 ++
 3 files changed, 8 insertions(+), 1 deletion(-)

diff --git a/src/BulletDynamics/Featherstone/btMultiBody.cpp b/src/BulletDynamics/Featherstone/btMultiBody.cpp
index 0c7da662e..4b99707ab 100644
--- a/src/BulletDynamics/Featherstone/btMultiBody.cpp
+++ b/src/BulletDynamics/Featherstone/btMultiBody.cpp
@@ -136,6 +136,9 @@ btMultiBody::btMultiBody(int n_links,
 
     m_baseForce.setValue(0, 0, 0);
     m_baseTorque.setValue(0, 0, 0);
+
+	clearConstraintForces();
+	clearForcesAndTorques();
 }
 
 btMultiBody::~btMultiBody()
diff --git a/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp b/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp
index 9eacc2264..5d2eb7e12 100644
--- a/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp
+++ b/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp
@@ -411,6 +411,8 @@ void	btMultiBodyDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo)
 
 	BT_PROFILE("solveConstraints");
 	
+	clearMultiBodyConstraintForces();
+
 	m_sortedConstraints.resize( m_constraints.size());
 	int i; 
 	for (i=0;i<getNumConstraints();i++)
@@ -669,7 +671,7 @@ void	btMultiBodyDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo)
 		}
 	}
 
-	clearMultiBodyConstraintForces();
+	
 
 	m_solverMultiBodyIslandCallback->processConstraints();
 	
diff --git a/src/BulletDynamics/Featherstone/btMultiBodyLink.h b/src/BulletDynamics/Featherstone/btMultiBodyLink.h
index 8129ae901..13e906cd6 100644
--- a/src/BulletDynamics/Featherstone/btMultiBodyLink.h
+++ b/src/BulletDynamics/Featherstone/btMultiBodyLink.h
@@ -182,6 +182,8 @@ btVector3 m_appliedConstraintForce;    // In WORLD frame
 		m_cachedRVector.setValue(0, 0, 0);
 		m_appliedForce.setValue( 0, 0, 0);
 		m_appliedTorque.setValue(0, 0, 0);
+		m_appliedConstraintForce.setValue(0,0,0);
+		m_appliedConstraintTorque.setValue(0,0,0);
 		//		
 		m_jointPos[0] = m_jointPos[1] = m_jointPos[2] = m_jointPos[4] = m_jointPos[5] = m_jointPos[6] = 0.f;
 		m_jointPos[3] = 1.f;			//"quat.w"