From c0bf832e393655d7e455c0c5789e7889673b5a65 Mon Sep 17 00:00:00 2001
From: Anthony Thibault <tony@highfidelity.io>
Date: Sun, 2 Dec 2018 16:18:36 -0800
Subject: [PATCH] Head and Hand offsets for Vive Trackers now function as
 expected

In the Settings > Controller... > Calibration menu, when using head or hand trackers
The provided Y and Z offsets should function correctly now.  This gives you the ability to adjust
the offsets of the pucks to the hands/head.  For example: this allows you to wear the hand pucks on your forearms
rather then the backs of your palms.
---
 .../qml/hifi/tablet/OpenVrConfiguration.qml   | 35 ++++++++++++-------
 plugins/openvr/src/ViveControllerManager.cpp  | 32 +++++++----------
 2 files changed, 35 insertions(+), 32 deletions(-)

diff --git a/interface/resources/qml/hifi/tablet/OpenVrConfiguration.qml b/interface/resources/qml/hifi/tablet/OpenVrConfiguration.qml
index e18fdea444..46817e07cf 100644
--- a/interface/resources/qml/hifi/tablet/OpenVrConfiguration.qml
+++ b/interface/resources/qml/hifi/tablet/OpenVrConfiguration.qml
@@ -207,12 +207,12 @@ Flickable {
                     width: 112
                     label: "Y Offset"
                     suffix: " cm"
-                    minimumValue: -10
+                    minimumValue: -50
+                    maximumValue: 50
                     realStepSize: 1
-                    realValue: -5
                     colorScheme: hifi.colorSchemes.dark
 
-                    onEditingFinished: {
+                    onRealValueChanged: {
                         sendConfigurationSettings();
                         openVrConfiguration.forceActiveFocus();
                     }
@@ -223,14 +223,14 @@ Flickable {
                     id: headZOffset
                     width: 112
                     label: "Z Offset"
-                    minimumValue: -10
+                    minimumValue: -50
+                    maximumValue: 50
                     realStepSize: 1
                     decimals: 1
                     suffix: " cm"
-                    realValue: -5
                     colorScheme: hifi.colorSchemes.dark
 
-                    onEditingFinished: {
+                    onRealValueChanged: {
                         sendConfigurationSettings();
                         openVrConfiguration.forceActiveFocus();
                     }
@@ -319,11 +319,12 @@ Flickable {
                     width: 112
                     suffix: " cm"
                     label: "Y Offset"
-                    minimumValue: -10
+                    minimumValue: -30
+                    maximumValue: 30
                     realStepSize: 1
                     colorScheme: hifi.colorSchemes.dark
 
-                    onEditingFinished: {
+                    onRealValueChanged: {
                         sendConfigurationSettings();
                         openVrConfiguration.forceActiveFocus();
                     }
@@ -335,12 +336,13 @@ Flickable {
                     width: 112
                     label: "Z Offset"
                     suffix: " cm"
-                    minimumValue: -10
+                    minimumValue: -30
+                    maximumValue: 30
                     realStepSize: 1
                     decimals: 1
                     colorScheme: hifi.colorSchemes.dark
 
-                    onEditingFinished: {
+                    onRealValueChanged: {
                         sendConfigurationSettings();
                         openVrConfiguration.forceActiveFocus();
                     }
@@ -574,7 +576,7 @@ Flickable {
                     colorScheme: hifi.colorSchemes.dark
                     realValue: 33.0
 
-                    onEditingFinished: {
+                    onRealValueChanged: {
                         sendConfigurationSettings();
                         openVrConfiguration.forceActiveFocus();
                     }
@@ -592,7 +594,7 @@ Flickable {
                     colorScheme: hifi.colorSchemes.dark
                     realValue: 48
 
-                    onEditingFinished: {
+                    onRealValueChanged: {
                         sendConfigurationSettings();
                         openVrConfiguration.forceActiveFocus();
                     }
@@ -771,7 +773,7 @@ Flickable {
                 realStepSize: 1.0
                 colorScheme: hifi.colorSchemes.dark
 
-                onEditingFinished: {
+                onRealValueChanged: {
                     calibrationTimer.interval = realValue * 1000;
                     openVrConfiguration.countDown = realValue;
                     numberAnimation.duration = calibrationTimer.interval;
@@ -977,6 +979,13 @@ Flickable {
                 var configurationType = settings["trackerConfiguration"];
                 displayTrackerConfiguration(configurationType);
 
+                // default offset for user wearing puck on the center of their forehead.
+                headYOffset.realValue = 4; // (cm), puck is above the head joint.
+                headZOffset.realValue = 8; // (cm), puck is in front of the head joint.
+
+                // defaults for user wearing the pucks on the backs of their palms.
+                handYOffset.realValue = 8; // (cm), puck is past the the hand joint.  (set this to zero if puck is on the wrist)
+                handZOffset.realValue = -4; // (cm), puck is on above hand joint.
 
                 var HmdHead = settings["HMDHead"];
                 var viveController = settings["handController"];
diff --git a/plugins/openvr/src/ViveControllerManager.cpp b/plugins/openvr/src/ViveControllerManager.cpp
index af4f4da18c..283556f86a 100644
--- a/plugins/openvr/src/ViveControllerManager.cpp
+++ b/plugins/openvr/src/ViveControllerManager.cpp
@@ -892,7 +892,7 @@ glm::mat4 ViveControllerManager::InputDevice::calculateDefaultToReferenceForHead
     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 centerEyeTrans = headPuckPose.translation + centerEyeRotMat * -glm::vec3(0.0f, _headPuckYOffset, _headPuckZOffset);
 
     glm::mat4 E_s(glm::vec4(centerEyeRotMat[0], 0.0f),
                   glm::vec4(centerEyeRotMat[1], 0.0f),
@@ -1056,7 +1056,7 @@ void ViveControllerManager::InputDevice::hapticsHelper(float deltaTime, bool lef
         float hapticTime = strength * MAX_HAPTIC_TIME;
         if (hapticTime < duration * 1000.0f) {
             _system->TriggerHapticPulse(deviceIndex, 0, hapticTime);
-       }
+        }
 
         float remainingHapticTime = duration - (hapticTime / 1000.0f + deltaTime * 1000.0f); // in milliseconds
         if (leftHand) {
@@ -1077,23 +1077,20 @@ void ViveControllerManager::InputDevice::calibrateLeftHand(const glm::mat4& defa
     }
 
     // This allows the user to not have to match the t-pose exactly.  We assume that the y facing of the hand lies in the plane of the puck.
-    // Where the plane of the puck is defined by the the local z-axis of the puck, which is facing out of the vive logo/power button.
+    // Where the plane of the puck is defined by the the local z-axis of the puck, which is pointing out of the camera mount on the bottom of the puck.
     glm::vec3 zPrime = handPoseZAxis;
     glm::vec3 xPrime = glm::normalize(glm::cross(referenceHandYAxis, 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));
 
-    glm::vec3 translationOffset = glm::vec3(0.0f, _handPuckYOffset, _handPuckZOffset);
-    glm::quat initialRotation = handPose.getRotation();
-    glm::quat finalRotation = glmExtractRotation(newHandMat);
-
-    glm::quat rotationOffset = glm::inverse(initialRotation) * finalRotation;
-
-    glm::mat4 offsetMat = createMatFromQuatAndPos(rotationOffset, translationOffset);
+    glm::quat initialRot = handPose.getRotation();
+    glm::quat postOffsetRot = glm::inverse(initialRot) * glmExtractRotation(newHandMat);
+    glm::vec3 postOffsetTrans = postOffsetRot * -glm::vec3(0.0f, _handPuckYOffset, _handPuckZOffset);
+    glm::mat4 postOffsetMat = createMatFromQuatAndPos(postOffsetRot, postOffsetTrans);
 
     _jointToPuckMap[controller::LEFT_HAND] = handPair.first;
-    _pucksPostOffset[handPair.first] = offsetMat;
+    _pucksPostOffset[handPair.first] = postOffsetMat;
 }
 
 void ViveControllerManager::InputDevice::calibrateRightHand(const glm::mat4& defaultToReferenceMat, const controller::InputCalibrationData& inputCalibration, PuckPosePair& handPair) {
@@ -1113,16 +1110,13 @@ void ViveControllerManager::InputDevice::calibrateRightHand(const glm::mat4& def
     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 = handPose.getRotation();
-    glm::quat finalRotation = glmExtractRotation(newHandMat);
-
-    glm::quat rotationOffset = glm::inverse(initialRotation) * finalRotation;
-
-    glm::mat4 offsetMat = createMatFromQuatAndPos(rotationOffset, translationOffset);
+    glm::quat initialRot = handPose.getRotation();
+    glm::quat postOffsetRot = glm::inverse(initialRot) * glmExtractRotation(newHandMat);
+    glm::vec3 postOffsetTrans = postOffsetRot * -glm::vec3(0.0f, _handPuckYOffset, _handPuckZOffset);
+    glm::mat4 postOffsetMat = createMatFromQuatAndPos(postOffsetRot, postOffsetTrans);
 
     _jointToPuckMap[controller::RIGHT_HAND] = handPair.first;
-    _pucksPostOffset[handPair.first] = offsetMat;
+    _pucksPostOffset[handPair.first] = postOffsetMat;
 }