//// ViveControllerManager.cpp// input-plugins/src/input-plugins//// Created by Sam Gondelman on 6/29/15.// Copyright 2013 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// #include "ViveControllerManager.h"#include <algorithm> #include <PerfStat.h>#include <PathUtils.h>#include <GeometryCache.h>#include <gpu/Batch.h>#include <gpu/Context.h>#include <DeferredLightingEffect.h>#include <NumericalConstants.h>#include <ui-plugins/PluginContainer.h>#include <UserActivityLogger.h>#include <NumericalConstants.h>#include <Preferences.h>#include <SettingHandle.h>#include <OffscreenUi.h>#include <GLMHelpers.h>#include <glm/ext.hpp>#include <glm/gtc/quaternion.hpp> #include <controllers/UserInputMapper.h> #include <controllers/StandardControls.h> extern PoseData _nextSimPoseData; vr::IVRSystem* acquireOpenVrSystem();void releaseOpenVrSystem(); static const char* CONTROLLER_MODEL_STRING = "vr_controller_05_wireless_b";const quint64 CALIBRATION_TIMELAPSE = 1 * USECS_PER_SECOND; static const char* MENU_PARENT = "Avatar";static const char* MENU_NAME = "Vive Controllers";static const char* MENU_PATH = "Avatar" ">" "Vive Controllers";static const char* RENDER_CONTROLLERS = "Render Hand Controllers";static const int MIN_PUCK_COUNT = 2;static const int MIN_FEET_AND_HIPS = 3;static const int MIN_FEET_HIPS_CHEST = 4;static const int MIN_FEET_HIPS_HEAD = 4;static const int MIN_FEET_HIPS_SHOULDERS = 5;static const int MIN_FEET_HIPS_CHEST_HEAD = 5;static const int FIRST_FOOT = 0;static const int SECOND_FOOT = 1;static const int HIP = 2;static const int CHEST = 3;static float HEAD_PUCK_Y_OFFSET = -0.0254f;static float HEAD_PUCK_Z_OFFSET = -0.152f; const char* ViveControllerManager::NAME { "OpenVR" }; const std::map<vr::ETrackingResult, QString> TRACKING_RESULT_TO_STRING = { {vr::TrackingResult_Uninitialized, QString("vr::TrackingResult_Uninitialized")}, {vr::TrackingResult_Calibrating_InProgress, QString("vr::TrackingResult_Calibrating_InProgess")}, {vr::TrackingResult_Calibrating_OutOfRange, QString("TrackingResult_Calibrating_OutOfRange")}, {vr::TrackingResult_Running_OK, QString("TrackingResult_Running_Ok")}, {vr::TrackingResult_Running_OutOfRange, QString("TrackingResult_Running_OutOfRange")}}; static glm::mat4 computeOffset(glm::mat4 defaultToReferenceMat, glm::mat4 defaultJointMat, controller::Pose puckPose) { glm::mat4 poseMat = createMatFromQuatAndPos(puckPose.rotation, puckPose.translation); glm::mat4 referenceJointMat = defaultToReferenceMat * defaultJointMat; return glm::inverse(poseMat) * referenceJointMat;} static bool sortPucksYPosition(PuckPosePair firstPuck, PuckPosePair secondPuck) { return (firstPuck.second.translation.y < secondPuck.second.translation.y);} static bool sortPucksXPosition(PuckPosePair firstPuck, PuckPosePair secondPuck) { return (firstPuck.second.translation.x < secondPuck.second.translation.x);} static bool determineFeetOrdering(const controller::Pose& poseA, const controller::Pose& poseB, glm::vec3 axis, glm::vec3 axisOrigin) { glm::vec3 poseAPosition = poseA.getTranslation(); glm::vec3 poseBPosition = poseB.getTranslation(); glm::vec3 poseAVector = poseAPosition - axisOrigin; glm::vec3 poseBVector = poseBPosition - axisOrigin; float poseAProjection = glm::dot(poseAVector, axis); float poseBProjection = glm::dot(poseBVector, axis); return (poseAProjection > poseBProjection);} static glm::vec3 getReferenceHeadXAxis(glm::mat4 defaultToReferenceMat, glm::mat4 defaultHead) { glm::mat4 finalHead = defaultToReferenceMat * defaultHead; return glmExtractRotation(finalHead) * Vectors::UNIT_X;} static glm::vec3 getReferenceHeadPosition(glm::mat4 defaultToReferenceMat, glm::mat4 defaultHead) { glm::mat4 finalHead = defaultToReferenceMat * defaultHead; return extractTranslation(finalHead);} static QString deviceTrackingResultToString(vr::ETrackingResult trackingResult) { QString result; auto iterator = TRACKING_RESULT_TO_STRING.find(trackingResult); if (iterator != TRACKING_RESULT_TO_STRING.end()) { return iterator->second; } return result;} bool ViveControllerManager::isSupported() const { return openVrSupported();} bool ViveControllerManager::activate() { InputPlugin::activate(); if (!_system) { _system = acquireOpenVrSystem(); } if (!_system) { return false; } _container->addMenu(MENU_PATH); _container->addMenuItem(PluginType::INPUT_PLUGIN, MENU_PATH, RENDER_CONTROLLERS, [this](bool clicked) { this->setRenderControllers(clicked); }, true, true); enableOpenVrKeyboard(_container); // register with UserInputMapper auto userInputMapper = DependencyManager::get<controller::UserInputMapper>(); userInputMapper->registerDevice(_inputDevice); _registeredWithInputMapper = true; return true;} void ViveControllerManager::deactivate() { InputPlugin::deactivate(); disableOpenVrKeyboard(); _container->removeMenuItem(MENU_NAME, RENDER_CONTROLLERS); _container->removeMenu(MENU_PATH); if (_system) { _container->makeRenderingContextCurrent(); releaseOpenVrSystem(); _system = nullptr; } _inputDevice->_poseStateMap.clear(); // unregister with UserInputMapper auto userInputMapper = DependencyManager::get<controller::UserInputMapper>(); userInputMapper->removeDevice(_inputDevice->_deviceID); _registeredWithInputMapper = false;} bool ViveControllerManager::isHeadControllerMounted() const { if (_inputDevice && _inputDevice->isHeadControllerMounted()) { return true; } vr::EDeviceActivityLevel activityLevel = _system->GetTrackedDeviceActivityLevel(vr::k_unTrackedDeviceIndex_Hmd); return activityLevel == vr::k_EDeviceActivityLevel_UserInteraction;} void ViveControllerManager::pluginUpdate(float deltaTime, const controller::InputCalibrationData& inputCalibrationData) { if (!_system) { return; } auto userInputMapper = DependencyManager::get<controller::UserInputMapper>(); handleOpenVrEvents(); if (openVrQuitRequested()) { deactivate(); return; } // because update mutates the internal state we need to lock userInputMapper->withLock([&, this]() { _inputDevice->update(deltaTime, inputCalibrationData); }); if (_inputDevice->_trackedControllers == 0 && _registeredWithInputMapper) { userInputMapper->removeDevice(_inputDevice->_deviceID); _registeredWithInputMapper = false; _inputDevice->_poseStateMap.clear(); } if (!_registeredWithInputMapper && _inputDevice->_trackedControllers > 0) { userInputMapper->registerDevice(_inputDevice); _registeredWithInputMapper = true; }} ViveControllerManager::InputDevice::InputDevice(vr::IVRSystem*& system) : controller::InputDevice("Vive"), _system(system) { _configStringMap[Config::Auto] = QString("Auto"); _configStringMap[Config::Feet] = QString("Feet"); _configStringMap[Config::FeetAndHips] = QString("FeetAndHips"); _configStringMap[Config::FeetHipsAndChest] = QString("FeetHipsAndChest"); _configStringMap[Config::FeetHipsAndShoulders] = QString("FeetHipsAndShoulders"); _configStringMap[Config::FeetHipsChestAndHead] = QString("FeetHipsChestAndHead"); _configStringMap[Config::FeetHipsAndHead] = QString("FeetHipsAndHead"); if (openVrSupported()) { createPreferences(); }} void ViveControllerManager::InputDevice::update(float deltaTime, const controller::InputCalibrationData& inputCalibrationData) { _poseStateMap.clear(); _buttonPressedMap.clear(); _validTrackedObjects.clear(); // While the keyboard is open, we defer strictly to the keyboard values if (isOpenVrKeyboardShown()) { _axisStateMap.clear(); return; } PerformanceTimer perfTimer("ViveControllerManager::update"); auto leftHandDeviceIndex = _system->GetTrackedDeviceIndexForControllerRole(vr::TrackedControllerRole_LeftHand); auto rightHandDeviceIndex = _system->GetTrackedDeviceIndexForControllerRole(vr::TrackedControllerRole_RightHand); handleHandController(deltaTime, leftHandDeviceIndex, inputCalibrationData, true); handleHandController(deltaTime, rightHandDeviceIndex, inputCalibrationData, false); // collect poses for all generic trackers for (int i = 0; i < vr::k_unMaxTrackedDeviceCount; i++) { handleTrackedObject(i, inputCalibrationData); handleHmd(i, inputCalibrationData); } // handle haptics { Locker locker(_lock); if (_leftHapticDuration > 0.0f) { hapticsHelper(deltaTime, true); } if (_rightHapticDuration > 0.0f) { hapticsHelper(deltaTime, false); } } int numTrackedControllers = 0; if (leftHandDeviceIndex != vr::k_unTrackedDeviceIndexInvalid) { numTrackedControllers++; } if (rightHandDeviceIndex != vr::k_unTrackedDeviceIndexInvalid) { numTrackedControllers++; } _trackedControllers = numTrackedControllers; if (checkForCalibrationEvent()) { quint64 currentTime = usecTimestampNow(); if (!_timeTilCalibrationSet) { _timeTilCalibrationSet = true; _timeTilCalibration = currentTime + CALIBRATION_TIMELAPSE; } if (currentTime > _timeTilCalibration && !_triggersPressedHandled) { _triggersPressedHandled = true; calibrateOrUncalibrate(inputCalibrationData); } } else { _triggersPressedHandled = false; _timeTilCalibrationSet = false; } updateCalibratedLimbs(); _lastSimPoseData = _nextSimPoseData;} void ViveControllerManager::InputDevice::handleTrackedObject(uint32_t deviceIndex, const controller::InputCalibrationData& inputCalibrationData) { uint32_t poseIndex = controller::TRACKED_OBJECT_00 + deviceIndex; printDeviceTrackingResultChange(deviceIndex); if (_system->IsTrackedDeviceConnected(deviceIndex) && _system->GetTrackedDeviceClass(deviceIndex) == vr::TrackedDeviceClass_GenericTracker && _nextSimPoseData.vrPoses[deviceIndex].bPoseIsValid && poseIndex <= controller::TRACKED_OBJECT_15) { mat4& mat = mat4(); vec3 linearVelocity = vec3(); vec3 angularVelocity = vec3(); // check if the device is tracking out of range, then process the correct pose depending on the result. if (_nextSimPoseData.vrPoses[deviceIndex].eTrackingResult != vr::TrackingResult_Running_OutOfRange) { mat = _nextSimPoseData.poses[deviceIndex]; linearVelocity = _nextSimPoseData.linearVelocities[deviceIndex]; angularVelocity = _nextSimPoseData.angularVelocities[deviceIndex]; } else { mat = _lastSimPoseData.poses[deviceIndex]; linearVelocity = _lastSimPoseData.linearVelocities[deviceIndex]; angularVelocity = _lastSimPoseData.angularVelocities[deviceIndex]; // make sure that we do not overwrite the pose in the _lastSimPose with incorrect data. _nextSimPoseData.poses[deviceIndex] = _lastSimPoseData.poses[deviceIndex]; _nextSimPoseData.linearVelocities[deviceIndex] = _lastSimPoseData.linearVelocities[deviceIndex]; _nextSimPoseData.angularVelocities[deviceIndex] = _lastSimPoseData.angularVelocities[deviceIndex]; } controller::Pose pose(extractTranslation(mat), glmExtractRotation(mat), linearVelocity, angularVelocity); // transform into avatar frame glm::mat4 controllerToAvatar = glm::inverse(inputCalibrationData.avatarMat) * inputCalibrationData.sensorToWorldMat; _poseStateMap[poseIndex] = pose.transform(controllerToAvatar); _validTrackedObjects.push_back(std::make_pair(poseIndex, _poseStateMap[poseIndex])); } else { controller::Pose invalidPose; _poseStateMap[poseIndex] = invalidPose; }} void ViveControllerManager::InputDevice::calibrateOrUncalibrate(const controller::InputCalibrationData& inputCalibration) { if (!_calibrated) { calibrate(inputCalibration); } else { uncalibrate(); }} void ViveControllerManager::InputDevice::calibrate(const controller::InputCalibrationData& inputCalibration) { qDebug() << "Puck Calibration: Starting..."; // convert the hmd head from sensor space to avatar space glm::mat4 hmdSensorFlippedMat = inputCalibration.hmdSensorMat * Matrices::Y_180; glm::mat4 sensorToAvatarMat = glm::inverse(inputCalibration.avatarMat) * inputCalibration.sensorToWorldMat; glm::mat4 hmdAvatarMat = sensorToAvatarMat * hmdSensorFlippedMat; // cancel the roll and pitch for the hmd head glm::quat hmdRotation = cancelOutRollAndPitch(glmExtractRotation(hmdAvatarMat)); glm::vec3 hmdTranslation = extractTranslation(hmdAvatarMat); glm::mat4 currentHmd = createMatFromQuatAndPos(hmdRotation, hmdTranslation); // calculate the offset from the centerOfEye to defaultHeadMat glm::mat4 defaultHeadOffset = glm::inverse(inputCalibration.defaultCenterEyeMat) * inputCalibration.defaultHeadMat; glm::mat4 currentHead = currentHmd * defaultHeadOffset; // calculate the defaultToRefrenceXform glm::mat4 defaultToReferenceMat = currentHead * glm::inverse(inputCalibration.defaultHeadMat); int puckCount = (int)_validTrackedObjects.size(); qDebug() << "Puck Calibration: " << puckCount << " pucks found for calibration"; _config = _preferedConfig; if (_config != Config::Auto && puckCount < MIN_PUCK_COUNT) { qDebug() << "Puck Calibration: Failed: Could not meet the minimal # of pucks"; uncalibrate(); return; } else if (_config == Config::Auto){ if (puckCount == MIN_PUCK_COUNT) { _config = Config::Feet; qDebug() << "Puck Calibration: Auto Config: " << configToString(_config) << " configuration"; } else if (puckCount == MIN_FEET_AND_HIPS) { _config = Config::FeetAndHips; qDebug() << "Puck Calibration: Auto Config: " << configToString(_config) << " configuration"; } else if (puckCount >= MIN_FEET_HIPS_CHEST) { _config = Config::FeetHipsAndChest; qDebug() << "Puck Calibration: Auto Config: " << configToString(_config) << " configuration"; } else { qDebug() << "Puck Calibration: Auto Config Failed: Could not meet the minimal # of pucks"; uncalibrate(); return; } } std::sort(_validTrackedObjects.begin(), _validTrackedObjects.end(), sortPucksYPosition); if (_config == Config::Feet) { calibrateFeet(defaultToReferenceMat, inputCalibration); } else if (_config == Config::FeetAndHips && puckCount >= MIN_FEET_AND_HIPS) { calibrateFeet(defaultToReferenceMat, inputCalibration); calibrateHips(defaultToReferenceMat, inputCalibration); } else if (_config == Config::FeetHipsAndChest && puckCount >= MIN_FEET_HIPS_CHEST) { calibrateFeet(defaultToReferenceMat, inputCalibration); calibrateHips(defaultToReferenceMat, inputCalibration); calibrateChest(defaultToReferenceMat, inputCalibration); } else if (_config == Config::FeetHipsAndShoulders && puckCount >= MIN_FEET_HIPS_SHOULDERS) { calibrateFeet(defaultToReferenceMat, inputCalibration); calibrateHips(defaultToReferenceMat, inputCalibration); int firstShoulderIndex = 3; int secondShoulderIndex = 4; calibrateShoulders(defaultToReferenceMat, inputCalibration, firstShoulderIndex, secondShoulderIndex); } else if (_config == Config::FeetHipsAndHead && puckCount == MIN_FEET_HIPS_HEAD) { glm::mat4 headPuckDefaultToReferenceMat = recalculateDefaultToReferenceForHeadPuck(inputCalibration); glm::vec3 headXAxis = getReferenceHeadXAxis(headPuckDefaultToReferenceMat, inputCalibration.defaultHeadMat); glm::vec3 headPosition = getReferenceHeadPosition(headPuckDefaultToReferenceMat, inputCalibration.defaultHeadMat); calibrateFeet(headPuckDefaultToReferenceMat, inputCalibration, headXAxis, headPosition); calibrateHips(headPuckDefaultToReferenceMat, inputCalibration); calibrateHead(headPuckDefaultToReferenceMat, inputCalibration); _overrideHead = true; } else if (_config == Config::FeetHipsChestAndHead && puckCount == MIN_FEET_HIPS_CHEST_HEAD) { glm::mat4 headPuckDefaultToReferenceMat = recalculateDefaultToReferenceForHeadPuck(inputCalibration); glm::vec3 headXAxis = getReferenceHeadXAxis(headPuckDefaultToReferenceMat, inputCalibration.defaultHeadMat); glm::vec3 headPosition = getReferenceHeadPosition(headPuckDefaultToReferenceMat, inputCalibration.defaultHeadMat); calibrateFeet(headPuckDefaultToReferenceMat, inputCalibration, headXAxis, headPosition); calibrateHips(headPuckDefaultToReferenceMat, inputCalibration); calibrateChest(headPuckDefaultToReferenceMat, inputCalibration); calibrateHead(headPuckDefaultToReferenceMat, inputCalibration); _overrideHead = true; } else { qDebug() << "Puck Calibration: " << configToString(_config) << " Config Failed: Could not meet the minimal # of pucks"; uncalibrate(); return; } _calibrated = true; qDebug() << "PuckCalibration: " << configToString(_config) << " Configuration Successful";} void ViveControllerManager::InputDevice::uncalibrate() { _config = Config::Auto; _pucksOffset.clear(); _jointToPuckMap.clear(); _calibrated = false; _overrideHead = false;} void ViveControllerManager::InputDevice::updateCalibratedLimbs() { _poseStateMap[controller::LEFT_FOOT] = addOffsetToPuckPose(controller::LEFT_FOOT); _poseStateMap[controller::RIGHT_FOOT] = addOffsetToPuckPose(controller::RIGHT_FOOT); _poseStateMap[controller::HIPS] = addOffsetToPuckPose(controller::HIPS); _poseStateMap[controller::SPINE2] = addOffsetToPuckPose(controller::SPINE2); _poseStateMap[controller::RIGHT_ARM] = addOffsetToPuckPose(controller::RIGHT_ARM); _poseStateMap[controller::LEFT_ARM] = addOffsetToPuckPose(controller::LEFT_ARM); if (_overrideHead) { _poseStateMap[controller::HEAD] = addOffsetToPuckPose(controller::HEAD); }} controller::Pose ViveControllerManager::InputDevice::addOffsetToPuckPose(int joint) const { auto puck = _jointToPuckMap.find(joint); if (puck != _jointToPuckMap.end()) { uint32_t puckIndex = puck->second; auto puckPose = _poseStateMap.find(puckIndex); auto puckOffset = _pucksOffset.find(puckIndex); if ((puckPose != _poseStateMap.end()) && (puckOffset != _pucksOffset.end())) { return puckPose->second.postTransform(puckOffset->second); } } return controller::Pose();} void ViveControllerManager::InputDevice::handleHmd(uint32_t deviceIndex, const controller::InputCalibrationData& inputCalibrationData) { uint32_t poseIndex = controller::TRACKED_OBJECT_00 + deviceIndex; if (_system->IsTrackedDeviceConnected(deviceIndex) && _system->GetTrackedDeviceClass(deviceIndex) == vr::TrackedDeviceClass_HMD && _nextSimPoseData.vrPoses[deviceIndex].bPoseIsValid) { const mat4& mat = _nextSimPoseData.poses[deviceIndex]; const vec3 linearVelocity = _nextSimPoseData.linearVelocities[deviceIndex]; const vec3 angularVelocity = _nextSimPoseData.angularVelocities[deviceIndex]; handleHeadPoseEvent(inputCalibrationData, mat, linearVelocity, angularVelocity); }} void ViveControllerManager::InputDevice::handleHandController(float deltaTime, uint32_t deviceIndex, const controller::InputCalibrationData& inputCalibrationData, bool isLeftHand) { if (_system->IsTrackedDeviceConnected(deviceIndex) && _system->GetTrackedDeviceClass(deviceIndex) == vr::TrackedDeviceClass_Controller && _nextSimPoseData.vrPoses[deviceIndex].bPoseIsValid) { // process pose const mat4& mat = _nextSimPoseData.poses[deviceIndex]; const vec3 linearVelocity = _nextSimPoseData.linearVelocities[deviceIndex]; const vec3 angularVelocity = _nextSimPoseData.angularVelocities[deviceIndex]; handlePoseEvent(deltaTime, inputCalibrationData, mat, linearVelocity, angularVelocity, isLeftHand); vr::VRControllerState_t controllerState = vr::VRControllerState_t(); if (_system->GetControllerState(deviceIndex, &controllerState, sizeof(vr::VRControllerState_t))) { // process each button for (uint32_t i = 0; i < vr::k_EButton_Max; ++i) { auto mask = vr::ButtonMaskFromId((vr::EVRButtonId)i); bool pressed = 0 != (controllerState.ulButtonPressed & mask); bool touched = 0 != (controllerState.ulButtonTouched & mask); handleButtonEvent(deltaTime, i, pressed, touched, isLeftHand); } // process each axis for (uint32_t i = 0; i < vr::k_unControllerStateAxisCount; i++) { handleAxisEvent(deltaTime, i, controllerState.rAxis[i].x, controllerState.rAxis[i].y, isLeftHand); } // pseudo buttons the depend on both of the above for-loopsLines 1–500 of 959. Load more
// // ViveControllerManager.cpp // input-plugins/src/input-plugins // // Created by Sam Gondelman on 6/29/15. // Copyright 2013 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 // #include "ViveControllerManager.h" #include <algorithm> #include <PerfStat.h> #include <PathUtils.h> #include <GeometryCache.h> #include <gpu/Batch.h> #include <gpu/Context.h> #include <DeferredLightingEffect.h> #include <NumericalConstants.h> #include <ui-plugins/PluginContainer.h> #include <UserActivityLogger.h> #include <NumericalConstants.h> #include <Preferences.h> #include <SettingHandle.h> #include <OffscreenUi.h> #include <GLMHelpers.h> #include <glm/ext.hpp> #include <glm/gtc/quaternion.hpp> #include <controllers/UserInputMapper.h> #include <controllers/StandardControls.h> extern PoseData _nextSimPoseData; vr::IVRSystem* acquireOpenVrSystem(); void releaseOpenVrSystem(); static const char* CONTROLLER_MODEL_STRING = "vr_controller_05_wireless_b"; const quint64 CALIBRATION_TIMELAPSE = 1 * USECS_PER_SECOND; static const char* MENU_PARENT = "Avatar"; static const char* MENU_NAME = "Vive Controllers"; static const char* MENU_PATH = "Avatar" ">" "Vive Controllers"; static const char* RENDER_CONTROLLERS = "Render Hand Controllers"; static const int MIN_PUCK_COUNT = 2; static const int MIN_FEET_AND_HIPS = 3; static const int MIN_FEET_HIPS_CHEST = 4; static const int MIN_FEET_HIPS_HEAD = 4; static const int MIN_FEET_HIPS_SHOULDERS = 5; static const int MIN_FEET_HIPS_CHEST_HEAD = 5; static const int FIRST_FOOT = 0; static const int SECOND_FOOT = 1; static const int HIP = 2; static const int CHEST = 3; static float HEAD_PUCK_Y_OFFSET = -0.0254f; static float HEAD_PUCK_Z_OFFSET = -0.152f; const char* ViveControllerManager::NAME { "OpenVR" }; const std::map<vr::ETrackingResult, QString> TRACKING_RESULT_TO_STRING = { {vr::TrackingResult_Uninitialized, QString("vr::TrackingResult_Uninitialized")}, {vr::TrackingResult_Calibrating_InProgress, QString("vr::TrackingResult_Calibrating_InProgess")}, {vr::TrackingResult_Calibrating_OutOfRange, QString("TrackingResult_Calibrating_OutOfRange")}, {vr::TrackingResult_Running_OK, QString("TrackingResult_Running_Ok")}, {vr::TrackingResult_Running_OutOfRange, QString("TrackingResult_Running_OutOfRange")} }; static glm::mat4 computeOffset(glm::mat4 defaultToReferenceMat, glm::mat4 defaultJointMat, controller::Pose puckPose) { glm::mat4 poseMat = createMatFromQuatAndPos(puckPose.rotation, puckPose.translation); glm::mat4 referenceJointMat = defaultToReferenceMat * defaultJointMat; return glm::inverse(poseMat) * referenceJointMat; } static bool sortPucksYPosition(PuckPosePair firstPuck, PuckPosePair secondPuck) { return (firstPuck.second.translation.y < secondPuck.second.translation.y); } static bool sortPucksXPosition(PuckPosePair firstPuck, PuckPosePair secondPuck) { return (firstPuck.second.translation.x < secondPuck.second.translation.x); } static bool determineFeetOrdering(const controller::Pose& poseA, const controller::Pose& poseB, glm::vec3 axis, glm::vec3 axisOrigin) { glm::vec3 poseAPosition = poseA.getTranslation(); glm::vec3 poseBPosition = poseB.getTranslation(); glm::vec3 poseAVector = poseAPosition - axisOrigin; glm::vec3 poseBVector = poseBPosition - axisOrigin; float poseAProjection = glm::dot(poseAVector, axis); float poseBProjection = glm::dot(poseBVector, axis); return (poseAProjection > poseBProjection); } static glm::vec3 getReferenceHeadXAxis(glm::mat4 defaultToReferenceMat, glm::mat4 defaultHead) { glm::mat4 finalHead = defaultToReferenceMat * defaultHead; return glmExtractRotation(finalHead) * Vectors::UNIT_X; } static glm::vec3 getReferenceHeadPosition(glm::mat4 defaultToReferenceMat, glm::mat4 defaultHead) { glm::mat4 finalHead = defaultToReferenceMat * defaultHead; return extractTranslation(finalHead); } static QString deviceTrackingResultToString(vr::ETrackingResult trackingResult) { QString result; auto iterator = TRACKING_RESULT_TO_STRING.find(trackingResult); if (iterator != TRACKING_RESULT_TO_STRING.end()) { return iterator->second; } return result; } bool ViveControllerManager::isSupported() const { return openVrSupported(); } bool ViveControllerManager::activate() { InputPlugin::activate(); if (!_system) { _system = acquireOpenVrSystem(); } if (!_system) { return false; } _container->addMenu(MENU_PATH); _container->addMenuItem(PluginType::INPUT_PLUGIN, MENU_PATH, RENDER_CONTROLLERS, [this](bool clicked) { this->setRenderControllers(clicked); }, true, true); enableOpenVrKeyboard(_container); // register with UserInputMapper auto userInputMapper = DependencyManager::get<controller::UserInputMapper>(); userInputMapper->registerDevice(_inputDevice); _registeredWithInputMapper = true; return true; } void ViveControllerManager::deactivate() { InputPlugin::deactivate(); disableOpenVrKeyboard(); _container->removeMenuItem(MENU_NAME, RENDER_CONTROLLERS); _container->removeMenu(MENU_PATH); if (_system) { _container->makeRenderingContextCurrent(); releaseOpenVrSystem(); _system = nullptr; } _inputDevice->_poseStateMap.clear(); // unregister with UserInputMapper auto userInputMapper = DependencyManager::get<controller::UserInputMapper>(); userInputMapper->removeDevice(_inputDevice->_deviceID); _registeredWithInputMapper = false; } bool ViveControllerManager::isHeadControllerMounted() const { if (_inputDevice && _inputDevice->isHeadControllerMounted()) { return true; } vr::EDeviceActivityLevel activityLevel = _system->GetTrackedDeviceActivityLevel(vr::k_unTrackedDeviceIndex_Hmd); return activityLevel == vr::k_EDeviceActivityLevel_UserInteraction; } void ViveControllerManager::pluginUpdate(float deltaTime, const controller::InputCalibrationData& inputCalibrationData) { if (!_system) { return; } auto userInputMapper = DependencyManager::get<controller::UserInputMapper>(); handleOpenVrEvents(); if (openVrQuitRequested()) { deactivate(); return; } // because update mutates the internal state we need to lock userInputMapper->withLock([&, this]() { _inputDevice->update(deltaTime, inputCalibrationData); }); if (_inputDevice->_trackedControllers == 0 && _registeredWithInputMapper) { userInputMapper->removeDevice(_inputDevice->_deviceID); _registeredWithInputMapper = false; _inputDevice->_poseStateMap.clear(); } if (!_registeredWithInputMapper && _inputDevice->_trackedControllers > 0) { userInputMapper->registerDevice(_inputDevice); _registeredWithInputMapper = true; } } ViveControllerManager::InputDevice::InputDevice(vr::IVRSystem*& system) : controller::InputDevice("Vive"), _system(system) { _configStringMap[Config::Auto] = QString("Auto"); _configStringMap[Config::Feet] = QString("Feet"); _configStringMap[Config::FeetAndHips] = QString("FeetAndHips"); _configStringMap[Config::FeetHipsAndChest] = QString("FeetHipsAndChest"); _configStringMap[Config::FeetHipsAndShoulders] = QString("FeetHipsAndShoulders"); _configStringMap[Config::FeetHipsChestAndHead] = QString("FeetHipsChestAndHead"); _configStringMap[Config::FeetHipsAndHead] = QString("FeetHipsAndHead"); if (openVrSupported()) { createPreferences(); } } void ViveControllerManager::InputDevice::update(float deltaTime, const controller::InputCalibrationData& inputCalibrationData) { _poseStateMap.clear(); _buttonPressedMap.clear(); _validTrackedObjects.clear(); // While the keyboard is open, we defer strictly to the keyboard values if (isOpenVrKeyboardShown()) { _axisStateMap.clear(); return; } PerformanceTimer perfTimer("ViveControllerManager::update"); auto leftHandDeviceIndex = _system->GetTrackedDeviceIndexForControllerRole(vr::TrackedControllerRole_LeftHand); auto rightHandDeviceIndex = _system->GetTrackedDeviceIndexForControllerRole(vr::TrackedControllerRole_RightHand); handleHandController(deltaTime, leftHandDeviceIndex, inputCalibrationData, true); handleHandController(deltaTime, rightHandDeviceIndex, inputCalibrationData, false); // collect poses for all generic trackers for (int i = 0; i < vr::k_unMaxTrackedDeviceCount; i++) { handleTrackedObject(i, inputCalibrationData); handleHmd(i, inputCalibrationData); } // handle haptics { Locker locker(_lock); if (_leftHapticDuration > 0.0f) { hapticsHelper(deltaTime, true); } if (_rightHapticDuration > 0.0f) { hapticsHelper(deltaTime, false); } } int numTrackedControllers = 0; if (leftHandDeviceIndex != vr::k_unTrackedDeviceIndexInvalid) { numTrackedControllers++; } if (rightHandDeviceIndex != vr::k_unTrackedDeviceIndexInvalid) { numTrackedControllers++; } _trackedControllers = numTrackedControllers; if (checkForCalibrationEvent()) { quint64 currentTime = usecTimestampNow(); if (!_timeTilCalibrationSet) { _timeTilCalibrationSet = true; _timeTilCalibration = currentTime + CALIBRATION_TIMELAPSE; } if (currentTime > _timeTilCalibration && !_triggersPressedHandled) { _triggersPressedHandled = true; calibrateOrUncalibrate(inputCalibrationData); } } else { _triggersPressedHandled = false; _timeTilCalibrationSet = false; } updateCalibratedLimbs(); _lastSimPoseData = _nextSimPoseData; } void ViveControllerManager::InputDevice::handleTrackedObject(uint32_t deviceIndex, const controller::InputCalibrationData& inputCalibrationData) { uint32_t poseIndex = controller::TRACKED_OBJECT_00 + deviceIndex; printDeviceTrackingResultChange(deviceIndex); if (_system->IsTrackedDeviceConnected(deviceIndex) && _system->GetTrackedDeviceClass(deviceIndex) == vr::TrackedDeviceClass_GenericTracker && _nextSimPoseData.vrPoses[deviceIndex].bPoseIsValid && poseIndex <= controller::TRACKED_OBJECT_15) { mat4& mat = mat4(); vec3 linearVelocity = vec3(); vec3 angularVelocity = vec3(); // check if the device is tracking out of range, then process the correct pose depending on the result. if (_nextSimPoseData.vrPoses[deviceIndex].eTrackingResult != vr::TrackingResult_Running_OutOfRange) { mat = _nextSimPoseData.poses[deviceIndex]; linearVelocity = _nextSimPoseData.linearVelocities[deviceIndex]; angularVelocity = _nextSimPoseData.angularVelocities[deviceIndex]; } else { mat = _lastSimPoseData.poses[deviceIndex]; linearVelocity = _lastSimPoseData.linearVelocities[deviceIndex]; angularVelocity = _lastSimPoseData.angularVelocities[deviceIndex]; // make sure that we do not overwrite the pose in the _lastSimPose with incorrect data. _nextSimPoseData.poses[deviceIndex] = _lastSimPoseData.poses[deviceIndex]; _nextSimPoseData.linearVelocities[deviceIndex] = _lastSimPoseData.linearVelocities[deviceIndex]; _nextSimPoseData.angularVelocities[deviceIndex] = _lastSimPoseData.angularVelocities[deviceIndex]; } controller::Pose pose(extractTranslation(mat), glmExtractRotation(mat), linearVelocity, angularVelocity); // transform into avatar frame glm::mat4 controllerToAvatar = glm::inverse(inputCalibrationData.avatarMat) * inputCalibrationData.sensorToWorldMat; _poseStateMap[poseIndex] = pose.transform(controllerToAvatar); _validTrackedObjects.push_back(std::make_pair(poseIndex, _poseStateMap[poseIndex])); } else { controller::Pose invalidPose; _poseStateMap[poseIndex] = invalidPose; } } void ViveControllerManager::InputDevice::calibrateOrUncalibrate(const controller::InputCalibrationData& inputCalibration) { if (!_calibrated) { calibrate(inputCalibration); } else { uncalibrate(); } } void ViveControllerManager::InputDevice::calibrate(const controller::InputCalibrationData& inputCalibration) { qDebug() << "Puck Calibration: Starting..."; // convert the hmd head from sensor space to avatar space glm::mat4 hmdSensorFlippedMat = inputCalibration.hmdSensorMat * Matrices::Y_180; glm::mat4 sensorToAvatarMat = glm::inverse(inputCalibration.avatarMat) * inputCalibration.sensorToWorldMat; glm::mat4 hmdAvatarMat = sensorToAvatarMat * hmdSensorFlippedMat; // cancel the roll and pitch for the hmd head glm::quat hmdRotation = cancelOutRollAndPitch(glmExtractRotation(hmdAvatarMat)); glm::vec3 hmdTranslation = extractTranslation(hmdAvatarMat); glm::mat4 currentHmd = createMatFromQuatAndPos(hmdRotation, hmdTranslation); // calculate the offset from the centerOfEye to defaultHeadMat glm::mat4 defaultHeadOffset = glm::inverse(inputCalibration.defaultCenterEyeMat) * inputCalibration.defaultHeadMat; glm::mat4 currentHead = currentHmd * defaultHeadOffset; // calculate the defaultToRefrenceXform glm::mat4 defaultToReferenceMat = currentHead * glm::inverse(inputCalibration.defaultHeadMat); int puckCount = (int)_validTrackedObjects.size(); qDebug() << "Puck Calibration: " << puckCount << " pucks found for calibration"; _config = _preferedConfig; if (_config != Config::Auto && puckCount < MIN_PUCK_COUNT) { qDebug() << "Puck Calibration: Failed: Could not meet the minimal # of pucks"; uncalibrate(); return; } else if (_config == Config::Auto){ if (puckCount == MIN_PUCK_COUNT) { _config = Config::Feet; qDebug() << "Puck Calibration: Auto Config: " << configToString(_config) << " configuration"; } else if (puckCount == MIN_FEET_AND_HIPS) { _config = Config::FeetAndHips; qDebug() << "Puck Calibration: Auto Config: " << configToString(_config) << " configuration"; } else if (puckCount >= MIN_FEET_HIPS_CHEST) { _config = Config::FeetHipsAndChest; qDebug() << "Puck Calibration: Auto Config: " << configToString(_config) << " configuration"; } else { qDebug() << "Puck Calibration: Auto Config Failed: Could not meet the minimal # of pucks"; uncalibrate(); return; } } std::sort(_validTrackedObjects.begin(), _validTrackedObjects.end(), sortPucksYPosition); if (_config == Config::Feet) { calibrateFeet(defaultToReferenceMat, inputCalibration); } else if (_config == Config::FeetAndHips && puckCount >= MIN_FEET_AND_HIPS) { calibrateFeet(defaultToReferenceMat, inputCalibration); calibrateHips(defaultToReferenceMat, inputCalibration); } else if (_config == Config::FeetHipsAndChest && puckCount >= MIN_FEET_HIPS_CHEST) { calibrateFeet(defaultToReferenceMat, inputCalibration); calibrateHips(defaultToReferenceMat, inputCalibration); calibrateChest(defaultToReferenceMat, inputCalibration); } else if (_config == Config::FeetHipsAndShoulders && puckCount >= MIN_FEET_HIPS_SHOULDERS) { calibrateFeet(defaultToReferenceMat, inputCalibration); calibrateHips(defaultToReferenceMat, inputCalibration); int firstShoulderIndex = 3; int secondShoulderIndex = 4; calibrateShoulders(defaultToReferenceMat, inputCalibration, firstShoulderIndex, secondShoulderIndex); } else if (_config == Config::FeetHipsAndHead && puckCount == MIN_FEET_HIPS_HEAD) { glm::mat4 headPuckDefaultToReferenceMat = recalculateDefaultToReferenceForHeadPuck(inputCalibration); glm::vec3 headXAxis = getReferenceHeadXAxis(headPuckDefaultToReferenceMat, inputCalibration.defaultHeadMat); glm::vec3 headPosition = getReferenceHeadPosition(headPuckDefaultToReferenceMat, inputCalibration.defaultHeadMat); calibrateFeet(headPuckDefaultToReferenceMat, inputCalibration, headXAxis, headPosition); calibrateHips(headPuckDefaultToReferenceMat, inputCalibration); calibrateHead(headPuckDefaultToReferenceMat, inputCalibration); _overrideHead = true; } else if (_config == Config::FeetHipsChestAndHead && puckCount == MIN_FEET_HIPS_CHEST_HEAD) { glm::mat4 headPuckDefaultToReferenceMat = recalculateDefaultToReferenceForHeadPuck(inputCalibration); glm::vec3 headXAxis = getReferenceHeadXAxis(headPuckDefaultToReferenceMat, inputCalibration.defaultHeadMat); glm::vec3 headPosition = getReferenceHeadPosition(headPuckDefaultToReferenceMat, inputCalibration.defaultHeadMat); calibrateFeet(headPuckDefaultToReferenceMat, inputCalibration, headXAxis, headPosition); calibrateHips(headPuckDefaultToReferenceMat, inputCalibration); calibrateChest(headPuckDefaultToReferenceMat, inputCalibration); calibrateHead(headPuckDefaultToReferenceMat, inputCalibration); _overrideHead = true; } else { qDebug() << "Puck Calibration: " << configToString(_config) << " Config Failed: Could not meet the minimal # of pucks"; uncalibrate(); return; } _calibrated = true; qDebug() << "PuckCalibration: " << configToString(_config) << " Configuration Successful"; } void ViveControllerManager::InputDevice::uncalibrate() { _config = Config::Auto; _pucksOffset.clear(); _jointToPuckMap.clear(); _calibrated = false; _overrideHead = false; } void ViveControllerManager::InputDevice::updateCalibratedLimbs() { _poseStateMap[controller::LEFT_FOOT] = addOffsetToPuckPose(controller::LEFT_FOOT); _poseStateMap[controller::RIGHT_FOOT] = addOffsetToPuckPose(controller::RIGHT_FOOT); _poseStateMap[controller::HIPS] = addOffsetToPuckPose(controller::HIPS); _poseStateMap[controller::SPINE2] = addOffsetToPuckPose(controller::SPINE2); _poseStateMap[controller::RIGHT_ARM] = addOffsetToPuckPose(controller::RIGHT_ARM); _poseStateMap[controller::LEFT_ARM] = addOffsetToPuckPose(controller::LEFT_ARM); if (_overrideHead) { _poseStateMap[controller::HEAD] = addOffsetToPuckPose(controller::HEAD); } } controller::Pose ViveControllerManager::InputDevice::addOffsetToPuckPose(int joint) const { auto puck = _jointToPuckMap.find(joint); if (puck != _jointToPuckMap.end()) { uint32_t puckIndex = puck->second; auto puckPose = _poseStateMap.find(puckIndex); auto puckOffset = _pucksOffset.find(puckIndex); if ((puckPose != _poseStateMap.end()) && (puckOffset != _pucksOffset.end())) { return puckPose->second.postTransform(puckOffset->second); } } return controller::Pose(); } void ViveControllerManager::InputDevice::handleHmd(uint32_t deviceIndex, const controller::InputCalibrationData& inputCalibrationData) { uint32_t poseIndex = controller::TRACKED_OBJECT_00 + deviceIndex; if (_system->IsTrackedDeviceConnected(deviceIndex) && _system->GetTrackedDeviceClass(deviceIndex) == vr::TrackedDeviceClass_HMD && _nextSimPoseData.vrPoses[deviceIndex].bPoseIsValid) { const mat4& mat = _nextSimPoseData.poses[deviceIndex]; const vec3 linearVelocity = _nextSimPoseData.linearVelocities[deviceIndex]; const vec3 angularVelocity = _nextSimPoseData.angularVelocities[deviceIndex]; handleHeadPoseEvent(inputCalibrationData, mat, linearVelocity, angularVelocity); } } void ViveControllerManager::InputDevice::handleHandController(float deltaTime, uint32_t deviceIndex, const controller::InputCalibrationData& inputCalibrationData, bool isLeftHand) { if (_system->IsTrackedDeviceConnected(deviceIndex) && _system->GetTrackedDeviceClass(deviceIndex) == vr::TrackedDeviceClass_Controller && _nextSimPoseData.vrPoses[deviceIndex].bPoseIsValid) { // process pose const mat4& mat = _nextSimPoseData.poses[deviceIndex]; const vec3 linearVelocity = _nextSimPoseData.linearVelocities[deviceIndex]; const vec3 angularVelocity = _nextSimPoseData.angularVelocities[deviceIndex]; handlePoseEvent(deltaTime, inputCalibrationData, mat, linearVelocity, angularVelocity, isLeftHand); vr::VRControllerState_t controllerState = vr::VRControllerState_t(); if (_system->GetControllerState(deviceIndex, &controllerState, sizeof(vr::VRControllerState_t))) { // process each button for (uint32_t i = 0; i < vr::k_EButton_Max; ++i) { auto mask = vr::ButtonMaskFromId((vr::EVRButtonId)i); bool pressed = 0 != (controllerState.ulButtonPressed & mask); bool touched = 0 != (controllerState.ulButtonTouched & mask); handleButtonEvent(deltaTime, i, pressed, touched, isLeftHand); } // process each axis for (uint32_t i = 0; i < vr::k_unControllerStateAxisCount; i++) { handleAxisEvent(deltaTime, i, controllerState.rAxis[i].x, controllerState.rAxis[i].y, isLeftHand); } // pseudo buttons the depend on both of the above for-loops