diff --git a/interface/src/avatar/MyAvatar.cpp b/interface/src/avatar/MyAvatar.cpp index f7116a60db..1bec40c35a 100755 --- a/interface/src/avatar/MyAvatar.cpp +++ b/interface/src/avatar/MyAvatar.cpp @@ -3035,9 +3035,9 @@ glm::mat4 MyAvatar::getCenterEyeCalibrationMat() const { if (rightEyeIndex >= 0 && leftEyeIndex >= 0) { auto centerEyePos = (getAbsoluteDefaultJointTranslationInObjectFrame(rightEyeIndex) + getAbsoluteDefaultJointTranslationInObjectFrame(leftEyeIndex)) * 0.5f; auto centerEyeRot = Quaternions::Y_180; - return createMatFromQuatAndPos(centerEyeRot, centerEyePos); + return createMatFromQuatAndPos(centerEyeRot, centerEyePos / getSensorToWorldScale()); } else { - return createMatFromQuatAndPos(DEFAULT_AVATAR_MIDDLE_EYE_ROT, DEFAULT_AVATAR_MIDDLE_EYE_POS); + return createMatFromQuatAndPos(DEFAULT_AVATAR_MIDDLE_EYE_ROT, DEFAULT_AVATAR_MIDDLE_EYE_POS / getSensorToWorldScale()); } } @@ -3047,9 +3047,9 @@ glm::mat4 MyAvatar::getHeadCalibrationMat() const { if (headIndex >= 0) { auto headPos = getAbsoluteDefaultJointTranslationInObjectFrame(headIndex); auto headRot = getAbsoluteDefaultJointRotationInObjectFrame(headIndex); - return createMatFromQuatAndPos(headRot, headPos); + return createMatFromQuatAndPos(headRot, headPos / getSensorToWorldScale()); } else { - return createMatFromQuatAndPos(DEFAULT_AVATAR_HEAD_ROT, DEFAULT_AVATAR_HEAD_POS); + return createMatFromQuatAndPos(DEFAULT_AVATAR_HEAD_ROT, DEFAULT_AVATAR_HEAD_POS / getSensorToWorldScale()); } } @@ -3059,9 +3059,9 @@ glm::mat4 MyAvatar::getSpine2CalibrationMat() const { if (spine2Index >= 0) { auto spine2Pos = getAbsoluteDefaultJointTranslationInObjectFrame(spine2Index); auto spine2Rot = getAbsoluteDefaultJointRotationInObjectFrame(spine2Index); - return createMatFromQuatAndPos(spine2Rot, spine2Pos); + return createMatFromQuatAndPos(spine2Rot, spine2Pos / getSensorToWorldScale()); } else { - return createMatFromQuatAndPos(DEFAULT_AVATAR_SPINE2_ROT, DEFAULT_AVATAR_SPINE2_POS); + return createMatFromQuatAndPos(DEFAULT_AVATAR_SPINE2_ROT, DEFAULT_AVATAR_SPINE2_POS / getSensorToWorldScale()); } } @@ -3071,9 +3071,9 @@ glm::mat4 MyAvatar::getHipsCalibrationMat() const { if (hipsIndex >= 0) { auto hipsPos = getAbsoluteDefaultJointTranslationInObjectFrame(hipsIndex); auto hipsRot = getAbsoluteDefaultJointRotationInObjectFrame(hipsIndex); - return createMatFromQuatAndPos(hipsRot, hipsPos); + return createMatFromQuatAndPos(hipsRot, hipsPos / getSensorToWorldScale()); } else { - return createMatFromQuatAndPos(DEFAULT_AVATAR_HIPS_ROT, DEFAULT_AVATAR_HIPS_POS); + return createMatFromQuatAndPos(DEFAULT_AVATAR_HIPS_ROT, DEFAULT_AVATAR_HIPS_POS / getSensorToWorldScale()); } } @@ -3083,9 +3083,9 @@ glm::mat4 MyAvatar::getLeftFootCalibrationMat() const { if (leftFootIndex >= 0) { auto leftFootPos = getAbsoluteDefaultJointTranslationInObjectFrame(leftFootIndex); auto leftFootRot = getAbsoluteDefaultJointRotationInObjectFrame(leftFootIndex); - return createMatFromQuatAndPos(leftFootRot, leftFootPos); + return createMatFromQuatAndPos(leftFootRot, leftFootPos / getSensorToWorldScale()); } else { - return createMatFromQuatAndPos(DEFAULT_AVATAR_LEFTFOOT_ROT, DEFAULT_AVATAR_LEFTFOOT_POS); + return createMatFromQuatAndPos(DEFAULT_AVATAR_LEFTFOOT_ROT, DEFAULT_AVATAR_LEFTFOOT_POS / getSensorToWorldScale()); } } @@ -3095,9 +3095,9 @@ glm::mat4 MyAvatar::getRightFootCalibrationMat() const { if (rightFootIndex >= 0) { auto rightFootPos = getAbsoluteDefaultJointTranslationInObjectFrame(rightFootIndex); auto rightFootRot = getAbsoluteDefaultJointRotationInObjectFrame(rightFootIndex); - return createMatFromQuatAndPos(rightFootRot, rightFootPos); + return createMatFromQuatAndPos(rightFootRot, rightFootPos / getSensorToWorldScale()); } else { - return createMatFromQuatAndPos(DEFAULT_AVATAR_RIGHTFOOT_ROT, DEFAULT_AVATAR_RIGHTFOOT_POS); + return createMatFromQuatAndPos(DEFAULT_AVATAR_RIGHTFOOT_ROT, DEFAULT_AVATAR_RIGHTFOOT_POS / getSensorToWorldScale()); } } @@ -3107,9 +3107,9 @@ glm::mat4 MyAvatar::getRightArmCalibrationMat() const { if (rightArmIndex >= 0) { auto rightArmPos = getAbsoluteDefaultJointTranslationInObjectFrame(rightArmIndex); auto rightArmRot = getAbsoluteDefaultJointRotationInObjectFrame(rightArmIndex); - return createMatFromQuatAndPos(rightArmRot, rightArmPos); + return createMatFromQuatAndPos(rightArmRot, rightArmPos / getSensorToWorldScale()); } else { - return createMatFromQuatAndPos(DEFAULT_AVATAR_RIGHTARM_ROT, DEFAULT_AVATAR_RIGHTARM_POS); + return createMatFromQuatAndPos(DEFAULT_AVATAR_RIGHTARM_ROT, DEFAULT_AVATAR_RIGHTARM_POS / getSensorToWorldScale()); } } @@ -3118,9 +3118,9 @@ glm::mat4 MyAvatar::getLeftArmCalibrationMat() const { if (leftArmIndex >= 0) { auto leftArmPos = getAbsoluteDefaultJointTranslationInObjectFrame(leftArmIndex); auto leftArmRot = getAbsoluteDefaultJointRotationInObjectFrame(leftArmIndex); - return createMatFromQuatAndPos(leftArmRot, leftArmPos); + return createMatFromQuatAndPos(leftArmRot, leftArmPos / getSensorToWorldScale()); } else { - return createMatFromQuatAndPos(DEFAULT_AVATAR_LEFTARM_ROT, DEFAULT_AVATAR_LEFTARM_POS); + return createMatFromQuatAndPos(DEFAULT_AVATAR_LEFTARM_ROT, DEFAULT_AVATAR_LEFTARM_POS / getSensorToWorldScale()); } } @@ -3129,9 +3129,9 @@ glm::mat4 MyAvatar::getRightHandCalibrationMat() const { if (rightHandIndex >= 0) { auto rightHandPos = getAbsoluteDefaultJointTranslationInObjectFrame(rightHandIndex); auto rightHandRot = getAbsoluteDefaultJointRotationInObjectFrame(rightHandIndex); - return createMatFromQuatAndPos(rightHandRot, rightHandPos); + return createMatFromQuatAndPos(rightHandRot, rightHandPos / getSensorToWorldScale()); } else { - return createMatFromQuatAndPos(DEFAULT_AVATAR_RIGHTHAND_ROT, DEFAULT_AVATAR_RIGHTHAND_POS); + return createMatFromQuatAndPos(DEFAULT_AVATAR_RIGHTHAND_ROT, DEFAULT_AVATAR_RIGHTHAND_POS / getSensorToWorldScale()); } } @@ -3140,9 +3140,9 @@ glm::mat4 MyAvatar::getLeftHandCalibrationMat() const { if (leftHandIndex >= 0) { auto leftHandPos = getAbsoluteDefaultJointTranslationInObjectFrame(leftHandIndex); auto leftHandRot = getAbsoluteDefaultJointRotationInObjectFrame(leftHandIndex); - return createMatFromQuatAndPos(leftHandRot, leftHandPos); + return createMatFromQuatAndPos(leftHandRot, leftHandPos / getSensorToWorldScale()); } else { - return createMatFromQuatAndPos(DEFAULT_AVATAR_LEFTHAND_ROT, DEFAULT_AVATAR_LEFTHAND_POS); + return createMatFromQuatAndPos(DEFAULT_AVATAR_LEFTHAND_ROT, DEFAULT_AVATAR_LEFTHAND_POS / getSensorToWorldScale()); } } diff --git a/libraries/shared/src/GLMHelpers.cpp b/libraries/shared/src/GLMHelpers.cpp index 394517aac4..39fec45d90 100644 --- a/libraries/shared/src/GLMHelpers.cpp +++ b/libraries/shared/src/GLMHelpers.cpp @@ -473,6 +473,14 @@ glm::mat4 createMatFromScaleQuatAndPos(const glm::vec3& scale, const glm::quat& glm::vec4(zAxis, 0.0f), glm::vec4(trans, 1.0f)); } +glm::mat4 createMatFromScale(const glm::vec3& scale) { + glm::vec3 xAxis = glm::vec3(scale.x, 0.0f, 0.0f); + glm::vec3 yAxis = glm::vec3(0.0f, scale.y, 0.0f); + glm::vec3 zAxis = glm::vec3(0.0f, 0.0f, scale.z); + return glm::mat4(glm::vec4(xAxis, 0.0f), glm::vec4(yAxis, 0.0f), + glm::vec4(zAxis, 0.0f), glm::vec4(Vectors::ZERO, 1.0f)); +} + // cancel out roll glm::quat cancelOutRoll(const glm::quat& q) { glm::vec3 forward = q * Vectors::FRONT; diff --git a/libraries/shared/src/GLMHelpers.h b/libraries/shared/src/GLMHelpers.h index 3386ea2c22..7248f4cb46 100644 --- a/libraries/shared/src/GLMHelpers.h +++ b/libraries/shared/src/GLMHelpers.h @@ -231,6 +231,7 @@ glm::tvec4 lerp(const glm::tvec4& x, const glm::tvec4& y, T a) glm::mat4 createMatFromQuatAndPos(const glm::quat& q, const glm::vec3& p); glm::mat4 createMatFromScaleQuatAndPos(const glm::vec3& scale, const glm::quat& rot, const glm::vec3& trans); +glm::mat4 createMatFromScale(const glm::vec3& scale); glm::quat cancelOutRoll(const glm::quat& q); glm::quat cancelOutRollAndPitch(const glm::quat& q); glm::mat4 cancelOutRollAndPitch(const glm::mat4& m); diff --git a/plugins/openvr/src/ViveControllerManager.cpp b/plugins/openvr/src/ViveControllerManager.cpp index 5a1c23839e..81173722fb 100644 --- a/plugins/openvr/src/ViveControllerManager.cpp +++ b/plugins/openvr/src/ViveControllerManager.cpp @@ -375,7 +375,7 @@ void ViveControllerManager::InputDevice::update(float deltaTime, const controlle calibrateFromHandController(inputCalibrationData); calibrateFromUI(inputCalibrationData); - updateCalibratedLimbs(); + updateCalibratedLimbs(inputCalibrationData); _lastSimPoseData = _nextSimPoseData; } @@ -676,40 +676,55 @@ void ViveControllerManager::InputDevice::uncalibrate() { _overrideHands = false; } -void ViveControllerManager::InputDevice::updateCalibratedLimbs() { - _poseStateMap[controller::LEFT_FOOT] = addOffsetToPuckPose(controller::LEFT_FOOT); - _poseStateMap[controller::RIGHT_FOOT] = addOffsetToPuckPose(controller::RIGHT_FOOT); - _poseStateMap[controller::HIPS] = addOffsetToPuckPose(controller::HIPS); - _poseStateMap[controller::SPINE2] = addOffsetToPuckPose(controller::SPINE2); - _poseStateMap[controller::RIGHT_ARM] = addOffsetToPuckPose(controller::RIGHT_ARM); - _poseStateMap[controller::LEFT_ARM] = addOffsetToPuckPose(controller::LEFT_ARM); +void ViveControllerManager::InputDevice::updateCalibratedLimbs(const controller::InputCalibrationData& inputCalibration) { + _poseStateMap[controller::LEFT_FOOT] = addOffsetToPuckPose(inputCalibration, controller::LEFT_FOOT); + _poseStateMap[controller::RIGHT_FOOT] = addOffsetToPuckPose(inputCalibration, controller::RIGHT_FOOT); + _poseStateMap[controller::HIPS] = addOffsetToPuckPose(inputCalibration, controller::HIPS); + _poseStateMap[controller::SPINE2] = addOffsetToPuckPose(inputCalibration, controller::SPINE2); + _poseStateMap[controller::RIGHT_ARM] = addOffsetToPuckPose(inputCalibration, controller::RIGHT_ARM); + _poseStateMap[controller::LEFT_ARM] = addOffsetToPuckPose(inputCalibration, controller::LEFT_ARM); if (_overrideHead) { - _poseStateMap[controller::HEAD] = addOffsetToPuckPose(controller::HEAD); + _poseStateMap[controller::HEAD] = addOffsetToPuckPose(inputCalibration, controller::HEAD); } if (_overrideHands) { - _poseStateMap[controller::LEFT_HAND] = addOffsetToPuckPose(controller::LEFT_HAND); - _poseStateMap[controller::RIGHT_HAND] = addOffsetToPuckPose(controller::RIGHT_HAND); + _poseStateMap[controller::LEFT_HAND] = addOffsetToPuckPose(inputCalibration, controller::LEFT_HAND); + _poseStateMap[controller::RIGHT_HAND] = addOffsetToPuckPose(inputCalibration, controller::RIGHT_HAND); } } -controller::Pose ViveControllerManager::InputDevice::addOffsetToPuckPose(int joint) const { +controller::Pose ViveControllerManager::InputDevice::addOffsetToPuckPose(const controller::InputCalibrationData& inputCalibration, int joint) const { auto puck = _jointToPuckMap.find(joint); if (puck != _jointToPuckMap.end()) { uint32_t puckIndex = puck->second; - auto puckPose = _poseStateMap.find(puckIndex); - auto puckPostOffset = _pucksPostOffset.find(puckIndex); - auto puckPreOffset = _pucksPreOffset.find(puckIndex); - if (puckPose != _poseStateMap.end()) { - if (puckPreOffset != _pucksPreOffset.end() && puckPostOffset != _pucksPostOffset.end()) { - return puckPose->second.postTransform(puckPostOffset->second).transform(puckPreOffset->second); - } else if (puckPostOffset != _pucksPostOffset.end()) { - return puckPose->second.postTransform(puckPostOffset->second); - } else if (puckPreOffset != _pucksPreOffset.end()) { - return puckPose->second.transform(puckPreOffset->second); + // use sensor space pose. + auto puckPoseIter = _validTrackedObjects.begin(); + while (puckPoseIter != _validTrackedObjects.end()) { + if (puckPoseIter->first == puckIndex) { + break; } + puckPoseIter++; + } + + //auto puckPoseIter = _poseStateMap.find(puckIndex); + + if (puckPoseIter != _validTrackedObjects.end()) { + + glm::mat4 postMat; // identity + auto postIter = _pucksPostOffset.find(puckIndex); + if (postIter != _pucksPostOffset.end()) { + postMat = postIter->second; + } + + glm::mat4 preMat = glm::inverse(inputCalibration.avatarMat) * inputCalibration.sensorToWorldMat; + auto preIter = _pucksPreOffset.find(puckIndex); + if (preIter != _pucksPreOffset.end()) { + preMat = preMat * preIter->second; + } + + return puckPoseIter->second.postTransform(postMat).transform(preMat); } } return controller::Pose(); @@ -924,15 +939,12 @@ void ViveControllerManager::InputDevice::handleButtonEvent(float deltaTime, uint void ViveControllerManager::InputDevice::handleHeadPoseEvent(const controller::InputCalibrationData& inputCalibrationData, const mat4& mat, const vec3& linearVelocity, const vec3& angularVelocity) { - //perform a 180 flip to make the HMD face the +z instead of -z, beacuse the head faces +z glm::mat4 matYFlip = mat * Matrices::Y_180; controller::Pose pose(extractTranslation(matYFlip), glmExtractRotation(matYFlip), linearVelocity, angularVelocity); - - glm::mat4 sensorToAvatar = glm::inverse(inputCalibrationData.avatarMat) * inputCalibrationData.sensorToWorldMat; glm::mat4 defaultHeadOffset = glm::inverse(inputCalibrationData.defaultCenterEyeMat) * inputCalibrationData.defaultHeadMat; - controller::Pose hmdHeadPose = pose.transform(sensorToAvatar); - _poseStateMap[controller::HEAD] = hmdHeadPose.postTransform(defaultHeadOffset); + glm::mat4 sensorToAvatar = glm::inverse(inputCalibrationData.avatarMat) * inputCalibrationData.sensorToWorldMat; + _poseStateMap[controller::HEAD] = pose.postTransform(defaultHeadOffset).transform(sensorToAvatar); } void ViveControllerManager::InputDevice::handlePoseEvent(float deltaTime, const controller::InputCalibrationData& inputCalibrationData, diff --git a/plugins/openvr/src/ViveControllerManager.h b/plugins/openvr/src/ViveControllerManager.h index 9a7b2cbc93..4a7fcaf85e 100644 --- a/plugins/openvr/src/ViveControllerManager.h +++ b/plugins/openvr/src/ViveControllerManager.h @@ -79,10 +79,10 @@ private: void sendUserActivityData(QString activity); void configureCalibrationSettings(const QJsonObject configurationSettings); QJsonObject configurationSettings(); - controller::Pose addOffsetToPuckPose(int joint) const; + controller::Pose addOffsetToPuckPose(const controller::InputCalibrationData& inputCalibration, int joint) const; glm::mat4 calculateDefaultToReferenceForHeadPuck(const controller::InputCalibrationData& inputCalibration); glm::mat4 calculateDefaultToReferenceForHmd(const controller::InputCalibrationData& inputCalibration); - void updateCalibratedLimbs(); + void updateCalibratedLimbs(const controller::InputCalibrationData& inputCalibration); bool checkForCalibrationEvent(); void handleHandController(float deltaTime, uint32_t deviceIndex, const controller::InputCalibrationData& inputCalibrationData, bool isLeftHand); void handleHmd(uint32_t deviceIndex, const controller::InputCalibrationData& inputCalibrationData);