From e54060d6bb5d2d53952adba0220dd068fcc3fd1c Mon Sep 17 00:00:00 2001 From: Sam Gondelman Date: Wed, 1 Jul 2015 14:36:12 -0700 Subject: [PATCH] more working on poses, LEFT_ and RIGHT_HAND input actions --- interface/src/Application.cpp | 33 +++++ interface/src/Application.h | 2 + interface/src/devices/SixenseManager.h | 2 +- .../openvr/ViveControllerManager.cpp | 125 +++--------------- .../openvr/ViveControllerManager.h | 7 +- .../src/input-plugins/UserInputMapper.h | 15 ++- 6 files changed, 70 insertions(+), 114 deletions(-) diff --git a/interface/src/Application.cpp b/interface/src/Application.cpp index 9d00a62a60..e12dee4c60 100644 --- a/interface/src/Application.cpp +++ b/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()->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)) { diff --git a/interface/src/Application.h b/interface/src/Application.h index 67ec8beb57..9195d1e826 100644 --- a/interface/src/Application.h +++ b/interface/src/Application.h @@ -482,6 +482,8 @@ private: void cleanupBeforeQuit(); void update(float deltaTime); + + void setPalmData(Hand* hand, UserInputMapper::PoseValue pose, int index); // Various helper functions called during update() void updateLOD(); diff --git a/interface/src/devices/SixenseManager.h b/interface/src/devices/SixenseManager.h index 730233de98..460ad58572 100644 --- a/interface/src/devices/SixenseManager.h +++ b/interface/src/devices/SixenseManager.h @@ -26,7 +26,7 @@ #endif -#include "ui/UserInputMapper.h" +#include class PalmData; diff --git a/libraries/display-plugins/src/display-plugins/openvr/ViveControllerManager.cpp b/libraries/display-plugins/src/display-plugins/openvr/ViveControllerManager.cpp index a130cf4742..26c23af002 100644 --- a/libraries/display-plugins/src/display-plugins/openvr/ViveControllerManager.cpp +++ b/libraries/display-plugins/src/display-plugins/openvr/ViveControllerManager.cpp @@ -12,15 +12,13 @@ #include "ViveControllerManager.h" #include -#include #include -#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()->getMyAvatar()->getHand(); if (_isInitialized && _isEnabled) { PerformanceTimer perfTimer("Vive Controllers"); @@ -79,120 +75,32 @@ void ViveControllerManager::update() { const mat4& mat = _trackedDevicePoseMat4[unTrackedDevice]; - PalmData* palm; - 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); + handlePoseEvent(mat, trackedControllerCount - 1); // handle inputs vr::VRControllerState_t* controllerState; if(_hmd->GetControllerState(unTrackedDevice, controllerState)) { } - - // 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(); + if (trackedControllerCount == 0) { if (_deviceID != 0) { - Application::getUserInputMapper()->removeDevice(_deviceID); + userInputMapper->removeDevice(_deviceID); _deviceID = 0; - if (_prevPalms[0]) { - _prevPalms[0]->setActive(false); - } - if (_prevPalms[1]) { - _prevPalms[1]->setActive(false); - } + _poseStateMap[makeInput(LEFT_HAND).getChannel()] = UserInputMapper::PoseValue(); + _poseStateMap[makeInput(RIGHT_HAND).getChannel()] = UserInputMapper::PoseValue(); } _trackedControllers = trackedControllerCount; return; } if (_trackedControllers == 0 && trackedControllerCount > 0) { - registerToUserInputMapper(*Application::getUserInputMapper()); - assignDefaultInputMapping(*Application::getUserInputMapper()); + registerToUserInputMapper(*userInputMapper); + 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); } diff --git a/libraries/display-plugins/src/display-plugins/openvr/ViveControllerManager.h b/libraries/display-plugins/src/display-plugins/openvr/ViveControllerManager.h index 542b33d0f8..bda0fa5f6f 100644 --- a/libraries/display-plugins/src/display-plugins/openvr/ViveControllerManager.h +++ b/libraries/display-plugins/src/display-plugins/openvr/ViveControllerManager.h @@ -15,9 +15,9 @@ #include #include -#include "ui/UserInputMapper.h" +#include -class PalmData; +#include class ViveControllerManager : public QObject { Q_OBJECT @@ -65,12 +65,11 @@ 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; diff --git a/libraries/input-plugins/src/input-plugins/UserInputMapper.h b/libraries/input-plugins/src/input-plugins/UserInputMapper.h index ae970c52d2..639d2bb132 100755 --- a/libraries/input-plugins/src/input-plugins/UserInputMapper.h +++ b/libraries/input-plugins/src/input-plugins/UserInputMapper.h @@ -82,12 +82,19 @@ public: class PoseValue { public: - glm::vec3 translation{ 0.0f }; - glm::quat rotation; + glm::vec3 _translation{ 0.0f }; + 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 ButtonGetter; @@ -160,7 +167,7 @@ public: QVector 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.