Bart/bullet3
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

separate multibody position prediction into standalone function

Browse Source
This commit is contained in:
Xuchen Han
2019-08-08 17:14:13 -07:00
parent 96e8dcef0f
commit 436b6c6963
5 changed files with 259 additions and 81 deletions
Download Patch File Download Diff File Expand all files Collapse all files

64
src/BulletDynamics/Featherstone/btMultiBodyLink.h
View File

@@ -191,16 +191,11 @@ struct btMultibodyLink
}
// routine to update m_cachedRotParentToThis and m_cachedRVector
void updateCacheMultiDof(btScalar *pq = 0, bool predict = false)
void updateCacheMultiDof(btScalar *pq = 0)
{
btScalar *pJointPos;
if (!predict)
pJointPos = (pq ? pq : &m_jointPos[0]);
else
pJointPos = &m_jointPos_interpolate[0];
btQuaternion& cachedRot = predict ? m_cachedRotParentToThis_interpolate : m_cachedRotParentToThis;
btVector3& cachedVector = predict ? m_cachedRVector_interpolate : m_cachedRVector;
btScalar *pJointPos = (pq ? pq : &m_jointPos[0]);
btQuaternion& cachedRot = m_cachedRotParentToThis;
btVector3& cachedVector =m_cachedRVector;
switch (m_jointType)
{
case eRevolute:
@@ -245,6 +240,57 @@ struct btMultibodyLink
}
}
}
void updateInterpolationCacheMultiDof()
{
btScalar *pJointPos = &m_jointPos_interpolate[0];
btQuaternion& cachedRot = m_cachedRotParentToThis_interpolate;
btVector3& cachedVector = m_cachedRVector_interpolate;
switch (m_jointType)
{
case eRevolute:
{
cachedRot = btQuaternion(getAxisTop(0), -pJointPos[0]) * m_zeroRotParentToThis;
cachedVector = m_dVector + quatRotate(m_cachedRotParentToThis, m_eVector);
break;
}
case ePrismatic:
{
// m_cachedRotParentToThis never changes, so no need to update
cachedVector = m_dVector + quatRotate(m_cachedRotParentToThis, m_eVector) + pJointPos[0] * getAxisBottom(0);
break;
}
case eSpherical:
{
cachedRot = btQuaternion(pJointPos[0], pJointPos[1], pJointPos[2], -pJointPos[3]) * m_zeroRotParentToThis;
cachedVector = m_dVector + quatRotate(cachedRot, m_eVector);
break;
}
case ePlanar:
{
cachedRot = btQuaternion(getAxisTop(0), -pJointPos[0]) * m_zeroRotParentToThis;
cachedVector = quatRotate(btQuaternion(getAxisTop(0), -pJointPos[0]), pJointPos[1] * getAxisBottom(1) + pJointPos[2] * getAxisBottom(2)) + quatRotate(cachedRot, m_eVector);
break;
}
case eFixed:
{
cachedRot = m_zeroRotParentToThis;
cachedVector = m_dVector + quatRotate(cachedRot, m_eVector);
break;
}
default:
{
//invalid type
btAssert(0);
}
}
}
};
#endif //BT_MULTIBODY_LINK_H