/// <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; }