//--------------------------------------------------------------------------------------
// The Taylor Series smooths and removes jitter based on a taylor series expansion
//--------------------------------------------------------------------------------------
VOID FilterTaylorSeries::Update( const XMVECTOR* pJointPositions )
{
const FLOAT fJitterRadius = 0.05f;
const FLOAT fAlphaCoef = 1.0f - m_fSmoothing;
const FLOAT fBetaCoeff = (fAlphaCoef * fAlphaCoef ) / ( 2 - fAlphaCoef );
XMVECTOR vRawPos;
XMVECTOR vCurFilteredPos, vCurEstVel, vCurEstVel2, vCurEstVel3;
XMVECTOR vPrevFilteredPos, vPrevEstVel, vPrevEstVel2, vPrevEstVel3;
XMVECTOR vDiff;
FLOAT fDiff;
XMVECTOR vPredicted, vError;
XMVECTOR vConstants = { 0.0f, 1.0f, 0.5f, 0.1667f };
for (INT i = 0; i < NUI_SKELETON_POSITION_COUNT; i++)
{
vRawPos = pJointPositions[i];
vPrevFilteredPos = m_History[i].vPos;
vPrevEstVel = m_History[i].vEstVel;
vPrevEstVel2 = m_History[i].vEstVel2;
vPrevEstVel3 = m_History[i].vEstVel3;
if (!JointPositionIsValid(vPrevFilteredPos))
{
vCurFilteredPos = vRawPos;
vCurEstVel = XMVectorZero();
vCurEstVel2 = XMVectorZero();
vCurEstVel3 = XMVectorZero();
}
else if (!JointPositionIsValid(vRawPos))
{
vCurFilteredPos = vPrevFilteredPos;
vCurEstVel = vPrevEstVel;
vCurEstVel2 = vPrevEstVel2;
vCurEstVel3 = vPrevEstVel3;
}
else
{
// If the current and previous frames have valid data, perform interpolation
vDiff = XMVectorSubtract(vPrevFilteredPos, vRawPos);
fDiff = fabs(XMVector3Length(vDiff).x);
if (fDiff <= fJitterRadius)
{
vCurFilteredPos = XMVectorAdd(XMVectorScale(vRawPos, fDiff/fJitterRadius),
XMVectorScale(vPrevFilteredPos, 1.0f - fDiff/fJitterRadius));
}
else
{
vCurFilteredPos = vRawPos;
}
vPredicted = XMVectorAdd(vPrevFilteredPos, vPrevEstVel);
vPredicted = XMVectorAdd(vPredicted, vPrevEstVel2 * (vConstants.y * vConstants.y * vConstants.z));
vPredicted = XMVectorAdd(vPredicted, vPrevEstVel3 * (vConstants.y * vConstants.y * vConstants.y * vConstants.w));
vError = XMVectorSubtract(vCurFilteredPos, vPredicted);
vCurFilteredPos = XMVectorAdd(vPredicted, vError * fAlphaCoef);
vCurEstVel = XMVectorAdd(vPrevEstVel, vError * fBetaCoeff);
vCurEstVel2 = XMVectorSubtract(vCurEstVel, vPrevEstVel);
vCurEstVel3 = XMVectorSubtract(vCurEstVel2, vPrevEstVel2);
}
// Update the state
m_History[i].vPos = vCurFilteredPos;
m_History[i].vEstVel = vCurEstVel;
m_History[i].vEstVel2 = vCurEstVel2;
m_History[i].vEstVel3 = vCurEstVel3;
// Output the data
m_FilteredJoints[i] = vCurFilteredPos;
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;
}
}
VOID FilterDoubleExponential::Update( const NUI_SKELETON_DATA* pSkeletonData, UINT i, NUI_TRANSFORM_SMOOTH_PARAMETERS smoothingParams )
{
XMVECTOR vPrevRawPosition;
XMVECTOR vPrevFilteredPosition;
XMVECTOR vPrevTrend;
XMVECTOR vRawPosition;
XMVECTOR vFilteredPosition;
XMVECTOR vPredictedPosition;
XMVECTOR vDiff;
XMVECTOR vTrend;
XMVECTOR vLength;
FLOAT fDiff;
BOOL bJointIsValid;
const XMVECTOR* __restrict pJointPositions = pSkeletonData->SkeletonPositions;
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 = XMVectorZero();
m_History[i].m_dwFrameCount++;
}
else if (m_History[i].m_dwFrameCount == 1)
{
vFilteredPosition = XMVectorScale(XMVectorAdd(vRawPosition, vPrevRawPosition), 0.5f);
vDiff = XMVectorSubtract(vFilteredPosition, vPrevFilteredPosition);
vTrend = XMVectorAdd(XMVectorScale(vDiff, smoothingParams.fCorrection), XMVectorScale(vPrevTrend, 1.0f - smoothingParams.fCorrection));
m_History[i].m_dwFrameCount++;
}
else
{
// First apply jitter filter
vDiff = XMVectorSubtract(vRawPosition, vPrevFilteredPosition);
vLength = XMVector3Length(vDiff);
fDiff = fabs(XMVectorGetX(vLength));
if (fDiff <= smoothingParams.fJitterRadius)
{
vFilteredPosition = XMVectorAdd(XMVectorScale(vRawPosition, fDiff/smoothingParams.fJitterRadius),
XMVectorScale(vPrevFilteredPosition, 1.0f - fDiff/smoothingParams.fJitterRadius));
}
else
{
vFilteredPosition = vRawPosition;
}
// Now the double exponential smoothing filter
vFilteredPosition = XMVectorAdd(XMVectorScale(vFilteredPosition, 1.0f - smoothingParams.fSmoothing),
XMVectorScale(XMVectorAdd(vPrevFilteredPosition, vPrevTrend), smoothingParams.fSmoothing));
vDiff = XMVectorSubtract(vFilteredPosition, vPrevFilteredPosition);
vTrend = XMVectorAdd(XMVectorScale(vDiff, smoothingParams.fCorrection), XMVectorScale(vPrevTrend, 1.0f - smoothingParams.fCorrection));
}
// Predict into the future to reduce latency
vPredictedPosition = XMVectorAdd(vFilteredPosition, XMVectorScale(vTrend, smoothingParams.fPrediction));
// Check that we are not too far away from raw data
vDiff = XMVectorSubtract(vPredictedPosition, vRawPosition);
vLength = XMVector3Length(vDiff);
fDiff = fabs(XMVectorGetX(vLength));
if (fDiff > smoothingParams.fMaxDeviationRadius)
{
vPredictedPosition = XMVectorAdd(XMVectorScale(vPredictedPosition, smoothingParams.fMaxDeviationRadius/fDiff),
XMVectorScale(vRawPosition, 1.0f - smoothingParams.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;
}
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;
}