From 22d8adcb9951ab22c3b10693b692c6edb4ee7c23 Mon Sep 17 00:00:00 2001 From: "Anthony J. Thibault" Date: Thu, 13 Jul 2017 10:39:48 -0700 Subject: [PATCH 01/14] Better shoulder/arm puck calibration. --- .../resources/avatar/avatar-animation.json | 36 +++++----- plugins/openvr/src/ViveControllerManager.cpp | 66 ++++++++++++++----- plugins/openvr/src/ViveControllerManager.h | 3 +- 3 files changed, 71 insertions(+), 34 deletions(-) diff --git a/interface/resources/avatar/avatar-animation.json b/interface/resources/avatar/avatar-animation.json index a493d8e9af..a179f452c1 100644 --- a/interface/resources/avatar/avatar-animation.json +++ b/interface/resources/avatar/avatar-animation.json @@ -61,6 +61,24 @@ "weight": 1.0, "flexCoefficients": [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] + }, { "jointName": "RightHand", "positionVar": "rightHandPosition", @@ -126,24 +144,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/plugins/openvr/src/ViveControllerManager.cpp b/plugins/openvr/src/ViveControllerManager.cpp index 07b3b2f73d..216d50a4af 100644 --- a/plugins/openvr/src/ViveControllerManager.cpp +++ b/plugins/openvr/src/ViveControllerManager.cpp @@ -624,7 +624,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 +655,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(); @@ -970,7 +978,7 @@ 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) { @@ -1001,7 +1009,7 @@ 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; } @@ -1037,21 +1045,21 @@ 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) { _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) { _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, @@ -1061,16 +1069,44 @@ void ViveControllerManager::InputDevice::calibrateShoulders(glm::mat4& defaultTo 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; + const float PUCK_TO_USER_REF_ARM_Y_OFFSET = -0.06; + 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); + + // move the userRefArm to the same height as the puck. + userRefLeftArm[3][1] = firstShoulderPose.translation.y + PUCK_TO_USER_REF_ARM_Y_OFFSET; + userRefRightArm[3][1] = secondShoulderPose.translation.y + PUCK_TO_USER_REF_ARM_Y_OFFSET; + + // 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); + + // move the userRefArm to the same height as the puck + userRefLeftArm[3][1] = secondShoulderPose.translation.y + PUCK_TO_USER_REF_ARM_Y_OFFSET; + userRefRightArm[3][1] = firstShoulderPose.translation.y + PUCK_TO_USER_REF_ARM_Y_OFFSET; + + // 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)); } } @@ -1078,7 +1114,7 @@ void ViveControllerManager::InputDevice::calibrateHead(glm::mat4& defaultToRefer 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..d82b895579 100644 --- a/plugins/openvr/src/ViveControllerManager.h +++ b/plugins/openvr/src/ViveControllerManager.h @@ -161,7 +161,8 @@ private: FilteredStick _filteredRightStick; std::vector _validTrackedObjects; - std::map _pucksOffset; + std::map _pucksPostOffset; + std::map _pucksPreOffset; std::map _jointToPuckMap; std::map _configStringMap; PoseData _lastSimPoseData; From fe711ccbe9044fe6b3aaf4e026f472a15feab949 Mon Sep 17 00:00:00 2001 From: "Anthony J. Thibault" Date: Thu, 13 Jul 2017 15:01:14 -0700 Subject: [PATCH 02/14] Refactored how controller poses are stored in MyAvatar. --- interface/src/Application.cpp | 105 +++++----- interface/src/avatar/AvatarActionHold.cpp | 4 +- interface/src/avatar/MyAvatar.cpp | 228 ++++++---------------- interface/src/avatar/MyAvatar.h | 60 +----- interface/src/avatar/MyHead.cpp | 2 +- interface/src/avatar/MySkeletonModel.cpp | 148 +++++++++----- interface/src/avatar/MySkeletonModel.h | 2 +- 7 files changed, 228 insertions(+), 321 deletions(-) diff --git a/interface/src/Application.cpp b/interface/src/Application.cpp index ddd1870723..9497456677 100644 --- a/interface/src/Application.cpp +++ b/interface/src/Application.cpp @@ -4335,10 +4335,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)); @@ -4750,52 +4749,64 @@ 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 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 + }; - // 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) - }; - } + 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 f6fb87dad9..3b480ac9bf 100755 --- a/interface/src/avatar/MyAvatar.cpp +++ b/interface/src/avatar/MyAvatar.cpp @@ -422,7 +422,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() + @@ -657,7 +657,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 { @@ -753,37 +753,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; @@ -791,7 +791,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; @@ -1425,159 +1425,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() { @@ -1635,7 +1519,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 { @@ -1869,8 +1753,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)); @@ -2023,7 +1907,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 @@ -2641,10 +2525,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); @@ -2953,19 +2837,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); @@ -2990,19 +2874,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 4d599230fb..9b9f841ff0 100644 --- a/interface/src/avatar/MyAvatar.h +++ b/interface/src/avatar/MyAvatar.h @@ -466,49 +466,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; @@ -794,18 +757,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::unordered_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 9f2d080cd6..40d1fa3ffc 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..f97b20dcca 100644 --- a/interface/src/avatar/MySkeletonModel.cpp +++ b/interface/src/avatar/MySkeletonModel.cpp @@ -46,13 +46,14 @@ 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; @@ -67,7 +68,7 @@ void MySkeletonModel::updateRig(float deltaTime, glm::mat4 parentTransform) { params.controllerActiveFlags[Rig::ControllerType_Head] = false; } - auto avatarHipsPose = myAvatar->getHipsControllerPoseInAvatarFrame(); + auto avatarHipsPose = myAvatar->getControllerPoseInAvatarFrame(controller::Action::HIPS); if (avatarHipsPose.isValid()) { AnimPose pose(avatarHipsPose.getRotation(), avatarHipsPose.getTranslation()); params.controllerPoses[Rig::ControllerType_Hips] = avatarToRigPose * pose; @@ -77,7 +78,7 @@ void MySkeletonModel::updateRig(float deltaTime, glm::mat4 parentTransform) { params.controllerActiveFlags[Rig::ControllerType_Hips] = false; } - auto avatarSpine2Pose = myAvatar->getSpine2ControllerPoseInAvatarFrame(); + auto avatarSpine2Pose = myAvatar->getControllerPoseInAvatarFrame(controller::Action::SPINE2); if (avatarSpine2Pose.isValid()) { AnimPose pose(avatarSpine2Pose.getRotation(), avatarSpine2Pose.getTranslation()); params.controllerPoses[Rig::ControllerType_Spine2] = avatarToRigPose * pose; @@ -87,7 +88,7 @@ void MySkeletonModel::updateRig(float deltaTime, glm::mat4 parentTransform) { params.controllerActiveFlags[Rig::ControllerType_Spine2] = false; } - auto avatarRightArmPose = myAvatar->getRightArmControllerPoseInAvatarFrame(); + auto avatarRightArmPose = myAvatar->getControllerPoseInAvatarFrame(controller::Action::RIGHT_ARM); if (avatarRightArmPose.isValid()) { AnimPose pose(avatarRightArmPose.getRotation(), avatarRightArmPose.getTranslation()); params.controllerPoses[Rig::ControllerType_RightArm] = avatarToRigPose * pose; @@ -96,8 +97,8 @@ void MySkeletonModel::updateRig(float deltaTime, glm::mat4 parentTransform) { params.controllerPoses[Rig::ControllerType_RightArm] = AnimPose::identity; params.controllerActiveFlags[Rig::ControllerType_RightArm] = false; } - - auto avatarLeftArmPose = myAvatar->getLeftArmControllerPoseInAvatarFrame(); + + auto avatarLeftArmPose = myAvatar->getControllerPoseInAvatarFrame(controller::Action::LEFT_ARM); if (avatarLeftArmPose.isValid()) { AnimPose pose(avatarLeftArmPose.getRotation(), avatarLeftArmPose.getTranslation()); params.controllerPoses[Rig::ControllerType_LeftArm] = avatarToRigPose * pose; @@ -107,7 +108,7 @@ void MySkeletonModel::updateRig(float deltaTime, glm::mat4 parentTransform) { params.controllerActiveFlags[Rig::ControllerType_LeftArm] = false; } - auto avatarLeftHandPose = myAvatar->getLeftHandControllerPoseInAvatarFrame(); + auto avatarLeftHandPose = myAvatar->getControllerPoseInAvatarFrame(controller::Action::LEFT_HAND); if (avatarLeftHandPose.isValid()) { AnimPose pose(avatarLeftHandPose.getRotation(), avatarLeftHandPose.getTranslation()); params.controllerPoses[Rig::ControllerType_LeftHand] = avatarToRigPose * pose; @@ -117,7 +118,7 @@ void MySkeletonModel::updateRig(float deltaTime, glm::mat4 parentTransform) { params.controllerActiveFlags[Rig::ControllerType_LeftHand] = false; } - auto avatarRightHandPose = myAvatar->getRightHandControllerPoseInAvatarFrame(); + auto avatarRightHandPose = myAvatar->getControllerPoseInAvatarFrame(controller::Action::RIGHT_HAND); if (avatarRightHandPose.isValid()) { AnimPose pose(avatarRightHandPose.getRotation(), avatarRightHandPose.getTranslation()); params.controllerPoses[Rig::ControllerType_RightHand] = avatarToRigPose * pose; @@ -127,7 +128,7 @@ void MySkeletonModel::updateRig(float deltaTime, glm::mat4 parentTransform) { params.controllerActiveFlags[Rig::ControllerType_RightHand] = false; } - auto avatarLeftFootPose = myAvatar->getLeftFootControllerPoseInAvatarFrame(); + auto avatarLeftFootPose = myAvatar->getControllerPoseInAvatarFrame(controller::Action::LEFT_FOOT); if (avatarLeftFootPose.isValid()) { AnimPose pose(avatarLeftFootPose.getRotation(), avatarLeftFootPose.getTranslation()); params.controllerPoses[Rig::ControllerType_LeftFoot] = avatarToRigPose * pose; @@ -137,7 +138,7 @@ void MySkeletonModel::updateRig(float deltaTime, glm::mat4 parentTransform) { params.controllerActiveFlags[Rig::ControllerType_LeftFoot] = false; } - auto avatarRightFootPose = myAvatar->getRightFootControllerPoseInAvatarFrame(); + auto avatarRightFootPose = myAvatar->getControllerPoseInAvatarFrame(controller::Action::RIGHT_FOOT); if (avatarRightFootPose.isValid()) { AnimPose pose(avatarRightFootPose.getRotation(), avatarRightFootPose.getTranslation()); params.controllerPoses[Rig::ControllerType_RightFoot] = avatarToRigPose * pose; @@ -175,49 +176,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 From 2e0bc36cfda23a12cc22691ae0e68b425cd1ed1a Mon Sep 17 00:00:00 2001 From: "Anthony J. Thibault" Date: Thu, 13 Jul 2017 15:02:05 -0700 Subject: [PATCH 03/14] warning fix + whitespace --- plugins/openvr/src/ViveControllerManager.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/plugins/openvr/src/ViveControllerManager.cpp b/plugins/openvr/src/ViveControllerManager.cpp index 216d50a4af..ddafa5cb5b 100644 --- a/plugins/openvr/src/ViveControllerManager.cpp +++ b/plugins/openvr/src/ViveControllerManager.cpp @@ -716,7 +716,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); - } + } } } @@ -1073,7 +1073,7 @@ void ViveControllerManager::InputDevice::calibrateShoulders(glm::mat4& defaultTo glm::mat4 refRightArm = defaultToReferenceMat * inputCalibration.defaultRightArm; glm::mat4 userRefLeftArm = refLeftArm; glm::mat4 userRefRightArm = refRightArm; - const float PUCK_TO_USER_REF_ARM_Y_OFFSET = -0.06; + const float PUCK_TO_USER_REF_ARM_Y_OFFSET = -0.06f; if (firstShoulderPose.translation.x < secondShoulderPose.translation.x) { _jointToPuckMap[controller::LEFT_ARM] = firstShoulder.first; From c85e187c6174c9894a660b433464519db1e0ccad Mon Sep 17 00:00:00 2001 From: "Anthony J. Thibault" Date: Thu, 13 Jul 2017 18:12:33 -0700 Subject: [PATCH 04/14] first stab at secondary target pose support --- interface/src/Application.cpp | 16 ++- interface/src/avatar/MySkeletonModel.cpp | 132 +++++++----------- .../animation/src/AnimInverseKinematics.cpp | 31 ++++ .../animation/src/AnimInverseKinematics.h | 6 + libraries/animation/src/Rig.cpp | 71 ++++++---- libraries/animation/src/Rig.h | 43 ++++-- .../controllers/src/controllers/Actions.h | 14 +- 7 files changed, 190 insertions(+), 123 deletions(-) diff --git a/interface/src/Application.cpp b/interface/src/Application.cpp index 9497456677..37001b55bb 100644 --- a/interface/src/Application.cpp +++ b/interface/src/Application.cpp @@ -4749,7 +4749,7 @@ void Application::update(float deltaTime) { myAvatar->setDriveKey(MyAvatar::ZOOM, userInputMapper->getActionState(controller::Action::TRANSLATE_CAMERA_Z)); } - static std::vector avatarControllerActions = { + static const std::vector avatarControllerActions = { controller::Action::LEFT_HAND, controller::Action::RIGHT_HAND, controller::Action::LEFT_FOOT, @@ -4796,7 +4796,19 @@ void Application::update(float deltaTime) { controller::Action::RIGHT_HAND_PINKY1, controller::Action::RIGHT_HAND_PINKY2, controller::Action::RIGHT_HAND_PINKY3, - controller::Action::RIGHT_HAND_PINKY4 + 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 }; glm::mat4 myAvatarMatrix = createMatFromQuatAndPos(myAvatar->getOrientation(), myAvatar->getPosition()); diff --git a/interface/src/avatar/MySkeletonModel.cpp b/interface/src/avatar/MySkeletonModel.cpp index f97b20dcca..4a86e0ce0a 100644 --- a/interface/src/avatar/MySkeletonModel.cpp +++ b/interface/src/avatar/MySkeletonModel.cpp @@ -56,96 +56,72 @@ void MySkeletonModel::updateRig(float deltaTime, glm::mat4 parentTransform) { 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->getControllerPoseInAvatarFrame(controller::Action::HIPS); - 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 pose = myAvatar->getControllerPoseInAvatarFrame(pair.first); + if (pose.isValid()) { + AnimPose pose(pose.getRotation(), pose.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->getControllerPoseInAvatarFrame(controller::Action::SPINE2); - 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->getControllerPoseInAvatarFrame(controller::Action::RIGHT_ARM); - 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; - } + 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 avatarLeftArmPose = myAvatar->getControllerPoseInAvatarFrame(controller::Action::LEFT_ARM); - 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; - } - - auto avatarLeftHandPose = myAvatar->getControllerPoseInAvatarFrame(controller::Action::LEFT_HAND); - 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->getControllerPoseInAvatarFrame(controller::Action::RIGHT_HAND); - 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->getControllerPoseInAvatarFrame(controller::Action::LEFT_FOOT); - 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->getControllerPoseInAvatarFrame(controller::Action::RIGHT_FOOT); - 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 pose = myAvatar->getControllerPoseInAvatarFrame(pair.first); + if (pose.isValid()) { + AnimPose pose(pose.getRotation(), pose.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(); diff --git a/libraries/animation/src/AnimInverseKinematics.cpp b/libraries/animation/src/AnimInverseKinematics.cpp index d7076a443e..86ebe5fc91 100644 --- a/libraries/animation/src/AnimInverseKinematics.cpp +++ b/libraries/animation/src/AnimInverseKinematics.cpp @@ -921,6 +921,7 @@ const AnimPoseVec& AnimInverseKinematics::overlay(const AnimVariantMap& animVars { PROFILE_RANGE_EX(simulation_animation, "ik/ccd", 0xffff00ff, 0); preconditionRelativePosesToAvoidLimbLock(context, targets); + setSecondaryTargets(context); solve(context, targets); } @@ -1015,6 +1016,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); @@ -1722,6 +1739,20 @@ void AnimInverseKinematics::preconditionRelativePosesToAvoidLimbLock(const AnimC } } +// overwrites _relativePoses with secondary poses. +void AnimInverseKinematics::setSecondaryTargets(const AnimContext& context) { + AnimPose rigToGeometryPose = AnimPose(glm::inverse(context.getGeometryToRigMatrix())); + for (auto& iter : _secondaryTargetsInRigFrame) { + AnimPose absPose = rigToGeometryPose * iter.second; + AnimPose parentAbsPose; + int parentIndex = _skeleton->getParentIndex(iter.first); + if (parentIndex >= 0) { + parentAbsPose = _skeleton->getAbsolutePose(parentIndex, _relativePoses); + } + _relativePoses[iter.first] = parentAbsPose.inverse() * absPose; + } +} + 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 d473ae3698..2ca333323a 100644 --- a/libraries/animation/src/AnimInverseKinematics.h +++ b/libraries/animation/src/AnimInverseKinematics.h @@ -63,6 +63,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; } @@ -83,6 +86,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); // used to pre-compute information about each joint influeced by a spline IK target. struct SplineJointInfo { @@ -136,6 +140,8 @@ protected: AnimPoseVec _relativePoses; // current relative poses AnimPoseVec _limitCenterPoses; // relative + std::map _secondaryTargetsInRigFrame; + std::map> _splineJointInfoMap; // experimental data for moving hips during IK diff --git a/libraries/animation/src/Rig.cpp b/libraries/animation/src/Rig.cpp index 6ebb68773f..9db8b187b6 100644 --- a/libraries/animation/src/Rig.cpp +++ b/libraries/animation/src/Rig.cpp @@ -1474,24 +1474,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) { @@ -1512,34 +1513,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 c17a7b9c8f..5718c62c26 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 ec4800c9aa..9aa9bbdb29 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,7 +147,18 @@ enum class Action { RIGHT_HAND_PINKY3, RIGHT_HAND_PINKY4, - NUM_ACTIONS, + 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, + + NUM_ACTIONS }; template From 9f6641ed1087d5926ea4d95378af20ab8501b60a Mon Sep 17 00:00:00 2001 From: "Anthony J. Thibault" Date: Mon, 24 Jul 2017 17:22:48 -0700 Subject: [PATCH 05/14] Shoulder puck calibration work in progress * AnimInverseKinematics: debug draw for secondary targets * AnimInverseKienmatics: better clean up of ik target debug draw * GeometryUtil: added findPlaneFromPoints() * ViveControllerManager: external dependency on eigen * ViveControllerManager: record history of left/right hand controllers * ViveControllerManager: use history to determine user shoulder location for better calibration * ViveControllerManager: pass defaultToReferenceMat by const ref to calibrate functions. * CMake: added external depenency to eigen linear algebra library. --- cmake/externals/eigen/CMakeLists.txt | 20 +++ cmake/modules/FindEigen.cmake | 26 ++++ .../resources/avatar/avatar-animation.json | 18 --- interface/src/Application.cpp | 2 +- .../animation/src/AnimInverseKinematics.cpp | 29 +++- libraries/shared/src/GeometryUtil.cpp | 52 ++++++++ libraries/shared/src/GeometryUtil.h | 2 + plugins/openvr/CMakeLists.txt | 6 + plugins/openvr/src/ViveControllerManager.cpp | 126 ++++++++++++++---- plugins/openvr/src/ViveControllerManager.h | 27 ++-- 10 files changed, 252 insertions(+), 56 deletions(-) create mode 100644 cmake/externals/eigen/CMakeLists.txt create mode 100644 cmake/modules/FindEigen.cmake diff --git a/cmake/externals/eigen/CMakeLists.txt b/cmake/externals/eigen/CMakeLists.txt new file mode 100644 index 0000000000..15d94576cc --- /dev/null +++ b/cmake/externals/eigen/CMakeLists.txt @@ -0,0 +1,20 @@ +set(EXTERNAL_NAME eigen) + +include(ExternalProject) +ExternalProject_Add( + ${EXTERNAL_NAME} + URL http://bitbucket.org/eigen/eigen/get/3.3.4.zip + URL_MD5 e337acc279874bc6a56da4d973a723fb + CONFIGURE_COMMAND "" + BUILD_COMMAND "" + INSTALL_COMMAND "" + LOG_DOWNLOAD 1 +) + +# Hide this external target (for ide users) +set_target_properties(${EXTERNAL_NAME} PROPERTIES FOLDER "hidden/externals") + +ExternalProject_Get_Property(${EXTERNAL_NAME} INSTALL_DIR) + +string(TOUPPER ${EXTERNAL_NAME} EXTERNAL_NAME_UPPER) +set(${EXTERNAL_NAME_UPPER}_INCLUDE_DIRS ${INSTALL_DIR} CACHE PATH "List of eigen include directories") \ No newline at end of file diff --git a/cmake/modules/FindEigen.cmake b/cmake/modules/FindEigen.cmake new file mode 100644 index 0000000000..4b09ed8f2b --- /dev/null +++ b/cmake/modules/FindEigen.cmake @@ -0,0 +1,26 @@ +# +# FindEigen.cmake +# +# Try to find Eigen include path. +# Once done this will define +# +# EIGEN_INCLUDE_DIRS +# +# Created on 7/14/2017 by Anthony Thibault +# Copyright 2017 High Fidelity, Inc. +# +# Distributed under the Apache License, Version 2.0. +# See the accompanying file LICENSE or http://www.apache.org/licenses/LICENSE-2.0.html +# + +# setup hints for Eigen search +include("${MACRO_DIR}/HifiLibrarySearchHints.cmake") +hifi_library_search_hints("eigen") + +# locate dir +string(CONCAT EIGEN_INCLUDE_DIRS ${EIGEN_INCLUDE_DIRS} "/src/eigen") + +include(FindPackageHandleStandardArgs) +find_package_handle_standard_args(EIGEN DEFAULT_MSG EIGEN_INCLUDE_DIRS) + +mark_as_advanced(EIGEN_INCLUDE_DIRS EIGEN_SEARCH_DIRS) \ No newline at end of file diff --git a/interface/resources/avatar/avatar-animation.json b/interface/resources/avatar/avatar-animation.json index a179f452c1..ee2b916d1e 100644 --- a/interface/resources/avatar/avatar-animation.json +++ b/interface/resources/avatar/avatar-animation.json @@ -61,24 +61,6 @@ "weight": 1.0, "flexCoefficients": [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] - }, { "jointName": "RightHand", "positionVar": "rightHandPosition", diff --git a/interface/src/Application.cpp b/interface/src/Application.cpp index 37001b55bb..7b10c9a364 100644 --- a/interface/src/Application.cpp +++ b/interface/src/Application.cpp @@ -4811,10 +4811,10 @@ void Application::update(float deltaTime) { controller::Action::RIGHT_TOE_BASE }; + // 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)); diff --git a/libraries/animation/src/AnimInverseKinematics.cpp b/libraries/animation/src/AnimInverseKinematics.cpp index 86ebe5fc91..908519880d 100644 --- a/libraries/animation/src/AnimInverseKinematics.cpp +++ b/libraries/animation/src/AnimInverseKinematics.cpp @@ -23,6 +23,8 @@ #include "CubicHermiteSpline.h" #include "AnimUtil.h" +static const int MAX_TARGET_MARKERS = 30; + static void lookupJointChainInfo(AnimInverseKinematics::JointChainInfo* jointChainInfos, size_t numJointChainInfos, int indexA, int indexB, AnimInverseKinematics::JointChainInfo** jointChainInfoA, @@ -94,6 +96,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) { @@ -898,19 +906,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); } } @@ -1744,12 +1763,16 @@ void AnimInverseKinematics::setSecondaryTargets(const AnimContext& context) { 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); } + // AJT: for now ignore translation on secondary poses. + glm::vec3 origTrans = _relativePoses[iter.first].trans(); _relativePoses[iter.first] = parentAbsPose.inverse() * absPose; + _relativePoses[iter.first].trans() = origTrans; } } 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..6633b99b31 100644 --- a/plugins/openvr/CMakeLists.txt +++ b/plugins/openvr/CMakeLists.txt @@ -18,8 +18,14 @@ if (WIN32) include_hifi_library_headers(octree) add_dependency_external_projects(OpenVR) + add_dependency_external_projects(eigen) + find_package(OpenVR REQUIRED) target_include_directories(${TARGET_NAME} PRIVATE ${OPENVR_INCLUDE_DIRS}) target_link_libraries(${TARGET_NAME} ${OPENVR_LIBRARIES}) target_link_libraries(${TARGET_NAME} Winmm.lib) + + # header only library + find_package(eigen REQUIRED) + target_include_directories(${TARGET_NAME} PRIVATE ${EIGEN_INCLUDE_DIRS}) endif() diff --git a/plugins/openvr/src/ViveControllerManager.cpp b/plugins/openvr/src/ViveControllerManager.cpp index ddafa5cb5b..02d3afd713 100644 --- a/plugins/openvr/src/ViveControllerManager.cpp +++ b/plugins/openvr/src/ViveControllerManager.cpp @@ -35,6 +35,7 @@ #include #include +#include extern PoseData _nextSimPoseData; @@ -282,7 +283,13 @@ void ViveControllerManager::pluginUpdate(float deltaTime, const controller::Inpu } } -ViveControllerManager::InputDevice::InputDevice(vr::IVRSystem*& system) : controller::InputDevice("Vive"), _system(system) { +static const size_t CONTROLLER_HISTORY_SIZE = 90 * 3; + +ViveControllerManager::InputDevice::InputDevice(vr::IVRSystem*& system) : + controller::InputDevice("Vive"), + _system(system), + _leftControllerHistory(CONTROLLER_HISTORY_SIZE), + _rightControllerHistory(CONTROLLER_HISTORY_SIZE) { _configStringMap[Config::None] = QString("None"); _configStringMap[Config::Feet] = QString("Feet"); @@ -534,7 +541,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 +576,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 +590,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); @@ -637,8 +644,17 @@ void ViveControllerManager::InputDevice::updateCalibratedLimbs() { _poseStateMap[controller::RIGHT_FOOT] = addOffsetToPuckPose(controller::RIGHT_FOOT); _poseStateMap[controller::HIPS] = addOffsetToPuckPose(controller::HIPS); _poseStateMap[controller::SPINE2] = addOffsetToPuckPose(controller::SPINE2); - _poseStateMap[controller::RIGHT_ARM] = addOffsetToPuckPose(controller::RIGHT_ARM); - _poseStateMap[controller::LEFT_ARM] = addOffsetToPuckPose(controller::LEFT_ARM); + controller::Pose rightArmPose = addOffsetToPuckPose(controller::RIGHT_ARM); + _poseStateMap[controller::RIGHT_ARM] = rightArmPose; + if (rightArmPose.isValid()) { + // AJT: TODO: compute rotation for RIGHT_SHOULDER AS WELL. + } + + controller::Pose leftArmPose = addOffsetToPuckPose(controller::LEFT_ARM); + _poseStateMap[controller::LEFT_ARM] = leftArmPose; + if (leftArmPose.isValid()) { + // AJT: TODO: compute rotation for LEFT_SHOULDER AS WELL. + } if (_overrideHead) { _poseStateMap[controller::HEAD] = addOffsetToPuckPose(controller::HEAD); @@ -899,6 +915,16 @@ void ViveControllerManager::InputDevice::handlePoseEvent(float deltaTime, const // transform into avatar frame glm::mat4 controllerToAvatar = glm::inverse(inputCalibrationData.avatarMat) * inputCalibrationData.sensorToWorldMat; _poseStateMap[isLeftHand ? controller::LEFT_HAND : controller::RIGHT_HAND] = pose.transform(controllerToAvatar); + + // AJT: TODO ONLY DO THIS IF CALIBRATING! + // record controller history + if (isLeftHand) { + _leftControllerHistory[_leftControllerHistoryCursor] = pose.translation; + _leftControllerHistoryCursor = (_leftControllerHistoryCursor + 1) % CONTROLLER_HISTORY_SIZE; + } else { + _rightControllerHistory[_rightControllerHistoryCursor] = pose.translation; + _rightControllerHistoryCursor = (_rightControllerHistoryCursor + 1) % CONTROLLER_HISTORY_SIZE; + } } bool ViveControllerManager::InputDevice::triggerHapticPulse(float strength, float duration, controller::Hand hand) { @@ -950,7 +976,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); @@ -981,7 +1007,7 @@ void ViveControllerManager::InputDevice::calibrateLeftHand(glm::mat4& defaultToR _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); @@ -1013,7 +1039,7 @@ void ViveControllerManager::InputDevice::calibrateRightHand(glm::mat4& defaultTo } -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]; @@ -1030,7 +1056,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; @@ -1052,17 +1078,75 @@ void ViveControllerManager::InputDevice::calibrateFoot(glm::mat4& defaultToRefer } } -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; _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; _pucksPostOffset[_validTrackedObjects[CHEST].first] = computeOffset(defaultToReferenceMat, inputCalibration.defaultSpine2, _validTrackedObjects[CHEST].second); } -void ViveControllerManager::InputDevice::calibrateShoulders(glm::mat4& defaultToReferenceMat, const controller::InputCalibrationData& inputCalibration, +// assuming the user has kept his arms straight to his sides and has rotated his hand controllers +// up and down laterally about his shoulders. This will attempt to locate the users actual shoulders +// by calculating a best fit circle around the points, in the plane of the head's forward normal. +static glm::vec3 computeUserShoulderPositionFromHistory(const glm::mat4& headMat, const std::vector& history) { + glm::mat4 invHeadMat = glm::inverse(headMat); + + // AJT: TODO: we could perhaps reject the approximation if the radius is larger or smaller then we expect. + // AJT: TODO: abort if history is too small or invalid... + + // Take equation for a circle: (x - h)^2 + (y - k)^2 = r^2 + // And put it into the linear form: A * x = b + // + // where A = | (2 * x0) (2 * y0) 1 | + // | (2 * x1) (2 * y1) 1 | + // | . . . | + // | (2 * xn) (2 * yn) 1 | + // + // and b = | x0^2 + y0^2 | + // | x1^2 + y1^2 | + // | . | + // | xn^2 + yn^2 | + // + // and x = | h | + // | k | + // | r^2 - h^2 - k^2 | + // + + // Build aMat and bMat + Eigen::MatrixXf aMat(CONTROLLER_HISTORY_SIZE, 3); + Eigen::MatrixXf bMat(CONTROLLER_HISTORY_SIZE, 1); + for (int r = 0; r < CONTROLLER_HISTORY_SIZE; r++) { + // transform history[r] into local head coordinates + glm::vec3 p = transformPoint(invHeadMat, history[r]); + + // update the aMat with the appropriate 2d history values + aMat(r, 0) = 2.0f * p.x; + aMat(r, 1) = 2.0f * p.y; + aMat(r, 2) = 1.0f; + + // update the bMat with the appropriate 2d history values + bMat(r, 0) = p.x * p.x + p.y * p.y; + } + + // Then use least squares approximation to solve for the best x. + // http://math.mit.edu/~gs/linearalgebra/ila0403.pdf + Eigen::MatrixXf aTransMat = aMat.transpose(); + Eigen::MatrixXf xMat = (aTransMat * aMat).inverse() * aTransMat * bMat; + + // extract the 2d center of the circle from the xMat + glm::vec3 center(xMat(0, 0), xMat(1, 0), 0.0f); + + // we don't need the radius, but it's included here for completeness. + // float radius = center.x * center.x + center.y * center.y - xMat(2, 0); + + // transform center back into sensor coordinates + return transformPoint(headMat, center); +} + +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]; @@ -1071,18 +1155,18 @@ void ViveControllerManager::InputDevice::calibrateShoulders(glm::mat4& defaultTo glm::mat4 refLeftArm = defaultToReferenceMat * inputCalibration.defaultLeftArm; glm::mat4 refRightArm = defaultToReferenceMat * inputCalibration.defaultRightArm; + glm::mat4 userRefLeftArm = refLeftArm; glm::mat4 userRefRightArm = refRightArm; - const float PUCK_TO_USER_REF_ARM_Y_OFFSET = -0.06f; + + glm::mat4 headMat = defaultToReferenceMat * inputCalibration.defaultHeadMat; + userRefLeftArm[3] = glm::vec4(computeUserShoulderPositionFromHistory(headMat, _leftControllerHistory), 1.0f); + userRefRightArm[3] = glm::vec4(computeUserShoulderPositionFromHistory(headMat, _rightControllerHistory), 1.0f); if (firstShoulderPose.translation.x < secondShoulderPose.translation.x) { _jointToPuckMap[controller::LEFT_ARM] = firstShoulder.first; _jointToPuckMap[controller::RIGHT_ARM] = secondShoulder.first; - // move the userRefArm to the same height as the puck. - userRefLeftArm[3][1] = firstShoulderPose.translation.y + PUCK_TO_USER_REF_ARM_Y_OFFSET; - userRefRightArm[3][1] = secondShoulderPose.translation.y + PUCK_TO_USER_REF_ARM_Y_OFFSET; - // compute the post offset from the userRefArm _pucksPostOffset[firstShoulder.first] = computeOffset(Matrices::IDENTITY, userRefLeftArm, firstShoulderPose); _pucksPostOffset[secondShoulder.first] = computeOffset(Matrices::IDENTITY, userRefRightArm, secondShoulderPose); @@ -1095,10 +1179,6 @@ void ViveControllerManager::InputDevice::calibrateShoulders(glm::mat4& defaultTo _jointToPuckMap[controller::LEFT_ARM] = secondShoulder.first; _jointToPuckMap[controller::RIGHT_ARM] = firstShoulder.first; - // move the userRefArm to the same height as the puck - userRefLeftArm[3][1] = secondShoulderPose.translation.y + PUCK_TO_USER_REF_ARM_Y_OFFSET; - userRefRightArm[3][1] = firstShoulderPose.translation.y + PUCK_TO_USER_REF_ARM_Y_OFFSET; - // compute the post offset from the userRefArm _pucksPostOffset[secondShoulder.first] = computeOffset(Matrices::IDENTITY, userRefLeftArm, secondShoulderPose); _pucksPostOffset[firstShoulder.first] = computeOffset(Matrices::IDENTITY, userRefRightArm, firstShoulderPose); @@ -1110,7 +1190,7 @@ void ViveControllerManager::InputDevice::calibrateShoulders(glm::mat4& defaultTo } } -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; diff --git a/plugins/openvr/src/ViveControllerManager.h b/plugins/openvr/src/ViveControllerManager.h index d82b895579..329901cd25 100644 --- a/plugins/openvr/src/ViveControllerManager.h +++ b/plugins/openvr/src/ViveControllerManager.h @@ -93,18 +93,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(); @@ -190,6 +190,11 @@ private: bool _overrideHands { false }; mutable std::recursive_mutex _lock; + std::vector _leftControllerHistory; + size_t _leftControllerHistoryCursor { 0 }; + std::vector _rightControllerHistory; + size_t _rightControllerHistoryCursor { 0 }; + QString configToString(Config config); friend class ViveControllerManager; }; From c81875a2808ada23429daa191aef5789842097ed Mon Sep 17 00:00:00 2001 From: "Anthony J. Thibault" Date: Tue, 25 Jul 2017 15:24:09 -0700 Subject: [PATCH 06/14] Improved shoulder calibration using hard-coded measured shoulder width --- .../animation/src/AnimInverseKinematics.cpp | 25 ++++++++++++ libraries/animation/src/AnimUtil.cpp | 11 +++++ libraries/animation/src/AnimUtil.h | 2 + plugins/openvr/src/ViveControllerManager.cpp | 40 ++++++++++++++----- 4 files changed, 67 insertions(+), 11 deletions(-) diff --git a/libraries/animation/src/AnimInverseKinematics.cpp b/libraries/animation/src/AnimInverseKinematics.cpp index b9eb1c2912..10c4011f52 100644 --- a/libraries/animation/src/AnimInverseKinematics.cpp +++ b/libraries/animation/src/AnimInverseKinematics.cpp @@ -1862,15 +1862,40 @@ void AnimInverseKinematics::preconditionRelativePosesToAvoidLimbLock(const AnimC // overwrites _relativePoses with secondary poses. void AnimInverseKinematics::setSecondaryTargets(const AnimContext& context) { + + if (_secondaryTargetsInRigFrame.empty()) { + return; + } + + // shoulder joint should look-at position of arm joint. + const int leftArmIndex = _skeleton->nameToJointIndex("LeftArm"); + const int rightArmIndex = _skeleton->nameToJointIndex("RightArm"); + 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 (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; + } + // AJT: for now ignore translation on secondary poses. glm::vec3 origTrans = _relativePoses[iter.first].trans(); _relativePoses[iter.first] = parentAbsPose.inverse() * absPose; 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/plugins/openvr/src/ViveControllerManager.cpp b/plugins/openvr/src/ViveControllerManager.cpp index 02d3afd713..4c34a4cd27 100644 --- a/plugins/openvr/src/ViveControllerManager.cpp +++ b/plugins/openvr/src/ViveControllerManager.cpp @@ -644,17 +644,8 @@ void ViveControllerManager::InputDevice::updateCalibratedLimbs() { _poseStateMap[controller::RIGHT_FOOT] = addOffsetToPuckPose(controller::RIGHT_FOOT); _poseStateMap[controller::HIPS] = addOffsetToPuckPose(controller::HIPS); _poseStateMap[controller::SPINE2] = addOffsetToPuckPose(controller::SPINE2); - controller::Pose rightArmPose = addOffsetToPuckPose(controller::RIGHT_ARM); - _poseStateMap[controller::RIGHT_ARM] = rightArmPose; - if (rightArmPose.isValid()) { - // AJT: TODO: compute rotation for RIGHT_SHOULDER AS WELL. - } - - controller::Pose leftArmPose = addOffsetToPuckPose(controller::LEFT_ARM); - _poseStateMap[controller::LEFT_ARM] = leftArmPose; - if (leftArmPose.isValid()) { - // AJT: TODO: compute rotation for LEFT_SHOULDER AS WELL. - } + _poseStateMap[controller::RIGHT_ARM] = addOffsetToPuckPose(controller::RIGHT_ARM); + _poseStateMap[controller::LEFT_ARM] = addOffsetToPuckPose(controller::LEFT_ARM); if (_overrideHead) { _poseStateMap[controller::HEAD] = addOffsetToPuckPose(controller::HEAD); @@ -1146,6 +1137,25 @@ static glm::vec3 computeUserShoulderPositionFromHistory(const glm::mat4& headMat return transformPoint(headMat, center); } +// y axis comes out of puck usb port/green light +// -z axis comes out of puck center/vive logo +static glm::vec3 computeUserShoulderPositionFromMeasurements(const glm::mat4& headMat, const controller::Pose& armPuck, bool isLeftHand) { + // AJT: TODO measurments are hard coded! + const float ARM_CIRC = 0.33f; // 13 inches + const float SHOULDER_SPAN = 0.4826; // 19 inches + + float armRadius = ARM_CIRC / TWO_PI; + + float sign = isLeftHand ? 1.0f : -1.0f; + float localArmX = sign * SHOULDER_SPAN / 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]; @@ -1160,13 +1170,18 @@ void ViveControllerManager::InputDevice::calibrateShoulders(const glm::mat4& def glm::mat4 userRefRightArm = refRightArm; glm::mat4 headMat = defaultToReferenceMat * inputCalibration.defaultHeadMat; + /* AJT: TODO REMOVE userRefLeftArm[3] = glm::vec4(computeUserShoulderPositionFromHistory(headMat, _leftControllerHistory), 1.0f); userRefRightArm[3] = glm::vec4(computeUserShoulderPositionFromHistory(headMat, _rightControllerHistory), 1.0f); + */ if (firstShoulderPose.translation.x < secondShoulderPose.translation.x) { _jointToPuckMap[controller::LEFT_ARM] = firstShoulder.first; _jointToPuckMap[controller::RIGHT_ARM] = secondShoulder.first; + userRefLeftArm[3] = glm::vec4(computeUserShoulderPositionFromMeasurements(headMat, firstShoulderPose, true), 1.0f); + userRefRightArm[3] = glm::vec4(computeUserShoulderPositionFromMeasurements(headMat, secondShoulderPose, false), 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); @@ -1179,6 +1194,9 @@ void ViveControllerManager::InputDevice::calibrateShoulders(const glm::mat4& def _jointToPuckMap[controller::LEFT_ARM] = secondShoulder.first; _jointToPuckMap[controller::RIGHT_ARM] = firstShoulder.first; + userRefLeftArm[3] = glm::vec4(computeUserShoulderPositionFromMeasurements(headMat, secondShoulderPose, true), 1.0f); + userRefRightArm[3] = glm::vec4(computeUserShoulderPositionFromMeasurements(headMat, firstShoulderPose, false), 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); From 572213daaf6ac3c4a90c87f51d95ab82a63614cf Mon Sep 17 00:00:00 2001 From: "Anthony J. Thibault" Date: Tue, 25 Jul 2017 17:05:07 -0700 Subject: [PATCH 07/14] Added shoulder config to Controller Settings UI All offset values in Controller Settings UI are now in cm. --- .../qml/hifi/tablet/OpenVrConfiguration.qml | 88 +++++++++++++++---- plugins/openvr/src/ViveControllerManager.cpp | 38 +++++--- plugins/openvr/src/ViveControllerManager.h | 2 + 3 files changed, 95 insertions(+), 33 deletions(-) diff --git a/interface/resources/qml/hifi/tablet/OpenVrConfiguration.qml b/interface/resources/qml/hifi/tablet/OpenVrConfiguration.qml index 90d6ba7022..36424e22b4 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 @@ -1006,10 +1049,17 @@ Rectangle { "Z": handZOffset.value } + var shoulderObject = { + "override": shouldersChecked, + "armCircumference": armCircumference.value, + "shoulderWidth": shoulderWidth.value + } + var settingsObject = { "bodyConfiguration": trackerConfiguration, "headConfiguration": headObject, "handConfiguration": handObject, + "shoulderConfiguration": shoulderObject, "desktopMode": viveInDesktop.checked } diff --git a/plugins/openvr/src/ViveControllerManager.cpp b/plugins/openvr/src/ViveControllerManager.cpp index 4c34a4cd27..395e131124 100644 --- a/plugins/openvr/src/ViveControllerManager.cpp +++ b/plugins/openvr/src/ViveControllerManager.cpp @@ -378,6 +378,8 @@ void ViveControllerManager::InputDevice::calibrateFromUI(const controller::Input } } +static const float CM_TO_M = 0.01f; + void ViveControllerManager::InputDevice::configureCalibrationSettings(const QJsonObject configurationSettings) { Locker locker(_lock); if (!configurationSettings.empty()) { @@ -391,8 +393,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; } @@ -401,11 +403,18 @@ 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() == "shoulderConfiguration") { + QJsonObject shoulderObj = iter.value().toObject(); + bool shouldersChecked = shoulderObj["override"].toBool(); + if (shouldersChecked) { + _armCircumference = shoulderObj["armCircumference"].toDouble() * CM_TO_M; + _shoulderWidth = shoulderObj["shoulderWidth"].toDouble() * CM_TO_M; + } } iter++; } @@ -1139,15 +1148,12 @@ static glm::vec3 computeUserShoulderPositionFromHistory(const glm::mat4& headMat // y axis comes out of puck usb port/green light // -z axis comes out of puck center/vive logo -static glm::vec3 computeUserShoulderPositionFromMeasurements(const glm::mat4& headMat, const controller::Pose& armPuck, bool isLeftHand) { - // AJT: TODO measurments are hard coded! - const float ARM_CIRC = 0.33f; // 13 inches - const float SHOULDER_SPAN = 0.4826; // 19 inches +static glm::vec3 computeUserShoulderPositionFromMeasurements(float armCirc, float shoulderSpan, const glm::mat4& headMat, const controller::Pose& armPuck, bool isLeftHand) { - float armRadius = ARM_CIRC / TWO_PI; + float armRadius = armCirc / TWO_PI; float sign = isLeftHand ? 1.0f : -1.0f; - float localArmX = sign * SHOULDER_SPAN / 2.0f; + float localArmX = sign * shoulderSpan / 2.0f; controller::Pose localPuck = armPuck.transform(glm::inverse(headMat)); glm::mat4 localPuckMat = localPuck.getMatrix(); @@ -1179,8 +1185,10 @@ void ViveControllerManager::InputDevice::calibrateShoulders(const glm::mat4& def _jointToPuckMap[controller::LEFT_ARM] = firstShoulder.first; _jointToPuckMap[controller::RIGHT_ARM] = secondShoulder.first; - userRefLeftArm[3] = glm::vec4(computeUserShoulderPositionFromMeasurements(headMat, firstShoulderPose, true), 1.0f); - userRefRightArm[3] = glm::vec4(computeUserShoulderPositionFromMeasurements(headMat, secondShoulderPose, false), 1.0f); + 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); @@ -1194,8 +1202,10 @@ void ViveControllerManager::InputDevice::calibrateShoulders(const glm::mat4& def _jointToPuckMap[controller::LEFT_ARM] = secondShoulder.first; _jointToPuckMap[controller::RIGHT_ARM] = firstShoulder.first; - userRefLeftArm[3] = glm::vec4(computeUserShoulderPositionFromMeasurements(headMat, secondShoulderPose, true), 1.0f); - userRefRightArm[3] = glm::vec4(computeUserShoulderPositionFromMeasurements(headMat, firstShoulderPose, false), 1.0f); + 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); diff --git a/plugins/openvr/src/ViveControllerManager.h b/plugins/openvr/src/ViveControllerManager.h index 329901cd25..41ed494ffb 100644 --- a/plugins/openvr/src/ViveControllerManager.h +++ b/plugins/openvr/src/ViveControllerManager.h @@ -182,6 +182,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 }; From 2a45c6d3dc170b913cd2c5519c97de4ea2f70aae Mon Sep 17 00:00:00 2001 From: "Anthony J. Thibault" Date: Wed, 26 Jul 2017 16:53:18 -0700 Subject: [PATCH 08/14] Removed eigen dependency, fixed Debug Draw IK Chains Also, clarified for special case for secondary shoulder joint look-at constraint. --- cmake/externals/eigen/CMakeLists.txt | 20 ----- cmake/modules/FindEigen.cmake | 26 ------- .../animation/src/AnimInverseKinematics.cpp | 28 +++++-- plugins/openvr/CMakeLists.txt | 5 -- plugins/openvr/src/ViveControllerManager.cpp | 78 +------------------ plugins/openvr/src/ViveControllerManager.h | 5 -- 6 files changed, 21 insertions(+), 141 deletions(-) delete mode 100644 cmake/externals/eigen/CMakeLists.txt delete mode 100644 cmake/modules/FindEigen.cmake diff --git a/cmake/externals/eigen/CMakeLists.txt b/cmake/externals/eigen/CMakeLists.txt deleted file mode 100644 index 15d94576cc..0000000000 --- a/cmake/externals/eigen/CMakeLists.txt +++ /dev/null @@ -1,20 +0,0 @@ -set(EXTERNAL_NAME eigen) - -include(ExternalProject) -ExternalProject_Add( - ${EXTERNAL_NAME} - URL http://bitbucket.org/eigen/eigen/get/3.3.4.zip - URL_MD5 e337acc279874bc6a56da4d973a723fb - CONFIGURE_COMMAND "" - BUILD_COMMAND "" - INSTALL_COMMAND "" - LOG_DOWNLOAD 1 -) - -# Hide this external target (for ide users) -set_target_properties(${EXTERNAL_NAME} PROPERTIES FOLDER "hidden/externals") - -ExternalProject_Get_Property(${EXTERNAL_NAME} INSTALL_DIR) - -string(TOUPPER ${EXTERNAL_NAME} EXTERNAL_NAME_UPPER) -set(${EXTERNAL_NAME_UPPER}_INCLUDE_DIRS ${INSTALL_DIR} CACHE PATH "List of eigen include directories") \ No newline at end of file diff --git a/cmake/modules/FindEigen.cmake b/cmake/modules/FindEigen.cmake deleted file mode 100644 index 4b09ed8f2b..0000000000 --- a/cmake/modules/FindEigen.cmake +++ /dev/null @@ -1,26 +0,0 @@ -# -# FindEigen.cmake -# -# Try to find Eigen include path. -# Once done this will define -# -# EIGEN_INCLUDE_DIRS -# -# Created on 7/14/2017 by Anthony Thibault -# Copyright 2017 High Fidelity, Inc. -# -# Distributed under the Apache License, Version 2.0. -# See the accompanying file LICENSE or http://www.apache.org/licenses/LICENSE-2.0.html -# - -# setup hints for Eigen search -include("${MACRO_DIR}/HifiLibrarySearchHints.cmake") -hifi_library_search_hints("eigen") - -# locate dir -string(CONCAT EIGEN_INCLUDE_DIRS ${EIGEN_INCLUDE_DIRS} "/src/eigen") - -include(FindPackageHandleStandardArgs) -find_package_handle_standard_args(EIGEN DEFAULT_MSG EIGEN_INCLUDE_DIRS) - -mark_as_advanced(EIGEN_INCLUDE_DIRS EIGEN_SEARCH_DIRS) \ No newline at end of file diff --git a/libraries/animation/src/AnimInverseKinematics.cpp b/libraries/animation/src/AnimInverseKinematics.cpp index 10c4011f52..74fbf00067 100644 --- a/libraries/animation/src/AnimInverseKinematics.cpp +++ b/libraries/animation/src/AnimInverseKinematics.cpp @@ -27,9 +27,9 @@ 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++) { @@ -1640,8 +1640,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 @@ -1867,9 +1869,19 @@ void AnimInverseKinematics::setSecondaryTargets(const AnimContext& context) { return; } - // shoulder joint should look-at position of arm joint. + // 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) { @@ -1883,7 +1895,7 @@ void AnimInverseKinematics::setSecondaryTargets(const AnimContext& context) { } // if parent should "look-at" child joint position. - if (iter.first == leftArmIndex || iter.first == rightArmIndex) { + if (shoulderShouldLookAtArm && (iter.first == leftArmIndex || iter.first == rightArmIndex)) { AnimPose grandParentAbsPose; int grandParentIndex = _skeleton->getParentIndex(parentIndex); @@ -1896,7 +1908,7 @@ void AnimInverseKinematics::setSecondaryTargets(const AnimContext& context) { _relativePoses[parentIndex] = grandParentAbsPose.inverse() * parentAbsPose; } - // AJT: for now ignore translation on secondary poses. + // 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; diff --git a/plugins/openvr/CMakeLists.txt b/plugins/openvr/CMakeLists.txt index 6633b99b31..7878ae2d7e 100644 --- a/plugins/openvr/CMakeLists.txt +++ b/plugins/openvr/CMakeLists.txt @@ -18,14 +18,9 @@ if (WIN32) include_hifi_library_headers(octree) add_dependency_external_projects(OpenVR) - add_dependency_external_projects(eigen) find_package(OpenVR REQUIRED) target_include_directories(${TARGET_NAME} PRIVATE ${OPENVR_INCLUDE_DIRS}) target_link_libraries(${TARGET_NAME} ${OPENVR_LIBRARIES}) target_link_libraries(${TARGET_NAME} Winmm.lib) - - # header only library - find_package(eigen REQUIRED) - target_include_directories(${TARGET_NAME} PRIVATE ${EIGEN_INCLUDE_DIRS}) endif() diff --git a/plugins/openvr/src/ViveControllerManager.cpp b/plugins/openvr/src/ViveControllerManager.cpp index 395e131124..61a0b33255 100644 --- a/plugins/openvr/src/ViveControllerManager.cpp +++ b/plugins/openvr/src/ViveControllerManager.cpp @@ -35,8 +35,6 @@ #include #include -#include - extern PoseData _nextSimPoseData; vr::IVRSystem* acquireOpenVrSystem(); @@ -287,9 +285,7 @@ static const size_t CONTROLLER_HISTORY_SIZE = 90 * 3; ViveControllerManager::InputDevice::InputDevice(vr::IVRSystem*& system) : controller::InputDevice("Vive"), - _system(system), - _leftControllerHistory(CONTROLLER_HISTORY_SIZE), - _rightControllerHistory(CONTROLLER_HISTORY_SIZE) { + _system(system) { _configStringMap[Config::None] = QString("None"); _configStringMap[Config::Feet] = QString("Feet"); @@ -915,16 +911,6 @@ void ViveControllerManager::InputDevice::handlePoseEvent(float deltaTime, const // transform into avatar frame glm::mat4 controllerToAvatar = glm::inverse(inputCalibrationData.avatarMat) * inputCalibrationData.sensorToWorldMat; _poseStateMap[isLeftHand ? controller::LEFT_HAND : controller::RIGHT_HAND] = pose.transform(controllerToAvatar); - - // AJT: TODO ONLY DO THIS IF CALIBRATING! - // record controller history - if (isLeftHand) { - _leftControllerHistory[_leftControllerHistoryCursor] = pose.translation; - _leftControllerHistoryCursor = (_leftControllerHistoryCursor + 1) % CONTROLLER_HISTORY_SIZE; - } else { - _rightControllerHistory[_rightControllerHistoryCursor] = pose.translation; - _rightControllerHistoryCursor = (_rightControllerHistoryCursor + 1) % CONTROLLER_HISTORY_SIZE; - } } bool ViveControllerManager::InputDevice::triggerHapticPulse(float strength, float duration, controller::Hand hand) { @@ -1088,64 +1074,6 @@ void ViveControllerManager::InputDevice::calibrateChest(const glm::mat4& default _pucksPostOffset[_validTrackedObjects[CHEST].first] = computeOffset(defaultToReferenceMat, inputCalibration.defaultSpine2, _validTrackedObjects[CHEST].second); } -// assuming the user has kept his arms straight to his sides and has rotated his hand controllers -// up and down laterally about his shoulders. This will attempt to locate the users actual shoulders -// by calculating a best fit circle around the points, in the plane of the head's forward normal. -static glm::vec3 computeUserShoulderPositionFromHistory(const glm::mat4& headMat, const std::vector& history) { - glm::mat4 invHeadMat = glm::inverse(headMat); - - // AJT: TODO: we could perhaps reject the approximation if the radius is larger or smaller then we expect. - // AJT: TODO: abort if history is too small or invalid... - - // Take equation for a circle: (x - h)^2 + (y - k)^2 = r^2 - // And put it into the linear form: A * x = b - // - // where A = | (2 * x0) (2 * y0) 1 | - // | (2 * x1) (2 * y1) 1 | - // | . . . | - // | (2 * xn) (2 * yn) 1 | - // - // and b = | x0^2 + y0^2 | - // | x1^2 + y1^2 | - // | . | - // | xn^2 + yn^2 | - // - // and x = | h | - // | k | - // | r^2 - h^2 - k^2 | - // - - // Build aMat and bMat - Eigen::MatrixXf aMat(CONTROLLER_HISTORY_SIZE, 3); - Eigen::MatrixXf bMat(CONTROLLER_HISTORY_SIZE, 1); - for (int r = 0; r < CONTROLLER_HISTORY_SIZE; r++) { - // transform history[r] into local head coordinates - glm::vec3 p = transformPoint(invHeadMat, history[r]); - - // update the aMat with the appropriate 2d history values - aMat(r, 0) = 2.0f * p.x; - aMat(r, 1) = 2.0f * p.y; - aMat(r, 2) = 1.0f; - - // update the bMat with the appropriate 2d history values - bMat(r, 0) = p.x * p.x + p.y * p.y; - } - - // Then use least squares approximation to solve for the best x. - // http://math.mit.edu/~gs/linearalgebra/ila0403.pdf - Eigen::MatrixXf aTransMat = aMat.transpose(); - Eigen::MatrixXf xMat = (aTransMat * aMat).inverse() * aTransMat * bMat; - - // extract the 2d center of the circle from the xMat - glm::vec3 center(xMat(0, 0), xMat(1, 0), 0.0f); - - // we don't need the radius, but it's included here for completeness. - // float radius = center.x * center.x + center.y * center.y - xMat(2, 0); - - // transform center back into sensor coordinates - return transformPoint(headMat, center); -} - // 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) { @@ -1176,10 +1104,6 @@ void ViveControllerManager::InputDevice::calibrateShoulders(const glm::mat4& def glm::mat4 userRefRightArm = refRightArm; glm::mat4 headMat = defaultToReferenceMat * inputCalibration.defaultHeadMat; - /* AJT: TODO REMOVE - userRefLeftArm[3] = glm::vec4(computeUserShoulderPositionFromHistory(headMat, _leftControllerHistory), 1.0f); - userRefRightArm[3] = glm::vec4(computeUserShoulderPositionFromHistory(headMat, _rightControllerHistory), 1.0f); - */ if (firstShoulderPose.translation.x < secondShoulderPose.translation.x) { _jointToPuckMap[controller::LEFT_ARM] = firstShoulder.first; diff --git a/plugins/openvr/src/ViveControllerManager.h b/plugins/openvr/src/ViveControllerManager.h index 41ed494ffb..1b466d7329 100644 --- a/plugins/openvr/src/ViveControllerManager.h +++ b/plugins/openvr/src/ViveControllerManager.h @@ -192,11 +192,6 @@ private: bool _overrideHands { false }; mutable std::recursive_mutex _lock; - std::vector _leftControllerHistory; - size_t _leftControllerHistoryCursor { 0 }; - std::vector _rightControllerHistory; - size_t _rightControllerHistoryCursor { 0 }; - QString configToString(Config config); friend class ViveControllerManager; }; From 8dc97142827ec1e9fb1f72af5a3711239cbfa58b Mon Sep 17 00:00:00 2001 From: "Anthony J. Thibault" Date: Thu, 27 Jul 2017 09:08:34 -0700 Subject: [PATCH 09/14] Pass armCircumference and shoulderWidth to qml. --- .../resources/qml/hifi/tablet/OpenVrConfiguration.qml | 3 +++ plugins/openvr/src/ViveControllerManager.cpp | 7 +++++++ 2 files changed, 10 insertions(+) diff --git a/interface/resources/qml/hifi/tablet/OpenVrConfiguration.qml b/interface/resources/qml/hifi/tablet/OpenVrConfiguration.qml index 36424e22b4..e3aff97557 100644 --- a/interface/resources/qml/hifi/tablet/OpenVrConfiguration.qml +++ b/interface/resources/qml/hifi/tablet/OpenVrConfiguration.qml @@ -878,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; diff --git a/plugins/openvr/src/ViveControllerManager.cpp b/plugins/openvr/src/ViveControllerManager.cpp index 61a0b33255..5aaf92ed0b 100644 --- a/plugins/openvr/src/ViveControllerManager.cpp +++ b/plugins/openvr/src/ViveControllerManager.cpp @@ -375,6 +375,7 @@ 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); @@ -411,6 +412,10 @@ void ViveControllerManager::InputDevice::configureCalibrationSettings(const QJso _armCircumference = shoulderObj["armCircumference"].toDouble() * CM_TO_M; _shoulderWidth = shoulderObj["shoulderWidth"].toDouble() * CM_TO_M; } + } 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++; } @@ -429,6 +434,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; } From 8f5c41af2dac88a6e9f3ddb6b07225c0807e47d0 Mon Sep 17 00:00:00 2001 From: "Anthony J. Thibault" Date: Thu, 27 Jul 2017 09:44:37 -0700 Subject: [PATCH 10/14] changed _controllerPoseMap from unordered_map to map --- interface/src/avatar/MyAvatar.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/interface/src/avatar/MyAvatar.h b/interface/src/avatar/MyAvatar.h index 5983c6eb0e..298452cad8 100644 --- a/interface/src/avatar/MyAvatar.h +++ b/interface/src/avatar/MyAvatar.h @@ -767,7 +767,7 @@ private: glm::vec3 _hoverReferenceCameraFacing { 0.0f, 0.0f, -1.0f }; // hmd sensor space // all poses are in sensor-frame - std::unordered_map _controllerPoseMap; + std::map _controllerPoseMap; mutable std::mutex _controllerPoseMapMutex; bool _hmdLeanRecenterEnabled = true; From 778c8bf9a749e76fa479c7d45f4f9aa657fb4264 Mon Sep 17 00:00:00 2001 From: "Anthony J. Thibault" Date: Thu, 27 Jul 2017 10:08:35 -0700 Subject: [PATCH 11/14] clang build error fix --- interface/src/avatar/MySkeletonModel.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/interface/src/avatar/MySkeletonModel.cpp b/interface/src/avatar/MySkeletonModel.cpp index 4a86e0ce0a..6e29af0d33 100644 --- a/interface/src/avatar/MySkeletonModel.cpp +++ b/interface/src/avatar/MySkeletonModel.cpp @@ -82,9 +82,9 @@ void MySkeletonModel::updateRig(float deltaTime, glm::mat4 parentTransform) { }; for (auto pair : primaryControllers) { - auto pose = myAvatar->getControllerPoseInAvatarFrame(pair.first); - if (pose.isValid()) { - AnimPose pose(pose.getRotation(), pose.getTranslation()); + 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 { From bcb4cc32d26718ee9c622202cc7339252dc44e87 Mon Sep 17 00:00:00 2001 From: "Anthony J. Thibault" Date: Thu, 27 Jul 2017 10:26:10 -0700 Subject: [PATCH 12/14] another clang error fix --- interface/src/avatar/MySkeletonModel.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/interface/src/avatar/MySkeletonModel.cpp b/interface/src/avatar/MySkeletonModel.cpp index 6e29af0d33..6d468c3f30 100644 --- a/interface/src/avatar/MySkeletonModel.cpp +++ b/interface/src/avatar/MySkeletonModel.cpp @@ -113,9 +113,9 @@ void MySkeletonModel::updateRig(float deltaTime, glm::mat4 parentTransform) { }; for (auto pair : secondaryControllers) { - auto pose = myAvatar->getControllerPoseInAvatarFrame(pair.first); - if (pose.isValid()) { - AnimPose pose(pose.getRotation(), pose.getTranslation()); + 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 { From 6c2b6674bb22336a83d95b8830f51248b82b004b Mon Sep 17 00:00:00 2001 From: "Anthony J. Thibault" Date: Thu, 27 Jul 2017 17:33:54 -0700 Subject: [PATCH 13/14] load and save shoulderConfig to settings. --- .../qml/hifi/tablet/OpenVrConfiguration.qml | 9 +--- plugins/openvr/src/ViveControllerManager.cpp | 41 +++++++++++++++---- plugins/openvr/src/ViveControllerManager.h | 3 ++ 3 files changed, 38 insertions(+), 15 deletions(-) diff --git a/interface/resources/qml/hifi/tablet/OpenVrConfiguration.qml b/interface/resources/qml/hifi/tablet/OpenVrConfiguration.qml index e3aff97557..78c10e2ffa 100644 --- a/interface/resources/qml/hifi/tablet/OpenVrConfiguration.qml +++ b/interface/resources/qml/hifi/tablet/OpenVrConfiguration.qml @@ -1052,17 +1052,12 @@ Rectangle { "Z": handZOffset.value } - var shoulderObject = { - "override": shouldersChecked, - "armCircumference": armCircumference.value, - "shoulderWidth": shoulderWidth.value - } - var settingsObject = { "bodyConfiguration": trackerConfiguration, "headConfiguration": headObject, "handConfiguration": handObject, - "shoulderConfiguration": shoulderObject, + "armCircumference": armCircumference.value, + "shoulderWidth": shoulderWidth.value, "desktopMode": viveInDesktop.checked } diff --git a/plugins/openvr/src/ViveControllerManager.cpp b/plugins/openvr/src/ViveControllerManager.cpp index 5aaf92ed0b..5a1c23839e 100644 --- a/plugins/openvr/src/ViveControllerManager.cpp +++ b/plugins/openvr/src/ViveControllerManager.cpp @@ -167,6 +167,7 @@ void ViveControllerManager::setConfigurationSettings(const QJsonObject configura } } _inputDevice->configureCalibrationSettings(configurationSettings); + saveSettings(); } } @@ -187,6 +188,8 @@ QString ViveControllerManager::configurationLayout() { bool ViveControllerManager::activate() { InputPlugin::activate(); + loadSettings(); + if (!_system) { _system = acquireOpenVrSystem(); } @@ -229,6 +232,8 @@ void ViveControllerManager::deactivate() { auto userInputMapper = DependencyManager::get(); userInputMapper->removeDevice(_inputDevice->_deviceID); _registeredWithInputMapper = false; + + saveSettings(); } bool ViveControllerManager::isHeadControllerMounted() const { @@ -281,7 +286,34 @@ void ViveControllerManager::pluginUpdate(float deltaTime, const controller::Inpu } } -static const size_t CONTROLLER_HISTORY_SIZE = 90 * 3; +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"), @@ -405,13 +437,6 @@ void ViveControllerManager::InputDevice::configureCalibrationSettings(const QJso } else { _handConfig = HandConfig::HandController; } - } else if (iter.key() == "shoulderConfiguration") { - QJsonObject shoulderObj = iter.value().toObject(); - bool shouldersChecked = shoulderObj["override"].toBool(); - if (shouldersChecked) { - _armCircumference = shoulderObj["armCircumference"].toDouble() * CM_TO_M; - _shoulderWidth = shoulderObj["shoulderWidth"].toDouble() * CM_TO_M; - } } else if (iter.key() == "armCircumference") { _armCircumference = (float)iter.value().toDouble() * CM_TO_M; } else if (iter.key() == "shoulderWidth") { diff --git a/plugins/openvr/src/ViveControllerManager.h b/plugins/openvr/src/ViveControllerManager.h index 1b466d7329..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: From 25b5cb4762a0969609bfcc9ce474edabaf4f1d02 Mon Sep 17 00:00:00 2001 From: "Anthony J. Thibault" Date: Tue, 1 Aug 2017 13:17:33 -0700 Subject: [PATCH 14/14] Switch order of precondition and setSecondaryTargets --- libraries/animation/src/AnimInverseKinematics.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/libraries/animation/src/AnimInverseKinematics.cpp b/libraries/animation/src/AnimInverseKinematics.cpp index 74fbf00067..d17fbebf3a 100644 --- a/libraries/animation/src/AnimInverseKinematics.cpp +++ b/libraries/animation/src/AnimInverseKinematics.cpp @@ -1056,8 +1056,9 @@ const AnimPoseVec& AnimInverseKinematics::overlay(const AnimVariantMap& animVars { PROFILE_RANGE_EX(simulation_animation, "ik/ccd", 0xffff00ff, 0); - preconditionRelativePosesToAvoidLimbLock(context, targets); setSecondaryTargets(context); + preconditionRelativePosesToAvoidLimbLock(context, targets); + solve(context, targets, dt, jointChainInfoVec); } @@ -1610,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++) {