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