Merge branch 'master' of https://github.com/highfidelity/hifi

interface/resources/avatar/avatar-animation.json

@@ -114,6 +114,24 @@
                                     "weightVar": "headWeight",
                                     "weight": 4.0,
                                     "flexCoefficients": [1, 0.05, 0.25, 0.25, 0.25]
+                                },
+                                {
+                                    "jointName": "LeftArm",
+                                    "positionVar": "leftArmPosition",
+                                    "rotationVar": "leftArmRotation",
+                                    "typeVar": "leftArmType",
+                                    "weightVar": "leftArmWeight",
+                                    "weight": 0.75,
+                                    "flexCoefficients": [1.0, 0.35, 0.2, 0.1, 0.05, 0.0, 0.0, 0.0]
+                                },
+                                {
+                                    "jointName": "RightArm",
+                                    "positionVar": "rightArmPosition",
+                                    "rotationVar": "rightArmRotation",
+                                    "typeVar": "rightArmType",
+                                    "weightVar": "rightArmWeight",
+                                    "weight": 0.75,
+                                    "flexCoefficients": [1.0, 0.35, 0.2, 0.1, 0.05, 0.0, 0.0, 0.0]
                                 }
                             ]
                         },

interface/resources/controllers/standard.json

@@ -66,6 +66,8 @@
         { "from": "Standard.Hips", "to": "Actions.Hips" },
         { "from": "Standard.Spine2", "to": "Actions.Spine2" },
 
-        { "from": "Standard.Head", "to": "Actions.Head" }
+        { "from": "Standard.Head", "to": "Actions.Head" },
+        { "from": "Standard.LeftArm", "to": "Actions.LeftArm" },
+        { "from": "Standard.RightArm", "to": "Actions.RightArm" }
     ]
 }

interface/resources/controllers/vive.json

@@ -61,6 +61,9 @@
             "when": [ "Application.InHMD"]
         },
 
-        { "from": "Vive.Head", "to" : "Standard.Head", "when" : [ "Application.InHMD"] }
+        { "from": "Vive.Head", "to" : "Standard.Head", "when" : [ "Application.InHMD"] },
+
+        { "from": "Vive.RightArm", "to" : "Standard.RightArm", "when" : [ "Application.InHMD"] },
+        { "from": "Vive.LeftArm", "to" : "Standard.LeftArm", "when" : [ "Application.InHMD"] }
     ]
 }

interface/src/Application.cpp

@@ -4363,7 +4363,11 @@ void Application::update(float deltaTime) {
         myAvatar->getSpine2CalibrationMat(),
         myAvatar->getHipsCalibrationMat(),
         myAvatar->getLeftFootCalibrationMat(),
-        myAvatar->getRightFootCalibrationMat()
+        myAvatar->getRightFootCalibrationMat(),
+        myAvatar->getRightArmCalibrationMat(),
+        myAvatar->getLeftArmCalibrationMat(),
+        myAvatar->getRightHandCalibrationMat(),
+        myAvatar->getLeftHandCalibrationMat()
     };
 
     InputPluginPointer keyboardMousePlugin;
@@ -4419,6 +4423,10 @@ void Application::update(float deltaTime) {
     controller::Pose headPose = userInputMapper->getPoseState(controller::Action::HEAD);
     myAvatar->setHeadControllerPoseInSensorFrame(headPose.transform(avatarToSensorMatrix));
 
+    controller::Pose leftArmPose = userInputMapper->getPoseState(controller::Action::LEFT_ARM);
+    controller::Pose rightArmPose = userInputMapper->getPoseState(controller::Action::RIGHT_ARM);
+    myAvatar->setArmControllerPosesInSensorFrame(leftArmPose.transform(avatarToSensorMatrix), rightArmPose.transform(avatarToSensorMatrix));
+
     updateThreads(deltaTime); // If running non-threaded, then give the threads some time to process...
     updateDialogs(deltaTime); // update various stats dialogs if present
 

interface/src/avatar/MyAvatar.cpp

@@ -90,6 +90,14 @@ const float MyAvatar::ZOOM_DEFAULT = 1.5f;
 // static const glm::quat DEFAULT_AVATAR_MIDDLE_EYE_ROT { Quaternions::Y_180 };
 static const glm::vec3 DEFAULT_AVATAR_MIDDLE_EYE_POS { 0.0f, 0.6f, 0.0f };
 static const glm::vec3 DEFAULT_AVATAR_HEAD_POS { 0.0f, 0.53f, 0.0f };
+static const glm::vec3 DEFAULT_AVATAR_RIGHTARM_POS { -0.134824f, 0.396348f, -0.0515777f };
+static const glm::quat DEFAULT_AVATAR_RIGHTARM_ROT { -0.536241f, 0.536241f, -0.460918f, -0.460918f };
+static const glm::vec3 DEFAULT_AVATAR_LEFTARM_POS { 0.134795f, 0.396349f, -0.0515881f };
+static const glm::quat DEFAULT_AVATAR_LEFTARM_ROT { 0.536257f, 0.536258f, -0.460899f, 0.4609f };
+static const glm::vec3 DEFAULT_AVATAR_RIGHTHAND_POS { -0.72768f, 0.396349f, -0.0515779f };
+static const glm::quat DEFAULT_AVATAR_RIGHTHAND_ROT { 0.479184f, -0.520013f, 0.522537f, 0.476365f};
+static const glm::vec3 DEFAULT_AVATAR_LEFTHAND_POS { 0.727588f, 0.39635f, -0.0515878f };
+static const glm::quat DEFAULT_AVATAR_LEFTHAND_ROT { -0.479181f, -0.52001f, 0.52254f, -0.476369f };
 static const glm::vec3 DEFAULT_AVATAR_NECK_POS { 0.0f, 0.445f, 0.025f };
 static const glm::vec3 DEFAULT_AVATAR_SPINE2_POS { 0.0f, 0.32f, 0.02f };
 static const glm::vec3 DEFAULT_AVATAR_HIPS_POS { 0.0f, 0.0f, 0.0f };
@@ -1434,6 +1442,37 @@ controller::Pose MyAvatar::getHeadControllerPoseInAvatarFrame() const {
     return getHeadControllerPoseInWorldFrame().transform(invAvatarMatrix);
 }
 
+void MyAvatar::setArmControllerPosesInSensorFrame(const controller::Pose& left, const controller::Pose& right) {
+    _leftArmControllerPoseInSensorFrameCache.set(left);
+    _rightArmControllerPoseInSensorFrameCache.set(right);
+}
+
+controller::Pose MyAvatar::getLeftArmControllerPoseInSensorFrame() const {
+    return _leftArmControllerPoseInSensorFrameCache.get();
+}
+
+controller::Pose MyAvatar::getRightArmControllerPoseInSensorFrame() const {
+    return _rightArmControllerPoseInSensorFrameCache.get();
+}
+
+controller::Pose MyAvatar::getLeftArmControllerPoseInWorldFrame() const {
+    return getLeftArmControllerPoseInSensorFrame().transform(getSensorToWorldMatrix());
+}
+
+controller::Pose MyAvatar::getRightArmControllerPoseInWorldFrame() const {
+    return getRightArmControllerPoseInSensorFrame().transform(getSensorToWorldMatrix());
+}
+
+controller::Pose MyAvatar::getLeftArmControllerPoseInAvatarFrame() const {
+    glm::mat4 worldToAvatarMat = glm::inverse(createMatFromQuatAndPos(getOrientation(), getPosition()));
+    return getLeftArmControllerPoseInWorldFrame().transform(worldToAvatarMat);
+}
+
+controller::Pose MyAvatar::getRightArmControllerPoseInAvatarFrame() const {
+    glm::mat4 worldToAvatarMat = glm::inverse(createMatFromQuatAndPos(getOrientation(), getPosition()));
+    return getRightArmControllerPoseInWorldFrame().transform(worldToAvatarMat);
+}
+
 void MyAvatar::updateMotors() {
     _characterController.clearMotors();
     glm::quat motorRotation;
@@ -2747,6 +2786,51 @@ glm::mat4 MyAvatar::getRightFootCalibrationMat() const {
     }
 }
 
+
+glm::mat4 MyAvatar::getRightArmCalibrationMat() const {
+    int rightArmIndex = _skeletonModel->getRig().indexOfJoint("RightArm");
+    if (rightArmIndex >= 0) {
+        auto rightArmPos = getAbsoluteDefaultJointTranslationInObjectFrame(rightArmIndex);
+        auto rightArmRot = getAbsoluteDefaultJointRotationInObjectFrame(rightArmIndex);
+        return createMatFromQuatAndPos(rightArmRot, rightArmPos);
+    } else {
+        return createMatFromQuatAndPos(DEFAULT_AVATAR_RIGHTARM_ROT, DEFAULT_AVATAR_RIGHTARM_POS);
+    }
+}
+
+glm::mat4 MyAvatar::getLeftArmCalibrationMat() const {
+    int leftArmIndex = _skeletonModel->getRig().indexOfJoint("LeftArm");
+    if (leftArmIndex >= 0) {
+        auto leftArmPos = getAbsoluteDefaultJointTranslationInObjectFrame(leftArmIndex);
+        auto leftArmRot = getAbsoluteDefaultJointRotationInObjectFrame(leftArmIndex);
+        return createMatFromQuatAndPos(leftArmRot, leftArmPos);
+    } else {
+        return createMatFromQuatAndPos(DEFAULT_AVATAR_LEFTARM_ROT, DEFAULT_AVATAR_RIGHTARM_POS);
+    }
+}
+
+glm::mat4 MyAvatar::getRightHandCalibrationMat() const {
+    int rightHandIndex = _skeletonModel->getRig().indexOfJoint("RightHand");
+    if (rightHandIndex >= 0) {
+        auto rightHandPos = getAbsoluteDefaultJointTranslationInObjectFrame(rightHandIndex);
+        auto rightHandRot = getAbsoluteDefaultJointRotationInObjectFrame(rightHandIndex);
+        return createMatFromQuatAndPos(rightHandRot, rightHandPos);
+    } else {
+        return createMatFromQuatAndPos(DEFAULT_AVATAR_RIGHTHAND_ROT, DEFAULT_AVATAR_RIGHTHAND_POS);
+    }
+}
+
+glm::mat4 MyAvatar::getLeftHandCalibrationMat() const {
+    int leftHandIndex = _skeletonModel->getRig().indexOfJoint("LeftHand");
+    if (leftHandIndex >= 0) {
+        auto leftHandPos = getAbsoluteDefaultJointTranslationInObjectFrame(leftHandIndex);
+        auto leftHandRot = getAbsoluteDefaultJointRotationInObjectFrame(leftHandIndex);
+        return createMatFromQuatAndPos(leftHandRot, leftHandPos);
+    } else {
+        return createMatFromQuatAndPos(DEFAULT_AVATAR_LEFTHAND_ROT, DEFAULT_AVATAR_LEFTHAND_POS);
+    }
+}
+
 bool MyAvatar::pinJoint(int index, const glm::vec3& position, const glm::quat& orientation) {
     auto hipsIndex = getJointIndex("Hips");
     if (index != hipsIndex) {

interface/src/avatar/MyAvatar.h

@@ -471,6 +471,14 @@ public:
     controller::Pose getHeadControllerPoseInWorldFrame() const;
     controller::Pose getHeadControllerPoseInAvatarFrame() const;
 
+    void setArmControllerPosesInSensorFrame(const controller::Pose& left, const controller::Pose& right);
+    controller::Pose getLeftArmControllerPoseInSensorFrame() const;
+    controller::Pose getRightArmControllerPoseInSensorFrame() const;
+    controller::Pose getLeftArmControllerPoseInWorldFrame() const;
+    controller::Pose getRightArmControllerPoseInWorldFrame() const;
+    controller::Pose getLeftArmControllerPoseInAvatarFrame() const;
+    controller::Pose getRightArmControllerPoseInAvatarFrame() const;
+
     bool hasDriveInput() const;
 
     Q_INVOKABLE void setCollisionsEnabled(bool enabled);
@@ -488,6 +496,10 @@ public:
     glm::mat4 getHipsCalibrationMat() const;
     glm::mat4 getLeftFootCalibrationMat() const;
     glm::mat4 getRightFootCalibrationMat() const;
+    glm::mat4 getRightArmCalibrationMat() const;
+    glm::mat4 getLeftArmCalibrationMat() const;
+    glm::mat4 getLeftHandCalibrationMat() const;
+    glm::mat4 getRightHandCalibrationMat() const;
 
     void addHoldAction(AvatarActionHold* holdAction);  // thread-safe
     void removeHoldAction(AvatarActionHold* holdAction);  // thread-safe
@@ -737,6 +749,8 @@ private:
     ThreadSafeValueCache<controller::Pose> _hipsControllerPoseInSensorFrameCache{ controller::Pose() };
     ThreadSafeValueCache<controller::Pose> _spine2ControllerPoseInSensorFrameCache{ controller::Pose() };
     ThreadSafeValueCache<controller::Pose> _headControllerPoseInSensorFrameCache{ controller::Pose() };
+    ThreadSafeValueCache<controller::Pose> _leftArmControllerPoseInSensorFrameCache{ controller::Pose() };
+    ThreadSafeValueCache<controller::Pose> _rightArmControllerPoseInSensorFrameCache{ controller::Pose() };
 
     bool _hmdLeanRecenterEnabled = true;
 

interface/src/avatar/MySkeletonModel.cpp

@@ -92,6 +92,26 @@ void MySkeletonModel::updateRig(float deltaTime, glm::mat4 parentTransform) {
         headParams.spine2Enabled = false;
     }
 
+    auto avatarRightArmPose = myAvatar->getRightArmControllerPoseInAvatarFrame();
+    if (avatarRightArmPose.isValid()) {
+        glm::mat4 rightArmMat = Matrices::Y_180 * createMatFromQuatAndPos(avatarRightArmPose.getRotation(), avatarRightArmPose.getTranslation());
+        headParams.rightArmPosition = extractTranslation(rightArmMat);
+        headParams.rightArmRotation = glmExtractRotation(rightArmMat);
+        headParams.rightArmEnabled = true;
+    } else {
+        headParams.rightArmEnabled = false;
+    }
+    
+    auto avatarLeftArmPose = myAvatar->getLeftArmControllerPoseInAvatarFrame();
+    if (avatarLeftArmPose.isValid()) {
+        glm::mat4 leftArmMat = Matrices::Y_180 * createMatFromQuatAndPos(avatarLeftArmPose.getRotation(), avatarLeftArmPose.getTranslation());
+        headParams.leftArmPosition = extractTranslation(leftArmMat);
+        headParams.leftArmRotation = glmExtractRotation(leftArmMat);
+        headParams.leftArmEnabled = true;
+    } else {
+        headParams.leftArmEnabled = false;
+    }
+
     headParams.isTalking = head->getTimeWithoutTalking() <= 1.5f;
 
     _rig.updateFromHeadParameters(headParams, deltaTime);

libraries/animation/src/Rig.cpp

@@ -1047,6 +1047,22 @@ void Rig::updateFromHeadParameters(const HeadParameters& params, float dt) {
     } else {
         _animVars.set("spine2Type", (int)IKTarget::Type::Unknown);
     }
+
+    if (params.leftArmEnabled) {
+        _animVars.set("leftArmType", (int)IKTarget::Type::RotationAndPosition);
+        _animVars.set("leftArmPosition", params.leftArmPosition);
+        _animVars.set("leftArmRotation", params.leftArmRotation);        
+    } else {
+        _animVars.set("leftArmType", (int)IKTarget::Type::Unknown);
+    }
+
+    if (params.rightArmEnabled) {
+        _animVars.set("rightArmType", (int)IKTarget::Type::RotationAndPosition);
+        _animVars.set("rightArmPosition", params.rightArmPosition);
+        _animVars.set("rightArmRotation", params.rightArmRotation);
+    } else {
+        _animVars.set("rightArmType", (int)IKTarget::Type::Unknown);
+    }
 }
 
 void Rig::updateFromEyeParameters(const EyeParameters& params) {

libraries/animation/src/Rig.h

@@ -46,9 +46,15 @@ public:
         glm::mat4 spine2Matrix = glm::mat4();         // rig space
         glm::quat rigHeadOrientation = glm::quat();   // rig space (-z forward)
         glm::vec3 rigHeadPosition = glm::vec3();      // rig space
+        glm::vec3 rightArmPosition = glm::vec3();     // rig space
+        glm::quat rightArmRotation = glm::quat();     // rig space
+        glm::vec3 leftArmPosition = glm::vec3();      // rig space
+        glm::quat leftArmRotation = glm::quat();      // rig space
         bool hipsEnabled = false;
         bool headEnabled = false;
         bool spine2Enabled = false;
+        bool leftArmEnabled = false;
+        bool rightArmEnabled = false;
         bool isTalking = false;
     };
 

libraries/controllers/src/controllers/Actions.cpp

@@ -51,6 +51,8 @@ namespace controller {
 
             makePosePair(Action::LEFT_HAND, "LeftHand"),
             makePosePair(Action::RIGHT_HAND, "RightHand"),
+            makePosePair(Action::RIGHT_ARM, "RightArm"),
+            makePosePair(Action::LEFT_ARM, "LeftArm"),
             makePosePair(Action::LEFT_FOOT, "LeftFoot"),
             makePosePair(Action::RIGHT_FOOT, "RightFoot"),
             makePosePair(Action::HIPS, "Hips"),

libraries/controllers/src/controllers/Actions.h

@@ -42,6 +42,8 @@ enum class Action {
 
     LEFT_HAND = NUM_COMBINED_AXES,
     RIGHT_HAND,
+    LEFT_ARM,
+    RIGHT_ARM,
     LEFT_FOOT,
     RIGHT_FOOT,
     HIPS,

libraries/controllers/src/controllers/Input.h

@@ -24,7 +24,11 @@ struct InputCalibrationData {
     glm::mat4 defaultSpine2;       // default pose for spine2 joint in avatar space
     glm::mat4 defaultHips;         // default pose for hips joint in avatar space
     glm::mat4 defaultLeftFoot;     // default pose for leftFoot joint in avatar space
-    glm::mat4 defaultRightFoot;    // default pose for leftFoot joint in avatar space
+    glm::mat4 defaultRightFoot;    // default pose for rightFoot joint in avatar space
+    glm::mat4 defaultRightArm;     // default pose for rightArm joint in avatar space
+    glm::mat4 defaultLeftArm;      // default pose for leftArm joint in avatar space
+    glm::mat4 defaultRightHand;    // default pose for rightHand joint in avatar space
+    glm::mat4 defaultLeftHand;     // default pose for leftHand joint in avatar space
 };
 
 enum class ChannelType {

libraries/controllers/src/controllers/StandardController.cpp

@@ -104,6 +104,8 @@ Input::NamedVector StandardController::getAvailableInputs() const {
         makePair(RIGHT_HAND, "RightHand"),
         makePair(LEFT_FOOT, "LeftFoot"),
         makePair(RIGHT_FOOT, "RightFoot"),
+        makePair(RIGHT_ARM, "RightArm"),
+        makePair(LEFT_ARM, "LeftArm"),
         makePair(HIPS, "Hips"),
         makePair(SPINE2, "Spine2"),
         makePair(HEAD, "Head"),

libraries/ktx/src/khronos/KHR.h

@@ -212,10 +212,11 @@ namespace khronos {
             template <uint32_t ALIGNMENT> 
             inline uint32_t evalAlignedCompressedBlockCount(uint32_t value) {
                 // FIXME add static assert that ALIGNMENT is a power of 2
-                return (value + (ALIGNMENT - 1) / ALIGNMENT);
+                static uint32_t ALIGNMENT_REMAINDER = ALIGNMENT - 1;
+                return (value + ALIGNMENT_REMAINDER) / ALIGNMENT;
             }
 
-            inline uint8_t evalBlockAlignemnt(InternalFormat format, uint32_t value) {
+            inline uint32_t evalCompressedBlockCount(InternalFormat format, uint32_t value) {
                 switch (format) {
                     case InternalFormat::COMPRESSED_SRGB_S3TC_DXT1_EXT: // BC1
                     case InternalFormat::COMPRESSED_SRGB_ALPHA_S3TC_DXT1_EXT: // BC1A

libraries/ktx/src/ktx/KTX.cpp

@@ -30,7 +30,7 @@ uint32_t Header::evalMaxDimension() const {
 
 uint32_t Header::evalPixelOrBlockDimension(uint32_t pixelDimension) const {
     if (isCompressed()) {
-        return khronos::gl::texture::evalBlockAlignemnt(getGLInternaFormat(), pixelDimension);
+        return khronos::gl::texture::evalCompressedBlockCount(getGLInternaFormat(), pixelDimension);
     } 
     return pixelDimension;
 }

plugins/openvr/src/ViveControllerManager.cpp

@@ -33,6 +33,7 @@
 
 #include <controllers/StandardControls.h>
 
+
 extern PoseData _nextSimPoseData;
 
 vr::IVRSystem* acquireOpenVrSystem();
@@ -49,6 +50,7 @@ static const char* RENDER_CONTROLLERS = "Render Hand Controllers";
 static const int MIN_PUCK_COUNT = 2;
 static const int MIN_FEET_AND_HIPS = 3;
 static const int MIN_FEET_HIPS_CHEST = 4;
+static const int MIN_FEET_HIPS_SHOULDERS = 5;
 static const int FIRST_FOOT = 0;
 static const int SECOND_FOOT = 1;
 static const int HIP = 2;
@@ -70,10 +72,14 @@ static glm::mat4 computeOffset(glm::mat4 defaultToReferenceMat, glm::mat4 defaul
     return glm::inverse(poseMat) * referenceJointMat;
 }
 
-static bool sortPucksYPosition(std::pair<uint32_t, controller::Pose> firstPuck, std::pair<uint32_t, controller::Pose> secondPuck) {
+static bool sortPucksYPosition(PuckPosePair firstPuck, PuckPosePair secondPuck) {
     return (firstPuck.second.translation.y < secondPuck.second.translation.y);
 }
 
+static bool sortPucksXPosition(PuckPosePair firstPuck, PuckPosePair secondPuck) {
+    return (firstPuck.second.translation.x < secondPuck.second.translation.x);
+}
+
 static QString deviceTrackingResultToString(vr::ETrackingResult trackingResult) {
     QString result;
     auto iterator = TRACKING_RESULT_TO_STRING.find(trackingResult);
@@ -168,6 +174,7 @@ ViveControllerManager::InputDevice::InputDevice(vr::IVRSystem*& system) : contro
     _configStringMap[Config::Feet] =  QString("Feet");
     _configStringMap[Config::FeetAndHips] =  QString("FeetAndHips");
     _configStringMap[Config::FeetHipsAndChest] =  QString("FeetHipsAndChest");
+    _configStringMap[Config::FeetHipsAndShoulders] = QString("FeetHipsAndShoulders");
 
     if (openVrSupported()) {
         createPreferences();
@@ -333,34 +340,21 @@ void ViveControllerManager::InputDevice::calibrate(const controller::InputCalibr
 
     std::sort(_validTrackedObjects.begin(), _validTrackedObjects.end(), sortPucksYPosition);
 
-    auto& firstFoot = _validTrackedObjects[FIRST_FOOT];
-    auto& secondFoot = _validTrackedObjects[SECOND_FOOT];
-    controller::Pose& firstFootPose = firstFoot.second;
-    controller::Pose& secondFootPose = secondFoot.second;
-
-    if (firstFootPose.translation.x < secondFootPose.translation.x) {
-        _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);
-
-    } 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);
-    }
-
     if (_config == Config::Feet) {
-        // done
+        calibrateFeet(defaultToReferenceMat, inputCalibration);
     } else if (_config == Config::FeetAndHips && puckCount >= MIN_FEET_AND_HIPS) {
-        _jointToPuckMap[controller::HIPS] = _validTrackedObjects[HIP].first;
+        calibrateFeet(defaultToReferenceMat, inputCalibration);
-        _pucksOffset[_validTrackedObjects[HIP].first] = computeOffset(defaultToReferenceMat, inputCalibration.defaultHips, _validTrackedObjects[HIP].second);
+        calibrateHips(defaultToReferenceMat, inputCalibration);
     } else if (_config == Config::FeetHipsAndChest && puckCount >= MIN_FEET_HIPS_CHEST) {
-        _jointToPuckMap[controller::HIPS] = _validTrackedObjects[HIP].first;
+        calibrateFeet(defaultToReferenceMat, inputCalibration);
-        _pucksOffset[_validTrackedObjects[HIP].first] = computeOffset(defaultToReferenceMat, inputCalibration.defaultHips, _validTrackedObjects[HIP].second);
+        calibrateHips(defaultToReferenceMat, inputCalibration);
-        _jointToPuckMap[controller::SPINE2] = _validTrackedObjects[CHEST].first;
+        calibrateChest(defaultToReferenceMat, inputCalibration);
-        _pucksOffset[_validTrackedObjects[CHEST].first] = computeOffset(defaultToReferenceMat, inputCalibration.defaultSpine2, _validTrackedObjects[CHEST].second);
+    } else if (_config == Config::FeetHipsAndShoulders && puckCount >= MIN_FEET_HIPS_SHOULDERS) {
+        calibrateFeet(defaultToReferenceMat, inputCalibration);
+        calibrateHips(defaultToReferenceMat, inputCalibration);
+        int firstShoulderIndex = 3;
+        int secondShoulderIndex = 4;
+        calibrateShoulders(defaultToReferenceMat, inputCalibration, firstShoulderIndex, secondShoulderIndex);
     } else {
         qDebug() << "Puck Calibration: " << configToString(_config) << " Config Failed: Could not meet the minimal # of pucks";
         uncalibrate();
@@ -382,6 +376,8 @@ void ViveControllerManager::InputDevice::updateCalibratedLimbs() {
     _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);
 }
 
 controller::Pose ViveControllerManager::InputDevice::addOffsetToPuckPose(int joint) const {
@@ -615,6 +611,56 @@ void ViveControllerManager::InputDevice::hapticsHelper(float deltaTime, bool lef
     }
 }
 
+void ViveControllerManager::InputDevice::calibrateFeet(glm::mat4& defaultToReferenceMat, const controller::InputCalibrationData& inputCalibration) {
+    auto& firstFoot = _validTrackedObjects[FIRST_FOOT];
+    auto& secondFoot = _validTrackedObjects[SECOND_FOOT];
+    controller::Pose& firstFootPose = firstFoot.second;
+    controller::Pose& secondFootPose = secondFoot.second;
+    
+    if (firstFootPose.translation.x < secondFootPose.translation.x) {
+        _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);   
+    } 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);
+    }
+}
+
+void ViveControllerManager::InputDevice::calibrateHips(glm::mat4& defaultToReferenceMat, const controller::InputCalibrationData& inputCalibration) {
+    _jointToPuckMap[controller::HIPS] = _validTrackedObjects[HIP].first;
+    _pucksOffset[_validTrackedObjects[HIP].first] = computeOffset(defaultToReferenceMat, inputCalibration.defaultHips, _validTrackedObjects[HIP].second);
+}
+
+void ViveControllerManager::InputDevice::calibrateChest(glm::mat4& defaultToReferenceMat, const controller::InputCalibrationData& inputCalibration) {
+    _jointToPuckMap[controller::SPINE2] = _validTrackedObjects[CHEST].first;
+    _pucksOffset[_validTrackedObjects[CHEST].first] = computeOffset(defaultToReferenceMat, inputCalibration.defaultSpine2, _validTrackedObjects[CHEST].second);
+}
+
+void ViveControllerManager::InputDevice::calibrateShoulders(glm::mat4& defaultToReferenceMat, const controller::InputCalibrationData& inputCalibration,
+                                                            int firstShoulderIndex, int secondShoulderIndex) {
+    const PuckPosePair& firstShoulder = _validTrackedObjects[firstShoulderIndex];
+    const PuckPosePair& secondShoulder = _validTrackedObjects[secondShoulderIndex];
+    const controller::Pose& firstShoulderPose = firstShoulder.second;
+    const controller::Pose& secondShoulderPose = secondShoulder.second;
+
+    if (firstShoulderPose.translation.x < secondShoulderPose.translation.x) {
+        _jointToPuckMap[controller::LEFT_ARM] = firstShoulder.first;
+        _pucksOffset[firstShoulder.first] = computeOffset(defaultToReferenceMat, inputCalibration.defaultLeftArm, firstShoulder.second);
+        _jointToPuckMap[controller::RIGHT_ARM] = secondShoulder.first;
+        _pucksOffset[secondShoulder.first] = computeOffset(defaultToReferenceMat, inputCalibration.defaultRightArm, secondShoulder.second);
+    } else {
+        _jointToPuckMap[controller::LEFT_ARM] = secondShoulder.first;
+        _pucksOffset[secondShoulder.first] = computeOffset(defaultToReferenceMat, inputCalibration.defaultLeftArm, secondShoulder.second);
+        _jointToPuckMap[controller::RIGHT_ARM] = firstShoulder.first;
+        _pucksOffset[firstShoulder.first] = computeOffset(defaultToReferenceMat, inputCalibration.defaultRightArm, firstShoulder.second);
+    }
+}   
+
+
 void ViveControllerManager::InputDevice::loadSettings() {
     Settings settings;
     settings.beginGroup("PUCK_CONFIG");
@@ -646,6 +692,8 @@ void ViveControllerManager::InputDevice::setConfigFromString(const QString& valu
         _preferedConfig = Config::FeetAndHips;
     } else if (value == "FeetHipsAndChest") {
         _preferedConfig = Config::FeetHipsAndChest;
+    } else if (value == "FeetHipsAndShoulders") {
+        _preferedConfig = Config::FeetHipsAndShoulders;
     }
 }
 
@@ -658,7 +706,7 @@ void ViveControllerManager::InputDevice::createPreferences() {
         auto getter = [this]()->QString { return _configStringMap[_preferedConfig]; };
         auto setter = [this](const QString& value) { setConfigFromString(value); saveSettings(); };
         auto preference = new ComboBoxPreference(VIVE_PUCKS_CONFIG, "Configuration", getter, setter);
-        QStringList list = (QStringList() << "Auto" << "Feet" << "FeetAndHips" << "FeetHipsAndChest");
+        QStringList list = {"Auto", "Feet", "FeetAndHips", "FeetHipsAndChest", "FeetHipsAndShoulders"};
         preference->setItems(list);
         preferences->addPreference(preference);
 
@@ -710,6 +758,8 @@ controller::Input::NamedVector ViveControllerManager::InputDevice::getAvailableI
         makePair(HIPS, "Hips"),
         makePair(SPINE2, "Spine2"),
         makePair(HEAD, "Head"),
+        makePair(LEFT_ARM, "LeftArm"),
+        makePair(RIGHT_ARM, "RightArm"),
 
         // 16 tracked poses
         makePair(TRACKED_OBJECT_00, "TrackedObject00"),

plugins/openvr/src/ViveControllerManager.h

@@ -27,6 +27,8 @@
 #include <render/Scene.h>
 #include "OpenVrHelpers.h"
 
+using PuckPosePair = std::pair<uint32_t, controller::Pose>;
+
 namespace vr {
     class IVRSystem;
 }
@@ -78,6 +80,15 @@ private:
                                  const vec3& angularVelocity);
         void partitionTouchpad(int sButton, int xAxis, int yAxis, int centerPsuedoButton, int xPseudoButton, int yPseudoButton);
         void printDeviceTrackingResultChange(uint32_t deviceIndex);
+        void setConfigFromString(const QString& value);
+        void loadSettings();
+        void saveSettings() const;
+        void calibrateFeet(glm::mat4& defaultToReferenceMat, const controller::InputCalibrationData& inputCalibration);
+        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,
+                                int firstShoulderIndex, int secondShoulderIndex);
 
         class FilteredStick {
         public:
@@ -102,13 +113,19 @@ private:
             float _timer { 0.0f };
             glm::vec2 _stick { 0.0f, 0.0f };
         };
-        enum class Config { Feet, FeetAndHips, FeetHipsAndChest, Auto };
+        enum class Config {
+            Auto,
+            Feet,
+            FeetAndHips,
+            FeetHipsAndChest,
+            FeetHipsAndShoulders,
+        };
         Config _config { Config::Auto };
         Config _preferedConfig { Config::Auto };
         FilteredStick _filteredLeftStick;
         FilteredStick _filteredRightStick;
 
-        std::vector<std::pair<uint32_t, controller::Pose>> _validTrackedObjects;
+        std::vector<PuckPosePair> _validTrackedObjects;
         std::map<uint32_t, glm::mat4> _pucksOffset;
         std::map<int, uint32_t> _jointToPuckMap;
         std::map<Config, QString> _configStringMap;
@@ -131,9 +148,6 @@ private:
         mutable std::recursive_mutex _lock;
 
         QString configToString(Config config);
-        void setConfigFromString(const QString& value);
-        void loadSettings();
-        void saveSettings() const;
         friend class ViveControllerManager;
     };