79 lines
2.6 KiB
C#
79 lines
2.6 KiB
C#
using System.Collections.Generic;
|
|
|
|
public class Smoother
|
|
{
|
|
// Max allowed size of history list.
|
|
private int maxSize = 100;
|
|
|
|
// In case smoother has enough frames in history to perform smooth action.
|
|
private bool hasEnoughForSmoothing = false;
|
|
|
|
// Number of the latest frames used to smooth current position; default 5.
|
|
public int NumberSmoothingFrames { get; set; } = 5;
|
|
|
|
// Holds received data about moves.
|
|
private List<SkeletonPosition> rawData = new List<SkeletonPosition>();
|
|
|
|
// Holds received which are smoothened a little bit.
|
|
private List<SkeletonPosition> smoothenedData = new List<SkeletonPosition>();
|
|
|
|
// Process skeleton position and sends back smoothened or raw based on passed parameter.
|
|
public SkeletonPosition ReceiveNewSensorData(SkeletonPosition newData, bool smoothing)
|
|
{
|
|
// In case list is too big.
|
|
if(rawData.Count > maxSize)
|
|
{
|
|
Resize();
|
|
}
|
|
|
|
// Add new frame data to raw data used for smoothing.
|
|
rawData.Add(newData);
|
|
|
|
// In case value for smoothing is invalid just return original raw frame.
|
|
if (NumberSmoothingFrames <= 1)
|
|
{
|
|
return rawData[rawData.Count - 1];
|
|
}
|
|
|
|
// Mark that smoother has enough frames for smoothing.
|
|
if (rawData.Count > NumberSmoothingFrames)
|
|
{
|
|
hasEnoughForSmoothing = true;
|
|
}
|
|
|
|
if (smoothenedData.Count == 0)
|
|
{
|
|
smoothenedData.Add(newData);
|
|
}
|
|
else
|
|
{
|
|
SkeletonPosition temp = smoothenedData[smoothenedData.Count - 1] + newData;
|
|
if(hasEnoughForSmoothing)
|
|
{
|
|
temp = temp - rawData[rawData.Count - NumberSmoothingFrames];
|
|
}
|
|
smoothenedData.Add(temp);
|
|
}
|
|
|
|
// Smoothened timestamp has the same timestamp as the latest received.
|
|
smoothenedData[smoothenedData.Count - 1].Timestamp = rawData[rawData.Count - 1].Timestamp;
|
|
|
|
return smoothing && hasEnoughForSmoothing
|
|
? smoothenedData[smoothenedData.Count - 1] / (float)NumberSmoothingFrames
|
|
: rawData[rawData.Count - 1];
|
|
}
|
|
|
|
// Deletes old position data from list which do not have more impact on smoothing algorithm.
|
|
public void Resize()
|
|
{
|
|
if (rawData.Count > NumberSmoothingFrames)
|
|
{
|
|
rawData.RemoveRange(0, rawData.Count - NumberSmoothingFrames);
|
|
}
|
|
if (smoothenedData.Count > NumberSmoothingFrames)
|
|
{
|
|
smoothenedData.RemoveRange(0, smoothenedData.Count - NumberSmoothingFrames);
|
|
}
|
|
}
|
|
}
|