public Joint UpdateJoint(Kinect.JointType jt, Joint rawJoint, TRANSFORM_SMOOTH_PARAMETERS smoothingParams)
{
Joint prevFilteredJoint;
Joint prevRawJoint;
Joint prevTrend;
Joint filteredJoint;
Joint predictedJoint;
Joint diff;
Joint trend;
float fDiff;
prevFilteredJoint = history[jt].FilteredJoint;
prevTrend = history[jt].Trend;
prevRawJoint = history[jt].RawJoint;
// if joint is invalid, reset the filter
bool jointIsValid = JointPositionIsValid(rawJoint);
if (!jointIsValid)
{
history[jt].FrameCount = 0;
}
// initial start values
if (history[jt].FrameCount == 0)
{
filteredJoint = rawJoint;
trend = new Joint();
history[jt].FrameCount++;
}
else if (history[jt].FrameCount == 1)
{
filteredJoint = Joint.Scale(Joint.Add(rawJoint, prevRawJoint), 0.5f);
diff = Joint.Subtract(filteredJoint, prevFilteredJoint);
trend = Joint.Add(Joint.Scale(diff, smoothingParams.fCorrection), Joint.Scale(prevTrend, 1.0f - smoothingParams.fCorrection));
history[jt].FrameCount++;
}
else
{
// First apply jitter filter
diff = Joint.Subtract(rawJoint, prevFilteredJoint);
fDiff = diff.Position.sqrMagnitude;// CSVectorLength(diff);
if (fDiff <= smoothingParams.fJitterRadius)
{
filteredJoint = Joint.Add(Joint.Scale(rawJoint, fDiff / smoothingParams.fJitterRadius),
Joint.Scale(prevFilteredJoint, 1.0f - fDiff / smoothingParams.fJitterRadius));
}
else
{
filteredJoint = rawJoint;
}
// Now the double exponential smoothing filter
filteredJoint = Joint.Add(Joint.Scale(filteredJoint, 1.0f - smoothingParams.fSmoothing),
Joint.Scale(Joint.Add(prevFilteredJoint, prevTrend), smoothingParams.fSmoothing));
diff = Joint.Subtract(filteredJoint, prevFilteredJoint);
trend = Joint.Add(Joint.Scale(diff, smoothingParams.fCorrection), Joint.Scale(prevTrend, 1.0f - smoothingParams.fCorrection));
}
// Predict into the future to reduce latency
predictedJoint = Joint.Add(filteredJoint, Joint.Scale(trend, smoothingParams.fPrediction));
// Check that we are not too far away from raw data
diff = Joint.Subtract(predictedJoint, rawJoint);
fDiff = diff.Position.sqrMagnitude;// XMVector3Length(vLength);
if (fDiff > smoothingParams.fMaxDeviationRadius)
{
predictedJoint = Joint.Add(Joint.Scale(predictedJoint, smoothingParams.fMaxDeviationRadius / fDiff),
Joint.Scale(rawJoint, 1.0f - smoothingParams.fMaxDeviationRadius / fDiff));
}
// Save the data from this frame
history[jt].RawJoint = rawJoint;
history[jt].FilteredJoint = filteredJoint;
history[jt].Trend = trend;
// Output the data
filteredJoints[jt] = predictedJoint;
return(predictedJoint);
}
private static void CreateJoint(Dictionary <string, Joint> list, Transform bone, Joint parent)
{
// check that it is not already created
Joint node = list.FirstOrDefault(x => x.Key == bone.name).Value;
if (node != null)
{
return;
}
// create the joint and add to the list
node = new Joint();
node.Init(bone.name);
node.SetRawtData(bone.position, bone.rotation);
list.Add(bone.name, node);
// if this is a child joint, set the property
if (parent != null)
{
parent.AddChild(node);
}
// traverse children to create tree
foreach (Transform child in bone)
{
CreateJoint(list, child, node);
}
}