From fa27bb7e11e89ed4b5487cf8cbbf622939221771 Mon Sep 17 00:00:00 2001
From: Anthony Thibault <tony@highfidelity.io>
Date: Mon, 24 Sep 2018 16:56:22 -0700
Subject: [PATCH] Bug fix for Vive puck calibration of hands and feet
Before this PR, there were small errors that can occur if you calibrate while not facing down the z-axis in vive sensor space.
As indicated by the small arrow in the steam vr grid.
The hands could end up flipped 180 degrees and the feet could end up with an incorrect offset resulting in a wider or narrower stance then expected.
This PR is intended to fix these issues.
---
plugins/openvr/src/ViveControllerManager.cpp | 33 +++++++++-----------
1 file changed, 15 insertions(+), 18 deletions(-)
diff --git a/plugins/openvr/src/ViveControllerManager.cpp b/plugins/openvr/src/ViveControllerManager.cpp
index 0deb4c75e7..f5b4411f51 100644
--- a/plugins/openvr/src/ViveControllerManager.cpp
+++ b/plugins/openvr/src/ViveControllerManager.cpp
@@ -568,6 +568,7 @@ void ViveControllerManager::InputDevice::calibrate(const controller::InputCalibr
return;
}
+ // Compute the defaultToRefrenceMat, this will take inputCalibration default poses into the reference frame. (sensor space)
glm::mat4 defaultToReferenceMat = glm::mat4();
if (_headConfig == HeadConfig::HMD) {
defaultToReferenceMat = calculateDefaultToReferenceForHmd(inputCalibration);
@@ -1025,17 +1026,15 @@ void ViveControllerManager::InputDevice::hapticsHelper(float deltaTime, bool lef
void ViveControllerManager::InputDevice::calibrateLeftHand(const glm::mat4& defaultToReferenceMat, const controller::InputCalibrationData& inputCalibration, PuckPosePair& handPair) {
controller::Pose& handPose = handPair.second;
- glm::mat4 handPoseAvatarMat = createMatFromQuatAndPos(handPose.getRotation(), handPose.getTranslation());
- glm::vec3 handPoseTranslation = extractTranslation(handPoseAvatarMat);
- glm::vec3 handPoseZAxis = glmExtractRotation(handPoseAvatarMat) * glm::vec3(0.0f, 0.0f, 1.0f);
- glm::vec3 avatarHandYAxis = transformVectorFast(inputCalibration.defaultLeftHand, glm::vec3(0.0f, 1.0f, 0.0f));
+ glm::vec3 handPoseZAxis = handPose.getRotation() * glm::vec3(0.0f, 0.0f, 1.0f);
+ glm::vec3 defaultHandYAxis = transformVectorFast(defaultToReferenceMat * inputCalibration.defaultLeftHand, glm::vec3(0.0f, 1.0f, 0.0f));
const float EPSILON = 1.0e-4f;
- if (fabsf(fabsf(glm::dot(glm::normalize(avatarHandYAxis), glm::normalize(handPoseZAxis))) - 1.0f) < EPSILON) {
+ if (fabsf(fabsf(glm::dot(glm::normalize(defaultHandYAxis), glm::normalize(handPoseZAxis))) - 1.0f) < EPSILON) {
handPoseZAxis = glm::vec3(0.0f, 0.0f, 1.0f);
}
glm::vec3 zPrime = handPoseZAxis;
- glm::vec3 xPrime = glm::normalize(glm::cross(avatarHandYAxis, handPoseZAxis));
+ glm::vec3 xPrime = glm::normalize(glm::cross(defaultHandYAxis, handPoseZAxis));
glm::vec3 yPrime = glm::normalize(glm::cross(zPrime, xPrime));
glm::mat4 newHandMat = glm::mat4(glm::vec4(xPrime, 0.0f), glm::vec4(yPrime, 0.0f),
@@ -1043,7 +1042,7 @@ void ViveControllerManager::InputDevice::calibrateLeftHand(const glm::mat4& defa
glm::vec3 translationOffset = glm::vec3(0.0f, _handPuckYOffset, _handPuckZOffset);
- glm::quat initialRotation = glmExtractRotation(handPoseAvatarMat);
+ glm::quat initialRotation = handPose.getRotation();
glm::quat finalRotation = glmExtractRotation(newHandMat);
glm::quat rotationOffset = glm::inverse(initialRotation) * finalRotation;
@@ -1056,17 +1055,15 @@ void ViveControllerManager::InputDevice::calibrateLeftHand(const glm::mat4& defa
void ViveControllerManager::InputDevice::calibrateRightHand(const glm::mat4& defaultToReferenceMat, const controller::InputCalibrationData& inputCalibration, PuckPosePair& handPair) {
controller::Pose& handPose = handPair.second;
- glm::mat4 handPoseAvatarMat = createMatFromQuatAndPos(handPose.getRotation(), handPose.getTranslation());
- glm::vec3 handPoseTranslation = extractTranslation(handPoseAvatarMat);
- glm::vec3 handPoseZAxis = glmExtractRotation(handPoseAvatarMat) * glm::vec3(0.0f, 0.0f, 1.0f);
- glm::vec3 avatarHandYAxis = transformVectorFast(inputCalibration.defaultRightHand, glm::vec3(0.0f, 1.0f, 0.0f));
+ glm::vec3 handPoseZAxis = handPose.getRotation() * glm::vec3(0.0f, 0.0f, 1.0f);
+ glm::vec3 defaultHandYAxis = transformVectorFast(defaultToReferenceMat * inputCalibration.defaultRightHand, glm::vec3(0.0f, 1.0f, 0.0f));
const float EPSILON = 1.0e-4f;
- if (fabsf(fabsf(glm::dot(glm::normalize(avatarHandYAxis), glm::normalize(handPoseZAxis))) - 1.0f) < EPSILON) {
+ if (fabsf(fabsf(glm::dot(glm::normalize(defaultHandYAxis), glm::normalize(handPoseZAxis))) - 1.0f) < EPSILON) {
handPoseZAxis = glm::vec3(0.0f, 0.0f, 1.0f);
}
glm::vec3 zPrime = handPoseZAxis;
- glm::vec3 xPrime = glm::normalize(glm::cross(avatarHandYAxis, handPoseZAxis));
+ glm::vec3 xPrime = glm::normalize(glm::cross(defaultHandYAxis, handPoseZAxis));
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));
@@ -1074,7 +1071,7 @@ void ViveControllerManager::InputDevice::calibrateRightHand(const glm::mat4& def
glm::vec3 translationOffset = glm::vec3(0.0f, _handPuckYOffset, _handPuckZOffset);
- glm::quat initialRotation = glmExtractRotation(handPoseAvatarMat);
+ glm::quat initialRotation = handPose.getRotation();
glm::quat finalRotation = glmExtractRotation(newHandMat);
glm::quat rotationOffset = glm::inverse(initialRotation) * finalRotation;
@@ -1105,15 +1102,15 @@ void ViveControllerManager::InputDevice::calibrateFeet(const glm::mat4& defaultT
void ViveControllerManager::InputDevice::calibrateFoot(const 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 puckPoseMat = 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::vec3 localXAxisInPuckFrame = glm::normalize(transformVectorFast(glm::inverse(puckPoseMat) * defaultToReferenceMat, glm::vec3(-1.0f, 0.0f, 0.0f)));
+ float distance = glm::dot(translationOffset, localXAxisInPuckFrame);
+ glm::vec3 finalTranslation = translationOffset - (distance * localXAxisInPuckFrame);
glm::mat4 finalOffset = createMatFromQuatAndPos(rotationOffset, finalTranslation);
if (isLeftFoot) {