From 3ef0ce4890989e8907d306e2a06099c6da309b1d Mon Sep 17 00:00:00 2001
From: WenlongLu <wenlong.will.lu@gmail.com>
Date: Thu, 19 Nov 2020 15:58:28 -0800
Subject: [PATCH] fix the bug where forces are ignore at a static/kinematic
 link while one of its ancesters is not.

---
 src/BulletDynamics/Featherstone/btMultiBody.cpp | 15 +++++++++++++--
 src/BulletDynamics/Featherstone/btMultiBody.h   |  2 ++
 2 files changed, 15 insertions(+), 2 deletions(-)

diff --git a/src/BulletDynamics/Featherstone/btMultiBody.cpp b/src/BulletDynamics/Featherstone/btMultiBody.cpp
index c9bf21991..ef3043d56 100644
--- a/src/BulletDynamics/Featherstone/btMultiBody.cpp
+++ b/src/BulletDynamics/Featherstone/btMultiBody.cpp
@@ -898,7 +898,7 @@ void btMultiBody::computeAccelerationsArticulatedBodyAlgorithmMultiDof(btScalar
 
 		// calculate zhat_i^A
 		//
-		if (isLinkStaticOrKinematic(i))
+		if (isLinkAndAllAncestorsStaticOrKinematic(i))
 		{
 			zeroAccSpatFrc[i].setZero();
 		}
@@ -967,7 +967,7 @@ void btMultiBody::computeAccelerationsArticulatedBodyAlgorithmMultiDof(btScalar
 	// (part of TreeForwardDynamics in Mirtich.)
 	for (int i = num_links - 1; i >= 0; --i)
 	{
-		if(isLinkStaticOrKinematic(i))
+		if(isLinkAndAllAncestorsStaticOrKinematic(i))
 			continue;
 		const int parent = m_links[i].m_parent;
 		fromParent.m_rotMat = rot_from_parent[i + 1];
@@ -2419,3 +2419,14 @@ bool btMultiBody::isLinkStaticOrKinematic(const int i) const
 	}
 	return false;
 }
+
+bool btMultiBody::isLinkAndAllAncestorsStaticOrKinematic(const int i) const
+{
+	int link = i;
+	while (link != -1) {
+		if (!isLinkStaticOrKinematic(link))
+			return false;
+		link = m_links[link].m_parent;
+	}
+	return isBaseStaticOrKinematic();
+}
diff --git a/src/BulletDynamics/Featherstone/btMultiBody.h b/src/BulletDynamics/Featherstone/btMultiBody.h
index 08efdc78e..6d6ab2124 100644
--- a/src/BulletDynamics/Featherstone/btMultiBody.h
+++ b/src/BulletDynamics/Featherstone/btMultiBody.h
@@ -720,6 +720,8 @@ public:
 
 	bool isLinkStaticOrKinematic(const int i) const;
 
+	bool isLinkAndAllAncestorsStaticOrKinematic(const int i) const;
+
 private:
 	btMultiBody(const btMultiBody &);     // not implemented
 	void operator=(const btMultiBody &);  // not implemented