From bce9e9ea82a92b5f66aff1b7df8b743ff947ebe9 Mon Sep 17 00:00:00 2001 From: Dante Ruiz Date: Tue, 9 May 2017 17:51:30 +0100 Subject: [PATCH] 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);