From 15cad1fee914264de3e7a084f499efd813dbfe3e Mon Sep 17 00:00:00 2001 From: Dante Ruiz Date: Tue, 2 May 2017 21:33:58 +0100 Subject: [PATCH 01/15] saving work --- plugins/openvr/src/ViveControllerManager.cpp | 72 +++++++++++++++++++- plugins/openvr/src/ViveControllerManager.h | 16 ++++- 2 files changed, 83 insertions(+), 5 deletions(-) diff --git a/plugins/openvr/src/ViveControllerManager.cpp b/plugins/openvr/src/ViveControllerManager.cpp index 86b37135d2..293885f8ee 100644 --- a/plugins/openvr/src/ViveControllerManager.cpp +++ b/plugins/openvr/src/ViveControllerManager.cpp @@ -10,6 +10,7 @@ // #include "ViveControllerManager.h" +#include #include #include @@ -41,9 +42,16 @@ 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"; +const quint64 CALIBRATION_TIMELAPSE = 3000000; const char* ViveControllerManager::NAME { "OpenVR" }; +bool sortPucksYPosition(std::pair firstPuck, std::pair secondPuck) { + controller::Pose firstPose = firstPuck.second; + controller::Pose secondPose = secondPuck.second; + return (firstPose.translation.y < secondPose.translation.y); +} + bool ViveControllerManager::isSupported() const { return openVrSupported(); } @@ -125,6 +133,7 @@ void ViveControllerManager::pluginUpdate(float deltaTime, const controller::Inpu 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()) { @@ -164,10 +173,10 @@ void ViveControllerManager::InputDevice::update(float deltaTime, const controlle numTrackedControllers++; } _trackedControllers = numTrackedControllers; + calibrate(inputCalibrationData); } void ViveControllerManager::InputDevice::handleTrackedObject(uint32_t deviceIndex, const controller::InputCalibrationData& inputCalibrationData) { - uint32_t poseIndex = controller::TRACKED_OBJECT_00 + deviceIndex; if (_system->IsTrackedDeviceConnected(deviceIndex) && @@ -185,12 +194,71 @@ void ViveControllerManager::InputDevice::handleTrackedObject(uint32_t deviceInde // 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::calibrate(const controller::InputCalibrationData& inputCalibration) { + quint64 currentTime = usecTimestampNow(); + + auto leftTrigger = _buttonPressedMap.find(controller::LT); + auto rightTrigger = _buttonPressedMap.find(controller::RT); + if ((leftTrigger != _buttonPressedMap.end()) && (rightTrigger != _buttonPressedMap.end())) { + if (!_calibrated) { + auto puckCount = _validTrackedObjects.size(); + if (puckCount == 2) { + qDebug() << "-------------< configure feet <-------------"; + _config = Config::Feet; + } else if (puckCount == 3) { + qDebug() << "-------------> configure feet and hips <-------------"; + _config = Config::FeetAndHips; + } else if (puckCount >= 4) { + _config = Config::FeetHipsAndChest; + } else { + qDebug() << "Could not configure # pucks " << puckCount; + return; + } + + std::sort(_validTrackedObjects.begin(), _validTrackedObjects.end(), sortPucksYPosition); + auto firstFoot = _validTrackedObjects[0]; + auto secondFoot = _validTrackedObjects[1]; + controller::Pose firstFootPose = firstFoot.second; + controller::Pose secondFootPose = secondFoot.second; + if (firstFootPose.translation.x > secondFootPose.translation.x) { + _jointToPuckMap[controller::LEFT_FOOT] = firstFoot.first; + _jointToPuckMap[controller::RIGHT_FOOT] = secondFoot.first; + } else { + _jointToPuckMap[controller::LEFT_FOOT] = secondFoot.first; + _jointToPuckMap[controller::RIGHT_FOOT] = firstFoot.first; + + } + + if (_config == Config::Feet) { + // done + } else if (_config == Config::FeetAndHips) { + _jointToPuckMap[controller::HIPS] = _validTrackedObjects[2].first; + } else if (_config == Config::FeetHipsAndChest) { + _jointToPuckMap[controller::HIPS] = _validTrackedObjects[2].first; + _jointToPuckMap[controller::SPINE2] = _validTrackedObjects[2].first; + } + + computePucksOffset(inputCalibration); + _calibrated = true; + + } else { + qDebug() << "---- de-calibrated ---"; + + } + + } +} + +void ViveControllerManager::InputDevice::computePucksOffset(const controller::InputCalibrationData& inputCalibration) { + +} void ViveControllerManager::InputDevice::handleHandController(float deltaTime, uint32_t deviceIndex, const controller::InputCalibrationData& inputCalibrationData, bool isLeftHand) { if (_system->IsTrackedDeviceConnected(deviceIndex) && @@ -353,7 +421,7 @@ void ViveControllerManager::InputDevice::hapticsHelper(float deltaTime, bool lef float hapticTime = strength * MAX_HAPTIC_TIME; if (hapticTime < duration * 1000.0f) { _system->TriggerHapticPulse(deviceIndex, 0, hapticTime); - } + } float remainingHapticTime = duration - (hapticTime / 1000.0f + deltaTime * 1000.0f); // in milliseconds if (leftHand) { diff --git a/plugins/openvr/src/ViveControllerManager.h b/plugins/openvr/src/ViveControllerManager.h index dc1883d5e4..5d844b6499 100644 --- a/plugins/openvr/src/ViveControllerManager.h +++ b/plugins/openvr/src/ViveControllerManager.h @@ -14,9 +14,11 @@ #include #include +#include +#include +#include #include - #include #include #include @@ -58,7 +60,8 @@ private: bool triggerHapticPulse(float strength, float duration, controller::Hand hand) override; void hapticsHelper(float deltaTime, bool leftHand); - + void calibrate(const controller::InputCalibrationData& inputCalibration); + void computePucksOffset(const controller::InputCalibrationData& inputCalibration); void handleHandController(float deltaTime, uint32_t deviceIndex, const controller::InputCalibrationData& inputCalibrationData, bool isLeftHand); void handleTrackedObject(uint32_t deviceIndex, const controller::InputCalibrationData& inputCalibrationData); void handleButtonEvent(float deltaTime, uint32_t button, bool pressed, bool touched, bool isLeftHand); @@ -90,10 +93,14 @@ private: float _timer { 0.0f }; glm::vec2 _stick { 0.0f, 0.0f }; }; - + enum class Config { Feet, FeetAndHips, FeetHipsAndChest, NoConfig }; + Config _config { Config::NoConfig }; FilteredStick _filteredLeftStick; FilteredStick _filteredRightStick; + std::vector> _validTrackedObjects; + std::map _pucksOffset; + std::map _jointToPuckMap; // perform an action when the InputDevice mutex is acquired. using Locker = std::unique_lock; template @@ -101,10 +108,13 @@ private: int _trackedControllers { 0 }; vr::IVRSystem*& _system; + quint64 _calibrateAfterTimelapse { 0.f }; float _leftHapticStrength { 0.0f }; float _leftHapticDuration { 0.0f }; float _rightHapticStrength { 0.0f }; float _rightHapticDuration { 0.0f }; + bool _calibrate { false }; + bool _calibrated { false }; mutable std::recursive_mutex _lock; friend class ViveControllerManager; From 8e2b25eacc80c8da62f80cb52dff03c0ecd309b2 Mon Sep 17 00:00:00 2001 From: Dante Ruiz Date: Wed, 3 May 2017 00:25:41 +0100 Subject: [PATCH 02/15] working on computing offsets --- plugins/openvr/src/ViveControllerManager.cpp | 34 +++++++++++++++----- plugins/openvr/src/ViveControllerManager.h | 3 +- 2 files changed, 27 insertions(+), 10 deletions(-) diff --git a/plugins/openvr/src/ViveControllerManager.cpp b/plugins/openvr/src/ViveControllerManager.cpp index 293885f8ee..73cb26410c 100644 --- a/plugins/openvr/src/ViveControllerManager.cpp +++ b/plugins/openvr/src/ViveControllerManager.cpp @@ -22,6 +22,7 @@ #include #include #include +#include #include @@ -46,6 +47,16 @@ const quint64 CALIBRATION_TIMELAPSE = 3000000; const char* ViveControllerManager::NAME { "OpenVR" }; +/*glm::mat4 computeDefualtHead(glm::mat4 centerOfEye, glm::mat4 jointHead) { + headOffset = glm::muliply(glm::inverse(centerOfEye), jointHead); + return glm:: + }*/ + +glm::mat4 computeOffset(glm::mat4 defaultRefrence, glm::mat4 defaultJointMat, controller::Pose puckPose) { + qDebug() << "-------------> computing offset <-------------"; + glm::mat4 puckMat = create + return mat4(); +} bool sortPucksYPosition(std::pair firstPuck, std::pair secondPuck) { controller::Pose firstPose = firstPuck.second; controller::Pose secondPose = secondPuck.second; @@ -203,11 +214,17 @@ void ViveControllerManager::InputDevice::handleTrackedObject(uint32_t deviceInde void ViveControllerManager::InputDevice::calibrate(const controller::InputCalibrationData& inputCalibration) { quint64 currentTime = usecTimestampNow(); - + auto leftTrigger = _buttonPressedMap.find(controller::LT); auto rightTrigger = _buttonPressedMap.find(controller::RT); if ((leftTrigger != _buttonPressedMap.end()) && (rightTrigger != _buttonPressedMap.end())) { if (!_calibrated) { + glm::mat4 controllerToAvatar = glm::inverse(inputCalibration.avatarMat) * inputCalibration.sensorToWorldMat; + glm::mat4 currentHead = inputCalibration.hmdSensorMat * controllerToAvatar; + glm::quat canceledRollAndPitch = cancelOutRollAndPitch(glmExtractRotation(currentHead)); + glm::vec3 currentHeadPosition = extractTranslation(currentHead); + currentHead = createMatFromQuatAndPos(canceledRollAndPitch, currentHeadPosition); + glm::mat4 defaultRefrenceXform = currentHead * glm::inverse(inputCalibration.defaultHeadMat); auto puckCount = _validTrackedObjects.size(); if (puckCount == 2) { qDebug() << "-------------< configure feet <-------------"; @@ -229,23 +246,27 @@ void ViveControllerManager::InputDevice::calibrate(const controller::InputCalibr controller::Pose secondFootPose = secondFoot.second; if (firstFootPose.translation.x > secondFootPose.translation.x) { _jointToPuckMap[controller::LEFT_FOOT] = firstFoot.first; + _pucksOffset[firstFoot.first] = computeOffset(defaultRefrenceXform, inputCalibration.defaultLeftFoot, firstFootPose); _jointToPuckMap[controller::RIGHT_FOOT] = secondFoot.first; + _pucksOffset[secondFoot.first] = computeOffset(defaultRefrenceXform, inputCalibration.defaultRightFoot, secondFootPose); } else { _jointToPuckMap[controller::LEFT_FOOT] = secondFoot.first; + _pucksOffset[secondFoot.first] = computeOffset(defaultRefrenceXform, inputCalibration.defaultLeftFoot, secondFootPose); _jointToPuckMap[controller::RIGHT_FOOT] = firstFoot.first; - + _pucksOffset[firstFoot.first] = computeOffset(defaultRefrenceXform, inputCalibration.defaultRightFoot, firstFootPose); } if (_config == Config::Feet) { // done } else if (_config == Config::FeetAndHips) { _jointToPuckMap[controller::HIPS] = _validTrackedObjects[2].first; + _pucksOffset[_validTrackedObjects[2].first] = computeOffset(defaultRefrenceXform, inputCalibration.defaultHips, _validTrackedObjects[2].second); } else if (_config == Config::FeetHipsAndChest) { _jointToPuckMap[controller::HIPS] = _validTrackedObjects[2].first; + _pucksOffset[_validTrackedObjects[2].first] = computeOffset(defaultRefrenceXform, inputCalibration.defaultHips, _validTrackedObjects[2].second); _jointToPuckMap[controller::SPINE2] = _validTrackedObjects[2].first; - } - - computePucksOffset(inputCalibration); + _pucksOffset[_validTrackedObjects[3].first] = computeOffset(defaultRefrenceXform, inputCalibration.defaultSpine2, _validTrackedObjects[3].second); + } _calibrated = true; } else { @@ -256,9 +277,6 @@ void ViveControllerManager::InputDevice::calibrate(const controller::InputCalibr } } -void ViveControllerManager::InputDevice::computePucksOffset(const controller::InputCalibrationData& inputCalibration) { - -} void ViveControllerManager::InputDevice::handleHandController(float deltaTime, uint32_t deviceIndex, const controller::InputCalibrationData& inputCalibrationData, bool isLeftHand) { if (_system->IsTrackedDeviceConnected(deviceIndex) && diff --git a/plugins/openvr/src/ViveControllerManager.h b/plugins/openvr/src/ViveControllerManager.h index 5d844b6499..966f1e879b 100644 --- a/plugins/openvr/src/ViveControllerManager.h +++ b/plugins/openvr/src/ViveControllerManager.h @@ -61,7 +61,6 @@ private: bool triggerHapticPulse(float strength, float duration, controller::Hand hand) override; void hapticsHelper(float deltaTime, bool leftHand); void calibrate(const controller::InputCalibrationData& inputCalibration); - void computePucksOffset(const controller::InputCalibrationData& inputCalibration); void handleHandController(float deltaTime, uint32_t deviceIndex, const controller::InputCalibrationData& inputCalibrationData, bool isLeftHand); void handleTrackedObject(uint32_t deviceIndex, const controller::InputCalibrationData& inputCalibrationData); void handleButtonEvent(float deltaTime, uint32_t button, bool pressed, bool touched, bool isLeftHand); @@ -99,7 +98,7 @@ private: FilteredStick _filteredRightStick; std::vector> _validTrackedObjects; - std::map _pucksOffset; + std::map _pucksOffset; std::map _jointToPuckMap; // perform an action when the InputDevice mutex is acquired. using Locker = std::unique_lock; From fb502a7fede162834a1c7e495b805efb6f1a02ac Mon Sep 17 00:00:00 2001 From: Dante Ruiz Date: Wed, 3 May 2017 00:37:35 +0100 Subject: [PATCH 03/15] add offset function --- plugins/openvr/src/ViveControllerManager.cpp | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) diff --git a/plugins/openvr/src/ViveControllerManager.cpp b/plugins/openvr/src/ViveControllerManager.cpp index 73cb26410c..29d036fd31 100644 --- a/plugins/openvr/src/ViveControllerManager.cpp +++ b/plugins/openvr/src/ViveControllerManager.cpp @@ -54,8 +54,9 @@ const char* ViveControllerManager::NAME { "OpenVR" }; glm::mat4 computeOffset(glm::mat4 defaultRefrence, glm::mat4 defaultJointMat, controller::Pose puckPose) { qDebug() << "-------------> computing offset <-------------"; - glm::mat4 puckMat = create - return mat4(); + glm::mat4 poseMat = createMatFromQuatAndPos(puckPose.rotation, puckPose.translation); + glm::mat4 refrenceJointMat = defaultRefrence * defaultJointMat; + return glm::inverse(poseMat) * refrenceJointMat; } bool sortPucksYPosition(std::pair firstPuck, std::pair secondPuck) { controller::Pose firstPose = firstPuck.second; @@ -221,10 +222,11 @@ void ViveControllerManager::InputDevice::calibrate(const controller::InputCalibr if (!_calibrated) { glm::mat4 controllerToAvatar = glm::inverse(inputCalibration.avatarMat) * inputCalibration.sensorToWorldMat; glm::mat4 currentHead = inputCalibration.hmdSensorMat * controllerToAvatar; + glm::mat4 defaultHeadOffset = glm::inverse(inputCalibration.defaultCenterEyeMat) * inputCalibration.defaultHeadMat; glm::quat canceledRollAndPitch = cancelOutRollAndPitch(glmExtractRotation(currentHead)); glm::vec3 currentHeadPosition = extractTranslation(currentHead); currentHead = createMatFromQuatAndPos(canceledRollAndPitch, currentHeadPosition); - glm::mat4 defaultRefrenceXform = currentHead * glm::inverse(inputCalibration.defaultHeadMat); + glm::mat4 defaultRefrenceXform = currentHead * defaultHeadOffset; auto puckCount = _validTrackedObjects.size(); if (puckCount == 2) { qDebug() << "-------------< configure feet <-------------"; From 31f6038e457586d6ceaafb94b8e6eff55d8b45c7 Mon Sep 17 00:00:00 2001 From: Dante Ruiz Date: Wed, 3 May 2017 20:08:42 +0100 Subject: [PATCH 04/15] limbs tracking pucks --- interface/resources/controllers/vive.json | 6 ++- plugins/openvr/src/ViveControllerManager.cpp | 57 ++++++++++++++++---- plugins/openvr/src/ViveControllerManager.h | 2 + 3 files changed, 54 insertions(+), 11 deletions(-) diff --git a/interface/resources/controllers/vive.json b/interface/resources/controllers/vive.json index 4fbdb37abf..5bdffadbbf 100644 --- a/interface/resources/controllers/vive.json +++ b/interface/resources/controllers/vive.json @@ -35,6 +35,10 @@ { "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.RightHand", "to": "Standard.RightHand", "when": [ "Application.InHMD" ] }, + { "from": "Vive.LeftFoot", "to" : "Standard.LeftFoot", "when": [ "Application.InHMD"] }, + { "from": "Vive.RightFoot", "to" : "Standard.RightFoot", "when": [ "Application.InHMD"] }, + { "from": "Vive.Hips", "to" : "Standard.Hips", "when": [ "Application.InHMD"] }, + { "from": "Vive.Spine2", "to" : "Standard.Spine2", "when": [ "Application.InHMD"] } ] } diff --git a/plugins/openvr/src/ViveControllerManager.cpp b/plugins/openvr/src/ViveControllerManager.cpp index 29d036fd31..1a1f6b0977 100644 --- a/plugins/openvr/src/ViveControllerManager.cpp +++ b/plugins/openvr/src/ViveControllerManager.cpp @@ -53,7 +53,7 @@ const char* ViveControllerManager::NAME { "OpenVR" }; }*/ glm::mat4 computeOffset(glm::mat4 defaultRefrence, glm::mat4 defaultJointMat, controller::Pose puckPose) { - qDebug() << "-------------> computing offset <-------------"; + ///qDebug() << "-------------> computing offset <-------------"; glm::mat4 poseMat = createMatFromQuatAndPos(puckPose.rotation, puckPose.translation); glm::mat4 refrenceJointMat = defaultRefrence * defaultJointMat; return glm::inverse(poseMat) * refrenceJointMat; @@ -134,6 +134,7 @@ void ViveControllerManager::pluginUpdate(float deltaTime, const controller::Inpu userInputMapper->removeDevice(_inputDevice->_deviceID); _registeredWithInputMapper = false; _inputDevice->_poseStateMap.clear(); + //qDebug() << " ----------->>!!!!!!! clear pose state map !!!!!!!<<----------------"; } if (!_registeredWithInputMapper && _inputDevice->_trackedControllers > 0) { @@ -186,6 +187,7 @@ void ViveControllerManager::InputDevice::update(float deltaTime, const controlle } _trackedControllers = numTrackedControllers; calibrate(inputCalibrationData); + updateCalibratedLimbs(); } void ViveControllerManager::InputDevice::handleTrackedObject(uint32_t deviceIndex, const controller::InputCalibrationData& inputCalibrationData) { @@ -220,21 +222,28 @@ void ViveControllerManager::InputDevice::calibrate(const controller::InputCalibr auto rightTrigger = _buttonPressedMap.find(controller::RT); if ((leftTrigger != _buttonPressedMap.end()) && (rightTrigger != _buttonPressedMap.end())) { if (!_calibrated) { + // conver the hmd head from sensor space to avatar space glm::mat4 controllerToAvatar = glm::inverse(inputCalibration.avatarMat) * inputCalibration.sensorToWorldMat; - glm::mat4 currentHead = inputCalibration.hmdSensorMat * controllerToAvatar; + glm::mat4 hmdHeadInAvatarSpace = inputCalibration.hmdSensorMat * controllerToAvatar; + + // cancel the roll and pitch for the hmd head + glm::quat canceledRollAndPitch = cancelOutRollAndPitch(glmExtractRotation(hmdHeadInAvatarSpace)); + glm::vec3 hmdHeadPosition = extractTranslation(hmdHeadInAvatarSpace); + glm::mat4 hmdHeadMat = createMatFromQuatAndPos(canceledRollAndPitch, hmdHeadPosition); + + //calculate the offset from the centerEye to the head hoint glm::mat4 defaultHeadOffset = glm::inverse(inputCalibration.defaultCenterEyeMat) * inputCalibration.defaultHeadMat; - glm::quat canceledRollAndPitch = cancelOutRollAndPitch(glmExtractRotation(currentHead)); - glm::vec3 currentHeadPosition = extractTranslation(currentHead); - currentHead = createMatFromQuatAndPos(canceledRollAndPitch, currentHeadPosition); - glm::mat4 defaultRefrenceXform = currentHead * defaultHeadOffset; + + glm::mat4 defaultRefrenceXform = hmdHeadMat * defaultHeadOffset; auto puckCount = _validTrackedObjects.size(); if (puckCount == 2) { - qDebug() << "-------------< configure feet <-------------"; + //qDebug() << "-------------< configure feet <-------------"; _config = Config::Feet; } else if (puckCount == 3) { - qDebug() << "-------------> configure feet and hips <-------------"; + //qDebug() << "-------------> configure feet and hips <-------------"; _config = Config::FeetAndHips; } else if (puckCount >= 4) { + //qDebug() << "-------------> configure feet, hips and chest <---------------"; _config = Config::FeetHipsAndChest; } else { qDebug() << "Could not configure # pucks " << puckCount; @@ -242,17 +251,19 @@ void ViveControllerManager::InputDevice::calibrate(const controller::InputCalibr } std::sort(_validTrackedObjects.begin(), _validTrackedObjects.end(), sortPucksYPosition); + auto firstFoot = _validTrackedObjects[0]; auto secondFoot = _validTrackedObjects[1]; controller::Pose firstFootPose = firstFoot.second; controller::Pose secondFootPose = secondFoot.second; - if (firstFootPose.translation.x > secondFootPose.translation.x) { + if (firstFootPose.translation.x < secondFootPose.translation.x) { _jointToPuckMap[controller::LEFT_FOOT] = firstFoot.first; _pucksOffset[firstFoot.first] = computeOffset(defaultRefrenceXform, inputCalibration.defaultLeftFoot, firstFootPose); _jointToPuckMap[controller::RIGHT_FOOT] = secondFoot.first; _pucksOffset[secondFoot.first] = computeOffset(defaultRefrenceXform, inputCalibration.defaultRightFoot, secondFootPose); } else { _jointToPuckMap[controller::LEFT_FOOT] = secondFoot.first; + qDebug() << " --------> Printing out the offset pose <------------"; _pucksOffset[secondFoot.first] = computeOffset(defaultRefrenceXform, inputCalibration.defaultLeftFoot, secondFootPose); _jointToPuckMap[controller::RIGHT_FOOT] = firstFoot.first; _pucksOffset[firstFoot.first] = computeOffset(defaultRefrenceXform, inputCalibration.defaultRightFoot, firstFootPose); @@ -266,7 +277,7 @@ void ViveControllerManager::InputDevice::calibrate(const controller::InputCalibr } else if (_config == Config::FeetHipsAndChest) { _jointToPuckMap[controller::HIPS] = _validTrackedObjects[2].first; _pucksOffset[_validTrackedObjects[2].first] = computeOffset(defaultRefrenceXform, inputCalibration.defaultHips, _validTrackedObjects[2].second); - _jointToPuckMap[controller::SPINE2] = _validTrackedObjects[2].first; + _jointToPuckMap[controller::SPINE2] = _validTrackedObjects[3].first; _pucksOffset[_validTrackedObjects[3].first] = computeOffset(defaultRefrenceXform, inputCalibration.defaultSpine2, _validTrackedObjects[3].second); } _calibrated = true; @@ -279,6 +290,28 @@ void ViveControllerManager::InputDevice::calibrate(const controller::InputCalibr } } +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); +} + +controller::Pose ViveControllerManager::InputDevice::addOffsetToPuckPose(int joint) { + auto puck = _jointToPuckMap.find(joint); + if (puck != _jointToPuckMap.end()) { + uint32_t puckIndex = puck->second; + controller::Pose puckPose = _poseStateMap[puckIndex]; + glm::mat4 puckOffset = _pucksOffset[puckIndex]; + puckPose.postTransform(puckOffset); + //qDebug() << "-----------> adding offset to puck <-------------- " << puckPose.valid; + return puckPose; + + } + //qDebug() << "---------> joint is not mapped to any thing <--------------"; + return controller::Pose(); +} + void ViveControllerManager::InputDevice::handleHandController(float deltaTime, uint32_t deviceIndex, const controller::InputCalibrationData& inputCalibrationData, bool isLeftHand) { if (_system->IsTrackedDeviceConnected(deviceIndex) && @@ -492,6 +525,10 @@ controller::Input::NamedVector ViveControllerManager::InputDevice::getAvailableI // 3d location of controller makePair(LEFT_HAND, "LeftHand"), makePair(RIGHT_HAND, "RightHand"), + makePair(LEFT_FOOT, "LeftFoot"), + makePair(RIGHT_FOOT, "RightFoot"), + makePair(HIPS, "Hips"), + makePair(SPINE2, "Spine2"), // 16 tracked poses makePair(TRACKED_OBJECT_00, "TrackedObject00"), diff --git a/plugins/openvr/src/ViveControllerManager.h b/plugins/openvr/src/ViveControllerManager.h index 966f1e879b..43dd982f80 100644 --- a/plugins/openvr/src/ViveControllerManager.h +++ b/plugins/openvr/src/ViveControllerManager.h @@ -61,6 +61,8 @@ private: bool triggerHapticPulse(float strength, float duration, controller::Hand hand) override; void hapticsHelper(float deltaTime, bool leftHand); void calibrate(const controller::InputCalibrationData& inputCalibration); + controller::Pose addOffsetToPuckPose(int joint); + void updateCalibratedLimbs(); void handleHandController(float deltaTime, uint32_t deviceIndex, const controller::InputCalibrationData& inputCalibrationData, bool isLeftHand); void handleTrackedObject(uint32_t deviceIndex, const controller::InputCalibrationData& inputCalibrationData); void handleButtonEvent(float deltaTime, uint32_t button, bool pressed, bool touched, bool isLeftHand); From 20c8356bb2b1e7e292f8783a5d5014774edeb43a Mon Sep 17 00:00:00 2001 From: Dante Ruiz Date: Fri, 5 May 2017 22:55:53 +0100 Subject: [PATCH 05/15] debugging rotation fix --- plugins/openvr/src/ViveControllerManager.cpp | 72 ++++++++++++++++---- 1 file changed, 60 insertions(+), 12 deletions(-) diff --git a/plugins/openvr/src/ViveControllerManager.cpp b/plugins/openvr/src/ViveControllerManager.cpp index 1a1f6b0977..8d480a8e6d 100644 --- a/plugins/openvr/src/ViveControllerManager.cpp +++ b/plugins/openvr/src/ViveControllerManager.cpp @@ -1,6 +1,4 @@ -// -// ViveControllerManager.cpp -// input-plugins/src/input-plugins + // // Created by Sam Gondelman on 6/29/15. // Copyright 2013 High Fidelity, Inc. @@ -23,6 +21,9 @@ #include #include #include +#include +#include +#include #include @@ -52,11 +53,11 @@ const char* ViveControllerManager::NAME { "OpenVR" }; return glm:: }*/ -glm::mat4 computeOffset(glm::mat4 defaultRefrence, glm::mat4 defaultJointMat, controller::Pose puckPose) { +glm::mat4 computeOffset(glm::mat4 defaultToRefrenceMat, glm::mat4 defaultJointMat, controller::Pose puckPose) { ///qDebug() << "-------------> computing offset <-------------"; glm::mat4 poseMat = createMatFromQuatAndPos(puckPose.rotation, puckPose.translation); - glm::mat4 refrenceJointMat = defaultRefrence * defaultJointMat; - return glm::inverse(poseMat) * refrenceJointMat; + glm::mat4 refrenceJointMat = defaultToRefrenceMat * defaultJointMat; + return ( glm::inverse(poseMat) * refrenceJointMat); } bool sortPucksYPosition(std::pair firstPuck, std::pair secondPuck) { controller::Pose firstPose = firstPuck.second; @@ -64,6 +65,12 @@ bool sortPucksYPosition(std::pair firstPuck, std::pa return (firstPose.translation.y < secondPose.translation.y); } +void printPose(controller::Pose pose) { + qDebug() << "-------------> printing out controller::Pose <--------------"; + qDebug() << QString::fromStdString(glm::to_string(pose.translation)); + qDebug() << QString::fromStdString(glm::to_string(pose.rotation)); +} + bool ViveControllerManager::isSupported() const { return openVrSupported(); } @@ -223,18 +230,40 @@ void ViveControllerManager::InputDevice::calibrate(const controller::InputCalibr if ((leftTrigger != _buttonPressedMap.end()) && (rightTrigger != _buttonPressedMap.end())) { if (!_calibrated) { // conver the hmd head from sensor space to avatar space - glm::mat4 controllerToAvatar = glm::inverse(inputCalibration.avatarMat) * inputCalibration.sensorToWorldMat; - glm::mat4 hmdHeadInAvatarSpace = inputCalibration.hmdSensorMat * controllerToAvatar; + qDebug() << " -------------------> begin:::InputDevice::calibrate <--------------------"; + glm::mat4 worldToAvatarMat = glm::inverse(inputCalibration.avatarMat) * inputCalibration.sensorToWorldMat; + glm::mat4 hmdHeadInAvatarSpace = worldToAvatarMat * inputCalibration.hmdSensorMat; // cancel the roll and pitch for the hmd head glm::quat canceledRollAndPitch = cancelOutRollAndPitch(glmExtractRotation(hmdHeadInAvatarSpace)); glm::vec3 hmdHeadPosition = extractTranslation(hmdHeadInAvatarSpace); glm::mat4 hmdHeadMat = createMatFromQuatAndPos(canceledRollAndPitch, hmdHeadPosition); + glm::mat4 defaultHeadOffset = glm::inverse(inputCalibration.defaultCenterEyeMat) * inputCalibration.defaultHeadMat; + glm::mat4 defaultRefrenceXform = hmdHeadMat * defaultHeadOffset; + + glm::quat tmpHmdHeadRotation = glmExtractRotation(hmdHeadMat); + /*qDebug() << "----------- hmd head position < ------"; + qDebug() << QString::fromStdString(glm::to_string(hmdHeadPosition)); + qDebug() << "<--------------- hmd head roatation <---------"; + qDebug() << QString::fromStdString(glm::to_string(canceledRollAndPitch));*/ //calculate the offset from the centerEye to the head hoint - glm::mat4 defaultHeadOffset = glm::inverse(inputCalibration.defaultCenterEyeMat) * inputCalibration.defaultHeadMat; + //glm::mat4 defaultHeadOffset = glm::inverse(inputCalibration.defaultCenterEyeMat) * inputCalibration.defaultHeadMat; + glm::vec3 tmpHeadOffsetPosition = extractTranslation(defaultHeadOffset); + glm::quat tmpHeadOffsetRotation = glmExtractRotation(defaultHeadOffset); + + /*qDebug() << "----------------> head offset position <------------------"; + qDebug() << QString::fromStdString(glm::to_string(tmpHeadOffsetPosition)); + qDebug() << "---------------> head offset rotation <-----------------"; + qDebug() << QString::fromStdString(glm::to_string(tmpHeadOffsetRotation));*/ + + glm::vec3 defaultRefrencePosition = extractTranslation(defaultRefrenceXform); + glm::quat defaultRefrenceRotation = glmExtractRotation(defaultRefrenceXform); + qDebug() << "----------------> defaultToReference position <------------------"; + qDebug() << QString::fromStdString(glm::to_string(defaultRefrencePosition)); + qDebug() << "---------------> defaultToReference rotation <-----------------"; + qDebug() << QString::fromStdString(glm::to_string(defaultRefrenceRotation)); - glm::mat4 defaultRefrenceXform = hmdHeadMat * defaultHeadOffset; auto puckCount = _validTrackedObjects.size(); if (puckCount == 2) { //qDebug() << "-------------< configure feet <-------------"; @@ -246,7 +275,7 @@ void ViveControllerManager::InputDevice::calibrate(const controller::InputCalibr //qDebug() << "-------------> configure feet, hips and chest <---------------"; _config = Config::FeetHipsAndChest; } else { - qDebug() << "Could not configure # pucks " << puckCount; + //qDebug() << "Could not configure # pucks " << puckCount; return; } @@ -258,18 +287,33 @@ void ViveControllerManager::InputDevice::calibrate(const controller::InputCalibr controller::Pose secondFootPose = secondFoot.second; if (firstFootPose.translation.x < secondFootPose.translation.x) { _jointToPuckMap[controller::LEFT_FOOT] = firstFoot.first; + + qDebug() << " --------> Printing out the offset pose <------------"; + glm::mat4 offset = computeOffset(defaultRefrenceXform, inputCalibration.defaultLeftFoot, firstFootPose); + qDebug() << "----------------> offset Position <------------------"; + qDebug() << QString::fromStdString(glm::to_string(extractTranslation(offset))); + qDebug() << "---------------> offset rotation <-----------------"; + qDebug() << QString::fromStdString(glm::to_string(glmExtractRotation(offset))); _pucksOffset[firstFoot.first] = computeOffset(defaultRefrenceXform, inputCalibration.defaultLeftFoot, firstFootPose); _jointToPuckMap[controller::RIGHT_FOOT] = secondFoot.first; _pucksOffset[secondFoot.first] = computeOffset(defaultRefrenceXform, inputCalibration.defaultRightFoot, secondFootPose); } else { _jointToPuckMap[controller::LEFT_FOOT] = secondFoot.first; qDebug() << " --------> Printing out the offset pose <------------"; + glm::mat4 offset = computeOffset(defaultRefrenceXform, inputCalibration.defaultLeftFoot, secondFootPose); + qDebug() << "----------------> offset Position <------------------"; + qDebug() << QString::fromStdString(glm::to_string(extractTranslation(offset))); + qDebug() << "---------------> offset rotation <-----------------"; + qDebug() << QString::fromStdString(glm::to_string(glmExtractRotation(offset))); _pucksOffset[secondFoot.first] = computeOffset(defaultRefrenceXform, inputCalibration.defaultLeftFoot, secondFootPose); _jointToPuckMap[controller::RIGHT_FOOT] = firstFoot.first; _pucksOffset[firstFoot.first] = computeOffset(defaultRefrenceXform, inputCalibration.defaultRightFoot, firstFootPose); } if (_config == Config::Feet) { + controller::Pose leftPose = addOffsetToPuckPose(controller::LEFT_FOOT); + qDebug() << "------------> InputDevice::updateCalibratedLimbs <---------"; + printPose(leftPose); // done } else if (_config == Config::FeetAndHips) { _jointToPuckMap[controller::HIPS] = _validTrackedObjects[2].first; @@ -279,7 +323,8 @@ void ViveControllerManager::InputDevice::calibrate(const controller::InputCalibr _pucksOffset[_validTrackedObjects[2].first] = computeOffset(defaultRefrenceXform, inputCalibration.defaultHips, _validTrackedObjects[2].second); _jointToPuckMap[controller::SPINE2] = _validTrackedObjects[3].first; _pucksOffset[_validTrackedObjects[3].first] = computeOffset(defaultRefrenceXform, inputCalibration.defaultSpine2, _validTrackedObjects[3].second); - } + } + qDebug() << "-------------------> end:::InputDevice::calibrate <--------------------"; _calibrated = true; } else { @@ -292,6 +337,9 @@ void ViveControllerManager::InputDevice::calibrate(const controller::InputCalibr void ViveControllerManager::InputDevice::updateCalibratedLimbs() { _poseStateMap[controller::LEFT_FOOT] = addOffsetToPuckPose(controller::LEFT_FOOT); + controller::Pose leftPose = addOffsetToPuckPose(controller::LEFT_FOOT); + //qDebug() << "------------> InputDevice::updateCalibratedLimbs <---------"; + //printPose(leftPose); _poseStateMap[controller::RIGHT_FOOT] = addOffsetToPuckPose(controller::RIGHT_FOOT); _poseStateMap[controller::HIPS] = addOffsetToPuckPose(controller::HIPS); _poseStateMap[controller::SPINE2] = addOffsetToPuckPose(controller::SPINE2); From 8813306857fd864d98998391bfdbd0a2154842b9 Mon Sep 17 00:00:00 2001 From: Dante Ruiz Date: Fri, 5 May 2017 23:59:38 +0100 Subject: [PATCH 06/15] fixed the wierd rotation issue --- plugins/openvr/src/ViveControllerManager.cpp | 62 +++----------------- 1 file changed, 9 insertions(+), 53 deletions(-) diff --git a/plugins/openvr/src/ViveControllerManager.cpp b/plugins/openvr/src/ViveControllerManager.cpp index 8d480a8e6d..fffdb7cd33 100644 --- a/plugins/openvr/src/ViveControllerManager.cpp +++ b/plugins/openvr/src/ViveControllerManager.cpp @@ -230,52 +230,30 @@ void ViveControllerManager::InputDevice::calibrate(const controller::InputCalibr if ((leftTrigger != _buttonPressedMap.end()) && (rightTrigger != _buttonPressedMap.end())) { if (!_calibrated) { // conver the hmd head from sensor space to avatar space - qDebug() << " -------------------> begin:::InputDevice::calibrate <--------------------"; glm::mat4 worldToAvatarMat = glm::inverse(inputCalibration.avatarMat) * inputCalibration.sensorToWorldMat; - glm::mat4 hmdHeadInAvatarSpace = worldToAvatarMat * inputCalibration.hmdSensorMat; + glm::mat4 hmdAvatarMat = worldToAvatarMat * inputCalibration.hmdSensorMat; // cancel the roll and pitch for the hmd head - glm::quat canceledRollAndPitch = cancelOutRollAndPitch(glmExtractRotation(hmdHeadInAvatarSpace)); - glm::vec3 hmdHeadPosition = extractTranslation(hmdHeadInAvatarSpace); - glm::mat4 hmdHeadMat = createMatFromQuatAndPos(canceledRollAndPitch, hmdHeadPosition); + glm::quat hmdRotation = cancelOutRollAndPitch(glmExtractRotation(hmdAvatarMat)); + glm::vec3 hmdTranslation = extractTranslation(hmdAvatarMat); + glm::mat4 currentHead = createMatFromQuatAndPos(hmdRotation, hmdTranslation); + + // calculate the offset from the centerOfEye to defaultHeadMat glm::mat4 defaultHeadOffset = glm::inverse(inputCalibration.defaultCenterEyeMat) * inputCalibration.defaultHeadMat; - glm::mat4 defaultRefrenceXform = hmdHeadMat * defaultHeadOffset; - - glm::quat tmpHmdHeadRotation = glmExtractRotation(hmdHeadMat); - /*qDebug() << "----------- hmd head position < ------"; - qDebug() << QString::fromStdString(glm::to_string(hmdHeadPosition)); - qDebug() << "<--------------- hmd head roatation <---------"; - qDebug() << QString::fromStdString(glm::to_string(canceledRollAndPitch));*/ - //calculate the offset from the centerEye to the head hoint - //glm::mat4 defaultHeadOffset = glm::inverse(inputCalibration.defaultCenterEyeMat) * inputCalibration.defaultHeadMat; - glm::vec3 tmpHeadOffsetPosition = extractTranslation(defaultHeadOffset); - glm::quat tmpHeadOffsetRotation = glmExtractRotation(defaultHeadOffset); - - /*qDebug() << "----------------> head offset position <------------------"; - qDebug() << QString::fromStdString(glm::to_string(tmpHeadOffsetPosition)); - qDebug() << "---------------> head offset rotation <-----------------"; - qDebug() << QString::fromStdString(glm::to_string(tmpHeadOffsetRotation));*/ + currentHead *= defaultHeadOffset; - glm::vec3 defaultRefrencePosition = extractTranslation(defaultRefrenceXform); - glm::quat defaultRefrenceRotation = glmExtractRotation(defaultRefrenceXform); - qDebug() << "----------------> defaultToReference position <------------------"; - qDebug() << QString::fromStdString(glm::to_string(defaultRefrencePosition)); - qDebug() << "---------------> defaultToReference rotation <-----------------"; - qDebug() << QString::fromStdString(glm::to_string(defaultRefrenceRotation)); + // calculate the defaultToRefrenceXform + glm::mat4 defaultRefrenceXform = currentHead * glm::inverse(inputCalibration.defaultHeadMat); auto puckCount = _validTrackedObjects.size(); if (puckCount == 2) { - //qDebug() << "-------------< configure feet <-------------"; _config = Config::Feet; } else if (puckCount == 3) { - //qDebug() << "-------------> configure feet and hips <-------------"; _config = Config::FeetAndHips; } else if (puckCount >= 4) { - //qDebug() << "-------------> configure feet, hips and chest <---------------"; _config = Config::FeetHipsAndChest; } else { - //qDebug() << "Could not configure # pucks " << puckCount; return; } @@ -287,24 +265,11 @@ void ViveControllerManager::InputDevice::calibrate(const controller::InputCalibr controller::Pose secondFootPose = secondFoot.second; if (firstFootPose.translation.x < secondFootPose.translation.x) { _jointToPuckMap[controller::LEFT_FOOT] = firstFoot.first; - - qDebug() << " --------> Printing out the offset pose <------------"; - glm::mat4 offset = computeOffset(defaultRefrenceXform, inputCalibration.defaultLeftFoot, firstFootPose); - qDebug() << "----------------> offset Position <------------------"; - qDebug() << QString::fromStdString(glm::to_string(extractTranslation(offset))); - qDebug() << "---------------> offset rotation <-----------------"; - qDebug() << QString::fromStdString(glm::to_string(glmExtractRotation(offset))); _pucksOffset[firstFoot.first] = computeOffset(defaultRefrenceXform, inputCalibration.defaultLeftFoot, firstFootPose); _jointToPuckMap[controller::RIGHT_FOOT] = secondFoot.first; _pucksOffset[secondFoot.first] = computeOffset(defaultRefrenceXform, inputCalibration.defaultRightFoot, secondFootPose); } else { _jointToPuckMap[controller::LEFT_FOOT] = secondFoot.first; - qDebug() << " --------> Printing out the offset pose <------------"; - glm::mat4 offset = computeOffset(defaultRefrenceXform, inputCalibration.defaultLeftFoot, secondFootPose); - qDebug() << "----------------> offset Position <------------------"; - qDebug() << QString::fromStdString(glm::to_string(extractTranslation(offset))); - qDebug() << "---------------> offset rotation <-----------------"; - qDebug() << QString::fromStdString(glm::to_string(glmExtractRotation(offset))); _pucksOffset[secondFoot.first] = computeOffset(defaultRefrenceXform, inputCalibration.defaultLeftFoot, secondFootPose); _jointToPuckMap[controller::RIGHT_FOOT] = firstFoot.first; _pucksOffset[firstFoot.first] = computeOffset(defaultRefrenceXform, inputCalibration.defaultRightFoot, firstFootPose); @@ -312,7 +277,6 @@ void ViveControllerManager::InputDevice::calibrate(const controller::InputCalibr if (_config == Config::Feet) { controller::Pose leftPose = addOffsetToPuckPose(controller::LEFT_FOOT); - qDebug() << "------------> InputDevice::updateCalibratedLimbs <---------"; printPose(leftPose); // done } else if (_config == Config::FeetAndHips) { @@ -324,12 +288,9 @@ void ViveControllerManager::InputDevice::calibrate(const controller::InputCalibr _jointToPuckMap[controller::SPINE2] = _validTrackedObjects[3].first; _pucksOffset[_validTrackedObjects[3].first] = computeOffset(defaultRefrenceXform, inputCalibration.defaultSpine2, _validTrackedObjects[3].second); } - qDebug() << "-------------------> end:::InputDevice::calibrate <--------------------"; _calibrated = true; } else { - qDebug() << "---- de-calibrated ---"; - } } @@ -337,9 +298,6 @@ void ViveControllerManager::InputDevice::calibrate(const controller::InputCalibr void ViveControllerManager::InputDevice::updateCalibratedLimbs() { _poseStateMap[controller::LEFT_FOOT] = addOffsetToPuckPose(controller::LEFT_FOOT); - controller::Pose leftPose = addOffsetToPuckPose(controller::LEFT_FOOT); - //qDebug() << "------------> InputDevice::updateCalibratedLimbs <---------"; - //printPose(leftPose); _poseStateMap[controller::RIGHT_FOOT] = addOffsetToPuckPose(controller::RIGHT_FOOT); _poseStateMap[controller::HIPS] = addOffsetToPuckPose(controller::HIPS); _poseStateMap[controller::SPINE2] = addOffsetToPuckPose(controller::SPINE2); @@ -352,11 +310,9 @@ controller::Pose ViveControllerManager::InputDevice::addOffsetToPuckPose(int joi controller::Pose puckPose = _poseStateMap[puckIndex]; glm::mat4 puckOffset = _pucksOffset[puckIndex]; puckPose.postTransform(puckOffset); - //qDebug() << "-----------> adding offset to puck <-------------- " << puckPose.valid; return puckPose; } - //qDebug() << "---------> joint is not mapped to any thing <--------------"; return controller::Pose(); } From a744db56aaebb7ad27c9a5ea697e69e8e22e9258 Mon Sep 17 00:00:00 2001 From: Dante Ruiz Date: Sat, 6 May 2017 00:21:52 +0100 Subject: [PATCH 07/15] removed dead code --- plugins/openvr/src/ViveControllerManager.cpp | 15 +-------------- 1 file changed, 1 insertion(+), 14 deletions(-) diff --git a/plugins/openvr/src/ViveControllerManager.cpp b/plugins/openvr/src/ViveControllerManager.cpp index fffdb7cd33..ff6edccf9f 100644 --- a/plugins/openvr/src/ViveControllerManager.cpp +++ b/plugins/openvr/src/ViveControllerManager.cpp @@ -48,29 +48,18 @@ const quint64 CALIBRATION_TIMELAPSE = 3000000; const char* ViveControllerManager::NAME { "OpenVR" }; -/*glm::mat4 computeDefualtHead(glm::mat4 centerOfEye, glm::mat4 jointHead) { - headOffset = glm::muliply(glm::inverse(centerOfEye), jointHead); - return glm:: - }*/ - glm::mat4 computeOffset(glm::mat4 defaultToRefrenceMat, glm::mat4 defaultJointMat, controller::Pose puckPose) { - ///qDebug() << "-------------> computing offset <-------------"; glm::mat4 poseMat = createMatFromQuatAndPos(puckPose.rotation, puckPose.translation); glm::mat4 refrenceJointMat = defaultToRefrenceMat * defaultJointMat; return ( glm::inverse(poseMat) * refrenceJointMat); } + bool sortPucksYPosition(std::pair firstPuck, std::pair secondPuck) { controller::Pose firstPose = firstPuck.second; controller::Pose secondPose = secondPuck.second; return (firstPose.translation.y < secondPose.translation.y); } -void printPose(controller::Pose pose) { - qDebug() << "-------------> printing out controller::Pose <--------------"; - qDebug() << QString::fromStdString(glm::to_string(pose.translation)); - qDebug() << QString::fromStdString(glm::to_string(pose.rotation)); -} - bool ViveControllerManager::isSupported() const { return openVrSupported(); } @@ -276,8 +265,6 @@ void ViveControllerManager::InputDevice::calibrate(const controller::InputCalibr } if (_config == Config::Feet) { - controller::Pose leftPose = addOffsetToPuckPose(controller::LEFT_FOOT); - printPose(leftPose); // done } else if (_config == Config::FeetAndHips) { _jointToPuckMap[controller::HIPS] = _validTrackedObjects[2].first; From 23c3a7511610b6d0c5bd388398f0796505077b62 Mon Sep 17 00:00:00 2001 From: Dante Ruiz Date: Tue, 9 May 2017 00:27:30 +0100 Subject: [PATCH 08/15] finally fixed rotation issue --- plugins/openvr/src/ViveControllerManager.cpp | 149 ++++++++++--------- plugins/openvr/src/ViveControllerManager.h | 2 +- 2 files changed, 79 insertions(+), 72 deletions(-) diff --git a/plugins/openvr/src/ViveControllerManager.cpp b/plugins/openvr/src/ViveControllerManager.cpp index ff6edccf9f..5a44a432ec 100644 --- a/plugins/openvr/src/ViveControllerManager.cpp +++ b/plugins/openvr/src/ViveControllerManager.cpp @@ -1,4 +1,3 @@ - // // Created by Sam Gondelman on 6/29/15. // Copyright 2013 High Fidelity, Inc. @@ -22,7 +21,6 @@ #include #include #include -#include #include @@ -48,10 +46,10 @@ const quint64 CALIBRATION_TIMELAPSE = 3000000; const char* ViveControllerManager::NAME { "OpenVR" }; -glm::mat4 computeOffset(glm::mat4 defaultToRefrenceMat, glm::mat4 defaultJointMat, controller::Pose puckPose) { +glm::mat4 computeOffset(glm::mat4 defaultToReferenceMat, glm::mat4 defaultJointMat, controller::Pose puckPose) { glm::mat4 poseMat = createMatFromQuatAndPos(puckPose.rotation, puckPose.translation); - glm::mat4 refrenceJointMat = defaultToRefrenceMat * defaultJointMat; - return ( glm::inverse(poseMat) * refrenceJointMat); + glm::mat4 referenceJointMat = defaultToReferenceMat * defaultJointMat; + return glm::inverse(poseMat) * referenceJointMat; } bool sortPucksYPosition(std::pair firstPuck, std::pair secondPuck) { @@ -130,7 +128,6 @@ void ViveControllerManager::pluginUpdate(float deltaTime, const controller::Inpu userInputMapper->removeDevice(_inputDevice->_deviceID); _registeredWithInputMapper = false; _inputDevice->_poseStateMap.clear(); - //qDebug() << " ----------->>!!!!!!! clear pose state map !!!!!!!<<----------------"; } if (!_registeredWithInputMapper && _inputDevice->_trackedControllers > 0) { @@ -213,78 +210,88 @@ void ViveControllerManager::InputDevice::handleTrackedObject(uint32_t deviceInde void ViveControllerManager::InputDevice::calibrate(const controller::InputCalibrationData& inputCalibration) { quint64 currentTime = usecTimestampNow(); - + auto leftTrigger = _buttonPressedMap.find(controller::LT); auto rightTrigger = _buttonPressedMap.find(controller::RT); if ((leftTrigger != _buttonPressedMap.end()) && (rightTrigger != _buttonPressedMap.end())) { - if (!_calibrated) { - // conver the hmd head from sensor space to avatar space - glm::mat4 worldToAvatarMat = glm::inverse(inputCalibration.avatarMat) * inputCalibration.sensorToWorldMat; - glm::mat4 hmdAvatarMat = worldToAvatarMat * inputCalibration.hmdSensorMat; + if (!_triggersPressedHandled) { + _triggersPressedHandled = true; + if (!_calibrated) { + // conver 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 currentHead = createMatFromQuatAndPos(hmdRotation, hmdTranslation); - - // calculate the offset from the centerOfEye to defaultHeadMat - glm::mat4 defaultHeadOffset = glm::inverse(inputCalibration.defaultCenterEyeMat) * inputCalibration.defaultHeadMat; + // cancel the roll and pitch for the hmd head + glm::quat hmdRotation = cancelOutRollAndPitch(glmExtractRotation(hmdAvatarMat)); + glm::vec3 hmdTranslation = extractTranslation(hmdAvatarMat); + glm::mat4 currentHead = createMatFromQuatAndPos(hmdRotation, hmdTranslation); + + // calculate the offset from the centerOfEye to defaultHeadMat + glm::mat4 defaultHeadOffset = glm::inverse(inputCalibration.defaultCenterEyeMat) * inputCalibration.defaultHeadMat; + + currentHead = currentHead * defaultHeadOffset; + + // calculate the defaultToRefrenceXform + glm::mat4 defaultReferenceXform = currentHead * glm::inverse(inputCalibration.defaultHeadMat); + + auto puckCount = _validTrackedObjects.size(); + if (puckCount == 2) { + _config = Config::Feet; + } else if (puckCount == 3) { + _config = Config::FeetAndHips; + } else if (puckCount >= 4) { + _config = Config::FeetHipsAndChest; + } else { + return; + } + + std::sort(_validTrackedObjects.begin(), _validTrackedObjects.end(), sortPucksYPosition); + + auto firstFoot = _validTrackedObjects[0]; + auto secondFoot = _validTrackedObjects[1]; + controller::Pose firstFootPose = firstFoot.second; + controller::Pose secondFootPose = secondFoot.second; + + if (firstFootPose.translation.x < secondFootPose.translation.x) { + _jointToPuckMap[controller::LEFT_FOOT] = firstFoot.first; + _pucksOffset[firstFoot.first] = computeOffset(defaultReferenceXform, inputCalibration.defaultLeftFoot, firstFootPose); + _jointToPuckMap[controller::RIGHT_FOOT] = secondFoot.first; + _pucksOffset[secondFoot.first] = computeOffset(defaultReferenceXform, inputCalibration.defaultRightFoot, secondFootPose); + + } else { + _jointToPuckMap[controller::LEFT_FOOT] = secondFoot.first; + _pucksOffset[secondFoot.first] = computeOffset(defaultReferenceXform, inputCalibration.defaultLeftFoot, secondFootPose); + _jointToPuckMap[controller::RIGHT_FOOT] = firstFoot.first; + _pucksOffset[firstFoot.first] = computeOffset(defaultReferenceXform, inputCalibration.defaultRightFoot, firstFootPose); + } + + if (_config == Config::Feet) { + // done + } else if (_config == Config::FeetAndHips) { + _jointToPuckMap[controller::HIPS] = _validTrackedObjects[2].first; + _pucksOffset[_validTrackedObjects[2].first] = computeOffset(defaultReferenceXform, inputCalibration.defaultHips, _validTrackedObjects[2].second); + } else if (_config == Config::FeetHipsAndChest) { + _jointToPuckMap[controller::HIPS] = _validTrackedObjects[2].first; + _pucksOffset[_validTrackedObjects[2].first] = computeOffset(defaultReferenceXform, inputCalibration.defaultHips, _validTrackedObjects[2].second); + _jointToPuckMap[controller::SPINE2] = _validTrackedObjects[3].first; + _pucksOffset[_validTrackedObjects[3].first] = computeOffset(defaultReferenceXform, inputCalibration.defaultSpine2, _validTrackedObjects[3].second); + } + _calibrated = true; - currentHead *= defaultHeadOffset; - - // calculate the defaultToRefrenceXform - glm::mat4 defaultRefrenceXform = currentHead * glm::inverse(inputCalibration.defaultHeadMat); - - auto puckCount = _validTrackedObjects.size(); - if (puckCount == 2) { - _config = Config::Feet; - } else if (puckCount == 3) { - _config = Config::FeetAndHips; - } else if (puckCount >= 4) { - _config = Config::FeetHipsAndChest; } else { - return; + _pucksOffset.clear(); + _jointToPuckMap.clear(); + _calibrated = false; } - - std::sort(_validTrackedObjects.begin(), _validTrackedObjects.end(), sortPucksYPosition); - - auto firstFoot = _validTrackedObjects[0]; - auto secondFoot = _validTrackedObjects[1]; - controller::Pose firstFootPose = firstFoot.second; - controller::Pose secondFootPose = secondFoot.second; - if (firstFootPose.translation.x < secondFootPose.translation.x) { - _jointToPuckMap[controller::LEFT_FOOT] = firstFoot.first; - _pucksOffset[firstFoot.first] = computeOffset(defaultRefrenceXform, inputCalibration.defaultLeftFoot, firstFootPose); - _jointToPuckMap[controller::RIGHT_FOOT] = secondFoot.first; - _pucksOffset[secondFoot.first] = computeOffset(defaultRefrenceXform, inputCalibration.defaultRightFoot, secondFootPose); - } else { - _jointToPuckMap[controller::LEFT_FOOT] = secondFoot.first; - _pucksOffset[secondFoot.first] = computeOffset(defaultRefrenceXform, inputCalibration.defaultLeftFoot, secondFootPose); - _jointToPuckMap[controller::RIGHT_FOOT] = firstFoot.first; - _pucksOffset[firstFoot.first] = computeOffset(defaultRefrenceXform, inputCalibration.defaultRightFoot, firstFootPose); - } - - if (_config == Config::Feet) { - // done - } else if (_config == Config::FeetAndHips) { - _jointToPuckMap[controller::HIPS] = _validTrackedObjects[2].first; - _pucksOffset[_validTrackedObjects[2].first] = computeOffset(defaultRefrenceXform, inputCalibration.defaultHips, _validTrackedObjects[2].second); - } else if (_config == Config::FeetHipsAndChest) { - _jointToPuckMap[controller::HIPS] = _validTrackedObjects[2].first; - _pucksOffset[_validTrackedObjects[2].first] = computeOffset(defaultRefrenceXform, inputCalibration.defaultHips, _validTrackedObjects[2].second); - _jointToPuckMap[controller::SPINE2] = _validTrackedObjects[3].first; - _pucksOffset[_validTrackedObjects[3].first] = computeOffset(defaultRefrenceXform, inputCalibration.defaultSpine2, _validTrackedObjects[3].second); - } - _calibrated = true; - - } else { } - + } else { + _triggersPressedHandled = false; } } void ViveControllerManager::InputDevice::updateCalibratedLimbs() { - _poseStateMap[controller::LEFT_FOOT] = addOffsetToPuckPose(controller::LEFT_FOOT); + _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); @@ -296,9 +303,9 @@ controller::Pose ViveControllerManager::InputDevice::addOffsetToPuckPose(int joi uint32_t puckIndex = puck->second; controller::Pose puckPose = _poseStateMap[puckIndex]; glm::mat4 puckOffset = _pucksOffset[puckIndex]; - puckPose.postTransform(puckOffset); - return puckPose; - + controller::Pose newPose = puckPose.postTransform(puckOffset); + return newPose; + } return controller::Pose(); } @@ -374,7 +381,7 @@ void ViveControllerManager::InputDevice::handleAxisEvent(float deltaTime, uint32 _axisStateMap[isLeftHand ? LY : RY] = stick.y; } else if (axis == vr::k_EButton_SteamVR_Trigger) { _axisStateMap[isLeftHand ? LT : RT] = x; - // The click feeling on the Vive controller trigger represents a value of *precisely* 1.0, + // The click feeling on the Vive controller trigger represents a value of *precisely* 1.0, // so we can expose that as an additional button if (x >= 1.0f) { _buttonPressedMap.insert(isLeftHand ? LT_CLICK : RT_CLICK); @@ -519,7 +526,7 @@ controller::Input::NamedVector ViveControllerManager::InputDevice::getAvailableI makePair(LEFT_FOOT, "LeftFoot"), makePair(RIGHT_FOOT, "RightFoot"), makePair(HIPS, "Hips"), - makePair(SPINE2, "Spine2"), + makePair(SPINE2, "Spine2"), // 16 tracked poses makePair(TRACKED_OBJECT_00, "TrackedObject00"), diff --git a/plugins/openvr/src/ViveControllerManager.h b/plugins/openvr/src/ViveControllerManager.h index 43dd982f80..1b0cf4213e 100644 --- a/plugins/openvr/src/ViveControllerManager.h +++ b/plugins/openvr/src/ViveControllerManager.h @@ -114,7 +114,7 @@ private: float _leftHapticDuration { 0.0f }; float _rightHapticStrength { 0.0f }; float _rightHapticDuration { 0.0f }; - bool _calibrate { false }; + bool _triggersPressedHandled { false }; bool _calibrated { false }; mutable std::recursive_mutex _lock; From 00546c55bb2d15ea7bf6ae34fafe1b5693e34b6f Mon Sep 17 00:00:00 2001 From: Dante Ruiz Date: Tue, 9 May 2017 00:30:06 +0100 Subject: [PATCH 09/15] minimize diff --- plugins/openvr/src/ViveControllerManager.cpp | 3 +++ 1 file changed, 3 insertions(+) diff --git a/plugins/openvr/src/ViveControllerManager.cpp b/plugins/openvr/src/ViveControllerManager.cpp index 5a44a432ec..00bf42b2d9 100644 --- a/plugins/openvr/src/ViveControllerManager.cpp +++ b/plugins/openvr/src/ViveControllerManager.cpp @@ -1,4 +1,7 @@ // +// ViveControllerManager.cpp +// input-plugins/src/input-plugins +// // Created by Sam Gondelman on 6/29/15. // Copyright 2013 High Fidelity, Inc. // From 03ba3f59441f8bee280b8525d5722e1a6fe91fdd Mon Sep 17 00:00:00 2001 From: Dante Ruiz Date: Tue, 9 May 2017 00:31:13 +0100 Subject: [PATCH 10/15] minimize diff 2.0 --- plugins/openvr/src/ViveControllerManager.h | 1 - 1 file changed, 1 deletion(-) diff --git a/plugins/openvr/src/ViveControllerManager.h b/plugins/openvr/src/ViveControllerManager.h index 1b0cf4213e..b9d7c3ea01 100644 --- a/plugins/openvr/src/ViveControllerManager.h +++ b/plugins/openvr/src/ViveControllerManager.h @@ -109,7 +109,6 @@ private: int _trackedControllers { 0 }; vr::IVRSystem*& _system; - quint64 _calibrateAfterTimelapse { 0.f }; float _leftHapticStrength { 0.0f }; float _leftHapticDuration { 0.0f }; float _rightHapticStrength { 0.0f }; From c3d57459d5f9bfeb9e4d518446bec08b0c586fd1 Mon Sep 17 00:00:00 2001 From: Dante Ruiz Date: Tue, 9 May 2017 00:32:47 +0100 Subject: [PATCH 11/15] minimize diff 3.0 --- plugins/openvr/src/ViveControllerManager.cpp | 2 -- 1 file changed, 2 deletions(-) diff --git a/plugins/openvr/src/ViveControllerManager.cpp b/plugins/openvr/src/ViveControllerManager.cpp index 00bf42b2d9..e22195b028 100644 --- a/plugins/openvr/src/ViveControllerManager.cpp +++ b/plugins/openvr/src/ViveControllerManager.cpp @@ -212,8 +212,6 @@ void ViveControllerManager::InputDevice::handleTrackedObject(uint32_t deviceInde } void ViveControllerManager::InputDevice::calibrate(const controller::InputCalibrationData& inputCalibration) { - quint64 currentTime = usecTimestampNow(); - auto leftTrigger = _buttonPressedMap.find(controller::LT); auto rightTrigger = _buttonPressedMap.find(controller::RT); if ((leftTrigger != _buttonPressedMap.end()) && (rightTrigger != _buttonPressedMap.end())) { From bce9e9ea82a92b5f66aff1b7df8b743ff947ebe9 Mon Sep 17 00:00:00 2001 From: Dante Ruiz Date: Tue, 9 May 2017 17:51:30 +0100 Subject: [PATCH 12/15] made requested chnages --- plugins/openvr/src/ViveControllerManager.cpp | 195 ++++++++++--------- plugins/openvr/src/ViveControllerManager.h | 5 +- 2 files changed, 110 insertions(+), 90 deletions(-) diff --git a/plugins/openvr/src/ViveControllerManager.cpp b/plugins/openvr/src/ViveControllerManager.cpp index e22195b028..ddf55bf670 100644 --- a/plugins/openvr/src/ViveControllerManager.cpp +++ b/plugins/openvr/src/ViveControllerManager.cpp @@ -45,20 +45,20 @@ 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"; -const quint64 CALIBRATION_TIMELAPSE = 3000000; +static const int MIN_PUCK_COUNT = 2; +static const int MIN_FEET_AND_HIPS = 3; +static const int MIN_FEET_HIPS_CHEST = 4; const char* ViveControllerManager::NAME { "OpenVR" }; -glm::mat4 computeOffset(glm::mat4 defaultToReferenceMat, glm::mat4 defaultJointMat, controller::Pose puckPose) { +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; } -bool sortPucksYPosition(std::pair firstPuck, std::pair secondPuck) { - controller::Pose firstPose = firstPuck.second; - controller::Pose secondPose = secondPuck.second; - return (firstPose.translation.y < secondPose.translation.y); +static bool sortPucksYPosition(std::pair firstPuck, std::pair secondPuck) { + return (firstPuck.second.translation.y < firstPuck.second.translation.y); } bool ViveControllerManager::isSupported() const { @@ -182,7 +182,16 @@ void ViveControllerManager::InputDevice::update(float deltaTime, const controlle numTrackedControllers++; } _trackedControllers = numTrackedControllers; - calibrate(inputCalibrationData); + + if (checkForCalibrationEvent()) { + if (!_triggersPressedHandled) { + _triggersPressedHandled = true; + calibrateOrUncalibrate(inputCalibrationData); + } + } else { + _triggersPressedHandled = false; + } + updateCalibratedLimbs(); } @@ -211,102 +220,110 @@ void ViveControllerManager::InputDevice::handleTrackedObject(uint32_t deviceInde } } -void ViveControllerManager::InputDevice::calibrate(const controller::InputCalibrationData& inputCalibration) { - auto leftTrigger = _buttonPressedMap.find(controller::LT); - auto rightTrigger = _buttonPressedMap.find(controller::RT); - if ((leftTrigger != _buttonPressedMap.end()) && (rightTrigger != _buttonPressedMap.end())) { - if (!_triggersPressedHandled) { - _triggersPressedHandled = true; - if (!_calibrated) { - // conver 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 currentHead = createMatFromQuatAndPos(hmdRotation, hmdTranslation); - - // calculate the offset from the centerOfEye to defaultHeadMat - glm::mat4 defaultHeadOffset = glm::inverse(inputCalibration.defaultCenterEyeMat) * inputCalibration.defaultHeadMat; - - currentHead = currentHead * defaultHeadOffset; - - // calculate the defaultToRefrenceXform - glm::mat4 defaultReferenceXform = currentHead * glm::inverse(inputCalibration.defaultHeadMat); - - auto puckCount = _validTrackedObjects.size(); - if (puckCount == 2) { - _config = Config::Feet; - } else if (puckCount == 3) { - _config = Config::FeetAndHips; - } else if (puckCount >= 4) { - _config = Config::FeetHipsAndChest; - } else { - return; - } - - std::sort(_validTrackedObjects.begin(), _validTrackedObjects.end(), sortPucksYPosition); - - auto firstFoot = _validTrackedObjects[0]; - auto secondFoot = _validTrackedObjects[1]; - controller::Pose firstFootPose = firstFoot.second; - controller::Pose secondFootPose = secondFoot.second; - - if (firstFootPose.translation.x < secondFootPose.translation.x) { - _jointToPuckMap[controller::LEFT_FOOT] = firstFoot.first; - _pucksOffset[firstFoot.first] = computeOffset(defaultReferenceXform, inputCalibration.defaultLeftFoot, firstFootPose); - _jointToPuckMap[controller::RIGHT_FOOT] = secondFoot.first; - _pucksOffset[secondFoot.first] = computeOffset(defaultReferenceXform, inputCalibration.defaultRightFoot, secondFootPose); - - } else { - _jointToPuckMap[controller::LEFT_FOOT] = secondFoot.first; - _pucksOffset[secondFoot.first] = computeOffset(defaultReferenceXform, inputCalibration.defaultLeftFoot, secondFootPose); - _jointToPuckMap[controller::RIGHT_FOOT] = firstFoot.first; - _pucksOffset[firstFoot.first] = computeOffset(defaultReferenceXform, inputCalibration.defaultRightFoot, firstFootPose); - } - - if (_config == Config::Feet) { - // done - } else if (_config == Config::FeetAndHips) { - _jointToPuckMap[controller::HIPS] = _validTrackedObjects[2].first; - _pucksOffset[_validTrackedObjects[2].first] = computeOffset(defaultReferenceXform, inputCalibration.defaultHips, _validTrackedObjects[2].second); - } else if (_config == Config::FeetHipsAndChest) { - _jointToPuckMap[controller::HIPS] = _validTrackedObjects[2].first; - _pucksOffset[_validTrackedObjects[2].first] = computeOffset(defaultReferenceXform, inputCalibration.defaultHips, _validTrackedObjects[2].second); - _jointToPuckMap[controller::SPINE2] = _validTrackedObjects[3].first; - _pucksOffset[_validTrackedObjects[3].first] = computeOffset(defaultReferenceXform, inputCalibration.defaultSpine2, _validTrackedObjects[3].second); - } - _calibrated = true; - - } else { - _pucksOffset.clear(); - _jointToPuckMap.clear(); - _calibrated = false; - } - } +void ViveControllerManager::InputDevice::calibrateOrUncalibrate(const controller::InputCalibrationData& inputCalibration) { + if (!_calibrated) { + calibrate(inputCalibration); } else { - _triggersPressedHandled = false; + uncalibrate(); } } +bool ViveControllerManager::InputDevice::checkForCalibrationEvent() { + auto& leftTrigger = _buttonPressedMap.find(controller::LT); + auto& rightTrigger = _buttonPressedMap.find(controller::RT); + return ((leftTrigger != _buttonPressedMap.end()) && (rightTrigger != _buttonPressedMap.end())); +} + +void ViveControllerManager::InputDevice::calibrate(const controller::InputCalibrationData& inputCalibration) { + // 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(); + if (puckCount == MIN_PUCK_COUNT) { + _config = Config::Feet; + } else if (puckCount == MIN_FEET_AND_HIPS) { + _config = Config::FeetAndHips; + } else if (puckCount >= MIN_FEET_HIPS_CHEST) { + _config = Config::FeetHipsAndChest; + } else { + return; + } + + std::sort(_validTrackedObjects.begin(), _validTrackedObjects.end(), sortPucksYPosition); + + int firstFootIndex = 0; + int secondFootIndex = 1; + + auto& firstFoot = _validTrackedObjects[firstFootIndex]; + auto& secondFoot = _validTrackedObjects[secondFootIndex]; + controller::Pose& firstFootPose = firstFoot.second; + controller::Pose& secondFootPose = secondFoot.second; + + if (firstFootPose.translation.x < secondFootPose.translation.x) { + _jointToPuckMap[controller::LEFT_FOOT] = firstFoot.first; + _pucksOffset[firstFoot.first] = computeOffset(defaultToReferenceMat, inputCalibration.defaultLeftFoot, firstFootPose); + _jointToPuckMap[controller::RIGHT_FOOT] = secondFoot.first; + _pucksOffset[secondFoot.first] = computeOffset(defaultToReferenceMat, inputCalibration.defaultRightFoot, secondFootPose); + + } else { + _jointToPuckMap[controller::LEFT_FOOT] = secondFoot.first; + _pucksOffset[secondFoot.first] = computeOffset(defaultToReferenceMat, inputCalibration.defaultLeftFoot, secondFootPose); + _jointToPuckMap[controller::RIGHT_FOOT] = firstFoot.first; + _pucksOffset[firstFoot.first] = computeOffset(defaultToReferenceMat, inputCalibration.defaultRightFoot, firstFootPose); + } + + if (_config == Config::Feet) { + // done + } else if (_config == Config::FeetAndHips) { + _jointToPuckMap[controller::HIPS] = _validTrackedObjects[2].first; + _pucksOffset[_validTrackedObjects[2].first] = computeOffset(defaultToReferenceMat, inputCalibration.defaultHips, _validTrackedObjects[2].second); + } else if (_config == Config::FeetHipsAndChest) { + _jointToPuckMap[controller::HIPS] = _validTrackedObjects[2].first; + _pucksOffset[_validTrackedObjects[2].first] = computeOffset(defaultToReferenceMat, inputCalibration.defaultHips, _validTrackedObjects[2].second); + _jointToPuckMap[controller::SPINE2] = _validTrackedObjects[3].first; + _pucksOffset[_validTrackedObjects[3].first] = computeOffset(defaultToReferenceMat, inputCalibration.defaultSpine2, _validTrackedObjects[3].second); + } + _calibrated = true; +} + +void ViveControllerManager::InputDevice::uncalibrate() { + _pucksOffset.clear(); + _jointToPuckMap.clear(); + _calibrated = false; +} + void ViveControllerManager::InputDevice::updateCalibratedLimbs() { - _poseStateMap[controller::LEFT_FOOT] = addOffsetToPuckPose(controller::LEFT_FOOT); + _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); } -controller::Pose ViveControllerManager::InputDevice::addOffsetToPuckPose(int joint) { +controller::Pose ViveControllerManager::InputDevice::addOffsetToPuckPose(int joint) const { auto puck = _jointToPuckMap.find(joint); if (puck != _jointToPuckMap.end()) { uint32_t puckIndex = puck->second; - controller::Pose puckPose = _poseStateMap[puckIndex]; - glm::mat4 puckOffset = _pucksOffset[puckIndex]; - controller::Pose newPose = puckPose.postTransform(puckOffset); - return newPose; + 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(); } diff --git a/plugins/openvr/src/ViveControllerManager.h b/plugins/openvr/src/ViveControllerManager.h index b9d7c3ea01..04e2d2b4aa 100644 --- a/plugins/openvr/src/ViveControllerManager.h +++ b/plugins/openvr/src/ViveControllerManager.h @@ -60,9 +60,12 @@ private: bool triggerHapticPulse(float strength, float duration, controller::Hand hand) override; void hapticsHelper(float deltaTime, bool leftHand); + void calibrateOrUncalibrate(const controller::InputCalibrationData& inputCalibration); void calibrate(const controller::InputCalibrationData& inputCalibration); - controller::Pose addOffsetToPuckPose(int joint); + void uncalibrate(); + controller::Pose addOffsetToPuckPose(int joint) const; void updateCalibratedLimbs(); + bool checkForCalibrationEvent(); void handleHandController(float deltaTime, uint32_t deviceIndex, const controller::InputCalibrationData& inputCalibrationData, bool isLeftHand); void handleTrackedObject(uint32_t deviceIndex, const controller::InputCalibrationData& inputCalibrationData); void handleButtonEvent(float deltaTime, uint32_t button, bool pressed, bool touched, bool isLeftHand); From d688b60282047afd7e56ea66b9f2c6f42b64d4d3 Mon Sep 17 00:00:00 2001 From: Dante Ruiz Date: Tue, 9 May 2017 18:02:12 +0100 Subject: [PATCH 13/15] no magic numbers --- plugins/openvr/src/ViveControllerManager.cpp | 23 +++++++++++--------- 1 file changed, 13 insertions(+), 10 deletions(-) diff --git a/plugins/openvr/src/ViveControllerManager.cpp b/plugins/openvr/src/ViveControllerManager.cpp index ddf55bf670..851d04497a 100644 --- a/plugins/openvr/src/ViveControllerManager.cpp +++ b/plugins/openvr/src/ViveControllerManager.cpp @@ -45,9 +45,13 @@ 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_PUCK_COUNT = 2; +static const int MIN_FEET_AND_HIPS = 3; +static const int MIN_FEET_HIPS_CHEST = 4; +static const int FIRST_FOOT = 0; +static const int SECOND_FOOT = 1; +static const int HIP = 2; +static const int CHEST = 3; const char* ViveControllerManager::NAME { "OpenVR" }; @@ -266,11 +270,10 @@ void ViveControllerManager::InputDevice::calibrate(const controller::InputCalibr std::sort(_validTrackedObjects.begin(), _validTrackedObjects.end(), sortPucksYPosition); - int firstFootIndex = 0; - int secondFootIndex = 1; - auto& firstFoot = _validTrackedObjects[firstFootIndex]; - auto& secondFoot = _validTrackedObjects[secondFootIndex]; + + auto& firstFoot = _validTrackedObjects[FIRST_FOOT]; + auto& secondFoot = _validTrackedObjects[SECOND_FOOT]; controller::Pose& firstFootPose = firstFoot.second; controller::Pose& secondFootPose = secondFoot.second; @@ -290,12 +293,12 @@ void ViveControllerManager::InputDevice::calibrate(const controller::InputCalibr if (_config == Config::Feet) { // done } else if (_config == Config::FeetAndHips) { - _jointToPuckMap[controller::HIPS] = _validTrackedObjects[2].first; + _jointToPuckMap[controller::HIPS] = _validTrackedObjects[HIP].first; _pucksOffset[_validTrackedObjects[2].first] = computeOffset(defaultToReferenceMat, inputCalibration.defaultHips, _validTrackedObjects[2].second); } else if (_config == Config::FeetHipsAndChest) { - _jointToPuckMap[controller::HIPS] = _validTrackedObjects[2].first; + _jointToPuckMap[controller::HIPS] = _validTrackedObjects[HIP].first; _pucksOffset[_validTrackedObjects[2].first] = computeOffset(defaultToReferenceMat, inputCalibration.defaultHips, _validTrackedObjects[2].second); - _jointToPuckMap[controller::SPINE2] = _validTrackedObjects[3].first; + _jointToPuckMap[controller::SPINE2] = _validTrackedObjects[CHEST].first; _pucksOffset[_validTrackedObjects[3].first] = computeOffset(defaultToReferenceMat, inputCalibration.defaultSpine2, _validTrackedObjects[3].second); } _calibrated = true; From c362ba91647cfbd7567112febca742ea4f8ffca2 Mon Sep 17 00:00:00 2001 From: Dante Ruiz Date: Tue, 9 May 2017 19:13:47 +0100 Subject: [PATCH 14/15] better calibration event --- plugins/openvr/src/ViveControllerManager.cpp | 25 ++++++++++++++------ plugins/openvr/src/ViveControllerManager.h | 2 ++ 2 files changed, 20 insertions(+), 7 deletions(-) diff --git a/plugins/openvr/src/ViveControllerManager.cpp b/plugins/openvr/src/ViveControllerManager.cpp index 851d04497a..8fedf926dc 100644 --- a/plugins/openvr/src/ViveControllerManager.cpp +++ b/plugins/openvr/src/ViveControllerManager.cpp @@ -21,6 +21,7 @@ #include #include #include +#include #include #include #include @@ -40,6 +41,7 @@ void releaseOpenVrSystem(); static const char* CONTROLLER_MODEL_STRING = "vr_controller_05_wireless_b"; +const quint64 CALIBRATION_TIMELAPSE = 3 * USECS_PER_SECOND; static const char* MENU_PARENT = "Avatar"; static const char* MENU_NAME = "Vive Controllers"; @@ -188,12 +190,19 @@ void ViveControllerManager::InputDevice::update(float deltaTime, const controlle _trackedControllers = numTrackedControllers; if (checkForCalibrationEvent()) { - if (!_triggersPressedHandled) { + 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(); @@ -232,12 +241,6 @@ void ViveControllerManager::InputDevice::calibrateOrUncalibrate(const controller } } -bool ViveControllerManager::InputDevice::checkForCalibrationEvent() { - auto& leftTrigger = _buttonPressedMap.find(controller::LT); - auto& rightTrigger = _buttonPressedMap.find(controller::RT); - return ((leftTrigger != _buttonPressedMap.end()) && (rightTrigger != _buttonPressedMap.end())); -} - void ViveControllerManager::InputDevice::calibrate(const controller::InputCalibrationData& inputCalibration) { // convert the hmd head from sensor space to avatar space glm::mat4 hmdSensorFlippedMat = inputCalibration.hmdSensorMat * Matrices::Y_180; @@ -416,6 +419,14 @@ enum ViveButtonChannel { RIGHT_APP_MENU }; +bool ViveControllerManager::InputDevice::checkForCalibrationEvent() { + auto& endOfMap = _buttonPressedMap.end(); + auto& leftTrigger = _buttonPressedMap.find(controller::LT); + auto& rightTrigger = _buttonPressedMap.find(controller::RT); + auto& leftAppButton = _buttonPressedMap.find(LEFT_APP_MENU); + auto& rightAppButton = _buttonPressedMap.find(RIGHT_APP_MENU); + return ((leftTrigger != endOfMap && leftAppButton != endOfMap) && (rightTrigger != endOfMap && rightAppButton != endOfMap)); +} // These functions do translation from the Steam IDs to the standard controller IDs void ViveControllerManager::InputDevice::handleButtonEvent(float deltaTime, uint32_t button, bool pressed, bool touched, bool isLeftHand) { diff --git a/plugins/openvr/src/ViveControllerManager.h b/plugins/openvr/src/ViveControllerManager.h index 04e2d2b4aa..9375fd20f0 100644 --- a/plugins/openvr/src/ViveControllerManager.h +++ b/plugins/openvr/src/ViveControllerManager.h @@ -112,12 +112,14 @@ private: int _trackedControllers { 0 }; vr::IVRSystem*& _system; + quint64 _timeTilCalibration { 0.0f }; float _leftHapticStrength { 0.0f }; float _leftHapticDuration { 0.0f }; float _rightHapticStrength { 0.0f }; float _rightHapticDuration { 0.0f }; bool _triggersPressedHandled { false }; bool _calibrated { false }; + bool _timeTilCalibrationSet { false }; mutable std::recursive_mutex _lock; friend class ViveControllerManager; From 53e254152da25fc43362715c03b41e466e9238cf Mon Sep 17 00:00:00 2001 From: Dante Ruiz Date: Tue, 9 May 2017 22:30:29 +0100 Subject: [PATCH 15/15] fixed puck location --- plugins/openvr/src/ViveControllerManager.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/plugins/openvr/src/ViveControllerManager.cpp b/plugins/openvr/src/ViveControllerManager.cpp index 8fedf926dc..db12d25e0d 100644 --- a/plugins/openvr/src/ViveControllerManager.cpp +++ b/plugins/openvr/src/ViveControllerManager.cpp @@ -41,7 +41,7 @@ void releaseOpenVrSystem(); static const char* CONTROLLER_MODEL_STRING = "vr_controller_05_wireless_b"; -const quint64 CALIBRATION_TIMELAPSE = 3 * USECS_PER_SECOND; +const quint64 CALIBRATION_TIMELAPSE = 2 * USECS_PER_SECOND; static const char* MENU_PARENT = "Avatar"; static const char* MENU_NAME = "Vive Controllers"; @@ -297,12 +297,12 @@ void ViveControllerManager::InputDevice::calibrate(const controller::InputCalibr // done } else if (_config == Config::FeetAndHips) { _jointToPuckMap[controller::HIPS] = _validTrackedObjects[HIP].first; - _pucksOffset[_validTrackedObjects[2].first] = computeOffset(defaultToReferenceMat, inputCalibration.defaultHips, _validTrackedObjects[2].second); + _pucksOffset[_validTrackedObjects[HIP].first] = computeOffset(defaultToReferenceMat, inputCalibration.defaultHips, _validTrackedObjects[HIP].second); } else if (_config == Config::FeetHipsAndChest) { _jointToPuckMap[controller::HIPS] = _validTrackedObjects[HIP].first; - _pucksOffset[_validTrackedObjects[2].first] = computeOffset(defaultToReferenceMat, inputCalibration.defaultHips, _validTrackedObjects[2].second); + _pucksOffset[_validTrackedObjects[HIP].first] = computeOffset(defaultToReferenceMat, inputCalibration.defaultHips, _validTrackedObjects[HIP].second); _jointToPuckMap[controller::SPINE2] = _validTrackedObjects[CHEST].first; - _pucksOffset[_validTrackedObjects[3].first] = computeOffset(defaultToReferenceMat, inputCalibration.defaultSpine2, _validTrackedObjects[3].second); + _pucksOffset[_validTrackedObjects[CHEST].first] = computeOffset(defaultToReferenceMat, inputCalibration.defaultSpine2, _validTrackedObjects[CHEST].second); } _calibrated = true; }