Fixes for Vive puck calibration

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());
     }
 }
 

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;

libraries/shared/src/GLMHelpers.h

@@ -231,6 +231,7 @@ glm::tvec4<T, P> lerp(const glm::tvec4<T, P>& x, const glm::tvec4<T, P>& 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);

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() {
+void ViveControllerManager::InputDevice::updateCalibratedLimbs(const controller::InputCalibrationData& inputCalibration) {
-    _poseStateMap[controller::LEFT_FOOT] = addOffsetToPuckPose(controller::LEFT_FOOT);
+    _poseStateMap[controller::LEFT_FOOT] = addOffsetToPuckPose(inputCalibration, controller::LEFT_FOOT);
-    _poseStateMap[controller::RIGHT_FOOT] = addOffsetToPuckPose(controller::RIGHT_FOOT);
+    _poseStateMap[controller::RIGHT_FOOT] = addOffsetToPuckPose(inputCalibration, controller::RIGHT_FOOT);
-    _poseStateMap[controller::HIPS] = addOffsetToPuckPose(controller::HIPS);
+    _poseStateMap[controller::HIPS] = addOffsetToPuckPose(inputCalibration, controller::HIPS);
-    _poseStateMap[controller::SPINE2] = addOffsetToPuckPose(controller::SPINE2);
+    _poseStateMap[controller::SPINE2] = addOffsetToPuckPose(inputCalibration, controller::SPINE2);
-    _poseStateMap[controller::RIGHT_ARM] = addOffsetToPuckPose(controller::RIGHT_ARM);
+    _poseStateMap[controller::RIGHT_ARM] = addOffsetToPuckPose(inputCalibration, controller::RIGHT_ARM);
-    _poseStateMap[controller::LEFT_ARM] = addOffsetToPuckPose(controller::LEFT_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::LEFT_HAND] = addOffsetToPuckPose(inputCalibration, controller::LEFT_HAND);
-        _poseStateMap[controller::RIGHT_HAND] = addOffsetToPuckPose(controller::RIGHT_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()) {
+        // use sensor space pose.
-            if (puckPreOffset != _pucksPreOffset.end() && puckPostOffset != _pucksPostOffset.end()) {
+        auto puckPoseIter = _validTrackedObjects.begin();
-                return puckPose->second.postTransform(puckPostOffset->second).transform(puckPreOffset->second);
+        while (puckPoseIter != _validTrackedObjects.end()) {
-            } else if (puckPostOffset != _pucksPostOffset.end()) {
+            if (puckPoseIter->first == puckIndex) {
-                return puckPose->second.postTransform(puckPostOffset->second);
+                break;
-            } else if (puckPreOffset != _pucksPreOffset.end()) {
-                return puckPose->second.transform(puckPreOffset->second);
             }
+            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);
+    glm::mat4 sensorToAvatar = glm::inverse(inputCalibrationData.avatarMat) * inputCalibrationData.sensorToWorldMat;
-    _poseStateMap[controller::HEAD] = hmdHeadPose.postTransform(defaultHeadOffset);
+    _poseStateMap[controller::HEAD] = pose.postTransform(defaultHeadOffset).transform(sensorToAvatar);
 }
 
 void ViveControllerManager::InputDevice::handlePoseEvent(float deltaTime, const controller::InputCalibrationData& inputCalibrationData,

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);