From fefcdfa9f6ef37185e39d0050de217f7d40d0499 Mon Sep 17 00:00:00 2001
From: Philip Rosedale <philip@highfidelity.io>
Date: Sat, 3 Jan 2015 21:03:23 -0800
Subject: [PATCH] Revert "Hydra hands deactivate when placed back on controller
 base"

---
 interface/src/devices/SixenseManager.cpp | 39 ++++++++++++++++--------
 interface/src/devices/SixenseManager.h   |  3 +-
 2 files changed, 28 insertions(+), 14 deletions(-)

diff --git a/interface/src/devices/SixenseManager.cpp b/interface/src/devices/SixenseManager.cpp
index 5fb4a71091..e2b4fedd50 100644
--- a/interface/src/devices/SixenseManager.cpp
+++ b/interface/src/devices/SixenseManager.cpp
@@ -84,8 +84,10 @@ void SixenseManager::initialize() {
 #ifdef HAVE_SIXENSE
     
     if (!_isInitialized) {
+        _lastMovement = 0;
+        _amountMoved = glm::vec3(0.0f);
         _lowVelocityFilter = false;
-        _controllersAtBase = true;
+        
         _calibrationState = CALIBRATION_STATE_IDLE;
         // By default we assume the _neckBase (in orb frame) is as high above the orb
         // as the "torso" is below it.
@@ -144,11 +146,15 @@ void SixenseManager::setFilter(bool filter) {
 
 void SixenseManager::update(float deltaTime) {
 #ifdef HAVE_SIXENSE
-    Hand* hand = Application::getInstance()->getAvatar()->getHand();
     if (_isInitialized && _isEnabled) {
-        // Disable the hands (and return to default pose) if both controllers are at base station
-        for (std::vector<PalmData>::iterator it = hand->getPalms().begin(); it != hand->getPalms().end(); it++) {
-            it->setActive(!_controllersAtBase);
+        // if the controllers haven't been moved in a while, disable
+        const unsigned int MOVEMENT_DISABLE_SECONDS = 3;
+        if (usecTimestampNow() - _lastMovement > (MOVEMENT_DISABLE_SECONDS * USECS_PER_SECOND)) {
+            Hand* hand = Application::getInstance()->getAvatar()->getHand();
+            for (std::vector<PalmData>::iterator it = hand->getPalms().begin(); it != hand->getPalms().end(); it++) {
+                it->setActive(false);
+            }
+            _lastMovement = usecTimestampNow();
         }
         
 #ifdef __APPLE__
@@ -166,6 +172,8 @@ void SixenseManager::update(float deltaTime) {
             _hydrasConnected = true;
             UserActivityLogger::getInstance().connectedDevice("spatial_controller", "hydra");
         }
+        MyAvatar* avatar = Application::getInstance()->getAvatar();
+        Hand* hand = avatar->getHand();
         
 #ifdef __APPLE__
         SixenseBaseFunction sixenseGetMaxControllers =
@@ -184,7 +192,7 @@ void SixenseManager::update(float deltaTime) {
         SixenseTakeIntAndSixenseControllerData sixenseGetNewestData =
         (SixenseTakeIntAndSixenseControllerData) _sixenseLibrary->resolve("sixenseGetNewestData");
 #endif
-        int numControllersAtBase = 0;
+        
         int numActiveControllers = 0;
         for (int i = 0; i < maxControllers && numActiveControllers < 2; i++) {
             if (!sixenseIsControllerEnabled(i)) {
@@ -213,11 +221,14 @@ void SixenseManager::update(float deltaTime) {
                 qDebug("Found new Sixense controller, ID %i", data->controller_index);
             }
             
+            palm->setActive(true);
+            
             //  Read controller buttons and joystick into the hand
             palm->setControllerButtons(data->buttons);
             palm->setTrigger(data->trigger);
             palm->setJoystick(data->joystick_x, data->joystick_y);
             
+            
             // Emulate the mouse so we can use scripts
             if (Menu::getInstance()->isOptionChecked(MenuOption::SixenseMouseInput)) {
                 emulateMouse(palm, numActiveControllers - 1);
@@ -227,12 +238,6 @@ void SixenseManager::update(float deltaTime) {
             glm::vec3 position(data->pos[0], data->pos[1], data->pos[2]);
             position *= METERS_PER_MILLIMETER;
             
-            // Check to see if this hand/controller is on the base
-            const float CONTROLLER_AT_BASE_DISTANCE = 0.075f;
-            if (glm::length(position) < CONTROLLER_AT_BASE_DISTANCE) {
-                numControllersAtBase++;
-            }
-            
             // Transform the measured position into body frame.
             glm::vec3 neck = _neckBase;
             // Zeroing y component of the "neck" effectively raises the measured position a little bit.
@@ -269,6 +274,14 @@ void SixenseManager::update(float deltaTime) {
                 palm->setRawRotation(rotation);
             }
             
+            // use the velocity to determine whether there's any movement (if the hand isn't new)
+            const float MOVEMENT_DISTANCE_THRESHOLD = 0.003f;
+            _amountMoved += rawVelocity * deltaTime;
+            if (glm::length(_amountMoved) > MOVEMENT_DISTANCE_THRESHOLD && foundHand) {
+                _lastMovement = usecTimestampNow();
+                _amountMoved = glm::vec3(0.0f);
+            }
+            
             // Store the one fingertip in the palm structure so we can track velocity
             const float FINGER_LENGTH = 0.3f;   //  meters
             const glm::vec3 FINGER_VECTOR(0.0f, 0.0f, FINGER_LENGTH);
@@ -285,7 +298,7 @@ void SixenseManager::update(float deltaTime) {
         if (numActiveControllers == 2) {
             updateCalibration(controllers);
         }
-        _controllersAtBase = (numControllersAtBase == 2);
+
     }
 #endif  // HAVE_SIXENSE
 }
diff --git a/interface/src/devices/SixenseManager.h b/interface/src/devices/SixenseManager.h
index 0a7ab78c0e..1954ac91bc 100644
--- a/interface/src/devices/SixenseManager.h
+++ b/interface/src/devices/SixenseManager.h
@@ -91,6 +91,8 @@ private:
     bool _isInitialized;
     bool _isEnabled;
     bool _hydrasConnected;
+    quint64 _lastMovement;
+    glm::vec3 _amountMoved;
 
     // for mouse emulation with the two controllers
     bool _triggerPressed[2];
@@ -99,7 +101,6 @@ private:
     int _oldY[2];
     
     bool _lowVelocityFilter;
-    bool _controllersAtBase;
 };
 
 #endif // hifi_SixenseManager_h