From f14a321d035ae778992d826f5b9b6c1d15b657f9 Mon Sep 17 00:00:00 2001
From: samcake <samuel.gateau@gmail.com>
Date: Fri, 23 Oct 2015 15:03:04 -0700
Subject: [PATCH] Adding a amoving Average for the velocity of the hydra

---
 .../src/input-plugins/SixenseManager.cpp      | 21 +++++++++++--
 .../src/input-plugins/SixenseManager.h        | 31 +++++++++++++++++++
 2 files changed, 49 insertions(+), 3 deletions(-)

diff --git a/libraries/input-plugins/src/input-plugins/SixenseManager.cpp b/libraries/input-plugins/src/input-plugins/SixenseManager.cpp
index 29e60ca5ec..1f72d66019 100644
--- a/libraries/input-plugins/src/input-plugins/SixenseManager.cpp
+++ b/libraries/input-plugins/src/input-plugins/SixenseManager.cpp
@@ -178,6 +178,7 @@ void SixenseManager::update(float deltaTime, bool jointsCaptured) {
 
     if (sixenseGetNumActiveControllers() == 0) {
         _poseStateMap.clear();
+        _collectedSamples.clear();
         return;
     }
 
@@ -234,9 +235,12 @@ void SixenseManager::update(float deltaTime, bool jointsCaptured) {
                 
             } else {
                 _poseStateMap.clear();
+                _collectedSamples.clear();
             }
         } else {
-            _poseStateMap[left ? controller::StandardPoseChannel::LEFT_HAND : controller::StandardPoseChannel::RIGHT_HAND] = controller::Pose();
+            auto hand = left ? controller::StandardPoseChannel::LEFT_HAND : controller::StandardPoseChannel::RIGHT_HAND;
+            _poseStateMap[hand] = controller::Pose();
+            _collectedSamples[hand].clear();
         }
     }
 
@@ -387,6 +391,8 @@ void SixenseManager::handleButtonEvent(unsigned int buttons, bool left) {
 
 void SixenseManager::handlePoseEvent(float deltaTime, glm::vec3 position, glm::quat rotation, bool left) {
 #ifdef HAVE_SIXENSE
+    auto hand = left ? controller::StandardPoseChannel::LEFT_HAND : controller::StandardPoseChannel::RIGHT_HAND;
+
     // From ABOVE the sixense coordinate frame looks like this:
     //
     //       |
@@ -401,7 +407,7 @@ void SixenseManager::handlePoseEvent(float deltaTime, glm::vec3 position, glm::q
     //                      |
     //                      |
     //                      z
-    auto prevPose = _poseStateMap[left ? controller::StandardPoseChannel::LEFT_HAND : controller::StandardPoseChannel::RIGHT_HAND];
+    auto prevPose = _poseStateMap[hand];
 
     // Transform the measured position into body frame.
     position = _avatarRotation * (position + _avatarPosition);
@@ -448,7 +454,10 @@ void SixenseManager::handlePoseEvent(float deltaTime, glm::vec3 position, glm::q
     glm::vec3 velocity(0.0f);
     glm::quat angularVelocity;
 
+
+
     if (prevPose.isValid() && deltaTime > std::numeric_limits<float>::epsilon()) {
+
         velocity = (position - prevPose.getTranslation()) / deltaTime;
 
         auto deltaRot = rotation * glm::conjugate(prevPose.getRotation());
@@ -456,9 +465,15 @@ void SixenseManager::handlePoseEvent(float deltaTime, glm::vec3 position, glm::q
         auto angle = glm::angle(deltaRot);
         angularVelocity = glm::angleAxis(angle / deltaTime, axis);
 
+        // Average
+        auto& samples = _collectedSamples[hand];
+        samples.addSample(velocity);
+        velocity = samples.average;
+    } else if (!prevPose.isValid()) {
+        _collectedSamples[hand].clear();
     }
 
-    _poseStateMap[left ? controller::StandardPoseChannel::LEFT_HAND : controller::StandardPoseChannel::RIGHT_HAND] = controller::Pose(position, rotation, velocity, angularVelocity);
+    _poseStateMap[hand] = controller::Pose(position, rotation, velocity, angularVelocity);
 #endif // HAVE_SIXENSE
 }
 
diff --git a/libraries/input-plugins/src/input-plugins/SixenseManager.h b/libraries/input-plugins/src/input-plugins/SixenseManager.h
index 368321e669..c0d908ed45 100644
--- a/libraries/input-plugins/src/input-plugins/SixenseManager.h
+++ b/libraries/input-plugins/src/input-plugins/SixenseManager.h
@@ -94,6 +94,36 @@ private:
     glm::vec3 _reachRight;
     float _lastDistance;
     bool _useSixenseFilter = true;
+
+    template <class T, int MAX_NUM_SAMPLES> class MovingAverage {
+    public:
+        using Samples = std::list< T >;
+        Samples samples;
+        T average;
+
+        void clear() {
+            samples.clear();
+        }
+
+        bool isAverageValid() const { return !samples.empty(); }
+
+        void addSample(T sample) {
+            samples.push_front(sample);
+            int numSamples = samples.size();
+
+            if (numSamples < MAX_NUM_SAMPLES) {
+                average = (sample + average * float(numSamples - 1)) / float(numSamples);
+            } else {
+                T tail = samples.back();
+                samples.pop_back();
+                average = average + (sample - tail) / float(numSamples);
+            }
+        }
+    };
+
+    static const int MAX_NUM_AVERAGING_SAMPLES = 10; // At ~100 updates per seconds this means averaging over ~.1s
+    using MovingAverageMap = std::map< int, MovingAverage< glm::vec3, MAX_NUM_AVERAGING_SAMPLES> >;
+    MovingAverageMap _collectedSamples;
     
 #ifdef __APPLE__
     QLibrary* _sixenseLibrary;
@@ -109,3 +139,4 @@ private:
 };
 
 #endif // hifi_SixenseManager_h
+