diff --git a/interface/resources/avatar/avatar-animation.json b/interface/resources/avatar/avatar-animation.json index a493d8e9af..ee2b916d1e 100644 --- a/interface/resources/avatar/avatar-animation.json +++ b/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] } ] }, diff --git a/interface/resources/qml/hifi/tablet/OpenVrConfiguration.qml b/interface/resources/qml/hifi/tablet/OpenVrConfiguration.qml index 90d6ba7022..78c10e2ffa 100644 --- a/interface/resources/qml/hifi/tablet/OpenVrConfiguration.qml +++ b/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 - value: -0.05 + stepSize: 1 + 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 - decimals: 4 - value: -0.05 + stepSize: 1 + decimals: 1 + 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 - decimals: 4 + stepSize: 1 + 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.topMargin: 10 + anchors.top: shoulderAdditionalConfig.visible ? shoulderAdditionalConfig.bottom : shoulderConfig.bottom + 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 } diff --git a/interface/src/Application.cpp b/interface/src/Application.cpp index 2bc1d1e20e..e5fb9949db 100644 --- a/interface/src/Application.cpp +++ b/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(worldHeadMat, glm::vec3(0.0f, 0.0f, TREE_SCALE)); + lookAtSpot = transformPoint(headPose.getMatrix(), 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); - controller::Pose rightHandPose = userInputMapper->getPoseState(controller::Action::RIGHT_HAND); - auto myAvatarMatrix = createMatFromQuatAndPos(myAvatar->getOrientation(), myAvatar->getPosition()); - auto worldToSensorMatrix = glm::inverse(myAvatar->getSensorToWorldMatrix()); - auto avatarToSensorMatrix = worldToSensorMatrix * myAvatarMatrix; - myAvatar->setHandControllerPosesInSensorFrame(leftHandPose.transform(avatarToSensorMatrix), rightHandPose.transform(avatarToSensorMatrix)); + static const std::vector avatarControllerActions = { + controller::Action::LEFT_HAND, + controller::Action::RIGHT_HAND, + controller::Action::LEFT_FOOT, + controller::Action::RIGHT_FOOT, + 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 - // fingers are not being controlled, finger joints are not updated in MySkeletonModel. - // Assumption: Finger poses are either all present and valid or not present at all; thus can test just one joint. - MyAvatar::FingerPosesMap leftHandFingerPoses; - if (myAvatar->getLeftHandFingerControllerPosesInSensorFrame().size() > 0 - || userInputMapper->getPoseState(controller::Action::LEFT_HAND_THUMB1).isValid()) { - for (int i = (int)controller::Action::LEFT_HAND_THUMB1; i <= (int)controller::Action::LEFT_HAND_PINKY4; i++) { - leftHandFingerPoses[i] = { - userInputMapper->getPoseState((controller::Action)i).transform(avatarToSensorMatrix), - userInputMapper->getActionName((controller::Action)i) - }; - } + // copy controller poses from userInputMapper to myAvatar. + glm::mat4 myAvatarMatrix = createMatFromQuatAndPos(myAvatar->getOrientation(), myAvatar->getPosition()); + glm::mat4 worldToSensorMatrix = glm::inverse(myAvatar->getSensorToWorldMatrix()); + glm::mat4 avatarToSensorMatrix = worldToSensorMatrix * myAvatarMatrix; + for (auto& action : avatarControllerActions) { + controller::Pose pose = userInputMapper->getPoseState(action); + myAvatar->setControllerPoseInSensorFrame(action, pose.transform(avatarToSensorMatrix)); } - 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 diff --git a/interface/src/avatar/AvatarActionHold.cpp b/interface/src/avatar/AvatarActionHold.cpp index c1d2f903f3..fe5355ff2e 100644 --- a/interface/src/avatar/AvatarActionHold.cpp +++ b/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()) { diff --git a/interface/src/avatar/MyAvatar.cpp b/interface/src/avatar/MyAvatar.cpp index 474aa48e63..36295e87b8 100755 --- a/interface/src/avatar/MyAvatar.cpp +++ b/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::setHandControllerPosesInSensorFrame(const controller::Pose& left, const controller::Pose& right) { - _leftHandControllerPoseInSensorFrameCache.set(left); - _rightHandControllerPoseInSensorFrameCache.set(right); +void MyAvatar::setControllerPoseInSensorFrame(controller::Action action, const controller::Pose& pose) { + std::lock_guard guard(_controllerPoseMapMutex); + auto iter = _controllerPoseMap.find(action); + if (iter != _controllerPoseMap.end()) { + iter->second = pose; + } else { + _controllerPoseMap.insert({ action, pose }); + } } -controller::Pose MyAvatar::getLeftHandControllerPoseInSensorFrame() const { - return _leftHandControllerPoseInSensorFrameCache.get(); +controller::Pose MyAvatar::getControllerPoseInSensorFrame(controller::Action action) const { + std::lock_guard 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 { - return _rightHandControllerPoseInSensorFrameCache.get(); +controller::Pose MyAvatar::getControllerPoseInWorldFrame(controller::Action action) const { + auto pose = getControllerPoseInSensorFrame(action); + if (pose.valid) { + return pose.transform(getSensorToWorldMatrix()); + } else { + return controller::Pose(); // invalid pose + } } -controller::Pose MyAvatar::getLeftHandControllerPoseInWorldFrame() const { - return _leftHandControllerPoseInSensorFrameCache.get().transform(getSensorToWorldMatrix()); -} - -controller::Pose MyAvatar::getRightHandControllerPoseInWorldFrame() const { - return _rightHandControllerPoseInSensorFrameCache.get().transform(getSensorToWorldMatrix()); -} - -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); +controller::Pose MyAvatar::getControllerPoseInAvatarFrame(controller::Action action) const { + auto pose = getControllerPoseInWorldFrame(action); + if (pose.valid) { + glm::mat4 invAvatarMatrix = glm::inverse(createMatFromQuatAndPos(getOrientation(), getPosition())); + return pose.transform(invAvatarMatrix); + } else { + return controller::Pose(); // invalid pose + } } 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 rightHandPose = getRightHandControllerPoseInWorldFrame(); + auto leftHandPose = getControllerPoseInWorldFrame(controller::Action::LEFT_HAND); + 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; - headOrientation = getHeadControllerPoseInSensorFrame().rotation * Quaternions::Y_180; + headPosition = headPose.translation; + 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); diff --git a/interface/src/avatar/MyAvatar.h b/interface/src/avatar/MyAvatar.h index fa5ffc4b93..dc4357be52 100644 --- a/interface/src/avatar/MyAvatar.h +++ b/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> 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 setArmControllerPosesInSensorFrame(const controller::Pose& left, const controller::Pose& right); - controller::Pose getLeftArmControllerPoseInSensorFrame() const; - controller::Pose getRightArmControllerPoseInSensorFrame() const; - controller::Pose getLeftArmControllerPoseInWorldFrame() const; - controller::Pose getRightArmControllerPoseInWorldFrame() const; - controller::Pose getLeftArmControllerPoseInAvatarFrame() const; - controller::Pose getRightArmControllerPoseInAvatarFrame() const; + void setControllerPoseInSensorFrame(controller::Action action, const controller::Pose& pose); + controller::Pose getControllerPoseInSensorFrame(controller::Action action) const; + controller::Pose getControllerPoseInWorldFrame(controller::Action action) const; + controller::Pose getControllerPoseInAvatarFrame(controller::Action action) 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 - ThreadSafeValueCache _leftHandControllerPoseInSensorFrameCache { controller::Pose() }; - ThreadSafeValueCache _rightHandControllerPoseInSensorFrameCache { controller::Pose() }; - ThreadSafeValueCache _leftHandFingerPosesInSensorFramceCache { }; - ThreadSafeValueCache _rightHandFingerPosesInSensorFramceCache { }; - ThreadSafeValueCache _leftFootControllerPoseInSensorFrameCache { controller::Pose() }; - ThreadSafeValueCache _rightFootControllerPoseInSensorFrameCache { controller::Pose() }; - ThreadSafeValueCache _hipsControllerPoseInSensorFrameCache { controller::Pose() }; - ThreadSafeValueCache _spine2ControllerPoseInSensorFrameCache { controller::Pose() }; - ThreadSafeValueCache _headControllerPoseInSensorFrameCache { controller::Pose() }; - ThreadSafeValueCache _leftArmControllerPoseInSensorFrameCache { controller::Pose() }; - ThreadSafeValueCache _rightArmControllerPoseInSensorFrameCache { controller::Pose() }; + // all poses are in sensor-frame + std::map _controllerPoseMap; + mutable std::mutex _controllerPoseMapMutex; bool _hmdLeanRecenterEnabled = true; AnimPose _prePhysicsRoomPose; diff --git a/interface/src/avatar/MyHead.cpp b/interface/src/avatar/MyHead.cpp index 7fc6b9fa26..7fc1850bb2 100644 --- a/interface/src/avatar/MyHead.cpp +++ b/interface/src/avatar/MyHead.cpp @@ -34,7 +34,7 @@ glm::quat MyHead::getHeadOrientation() const { // always the same. MyAvatar* myAvatar = static_cast(_owningAvatar); - auto headPose = myAvatar->getHeadControllerPoseInWorldFrame(); + auto headPose = myAvatar->getControllerPoseInWorldFrame(controller::Action::HEAD); if (headPose.isValid()) { return headPose.rotation * Quaternions::Y_180; } diff --git a/interface/src/avatar/MySkeletonModel.cpp b/interface/src/avatar/MySkeletonModel.cpp index 97309d9678..6d468c3f30 100644 --- a/interface/src/avatar/MySkeletonModel.cpp +++ b/interface/src/avatar/MySkeletonModel.cpp @@ -46,105 +46,82 @@ void MySkeletonModel::updateRig(float deltaTime, glm::mat4 parentTransform) { } MyAvatar* myAvatar = static_cast(_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.controllerActiveFlags[Rig::ControllerType_Head] = true; + params.primaryControllerPoses[Rig::PrimaryControllerType_Head] = avatarToRigPose * pose; + 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.controllerActiveFlags[Rig::ControllerType_Head] = false; + params.primaryControllerPoses[Rig::PrimaryControllerType_Head] = AnimPose(glm::vec3(1.0f), headRot, glm::vec3(0.0f)); + params.primaryControllerActiveFlags[Rig::PrimaryControllerType_Head] = false; } - auto avatarHipsPose = myAvatar->getHipsControllerPoseInAvatarFrame(); - if (avatarHipsPose.isValid()) { - AnimPose pose(avatarHipsPose.getRotation(), avatarHipsPose.getTranslation()); - params.controllerPoses[Rig::ControllerType_Hips] = avatarToRigPose * pose; - params.controllerActiveFlags[Rig::ControllerType_Hips] = true; - } else { - params.controllerPoses[Rig::ControllerType_Hips] = AnimPose::identity; - params.controllerActiveFlags[Rig::ControllerType_Hips] = false; + // + // primary controller poses, control IK targets directly. + // + + static const std::vector> primaryControllers = { + { controller::Action::LEFT_HAND, Rig::PrimaryControllerType_LeftHand }, + { controller::Action::RIGHT_HAND, Rig::PrimaryControllerType_RightHand }, + { 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()) { - 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; - } + // + // secondary controller poses, influence the pose of the skeleton indirectly. + // - auto avatarRightArmPose = myAvatar->getRightArmControllerPoseInAvatarFrame(); - if (avatarRightArmPose.isValid()) { - AnimPose pose(avatarRightArmPose.getRotation(), avatarRightArmPose.getTranslation()); - params.controllerPoses[Rig::ControllerType_RightArm] = avatarToRigPose * pose; - params.controllerActiveFlags[Rig::ControllerType_RightArm] = true; - } else { - params.controllerPoses[Rig::ControllerType_RightArm] = AnimPose::identity; - params.controllerActiveFlags[Rig::ControllerType_RightArm] = false; - } - - auto avatarLeftArmPose = myAvatar->getLeftArmControllerPoseInAvatarFrame(); - if (avatarLeftArmPose.isValid()) { - AnimPose pose(avatarLeftArmPose.getRotation(), avatarLeftArmPose.getTranslation()); - 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; - } + static const std::vector> secondaryControllers = { + { controller::Action::LEFT_SHOULDER, Rig::SecondaryControllerType_LeftShoulder }, + { controller::Action::RIGHT_SHOULDER, Rig::SecondaryControllerType_RightShoulder }, + { controller::Action::LEFT_ARM, Rig::SecondaryControllerType_LeftArm }, + { controller::Action::RIGHT_ARM, Rig::SecondaryControllerType_RightArm }, + { controller::Action::LEFT_FORE_ARM, Rig::SecondaryControllerType_LeftForeArm }, + { controller::Action::RIGHT_FORE_ARM, Rig::SecondaryControllerType_RightForeArm }, + { controller::Action::LEFT_UP_LEG, Rig::SecondaryControllerType_LeftUpLeg }, + { controller::Action::RIGHT_UP_LEG, Rig::SecondaryControllerType_RightUpLeg }, + { controller::Action::LEFT_LEG, Rig::SecondaryControllerType_LeftLeg }, + { controller::Action::RIGHT_LEG, Rig::SecondaryControllerType_RightLeg }, + { controller::Action::LEFT_TOE_BASE, Rig::SecondaryControllerType_LeftToeBase }, + { controller::Action::RIGHT_TOE_BASE, Rig::SecondaryControllerType_RightToeBase } + }; - auto avatarLeftHandPose = myAvatar->getLeftHandControllerPoseInAvatarFrame(); - if (avatarLeftHandPose.isValid()) { - AnimPose pose(avatarLeftHandPose.getRotation(), avatarLeftHandPose.getTranslation()); - params.controllerPoses[Rig::ControllerType_LeftHand] = avatarToRigPose * pose; - params.controllerActiveFlags[Rig::ControllerType_LeftHand] = true; - } else { - params.controllerPoses[Rig::ControllerType_LeftHand] = AnimPose::identity; - params.controllerActiveFlags[Rig::ControllerType_LeftHand] = 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; + for (auto pair : secondaryControllers) { + auto controllerPose = myAvatar->getControllerPoseInAvatarFrame(pair.first); + if (controllerPose.isValid()) { + AnimPose pose(controllerPose.getRotation(), controllerPose.getTranslation()); + params.secondaryControllerPoses[pair.second] = avatarToRigPose * pose; + params.secondaryControllerActiveFlags[pair.second] = true; + } else { + params.secondaryControllerPoses[pair.second] = AnimPose::identity; + params.secondaryControllerActiveFlags[pair.second] = 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(myAvatar->getRightHandFingerControllerPosesInSensorFrame()); + updateFingers(); } -void MySkeletonModel::updateFingers(const MyAvatar::FingerPosesMap& fingerPoses) { - // 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; +void MySkeletonModel::updateFingers() { MyAvatar* myAvatar = static_cast(_owningAvatar); - auto handPose = isLeftHand - ? myAvatar->getLeftHandControllerPoseInSensorFrame() - : myAvatar->getRightHandControllerPoseInSensorFrame(); - auto handJointRotation = handPose.getRotation(); - bool isHandValid = handPose.isValid(); - bool isFingerValid = false; - glm::quat previousJointRotation; - - while (posesMapItr != fingerPoses.end()) { - auto jointName = posesMapItr->second.second; - if (isHandValid && jointName.right(1) == "1") { - isFingerValid = posesMapItr->second.first.isValid(); - previousJointRotation = handJointRotation; + static std::vector>> fingerChains = { + { + { controller::Action::LEFT_HAND, "LeftHand" }, + { controller::Action::LEFT_HAND_THUMB1, "LeftHandThumb1" }, + { controller::Action::LEFT_HAND_THUMB2, "LeftHandThumb2" }, + { controller::Action::LEFT_HAND_THUMB3, "LeftHandThumb3" }, + { controller::Action::LEFT_HAND_THUMB4, "LeftHandThumb4" } + }, + { + { 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) { - auto thisJointRotation = posesMapItr->second.first.getRotation(); - const float CONTROLLER_PRIORITY = 2.0f; - _rig.setJointRotation(_rig.indexOfJoint(jointName), true, glm::inverse(previousJointRotation) * thisJointRotation, - CONTROLLER_PRIORITY); - previousJointRotation = thisJointRotation; - } else { - _rig.clearJointAnimationPriority(_rig.indexOfJoint(jointName)); + const float CONTROLLER_PRIORITY = 2.0f; + + for (auto& chain : fingerChains) { + glm::quat prevAbsRot = Quaternions::IDENTITY; + for (auto& link : chain) { + int index = _rig.indexOfJoint(link.second); + if (index >= 0) { + 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++; } } diff --git a/interface/src/avatar/MySkeletonModel.h b/interface/src/avatar/MySkeletonModel.h index 6867c596af..ad0ae1b8e9 100644 --- a/interface/src/avatar/MySkeletonModel.h +++ b/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 diff --git a/libraries/animation/src/AnimInverseKinematics.cpp b/libraries/animation/src/AnimInverseKinematics.cpp index 57c00e7183..d17fbebf3a 100644 --- a/libraries/animation/src/AnimInverseKinematics.cpp +++ b/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, - const AnimInverseKinematics::JointInfo** jointInfoA, - const AnimInverseKinematics::JointInfo** jointInfoB) { + int indexA, int indexB, + const AnimInverseKinematics::JointInfo** jointInfoA, + 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) { - QString name = QString("ikTarget%1").arg(target.getIndex()); + for (int i = 0; i < MAX_TARGET_MARKERS; i++) { + 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::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; - poses[info.jointIndex].trans() = info.trans; + if (info.jointIndex != _hipsIndex) { + 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; diff --git a/libraries/animation/src/AnimInverseKinematics.h b/libraries/animation/src/AnimInverseKinematics.h index 7f7640aa24..bdfbad408d 100644 --- a/libraries/animation/src/AnimInverseKinematics.h +++ b/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& 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 _secondaryTargetsInRigFrame; + mutable std::map> _splineJointInfoMap; // experimental data for moving hips during IK diff --git a/libraries/animation/src/AnimUtil.cpp b/libraries/animation/src/AnimUtil.cpp index bcf30642e8..65c605b5ba 100644 --- a/libraries/animation/src/AnimUtil.cpp +++ b/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); +} diff --git a/libraries/animation/src/AnimUtil.h b/libraries/animation/src/AnimUtil.h index d215fdc654..f2cceb361b 100644 --- a/libraries/animation/src/AnimUtil.h +++ b/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 diff --git a/libraries/animation/src/Rig.cpp b/libraries/animation/src/Rig.cpp index fc0ca73c96..21f98d3e01 100644 --- a/libraries/animation/src/Rig.cpp +++ b/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 leftHandEnabled = params.controllerActiveFlags[ControllerType_LeftHand]; - bool rightHandEnabled = params.controllerActiveFlags[ControllerType_RightHand]; - bool hipsEnabled = params.controllerActiveFlags[ControllerType_Hips]; - bool leftFootEnabled = params.controllerActiveFlags[ControllerType_LeftFoot]; - bool rightFootEnabled = params.controllerActiveFlags[ControllerType_RightFoot]; - bool leftArmEnabled = params.controllerActiveFlags[ControllerType_LeftArm]; - bool rightArmEnabled = params.controllerActiveFlags[ControllerType_RightArm]; - bool spine2Enabled = params.controllerActiveFlags[ControllerType_Spine2]; + bool headEnabled = params.primaryControllerActiveFlags[PrimaryControllerType_Head]; + bool leftHandEnabled = params.primaryControllerActiveFlags[PrimaryControllerType_LeftHand]; + bool rightHandEnabled = params.primaryControllerActiveFlags[PrimaryControllerType_RightHand]; + bool hipsEnabled = params.primaryControllerActiveFlags[PrimaryControllerType_Hips]; + bool leftFootEnabled = params.primaryControllerActiveFlags[PrimaryControllerType_LeftFoot]; + bool rightFootEnabled = params.primaryControllerActiveFlags[PrimaryControllerType_RightFoot]; + bool spine2Enabled = params.primaryControllerActiveFlags[PrimaryControllerType_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("hipsRotation", params.controllerPoses[ControllerType_Hips].rot()); + _animVars.set("hipsPosition", params.primaryControllerPoses[PrimaryControllerType_Hips].trans()); + _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("spine2Rotation", params.controllerPoses[ControllerType_Spine2].rot()); + _animVars.set("spine2Position", params.primaryControllerPoses[PrimaryControllerType_Spine2].trans()); + _animVars.set("spine2Rotation", params.primaryControllerPoses[PrimaryControllerType_Spine2].rot()); } else { _animVars.set("spine2Type", (int)IKTarget::Type::Unknown); } - if (leftArmEnabled) { - _animVars.set("leftArmType", (int)IKTarget::Type::RotationAndPosition); - _animVars.set("leftArmPosition", params.controllerPoses[ControllerType_LeftArm].trans()); - _animVars.set("leftArmRotation", params.controllerPoses[ControllerType_LeftArm].rot()); - } else { - _animVars.set("leftArmType", (int)IKTarget::Type::Unknown); - } + // set secondary targets + static const std::vector secondaryControllerJointNames = { + "LeftShoulder", + "RightShoulder", + "LeftArm", + "RightArm", + "LeftForeArm", + "RightForeArm", + "LeftUpLeg", + "RightUpLeg", + "LeftLeg", + "RightLeg", + "LeftToeBase", + "RightToeBase" + }; - if (rightArmEnabled) { - _animVars.set("rightArmType", (int)IKTarget::Type::RotationAndPosition); - _animVars.set("rightArmPosition", params.controllerPoses[ControllerType_RightArm].trans()); - _animVars.set("rightArmRotation", params.controllerPoses[ControllerType_RightArm].rot()); - } else { - _animVars.set("rightArmType", (int)IKTarget::Type::Unknown); + std::shared_ptr ikNode = getAnimInverseKinematicsNode(); + for (int i = 0; i < (int)NumSecondaryControllerTypes; i++) { + int index = indexOfJoint(secondaryControllerJointNames[i]); + if (index >= 0) { + if (params.secondaryControllerActiveFlags[i]) { + ikNode->setSecondaryTargetInRigFrame(index, params.secondaryControllerPoses[i]); + } else { + ikNode->clearSecondaryTarget(index); + } + } } } diff --git a/libraries/animation/src/Rig.h b/libraries/animation/src/Rig.h index 5293fa1fe7..7e1504e461 100644 --- a/libraries/animation/src/Rig.h +++ b/libraries/animation/src/Rig.h @@ -41,22 +41,39 @@ public: bool useNames; }; - enum ControllerType { - ControllerType_Head = 0, - ControllerType_LeftHand, - ControllerType_RightHand, - ControllerType_Hips, - ControllerType_LeftFoot, - ControllerType_RightFoot, - ControllerType_LeftArm, - ControllerType_RightArm, - ControllerType_Spine2, - NumControllerTypes + enum PrimaryControllerType { + PrimaryControllerType_Head = 0, + PrimaryControllerType_LeftHand, + PrimaryControllerType_RightHand, + PrimaryControllerType_Hips, + PrimaryControllerType_LeftFoot, + PrimaryControllerType_RightFoot, + PrimaryControllerType_Spine2, + NumPrimaryControllerTypes + }; + + // 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 - bool controllerActiveFlags[NumControllerTypes]; + AnimPose primaryControllerPoses[NumPrimaryControllerTypes]; // rig space + bool primaryControllerActiveFlags[NumPrimaryControllerTypes]; + AnimPose secondaryControllerPoses[NumSecondaryControllerTypes]; // rig space + bool secondaryControllerActiveFlags[NumSecondaryControllerTypes]; bool isTalking; float bodyCapsuleRadius; float bodyCapsuleHalfHeight; diff --git a/libraries/controllers/src/controllers/Actions.h b/libraries/controllers/src/controllers/Actions.h index 6319b5746e..a133d62c9f 100644 --- a/libraries/controllers/src/controllers/Actions.h +++ b/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 diff --git a/libraries/shared/src/GeometryUtil.cpp b/libraries/shared/src/GeometryUtil.cpp index f853240fe3..4ae907eb3b 100644 --- a/libraries/shared/src/GeometryUtil.cpp +++ b/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; +} diff --git a/libraries/shared/src/GeometryUtil.h b/libraries/shared/src/GeometryUtil.h index 857d423896..a5ee67748b 100644 --- a/libraries/shared/src/GeometryUtil.h +++ b/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 diff --git a/plugins/openvr/CMakeLists.txt b/plugins/openvr/CMakeLists.txt index 6a95ef6d76..7878ae2d7e 100644 --- a/plugins/openvr/CMakeLists.txt +++ b/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}) diff --git a/plugins/openvr/src/ViveControllerManager.cpp b/plugins/openvr/src/ViveControllerManager.cpp index 07b3b2f73d..5a1c23839e 100644 --- a/plugins/openvr/src/ViveControllerManager.cpp +++ b/plugins/openvr/src/ViveControllerManager.cpp @@ -35,7 +35,6 @@ #include #include - 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(); 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(); - _headPuckZOffset = headObject["Z"].toDouble(); + _headPuckYOffset = headObject["Y"].toDouble() * CM_TO_M; + _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(); - _handPuckZOffset = handsObject["Z"].toDouble(); + _handPuckYOffset = handsObject["Y"].toDouble() * CM_TO_M; + _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())) { - return puckPose->second.postTransform(puckOffset->second); + if (puckPose != _poseStateMap.end()) { + 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) { diff --git a/plugins/openvr/src/ViveControllerManager.h b/plugins/openvr/src/ViveControllerManager.h index a9bcc7e4e2..9a7b2cbc93 100644 --- a/plugins/openvr/src/ViveControllerManager.h +++ b/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 configureHands(glm::mat4& defaultToReferenceMat, const controller::InputCalibrationData& inputCalibration); - bool configureBody(glm::mat4& defaultToReferenceMat, const controller::InputCalibrationData& inputCalibration); - void calibrateLeftHand(glm::mat4& defaultToReferenceMat, const controller::InputCalibrationData& inputCalibration, PuckPosePair& handPair); - void calibrateRightHand(glm::mat4& defaultToReferenceMat, const controller::InputCalibrationData& inputCalibration, PuckPosePair& handPair); - void calibrateFeet(glm::mat4& defaultToReferenceMat, const controller::InputCalibrationData& inputCalibration); - void calibrateFoot(glm::mat4& defaultToReferenceMat, const controller::InputCalibrationData& inputCalibration, PuckPosePair& footPair, bool isLeftFoot); - void calibrateHips(glm::mat4& defaultToReferenceMat, const controller::InputCalibrationData& inputCalibration); - void calibrateChest(glm::mat4& defaultToReferenceMat, const controller::InputCalibrationData& inputCalibration); - void calibrateShoulders(glm::mat4& defaultToReferenceMat, const controller::InputCalibrationData& inputCalibration, + bool configureHead(const glm::mat4& defaultToReferenceMat, const controller::InputCalibrationData& inputCalibration); + bool configureHands(const glm::mat4& defaultToReferenceMat, const controller::InputCalibrationData& inputCalibration); + bool configureBody(const glm::mat4& defaultToReferenceMat, const controller::InputCalibrationData& inputCalibration); + void calibrateLeftHand(const glm::mat4& defaultToReferenceMat, const controller::InputCalibrationData& inputCalibration, PuckPosePair& handPair); + void calibrateRightHand(const glm::mat4& defaultToReferenceMat, const controller::InputCalibrationData& inputCalibration, PuckPosePair& handPair); + void calibrateFeet(const glm::mat4& defaultToReferenceMat, const controller::InputCalibrationData& inputCalibration); + void calibrateFoot(const glm::mat4& defaultToReferenceMat, const controller::InputCalibrationData& inputCalibration, PuckPosePair& footPair, bool isLeftFoot); + void calibrateHips(const glm::mat4& defaultToReferenceMat, const controller::InputCalibrationData& inputCalibration); + void calibrateChest(const 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 _validTrackedObjects; - std::map _pucksOffset; + std::map _pucksPostOffset; + std::map _pucksPreOffset; std::map _jointToPuckMap; std::map _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 };