Scarfski/bullet3
1
0
Fork 0
mirror of https://github.com/bulletphysics/bullet3.git synced 2026-10-21 00:01:00 +00:00
Code Issues Packages Projects Releases Wiki Activity

fix the bug where forces are ignore at a static/kinematic link while one of its ancesters is not.

Browse Source
This commit is contained in:
WenlongLu
2020-11-19 15:58:28 -08:00
parent ddbe6f7a5a
commit 3ef0ce4890
2 changed files with 15 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files

15
src/BulletDynamics/Featherstone/btMultiBody.cpp
View File

@@ -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();
}

2
src/BulletDynamics/Featherstone/btMultiBody.h
View File

@@ -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