private void FilterBodyJoints(ref KinectInterop.BodyData bodyData, int bodyIndex, ref KinectInterop.SmoothParameters tempSmoothingParams)
{
KinectManager manager = KinectManager.Instance;
int jointsCount = manager.GetJointCount();
for (int jointIndex = 0; jointIndex < jointsCount; jointIndex++)
{
// If not tracked, we smooth a bit more by using a bigger jitter radius
// Always filter feet highly as they are so noisy
if (bodyData.joint[jointIndex].trackingState != KinectInterop.TrackingState.Tracked)
{
tempSmoothingParams.jitterRadius = this.smoothParameters.jitterRadius * 2.0f;
tempSmoothingParams.maxDeviationRadius = smoothParameters.maxDeviationRadius * 2.0f;
}
else
{
tempSmoothingParams.jitterRadius = smoothParameters.jitterRadius;
tempSmoothingParams.maxDeviationRadius = smoothParameters.maxDeviationRadius;
}
bodyData.joint[jointIndex].position = FilterJoint(bodyData.joint[jointIndex].position, bodyIndex, jointIndex, ref tempSmoothingParams);
}
for (int jointIndex = 0; jointIndex < jointsCount; jointIndex++)
{
if (jointIndex == 0)
{
bodyData.position = bodyData.joint[jointIndex].position;
bodyData.orientation = bodyData.joint[jointIndex].orientation;
bodyData.joint[jointIndex].direction = Vector3.zero;
}
else
{
int jParent = (int)manager.GetParentJoint((KinectInterop.JointType)jointIndex);
if (bodyData.joint[jointIndex].trackingState != KinectInterop.TrackingState.NotTracked &&
bodyData.joint[jParent].trackingState != KinectInterop.TrackingState.NotTracked)
{
bodyData.joint[jointIndex].direction =
bodyData.joint[jointIndex].position - bodyData.joint[jParent].position;
}
}
}
}