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