Merge remote-tracking branch 'upstream/master' into 21484

cmake/templates/NSIS.template.in

@@ -484,16 +484,14 @@ Function InstallTypesPage
   StrCpy $OffsetUnits u
   StrCpy $Express "0"  
   
-  ${If} ${SectionIsSelected} ${@CLIENT_COMPONENT_NAME@}
+  ${NSD_CreateRadioButton} 30% $CurrentOffset$OffsetUnits 100% 10u "Express Install (Recommended)"; $\nInstalls High Fidelity Interface and High Fidelity Sandbox"
-    ${NSD_CreateRadioButton} 30% $CurrentOffset$OffsetUnits 100% 10u "Express Install (Recommended)"; $\nInstalls High Fidelity Interface and High Fidelity Sandbox"
+  pop $ExpressInstallRadioButton
-    pop $ExpressInstallRadioButton
+  ${NSD_OnClick} $ExpressInstallRadioButton ChangeExpressLabel
-    ${NSD_OnClick} $ExpressInstallRadioButton ChangeExpressLabel
+  IntOp $CurrentOffset $CurrentOffset + 15
-    IntOp $CurrentOffset $CurrentOffset + 15
   
-    ${NSD_CreateRadiobutton} 30% $CurrentOffset$OffsetUnits 100% 10u "Custom Install (Advanced)"
+  ${NSD_CreateRadiobutton} 30% $CurrentOffset$OffsetUnits 100% 10u "Custom Install (Advanced)"
-    pop $CustomInstallRadioButton
+  pop $CustomInstallRadioButton
-    ${NSD_OnClick} $CustomInstallRadioButton ChangeCustomLabel 
+  ${NSD_OnClick} $CustomInstallRadioButton ChangeCustomLabel 
-  ${EndIf}
   
   ; Express Install selected by default
   ${NSD_Check} $ExpressInstallRadioButton
@@ -518,7 +516,6 @@ FunctionEnd
 
 Function AbortFunction
   ; Check if Express is set, if so, abort the post install options page
-  Call HandleInstallTypes ; Sets Express if ExpressInstallRadioButton is checked and installs with defaults
   StrCmp $Express "1" 0 end
   Abort
   end:
@@ -529,10 +526,15 @@ Function PostInstallOptionsPage
 
   nsDialogs::Create 1018
   Pop $PostInstallDialog
-
+  
   ${If} $PostInstallDialog == error
     Abort
   ${EndIf}
+  
+  ; Check if Express is set, if so, abort the post install options page
+  StrCmp $Express "1" 0 end
+  Abort
+  end:
 
   StrCpy $CurrentOffset 0
   StrCpy $OffsetUnits u
@@ -617,12 +619,6 @@ Function PostInstallOptionsPage
     ${NSD_SetState} $CopyFromProductionCheckbox ${BST_UNCHECKED}
   ${EndIf}
   
-  ; Check if Express is set, if so, abort the post install options page
-  Call HandleInstallTypes ; Sets Express if ExpressInstallRadioButton is checked and installs with defaults
-  StrCmp $Express "1" 0 end
-  Abort
-  end:
-  
   nsDialogs::Show
 FunctionEnd
 
@@ -638,15 +634,30 @@ Var LaunchServerNowState
 Var LaunchClientNowState
 Var CopyFromProductionState
 Var CleanInstallState
-Var ExpressInstallState 
+Var ExpressInstallState
 Var CustomInstallState
 
 Function ReadInstallTypes
-  ${If} ${SectionIsSelected} ${@CLIENT_COMPONENT_NAME@}
+  ; check if the user asked for express/custom install
-    ; check if the user asked for express/custom install
+  ${NSD_GetState} $ExpressInstallRadioButton $ExpressInstallState
-    ${NSD_GetState} $ExpressInstallRadioButton $ExpressInstallState
+  ${NSD_GetState} $CustomInstallRadioButton $CustomInstallState
-    ${NSD_GetState} $CustomInstallRadioButton $CustomInstallState
+  
+  ${If} $ExpressInstallState == ${BST_CHECKED}
+    StrCpy $Express "1"
+    
+    StrCpy $DesktopClientState ${BST_CHECKED}  
+    StrCpy $ServerStartupState ${BST_CHECKED} 
+    StrCpy $LaunchServerNowState ${BST_CHECKED}
+    StrCpy $LaunchClientNowState ${BST_CHECKED}
+    StrCpy $CleanInstallState ${BST_UNCHECKED}
+    StrCpy $DesktopServerState ${BST_UNCHECKED}
+
+    ${If} @PR_BUILD@ == 1
+      StrCpy $CopyFromProductionState ${BST_UNCHECKED}
+    ${EndIf}
+    
   ${EndIf}
+  
 FunctionEnd
 
 Function ReadPostInstallOptions
@@ -684,28 +695,6 @@ Function ReadPostInstallOptions
   ${EndIf}
 FunctionEnd
 
-Function HandleInstallTypes
-  ${If} $ExpressInstallState == ${BST_CHECKED}
-    
-    StrCpy $Express "1"
-    
-    ; over ride custom checkboxes and select defaults 
-    ${NSD_SetState} $DesktopClientCheckbox ${BST_CHECKED} 
-    ${NSD_SetState} $ServerStartupCheckbox ${BST_CHECKED} 
-    ${NSD_SetState} $LaunchServerNowCheckbox ${BST_CHECKED}
-    ${NSD_SetState} $LaunchClientNowCheckbox ${BST_CHECKED}
-    
-    ${If} @PR_BUILD@ == 1
-      ${NSD_SetState} $CopyFromProductionCheckbox ${BST_UNCHECKED}
-    ${EndIf}    
-    
-    ; call ReadPostInstallOptions and HandlePostInstallOptions with defaults selected
-    Call ReadPostInstallOptions 
-    Call HandlePostInstallOptions 
-      
-  ${EndIf}
-FunctionEnd
-
 Function HandlePostInstallOptions
   ${If} ${SectionIsSelected} ${@CLIENT_COMPONENT_NAME@}
     ; check if the user asked for a desktop shortcut to High Fidelity
@@ -854,7 +843,7 @@ Section "-Core installation"
   Rename "$INSTDIR\resources\qml\styles-uit\RalewaySemibold.qml" "$INSTDIR\resources\qml\styles-uit\RalewaySemiBold.qml"
 
   ExecWait "$INSTDIR\vcredist_x64.exe /install /q /norestart"
-
+  
   ; Remove the Old Interface directory and vcredist_x64.exe (from installs prior to Server Console)
   RMDir /r "$INSTDIR\Interface"
   Delete "$INSTDIR\vcredist_x64.exe"
@@ -954,9 +943,9 @@ Section "-Core installation"
   Call ConditionalAddToRegisty
 
   !insertmacro MUI_STARTMENU_WRITE_END
-
+  
-@CPACK_NSIS_EXTRA_INSTALL_COMMANDS@
+  @CPACK_NSIS_EXTRA_INSTALL_COMMANDS@
-
+  
   ; Handle whichever post install options were set
   Call HandlePostInstallOptions
 

interface/resources/avatar/avatar-animation.json

@@ -126,24 +126,6 @@
                                     "weightVar": "headWeight",
                                     "weight": 4.0,
                                     "flexCoefficients": [1, 0.5, 0.25, 0.2, 0.1]
-                                },
-                                {
-                                    "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/qml/hifi/dialogs/content/AttachmentsContent.qml

@@ -50,7 +50,7 @@ Item {
                         margins: 4
                     }
                     clip: true
-                    snapMode: ListView.SnapToItem
+                    cacheBuffer: 4000
 
                     model: ListModel {}
                     delegate: Item {

interface/resources/qml/hifi/tablet/OpenVrConfiguration.qml

@@ -145,12 +145,13 @@ Rectangle {
         visible: headPuckBox.checked
         HifiControls.SpinBox {
             id: headYOffset
-            decimals: 4
+            decimals: 1
             width: 112
-            label: "Y: offset"
+            label: "Y Offset"
+            suffix: " cm"
             minimumValue: -10
-            stepSize: 0.0254
+            stepSize: 1
-            value: -0.05
+            value: -5
             colorScheme: hifi.colorSchemes.dark
 
             onEditingFinished: {
@@ -162,11 +163,12 @@ Rectangle {
         HifiControls.SpinBox {
             id: headZOffset
             width: 112
-            label: "Z: offset"
+            label: "Z Offset"
             minimumValue: -10
-            stepSize: 0.0254
+            stepSize: 1
-            decimals: 4
+            decimals: 1
-            value: -0.05
+            suffix: " cm"
+            value: -5
             colorScheme: hifi.colorSchemes.dark
 
             onEditingFinished: {
@@ -175,7 +177,6 @@ Rectangle {
         }
     }
 
-
     RalewayBold {
         id: hands
 
@@ -254,11 +255,12 @@ Rectangle {
 
         HifiControls.SpinBox {
             id: handYOffset
-            decimals: 4
+            decimals: 1
             width: 112
-            label: "Y: offset"
+            suffix: " cm"
+            label: "Y Offset"
             minimumValue: -10
-            stepSize: 0.0254
+            stepSize: 1
             colorScheme: hifi.colorSchemes.dark
 
             onEditingFinished: {
@@ -270,10 +272,11 @@ Rectangle {
         HifiControls.SpinBox {
             id: handZOffset
             width: 112
-            label: "Z: offset"
+            label: "Z Offset"
+            suffix: " cm"
             minimumValue: -10
-            stepSize: 0.0254
+            stepSize: 1
-            decimals: 4
+            decimals: 1
             colorScheme: hifi.colorSchemes.dark
 
             onEditingFinished: {
@@ -488,15 +491,55 @@ Rectangle {
         }
     }
 
+    Row {
+        id: shoulderAdditionalConfig
+        visible: shoulderBox.checked
+        anchors.top: shoulderConfig.bottom
+        anchors.topMargin: 5
+        anchors.left: openVrConfiguration.left
+        anchors.leftMargin: leftMargin + 20
+        spacing: 10
+
+        HifiControls.SpinBox {
+            id: armCircumference
+            decimals: 1
+            width: 160
+            suffix: " cm"
+            label: "Arm Circumference"
+            minimumValue: 0
+            stepSize: 1.0
+            colorScheme: hifi.colorSchemes.dark
+            value: 33.0
+
+            onEditingFinished: {
+                sendConfigurationSettings();
+            }
+        }
+
+        HifiControls.SpinBox {
+            id: shoulderWidth
+            width: 160
+            label: "Shoulder Width"
+            suffix: " cm"
+            minimumValue: 0
+            stepSize: 1.0
+            decimals: 1
+            colorScheme: hifi.colorSchemes.dark
+            value: 48
+
+            onEditingFinished: {
+                sendConfigurationSettings();
+            }
+        }
+    }
+
     Separator {
         id: bottomSeperator
         width: parent.width
-        anchors.top: shoulderConfig.bottom
+        anchors.top: shoulderAdditionalConfig.visible ? shoulderAdditionalConfig.bottom : shoulderConfig.bottom
-        anchors.topMargin: 10
+        anchors.topMargin: (shoulderAdditionalConfig.visible ? 25 : 10)
     }
 
-
-
     Rectangle {
         id: calibrationButton
         property int color: hifi.buttons.blue
@@ -835,6 +878,9 @@ Rectangle {
         var viveController = settings["handController"];
         var desktopMode = settings["desktopMode"];
 
+        armCircumference.value = settings.armCircumference;
+        shoulderWidth.value = settings.shoulderWidth;
+
         if (HmdHead) {
             headBox.checked = true;
             headPuckBox.checked = false;
@@ -1010,6 +1056,8 @@ Rectangle {
             "bodyConfiguration": trackerConfiguration,
             "headConfiguration": headObject,
             "handConfiguration": handObject,
+            "armCircumference": armCircumference.value,
+            "shoulderWidth": shoulderWidth.value,
             "desktopMode": viveInDesktop.checked
         }
 

interface/src/Application.cpp

@@ -4416,10 +4416,9 @@ void Application::updateMyAvatarLookAtPosition() {
             }
         } else {
             //  I am not looking at anyone else, so just look forward
-            auto headPose = myAvatar->getHeadControllerPoseInSensorFrame();
+            auto headPose = myAvatar->getControllerPoseInWorldFrame(controller::Action::HEAD);
             if (headPose.isValid()) {
-                glm::mat4 worldHeadMat = myAvatar->getSensorToWorldMatrix() * headPose.getMatrix();
+                lookAtSpot = transformPoint(headPose.getMatrix(), glm::vec3(0.0f, 0.0f, TREE_SCALE));
-                lookAtSpot = transformPoint(worldHeadMat, glm::vec3(0.0f, 0.0f, TREE_SCALE));
             } else {
                 lookAtSpot = myAvatar->getHead()->getEyePosition() +
                     (myAvatar->getHead()->getFinalOrientationInWorldFrame() * glm::vec3(0.0f, 0.0f, -TREE_SCALE));
@@ -4833,52 +4832,76 @@ void Application::update(float deltaTime) {
         myAvatar->setDriveKey(MyAvatar::ZOOM, userInputMapper->getActionState(controller::Action::TRANSLATE_CAMERA_Z));
     }
 
-    controller::Pose leftHandPose = userInputMapper->getPoseState(controller::Action::LEFT_HAND);
+    static const std::vector<controller::Action> avatarControllerActions = {
-    controller::Pose rightHandPose = userInputMapper->getPoseState(controller::Action::RIGHT_HAND);
+        controller::Action::LEFT_HAND,
-    auto myAvatarMatrix = createMatFromQuatAndPos(myAvatar->getOrientation(), myAvatar->getPosition());
+        controller::Action::RIGHT_HAND,
-    auto worldToSensorMatrix = glm::inverse(myAvatar->getSensorToWorldMatrix());
+        controller::Action::LEFT_FOOT,
-    auto avatarToSensorMatrix = worldToSensorMatrix * myAvatarMatrix;
+        controller::Action::RIGHT_FOOT,
-    myAvatar->setHandControllerPosesInSensorFrame(leftHandPose.transform(avatarToSensorMatrix), rightHandPose.transform(avatarToSensorMatrix));
+        controller::Action::HIPS,
+        controller::Action::SPINE2,
+        controller::Action::HEAD,
+        controller::Action::LEFT_HAND_THUMB1,
+        controller::Action::LEFT_HAND_THUMB2,
+        controller::Action::LEFT_HAND_THUMB3,
+        controller::Action::LEFT_HAND_THUMB4,
+        controller::Action::LEFT_HAND_INDEX1,
+        controller::Action::LEFT_HAND_INDEX2,
+        controller::Action::LEFT_HAND_INDEX3,
+        controller::Action::LEFT_HAND_INDEX4,
+        controller::Action::LEFT_HAND_MIDDLE1,
+        controller::Action::LEFT_HAND_MIDDLE2,
+        controller::Action::LEFT_HAND_MIDDLE3,
+        controller::Action::LEFT_HAND_MIDDLE4,
+        controller::Action::LEFT_HAND_RING1,
+        controller::Action::LEFT_HAND_RING2,
+        controller::Action::LEFT_HAND_RING3,
+        controller::Action::LEFT_HAND_RING4,
+        controller::Action::LEFT_HAND_PINKY1,
+        controller::Action::LEFT_HAND_PINKY2,
+        controller::Action::LEFT_HAND_PINKY3,
+        controller::Action::LEFT_HAND_PINKY4,
+        controller::Action::RIGHT_HAND_THUMB1,
+        controller::Action::RIGHT_HAND_THUMB2,
+        controller::Action::RIGHT_HAND_THUMB3,
+        controller::Action::RIGHT_HAND_THUMB4,
+        controller::Action::RIGHT_HAND_INDEX1,
+        controller::Action::RIGHT_HAND_INDEX2,
+        controller::Action::RIGHT_HAND_INDEX3,
+        controller::Action::RIGHT_HAND_INDEX4,
+        controller::Action::RIGHT_HAND_MIDDLE1,
+        controller::Action::RIGHT_HAND_MIDDLE2,
+        controller::Action::RIGHT_HAND_MIDDLE3,
+        controller::Action::RIGHT_HAND_MIDDLE4,
+        controller::Action::RIGHT_HAND_RING1,
+        controller::Action::RIGHT_HAND_RING2,
+        controller::Action::RIGHT_HAND_RING3,
+        controller::Action::RIGHT_HAND_RING4,
+        controller::Action::RIGHT_HAND_PINKY1,
+        controller::Action::RIGHT_HAND_PINKY2,
+        controller::Action::RIGHT_HAND_PINKY3,
+        controller::Action::RIGHT_HAND_PINKY4,
+        controller::Action::LEFT_ARM,
+        controller::Action::RIGHT_ARM,
+        controller::Action::LEFT_SHOULDER,
+        controller::Action::RIGHT_SHOULDER,
+        controller::Action::LEFT_FORE_ARM,
+        controller::Action::RIGHT_FORE_ARM,
+        controller::Action::LEFT_LEG,
+        controller::Action::RIGHT_LEG,
+        controller::Action::LEFT_UP_LEG,
+        controller::Action::RIGHT_UP_LEG,
+        controller::Action::LEFT_TOE_BASE,
+        controller::Action::RIGHT_TOE_BASE
+    };
 
-    // If have previously done finger poses or there are new valid finger poses, update finger pose values. This so that if
+    // copy controller poses from userInputMapper to myAvatar.
-    // fingers are not being controlled, finger joints are not updated in MySkeletonModel.
+    glm::mat4 myAvatarMatrix = createMatFromQuatAndPos(myAvatar->getOrientation(), myAvatar->getPosition());
-    // Assumption: Finger poses are either all present and valid or not present at all; thus can test just one joint.
+    glm::mat4 worldToSensorMatrix = glm::inverse(myAvatar->getSensorToWorldMatrix());
-    MyAvatar::FingerPosesMap leftHandFingerPoses;
+    glm::mat4 avatarToSensorMatrix = worldToSensorMatrix * myAvatarMatrix;
-    if (myAvatar->getLeftHandFingerControllerPosesInSensorFrame().size() > 0
+    for (auto& action : avatarControllerActions) {
-            || userInputMapper->getPoseState(controller::Action::LEFT_HAND_THUMB1).isValid()) {
+        controller::Pose pose = userInputMapper->getPoseState(action);
-        for (int i = (int)controller::Action::LEFT_HAND_THUMB1; i <= (int)controller::Action::LEFT_HAND_PINKY4; i++) {
+        myAvatar->setControllerPoseInSensorFrame(action, pose.transform(avatarToSensorMatrix));
-            leftHandFingerPoses[i] = {
-                userInputMapper->getPoseState((controller::Action)i).transform(avatarToSensorMatrix),
-                userInputMapper->getActionName((controller::Action)i)
-            };
-        }
     }
-    MyAvatar::FingerPosesMap rightHandFingerPoses;
-    if (myAvatar->getRightHandFingerControllerPosesInSensorFrame().size() > 0
-        || userInputMapper->getPoseState(controller::Action::RIGHT_HAND_THUMB1).isValid()) {
-        for (int i = (int)controller::Action::RIGHT_HAND_THUMB1; i <= (int)controller::Action::RIGHT_HAND_PINKY4; i++) {
-            rightHandFingerPoses[i] = {
-                userInputMapper->getPoseState((controller::Action)i).transform(avatarToSensorMatrix),
-                userInputMapper->getActionName((controller::Action)i)
-            };
-        }
-    }
-    myAvatar->setFingerControllerPosesInSensorFrame(leftHandFingerPoses, rightHandFingerPoses);
-
-    controller::Pose leftFootPose = userInputMapper->getPoseState(controller::Action::LEFT_FOOT);
-    controller::Pose rightFootPose = userInputMapper->getPoseState(controller::Action::RIGHT_FOOT);
-    myAvatar->setFootControllerPosesInSensorFrame(leftFootPose.transform(avatarToSensorMatrix), rightFootPose.transform(avatarToSensorMatrix));
-
-    controller::Pose hipsPose = userInputMapper->getPoseState(controller::Action::HIPS);
-    controller::Pose spine2Pose = userInputMapper->getPoseState(controller::Action::SPINE2);
-    myAvatar->setSpineControllerPosesInSensorFrame(hipsPose.transform(avatarToSensorMatrix), spine2Pose.transform(avatarToSensorMatrix));
-
-    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/AvatarActionHold.cpp

@@ -134,9 +134,9 @@ bool AvatarActionHold::getTarget(float deltaTimeStep, glm::quat& rotation, glm::
             // fetch the hand controller pose
             controller::Pose pose;
             if (isRightHand) {
-                pose = myAvatar->getRightHandControllerPoseInWorldFrame();
+                pose = myAvatar->getControllerPoseInWorldFrame(controller::Action::RIGHT_HAND);
             } else {
-                pose = myAvatar->getLeftHandControllerPoseInWorldFrame();
+                pose = myAvatar->getControllerPoseInWorldFrame(controller::Action::LEFT_HAND);
             }
 
             if (pose.isValid()) {

interface/src/avatar/MyAvatar.cpp

@@ -429,7 +429,7 @@ void MyAvatar::update(float deltaTime) {
     }
 
 #ifdef DEBUG_DRAW_HMD_MOVING_AVERAGE
-    glm::vec3 p = transformPoint(getSensorToWorldMatrix(), getHeadControllerPoseInAvatarFrame() *
+    glm::vec3 p = transformPoint(getSensorToWorldMatrix(), getControllerPoseInAvatarFrame(controller::Pose::HEAD) *
                                  glm::vec3(_headControllerFacingMovingAverage.x, 0.0f, _headControllerFacingMovingAverage.y));
     DebugDraw::getInstance().addMarker("facing-avg", getOrientation(), p, glm::vec4(1.0f));
     p = transformPoint(getSensorToWorldMatrix(), getHMDSensorPosition() +
@@ -664,7 +664,7 @@ void MyAvatar::updateFromHMDSensorMatrix(const glm::mat4& hmdSensorMatrix) {
 
     _hmdSensorPosition = newHmdSensorPosition;
     _hmdSensorOrientation = glm::quat_cast(hmdSensorMatrix);
-    auto headPose = _headControllerPoseInSensorFrameCache.get();
+    auto headPose = getControllerPoseInSensorFrame(controller::Action::HEAD);
     if (headPose.isValid()) {
         _headControllerFacing = getFacingDir2D(headPose.rotation);
     } else {
@@ -760,37 +760,37 @@ void MyAvatar::updateFromTrackers(float deltaTime) {
 }
 
 glm::vec3 MyAvatar::getLeftHandPosition() const {
-    auto pose = getLeftHandControllerPoseInAvatarFrame();
+    auto pose = getControllerPoseInAvatarFrame(controller::Action::LEFT_HAND);
     return pose.isValid() ? pose.getTranslation() : glm::vec3(0.0f);
 }
 
 glm::vec3 MyAvatar::getRightHandPosition() const {
-    auto pose = getRightHandControllerPoseInAvatarFrame();
+    auto pose = getControllerPoseInAvatarFrame(controller::Action::RIGHT_HAND);
     return pose.isValid() ? pose.getTranslation() : glm::vec3(0.0f);
 }
 
 glm::vec3 MyAvatar::getLeftHandTipPosition() const {
     const float TIP_LENGTH = 0.3f;
-    auto pose = getLeftHandControllerPoseInAvatarFrame();
+    auto pose = getControllerPoseInAvatarFrame(controller::Action::LEFT_HAND);
     return pose.isValid() ? pose.getTranslation() * pose.getRotation() + glm::vec3(0.0f, TIP_LENGTH, 0.0f) : glm::vec3(0.0f);
 }
 
 glm::vec3 MyAvatar::getRightHandTipPosition() const {
     const float TIP_LENGTH = 0.3f;
-    auto pose = getRightHandControllerPoseInAvatarFrame();
+    auto pose = getControllerPoseInAvatarFrame(controller::Action::RIGHT_HAND);
     return pose.isValid() ? pose.getTranslation() * pose.getRotation() + glm::vec3(0.0f, TIP_LENGTH, 0.0f) : glm::vec3(0.0f);
 }
 
 controller::Pose MyAvatar::getLeftHandPose() const {
-    return getLeftHandControllerPoseInAvatarFrame();
+    return getControllerPoseInAvatarFrame(controller::Action::LEFT_HAND);
 }
 
 controller::Pose MyAvatar::getRightHandPose() const {
-    return getRightHandControllerPoseInAvatarFrame();
+    return getControllerPoseInAvatarFrame(controller::Action::RIGHT_HAND);
 }
 
 controller::Pose MyAvatar::getLeftHandTipPose() const {
-    auto pose = getLeftHandControllerPoseInAvatarFrame();
+    auto pose = getLeftHandPose();
     glm::vec3 tipTrans = getLeftHandTipPosition();
     pose.velocity += glm::cross(pose.getAngularVelocity(), pose.getTranslation() - tipTrans);
     pose.translation = tipTrans;
@@ -798,7 +798,7 @@ controller::Pose MyAvatar::getLeftHandTipPose() const {
 }
 
 controller::Pose MyAvatar::getRightHandTipPose() const {
-    auto pose = getRightHandControllerPoseInAvatarFrame();
+    auto pose = getRightHandPose();
     glm::vec3 tipTrans = getRightHandTipPosition();
     pose.velocity += glm::cross(pose.getAngularVelocity(), pose.getTranslation() - tipTrans);
     pose.translation = tipTrans;
@@ -1430,159 +1430,43 @@ void MyAvatar::rebuildCollisionShape() {
     _characterController.setLocalBoundingBox(corner, diagonal);
 }
 
-
+void MyAvatar::setControllerPoseInSensorFrame(controller::Action action, const controller::Pose& pose) {
-void MyAvatar::setHandControllerPosesInSensorFrame(const controller::Pose& left, const controller::Pose& right) {
+    std::lock_guard<std::mutex> guard(_controllerPoseMapMutex);
-    _leftHandControllerPoseInSensorFrameCache.set(left);
+    auto iter = _controllerPoseMap.find(action);
-    _rightHandControllerPoseInSensorFrameCache.set(right);
+    if (iter != _controllerPoseMap.end()) {
+        iter->second = pose;
+    } else {
+        _controllerPoseMap.insert({ action, pose });
+    }
 }
 
-controller::Pose MyAvatar::getLeftHandControllerPoseInSensorFrame() const {
+controller::Pose MyAvatar::getControllerPoseInSensorFrame(controller::Action action) const {
-    return _leftHandControllerPoseInSensorFrameCache.get();
+    std::lock_guard<std::mutex> guard(_controllerPoseMapMutex);
+    auto iter = _controllerPoseMap.find(action);
+    if (iter != _controllerPoseMap.end()) {
+        return iter->second;
+    } else {
+        return controller::Pose(); // invalid pose
+    }
 }
 
-controller::Pose MyAvatar::getRightHandControllerPoseInSensorFrame() const {
+controller::Pose MyAvatar::getControllerPoseInWorldFrame(controller::Action action) const {
-    return _rightHandControllerPoseInSensorFrameCache.get();
+    auto pose = getControllerPoseInSensorFrame(action);
+    if (pose.valid) {
+        return pose.transform(getSensorToWorldMatrix());
+    } else {
+        return controller::Pose(); // invalid pose
+    }
 }
 
-controller::Pose MyAvatar::getLeftHandControllerPoseInWorldFrame() const {
+controller::Pose MyAvatar::getControllerPoseInAvatarFrame(controller::Action action) const {
-    return _leftHandControllerPoseInSensorFrameCache.get().transform(getSensorToWorldMatrix());
+    auto pose = getControllerPoseInWorldFrame(action);
-}
+    if (pose.valid) {
-
+        glm::mat4 invAvatarMatrix = glm::inverse(createMatFromQuatAndPos(getOrientation(), getPosition()));
-controller::Pose MyAvatar::getRightHandControllerPoseInWorldFrame() const {
+        return pose.transform(invAvatarMatrix);
-    return _rightHandControllerPoseInSensorFrameCache.get().transform(getSensorToWorldMatrix());
+    } else {
-}
+        return controller::Pose(); // invalid pose
-
+    }
-controller::Pose MyAvatar::getLeftHandControllerPoseInAvatarFrame() const {
-    glm::mat4 invAvatarMatrix = glm::inverse(createMatFromQuatAndPos(getOrientation(), getPosition()));
-    return getLeftHandControllerPoseInWorldFrame().transform(invAvatarMatrix);
-}
-
-controller::Pose MyAvatar::getRightHandControllerPoseInAvatarFrame() const {
-    glm::mat4 invAvatarMatrix = glm::inverse(createMatFromQuatAndPos(getOrientation(), getPosition()));
-    return getRightHandControllerPoseInWorldFrame().transform(invAvatarMatrix);
-}
-
-void MyAvatar::setFingerControllerPosesInSensorFrame(const FingerPosesMap& left, const FingerPosesMap& right) {
-    _leftHandFingerPosesInSensorFramceCache.set(left);
-    _rightHandFingerPosesInSensorFramceCache.set(right);
-}
-
-MyAvatar::FingerPosesMap MyAvatar::getLeftHandFingerControllerPosesInSensorFrame() const {
-    return _leftHandFingerPosesInSensorFramceCache.get();
-}
-
-MyAvatar::FingerPosesMap MyAvatar::getRightHandFingerControllerPosesInSensorFrame() const {
-    return _rightHandFingerPosesInSensorFramceCache.get();
-}
-
-void MyAvatar::setFootControllerPosesInSensorFrame(const controller::Pose& left, const controller::Pose& right) {
-    _leftFootControllerPoseInSensorFrameCache.set(left);
-    _rightFootControllerPoseInSensorFrameCache.set(right);
-}
-
-controller::Pose MyAvatar::getLeftFootControllerPoseInSensorFrame() const {
-    return _leftFootControllerPoseInSensorFrameCache.get();
-}
-
-controller::Pose MyAvatar::getRightFootControllerPoseInSensorFrame() const {
-    return _rightFootControllerPoseInSensorFrameCache.get();
-}
-
-controller::Pose MyAvatar::getLeftFootControllerPoseInWorldFrame() const {
-    return _leftFootControllerPoseInSensorFrameCache.get().transform(getSensorToWorldMatrix());
-}
-
-controller::Pose MyAvatar::getRightFootControllerPoseInWorldFrame() const {
-    return _rightFootControllerPoseInSensorFrameCache.get().transform(getSensorToWorldMatrix());
-}
-
-controller::Pose MyAvatar::getLeftFootControllerPoseInAvatarFrame() const {
-    glm::mat4 invAvatarMatrix = glm::inverse(createMatFromQuatAndPos(getOrientation(), getPosition()));
-    return getLeftFootControllerPoseInWorldFrame().transform(invAvatarMatrix);
-}
-
-controller::Pose MyAvatar::getRightFootControllerPoseInAvatarFrame() const {
-    glm::mat4 invAvatarMatrix = glm::inverse(createMatFromQuatAndPos(getOrientation(), getPosition()));
-    return getRightFootControllerPoseInWorldFrame().transform(invAvatarMatrix);
-}
-
-void MyAvatar::setSpineControllerPosesInSensorFrame(const controller::Pose& hips, const controller::Pose& spine2) {
-    _hipsControllerPoseInSensorFrameCache.set(hips);
-    _spine2ControllerPoseInSensorFrameCache.set(spine2);
-}
-
-controller::Pose MyAvatar::getHipsControllerPoseInSensorFrame() const {
-    return _hipsControllerPoseInSensorFrameCache.get();
-}
-
-controller::Pose MyAvatar::getSpine2ControllerPoseInSensorFrame() const {
-    return _spine2ControllerPoseInSensorFrameCache.get();
-}
-
-controller::Pose MyAvatar::getHipsControllerPoseInWorldFrame() const {
-    return _hipsControllerPoseInSensorFrameCache.get().transform(getSensorToWorldMatrix());
-}
-
-controller::Pose MyAvatar::getSpine2ControllerPoseInWorldFrame() const {
-    return _spine2ControllerPoseInSensorFrameCache.get().transform(getSensorToWorldMatrix());
-}
-
-controller::Pose MyAvatar::getHipsControllerPoseInAvatarFrame() const {
-    glm::mat4 invAvatarMatrix = glm::inverse(createMatFromQuatAndPos(getOrientation(), getPosition()));
-    return getHipsControllerPoseInWorldFrame().transform(invAvatarMatrix);
-}
-
-controller::Pose MyAvatar::getSpine2ControllerPoseInAvatarFrame() const {
-    glm::mat4 invAvatarMatrix = glm::inverse(createMatFromQuatAndPos(getOrientation(), getPosition()));
-    return getSpine2ControllerPoseInWorldFrame().transform(invAvatarMatrix);
-}
-
-void MyAvatar::setHeadControllerPoseInSensorFrame(const controller::Pose& head) {
-    _headControllerPoseInSensorFrameCache.set(head);
-}
-
-controller::Pose MyAvatar::getHeadControllerPoseInSensorFrame() const {
-    return _headControllerPoseInSensorFrameCache.get();
-}
-
-controller::Pose MyAvatar::getHeadControllerPoseInWorldFrame() const {
-    return _headControllerPoseInSensorFrameCache.get().transform(getSensorToWorldMatrix());
-}
-
-controller::Pose MyAvatar::getHeadControllerPoseInAvatarFrame() const {
-    glm::mat4 invAvatarMatrix = glm::inverse(createMatFromQuatAndPos(getOrientation(), getPosition()));
-    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() {
@@ -1645,7 +1529,7 @@ void MyAvatar::prepareForPhysicsSimulation() {
     _characterController.setParentVelocity(parentVelocity);
 
     _characterController.setPositionAndOrientation(getPosition(), getOrientation());
-    auto headPose = getHeadControllerPoseInAvatarFrame();
+    auto headPose = getControllerPoseInAvatarFrame(controller::Action::HEAD);
     if (headPose.isValid()) {
         _follow.prePhysicsUpdate(*this, deriveBodyFromHMDSensor(), _bodySensorMatrix, hasDriveInput());
     } else {
@@ -1899,8 +1783,8 @@ void MyAvatar::postUpdate(float deltaTime) {
     }
 
     if (_enableDebugDrawHandControllers) {
-        auto leftHandPose = getLeftHandControllerPoseInWorldFrame();
+        auto leftHandPose = getControllerPoseInWorldFrame(controller::Action::LEFT_HAND);
-        auto rightHandPose = getRightHandControllerPoseInWorldFrame();
+        auto rightHandPose = getControllerPoseInWorldFrame(controller::Action::RIGHT_HAND);
 
         if (leftHandPose.isValid()) {
             DebugDraw::getInstance().addMarker("leftHandController", leftHandPose.getRotation(), leftHandPose.getTranslation(), glm::vec4(1));
@@ -2053,7 +1937,7 @@ void MyAvatar::updateOrientation(float deltaTime) {
 
     getHead()->setBasePitch(getHead()->getBasePitch() + getDriveKey(PITCH) * _pitchSpeed * deltaTime);
 
-    auto headPose = getHeadControllerPoseInAvatarFrame();
+    auto headPose = getControllerPoseInAvatarFrame(controller::Action::HEAD);
     if (headPose.isValid()) {
         glm::quat localOrientation = headPose.rotation * Quaternions::Y_180;
         // these angles will be in radians
@@ -2689,10 +2573,10 @@ bool MyAvatar::isDriveKeyDisabled(DriveKeys key) const {
 glm::mat4 MyAvatar::deriveBodyFromHMDSensor() const {
     glm::vec3 headPosition;
     glm::quat headOrientation;
-    auto headPose = getHeadControllerPoseInSensorFrame();
+    auto headPose = getControllerPoseInSensorFrame(controller::Action::HEAD);
     if (headPose.isValid()) {
-        headPosition = getHeadControllerPoseInSensorFrame().translation;
+        headPosition = headPose.translation;
-        headOrientation = getHeadControllerPoseInSensorFrame().rotation * Quaternions::Y_180;
+        headOrientation = headPose.rotation * Quaternions::Y_180;
     }
     const glm::quat headOrientationYawOnly = cancelOutRollAndPitch(headOrientation);
 
@@ -3002,19 +2886,19 @@ glm::quat MyAvatar::getAbsoluteJointRotationInObjectFrame(int index) const {
 
     switch (index) {
         case CONTROLLER_LEFTHAND_INDEX: {
-            return getLeftHandControllerPoseInAvatarFrame().getRotation();
+            return getControllerPoseInAvatarFrame(controller::Action::LEFT_HAND).getRotation();
         }
         case CONTROLLER_RIGHTHAND_INDEX: {
-            return getRightHandControllerPoseInAvatarFrame().getRotation();
+            return getControllerPoseInAvatarFrame(controller::Action::RIGHT_HAND).getRotation();
         }
         case CAMERA_RELATIVE_CONTROLLER_LEFTHAND_INDEX: {
-            auto pose = _leftHandControllerPoseInSensorFrameCache.get();
+            auto pose = getControllerPoseInSensorFrame(controller::Action::LEFT_HAND);
             glm::mat4 controllerSensorMatrix = createMatFromQuatAndPos(pose.rotation, pose.translation);
             glm::mat4 result = computeCameraRelativeHandControllerMatrix(controllerSensorMatrix);
             return glmExtractRotation(result);
         }
         case CAMERA_RELATIVE_CONTROLLER_RIGHTHAND_INDEX: {
-            auto pose = _rightHandControllerPoseInSensorFrameCache.get();
+            auto pose = getControllerPoseInSensorFrame(controller::Action::RIGHT_HAND);
             glm::mat4 controllerSensorMatrix = createMatFromQuatAndPos(pose.rotation, pose.translation);
             glm::mat4 result = computeCameraRelativeHandControllerMatrix(controllerSensorMatrix);
             return glmExtractRotation(result);
@@ -3039,19 +2923,19 @@ glm::vec3 MyAvatar::getAbsoluteJointTranslationInObjectFrame(int index) const {
 
     switch (index) {
         case CONTROLLER_LEFTHAND_INDEX: {
-            return getLeftHandControllerPoseInAvatarFrame().getTranslation();
+            return getControllerPoseInAvatarFrame(controller::Action::LEFT_HAND).getTranslation();
         }
         case CONTROLLER_RIGHTHAND_INDEX: {
-            return getRightHandControllerPoseInAvatarFrame().getTranslation();
+            return getControllerPoseInAvatarFrame(controller::Action::RIGHT_HAND).getTranslation();
         }
         case CAMERA_RELATIVE_CONTROLLER_LEFTHAND_INDEX: {
-            auto pose = _leftHandControllerPoseInSensorFrameCache.get();
+            auto pose = getControllerPoseInSensorFrame(controller::Action::LEFT_HAND);
             glm::mat4 controllerSensorMatrix = createMatFromQuatAndPos(pose.rotation, pose.translation);
             glm::mat4 result = computeCameraRelativeHandControllerMatrix(controllerSensorMatrix);
             return extractTranslation(result);
         }
         case CAMERA_RELATIVE_CONTROLLER_RIGHTHAND_INDEX: {
-            auto pose = _rightHandControllerPoseInSensorFrameCache.get();
+            auto pose = getControllerPoseInSensorFrame(controller::Action::RIGHT_HAND);
             glm::mat4 controllerSensorMatrix = createMatFromQuatAndPos(pose.rotation, pose.translation);
             glm::mat4 result = computeCameraRelativeHandControllerMatrix(controllerSensorMatrix);
             return extractTranslation(result);

interface/src/avatar/MyAvatar.h

@@ -473,49 +473,12 @@ public:
 
     virtual void rebuildCollisionShape() override;
 
-    void setHandControllerPosesInSensorFrame(const controller::Pose& left, const controller::Pose& right);
-    controller::Pose getLeftHandControllerPoseInSensorFrame() const;
-    controller::Pose getRightHandControllerPoseInSensorFrame() const;
-    controller::Pose getLeftHandControllerPoseInWorldFrame() const;
-    controller::Pose getRightHandControllerPoseInWorldFrame() const;
-    controller::Pose getLeftHandControllerPoseInAvatarFrame() const;
-    controller::Pose getRightHandControllerPoseInAvatarFrame() const;
-
-    typedef std::map<int, std::pair<controller::Pose, QString>> FingerPosesMap;
-    void setFingerControllerPosesInSensorFrame(const FingerPosesMap& left, const FingerPosesMap& right);
-    FingerPosesMap getLeftHandFingerControllerPosesInSensorFrame() const;
-    FingerPosesMap getRightHandFingerControllerPosesInSensorFrame() const;
-
-    void setFootControllerPosesInSensorFrame(const controller::Pose& left, const controller::Pose& right);
-    controller::Pose getLeftFootControllerPoseInSensorFrame() const;
-    controller::Pose getRightFootControllerPoseInSensorFrame() const;
-    controller::Pose getLeftFootControllerPoseInWorldFrame() const;
-    controller::Pose getRightFootControllerPoseInWorldFrame() const;
-    controller::Pose getLeftFootControllerPoseInAvatarFrame() const;
-    controller::Pose getRightFootControllerPoseInAvatarFrame() const;
-
-    void setSpineControllerPosesInSensorFrame(const controller::Pose& hips, const controller::Pose& spine2);
-    controller::Pose getHipsControllerPoseInSensorFrame() const;
-    controller::Pose getSpine2ControllerPoseInSensorFrame() const;
-    controller::Pose getHipsControllerPoseInWorldFrame() const;
-    controller::Pose getSpine2ControllerPoseInWorldFrame() const;
-    controller::Pose getHipsControllerPoseInAvatarFrame() const;
-    controller::Pose getSpine2ControllerPoseInAvatarFrame() const;
-
-    void setHeadControllerPoseInSensorFrame(const controller::Pose& head);
-    controller::Pose getHeadControllerPoseInSensorFrame() const;
-    controller::Pose getHeadControllerPoseInWorldFrame() const;
-    controller::Pose getHeadControllerPoseInAvatarFrame() const;
     const glm::vec2& getHeadControllerFacingMovingAverage() const { return _headControllerFacingMovingAverage; }
 
-
+    void setControllerPoseInSensorFrame(controller::Action action, const controller::Pose& pose);
-    void setArmControllerPosesInSensorFrame(const controller::Pose& left, const controller::Pose& right);
+    controller::Pose getControllerPoseInSensorFrame(controller::Action action) const;
-    controller::Pose getLeftArmControllerPoseInSensorFrame() const;
+    controller::Pose getControllerPoseInWorldFrame(controller::Action action) const;
-    controller::Pose getRightArmControllerPoseInSensorFrame() const;
+    controller::Pose getControllerPoseInAvatarFrame(controller::Action action) const;
-    controller::Pose getLeftArmControllerPoseInWorldFrame() const;
-    controller::Pose getRightArmControllerPoseInWorldFrame() const;
-    controller::Pose getLeftArmControllerPoseInAvatarFrame() const;
-    controller::Pose getRightArmControllerPoseInAvatarFrame() const;
 
     bool hasDriveInput() const;
 
@@ -804,18 +767,9 @@ private:
     bool _hoverReferenceCameraFacingIsCaptured { false };
     glm::vec3 _hoverReferenceCameraFacing { 0.0f, 0.0f, -1.0f }; // hmd sensor space
 
-    // These are stored in SENSOR frame
+    // all poses are in sensor-frame
-    ThreadSafeValueCache<controller::Pose> _leftHandControllerPoseInSensorFrameCache { controller::Pose() };
+    std::map<controller::Action, controller::Pose> _controllerPoseMap;
-    ThreadSafeValueCache<controller::Pose> _rightHandControllerPoseInSensorFrameCache { controller::Pose() };
+    mutable std::mutex _controllerPoseMapMutex;
-    ThreadSafeValueCache<FingerPosesMap> _leftHandFingerPosesInSensorFramceCache { };
-    ThreadSafeValueCache<FingerPosesMap> _rightHandFingerPosesInSensorFramceCache { };
-    ThreadSafeValueCache<controller::Pose> _leftFootControllerPoseInSensorFrameCache { controller::Pose() };
-    ThreadSafeValueCache<controller::Pose> _rightFootControllerPoseInSensorFrameCache { controller::Pose() };
-    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;
     AnimPose _prePhysicsRoomPose;

interface/src/avatar/MyHead.cpp

@@ -34,7 +34,7 @@ glm::quat MyHead::getHeadOrientation() const {
     // always the same.
 
     MyAvatar* myAvatar = static_cast<MyAvatar*>(_owningAvatar);
-    auto headPose = myAvatar->getHeadControllerPoseInWorldFrame();
+    auto headPose = myAvatar->getControllerPoseInWorldFrame(controller::Action::HEAD);
     if (headPose.isValid()) {
         return headPose.rotation * Quaternions::Y_180;
     }

interface/src/avatar/MySkeletonModel.cpp

@@ -46,105 +46,82 @@ void MySkeletonModel::updateRig(float deltaTime, glm::mat4 parentTransform) {
     }
 
     MyAvatar* myAvatar = static_cast<MyAvatar*>(_owningAvatar);
+    assert(myAvatar);
 
     Rig::ControllerParameters params;
 
     AnimPose avatarToRigPose(glm::vec3(1.0f), Quaternions::Y_180, glm::vec3(0.0f));
 
     // input action is the highest priority source for head orientation.
-    auto avatarHeadPose = myAvatar->getHeadControllerPoseInAvatarFrame();
+    auto avatarHeadPose = myAvatar->getControllerPoseInAvatarFrame(controller::Action::HEAD);
     if (avatarHeadPose.isValid()) {
         AnimPose pose(avatarHeadPose.getRotation(), avatarHeadPose.getTranslation());
-        params.controllerPoses[Rig::ControllerType_Head] = avatarToRigPose * pose;
+        params.primaryControllerPoses[Rig::PrimaryControllerType_Head] = avatarToRigPose * pose;
-        params.controllerActiveFlags[Rig::ControllerType_Head] = true;
+        params.primaryControllerActiveFlags[Rig::PrimaryControllerType_Head] = true;
     } else {
         // even though full head IK is disabled, the rig still needs the head orientation to rotate the head up and
         // down in desktop mode.
         // preMult 180 is necessary to convert from avatar to rig coordinates.
         // postMult 180 is necessary to convert head from -z forward to z forward.
         glm::quat headRot = Quaternions::Y_180 * head->getFinalOrientationInLocalFrame() * Quaternions::Y_180;
-        params.controllerPoses[Rig::ControllerType_Head] = AnimPose(glm::vec3(1.0f), headRot, glm::vec3(0.0f));
+        params.primaryControllerPoses[Rig::PrimaryControllerType_Head] = AnimPose(glm::vec3(1.0f), headRot, glm::vec3(0.0f));
-        params.controllerActiveFlags[Rig::ControllerType_Head] = false;
+        params.primaryControllerActiveFlags[Rig::PrimaryControllerType_Head] = false;
     }
 
-    auto avatarHipsPose = myAvatar->getHipsControllerPoseInAvatarFrame();
+    //
-    if (avatarHipsPose.isValid()) {
+    // primary controller poses, control IK targets directly.
-        AnimPose pose(avatarHipsPose.getRotation(), avatarHipsPose.getTranslation());
+    //
-        params.controllerPoses[Rig::ControllerType_Hips] = avatarToRigPose * pose;
+
-        params.controllerActiveFlags[Rig::ControllerType_Hips] = true;
+    static const std::vector<std::pair<controller::Action, Rig::PrimaryControllerType>> primaryControllers = {
-    } else {
+        { controller::Action::LEFT_HAND, Rig::PrimaryControllerType_LeftHand },
-        params.controllerPoses[Rig::ControllerType_Hips] = AnimPose::identity;
+        { controller::Action::RIGHT_HAND, Rig::PrimaryControllerType_RightHand },
-        params.controllerActiveFlags[Rig::ControllerType_Hips] = false;
+        { controller::Action::HIPS, Rig::PrimaryControllerType_Hips },
+        { controller::Action::LEFT_FOOT, Rig::PrimaryControllerType_LeftFoot },
+        { controller::Action::RIGHT_FOOT, Rig::PrimaryControllerType_RightFoot },
+        { controller::Action::SPINE2, Rig::PrimaryControllerType_Spine2 }
+    };
+
+    for (auto pair : primaryControllers) {
+        auto controllerPose = myAvatar->getControllerPoseInAvatarFrame(pair.first);
+        if (controllerPose.isValid()) {
+            AnimPose pose(controllerPose.getRotation(), controllerPose.getTranslation());
+            params.primaryControllerPoses[pair.second] = avatarToRigPose * pose;
+            params.primaryControllerActiveFlags[pair.second] = true;
+        } else {
+            params.primaryControllerPoses[pair.second] = AnimPose::identity;
+            params.primaryControllerActiveFlags[pair.second] = false;
+        }
     }
 
-    auto avatarSpine2Pose = myAvatar->getSpine2ControllerPoseInAvatarFrame();
+    //
-    if (avatarSpine2Pose.isValid()) {
+    // secondary controller poses, influence the pose of the skeleton indirectly.
-        AnimPose pose(avatarSpine2Pose.getRotation(), avatarSpine2Pose.getTranslation());
+    //
-        params.controllerPoses[Rig::ControllerType_Spine2] = avatarToRigPose * pose;
-        params.controllerActiveFlags[Rig::ControllerType_Spine2] = true;
-    } else {
-        params.controllerPoses[Rig::ControllerType_Spine2] = AnimPose::identity;
-        params.controllerActiveFlags[Rig::ControllerType_Spine2] = false;
-    }
 
-    auto avatarRightArmPose = myAvatar->getRightArmControllerPoseInAvatarFrame();
+    static const std::vector<std::pair<controller::Action, Rig::SecondaryControllerType>> secondaryControllers = {
-    if (avatarRightArmPose.isValid()) {
+        { controller::Action::LEFT_SHOULDER, Rig::SecondaryControllerType_LeftShoulder },
-        AnimPose pose(avatarRightArmPose.getRotation(), avatarRightArmPose.getTranslation());
+        { controller::Action::RIGHT_SHOULDER, Rig::SecondaryControllerType_RightShoulder },
-        params.controllerPoses[Rig::ControllerType_RightArm] = avatarToRigPose * pose;
+        { controller::Action::LEFT_ARM, Rig::SecondaryControllerType_LeftArm },
-        params.controllerActiveFlags[Rig::ControllerType_RightArm] = true;
+        { controller::Action::RIGHT_ARM, Rig::SecondaryControllerType_RightArm },
-    } else {
+        { controller::Action::LEFT_FORE_ARM, Rig::SecondaryControllerType_LeftForeArm },
-        params.controllerPoses[Rig::ControllerType_RightArm] = AnimPose::identity;
+        { controller::Action::RIGHT_FORE_ARM, Rig::SecondaryControllerType_RightForeArm },
-        params.controllerActiveFlags[Rig::ControllerType_RightArm] = false;
+        { controller::Action::LEFT_UP_LEG, Rig::SecondaryControllerType_LeftUpLeg },
-    }
+        { controller::Action::RIGHT_UP_LEG, Rig::SecondaryControllerType_RightUpLeg },
-    
+        { controller::Action::LEFT_LEG, Rig::SecondaryControllerType_LeftLeg },
-    auto avatarLeftArmPose = myAvatar->getLeftArmControllerPoseInAvatarFrame();
+        { controller::Action::RIGHT_LEG, Rig::SecondaryControllerType_RightLeg },
-    if (avatarLeftArmPose.isValid()) {
+        { controller::Action::LEFT_TOE_BASE, Rig::SecondaryControllerType_LeftToeBase },
-        AnimPose pose(avatarLeftArmPose.getRotation(), avatarLeftArmPose.getTranslation());
+        { controller::Action::RIGHT_TOE_BASE, Rig::SecondaryControllerType_RightToeBase }
-        params.controllerPoses[Rig::ControllerType_LeftArm] = avatarToRigPose * pose;
+    };
-        params.controllerActiveFlags[Rig::ControllerType_LeftArm] = true;
-    } else {
-        params.controllerPoses[Rig::ControllerType_LeftArm] = AnimPose::identity;
-        params.controllerActiveFlags[Rig::ControllerType_LeftArm] = false;
-    }
 
-    auto avatarLeftHandPose = myAvatar->getLeftHandControllerPoseInAvatarFrame();
+    for (auto pair : secondaryControllers) {
-    if (avatarLeftHandPose.isValid()) {
+        auto controllerPose = myAvatar->getControllerPoseInAvatarFrame(pair.first);
-        AnimPose pose(avatarLeftHandPose.getRotation(), avatarLeftHandPose.getTranslation());
+        if (controllerPose.isValid()) {
-        params.controllerPoses[Rig::ControllerType_LeftHand] = avatarToRigPose * pose;
+            AnimPose pose(controllerPose.getRotation(), controllerPose.getTranslation());
-        params.controllerActiveFlags[Rig::ControllerType_LeftHand] = true;
+            params.secondaryControllerPoses[pair.second] = avatarToRigPose * pose;
-    } else {
+            params.secondaryControllerActiveFlags[pair.second] = true;
-        params.controllerPoses[Rig::ControllerType_LeftHand] = AnimPose::identity;
+        } else {
-        params.controllerActiveFlags[Rig::ControllerType_LeftHand] = false;
+            params.secondaryControllerPoses[pair.second] = AnimPose::identity;
-    }
+            params.secondaryControllerActiveFlags[pair.second] = false;
-
+        }
-    auto avatarRightHandPose = myAvatar->getRightHandControllerPoseInAvatarFrame();
-    if (avatarRightHandPose.isValid()) {
-        AnimPose pose(avatarRightHandPose.getRotation(), avatarRightHandPose.getTranslation());
-        params.controllerPoses[Rig::ControllerType_RightHand] = avatarToRigPose * pose;
-        params.controllerActiveFlags[Rig::ControllerType_RightHand] = true;
-    } else {
-        params.controllerPoses[Rig::ControllerType_RightHand] = AnimPose::identity;
-        params.controllerActiveFlags[Rig::ControllerType_RightHand] = false;
-    }
-
-    auto avatarLeftFootPose = myAvatar->getLeftFootControllerPoseInAvatarFrame();
-    if (avatarLeftFootPose.isValid()) {
-        AnimPose pose(avatarLeftFootPose.getRotation(), avatarLeftFootPose.getTranslation());
-        params.controllerPoses[Rig::ControllerType_LeftFoot] = avatarToRigPose * pose;
-        params.controllerActiveFlags[Rig::ControllerType_LeftFoot] = true;
-    } else {
-        params.controllerPoses[Rig::ControllerType_LeftFoot] = AnimPose::identity;
-        params.controllerActiveFlags[Rig::ControllerType_LeftFoot] = false;
-    }
-
-    auto avatarRightFootPose = myAvatar->getRightFootControllerPoseInAvatarFrame();
-    if (avatarRightFootPose.isValid()) {
-        AnimPose pose(avatarRightFootPose.getRotation(), avatarRightFootPose.getTranslation());
-        params.controllerPoses[Rig::ControllerType_RightFoot] = avatarToRigPose * pose;
-        params.controllerActiveFlags[Rig::ControllerType_RightFoot] = true;
-    } else {
-        params.controllerPoses[Rig::ControllerType_RightFoot] = AnimPose::identity;
-        params.controllerActiveFlags[Rig::ControllerType_RightFoot] = false;
     }
 
     params.bodyCapsuleRadius = myAvatar->getCharacterController()->getCapsuleRadius();
@@ -175,49 +152,106 @@ void MySkeletonModel::updateRig(float deltaTime, glm::mat4 parentTransform) {
 
     _rig.updateFromEyeParameters(eyeParams);
 
-    updateFingers(myAvatar->getLeftHandFingerControllerPosesInSensorFrame());
+    updateFingers();
-    updateFingers(myAvatar->getRightHandFingerControllerPosesInSensorFrame());
 }
 
 
-void MySkeletonModel::updateFingers(const MyAvatar::FingerPosesMap& fingerPoses) {
+void MySkeletonModel::updateFingers() {
-    // Assumes that finger poses are kept in order in the poses map.
-
-    if (fingerPoses.size() == 0) {
-        return;
-}
-
-    auto posesMapItr = fingerPoses.begin();
-
-    bool isLeftHand = posesMapItr->first < (int)controller::Action::RIGHT_HAND_THUMB1;
 
     MyAvatar* myAvatar = static_cast<MyAvatar*>(_owningAvatar);
-    auto handPose = isLeftHand 
-        ? myAvatar->getLeftHandControllerPoseInSensorFrame() 
-        : myAvatar->getRightHandControllerPoseInSensorFrame();
-    auto handJointRotation = handPose.getRotation();
 
-    bool isHandValid = handPose.isValid();
+    static std::vector<std::vector<std::pair<controller::Action, QString>>> fingerChains = {
-    bool isFingerValid = false;
+        {
-    glm::quat previousJointRotation;
+            { controller::Action::LEFT_HAND, "LeftHand" },
-
+            { controller::Action::LEFT_HAND_THUMB1, "LeftHandThumb1" },
-    while (posesMapItr != fingerPoses.end()) {
+            { controller::Action::LEFT_HAND_THUMB2, "LeftHandThumb2" },
-        auto jointName = posesMapItr->second.second;
+            { controller::Action::LEFT_HAND_THUMB3, "LeftHandThumb3" },
-        if (isHandValid && jointName.right(1) == "1") {
+            { controller::Action::LEFT_HAND_THUMB4, "LeftHandThumb4" }
-            isFingerValid = posesMapItr->second.first.isValid();
+        },
-            previousJointRotation = handJointRotation;
+        {
+            { controller::Action::LEFT_HAND, "LeftHand" },
+            { controller::Action::LEFT_HAND_INDEX1, "LeftHandIndex1" },
+            { controller::Action::LEFT_HAND_INDEX2, "LeftHandIndex2" },
+            { controller::Action::LEFT_HAND_INDEX3, "LeftHandIndex3" },
+            { controller::Action::LEFT_HAND_INDEX4, "LeftHandIndex4" }
+        },
+        {
+            { controller::Action::LEFT_HAND, "LeftHand" },
+            { controller::Action::LEFT_HAND_MIDDLE1, "LeftHandMiddle1" },
+            { controller::Action::LEFT_HAND_MIDDLE2, "LeftHandMiddle2" },
+            { controller::Action::LEFT_HAND_MIDDLE3, "LeftHandMiddle3" },
+            { controller::Action::LEFT_HAND_MIDDLE4, "LeftHandMiddle4" }
+        },
+        {
+            { controller::Action::LEFT_HAND, "LeftHand" },
+            { controller::Action::LEFT_HAND_RING1, "LeftHandRing1" },
+            { controller::Action::LEFT_HAND_RING2, "LeftHandRing2" },
+            { controller::Action::LEFT_HAND_RING3, "LeftHandRing3" },
+            { controller::Action::LEFT_HAND_RING4, "LeftHandRing4" }
+        },
+        {
+            { controller::Action::LEFT_HAND, "LeftHand" },
+            { controller::Action::LEFT_HAND_PINKY1, "LeftHandPinky1" },
+            { controller::Action::LEFT_HAND_PINKY2, "LeftHandPinky2" },
+            { controller::Action::LEFT_HAND_PINKY3, "LeftHandPinky3" },
+            { controller::Action::LEFT_HAND_PINKY4, "LeftHandPinky4" }
+        },
+        {
+            { controller::Action::RIGHT_HAND, "RightHand" },
+            { controller::Action::RIGHT_HAND_THUMB1, "RightHandThumb1" },
+            { controller::Action::RIGHT_HAND_THUMB2, "RightHandThumb2" },
+            { controller::Action::RIGHT_HAND_THUMB3, "RightHandThumb3" },
+            { controller::Action::RIGHT_HAND_THUMB4, "RightHandThumb4" }
+        },
+        {
+            { controller::Action::RIGHT_HAND, "RightHand" },
+            { controller::Action::RIGHT_HAND_INDEX1, "RightHandIndex1" },
+            { controller::Action::RIGHT_HAND_INDEX2, "RightHandIndex2" },
+            { controller::Action::RIGHT_HAND_INDEX3, "RightHandIndex3" },
+            { controller::Action::RIGHT_HAND_INDEX4, "RightHandIndex4" }
+        },
+        {
+            { controller::Action::RIGHT_HAND, "RightHand" },
+            { controller::Action::RIGHT_HAND_MIDDLE1, "RightHandMiddle1" },
+            { controller::Action::RIGHT_HAND_MIDDLE2, "RightHandMiddle2" },
+            { controller::Action::RIGHT_HAND_MIDDLE3, "RightHandMiddle3" },
+            { controller::Action::RIGHT_HAND_MIDDLE4, "RightHandMiddle4" }
+        },
+        {
+            { controller::Action::RIGHT_HAND, "RightHand" },
+            { controller::Action::RIGHT_HAND_RING1, "RightHandRing1" },
+            { controller::Action::RIGHT_HAND_RING2, "RightHandRing2" },
+            { controller::Action::RIGHT_HAND_RING3, "RightHandRing3" },
+            { controller::Action::RIGHT_HAND_RING4, "RightHandRing4" }
+        },
+        {
+            { controller::Action::RIGHT_HAND, "RightHand" },
+            { controller::Action::RIGHT_HAND_PINKY1, "RightHandPinky1" },
+            { controller::Action::RIGHT_HAND_PINKY2, "RightHandPinky2" },
+            { controller::Action::RIGHT_HAND_PINKY3, "RightHandPinky3" },
+            { controller::Action::RIGHT_HAND_PINKY4, "RightHandPinky4" }
         }
+    };
 
-        if (isHandValid && isFingerValid) {
+    const float CONTROLLER_PRIORITY = 2.0f;
-            auto thisJointRotation = posesMapItr->second.first.getRotation();
+
-            const float CONTROLLER_PRIORITY = 2.0f;
+    for (auto& chain : fingerChains) {
-            _rig.setJointRotation(_rig.indexOfJoint(jointName), true, glm::inverse(previousJointRotation) * thisJointRotation, 
+        glm::quat prevAbsRot = Quaternions::IDENTITY;
-                CONTROLLER_PRIORITY);
+        for (auto& link : chain) {
-            previousJointRotation = thisJointRotation;
+            int index = _rig.indexOfJoint(link.second);
-        } else {
+            if (index >= 0) {
-            _rig.clearJointAnimationPriority(_rig.indexOfJoint(jointName));
+                auto pose = myAvatar->getControllerPoseInSensorFrame(link.first);
+                if (pose.valid) {
+                    glm::quat relRot = glm::inverse(prevAbsRot) * pose.getRotation();
+                    // only set the rotation for the finger joints, not the hands.
+                    if (link.first != controller::Action::LEFT_HAND && link.first != controller::Action::RIGHT_HAND) {
+                        _rig.setJointRotation(index, true, relRot, CONTROLLER_PRIORITY);
+                    }
+                    prevAbsRot = pose.getRotation();
+                } else {
+                    _rig.clearJointAnimationPriority(index);
+                }
+            }
         }
-
-        posesMapItr++;
     }
 }

interface/src/avatar/MySkeletonModel.h

@@ -24,7 +24,7 @@ public:
     void updateRig(float deltaTime, glm::mat4 parentTransform) override;
 
 private:
-    void updateFingers(const MyAvatar::FingerPosesMap& fingerPoses);
+    void updateFingers();
 };
 
 #endif // hifi_MySkeletonModel_h

interface/src/ui/overlays/ContextOverlayInterface.cpp

@@ -257,15 +257,23 @@ void ContextOverlayInterface::openMarketplace() {
 }
 
 void ContextOverlayInterface::enableEntityHighlight(const EntityItemID& entityItemID) {
-    if (!qApp->getEntities()->getTree()->findEntityByEntityItemID(entityItemID)->getShouldHighlight()) {
+    auto entityTree = qApp->getEntities()->getTree();
-        qCDebug(context_overlay) << "Setting 'shouldHighlight' to 'true' for Entity ID:" << entityItemID;
+    entityTree->withReadLock([&] {
-        qApp->getEntities()->getTree()->findEntityByEntityItemID(entityItemID)->setShouldHighlight(true);
+        auto entityItem = entityTree->findEntityByEntityItemID(entityItemID);
-    }
+        if ((entityItem != NULL) && !entityItem->getShouldHighlight()) {
+            qCDebug(context_overlay) << "Setting 'shouldHighlight' to 'true' for Entity ID:" << entityItemID;
+            entityItem->setShouldHighlight(true);
+        }
+    });
 }
 
 void ContextOverlayInterface::disableEntityHighlight(const EntityItemID& entityItemID) {
-    if (qApp->getEntities()->getTree()->findEntityByEntityItemID(entityItemID)->getShouldHighlight()) {
+    auto entityTree = qApp->getEntities()->getTree();
-        qCDebug(context_overlay) << "Setting 'shouldHighlight' to 'false' for Entity ID:" << entityItemID;
+    entityTree->withReadLock([&] {
-        qApp->getEntities()->getTree()->findEntityByEntityItemID(entityItemID)->setShouldHighlight(false);
+        auto entityItem = entityTree->findEntityByEntityItemID(entityItemID);
-    }
+        if ((entityItem != NULL) && entityItem->getShouldHighlight()) {
+            qCDebug(context_overlay) << "Setting 'shouldHighlight' to 'false' for Entity ID:" << entityItemID;
+            entityItem->setShouldHighlight(false);
+        }
+    });
 }

libraries/animation/src/AnimInverseKinematics.cpp

@@ -23,12 +23,13 @@
 #include "CubicHermiteSpline.h"
 #include "AnimUtil.h"
 
+static const int MAX_TARGET_MARKERS = 30;
 static const float JOINT_CHAIN_INTERP_TIME = 0.25f;
 
 static void lookupJointInfo(const AnimInverseKinematics::JointChainInfo& jointChainInfo,
-                                 int indexA, int indexB,
+                            int indexA, int indexB,
-                                 const AnimInverseKinematics::JointInfo** jointInfoA,
+                            const AnimInverseKinematics::JointInfo** jointInfoA,
-                                 const AnimInverseKinematics::JointInfo** jointInfoB) {
+                            const AnimInverseKinematics::JointInfo** jointInfoB) {
     *jointInfoA = nullptr;
     *jointInfoB = nullptr;
     for (size_t i = 0; i < jointChainInfo.jointInfoVec.size(); i++) {
@@ -97,6 +98,12 @@ AnimInverseKinematics::~AnimInverseKinematics() {
     _rotationAccumulators.clear();
     _translationAccumulators.clear();
     _targetVarVec.clear();
+
+    // remove markers
+    for (int i = 0; i < MAX_TARGET_MARKERS; i++) {
+        QString name = QString("ikTarget%1").arg(i);
+        DebugDraw::getInstance().removeMyAvatarMarker(name);
+    }
 }
 
 void AnimInverseKinematics::loadDefaultPoses(const AnimPoseVec& poses) {
@@ -1015,19 +1022,30 @@ const AnimPoseVec& AnimInverseKinematics::overlay(const AnimVariantMap& animVars
                 // debug render ik targets
                 if (context.getEnableDebugDrawIKTargets()) {
                     const vec4 WHITE(1.0f);
+                    const vec4 GREEN(0.0f, 1.0f, 0.0f, 1.0f);
                     glm::mat4 rigToAvatarMat = createMatFromQuatAndPos(Quaternions::Y_180, glm::vec3());
+                    int targetNum = 0;
 
                     for (auto& target : targets) {
                         glm::mat4 geomTargetMat = createMatFromQuatAndPos(target.getRotation(), target.getTranslation());
                         glm::mat4 avatarTargetMat = rigToAvatarMat * context.getGeometryToRigMatrix() * geomTargetMat;
 
-                        QString name = QString("ikTarget%1").arg(target.getIndex());
+                        QString name = QString("ikTarget%1").arg(targetNum);
                         DebugDraw::getInstance().addMyAvatarMarker(name, glmExtractRotation(avatarTargetMat), extractTranslation(avatarTargetMat), WHITE);
+                        targetNum++;
+                    }
+
+                    // draw secondary ik targets
+                    for (auto& iter : _secondaryTargetsInRigFrame) {
+                        glm::mat4 avatarTargetMat = rigToAvatarMat * (glm::mat4)iter.second;
+                        QString name = QString("ikTarget%1").arg(targetNum);
+                        DebugDraw::getInstance().addMyAvatarMarker(name, glmExtractRotation(avatarTargetMat), extractTranslation(avatarTargetMat), GREEN);
+                        targetNum++;
                     }
                 } else if (context.getEnableDebugDrawIKTargets() != _previousEnableDebugIKTargets) {
                     // remove markers if they were added last frame.
-                    for (auto& target : targets) {
+                    for (int i = 0; i < MAX_TARGET_MARKERS; i++) {
-                        QString name = QString("ikTarget%1").arg(target.getIndex());
+                        QString name = QString("ikTarget%1").arg(i);
                         DebugDraw::getInstance().removeMyAvatarMarker(name);
                     }
                 }
@@ -1038,7 +1056,9 @@ const AnimPoseVec& AnimInverseKinematics::overlay(const AnimVariantMap& animVars
             {
                 PROFILE_RANGE_EX(simulation_animation, "ik/ccd", 0xffff00ff, 0);
 
+                setSecondaryTargets(context);
                 preconditionRelativePosesToAvoidLimbLock(context, targets);
+
                 solve(context, targets, dt, jointChainInfoVec);
             }
 
@@ -1125,6 +1145,22 @@ void AnimInverseKinematics::clearIKJointLimitHistory() {
     }
 }
 
+void AnimInverseKinematics::setSecondaryTargetInRigFrame(int jointIndex, const AnimPose& pose) {
+    auto iter = _secondaryTargetsInRigFrame.find(jointIndex);
+    if (iter != _secondaryTargetsInRigFrame.end()) {
+        iter->second = pose;
+    } else {
+        _secondaryTargetsInRigFrame.insert({ jointIndex, pose });
+    }
+}
+
+void AnimInverseKinematics::clearSecondaryTarget(int jointIndex) {
+    auto iter = _secondaryTargetsInRigFrame.find(jointIndex);
+    if (iter != _secondaryTargetsInRigFrame.end()) {
+        _secondaryTargetsInRigFrame.erase(iter);
+    }
+}
+
 RotationConstraint* AnimInverseKinematics::getConstraint(int index) const {
     RotationConstraint* constraint = nullptr;
     std::map<int, RotationConstraint*>::const_iterator constraintItr = _constraints.find(index);
@@ -1575,7 +1611,7 @@ void AnimInverseKinematics::debugDrawRelativePoses(const AnimContext& context) c
     const vec4 GREEN(0.0f, 1.0f, 0.0f, 1.0f);
     const vec4 BLUE(0.0f, 0.0f, 1.0f, 1.0f);
     const vec4 GRAY(0.2f, 0.2f, 0.2f, 1.0f);
-    const float AXIS_LENGTH = 2.0f; // cm
+    const float AXIS_LENGTH = 10.0f; // cm
 
     // draw each pose
     for (int i = 0; i < (int)poses.size(); i++) {
@@ -1605,8 +1641,10 @@ void AnimInverseKinematics::debugDrawIKChain(const JointChainInfo& jointChainInf
     // copy debug joint rotations into the relative poses
     for (size_t i = 0; i < jointChainInfo.jointInfoVec.size(); i++) {
         const JointInfo& info = jointChainInfo.jointInfoVec[i];
-        poses[info.jointIndex].rot() = info.rot;
+        if (info.jointIndex != _hipsIndex) {
-        poses[info.jointIndex].trans() = info.trans;
+            poses[info.jointIndex].rot() = info.rot;
+            poses[info.jointIndex].trans() = info.trans;
+        }
     }
 
     // convert relative poses to absolute
@@ -1825,6 +1863,59 @@ void AnimInverseKinematics::preconditionRelativePosesToAvoidLimbLock(const AnimC
     }
 }
 
+// overwrites _relativePoses with secondary poses.
+void AnimInverseKinematics::setSecondaryTargets(const AnimContext& context) {
+
+    if (_secondaryTargetsInRigFrame.empty()) {
+        return;
+    }
+
+    // special case for arm secondary poses.
+    // determine if shoulder joint should look-at position of arm joint.
+    bool shoulderShouldLookAtArm = false;
+    const int leftArmIndex = _skeleton->nameToJointIndex("LeftArm");
+    const int rightArmIndex = _skeleton->nameToJointIndex("RightArm");
+    const int leftShoulderIndex = _skeleton->nameToJointIndex("LeftShoulder");
+    const int rightShoulderIndex = _skeleton->nameToJointIndex("RightShoulder");
+    for (auto& iter : _secondaryTargetsInRigFrame) {
+        if (iter.first == leftShoulderIndex || iter.first == rightShoulderIndex) {
+            shoulderShouldLookAtArm = true;
+            break;
+        }
+    }
+
+    AnimPose rigToGeometryPose = AnimPose(glm::inverse(context.getGeometryToRigMatrix()));
+    for (auto& iter : _secondaryTargetsInRigFrame) {
+        AnimPose absPose = rigToGeometryPose * iter.second;
+        absPose.scale() = glm::vec3(1.0f);
+
+        AnimPose parentAbsPose;
+        int parentIndex = _skeleton->getParentIndex(iter.first);
+        if (parentIndex >= 0) {
+            parentAbsPose = _skeleton->getAbsolutePose(parentIndex, _relativePoses);
+        }
+
+        // if parent should "look-at" child joint position.
+        if (shoulderShouldLookAtArm && (iter.first == leftArmIndex || iter.first == rightArmIndex)) {
+
+            AnimPose grandParentAbsPose;
+            int grandParentIndex = _skeleton->getParentIndex(parentIndex);
+            if (parentIndex >= 0) {
+                grandParentAbsPose = _skeleton->getAbsolutePose(grandParentIndex, _relativePoses);
+            }
+
+            // the shoulder should rotate toward the arm joint via "look-at" constraint
+            parentAbsPose = boneLookAt(absPose.trans(), parentAbsPose);
+            _relativePoses[parentIndex] = grandParentAbsPose.inverse() * parentAbsPose;
+        }
+
+        // Ignore translation on secondary poses, to prevent them from distorting the skeleton.
+        glm::vec3 origTrans = _relativePoses[iter.first].trans();
+        _relativePoses[iter.first] = parentAbsPose.inverse() * absPose;
+        _relativePoses[iter.first].trans() = origTrans;
+    }
+}
+
 void AnimInverseKinematics::initRelativePosesFromSolutionSource(SolutionSource solutionSource, const AnimPoseVec& underPoses) {
     const float RELAX_BLEND_FACTOR = (1.0f / 16.0f);
     const float COPY_BLEND_FACTOR = 1.0f;

libraries/animation/src/AnimInverseKinematics.h

@@ -70,6 +70,9 @@ public:
         NumSolutionSources,
     };
 
+    void setSecondaryTargetInRigFrame(int jointIndex, const AnimPose& pose);
+    void clearSecondaryTarget(int jointIndex);
+
     void setSolutionSource(SolutionSource solutionSource) { _solutionSource = solutionSource; }
     void setSolutionSourceVar(const QString& solutionSourceVar) { _solutionSourceVar = solutionSourceVar; }
 
@@ -88,6 +91,7 @@ protected:
     void initRelativePosesFromSolutionSource(SolutionSource solutionSource, const AnimPoseVec& underPose);
     void blendToPoses(const AnimPoseVec& targetPoses, const AnimPoseVec& underPose, float blendFactor);
     void preconditionRelativePosesToAvoidLimbLock(const AnimContext& context, const std::vector<IKTarget>& targets);
+    void setSecondaryTargets(const AnimContext& context);
     AnimPose applyHipsOffset() const;
 
     // used to pre-compute information about each joint influeced by a spline IK target.
@@ -142,6 +146,8 @@ protected:
     AnimPoseVec _relativePoses; // current relative poses
     AnimPoseVec _limitCenterPoses;  // relative
 
+    std::map<int, AnimPose> _secondaryTargetsInRigFrame;
+
     mutable std::map<int, std::vector<SplineJointInfo>> _splineJointInfoMap;
 
     // experimental data for moving hips during IK

libraries/animation/src/AnimUtil.cpp

@@ -96,3 +96,14 @@ float accumulateTime(float startFrame, float endFrame, float timeScale, float cu
     return frame;
 }
 
+// rotate bone's y-axis with target.
+AnimPose boneLookAt(const glm::vec3& target, const AnimPose& bone) {
+    glm::vec3 u, v, w;
+    generateBasisVectors(target - bone.trans(), bone.rot() * Vectors::UNIT_X, u, v, w);
+    glm::mat4 lookAt(glm::vec4(v, 0.0f),
+                     glm::vec4(u, 0.0f),
+                     // AJT: TODO REVISIT THIS, this could be -w.
+                     glm::vec4(glm::normalize(glm::cross(v, u)), 0.0f),
+                     glm::vec4(bone.trans(), 1.0f));
+    return AnimPose(lookAt);
+}

libraries/animation/src/AnimUtil.h

@@ -31,4 +31,6 @@ inline glm::quat safeLerp(const glm::quat& a, const glm::quat& b, float alpha) {
     return glm::normalize(glm::lerp(a, bTemp, alpha));
 }
 
+AnimPose boneLookAt(const glm::vec3& target, const AnimPose& bone);
+
 #endif

libraries/animation/src/Rig.cpp

@@ -1399,24 +1399,25 @@ void Rig::updateFromControllerParameters(const ControllerParameters& params, flo
     _animVars.set("isTalking", params.isTalking);
     _animVars.set("notIsTalking", !params.isTalking);
 
-    bool headEnabled = params.controllerActiveFlags[ControllerType_Head];
+    bool headEnabled = params.primaryControllerActiveFlags[PrimaryControllerType_Head];
-    bool leftHandEnabled = params.controllerActiveFlags[ControllerType_LeftHand];
+    bool leftHandEnabled = params.primaryControllerActiveFlags[PrimaryControllerType_LeftHand];
-    bool rightHandEnabled = params.controllerActiveFlags[ControllerType_RightHand];
+    bool rightHandEnabled = params.primaryControllerActiveFlags[PrimaryControllerType_RightHand];
-    bool hipsEnabled = params.controllerActiveFlags[ControllerType_Hips];
+    bool hipsEnabled = params.primaryControllerActiveFlags[PrimaryControllerType_Hips];
-    bool leftFootEnabled = params.controllerActiveFlags[ControllerType_LeftFoot];
+    bool leftFootEnabled = params.primaryControllerActiveFlags[PrimaryControllerType_LeftFoot];
-    bool rightFootEnabled = params.controllerActiveFlags[ControllerType_RightFoot];
+    bool rightFootEnabled = params.primaryControllerActiveFlags[PrimaryControllerType_RightFoot];
-    bool leftArmEnabled = params.controllerActiveFlags[ControllerType_LeftArm];
+    bool spine2Enabled = params.primaryControllerActiveFlags[PrimaryControllerType_Spine2];
-    bool rightArmEnabled = params.controllerActiveFlags[ControllerType_RightArm];
-    bool spine2Enabled = params.controllerActiveFlags[ControllerType_Spine2];
 
-    updateHead(headEnabled, hipsEnabled, params.controllerPoses[ControllerType_Head]);
+    bool leftArmEnabled = params.secondaryControllerActiveFlags[SecondaryControllerType_LeftArm];
+    bool rightArmEnabled = params.secondaryControllerActiveFlags[SecondaryControllerType_RightArm];
+
+    updateHead(headEnabled, hipsEnabled, params.primaryControllerPoses[PrimaryControllerType_Head]);
 
     updateHands(leftHandEnabled, rightHandEnabled, hipsEnabled, leftArmEnabled, rightArmEnabled, dt,
-                params.controllerPoses[ControllerType_LeftHand], params.controllerPoses[ControllerType_RightHand],
+                params.primaryControllerPoses[PrimaryControllerType_LeftHand], params.primaryControllerPoses[PrimaryControllerType_RightHand],
                 params.bodyCapsuleRadius, params.bodyCapsuleHalfHeight, params.bodyCapsuleLocalOffset);
 
     updateFeet(leftFootEnabled, rightFootEnabled,
-               params.controllerPoses[ControllerType_LeftFoot], params.controllerPoses[ControllerType_RightFoot]);
+               params.primaryControllerPoses[PrimaryControllerType_LeftFoot], params.primaryControllerPoses[PrimaryControllerType_RightFoot]);
 
     // if the hips or the feet are being controlled.
     if (hipsEnabled || rightFootEnabled || leftFootEnabled) {
@@ -1437,34 +1438,46 @@ void Rig::updateFromControllerParameters(const ControllerParameters& params, flo
 
     if (hipsEnabled) {
         _animVars.set("hipsType", (int)IKTarget::Type::RotationAndPosition);
-        _animVars.set("hipsPosition", params.controllerPoses[ControllerType_Hips].trans());
+        _animVars.set("hipsPosition", params.primaryControllerPoses[PrimaryControllerType_Hips].trans());
-        _animVars.set("hipsRotation", params.controllerPoses[ControllerType_Hips].rot());
+        _animVars.set("hipsRotation", params.primaryControllerPoses[PrimaryControllerType_Hips].rot());
     } else {
         _animVars.set("hipsType", (int)IKTarget::Type::Unknown);
     }
 
     if (hipsEnabled && spine2Enabled) {
         _animVars.set("spine2Type", (int)IKTarget::Type::Spline);
-        _animVars.set("spine2Position", params.controllerPoses[ControllerType_Spine2].trans());
+        _animVars.set("spine2Position", params.primaryControllerPoses[PrimaryControllerType_Spine2].trans());
-        _animVars.set("spine2Rotation", params.controllerPoses[ControllerType_Spine2].rot());
+        _animVars.set("spine2Rotation", params.primaryControllerPoses[PrimaryControllerType_Spine2].rot());
     } else {
         _animVars.set("spine2Type", (int)IKTarget::Type::Unknown);
     }
 
-    if (leftArmEnabled) {
+    // set secondary targets
-        _animVars.set("leftArmType", (int)IKTarget::Type::RotationAndPosition);
+    static const std::vector<QString> secondaryControllerJointNames = {
-        _animVars.set("leftArmPosition", params.controllerPoses[ControllerType_LeftArm].trans());
+        "LeftShoulder",
-        _animVars.set("leftArmRotation", params.controllerPoses[ControllerType_LeftArm].rot());
+        "RightShoulder",
-    } else {
+        "LeftArm",
-        _animVars.set("leftArmType", (int)IKTarget::Type::Unknown);
+        "RightArm",
-    }
+        "LeftForeArm",
+        "RightForeArm",
+        "LeftUpLeg",
+        "RightUpLeg",
+        "LeftLeg",
+        "RightLeg",
+        "LeftToeBase",
+        "RightToeBase"
+    };
 
-    if (rightArmEnabled) {
+    std::shared_ptr<AnimInverseKinematics> ikNode = getAnimInverseKinematicsNode();
-        _animVars.set("rightArmType", (int)IKTarget::Type::RotationAndPosition);
+    for (int i = 0; i < (int)NumSecondaryControllerTypes; i++) {
-        _animVars.set("rightArmPosition", params.controllerPoses[ControllerType_RightArm].trans());
+        int index = indexOfJoint(secondaryControllerJointNames[i]);
-        _animVars.set("rightArmRotation", params.controllerPoses[ControllerType_RightArm].rot());
+        if (index >= 0) {
-    } else {
+            if (params.secondaryControllerActiveFlags[i]) {
-        _animVars.set("rightArmType", (int)IKTarget::Type::Unknown);
+                ikNode->setSecondaryTargetInRigFrame(index, params.secondaryControllerPoses[i]);
+            } else {
+                ikNode->clearSecondaryTarget(index);
+            }
+        }
     }
 }
 

libraries/animation/src/Rig.h

@@ -41,22 +41,39 @@ public:
         bool useNames;
     };
 
-    enum ControllerType {
+    enum PrimaryControllerType {
-        ControllerType_Head = 0,
+        PrimaryControllerType_Head = 0,
-        ControllerType_LeftHand,
+        PrimaryControllerType_LeftHand,
-        ControllerType_RightHand,
+        PrimaryControllerType_RightHand,
-        ControllerType_Hips,
+        PrimaryControllerType_Hips,
-        ControllerType_LeftFoot,
+        PrimaryControllerType_LeftFoot,
-        ControllerType_RightFoot,
+        PrimaryControllerType_RightFoot,
-        ControllerType_LeftArm,
+        PrimaryControllerType_Spine2,
-        ControllerType_RightArm,
+        NumPrimaryControllerTypes
-        ControllerType_Spine2,
+    };
-        NumControllerTypes
+
+    // NOTE: These should ordered such that joint parents appear before their children.
+    enum SecondaryControllerType {
+        SecondaryControllerType_LeftShoulder = 0,
+        SecondaryControllerType_RightShoulder,
+        SecondaryControllerType_LeftArm,
+        SecondaryControllerType_RightArm,
+        SecondaryControllerType_LeftForeArm,
+        SecondaryControllerType_RightForeArm,
+        SecondaryControllerType_LeftUpLeg,
+        SecondaryControllerType_RightUpLeg,
+        SecondaryControllerType_LeftLeg,
+        SecondaryControllerType_RightLeg,
+        SecondaryControllerType_LeftToeBase,
+        SecondaryControllerType_RightToeBase,
+        NumSecondaryControllerTypes
     };
 
     struct ControllerParameters {
-        AnimPose controllerPoses[NumControllerTypes];  // rig space
+        AnimPose primaryControllerPoses[NumPrimaryControllerTypes];  // rig space
-        bool controllerActiveFlags[NumControllerTypes];
+        bool primaryControllerActiveFlags[NumPrimaryControllerTypes];
+        AnimPose secondaryControllerPoses[NumSecondaryControllerTypes];  // rig space
+        bool secondaryControllerActiveFlags[NumSecondaryControllerTypes];
         bool isTalking;
         float bodyCapsuleRadius;
         float bodyCapsuleHalfHeight;

libraries/controllers/src/controllers/Actions.h

@@ -101,6 +101,7 @@ enum class Action {
     // Bisected aliases for TRANSLATE_CAMERA_Z
     BOOM_IN,
     BOOM_OUT,
+
     LEFT_ARM,
     RIGHT_ARM,
 
@@ -146,6 +147,17 @@ enum class Action {
     RIGHT_HAND_PINKY3,
     RIGHT_HAND_PINKY4,
 
+    LEFT_SHOULDER,
+    RIGHT_SHOULDER,
+    LEFT_FORE_ARM,
+    RIGHT_FORE_ARM,
+    LEFT_LEG,
+    RIGHT_LEG,
+    LEFT_UP_LEG,
+    RIGHT_UP_LEG,
+    LEFT_TOE_BASE,
+    RIGHT_TOE_BASE,
+
     TRACKED_OBJECT_00,
     TRACKED_OBJECT_01,
     TRACKED_OBJECT_02,
@@ -163,7 +175,7 @@ enum class Action {
     TRACKED_OBJECT_14,
     TRACKED_OBJECT_15,
 
-    NUM_ACTIONS,
+    NUM_ACTIONS
 };
 
 template <typename T>

libraries/shared/src/GeometryUtil.cpp

@@ -605,3 +605,55 @@ float coneSphereAngle(const glm::vec3& coneCenter, const glm::vec3& coneDirectio
 
     return glm::max(0.0f, theta - phi);
 }
+
+// given a set of points, compute a best fit plane that passes as close as possible through all the points.
+// http://www.ilikebigbits.com/blog/2015/3/2/plane-from-points
+bool findPlaneFromPoints(const glm::vec3* points, size_t numPoints, glm::vec3& planeNormalOut, glm::vec3& pointOnPlaneOut) {
+    if (numPoints < 3) {
+        return false;
+    }
+    glm::vec3 sum;
+    for (size_t i = 0; i < numPoints; i++) {
+        sum += points[i];
+    }
+    glm::vec3 centroid = sum * (1.0f / (float)numPoints);
+    float xx = 0.0f, xy = 0.0f, xz = 0.0f;
+    float yy = 0.0f, yz = 0.0f, zz = 0.0f;
+
+    for (size_t i = 0; i < numPoints; i++) {
+        glm::vec3 r = points[i] - centroid;
+        xx += r.x * r.x;
+        xy += r.x * r.y;
+        xz += r.x * r.z;
+        yy += r.y * r.y;
+        yz += r.y * r.z;
+        zz += r.z * r.z;
+    }
+
+    float det_x = yy * zz - yz * yz;
+    float det_y = xx * zz - xz * xz;
+    float det_z = xx * yy - xy * xy;
+    float det_max = std::max(std::max(det_x, det_y), det_z);
+
+    if (det_max == 0.0f) {
+        return false; // The points don't span a plane
+    }
+
+    glm::vec3 dir;
+    if (det_max == det_x) {
+        float a = (xz * yz - xy * zz) / det_x;
+        float b = (xy * yz - xz * yy) / det_x;
+        dir = glm::vec3(1.0f, a, b);
+    } else if (det_max == det_y) {
+        float a = (yz * xz - xy * zz) / det_y;
+        float b = (xy * xz - yz * xx) / det_y;
+        dir = glm::vec3(a, 1.0f, b);
+    } else {
+        float a = (yz * xy - xz * yy) / det_z;
+        float b = (xz * xy - yz * xx) / det_z;
+        dir = glm::vec3(a, b, 1.0f);
+    }
+    pointOnPlaneOut = centroid;
+    planeNormalOut = glm::normalize(dir);
+    return true;
+}

libraries/shared/src/GeometryUtil.h

@@ -163,5 +163,7 @@ private:
     static void copyCleanArray(int& lengthA, glm::vec2* vertexArrayA, int& lengthB, glm::vec2* vertexArrayB);
 };
 
+// given a set of points, compute a best fit plane that passes as close as possible through all the points.
+bool findPlaneFromPoints(const glm::vec3* points, size_t numPoints, glm::vec3& planeNormalOut, glm::vec3& pointOnPlaneOut);
 
 #endif // hifi_GeometryUtil_h

plugins/openvr/CMakeLists.txt

@@ -18,6 +18,7 @@ if (WIN32)
     include_hifi_library_headers(octree)
 
     add_dependency_external_projects(OpenVR)
+
     find_package(OpenVR REQUIRED)
     target_include_directories(${TARGET_NAME} PRIVATE ${OPENVR_INCLUDE_DIRS})
     target_link_libraries(${TARGET_NAME} ${OPENVR_LIBRARIES})

plugins/openvr/src/ViveControllerManager.cpp

@@ -35,7 +35,6 @@
 #include <Plugins/InputConfiguration.h>
 #include <controllers/StandardControls.h>
 
-
 extern PoseData _nextSimPoseData;
 
 vr::IVRSystem* acquireOpenVrSystem();
@@ -168,6 +167,7 @@ void ViveControllerManager::setConfigurationSettings(const QJsonObject configura
             }
         }
         _inputDevice->configureCalibrationSettings(configurationSettings);
+        saveSettings();
     }
 }
 
@@ -188,6 +188,8 @@ QString ViveControllerManager::configurationLayout() {
 bool ViveControllerManager::activate() {
     InputPlugin::activate();
 
+    loadSettings();
+
     if (!_system) {
         _system = acquireOpenVrSystem();
     }
@@ -230,6 +232,8 @@ void ViveControllerManager::deactivate() {
     auto userInputMapper = DependencyManager::get<controller::UserInputMapper>();
     userInputMapper->removeDevice(_inputDevice->_deviceID);
     _registeredWithInputMapper = false;
+
+    saveSettings();
 }
 
 bool ViveControllerManager::isHeadControllerMounted() const {
@@ -282,7 +286,38 @@ void ViveControllerManager::pluginUpdate(float deltaTime, const controller::Inpu
     }
 }
 
-ViveControllerManager::InputDevice::InputDevice(vr::IVRSystem*& system) : controller::InputDevice("Vive"), _system(system) {
+void ViveControllerManager::loadSettings() {
+    Settings settings;
+    QString nameString = getName();
+    settings.beginGroup(nameString);
+    {
+        if (_inputDevice) {
+            const double DEFAULT_ARM_CIRCUMFERENCE = 0.33;
+            const double DEFAULT_SHOULDER_WIDTH = 0.48;
+            _inputDevice->_armCircumference = settings.value("armCircumference", QVariant(DEFAULT_ARM_CIRCUMFERENCE)).toDouble();
+            _inputDevice->_shoulderWidth = settings.value("shoulderWidth", QVariant(DEFAULT_SHOULDER_WIDTH)).toDouble();
+        }
+    }
+    settings.endGroup();
+}
+
+void ViveControllerManager::saveSettings() const {
+    Settings settings;
+    QString nameString = getName();
+    settings.beginGroup(nameString);
+    {
+        if (_inputDevice) {
+            settings.setValue(QString("armCircumference"), _inputDevice->_armCircumference);
+            settings.setValue(QString("shoulderWidth"), _inputDevice->_shoulderWidth);
+        }
+    }
+    settings.endGroup();
+}
+
+
+ViveControllerManager::InputDevice::InputDevice(vr::IVRSystem*& system) :
+    controller::InputDevice("Vive"),
+    _system(system) {
 
     _configStringMap[Config::None] = QString("None");
     _configStringMap[Config::Feet] = QString("Feet");
@@ -371,6 +406,9 @@ void ViveControllerManager::InputDevice::calibrateFromUI(const controller::Input
     }
 }
 
+static const float CM_TO_M = 0.01f;
+static const float M_TO_CM = 100.0f;
+
 void ViveControllerManager::InputDevice::configureCalibrationSettings(const QJsonObject configurationSettings) {
     Locker locker(_lock);
     if (!configurationSettings.empty()) {
@@ -384,8 +422,8 @@ void ViveControllerManager::InputDevice::configureCalibrationSettings(const QJso
                 bool overrideHead = headObject["override"].toBool();
                 if (overrideHead) {
                     _headConfig = HeadConfig::Puck;
-                    _headPuckYOffset = headObject["Y"].toDouble();
+                    _headPuckYOffset = headObject["Y"].toDouble() * CM_TO_M;
-                    _headPuckZOffset = headObject["Z"].toDouble();
+                    _headPuckZOffset = headObject["Z"].toDouble() * CM_TO_M;
                 } else {
                     _headConfig = HeadConfig::HMD;
                 }
@@ -394,11 +432,15 @@ void ViveControllerManager::InputDevice::configureCalibrationSettings(const QJso
                 bool overrideHands = handsObject["override"].toBool();
                 if (overrideHands) {
                     _handConfig = HandConfig::Pucks;
-                    _handPuckYOffset = handsObject["Y"].toDouble();
+                    _handPuckYOffset = handsObject["Y"].toDouble() * CM_TO_M;
-                    _handPuckZOffset = handsObject["Z"].toDouble();
+                    _handPuckZOffset = handsObject["Z"].toDouble() * CM_TO_M;
                 } else {
                     _handConfig = HandConfig::HandController;
                 }
+            } else if (iter.key() == "armCircumference") {
+                _armCircumference = (float)iter.value().toDouble() * CM_TO_M;
+            } else if (iter.key() == "shoulderWidth") {
+                _shoulderWidth = (float)iter.value().toDouble() * CM_TO_M;
             }
             iter++;
         }
@@ -417,6 +459,8 @@ QJsonObject ViveControllerManager::InputDevice::configurationSettings() {
     configurationSettings["HMDHead"] = (_headConfig == HeadConfig::HMD);
     configurationSettings["handController"] = (_handConfig == HandConfig::HandController);
     configurationSettings["puckCount"] = (int)_validTrackedObjects.size();
+    configurationSettings["armCircumference"] = (double)_armCircumference * M_TO_CM;
+    configurationSettings["shoulderWidth"] = (double)_shoulderWidth * M_TO_CM;
     return configurationSettings;
 }
 
@@ -534,7 +578,7 @@ void ViveControllerManager::InputDevice::calibrate(const controller::InputCalibr
     }
 }
 
-bool ViveControllerManager::InputDevice::configureHands(glm::mat4& defaultToReferenceMat, const controller::InputCalibrationData& inputCalibration) {
+bool ViveControllerManager::InputDevice::configureHands(const glm::mat4& defaultToReferenceMat, const controller::InputCalibrationData& inputCalibration) {
     std::sort(_validTrackedObjects.begin(), _validTrackedObjects.end(), sortPucksXPosition);
     int puckCount = (int)_validTrackedObjects.size();
     if (_handConfig == HandConfig::Pucks && puckCount >= MIN_PUCK_COUNT) {
@@ -569,7 +613,7 @@ bool ViveControllerManager::InputDevice::configureHands(glm::mat4& defaultToRefe
     return false;
 }
 
-bool ViveControllerManager::InputDevice::configureHead(glm::mat4& defaultToReferenceMat, const controller::InputCalibrationData& inputCalibration) {
+bool ViveControllerManager::InputDevice::configureHead(const glm::mat4& defaultToReferenceMat, const controller::InputCalibrationData& inputCalibration) {
     std::sort(_validTrackedObjects.begin(), _validTrackedObjects.end(), sortPucksYPosition);
     int puckCount = (int)_validTrackedObjects.size();
     if (_headConfig == HeadConfig::Puck && puckCount >= MIN_HEAD) {
@@ -583,7 +627,7 @@ bool ViveControllerManager::InputDevice::configureHead(glm::mat4& defaultToRefer
     return false;
 }
 
-bool ViveControllerManager::InputDevice::configureBody(glm::mat4& defaultToReferenceMat, const controller::InputCalibrationData& inputCalibration) {
+bool ViveControllerManager::InputDevice::configureBody(const glm::mat4& defaultToReferenceMat, const controller::InputCalibrationData& inputCalibration) {
     std::sort(_validTrackedObjects.begin(), _validTrackedObjects.end(), sortPucksYPosition);
     int puckCount = (int)_validTrackedObjects.size();
     glm::vec3 headXAxis = getReferenceHeadXAxis(defaultToReferenceMat, inputCalibration.defaultHeadMat);
@@ -624,7 +668,8 @@ bool ViveControllerManager::InputDevice::configureBody(glm::mat4& defaultToRefer
 
 void ViveControllerManager::InputDevice::uncalibrate() {
     _config = Config::None;
-    _pucksOffset.clear();
+    _pucksPostOffset.clear();
+    _pucksPreOffset.clear();
     _jointToPuckMap.clear();
     _calibrated = false;
     _overrideHead = false;
@@ -654,10 +699,17 @@ controller::Pose ViveControllerManager::InputDevice::addOffsetToPuckPose(int joi
     if (puck != _jointToPuckMap.end()) {
         uint32_t puckIndex = puck->second;
         auto puckPose = _poseStateMap.find(puckIndex);
-        auto puckOffset = _pucksOffset.find(puckIndex);
+        auto puckPostOffset = _pucksPostOffset.find(puckIndex);
+        auto puckPreOffset = _pucksPreOffset.find(puckIndex);
 
-        if ((puckPose != _poseStateMap.end()) && (puckOffset != _pucksOffset.end())) {
+        if (puckPose != _poseStateMap.end()) {
-            return puckPose->second.postTransform(puckOffset->second);
+            if (puckPreOffset != _pucksPreOffset.end() && puckPostOffset != _pucksPostOffset.end()) {
+                return puckPose->second.postTransform(puckPostOffset->second).transform(puckPreOffset->second);
+            } else if (puckPostOffset != _pucksPostOffset.end()) {
+                return puckPose->second.postTransform(puckPostOffset->second);
+            } else if (puckPreOffset != _pucksPreOffset.end()) {
+                return puckPose->second.transform(puckPreOffset->second);
+            }
         }
     }
     return controller::Pose();
@@ -708,7 +760,7 @@ void ViveControllerManager::InputDevice::handleHandController(float deltaTime, u
             // pseudo buttons the depend on both of the above for-loops
             partitionTouchpad(controller::LS, controller::LX, controller::LY, controller::LS_CENTER, controller::LS_X, controller::LS_Y);
             partitionTouchpad(controller::RS, controller::RX, controller::RY, controller::RS_CENTER, controller::RS_X, controller::RS_Y);
-         }
+        }
     }
 }
 
@@ -942,7 +994,7 @@ void ViveControllerManager::InputDevice::hapticsHelper(float deltaTime, bool lef
     }
 }
 
-void ViveControllerManager::InputDevice::calibrateLeftHand(glm::mat4& defaultToReferenceMat, const controller::InputCalibrationData& inputCalibration, PuckPosePair& handPair) {
+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);
@@ -970,10 +1022,10 @@ void ViveControllerManager::InputDevice::calibrateLeftHand(glm::mat4& defaultToR
     glm::mat4 offsetMat = createMatFromQuatAndPos(rotationOffset, translationOffset);
 
     _jointToPuckMap[controller::LEFT_HAND] = handPair.first;
-    _pucksOffset[handPair.first] = offsetMat;
+    _pucksPostOffset[handPair.first] = offsetMat;
 }
 
-void ViveControllerManager::InputDevice::calibrateRightHand(glm::mat4& defaultToReferenceMat, const controller::InputCalibrationData& inputCalibration, PuckPosePair& handPair) {
+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);
@@ -1001,11 +1053,11 @@ void ViveControllerManager::InputDevice::calibrateRightHand(glm::mat4& defaultTo
     glm::mat4 offsetMat = createMatFromQuatAndPos(rotationOffset, translationOffset);
 
     _jointToPuckMap[controller::RIGHT_HAND] = handPair.first;
-    _pucksOffset[handPair.first] = offsetMat;
+    _pucksPostOffset[handPair.first] = offsetMat;
 }
 
 
-void ViveControllerManager::InputDevice::calibrateFeet(glm::mat4& defaultToReferenceMat, const controller::InputCalibrationData& inputCalibration) {
+void ViveControllerManager::InputDevice::calibrateFeet(const glm::mat4& defaultToReferenceMat, const controller::InputCalibrationData& inputCalibration) {
     glm::vec3 headXAxis = getReferenceHeadXAxis(defaultToReferenceMat, inputCalibration.defaultHeadMat);
     glm::vec3 headPosition = getReferenceHeadPosition(defaultToReferenceMat, inputCalibration.defaultHeadMat);
     auto& firstFoot = _validTrackedObjects[FIRST_FOOT];
@@ -1022,7 +1074,7 @@ void ViveControllerManager::InputDevice::calibrateFeet(glm::mat4& defaultToRefer
     }
 }
 
-void ViveControllerManager::InputDevice::calibrateFoot(glm::mat4& defaultToReferenceMat, const controller::InputCalibrationData& inputCalibration, PuckPosePair& footPair, bool isLeftFoot){
+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 defaultFoot = isLeftFoot ? inputCalibration.defaultLeftFoot : inputCalibration.defaultRightFoot;
@@ -1037,48 +1089,96 @@ void ViveControllerManager::InputDevice::calibrateFoot(glm::mat4& defaultToRefer
 
     if (isLeftFoot) {
         _jointToPuckMap[controller::LEFT_FOOT] = footPair.first;
-        _pucksOffset[footPair.first] = finalOffset;
+        _pucksPostOffset[footPair.first] = finalOffset;
     } else {
         _jointToPuckMap[controller::RIGHT_FOOT] = footPair.first;
-        _pucksOffset[footPair.first] = finalOffset;
+        _pucksPostOffset[footPair.first] = finalOffset;
     }
 }
 
-void ViveControllerManager::InputDevice::calibrateHips(glm::mat4& defaultToReferenceMat, const controller::InputCalibrationData& inputCalibration) {
+void ViveControllerManager::InputDevice::calibrateHips(const glm::mat4& defaultToReferenceMat, const controller::InputCalibrationData& inputCalibration) {
     _jointToPuckMap[controller::HIPS] = _validTrackedObjects[HIP].first;
-    _pucksOffset[_validTrackedObjects[HIP].first] = computeOffset(defaultToReferenceMat, inputCalibration.defaultHips, _validTrackedObjects[HIP].second);
+    _pucksPostOffset[_validTrackedObjects[HIP].first] = computeOffset(defaultToReferenceMat, inputCalibration.defaultHips, _validTrackedObjects[HIP].second);
 }
 
-void ViveControllerManager::InputDevice::calibrateChest(glm::mat4& defaultToReferenceMat, const controller::InputCalibrationData& inputCalibration) {
+void ViveControllerManager::InputDevice::calibrateChest(const glm::mat4& defaultToReferenceMat, const controller::InputCalibrationData& inputCalibration) {
     _jointToPuckMap[controller::SPINE2] = _validTrackedObjects[CHEST].first;
-    _pucksOffset[_validTrackedObjects[CHEST].first] = computeOffset(defaultToReferenceMat, inputCalibration.defaultSpine2, _validTrackedObjects[CHEST].second);
+    _pucksPostOffset[_validTrackedObjects[CHEST].first] = computeOffset(defaultToReferenceMat, inputCalibration.defaultSpine2, _validTrackedObjects[CHEST].second);
 }
 
-void ViveControllerManager::InputDevice::calibrateShoulders(glm::mat4& defaultToReferenceMat, const controller::InputCalibrationData& inputCalibration,
+// y axis comes out of puck usb port/green light
+// -z axis comes out of puck center/vive logo
+static glm::vec3 computeUserShoulderPositionFromMeasurements(float armCirc, float shoulderSpan, const glm::mat4& headMat, const controller::Pose& armPuck, bool isLeftHand) {
+
+    float armRadius = armCirc / TWO_PI;
+
+    float sign = isLeftHand ? 1.0f : -1.0f;
+    float localArmX = sign * shoulderSpan / 2.0f;
+
+    controller::Pose localPuck = armPuck.transform(glm::inverse(headMat));
+    glm::mat4 localPuckMat = localPuck.getMatrix();
+    glm::vec3 localArmCenter = extractTranslation(localPuckMat) + armRadius * transformVectorFast(localPuckMat, Vectors::UNIT_Z);
+
+    return transformPoint(headMat, glm::vec3(localArmX, localArmCenter.y, localArmCenter.z));
+}
+
+void ViveControllerManager::InputDevice::calibrateShoulders(const 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;
 
+    glm::mat4 refLeftArm = defaultToReferenceMat * inputCalibration.defaultLeftArm;
+    glm::mat4 refRightArm = defaultToReferenceMat * inputCalibration.defaultRightArm;
+
+    glm::mat4 userRefLeftArm = refLeftArm;
+    glm::mat4 userRefRightArm = refRightArm;
+
+    glm::mat4 headMat = defaultToReferenceMat * inputCalibration.defaultHeadMat;
+
     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);
+
+        glm::vec3 leftPos = computeUserShoulderPositionFromMeasurements(_armCircumference, _shoulderWidth, headMat, firstShoulderPose, true);
+        userRefLeftArm[3] = glm::vec4(leftPos, 1.0f);
+        glm::vec3 rightPos = computeUserShoulderPositionFromMeasurements(_armCircumference, _shoulderWidth, headMat, secondShoulderPose, false);
+        userRefRightArm[3] = glm::vec4(rightPos, 1.0f);
+
+        // compute the post offset from the userRefArm
+        _pucksPostOffset[firstShoulder.first] = computeOffset(Matrices::IDENTITY, userRefLeftArm, firstShoulderPose);
+        _pucksPostOffset[secondShoulder.first] = computeOffset(Matrices::IDENTITY, userRefRightArm, secondShoulderPose);
+
+        // compute the pre offset from the diff between userRefArm and refArm transforms.
+        // as an optimization we don't do a full inverse, but subtract the translations.
+        _pucksPreOffset[firstShoulder.first] = createMatFromQuatAndPos(glm::quat(), extractTranslation(userRefLeftArm) - extractTranslation(refLeftArm));
+        _pucksPreOffset[secondShoulder.first] = createMatFromQuatAndPos(glm::quat(), extractTranslation(userRefRightArm) - extractTranslation(refRightArm));
     } 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);
+
+        glm::vec3 leftPos = computeUserShoulderPositionFromMeasurements(_armCircumference, _shoulderWidth, headMat, secondShoulderPose, true);
+        userRefLeftArm[3] = glm::vec4(leftPos, 1.0f);
+        glm::vec3 rightPos = computeUserShoulderPositionFromMeasurements(_armCircumference, _shoulderWidth, headMat, firstShoulderPose, false);
+        userRefRightArm[3] = glm::vec4(rightPos, 1.0f);
+
+        // compute the post offset from the userRefArm
+        _pucksPostOffset[secondShoulder.first] = computeOffset(Matrices::IDENTITY, userRefLeftArm, secondShoulderPose);
+        _pucksPostOffset[firstShoulder.first] = computeOffset(Matrices::IDENTITY, userRefRightArm, firstShoulderPose);
+
+        // compute the pre offset from the diff between userRefArm and refArm transforms.
+        // as an optimization we don't do a full inverse, but subtract the translations.
+        _pucksPreOffset[secondShoulder.first] = createMatFromQuatAndPos(glm::quat(), extractTranslation(userRefLeftArm) - extractTranslation(refLeftArm));
+        _pucksPreOffset[firstShoulder.first] = createMatFromQuatAndPos(glm::quat(), extractTranslation(userRefRightArm) - extractTranslation(refRightArm));
     }
 }
 
-void ViveControllerManager::InputDevice::calibrateHead(glm::mat4& defaultToReferenceMat, const controller::InputCalibrationData& inputCalibration) {
+void ViveControllerManager::InputDevice::calibrateHead(const glm::mat4& defaultToReferenceMat, const controller::InputCalibrationData& inputCalibration) {
     size_t headIndex = _validTrackedObjects.size() - 1;
     const PuckPosePair& head = _validTrackedObjects[headIndex];
     _jointToPuckMap[controller::HEAD] = head.first;
-    _pucksOffset[head.first] = computeOffset(defaultToReferenceMat, inputCalibration.defaultHeadMat, head.second);
+    _pucksPostOffset[head.first] = computeOffset(defaultToReferenceMat, inputCalibration.defaultHeadMat, head.second);
 }
 
 QString ViveControllerManager::InputDevice::configToString(Config config) {

plugins/openvr/src/ViveControllerManager.h

@@ -57,6 +57,9 @@ public:
 
     void setRenderControllers(bool renderControllers) { _renderControllers = renderControllers; }
 
+    virtual void saveSettings() const override;
+    virtual void loadSettings() override;
+
 private:
     class InputDevice : public controller::InputDevice {
     public:
@@ -93,18 +96,18 @@ private:
         void partitionTouchpad(int sButton, int xAxis, int yAxis, int centerPsuedoButton, int xPseudoButton, int yPseudoButton);
         void printDeviceTrackingResultChange(uint32_t deviceIndex);
         void setConfigFromString(const QString& value);
-        bool configureHead(glm::mat4& defaultToReferenceMat, const controller::InputCalibrationData& inputCalibration);
+        bool configureHead(const glm::mat4& defaultToReferenceMat, const controller::InputCalibrationData& inputCalibration);
-        bool configureHands(glm::mat4& defaultToReferenceMat, const controller::InputCalibrationData& inputCalibration);
+        bool configureHands(const glm::mat4& defaultToReferenceMat, const controller::InputCalibrationData& inputCalibration);
-        bool configureBody(glm::mat4& defaultToReferenceMat, const controller::InputCalibrationData& inputCalibration);
+        bool configureBody(const glm::mat4& defaultToReferenceMat, const controller::InputCalibrationData& inputCalibration);
-        void calibrateLeftHand(glm::mat4& defaultToReferenceMat, const controller::InputCalibrationData& inputCalibration, PuckPosePair& handPair);
+        void calibrateLeftHand(const glm::mat4& defaultToReferenceMat, const controller::InputCalibrationData& inputCalibration, PuckPosePair& handPair);
-        void calibrateRightHand(glm::mat4& defaultToReferenceMat, const controller::InputCalibrationData& inputCalibration, PuckPosePair& handPair);
+        void calibrateRightHand(const glm::mat4& defaultToReferenceMat, const controller::InputCalibrationData& inputCalibration, PuckPosePair& handPair);
-        void calibrateFeet(glm::mat4& defaultToReferenceMat, const controller::InputCalibrationData& inputCalibration);
+        void calibrateFeet(const glm::mat4& defaultToReferenceMat, const controller::InputCalibrationData& inputCalibration);
-        void calibrateFoot(glm::mat4& defaultToReferenceMat, const controller::InputCalibrationData& inputCalibration, PuckPosePair& footPair, bool isLeftFoot);
+        void calibrateFoot(const glm::mat4& defaultToReferenceMat, const controller::InputCalibrationData& inputCalibration, PuckPosePair& footPair, bool isLeftFoot);
-        void calibrateHips(glm::mat4& defaultToReferenceMat, const controller::InputCalibrationData& inputCalibration);
+        void calibrateHips(const glm::mat4& defaultToReferenceMat, const controller::InputCalibrationData& inputCalibration);
-        void calibrateChest(glm::mat4& defaultToReferenceMat, const controller::InputCalibrationData& inputCalibration);
+        void calibrateChest(const glm::mat4& defaultToReferenceMat, const controller::InputCalibrationData& inputCalibration);
-        void calibrateShoulders(glm::mat4& defaultToReferenceMat, const controller::InputCalibrationData& inputCalibration,
+        void calibrateShoulders(const glm::mat4& defaultToReferenceMat, const controller::InputCalibrationData& inputCalibration,
                                 int firstShoulderIndex, int secondShoulderIndex);
-        void calibrateHead(glm::mat4& defaultToReferenceMat, const controller::InputCalibrationData& inputCalibration);
+        void calibrateHead(const glm::mat4& defaultToReferenceMat, const controller::InputCalibrationData& inputCalibration);
         void calibrateFromHandController(const controller::InputCalibrationData& inputCalibrationData);
         void calibrateFromUI(const controller::InputCalibrationData& inputCalibrationData);
         void emitCalibrationStatus();
@@ -161,7 +164,8 @@ private:
         FilteredStick _filteredRightStick;
 
         std::vector<PuckPosePair> _validTrackedObjects;
-        std::map<uint32_t, glm::mat4> _pucksOffset;
+        std::map<uint32_t, glm::mat4> _pucksPostOffset;
+        std::map<uint32_t, glm::mat4> _pucksPreOffset;
         std::map<int, uint32_t> _jointToPuckMap;
         std::map<Config, QString> _configStringMap;
         PoseData _lastSimPoseData;
@@ -181,6 +185,8 @@ private:
         float _headPuckZOffset { -0.05f };
         float _handPuckYOffset { 0.0f };
         float _handPuckZOffset { 0.0f };
+        float _armCircumference { 0.33f };
+        float _shoulderWidth { 0.48f };
         bool _triggersPressedHandled { false };
         bool _calibrated { false };
         bool _timeTilCalibrationSet { false };

scripts/system/html/css/edit-style.css

@@ -267,18 +267,17 @@ input[type=number] {
     padding-right: 3px;
 }
 input[type=number]::-webkit-inner-spin-button {
-    -webkit-appearance: none;
     opacity: 1.0;
     display: block;
     position: relative;
     width: 10px;
-    height: 100%;
+    height: 90%;
     overflow: hidden;
     font-family: hifi-glyphs;
-    font-size: 46px;
+    font-size: 32px;
     color: #afafaf;
     cursor: pointer;
-    /*background-color: #000000;*/
+    background-color: #000000;
 }
 input[type=number]::-webkit-inner-spin-button:before,
 input[type=number]::-webkit-inner-spin-button:after {