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

Restrict kinematic multibody use cases to only a full chain of kinematic joints.

Browse Source
This commit is contained in:
WenlongLu
2020-11-24 10:36:32 -08:00
parent 8c8059831b
commit c377260d71
3 changed files with 39 additions and 5 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -898,7 +898,7 @@ void btMultiBody::computeAccelerationsArticulatedBodyAlgorithmMultiDof(btScalar
// calculate zhat_i^A
//
if (isLinkAndAllAncestorsStaticOrKinematic(i))
if (isLinkAndAllAncestorsKinematic(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(isLinkAndAllAncestorsStaticOrKinematic(i))
if(isLinkAndAllAncestorsKinematic(i))
continue;
const int parent = m_links[i].m_parent;
fromParent.m_rotMat = rot_from_parent[i + 1];
@@ -1115,7 +1115,7 @@ void btMultiBody::computeAccelerationsArticulatedBodyAlgorithmMultiDof(btScalar
fromParent.transform(spatAcc[parent + 1], spatAcc[i + 1]);
if(!isLinkStaticOrKinematic(i))
if(!isLinkAndAllAncestorsKinematic(i))
{
for (int dof = 0; dof < m_links[i].m_dofCount; ++dof)
{
@@ -1487,7 +1487,7 @@ void btMultiBody::calcAccelerationDeltasMultiDof(const btScalar *force, btScalar
// (part of TreeForwardDynamics in Mirtich.)
for (int i = num_links - 1; i >= 0; --i)
{
if(isLinkStaticOrKinematic(i))
if(isLinkAndAllAncestorsKinematic(i))
continue;
const int parent = m_links[i].m_parent;
fromParent.m_rotMat = rot_from_parent[i + 1];
@@ -1545,7 +1545,7 @@ void btMultiBody::calcAccelerationDeltasMultiDof(const btScalar *force, btScalar
// now do the loop over the m_links
for (int i = 0; i < num_links; ++i)
{
if(isLinkStaticOrKinematic(i))
if(isLinkAndAllAncestorsKinematic(i))
continue;
const int parent = m_links[i].m_parent;
fromParent.m_rotMat = rot_from_parent[i + 1];
@@ -2420,6 +2420,20 @@ bool btMultiBody::isLinkStaticOrKinematic(const int i) const
return false;
}
bool btMultiBody::isLinkKinematic(const int i) const
{
if (i == -1)
{
return isBaseKinematic();
}
else
{
if (m_links[i].m_collider)
return m_links[i].m_collider->isKinematic();
}
return false;
}
bool btMultiBody::isLinkAndAllAncestorsStaticOrKinematic(const int i) const
{
int link = i;
@@ -2430,3 +2444,14 @@ bool btMultiBody::isLinkAndAllAncestorsStaticOrKinematic(const int i) const
}
return isBaseStaticOrKinematic();
}
bool btMultiBody::isLinkAndAllAncestorsKinematic(const int i) const
{
int link = i;
while (link != -1) {
if (!isLinkKinematic(link))
return false;
link = m_links[link].m_parent;
}
return isBaseKinematic();
}

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

@@ -720,8 +720,12 @@ public:
bool isLinkStaticOrKinematic(const int i) const;
bool isLinkKinematic(const int i) const;
bool isLinkAndAllAncestorsStaticOrKinematic(const int i) const;
bool isLinkAndAllAncestorsKinematic(const int i) const;
private:
btMultiBody(const btMultiBody &); // not implemented
void operator=(const btMultiBody &); // not implemented

5
src/BulletDynamics/Featherstone/btMultiBodyLinkCollider.h
View File

@@ -135,6 +135,11 @@ public:
return isStaticOrKinematicObject();
}
bool isKinematic() const
{
return isKinematicObject();
}
void setDynamicType(int dynamicType)
{
int oldFlags = getCollisionFlags();