From 8538c700b5dfc16a0582aa5c0d33a5585887a722 Mon Sep 17 00:00:00 2001 From: Dante Ruiz Date: Thu, 25 May 2017 21:41:41 +0100 Subject: [PATCH] saving head work --- interface/resources/controllers/vive.json | 22 ++++---- plugins/openvr/src/ViveControllerManager.cpp | 53 ++++++++++++++++++-- plugins/openvr/src/ViveControllerManager.h | 26 +++++----- 3 files changed, 70 insertions(+), 31 deletions(-) diff --git a/interface/resources/controllers/vive.json b/interface/resources/controllers/vive.json index 591b6f515e..984f9c65fb 100644 --- a/interface/resources/controllers/vive.json +++ b/interface/resources/controllers/vive.json @@ -34,36 +34,32 @@ { "from": "Vive.RSCenter", "to": "Standard.RightPrimaryThumb" }, { "from": "Vive.RightApplicationMenu", "to": "Standard.RightSecondaryThumb" }, - { "from": "Vive.LeftHand", "to": "Standard.LeftHand", "when": [ "Application.InHMD" ] }, - { "from": "Vive.RightHand", "to": "Standard.RightHand", "when": [ "Application.InHMD" ] }, + { "from": "Vive.LeftHand", "to": "Standard.LeftHand" }, + { "from": "Vive.RightHand", "to": "Standard.RightHand" }, { "from": "Vive.LeftFoot", "to" : "Standard.LeftFoot", - "filters" : [{"type" : "lowVelocity", "rotation" : 1.0, "translation": 1.0}], - "when": [ "Application.InHMD"] + "filters" : [{"type" : "lowVelocity", "rotation" : 1.0, "translation": 1.0}] }, { "from": "Vive.RightFoot", "to" : "Standard.RightFoot", - "filters" : [{"type" : "lowVelocity", "rotation" : 1.0, "translation": 1.0}], - "when": [ "Application.InHMD"] + "filters" : [{"type" : "lowVelocity", "rotation" : 1.0, "translation": 1.0}] }, { "from": "Vive.Hips", "to" : "Standard.Hips", - "filters" : [{"type" : "lowVelocity", "rotation" : 0.01, "translation": 0.01}], - "when": [ "Application.InHMD"] + "filters" : [{"type" : "lowVelocity", "rotation" : 0.01, "translation": 0.01}] }, { "from": "Vive.Spine2", "to" : "Standard.Spine2", - "filters" : [{"type" : "lowVelocity", "rotation" : 0.01, "translation": 0.01}], - "when": [ "Application.InHMD"] + "filters" : [{"type" : "lowVelocity", "rotation" : 0.01, "translation": 0.01}] }, - { "from": "Vive.Head", "to" : "Standard.Head", "when" : [ "Application.InHMD"] }, + { "from": "Vive.Head", "to" : "Standard.Head"}, - { "from": "Vive.RightArm", "to" : "Standard.RightArm", "when" : [ "Application.InHMD"] }, - { "from": "Vive.LeftArm", "to" : "Standard.LeftArm", "when" : [ "Application.InHMD"] } + { "from": "Vive.RightArm", "to" : "Standard.RightArm"}, + { "from": "Vive.LeftArm", "to" : "Standard.LeftArm"} ] } diff --git a/plugins/openvr/src/ViveControllerManager.cpp b/plugins/openvr/src/ViveControllerManager.cpp index 016b2a1c55..a9bc066dc2 100644 --- a/plugins/openvr/src/ViveControllerManager.cpp +++ b/plugins/openvr/src/ViveControllerManager.cpp @@ -51,8 +51,7 @@ 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_SHOULDERS = 5; -static const int MIN_FEET_HIPS_CHEST_AND_HANDS = 6; -static const int MIN_FEET_HIPS_SHOULDERS_AND_HANDS = 7; +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; @@ -177,6 +176,7 @@ ViveControllerManager::InputDevice::InputDevice(vr::IVRSystem*& system) : contro _configStringMap[Config::FeetAndHips] = QString("FeetAndHips"); _configStringMap[Config::FeetHipsAndChest] = QString("FeetHipsAndChest"); _configStringMap[Config::FeetHipsAndShoulders] = QString("FeetHipsAndShoulders"); + _configStringMap[Config::FeetHipsChestAndHead] = QString("FeetHipsChestAndHead"); if (openVrSupported()) { createPreferences(); @@ -351,12 +351,18 @@ void ViveControllerManager::InputDevice::calibrate(const controller::InputCalibr calibrateFeet(defaultToReferenceMat, inputCalibration); calibrateHips(defaultToReferenceMat, inputCalibration); calibrateChest(defaultToReferenceMat, inputCalibration); - } else if (_config == Config::FeetHipsAndShoulders && puckCount >= MIN_FEET_HIPS_SHOULDERS){ + } 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::FeetHipsChestAndHead && puckCount == MIN_FEET_HIPS_CHEST_HEAD) { + glm::mat4 headPuckDefaultToReferenceMat = recalculateDefaultToReferenceForHeadPuck(inputCalibration); + calibrateFeet(headPuckDefaultToReferenceMat, inputCalibration); + calibrateHips(headPuckDefaultToReferenceMat, inputCalibration); + calibrateChest(headPuckDefaultToReferenceMat, inputCalibration); + calibrateHead(headPuckDefaultToReferenceMat, inputCalibration); } else { qDebug() << "Puck Calibration: " << configToString(_config) << " Config Failed: Could not meet the minimal # of pucks"; uncalibrate(); @@ -371,6 +377,7 @@ void ViveControllerManager::InputDevice::uncalibrate() { _pucksOffset.clear(); _jointToPuckMap.clear(); _calibrated = false; + _overrideHead = false; } void ViveControllerManager::InputDevice::updateCalibratedLimbs() { @@ -380,6 +387,10 @@ void ViveControllerManager::InputDevice::updateCalibratedLimbs() { _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 { @@ -445,6 +456,27 @@ void ViveControllerManager::InputDevice::handleHandController(float deltaTime, u } } +glm::mat4 ViveControllerManager::InputDevice::recalculateDefaultToReferenceForHeadPuck(const controller::InputCalibrationData& inputCalibration) { + size_t headPuckIndex = _validTrackedObjects.size() - 1; + controller::Pose headPuckPose = _validTrackedObjects[headPuckIndex].second; + + // make the head puck rotation to match the default head rotation + glm::quat headPuckRotation = cancelOutRollAndPitch(headPuckPose.getRotation()); + glm::quat defaultHeadRotation = glmExtractRotation(inputCalibration.defaultHeadMat); + glm::quat defaultRotationOffset = glm::inverse(headPuckRotation) * defaultHeadRotation; + glm::quat finalHeadPuckRotation = defaultRotationOffset * headPuckRotation; + glm::vec3 headPuckTranslation = headPuckPose.getTranslation(); + glm::mat4 headPuckMat = createMatFromQuatAndPos(finalHeadPuckRotation, headPuckTranslation); + + // calculate the offset from the centerOfEye to defaultHeadMat + glm::mat4 defaultHeadOffset = glm::inverse(inputCalibration.defaultCenterEyeMat) * inputCalibration.defaultHeadMat; + + glm::mat4 currentHead = headPuckMat * defaultHeadOffset; + + // calculate the defaultToRefrenceXform + return currentHead * glm::inverse(inputCalibration.defaultHeadMat); +} + void ViveControllerManager::InputDevice::partitionTouchpad(int sButton, int xAxis, int yAxis, int centerPseudoButton, int xPseudoButton, int yPseudoButton) { // Populate the L/RS_CENTER/OUTER pseudo buttons, corresponding to a partition of the L/RS space based on the X/Y values. const float CENTER_DEADBAND = 0.6f; @@ -664,7 +696,16 @@ void ViveControllerManager::InputDevice::calibrateShoulders(glm::mat4& defaultTo } void ViveControllerManager::InputDevice::calibrateHead(glm::mat4& defaultToReferenceMat, const controller::InputCalibrationData& inputCalibration) { - int headIndex = _validTrackedObjects.size() - 1; + size_t headIndex = _validTrackedObjects.size() - 1; + const PuckPosePair& head = _validTrackedObjects[headIndex]; + + // assume the person is wearing the head puck on his/her forehead + glm::mat4 defaultHeadOffset = glm::inverse(inputCalibration.defaultCenterEyeMat) * inputCalibration.defaultHeadMat; + controller::Pose newHead = head.second.postTransform(defaultHeadOffset); + + _jointToPuckMap[controller::HEAD] = head.first; + _pucksOffset[head.first] = computeOffset(defaultToReferenceMat, inputCalibration.defaultHeadMat, newHead); + _overrideHead = true; } @@ -701,6 +742,8 @@ void ViveControllerManager::InputDevice::setConfigFromString(const QString& valu _preferedConfig = Config::FeetHipsAndChest; } else if (value == "FeetHipsAndShoulders") { _preferedConfig = Config::FeetHipsAndShoulders; + } else if (value == "FeetHipsChestAndHead") { + _preferedConfig = Config::FeetHipsChestAndHead; } } @@ -713,7 +756,7 @@ void ViveControllerManager::InputDevice::createPreferences() { auto getter = [this]()->QString { return _configStringMap[_preferedConfig]; }; auto setter = [this](const QString& value) { setConfigFromString(value); saveSettings(); }; auto preference = new ComboBoxPreference(VIVE_PUCKS_CONFIG, "Configuration", getter, setter); - QStringList list = (QStringList() << "Auto" << "Feet" << "FeetAndHips" << "FeetHipsAndChest" << "FeetHipsAndShoulders"); + QStringList list = (QStringList() << "Feet" << "FeetAndHips" << "FeetHipsAndChest" << "FeetHipsAndShoulders" << "FeetHipsChestAndHead"); preference->setItems(list); preferences->addPreference(preference); diff --git a/plugins/openvr/src/ViveControllerManager.h b/plugins/openvr/src/ViveControllerManager.h index bc9558beaa..f3b87ea3a0 100644 --- a/plugins/openvr/src/ViveControllerManager.h +++ b/plugins/openvr/src/ViveControllerManager.h @@ -67,6 +67,7 @@ private: void calibrate(const controller::InputCalibrationData& inputCalibration); void uncalibrate(); controller::Pose addOffsetToPuckPose(int joint) const; + glm::mat4 recalculateDefaultToReferenceForHeadPuck(const controller::InputCalibrationData& inputCalibration); void updateCalibratedLimbs(); bool checkForCalibrationEvent(); void handleHandController(float deltaTime, uint32_t deviceIndex, const controller::InputCalibrationData& inputCalibrationData, bool isLeftHand); @@ -80,6 +81,17 @@ private: const vec3& angularVelocity); void partitionTouchpad(int sButton, int xAxis, int yAxis, int centerPsuedoButton, int xPseudoButton, int yPseudoButton); void printDeviceTrackingResultChange(uint32_t deviceIndex); + void setConfigFromString(const QString& value); + void loadSettings(); + void saveSettings() const; + void calibrateFeet(glm::mat4& defaultToReferenceMat, const controller::InputCalibrationData& inputCalibration); + void calibrateHips(glm::mat4& defaultToReferenceMat, const controller::InputCalibrationData& inputCalibration); + void calibrateChest(glm::mat4& defaultToReferenceMat, const controller::InputCalibrationData& inputCalibration); + + void calibrateShoulders(glm::mat4& defaultToReferenceMat, const controller::InputCalibrationData& inputCalibration, + int firstShoulderIndex, int secondShoulderIndex); + + void calibrateHead(glm::mat4& defaultToReferenceMat, const controller::InputCalibrationData& inputCalibration); class FilteredStick { public: @@ -106,9 +118,7 @@ private: }; enum class Config { Auto, - Head, Feet, - Shoulders, FeetAndHips, FeetHipsAndChest, FeetHipsAndShoulders, @@ -139,21 +149,11 @@ private: bool _triggersPressedHandled { false }; bool _calibrated { false }; bool _timeTilCalibrationSet { false }; - bool overrideHands { false }; + bool _overrideHead { false }; mutable std::recursive_mutex _lock; QString configToString(Config config); - void setConfigFromString(const QString& value); - void loadSettings(); - void saveSettings() const; - void calibrateFeet(glm::mat4& defaultToReferenceMat, const controller::InputCalibrationData& inputCalibration); - void calibrateHips(glm::mat4& defaultToReferenceMat, const controller::InputCalibrationData& inputCalibration); - void calibrateChest(glm::mat4& defaultToReferenceMat, const controller::InputCalibrationData& inputCalibration); - void calibrateShoulders(glm::mat4& defaultToReferenceMat, const controller::InputCalibrationData& inputCalibration, - int firstShoulderIndex, int secondShoulderIndex); - - void calibrateHead(glm::mat4& defaultToReferenceMat, const controller::InputCalibrationData& inputCalibration); friend class ViveControllerManager; };