From b48efc431789fe3ec55dfe8544c60930fc07f800 Mon Sep 17 00:00:00 2001 From: Dante Ruiz Date: Thu, 22 Jun 2017 22:48:32 +0100 Subject: [PATCH] fixed wide stance issue --- plugins/openvr/src/ViveControllerManager.cpp | 34 +++++++++++++++----- plugins/openvr/src/ViveControllerManager.h | 1 + 2 files changed, 27 insertions(+), 8 deletions(-) diff --git a/plugins/openvr/src/ViveControllerManager.cpp b/plugins/openvr/src/ViveControllerManager.cpp index d914cdcfad..648373ccc2 100644 --- a/plugins/openvr/src/ViveControllerManager.cpp +++ b/plugins/openvr/src/ViveControllerManager.cpp @@ -959,15 +959,33 @@ void ViveControllerManager::InputDevice::calibrateFeet(glm::mat4& defaultToRefer controller::Pose& secondFootPose = secondFoot.second; if (determineLimbOrdering(firstFootPose, secondFootPose, headXAxis, headPosition)) { - _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); + calibrateFoot(defaultToReferenceMat, inputCalibration, firstFoot, true); + calibrateFoot(defaultToReferenceMat, inputCalibration, secondFoot, false); } 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); + calibrateFoot(defaultToReferenceMat, inputCalibration, secondFoot, true); + calibrateFoot(defaultToReferenceMat, inputCalibration, firstFoot, false); + } +} + +void ViveControllerManager::InputDevice::calibrateFoot(glm::mat4& defaultToReferenceMat, const controller::InputCalibrationData& inputCalibration, PuckPosePair& footPair, bool isLeftFoot){ + controller::Pose footPose = footPair.second; + glm::mat4 puckPoseAvatarMat = createMatFromQuatAndPos(footPose.getRotation(), footPose.getTranslation()); + glm::mat4 defaultFoot = isLeftFoot ? inputCalibration.defaultLeftFoot : inputCalibration.defaultRightFoot; + glm::mat4 footOffset = computeOffset(defaultToReferenceMat, defaultFoot, footPose); + + glm::quat rotationOffset = glmExtractRotation(footOffset); + glm::vec3 translationOffset = extractTranslation(footOffset); + glm::vec3 avatarXAxisInPuckFrame = glm::normalize(transformVectorFast(glm::inverse(puckPoseAvatarMat), glm::vec3(-1.0f, 0.0f, 0.0f))); + float distance = glm::dot(translationOffset, avatarXAxisInPuckFrame); + glm::vec3 finalTranslation = translationOffset - (distance * avatarXAxisInPuckFrame); + glm::mat4 finalOffset = createMatFromQuatAndPos(rotationOffset, finalTranslation); + + if (isLeftFoot) { + _jointToPuckMap[controller::LEFT_FOOT] = footPair.first; + _pucksOffset[footPair.first] = finalOffset; + } else { + _jointToPuckMap[controller::RIGHT_FOOT] = footPair.first; + _pucksOffset[footPair.first] = finalOffset; } } diff --git a/plugins/openvr/src/ViveControllerManager.h b/plugins/openvr/src/ViveControllerManager.h index 67a9ff46fd..0b0406bb60 100644 --- a/plugins/openvr/src/ViveControllerManager.h +++ b/plugins/openvr/src/ViveControllerManager.h @@ -98,6 +98,7 @@ private: void calibrateLeftHand(glm::mat4& defaultToReferenceMat, const controller::InputCalibrationData& inputCalibration, PuckPosePair& handPair); void calibrateRightHand(glm::mat4& defaultToReferenceMat, const controller::InputCalibrationData& inputCalibration, PuckPosePair& handPair); void calibrateFeet(glm::mat4& defaultToReferenceMat, const controller::InputCalibrationData& inputCalibration); + void calibrateFoot(glm::mat4& defaultToReferenceMat, const controller::InputCalibrationData& inputCalibration, PuckPosePair& footPair, bool isLeftFoot); void calibrateHips(glm::mat4& defaultToReferenceMat, const controller::InputCalibrationData& inputCalibration); void calibrateChest(glm::mat4& defaultToReferenceMat, const controller::InputCalibrationData& inputCalibration); void calibrateShoulders(glm::mat4& defaultToReferenceMat, const controller::InputCalibrationData& inputCalibration,