void MD5Animation::update(float dt)
{
m_AnimationTime += dt;
if (m_AnimationTime > m_AnimationDuration)
{
m_AnimationTime = 0.0f;
}
// frame 5
// 5.5
// frame 6
float curframe = m_AnimationTime * float(m_FrameRate);
int frame0 = (int)floorf(curframe);
int frame1 = frame0 + 1;
if (frame0 == m_NumFrames - 1)
{
frame0 = 0;
}
float alpha = curframe - (float)frame0;
interpolateSkeletons(&m_FrameSkeletonArray[frame0], &m_FrameSkeletonArray[frame1], alpha);
}
void ActuatorHybridIKSlerp::process(float relativeTime)
{
//m_returnPose = m_startPose;
if (!m_ikAlreadyPerformed)
{
m_startPose = m_referenceSkeleton->getSubSkeleton(m_startPose);
if (m_perfomAnalyticalIK)
{
m_analyticalIKSolver.process();
}
if (m_adjustSwivel)
{
if (m_kinematicChain->getName() == "rarm")
{
m_swivelActuator.setAngle(-m_angle);
}else if( m_kinematicChain->getName() == "larm" )
{
m_swivelActuator.setAngle(m_angle);
}
if (m_kinematicChain->getName() == "rarm" || m_kinematicChain->getName() == "larm" )
m_swivelActuator.process();
}
m_ikAlreadyPerformed = true;
m_iterativeIKSolver.computeErrorVector();
m_currentError = (float)m_iterativeIKSolver.getErrorNorm();
}
unsigned int i = 0;
if (m_mean > 8.0E-4f || m_mean == 0.0f)
{
while(i < 20 && m_currentError > 0.005f )
{
m_iterativeIKSolver.setGlobalGain(m_currentGain);
m_iterativeIKSolver.process(); // TODO: DEBUG THIS FOR ORIENTATION CONSTRAINTS
i++;
m_currentError = (float)m_iterativeIKSolver.getErrorNorm();
m_mean = (m_mean + abs(m_lastError - m_currentError))/2;
float dError = m_lastError - m_currentError;
if (dError < m_mean)
{
m_currentGain += (1.0f);
}else
{
m_currentGain /= (1.2f);
}
m_lastError = m_currentError;
}
}
//if (m_currentError > 0.001f && m_mean > 8.0E-4f) LOG_DEBUG(ikLogger, " error for this actuator (" << m_kinematicChain->getName() << "): " << m_iterativeIKSolver.getErrorNorm());
LOG_DEBUG(ikLogger, " error for this actuator (HybridIKSlerp) (" << m_kinematicChain->getName() << "): " << m_iterativeIKSolver.getErrorNorm());
if (m_feedbackRequired && relativeTime > 0.99 && m_timeForFeedback)
{
stringstream ss;
ss << "error value: " << m_currentError;
m_feedbackMessage.feedback = ss.str();
UDPFeedbackSender::getInstance()->sendFeedbackMessage(feedbackMessageToString());
m_feedbackRequired = false;
}
m_targetPose = reinterpret_cast<SMRSkeleton*>(m_kinematicChain);
m_returnPose = interpolateSkeletons(m_startPose,*m_targetPose,relativeTime);
}
