//-------------------------------------------------------------------------------------- // Name: FilterAdaptiveDoubleExponential::UpdateSmoothingParameters() // Desc: Updates the smoothing parameters based on the smoothing filter's trend //-------------------------------------------------------------------------------------- void FilterAdaptiveDoubleExponential::Update( const SKinSkeletonRawData& pSkeletonData, const float fDeltaTime ) { for (uint32 i = 0; i < KIN_SKELETON_POSITION_COUNT; i++) { Vec4 vPreviousPosition = m_DoubleExponentialFilter.m_History[ i ].m_vRawPosition; Vec4 vCurrentPosition = pSkeletonData.vSkeletonPositions[ i ]; Vec4 vVelocity = ( vCurrentPosition - vPreviousPosition ) / fDeltaTime; float fVelocity = fabsf( vVelocity.GetLength() ); UpdateSmoothingParameters( i, fVelocity, pSkeletonData.eSkeletonPositionTrackingState[i] ); m_DoubleExponentialFilter.Update( pSkeletonData, i, m_SmoothingParams[ i ] ); } // Copy filtered data to output data memcpy( m_FilteredJoints, m_DoubleExponentialFilter.GetFilteredJoints(), sizeof( m_FilteredJoints ) ); }
void FilterDoubleExponential::Update( const SKinSkeletonRawData& pSkeletonData, uint32 i, const KIN_TRANSFORM_SMOOTH_PARAMETERS& smoothingParams ) { Vec4 vPrevRawPosition; Vec4 vPrevFilteredPosition; Vec4 vPrevTrend; Vec4 vRawPosition; Vec4 vFilteredPosition; Vec4 vPredictedPosition; Vec4 vDiff; Vec4 vTrend; Vec4 vLength; float fDiff; BOOL bJointIsValid; const Vec4* __restrict pJointPositions = pSkeletonData.vSkeletonPositions; vRawPosition = pJointPositions[i]; vPrevFilteredPosition = m_History[i].m_vFilteredPosition; vPrevTrend = m_History[i].m_vTrend; vPrevRawPosition = m_History[i].m_vRawPosition; bJointIsValid = JointPositionIsValid(vRawPosition); // If joint is invalid, reset the filter if (!bJointIsValid) { m_History[i].m_dwFrameCount = 0; } // Initial start values if (m_History[i].m_dwFrameCount == 0) { vFilteredPosition = vRawPosition; vTrend = Vec4(0,0,0,0); m_History[i].m_dwFrameCount++; } else if (m_History[i].m_dwFrameCount == 1) { vFilteredPosition = (vRawPosition + vPrevRawPosition) * 0.5f; vDiff = vFilteredPosition - vPrevFilteredPosition; vTrend = (vDiff * smoothingParams.m_fCorrection) + (vPrevTrend * (1.0f - smoothingParams.m_fCorrection)); m_History[i].m_dwFrameCount++; } else { // First apply jitter filter vDiff = vRawPosition - vPrevFilteredPosition; fDiff = fabs(vDiff.GetLength()); if (fDiff <= smoothingParams.m_fJitterRadius) { vFilteredPosition = vRawPosition * (fDiff/smoothingParams.m_fJitterRadius) + vPrevFilteredPosition * (1.0f - fDiff/smoothingParams.m_fJitterRadius); } else { vFilteredPosition = vRawPosition; } // Now the double exponential smoothing filter vFilteredPosition = vFilteredPosition * (1.0f - smoothingParams.m_fSmoothing) + (vPrevFilteredPosition + vPrevTrend) * smoothingParams.m_fSmoothing; vDiff = vFilteredPosition - vPrevFilteredPosition; vTrend = vDiff * smoothingParams.m_fCorrection + vPrevTrend * (1.0f - smoothingParams.m_fCorrection); } // Predict into the future to reduce latency vPredictedPosition = vFilteredPosition + (vTrend * smoothingParams.m_fPrediction); // Check that we are not too far away from raw data vDiff = vPredictedPosition - vRawPosition; fDiff = fabs(vDiff.GetLength()); if (fDiff > smoothingParams.m_fMaxDeviationRadius) { vPredictedPosition = vPredictedPosition * smoothingParams.m_fMaxDeviationRadius/fDiff + vRawPosition * (1.0f - smoothingParams.m_fMaxDeviationRadius/fDiff); } // Save the data from this frame m_History[i].m_vRawPosition = vRawPosition; m_History[i].m_vFilteredPosition = vFilteredPosition; m_History[i].m_vTrend = vTrend; // Output the data m_FilteredJoints[i] = vPredictedPosition; m_FilteredJoints[i].w = 1.0f; }
//-------------------------------------------------------------------------------------- // The Taylor Series smooths and removes jitter based on a taylor series expansion //-------------------------------------------------------------------------------------- void FilterTaylorSeries::Update( const SKinSkeletonRawData& pSkeletonData, const float fDeltaTime ) { const float fJitterRadius = 0.05f; const float fAlphaCoef = 1.0f - m_fSmoothing; const float fBetaCoeff = (fAlphaCoef * fAlphaCoef ) / ( 2 - fAlphaCoef ); Vec4 vRawPos; // Velocity, acceleration and Jolt are 1st, 2nd and 3rd degree derivatives of position respectively. Vec4 vCurFilteredPos, vEstVelocity, vEstAccelaration, vEstJolt; Vec4 vPrevFilteredPos, vPrevEstVelocity, vPrevEstAccelaration, vPrevEstJolt; Vec4 vDiff; float fDiff; Vec4 vPredicted, vError; Vec4 vConstants(0.0f, 1.0f, 0.5f, 0.1667f); for (int i = 0; i < KIN_SKELETON_POSITION_COUNT; i++) { vRawPos = pSkeletonData.vSkeletonPositions[i]; vPrevFilteredPos = m_History[i].vPos; vPrevEstVelocity = m_History[i].vEstVelocity; vPrevEstAccelaration = m_History[i].vEstAccelaration; vPrevEstJolt = m_History[i].vEstJolt; if (!JointPositionIsValid(vPrevFilteredPos)) { vCurFilteredPos = vRawPos; vEstVelocity = Vec4(0,0,0,0); vEstAccelaration = Vec4(0,0,0,0); vEstJolt = Vec4(0,0,0,0); } else if (!JointPositionIsValid(vRawPos)) { vCurFilteredPos = vPrevFilteredPos; vEstVelocity = vPrevEstVelocity; vEstAccelaration = vPrevEstAccelaration; vEstJolt = vPrevEstJolt; } else { // If the current and previous frames have valid data, perform interpolation vDiff = vPrevFilteredPos - vRawPos; fDiff = fabs(vDiff.GetLength()); if (fDiff <= fJitterRadius) { vCurFilteredPos = vRawPos * fDiff/fJitterRadius + vPrevFilteredPos * (1.0f - fDiff/fJitterRadius); } else { vCurFilteredPos = vRawPos; } vPredicted = vPrevFilteredPos + vPrevEstVelocity; vPredicted = vPredicted + vPrevEstAccelaration * (vConstants.y * vConstants.y * vConstants.z); vPredicted = vPredicted + vPrevEstJolt * (vConstants.y * vConstants.y * vConstants.y * vConstants.w); vError = vCurFilteredPos - vPredicted; vCurFilteredPos = vPredicted + vError * fAlphaCoef; vEstVelocity = vPrevEstVelocity + vError * fBetaCoeff; vEstAccelaration = vEstVelocity - vPrevEstVelocity; vEstJolt = vEstAccelaration - vPrevEstAccelaration; } // Update the state m_History[i].vPos = vCurFilteredPos; m_History[i].vEstVelocity = vEstVelocity; m_History[i].vEstAccelaration = vEstAccelaration; m_History[i].vEstJolt = vEstJolt; // Output the data m_FilteredJoints[i] = vCurFilteredPos; m_FilteredJoints[i].w = 1.0f; } }
//-------------------------------------------------------------------------------------- // Name: CombiJointFilter() // Desc: A filter for the positional data. This filter uses a combination of velocity // position history to filter the joint positions. //-------------------------------------------------------------------------------------- void FilterCombination::Update( const SKinSkeletonRawData& pSkeletonData, const float fDeltaTime ) { // Process each joint for ( uint32 nJoint = 0; nJoint < KIN_SKELETON_POSITION_COUNT; ++nJoint ) { // Remember where the camera thinks this joint should be m_History[ nJoint ].m_vWantedPos = pSkeletonData.vSkeletonPositions[ nJoint ]; Vec4 vDelta; vDelta = m_History[ nJoint ].m_vWantedPos - m_History[ nJoint ].m_vLastWantedPos; { Vec4 vBlended; // Calculate the vBlended value - could optimize this by remembering the running total and // subtracting the oldest value and then adding the newest. Saves adding them all up on each frame. vBlended = Vec4(0,0,0,0); for( uint32 k = 0; k < m_nUseTaps; ++k) { vBlended = vBlended + m_History[ nJoint ].m_vPrevDeltas[k]; } vBlended = vBlended / ((float)m_nUseTaps); vBlended.w = 0.0f; vDelta.w = 0.0f; float fDeltaLength = vDelta.GetLength(); float fBlendedLength = vBlended.GetLength(); m_History[ nJoint ].m_fWantedLocalBlendRate = m_fDefaultApplyRate; m_History[ nJoint ].m_bActive[0] = false; m_History[ nJoint ].m_bActive[1] = false; m_History[ nJoint ].m_bActive[2] = false; // Does the current velocity and history have a reasonable magnitude? if( fDeltaLength >= m_fDeltaLengthThreshold && fBlendedLength >= m_fBlendedLengthThreshold ) { float fDotProd; float fConfidence; if( m_bDotProdNormalize ) { Vec4 vDeltaOne = vDelta; vDeltaOne.Normalize(); Vec4 vBlendedOne = vBlended; vBlendedOne.Normalize(); fDotProd = vDeltaOne.Dot( vBlendedOne ); } else { fDotProd = vDelta.Dot(vBlended); } // Is the current frame aligned to the recent history? if( fDotProd >= m_fDotProdThreshold ) { fConfidence = fDotProd; m_History[ nJoint ].m_fWantedLocalBlendRate = min( fConfidence, 1.0f ); m_History[ nJoint ].m_bActive[0] = true; } } assert( m_History[ nJoint ].m_fWantedLocalBlendRate <= 1.0f ); } // Push the previous deltas down the history for( int j = m_nUseTaps-2; j >= 0; --j ) { m_History[ nJoint ].m_vPrevDeltas[j+1] = m_History[ nJoint ].m_vPrevDeltas[j]; } // Store the current history m_History[ nJoint ].m_vPrevDeltas[0] = vDelta; // Remember where the camera thought this joint was on the this frame m_History[ nJoint ].m_vLastWantedPos = m_History[ nJoint ].m_vWantedPos; } // Secondary and tertiary blending for ( uint32 pass = 0; pass < 2; ++pass ) { for ( uint32 bone = 0; bone < g_numBones; ++bone ) { float fRate1; float fRate2; fRate1 = m_History[ g_Bones[bone].startJoint ].m_fWantedLocalBlendRate; fRate2 = m_History[ g_Bones[bone].endJoint ].m_fWantedLocalBlendRate; // Blend down? Start to end if( (fRate1 * m_fDownBlendRate) > fRate2) { // Yes, apply m_History[ g_Bones[bone].endJoint ].m_fWantedLocalBlendRate = ( fRate1 * m_fDownBlendRate ); // Flag m_History[ g_Bones[bone].endJoint ].m_bActive[pass+1] = true; } // Blend down? End to start if( ( fRate2 * m_fDownBlendRate ) > fRate1) { // Yes, apply m_History[ g_Bones[bone].startJoint ].m_fWantedLocalBlendRate = ( fRate2 * m_fDownBlendRate ); // Flag m_History[ g_Bones[bone].startJoint ].m_bActive[pass+1] = true; } } } // Apply for ( uint32 joint = 0; joint < KIN_SKELETON_POSITION_COUNT; ++joint ) { // Blend the blend rate m_History[ joint ].m_fActualLocalBlendRate = Lerp(m_History[ joint ].m_fActualLocalBlendRate, m_History[ joint ].m_fWantedLocalBlendRate, m_fBlendBlendRate); // Blend the actual position towards the wanted positon m_History[ joint ].m_vPos = Lerp(m_History[ joint ].m_vPos, m_History[ joint ].m_vWantedPos, m_History[ joint ].m_fActualLocalBlendRate); m_FilteredJoints[ joint ] = m_History[ joint ].m_vPos; } }