Merge pull request #6174 from samcake/controllers

Controllers : Adding a moving average for the velocity field returned by the Hydra controller

examples/controllers/handPosesDebug.js

@@ -0,0 +1,101 @@
+//
+//  handPosesDebug.js
+//  examples
+//
+//  Copyright 2015 High Fidelity, Inc.
+//
+//  Distributed under the Apache License, Version 2.0.
+//  See the accompanying file LICENSE or http://www.apache.org/licenses/LICENSE-2.0.html
+//
+
+
+
+function makeSphere(color) {
+	var SPHERE_SIZE = 0.05;
+	var sphere = Overlays.addOverlay("sphere", {
+	    position: { x: 0, y: 0, z: 0 },
+	    size: SPHERE_SIZE,
+	    color: color,
+	    alpha: 1.0,
+	    solid: true,
+	    visible: true,
+	});
+
+	return sphere;
+}
+
+
+var NUM_HANDS = 2;
+var NUM_SPHERES_PER_HAND = 2;
+var LEFT_HAND = 0;
+var RIGHT_HAND = 1;
+
+var COLORS = [ { red: 255, green: 0, blue: 0 }, { red: 0, green: 0, blue: 255 } ];
+
+function index(handNum, indexNum) {
+	return handNum * NUM_HANDS + indexNum;
+}
+
+var app = {};
+
+
+function setup() {
+	app.spheres = new Array();
+
+	for (var h = 0; h < NUM_HANDS; h++) {
+		for (var s = 0; s < NUM_SPHERES_PER_HAND; s++) {
+			var i = index(h, s);
+			app.spheres[i] = makeSphere(COLORS[h]);
+			print("Added Sphere num " + i + " = " + JSON.stringify(app.spheres[i]));
+		}
+	}
+}
+
+function updateHand(handNum, deltaTime) {
+	var pose;
+	var handName = "right";
+	if (handNum == LEFT_HAND) {
+		pose = MyAvatar.getLeftHandPose();
+		handName = "left";
+	} else {
+		pose = MyAvatar.getRightHandPose();
+		handName = "right";
+	}
+
+	if (pose.valid) {
+		//print(handName + " hand moving" +  JSON.stringify(pose));
+		Overlays.editOverlay(app.spheres[index(handNum, 0)], {
+	        position: pose.translation,
+	        visible: true,
+	    });
+	   	var vpos = Vec3.sum(Vec3.multiply(10 * deltaTime, pose.velocity), pose.translation);
+	    Overlays.editOverlay(app.spheres[index(handNum, 1)], {
+	        position: vpos,
+	        visible: true,
+	    });
+	} else {
+		Overlays.editOverlay(app.spheres[index(handNum, 0)], {
+	        visible: false
+	    });
+
+	    Overlays.editOverlay(app.spheres[index(handNum, 1)], {
+	        visible: false
+	    });
+	}
+}
+
+function update(deltaTime) {
+	updateHand(LEFT_HAND, deltaTime);
+	updateHand(RIGHT_HAND, deltaTime);
+}
+
+function scriptEnding() {
+	print("Removing spheres = " + JSON.stringify(app.spheres));
+	for (var i = 0; i < app.spheres.length; i++) {
+		Overlays.deleteOverlay(app.spheres[i]);
+	}
+}
+
+setup();
+Script.update.connect(update);
+Script.scriptEnding.connect(scriptEnding);

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,13 @@ 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].first.clear();
+            _collectedSamples[hand].second.clear();
         }
     }
 
@@ -387,6 +392,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 +408,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 +455,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 +466,20 @@ 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.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].first.clear();
+        _collectedSamples[hand].second.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
 }
 

libraries/input-plugins/src/input-plugins/SixenseManager.h

@@ -24,6 +24,8 @@
 
 #endif
 
+#include <SimpleMovingAverage.h>
+
 #include <controllers/InputDevice.h>
 #include <controllers/StandardControls.h>
 
@@ -94,6 +96,12 @@ private:
     glm::vec3 _reachRight;
     float _lastDistance;
     bool _useSixenseFilter = true;
+
+
+    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__
     QLibrary* _sixenseLibrary;
@@ -109,3 +117,4 @@ private:
 };
 
 #endif // hifi_SixenseManager_h
+

libraries/shared/src/SimpleMovingAverage.h

@@ -40,4 +40,28 @@ private:
     float ONE_MINUS_WEIGHTING;
 };
 
+
+template <class T, int MAX_NUM_SAMPLES> class MovingAverage {
+public:
+    const float WEIGHTING = 1.0f / (float)MAX_NUM_SAMPLES;
+    const float ONE_MINUS_WEIGHTING = 1.0f - WEIGHTING;
+    int numSamples{ 0 };
+    T average;
+
+    void clear() {
+        numSamples = 0;
+    }
+
+    bool isAverageValid() const { return (numSamples > 0); }
+
+    void addSample(T sample) {
+        if (numSamples > 0) {
+            average = (sample * WEIGHTING) + (average * ONE_MINUS_WEIGHTING);
+        } else {
+            average = sample;
+        }
+        numSamples++;
+    }
+};
+
 #endif // hifi_SimpleMovingAverage_h