/// <summary>
/// Calculated the current velocity based on the given data and <see cref="previousData"/>.
/// </summary>
/// <param name="data">The newly retrieved position data</param>
/// <returns>The velocity at the timestamp of the newly received data.</returns>
private FeedbackData <Vector3>?CalculateVelocityFeedback(FeedbackData <Vector3> data)
{
FeedbackData <Vector3>?feedbackData = this.previousData;
if (feedbackData == null)
{
return(null);
}
long timediff = data.TimeStamp - feedbackData.Value.TimeStamp;
Vector3 traveledDist = new Vector3();
data.Data.Subtract(feedbackData.Value.Data, traveledDist);
Vector3 velocityData = new Vector3();
traveledDist.Divide(timediff / 1000f, velocityData);
FeedbackData <Vector3>?res = new FeedbackData <Vector3>
{
Data = velocityData,
TimeStamp = data.TimeStamp,
Stddev = (float)Math.Sqrt(Math.Pow(data.Stddev, 2) + Math.Pow(feedbackData.Value.Stddev, 2))
};
return(res);
}
/// <inheritdoc/>
public void NotifyPositionFeedback(FeedbackData <Vector3> data)
{
if ((this.previousData != null) && (data.TimeStamp <= this.previousData.Value.TimeStamp))
{
return;
}
FeedbackData <Vector3>?velocityFeedback = this.CalculateVelocityFeedback(data);
if (velocityFeedback != null)
{
for (int index = 0; index < this.velReceivers.Count; index++)
{
IVelocityFeedbackReceiver receiver = this.velReceivers[index];
receiver.NotifyVelocityFeedback(velocityFeedback.Value);
}
}
this.previousData = data;
}