From 8c15d1bab900e349a8d76670181af8e3cd95e326 Mon Sep 17 00:00:00 2001 From: "Anthony J. Thibault" Date: Wed, 5 Jul 2017 15:50:47 -0700 Subject: [PATCH 1/4] delete-trailing-whitespace --- plugins/openvr/src/ViveControllerManager.cpp | 24 ++++++++++---------- plugins/openvr/src/ViveControllerManager.h | 2 +- 2 files changed, 13 insertions(+), 13 deletions(-) diff --git a/plugins/openvr/src/ViveControllerManager.cpp b/plugins/openvr/src/ViveControllerManager.cpp index b0e7297879..2b0a485a00 100644 --- a/plugins/openvr/src/ViveControllerManager.cpp +++ b/plugins/openvr/src/ViveControllerManager.cpp @@ -124,7 +124,7 @@ static glm::mat4 calculateResetMat() { float const UI_RADIUS = 1.0f; float const UI_HEIGHT = 1.6f; float const UI_Z_OFFSET = 0.5; - + float xSize, zSize; chaperone->GetPlayAreaSize(&xSize, &zSize); glm::vec3 uiPos(0.0f, UI_HEIGHT, UI_RADIUS - (0.5f * zSize) - UI_Z_OFFSET); @@ -251,7 +251,7 @@ void ViveControllerManager::pluginUpdate(float deltaTime, const controller::Inpu _resetMat = calculateResetMat(); _resetMatCalculated = true; } - + _system->GetDeviceToAbsoluteTrackingPose(vr::TrackingUniverseStanding, 0, _nextSimPoseData.vrPoses, vr::k_unMaxTrackedDeviceCount); _nextSimPoseData.update(_resetMat); } else if (isDesktopMode()) { @@ -429,7 +429,7 @@ void ViveControllerManager::InputDevice::emitCalibrationStatus() { status["hand_pucks"] = (_handConfig == HandConfig::Pucks); status["puckCount"] = (int)_validTrackedObjects.size(); status["UI"] = _calibrate; - + emit inputConfiguration->calibrationStatus(status); } @@ -480,7 +480,7 @@ void ViveControllerManager::InputDevice::sendUserActivityData(QString activity) {"head_puck", (_headConfig == HeadConfig::Puck) ? true : false}, {"hand_pucks", (_handConfig == HandConfig::Pucks) ? true : false} }; - + UserActivityLogger::getInstance().logAction(activity, jsonData); } @@ -513,12 +513,12 @@ void ViveControllerManager::InputDevice::calibrate(const controller::InputCalibr glm::mat4 defaultToReferenceMat = glm::mat4(); if (_headConfig == HeadConfig::HMD) { defaultToReferenceMat = calculateDefaultToReferenceForHmd(inputCalibration); - } else if (_headConfig == HeadConfig::Puck) { + } else if (_headConfig == HeadConfig::Puck) { defaultToReferenceMat = calculateDefaultToReferenceForHeadPuck(inputCalibration); } - + _config = _preferedConfig; - + bool headConfigured = configureHead(defaultToReferenceMat, inputCalibration); bool handsConfigured = configureHands(defaultToReferenceMat, inputCalibration); bool bodyConfigured = configureBody(defaultToReferenceMat, inputCalibration); @@ -952,12 +952,12 @@ void ViveControllerManager::InputDevice::calibrateLeftHand(glm::mat4& defaultToR glm::mat4 newHandMat = glm::mat4(glm::vec4(xPrime, 0.0f), glm::vec4(yPrime, 0.0f), glm::vec4(zPrime, 0.0f), glm::vec4(0.0f, 0.0f, 0.0f, 1.0f)); - + glm::vec3 translationOffset = glm::vec3(0.0f, _handPuckYOffset, _handPuckZOffset); glm::quat initialRotation = glmExtractRotation(handPoseAvatarMat); glm::quat finalRotation = glmExtractRotation(newHandMat); - + glm::quat rotationOffset = glm::inverse(initialRotation) * finalRotation; glm::mat4 offsetMat = createMatFromQuatAndPos(rotationOffset, translationOffset); @@ -982,13 +982,13 @@ void ViveControllerManager::InputDevice::calibrateRightHand(glm::mat4& defaultTo glm::vec3 yPrime = glm::normalize(glm::cross(zPrime, xPrime)); glm::mat4 newHandMat = glm::mat4(glm::vec4(xPrime, 0.0f), glm::vec4(yPrime, 0.0f), glm::vec4(zPrime, 0.0f), glm::vec4(0.0f, 0.0f, 0.0f, 1.0f)); - + glm::vec3 translationOffset = glm::vec3(0.0f, _handPuckYOffset, _handPuckZOffset); glm::quat initialRotation = glmExtractRotation(handPoseAvatarMat); glm::quat finalRotation = glmExtractRotation(newHandMat); - + glm::quat rotationOffset = glm::inverse(initialRotation) * finalRotation; glm::mat4 offsetMat = createMatFromQuatAndPos(rotationOffset, translationOffset); @@ -1005,7 +1005,7 @@ void ViveControllerManager::InputDevice::calibrateFeet(glm::mat4& defaultToRefer auto& secondFoot = _validTrackedObjects[SECOND_FOOT]; controller::Pose& firstFootPose = firstFoot.second; controller::Pose& secondFootPose = secondFoot.second; - + if (determineLimbOrdering(firstFootPose, secondFootPose, headXAxis, headPosition)) { calibrateFoot(defaultToReferenceMat, inputCalibration, firstFoot, true); calibrateFoot(defaultToReferenceMat, inputCalibration, secondFoot, false); diff --git a/plugins/openvr/src/ViveControllerManager.h b/plugins/openvr/src/ViveControllerManager.h index d94320dfb2..2c7aee5dad 100644 --- a/plugins/openvr/src/ViveControllerManager.h +++ b/plugins/openvr/src/ViveControllerManager.h @@ -152,7 +152,7 @@ private: HandController, Pucks }; - + Config _config { Config::None }; Config _preferedConfig { Config::None }; HeadConfig _headConfig { HeadConfig::HMD }; From dee9769be34eb4317083f06deca1350ada12d26c Mon Sep 17 00:00:00 2001 From: "Anthony J. Thibault" Date: Thu, 6 Jul 2017 10:50:47 -0700 Subject: [PATCH 2/4] Can now use chest, hips, shoulder & feet pucks in desktop mode. * Fixed bug where head puck would sometimes not work properly when other vive trackers were enabled. * Fixed calibration to work in sensor space instead of avatar space, this was necessary because the sensor to world matrix is unreliable when in desktop mode. --- plugins/openvr/src/ViveControllerManager.cpp | 102 +++++++++---------- plugins/openvr/src/ViveControllerManager.h | 4 +- 2 files changed, 53 insertions(+), 53 deletions(-) diff --git a/plugins/openvr/src/ViveControllerManager.cpp b/plugins/openvr/src/ViveControllerManager.cpp index 2b0a485a00..a531dfad4c 100644 --- a/plugins/openvr/src/ViveControllerManager.cpp +++ b/plugins/openvr/src/ViveControllerManager.cpp @@ -466,7 +466,9 @@ 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])); + + // but _validTrackedObjects remain in sensor frame + _validTrackedObjects.push_back(std::make_pair(poseIndex, pose)); } else { controller::Pose invalidPose; _poseStateMap[poseIndex] = invalidPose; @@ -514,6 +516,7 @@ void ViveControllerManager::InputDevice::calibrate(const controller::InputCalibr if (_headConfig == HeadConfig::HMD) { defaultToReferenceMat = calculateDefaultToReferenceForHmd(inputCalibration); } else if (_headConfig == HeadConfig::Puck) { + std::sort(_validTrackedObjects.begin(), _validTrackedObjects.end(), sortPucksYPosition); defaultToReferenceMat = calculateDefaultToReferenceForHeadPuck(inputCalibration); } @@ -708,63 +711,65 @@ void ViveControllerManager::InputDevice::handleHandController(float deltaTime, u } } } + +// defaultToReferenceMat is an offset from avatar space to sensor space. +// it aligns the default head in avatar space with the hmd in sensor space. +// +// * E_a is the the default center-eye transform in avatar space. +// * E_s is the the hmd center-eye transform in sensor space, with roll and pitch removed. +// * D is the defaultReferenceMat. +// +// E_s = D * E_a => +// D = E_s * inverse(E_a) +// glm::mat4 ViveControllerManager::InputDevice::calculateDefaultToReferenceForHmd(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); + // the center-eye transform in avatar space. + glm::mat4 E_a = inputCalibration.defaultCenterEyeMat; - // calculate the offset from the centerOfEye to defaultHeadMat - glm::mat4 defaultHeadOffset = glm::inverse(inputCalibration.defaultCenterEyeMat) * inputCalibration.defaultHeadMat; + // the center-eye transform in sensor space. + glm::mat4 E_s = inputCalibration.hmdSensorMat * Matrices::Y_180; // the Y_180 is to convert hmd from -z forward to z forward. - glm::mat4 currentHead = currentHmd * defaultHeadOffset; + // cancel out roll and pitch on E_s + glm::quat rot = cancelOutRollAndPitch(glmExtractRotation(E_s)); + glm::vec3 trans = extractTranslation(E_s); + E_s = createMatFromQuatAndPos(rot, trans); - // calculate the defaultToRefrenceXform - glm::mat4 defaultToReferenceMat = currentHead * glm::inverse(inputCalibration.defaultHeadMat); - - return defaultToReferenceMat; + return E_s * glm::inverse(E_a); } +// defaultToReferenceMat is an offset from avatar space to sensor space. +// it aligns the default head in avatar space with the head-puck in sensor space. +// +// * E_a is the the default center-eye transform in avatar space. +// * E_s is the the head-puck center-eye transform in sensor space, with roll and pitch removed. +// * D is the defaultReferenceMat. +// +// E_s = D * E_a => +// D = E_s * inverse(E_a) +// glm::mat4 ViveControllerManager::InputDevice::calculateDefaultToReferenceForHeadPuck(const controller::InputCalibrationData& inputCalibration) { - glm::mat4 avatarToSensorMat = glm::inverse(inputCalibration.sensorToWorldMat) * inputCalibration.avatarMat; - glm::mat4 sensorToAvatarMat = glm::inverse(inputCalibration.avatarMat) * inputCalibration.sensorToWorldMat; + + // the center-eye transform in avatar space. + glm::mat4 E_a = inputCalibration.defaultCenterEyeMat; + + // calculate the center-eye transform in sensor space, via the head-puck size_t headPuckIndex = _validTrackedObjects.size() - 1; controller::Pose headPuckPose = _validTrackedObjects[headPuckIndex].second; - glm::mat4 headPuckAvatarMat = createMatFromQuatAndPos(headPuckPose.getRotation(), headPuckPose.getTranslation()) * Matrices::Y_180; - glm::vec3 headPuckTranslation = extractTranslation(headPuckAvatarMat); - glm::vec3 headPuckZAxis = cancelOutRollAndPitch(glmExtractRotation(headPuckAvatarMat)) * glm::vec3(0.0f, 0.0f, 1.0f); - glm::vec3 worldUp = glm::vec3(0.0f, 1.0f, 0.0f); - // check that the head puck z axis is not parrallel to the world up - const float EPSILON = 1.0e-4f; - glm::vec3 zAxis = glmExtractRotation(headPuckAvatarMat) * glm::vec3(0.0f, 0.0f, 1.0f); - if (fabsf(fabsf(glm::dot(glm::normalize(worldUp), glm::normalize(zAxis))) - 1.0f) < EPSILON) { - headPuckZAxis = glm::vec3(1.0f, 0.0f, 0.0f); - } + // AJT: TODO: handle case were forward is parallel with UNIT_Y. + glm::vec3 forward = headPuckPose.rotation * -Vectors::UNIT_Z; + glm::vec3 x = glm::normalize(glm::cross(Vectors::UNIT_Y, forward)); + glm::vec3 z = glm::normalize(glm::cross(x, Vectors::UNIT_Y)); + glm::mat3 centerEyeRotMat(x, Vectors::UNIT_Y, z); + glm::vec3 centerEyeTrans = headPuckPose.translation + centerEyeRotMat * glm::vec3(0.0f, _headPuckYOffset, _headPuckZOffset); - glm::vec3 yPrime = glm::vec3(0.0f, 1.0f, 0.0f); - glm::vec3 xPrime = glm::normalize(glm::cross(worldUp, headPuckZAxis)); - glm::vec3 zPrime = glm::normalize(glm::cross(xPrime, yPrime)); - glm::mat4 newHeadPuck = glm::mat4(glm::vec4(xPrime, 0.0f), glm::vec4(yPrime, 0.0f), - glm::vec4(zPrime, 0.0f), glm::vec4(headPuckTranslation, 1.0f)); + glm::mat4 E_s(glm::vec4(centerEyeRotMat[0], 0.0f), + glm::vec4(centerEyeRotMat[1], 0.0f), + glm::vec4(centerEyeRotMat[2], 0.0f), + glm::vec4(centerEyeTrans, 1.0f)); - glm::mat4 headPuckOffset = glm::mat4(glm::vec4(1.0f, 0.0f, 0.0f, 0.0f), glm::vec4(0.0f, 1.0f, 0.0f, 0.0f), - glm::vec4(0.0f, 0.0f, 1.0f, 0.0f), glm::vec4(0.0f, _headPuckYOffset, _headPuckZOffset, 1.0f)); - - glm::mat4 finalHeadPuck = newHeadPuck * headPuckOffset; - - glm::mat4 defaultHeadOffset = glm::inverse(inputCalibration.defaultCenterEyeMat) * inputCalibration.defaultHeadMat; - - glm::mat4 currentHead = finalHeadPuck * defaultHeadOffset; - - // calculate the defaultToRefrenceXform - glm::mat4 defaultToReferenceMat = currentHead * glm::inverse(inputCalibration.defaultHeadMat); - return defaultToReferenceMat; + return E_s * glm::inverse(E_a); } void ViveControllerManager::InputDevice::partitionTouchpad(int sButton, int xAxis, int yAxis, int centerPseudoButton, int xPseudoButton, int yPseudoButton) { @@ -1070,13 +1075,8 @@ void ViveControllerManager::InputDevice::calibrateShoulders(glm::mat4& defaultTo void ViveControllerManager::InputDevice::calibrateHead(glm::mat4& defaultToReferenceMat, const controller::InputCalibrationData& inputCalibration) { 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); + _pucksOffset[head.first] = computeOffset(defaultToReferenceMat, inputCalibration.defaultHeadMat, head.second); } QString ViveControllerManager::InputDevice::configToString(Config config) { diff --git a/plugins/openvr/src/ViveControllerManager.h b/plugins/openvr/src/ViveControllerManager.h index 2c7aee5dad..a9bcc7e4e2 100644 --- a/plugins/openvr/src/ViveControllerManager.h +++ b/plugins/openvr/src/ViveControllerManager.h @@ -177,8 +177,8 @@ private: float _leftHapticDuration { 0.0f }; float _rightHapticStrength { 0.0f }; float _rightHapticDuration { 0.0f }; - float _headPuckYOffset { 0.0f }; - float _headPuckZOffset { 0.0f }; + float _headPuckYOffset { -0.05f }; + float _headPuckZOffset { -0.05f }; float _handPuckYOffset { 0.0f }; float _handPuckZOffset { 0.0f }; bool _triggersPressedHandled { false }; From 4e9b8c1ef94e7e1384f2fc0e697708f72d229855 Mon Sep 17 00:00:00 2001 From: "Anthony J. Thibault" Date: Thu, 6 Jul 2017 11:01:59 -0700 Subject: [PATCH 3/4] updated comments --- plugins/openvr/src/ViveControllerManager.cpp | 10 ++++++---- 1 file changed, 6 insertions(+), 4 deletions(-) diff --git a/plugins/openvr/src/ViveControllerManager.cpp b/plugins/openvr/src/ViveControllerManager.cpp index a531dfad4c..e505051536 100644 --- a/plugins/openvr/src/ViveControllerManager.cpp +++ b/plugins/openvr/src/ViveControllerManager.cpp @@ -713,10 +713,10 @@ void ViveControllerManager::InputDevice::handleHandController(float deltaTime, u } // defaultToReferenceMat is an offset from avatar space to sensor space. -// it aligns the default head in avatar space with the hmd in sensor space. +// it aligns the default center-eye in avatar space with the hmd in sensor space. // -// * E_a is the the default center-eye transform in avatar space. -// * E_s is the the hmd center-eye transform in sensor space, with roll and pitch removed. +// * E_a is the the default center-of-the-eyes transform in avatar space. +// * E_s is the the hmd eye-center transform in sensor space, with roll and pitch removed. // * D is the defaultReferenceMat. // // E_s = D * E_a => @@ -739,7 +739,9 @@ glm::mat4 ViveControllerManager::InputDevice::calculateDefaultToReferenceForHmd( } // defaultToReferenceMat is an offset from avatar space to sensor space. -// it aligns the default head in avatar space with the head-puck in sensor space. +// it aligns the default center-of-the-eyes transform in avatar space with the head-puck in sensor space. +// The offset from the center-of-the-eyes to the head-puck can be configured via _headPuckYOffset and _headPuckZOffset, +// these values are exposed in the configuration UI. // // * E_a is the the default center-eye transform in avatar space. // * E_s is the the head-puck center-eye transform in sensor space, with roll and pitch removed. From eb486c8f470344f2239e4818bc8ea353929d2370 Mon Sep 17 00:00:00 2001 From: "Anthony J. Thibault" Date: Thu, 6 Jul 2017 11:03:03 -0700 Subject: [PATCH 4/4] comments --- plugins/openvr/src/ViveControllerManager.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/plugins/openvr/src/ViveControllerManager.cpp b/plugins/openvr/src/ViveControllerManager.cpp index e505051536..07b3b2f73d 100644 --- a/plugins/openvr/src/ViveControllerManager.cpp +++ b/plugins/openvr/src/ViveControllerManager.cpp @@ -739,9 +739,9 @@ glm::mat4 ViveControllerManager::InputDevice::calculateDefaultToReferenceForHmd( } // defaultToReferenceMat is an offset from avatar space to sensor space. -// it aligns the default center-of-the-eyes transform in avatar space with the head-puck in sensor space. +// It aligns the default center-of-the-eyes transform in avatar space with the head-puck in sensor space. // The offset from the center-of-the-eyes to the head-puck can be configured via _headPuckYOffset and _headPuckZOffset, -// these values are exposed in the configuration UI. +// These values are exposed in the configuration UI. // // * E_a is the the default center-eye transform in avatar space. // * E_s is the the head-puck center-eye transform in sensor space, with roll and pitch removed.