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