diff options
Diffstat (limited to 'src/BulletDynamics/Featherstone/btMultiBodyLink.h')
| -rw-r--r-- | src/BulletDynamics/Featherstone/btMultiBodyLink.h | 79 |
1 files changed, 68 insertions, 11 deletions
diff --git a/src/BulletDynamics/Featherstone/btMultiBodyLink.h b/src/BulletDynamics/Featherstone/btMultiBodyLink.h index 92d41dfac..1cd6b8c07 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodyLink.h +++ b/src/BulletDynamics/Featherstone/btMultiBodyLink.h @@ -111,6 +111,10 @@ struct btMultibodyLink btQuaternion m_cachedRotParentToThis; // rotates vectors in parent frame to vectors in local frame btVector3 m_cachedRVector; // vector from COM of parent to COM of this link, in local frame. + + // predicted verstion + btQuaternion m_cachedRotParentToThis_interpolate; // rotates vectors in parent frame to vectors in local frame + btVector3 m_cachedRVector_interpolate; // vector from COM of parent to COM of this link, in local frame. btVector3 m_appliedForce; // In WORLD frame btVector3 m_appliedTorque; // In WORLD frame @@ -119,6 +123,7 @@ struct btMultibodyLink btVector3 m_appliedConstraintTorque; // In WORLD frame btScalar m_jointPos[7]; + btScalar m_jointPos_interpolate[7]; //m_jointTorque is the joint torque applied by the user using 'addJointTorque'. //It gets set to zero after each internal stepSimulation call @@ -188,42 +193,43 @@ struct btMultibodyLink // routine to update m_cachedRotParentToThis and m_cachedRVector void updateCacheMultiDof(btScalar *pq = 0) { - btScalar *pJointPos = (pq ? pq : &m_jointPos[0]); - + btScalar *pJointPos = (pq ? pq : &m_jointPos[0]); + btQuaternion& cachedRot = m_cachedRotParentToThis; + btVector3& cachedVector =m_cachedRVector; switch (m_jointType) { case eRevolute: { - m_cachedRotParentToThis = btQuaternion(getAxisTop(0), -pJointPos[0]) * m_zeroRotParentToThis; - m_cachedRVector = m_dVector + quatRotate(m_cachedRotParentToThis, m_eVector); + 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 - m_cachedRVector = m_dVector + quatRotate(m_cachedRotParentToThis, m_eVector) + pJointPos[0] * getAxisBottom(0); + cachedVector = m_dVector + quatRotate(m_cachedRotParentToThis, m_eVector) + pJointPos[0] * getAxisBottom(0); break; } case eSpherical: { - m_cachedRotParentToThis = btQuaternion(pJointPos[0], pJointPos[1], pJointPos[2], -pJointPos[3]) * m_zeroRotParentToThis; - m_cachedRVector = m_dVector + quatRotate(m_cachedRotParentToThis, m_eVector); + cachedRot = btQuaternion(pJointPos[0], pJointPos[1], pJointPos[2], -pJointPos[3]) * m_zeroRotParentToThis; + cachedVector = m_dVector + quatRotate(cachedRot, m_eVector); break; } case ePlanar: { - m_cachedRotParentToThis = btQuaternion(getAxisTop(0), -pJointPos[0]) * m_zeroRotParentToThis; - m_cachedRVector = quatRotate(btQuaternion(getAxisTop(0), -pJointPos[0]), pJointPos[1] * getAxisBottom(1) + pJointPos[2] * getAxisBottom(2)) + quatRotate(m_cachedRotParentToThis, m_eVector); + 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: { - m_cachedRotParentToThis = m_zeroRotParentToThis; - m_cachedRVector = m_dVector + quatRotate(m_cachedRotParentToThis, m_eVector); + cachedRot = m_zeroRotParentToThis; + cachedVector = m_dVector + quatRotate(cachedRot, m_eVector); break; } @@ -234,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 |