void CopyPose::updateKinematics(const Timestamp& timestamp)
{
if (timestamp.interpolate) {
if (m_outputDynamic & CTL_POSITION)
interpolateOutput(&m_pos, CTL_POSITIONX, timestamp);
if (m_outputDynamic & CTL_ROTATION)
interpolateOutput(&m_rot, CTL_ROTATIONX, timestamp);
}
pushCache(timestamp);
}
void MovingFrame::updateKinematics(const Timestamp& timestamp)
{
if (timestamp.interpolate) {
if (timestamp.substep) {
// during substepping, update the internal pose from velocity information
Twist localvel = m_internalPose.M.Inverse(m_velocity);
m_internalPose.Integrate(localvel, 1.0/timestamp.realTimestep);
} else {
m_internalPose = m_nextPose;
}
// m_internalPose is updated, recompute the jacobian
updateJacobian();
}
pushCache(timestamp);
}
