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); }