more working on poses, LEFT_ and RIGHT_HAND input actions

interface/src/Application.cpp

@@ -2537,6 +2537,11 @@ void Application::update(float deltaTime) {
             _myAvatar->setDriveKeys(ROT_LEFT, userInputMapper->getActionState(UserInputMapper::YAW_LEFT));
             _myAvatar->setDriveKeys(ROT_RIGHT, userInputMapper->getActionState(UserInputMapper::YAW_RIGHT));
             // TODO: set hand positions somehow
+            UserInputMapper::PoseValue leftHand = userInputMapper->getPoseState(UserInputMapper::LEFT_HAND);
+            UserInputMapper::PoseValue rightHand = userInputMapper->getPoseState(UserInputMapper::RIGHT_HAND);
+            Hand* hand = DependencyManager::get<AvatarManager>()->getMyAvatar()->getHand();
+            setPalmData(hand, leftHand, LEFT_HAND_INDEX);
+            setPalmData(hand, rightHand, RIGHT_HAND_INDEX);
         }
         _myAvatar->setDriveKeys(BOOM_IN, userInputMapper->getActionState(UserInputMapper::BOOM_IN));
         _myAvatar->setDriveKeys(BOOM_OUT, userInputMapper->getActionState(UserInputMapper::BOOM_OUT));
@@ -2684,6 +2689,34 @@ void Application::update(float deltaTime) {
     }
 }
 
+void Application::setPalmData(Hand* hand, UserInputMapper::PoseValue pose, int index) {
+    PalmData* palm;
+    bool foundHand = false;
+    for (size_t j = 0; j < hand->getNumPalms(); j++) {
+        if (hand->getPalms()[j].getSixenseID() == index) {
+            palm = &(hand->getPalms()[j]);
+            foundHand = true;
+        }
+    }
+    if (!foundHand) {
+        PalmData newPalm(hand);
+        hand->getPalms().push_back(newPalm);
+        palm = &(hand->getPalms()[hand->getNumPalms() - 1]);
+        palm->setSixenseID(index);
+    }
+    
+    if (foundHand) {
+        palm->setActive(pose.isValid());
+    } else {
+        palm->setActive(false); // if this isn't a Sixsense ID palm, always make it inactive
+    }
+    
+    // TODO: velocity filters, tip velocities, et.c
+    // see SixenseManager
+    palm->setRawPosition(pose.getTranslation());
+    palm->setRawRotation(pose.getRotation());
+}
+
 int Application::sendNackPackets() {
 
     if (Menu::getInstance()->isOptionChecked(MenuOption::DisableNackPackets)) {

interface/src/Application.h

@@ -483,6 +483,8 @@ private:
 
     void update(float deltaTime);
     
+    void setPalmData(Hand* hand, UserInputMapper::PoseValue pose, int index);
+
     // Various helper functions called during update()
     void updateLOD();
     void updateMouseRay();

interface/src/devices/SixenseManager.h

@@ -26,7 +26,7 @@
 
 #endif
 
-#include "ui/UserInputMapper.h"
+#include <input-plugins/UserInputMapper.h>
 
 class PalmData;
 

libraries/display-plugins/src/display-plugins/openvr/ViveControllerManager.cpp

@@ -12,15 +12,13 @@
 #include "ViveControllerManager.h"
 
 #include <avatar/AvatarManager.h>
-#include <GLMHelpers.h>
 #include <PerfStat.h>
 
-#include "Application.h"
 #include "OpenVrDisplayPlugin.h"
 #include "OpenVrHelpers.h"
 #include "UserActivityLogger.h"
 
-extern vr::IVRSystem *_hmd{ nullptr };
+extern vr::IVRSystem *_hmd;
 extern vr::TrackedDevicePose_t _trackedDevicePose[vr::k_unMaxTrackedDeviceCount];
 extern mat4 _trackedDevicePoseMat4[vr::k_unMaxTrackedDeviceCount];
 
@@ -34,8 +32,7 @@ ViveControllerManager::ViveControllerManager() :
     _isEnabled(true),
     _trackedControllers(0)
 {
-    _prevPalms[0] = nullptr;
+    
-    _prevPalms[1] = nullptr;
 }
 
 ViveControllerManager::~ViveControllerManager() {
@@ -47,13 +44,12 @@ void ViveControllerManager::activate() {
         vr::HmdError eError = vr::HmdError_None;
         _hmd = vr::VR_Init(&eError);
         Q_ASSERT(eError == vr::HmdError_None);
-        Q_ASSERT(_hmd);
     }
+    Q_ASSERT(_hmd);
     _isInitialized = true;
 }
 
 void ViveControllerManager::update() {
-    Hand* hand = DependencyManager::get<AvatarManager>()->getMyAvatar()->getHand();
     if (_isInitialized && _isEnabled) {
         
         PerformanceTimer perfTimer("Vive Controllers");
@@ -79,26 +75,7 @@ void ViveControllerManager::update() {
             
             const mat4& mat = _trackedDevicePoseMat4[unTrackedDevice];
             
-            PalmData* palm;
+            handlePoseEvent(mat, trackedControllerCount - 1);
-            bool foundHand = false;
-            // FIXME: this shouldn't use SixenseID
-            for (size_t j = 0; j < hand->getNumPalms(); j++) {
-                if (hand->getPalms()[j].getSixenseID() == unTrackedDevice) {
-                    palm = &(hand->getPalms()[j]);
-                    _prevPalms[trackedControllerCount - 1] = palm;
-                    foundHand = true;
-                }
-            }
-            if (!foundHand) {
-                PalmData newPalm(hand);
-                hand->getPalms().push_back(newPalm);
-                palm = &(hand->getPalms()[hand->getNumPalms() - 1]);
-                palm->setSixenseID(unTrackedDevice);
-                _prevPalms[trackedControllerCount - 1] = palm;
-                //qDebug(interfaceapp, "Found new Vive hand controller, ID %i", unTrackedDevice);
-            }
-            
-            palm->setActive(true);
             
             // handle inputs
             vr::VRControllerState_t* controllerState;
@@ -106,93 +83,24 @@ void ViveControllerManager::update() {
                 
             }
 
-            // set position and rotation
-//          m.m[0][0], m.m[1][0], m.m[2][0], 0.0,
-//          m.m[0][1], m.m[1][1], m.m[2][1], 0.0,
-//          m.m[0][2], m.m[1][2], m.m[2][2], 0.0,
-//          m.m[0][3], m.m[1][3], m.m[2][3], 1.0f);
-            glm::vec3 position(_trackedDevicePoseMat4[3][0], _trackedDevicePoseMat4[3][1], _trackedDevicePoseMat4[3][2]);
-//           position *= METERS_PER_MILLIMETER;
-
-//           // 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.
-//           neck.y = 0.0f;
-//           position = _orbRotation * (position - neck);
-
-//           //  Rotation of Palm
-            glm::quat rotation = glm::quat_cast(_trackedDevicePoseMat4[unTrackedDevice]);
-            //rotation = glm::angleAxis(PI, glm::vec3(0.0f, 1.0f, 0.0f)) * rotation;
-
-//           //  Compute current velocity from position change
-//           glm::vec3 rawVelocity;
-//           if (deltaTime > 0.0f) {
-//               rawVelocity = (position - palm->getRawPosition()) / deltaTime;
-//           } else {
-//               rawVelocity = glm::vec3(0.0f);
-//           }
-//           palm->setRawVelocity(rawVelocity);   //  meters/sec
-//           
-//           // adjustment for hydra controllers fit into hands
-//           float sign = (i == 0) ? -1.0f : 1.0f;
-//           rotation *= glm::angleAxis(sign * PI/4.0f, glm::vec3(0.0f, 0.0f, 1.0f));
-//           
-//           //  Angular Velocity of Palm
-//           glm::quat deltaRotation = rotation * glm::inverse(palm->getRawRotation());
-//           glm::vec3 angularVelocity(0.0f);
-//           float rotationAngle = glm::angle(deltaRotation);
-//           if ((rotationAngle > EPSILON) && (deltaTime > 0.0f)) {
-//               angularVelocity = glm::normalize(glm::axis(deltaRotation));
-//               angularVelocity *= (rotationAngle / deltaTime);
-//               palm->setRawAngularVelocity(angularVelocity);
-//           } else {
-//               palm->setRawAngularVelocity(glm::vec3(0.0f));
-//           }
-//           
-//           if (_lowVelocityFilter) {
-//               //  Use a velocity sensitive filter to damp small motions and preserve large ones with
-//               //  no latency.
-//               float velocityFilter = glm::clamp(1.0f - glm::length(rawVelocity), 0.0f, 1.0f);
-//               position = palm->getRawPosition() * velocityFilter + position * (1.0f - velocityFilter);
-//               rotation = safeMix(palm->getRawRotation(), rotation, 1.0f - velocityFilter);
-//               palm->setRawPosition(position);
-//               palm->setRawRotation(rotation);
-//           } else {
-               palm->setRawPosition(position);
-               palm->setRawRotation(rotation);
-//           }
-//           
-//           // 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);
-//           const glm::vec3 newTipPosition = position + rotation * FINGER_VECTOR;
-//           glm::vec3 oldTipPosition = palm->getTipRawPosition();
-//           if (deltaTime > 0.0f) {
-//               palm->setTipVelocity((newTipPosition - oldTipPosition) / deltaTime);
-//           } else {
-//               palm->setTipVelocity(glm::vec3(0.0f));
-//           }
-//           palm->setTipPosition(newTipPosition);
         }
         
+        auto userInputMapper = DependencyManager::get<UserInputMapper>();
+        
         if (trackedControllerCount == 0) {
             if (_deviceID != 0) {
-                Application::getUserInputMapper()->removeDevice(_deviceID);
+                userInputMapper->removeDevice(_deviceID);
                 _deviceID = 0;
-                if (_prevPalms[0]) {
+                _poseStateMap[makeInput(LEFT_HAND).getChannel()] = UserInputMapper::PoseValue();
-                    _prevPalms[0]->setActive(false);
+                _poseStateMap[makeInput(RIGHT_HAND).getChannel()] = UserInputMapper::PoseValue();
-                }
-                if (_prevPalms[1]) {
-                    _prevPalms[1]->setActive(false);
-                }
             }
             _trackedControllers = trackedControllerCount;
             return;
         }
         
         if (_trackedControllers == 0 && trackedControllerCount > 0) {
-            registerToUserInputMapper(*Application::getUserInputMapper());
+            registerToUserInputMapper(*userInputMapper);
-            assignDefaultInputMapping(*Application::getUserInputMapper());
+            assignDefaultInputMapping(*userInputMapper);
             UserActivityLogger::getInstance().connectedDevice("spatial_controller", "steamVR");
         }
         
@@ -219,6 +127,13 @@ void ViveControllerManager::handleButtonEvent(unsigned int buttons, int index) {
 //    }
 }
 
+void ViveControllerManager::handlePoseEvent(const mat4& mat, int index) {
+    glm::vec3 position(mat[3][0], mat[3][1], mat[3][2]);
+    glm::quat rotation = glm::quat_cast(mat);
+    rotation = glm::angleAxis(PI, glm::vec3(0.0f, 1.0f, 0.0f)) * rotation;
+    _poseStateMap[makeInput(JointChannel(index)).getChannel()] = UserInputMapper::PoseValue(position, rotation);
+}
+
 void ViveControllerManager::registerToUserInputMapper(UserInputMapper& mapper) {
     // Grab the current free device ID
     _deviceID = mapper.getFreeDeviceID();
@@ -335,12 +250,12 @@ UserInputMapper::PoseValue ViveControllerManager::getPose(int channel) const {
 }
 
 UserInputMapper::Input ViveControllerManager::makeInput(unsigned int button, int index) {
-    return UserInputMapper::Input(0);
+    return UserInputMapper::Input();
 //    return UserInputMapper::Input(_deviceID, button | (index == 0 ? LEFT_MASK : RIGHT_MASK), UserInputMapper::ChannelType::BUTTON);
 }
 
 UserInputMapper::Input ViveControllerManager::makeInput(ViveControllerManager::JoystickAxisChannel axis, int index) {
-    return UserInputMapper::Input(0);
+    return UserInputMapper::Input();
 //    return UserInputMapper::Input(_deviceID, axis | (index == 0 ? LEFT_MASK : RIGHT_MASK), UserInputMapper::ChannelType::AXIS);
 }
 

libraries/display-plugins/src/display-plugins/openvr/ViveControllerManager.h

@@ -15,9 +15,9 @@
 #include <QObject>
 #include <unordered_set>
 
-#include "ui/UserInputMapper.h"
+#include <GLMHelpers.h>
 
-class PalmData;
+#include <input-plugins/UserInputMapper.h>
 
 class ViveControllerManager : public QObject {
     Q_OBJECT
@@ -65,13 +65,12 @@ private:
     
     void handleButtonEvent(unsigned int buttons, int index);
     void handleAxisEvent(float x, float y, float trigger, int index);
+    void handlePoseEvent(const mat4& mat, int index);
     
     bool _isInitialized;
     bool _isEnabled;
     int _trackedControllers;
     
-    PalmData* _prevPalms[2];
-    
 protected:
     int _deviceID = 0;
     

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

@@ -82,12 +82,19 @@ public:
 
     class PoseValue {
     public:
-        glm::vec3 translation{ 0.0f };
+        glm::vec3 _translation{ 0.0f };
-        glm::quat rotation;
+        glm::quat _rotation;
+        bool _valid;
 
-        PoseValue() {};
+        PoseValue() : _valid(false) {};
+        PoseValue(glm::vec3 translation, glm::quat rotation) : _translation(translation), _rotation(rotation), _valid(true) {}
         PoseValue(const PoseValue&) = default;
         PoseValue& operator = (const PoseValue&) = default;
+        bool operator ==(const PoseValue& right) const { return _translation == right.getTranslation() && _rotation == right.getRotation() && _valid == right.isValid(); }
+        
+        bool isValid() const { return _valid; }
+        glm::vec3 getTranslation() const { return _translation; }
+        glm::quat getRotation() const { return _rotation; }
     };
     
     typedef std::function<bool (const Input& input, int timestamp)> ButtonGetter;
@@ -160,7 +167,7 @@ public:
     QVector<Action> getAllActions();
     QString getActionName(Action action) { return UserInputMapper::_actionNames[(int) action]; }
     float getActionState(Action action) const { return _actionStates[action]; }
-    float getPoseState(Action action) const { return _poseStates[action]; }
+    PoseValue getPoseState(Action action) const { return _poseStates[action]; }
     void assignDefaulActionScales();
 
     // Add input channel to the mapper and check that all the used channels are registered.