From d400c694f6d821f912002173b0c4097c54d32eaa Mon Sep 17 00:00:00 2001
From: samcake <samuel.gateau@gmail.com>
Date: Fri, 23 Oct 2015 18:09:54 -0700
Subject: [PATCH] Cleaning up for release

---
 examples/controllers/handPosesDebug.js             | 11 +++++------
 .../src/input-plugins/SixenseManager.cpp           | 14 ++++++++++----
 .../src/input-plugins/SixenseManager.h             |  5 +++--
 3 files changed, 18 insertions(+), 12 deletions(-)

diff --git a/examples/controllers/handPosesDebug.js b/examples/controllers/handPosesDebug.js
index c4e7352445..6c933b2565 100644
--- a/examples/controllers/handPosesDebug.js
+++ b/examples/controllers/handPosesDebug.js
@@ -51,8 +51,7 @@ function setup() {
 	}
 }
 
-function updateHand(handNum) {
-
+function updateHand(handNum, deltaTime) {
 	var pose;
 	var handName = "right";
 	if (handNum == LEFT_HAND) {
@@ -69,7 +68,7 @@ function updateHand(handNum) {
 	        position: pose.translation,
 	        visible: true,
 	    });
-	   	var vpos = Vec3.sum(pose.velocity, pose.translation);
+	   	var vpos = Vec3.sum(Vec3.multiply(10 * deltaTime, pose.velocity), pose.translation);
 	    Overlays.editOverlay(app.spheres[index(handNum, 1)], {
 	        position: vpos,
 	        visible: true,
@@ -85,9 +84,9 @@ function updateHand(handNum) {
 	}
 }
 
-function update() {
-	updateHand(LEFT_HAND);
-	updateHand(RIGHT_HAND);
+function update(deltaTime) {
+	updateHand(LEFT_HAND, deltaTime);
+	updateHand(RIGHT_HAND, deltaTime);
 }
 
 function scriptEnding() {
diff --git a/libraries/input-plugins/src/input-plugins/SixenseManager.cpp b/libraries/input-plugins/src/input-plugins/SixenseManager.cpp
index 1f72d66019..7ce30ec26f 100644
--- a/libraries/input-plugins/src/input-plugins/SixenseManager.cpp
+++ b/libraries/input-plugins/src/input-plugins/SixenseManager.cpp
@@ -240,7 +240,8 @@ void SixenseManager::update(float deltaTime, bool jointsCaptured) {
         } else {
             auto hand = left ? controller::StandardPoseChannel::LEFT_HAND : controller::StandardPoseChannel::RIGHT_HAND;
             _poseStateMap[hand] = controller::Pose();
-            _collectedSamples[hand].clear();
+            _collectedSamples[hand].first.clear();
+            _collectedSamples[hand].second.clear();
         }
     }
 
@@ -467,10 +468,15 @@ void SixenseManager::handlePoseEvent(float deltaTime, glm::vec3 position, glm::q
 
         // Average
         auto& samples = _collectedSamples[hand];
-        samples.addSample(velocity);
-        velocity = samples.average;
+        samples.first.addSample(velocity);
+        velocity = samples.first.average;
+     
+        // FIXME: // Not using quaternion average yet for angular velocity because it s probably wrong but keep the MovingAverage in place
+        //samples.second.addSample(glm::vec4(angularVelocity.x, angularVelocity.y, angularVelocity.z, angularVelocity.w));
+        //angularVelocity = glm::quat(samples.second.average.w, samples.second.average.x, samples.second.average.y, samples.second.average.z);
     } else if (!prevPose.isValid()) {
-        _collectedSamples[hand].clear();
+        _collectedSamples[hand].first.clear();
+        _collectedSamples[hand].second.clear();
     }
 
     _poseStateMap[hand] = controller::Pose(position, rotation, velocity, angularVelocity);
diff --git a/libraries/input-plugins/src/input-plugins/SixenseManager.h b/libraries/input-plugins/src/input-plugins/SixenseManager.h
index 3bc82b365b..b9ca9d8479 100644
--- a/libraries/input-plugins/src/input-plugins/SixenseManager.h
+++ b/libraries/input-plugins/src/input-plugins/SixenseManager.h
@@ -121,8 +121,9 @@ private:
         }
     };
 
-    static const int MAX_NUM_AVERAGING_SAMPLES = 1000; // At ~100 updates per seconds this means averaging over ~.1s
-    using MovingAverageMap = std::map< int, MovingAverage< glm::vec3, MAX_NUM_AVERAGING_SAMPLES> >;
+    static const int MAX_NUM_AVERAGING_SAMPLES = 50; // At ~100 updates per seconds this means averaging over ~.5s
+    using Samples = std::pair<  MovingAverage< glm::vec3, MAX_NUM_AVERAGING_SAMPLES>, MovingAverage< glm::vec4, MAX_NUM_AVERAGING_SAMPLES> >;
+    using MovingAverageMap = std::map< int, Samples >;
     MovingAverageMap _collectedSamples;
     
 #ifdef __APPLE__