diff --git a/interface/resources/avatar/avatar-animation.json b/interface/resources/avatar/avatar-animation.json index f88e322d99..35f2d4b9af 100644 --- a/interface/resources/avatar/avatar-animation.json +++ b/interface/resources/avatar/avatar-animation.json @@ -114,6 +114,24 @@ "weightVar": "headWeight", "weight": 4.0, "flexCoefficients": [1, 0.05, 0.25, 0.25, 0.25] + }, + { + "jointName": "LeftArm", + "positionVar": "leftArmPosition", + "rotationVar": "leftArmRotation", + "typeVar": "leftArmType", + "weightVar": "leftArmWeight", + "weight": 0.75, + "flexCoefficients": [1.0, 0.35, 0.2, 0.1, 0.05, 0.0, 0.0, 0.0] + }, + { + "jointName": "RightArm", + "positionVar": "rightArmPosition", + "rotationVar": "rightArmRotation", + "typeVar": "rightArmType", + "weightVar": "rightArmWeight", + "weight": 0.75, + "flexCoefficients": [1.0, 0.35, 0.2, 0.1, 0.05, 0.0, 0.0, 0.0] } ] }, diff --git a/interface/resources/controllers/standard.json b/interface/resources/controllers/standard.json index 62eec9bc3c..75b4821118 100644 --- a/interface/resources/controllers/standard.json +++ b/interface/resources/controllers/standard.json @@ -66,6 +66,8 @@ { "from": "Standard.Hips", "to": "Actions.Hips" }, { "from": "Standard.Spine2", "to": "Actions.Spine2" }, - { "from": "Standard.Head", "to": "Actions.Head" } + { "from": "Standard.Head", "to": "Actions.Head" }, + { "from": "Standard.LeftArm", "to": "Actions.LeftArm" }, + { "from": "Standard.RightArm", "to": "Actions.RightArm" } ] } diff --git a/interface/resources/controllers/vive.json b/interface/resources/controllers/vive.json index 07c4047e0e..591b6f515e 100644 --- a/interface/resources/controllers/vive.json +++ b/interface/resources/controllers/vive.json @@ -3,7 +3,7 @@ "channels": [ { "from": "Vive.LY", "when": "Vive.LSY", "filters": ["invert"], "to": "Standard.LY" }, { "from": "Vive.LX", "when": "Vive.LSX", "to": "Standard.LX" }, - { + { "from": "Vive.LT", "to": "Standard.LT", "filters": [ { "type": "deadZone", "min": 0.05 } @@ -17,7 +17,7 @@ { "from": "Vive.RY", "when": "Vive.RSY", "filters": ["invert"], "to": "Standard.RY" }, { "from": "Vive.RX", "when": "Vive.RSX", "to": "Standard.RX" }, - { + { "from": "Vive.RT", "to": "Standard.RT", "filters": [ { "type": "deadZone", "min": 0.05 } @@ -36,31 +36,34 @@ { "from": "Vive.LeftHand", "to": "Standard.LeftHand", "when": [ "Application.InHMD" ] }, { "from": "Vive.RightHand", "to": "Standard.RightHand", "when": [ "Application.InHMD" ] }, - + { "from": "Vive.LeftFoot", "to" : "Standard.LeftFoot", "filters" : [{"type" : "lowVelocity", "rotation" : 1.0, "translation": 1.0}], "when": [ "Application.InHMD"] }, - + { "from": "Vive.RightFoot", "to" : "Standard.RightFoot", "filters" : [{"type" : "lowVelocity", "rotation" : 1.0, "translation": 1.0}], "when": [ "Application.InHMD"] }, - + { "from": "Vive.Hips", "to" : "Standard.Hips", "filters" : [{"type" : "lowVelocity", "rotation" : 0.01, "translation": 0.01}], "when": [ "Application.InHMD"] }, - + { "from": "Vive.Spine2", "to" : "Standard.Spine2", "filters" : [{"type" : "lowVelocity", "rotation" : 0.01, "translation": 0.01}], "when": [ "Application.InHMD"] }, - - { "from": "Vive.Head", "to" : "Standard.Head", "when" : [ "Application.InHMD"] } + + { "from": "Vive.Head", "to" : "Standard.Head", "when" : [ "Application.InHMD"] }, + + { "from": "Vive.RightArm", "to" : "Standard.RightArm", "when" : [ "Application.InHMD"] }, + { "from": "Vive.LeftArm", "to" : "Standard.LeftArm", "when" : [ "Application.InHMD"] } ] } diff --git a/interface/src/Application.cpp b/interface/src/Application.cpp index 6c1ea8eba7..f7628ee240 100644 --- a/interface/src/Application.cpp +++ b/interface/src/Application.cpp @@ -4363,7 +4363,11 @@ void Application::update(float deltaTime) { myAvatar->getSpine2CalibrationMat(), myAvatar->getHipsCalibrationMat(), myAvatar->getLeftFootCalibrationMat(), - myAvatar->getRightFootCalibrationMat() + myAvatar->getRightFootCalibrationMat(), + myAvatar->getRightArmCalibrationMat(), + myAvatar->getLeftArmCalibrationMat(), + myAvatar->getRightHandCalibrationMat(), + myAvatar->getLeftHandCalibrationMat() }; InputPluginPointer keyboardMousePlugin; @@ -4419,6 +4423,10 @@ void Application::update(float deltaTime) { controller::Pose headPose = userInputMapper->getPoseState(controller::Action::HEAD); myAvatar->setHeadControllerPoseInSensorFrame(headPose.transform(avatarToSensorMatrix)); + controller::Pose leftArmPose = userInputMapper->getPoseState(controller::Action::LEFT_ARM); + controller::Pose rightArmPose = userInputMapper->getPoseState(controller::Action::RIGHT_ARM); + myAvatar->setArmControllerPosesInSensorFrame(leftArmPose.transform(avatarToSensorMatrix), rightArmPose.transform(avatarToSensorMatrix)); + updateThreads(deltaTime); // If running non-threaded, then give the threads some time to process... updateDialogs(deltaTime); // update various stats dialogs if present diff --git a/interface/src/avatar/MyAvatar.cpp b/interface/src/avatar/MyAvatar.cpp index 9996df2afc..72a9281564 100755 --- a/interface/src/avatar/MyAvatar.cpp +++ b/interface/src/avatar/MyAvatar.cpp @@ -90,6 +90,14 @@ const float MyAvatar::ZOOM_DEFAULT = 1.5f; // static const glm::quat DEFAULT_AVATAR_MIDDLE_EYE_ROT { Quaternions::Y_180 }; static const glm::vec3 DEFAULT_AVATAR_MIDDLE_EYE_POS { 0.0f, 0.6f, 0.0f }; static const glm::vec3 DEFAULT_AVATAR_HEAD_POS { 0.0f, 0.53f, 0.0f }; +static const glm::vec3 DEFAULT_AVATAR_RIGHTARM_POS { -0.134824f, 0.396348f, -0.0515777f }; +static const glm::quat DEFAULT_AVATAR_RIGHTARM_ROT { -0.536241f, 0.536241f, -0.460918f, -0.460918f }; +static const glm::vec3 DEFAULT_AVATAR_LEFTARM_POS { 0.134795f, 0.396349f, -0.0515881f }; +static const glm::quat DEFAULT_AVATAR_LEFTARM_ROT { 0.536257f, 0.536258f, -0.460899f, 0.4609f }; +static const glm::vec3 DEFAULT_AVATAR_RIGHTHAND_POS { -0.72768f, 0.396349f, -0.0515779f }; +static const glm::quat DEFAULT_AVATAR_RIGHTHAND_ROT { 0.479184f, -0.520013f, 0.522537f, 0.476365f}; +static const glm::vec3 DEFAULT_AVATAR_LEFTHAND_POS { 0.727588f, 0.39635f, -0.0515878f }; +static const glm::quat DEFAULT_AVATAR_LEFTHAND_ROT { -0.479181f, -0.52001f, 0.52254f, -0.476369f }; static const glm::vec3 DEFAULT_AVATAR_NECK_POS { 0.0f, 0.445f, 0.025f }; static const glm::vec3 DEFAULT_AVATAR_SPINE2_POS { 0.0f, 0.32f, 0.02f }; static const glm::vec3 DEFAULT_AVATAR_HIPS_POS { 0.0f, 0.0f, 0.0f }; @@ -1434,6 +1442,37 @@ controller::Pose MyAvatar::getHeadControllerPoseInAvatarFrame() const { return getHeadControllerPoseInWorldFrame().transform(invAvatarMatrix); } +void MyAvatar::setArmControllerPosesInSensorFrame(const controller::Pose& left, const controller::Pose& right) { + _leftArmControllerPoseInSensorFrameCache.set(left); + _rightArmControllerPoseInSensorFrameCache.set(right); +} + +controller::Pose MyAvatar::getLeftArmControllerPoseInSensorFrame() const { + return _leftArmControllerPoseInSensorFrameCache.get(); +} + +controller::Pose MyAvatar::getRightArmControllerPoseInSensorFrame() const { + return _rightArmControllerPoseInSensorFrameCache.get(); +} + +controller::Pose MyAvatar::getLeftArmControllerPoseInWorldFrame() const { + return getLeftArmControllerPoseInSensorFrame().transform(getSensorToWorldMatrix()); +} + +controller::Pose MyAvatar::getRightArmControllerPoseInWorldFrame() const { + return getRightArmControllerPoseInSensorFrame().transform(getSensorToWorldMatrix()); +} + +controller::Pose MyAvatar::getLeftArmControllerPoseInAvatarFrame() const { + glm::mat4 worldToAvatarMat = glm::inverse(createMatFromQuatAndPos(getOrientation(), getPosition())); + return getLeftArmControllerPoseInWorldFrame().transform(worldToAvatarMat); +} + +controller::Pose MyAvatar::getRightArmControllerPoseInAvatarFrame() const { + glm::mat4 worldToAvatarMat = glm::inverse(createMatFromQuatAndPos(getOrientation(), getPosition())); + return getRightArmControllerPoseInWorldFrame().transform(worldToAvatarMat); +} + void MyAvatar::updateMotors() { _characterController.clearMotors(); glm::quat motorRotation; @@ -2747,6 +2786,51 @@ glm::mat4 MyAvatar::getRightFootCalibrationMat() const { } } + +glm::mat4 MyAvatar::getRightArmCalibrationMat() const { + int rightArmIndex = _skeletonModel->getRig().indexOfJoint("RightArm"); + if (rightArmIndex >= 0) { + auto rightArmPos = getAbsoluteDefaultJointTranslationInObjectFrame(rightArmIndex); + auto rightArmRot = getAbsoluteDefaultJointRotationInObjectFrame(rightArmIndex); + return createMatFromQuatAndPos(rightArmRot, rightArmPos); + } else { + return createMatFromQuatAndPos(DEFAULT_AVATAR_RIGHTARM_ROT, DEFAULT_AVATAR_RIGHTARM_POS); + } +} + +glm::mat4 MyAvatar::getLeftArmCalibrationMat() const { + int leftArmIndex = _skeletonModel->getRig().indexOfJoint("LeftArm"); + if (leftArmIndex >= 0) { + auto leftArmPos = getAbsoluteDefaultJointTranslationInObjectFrame(leftArmIndex); + auto leftArmRot = getAbsoluteDefaultJointRotationInObjectFrame(leftArmIndex); + return createMatFromQuatAndPos(leftArmRot, leftArmPos); + } else { + return createMatFromQuatAndPos(DEFAULT_AVATAR_LEFTARM_ROT, DEFAULT_AVATAR_RIGHTARM_POS); + } +} + +glm::mat4 MyAvatar::getRightHandCalibrationMat() const { + int rightHandIndex = _skeletonModel->getRig().indexOfJoint("RightHand"); + if (rightHandIndex >= 0) { + auto rightHandPos = getAbsoluteDefaultJointTranslationInObjectFrame(rightHandIndex); + auto rightHandRot = getAbsoluteDefaultJointRotationInObjectFrame(rightHandIndex); + return createMatFromQuatAndPos(rightHandRot, rightHandPos); + } else { + return createMatFromQuatAndPos(DEFAULT_AVATAR_RIGHTHAND_ROT, DEFAULT_AVATAR_RIGHTHAND_POS); + } +} + +glm::mat4 MyAvatar::getLeftHandCalibrationMat() const { + int leftHandIndex = _skeletonModel->getRig().indexOfJoint("LeftHand"); + if (leftHandIndex >= 0) { + auto leftHandPos = getAbsoluteDefaultJointTranslationInObjectFrame(leftHandIndex); + auto leftHandRot = getAbsoluteDefaultJointRotationInObjectFrame(leftHandIndex); + return createMatFromQuatAndPos(leftHandRot, leftHandPos); + } else { + return createMatFromQuatAndPos(DEFAULT_AVATAR_LEFTHAND_ROT, DEFAULT_AVATAR_LEFTHAND_POS); + } +} + bool MyAvatar::pinJoint(int index, const glm::vec3& position, const glm::quat& orientation) { auto hipsIndex = getJointIndex("Hips"); if (index != hipsIndex) { diff --git a/interface/src/avatar/MyAvatar.h b/interface/src/avatar/MyAvatar.h index fde350a43e..e0a3a1559e 100644 --- a/interface/src/avatar/MyAvatar.h +++ b/interface/src/avatar/MyAvatar.h @@ -471,6 +471,14 @@ public: controller::Pose getHeadControllerPoseInWorldFrame() const; controller::Pose getHeadControllerPoseInAvatarFrame() const; + void setArmControllerPosesInSensorFrame(const controller::Pose& left, const controller::Pose& right); + controller::Pose getLeftArmControllerPoseInSensorFrame() const; + controller::Pose getRightArmControllerPoseInSensorFrame() const; + controller::Pose getLeftArmControllerPoseInWorldFrame() const; + controller::Pose getRightArmControllerPoseInWorldFrame() const; + controller::Pose getLeftArmControllerPoseInAvatarFrame() const; + controller::Pose getRightArmControllerPoseInAvatarFrame() const; + bool hasDriveInput() const; Q_INVOKABLE void setCollisionsEnabled(bool enabled); @@ -488,6 +496,10 @@ public: glm::mat4 getHipsCalibrationMat() const; glm::mat4 getLeftFootCalibrationMat() const; glm::mat4 getRightFootCalibrationMat() const; + glm::mat4 getRightArmCalibrationMat() const; + glm::mat4 getLeftArmCalibrationMat() const; + glm::mat4 getLeftHandCalibrationMat() const; + glm::mat4 getRightHandCalibrationMat() const; void addHoldAction(AvatarActionHold* holdAction); // thread-safe void removeHoldAction(AvatarActionHold* holdAction); // thread-safe @@ -737,6 +749,8 @@ private: ThreadSafeValueCache _hipsControllerPoseInSensorFrameCache{ controller::Pose() }; ThreadSafeValueCache _spine2ControllerPoseInSensorFrameCache{ controller::Pose() }; ThreadSafeValueCache _headControllerPoseInSensorFrameCache{ controller::Pose() }; + ThreadSafeValueCache _leftArmControllerPoseInSensorFrameCache{ controller::Pose() }; + ThreadSafeValueCache _rightArmControllerPoseInSensorFrameCache{ controller::Pose() }; bool _hmdLeanRecenterEnabled = true; diff --git a/interface/src/avatar/MySkeletonModel.cpp b/interface/src/avatar/MySkeletonModel.cpp index 828a5f8a01..a88631f47f 100644 --- a/interface/src/avatar/MySkeletonModel.cpp +++ b/interface/src/avatar/MySkeletonModel.cpp @@ -92,6 +92,26 @@ void MySkeletonModel::updateRig(float deltaTime, glm::mat4 parentTransform) { headParams.spine2Enabled = false; } + auto avatarRightArmPose = myAvatar->getRightArmControllerPoseInAvatarFrame(); + if (avatarRightArmPose.isValid()) { + glm::mat4 rightArmMat = Matrices::Y_180 * createMatFromQuatAndPos(avatarRightArmPose.getRotation(), avatarRightArmPose.getTranslation()); + headParams.rightArmPosition = extractTranslation(rightArmMat); + headParams.rightArmRotation = glmExtractRotation(rightArmMat); + headParams.rightArmEnabled = true; + } else { + headParams.rightArmEnabled = false; + } + + auto avatarLeftArmPose = myAvatar->getLeftArmControllerPoseInAvatarFrame(); + if (avatarLeftArmPose.isValid()) { + glm::mat4 leftArmMat = Matrices::Y_180 * createMatFromQuatAndPos(avatarLeftArmPose.getRotation(), avatarLeftArmPose.getTranslation()); + headParams.leftArmPosition = extractTranslation(leftArmMat); + headParams.leftArmRotation = glmExtractRotation(leftArmMat); + headParams.leftArmEnabled = true; + } else { + headParams.leftArmEnabled = false; + } + headParams.isTalking = head->getTimeWithoutTalking() <= 1.5f; _rig.updateFromHeadParameters(headParams, deltaTime); diff --git a/libraries/animation/src/Rig.cpp b/libraries/animation/src/Rig.cpp index 99d2deb323..add3a461af 100644 --- a/libraries/animation/src/Rig.cpp +++ b/libraries/animation/src/Rig.cpp @@ -1047,6 +1047,22 @@ void Rig::updateFromHeadParameters(const HeadParameters& params, float dt) { } else { _animVars.set("spine2Type", (int)IKTarget::Type::Unknown); } + + if (params.leftArmEnabled) { + _animVars.set("leftArmType", (int)IKTarget::Type::RotationAndPosition); + _animVars.set("leftArmPosition", params.leftArmPosition); + _animVars.set("leftArmRotation", params.leftArmRotation); + } else { + _animVars.set("leftArmType", (int)IKTarget::Type::Unknown); + } + + if (params.rightArmEnabled) { + _animVars.set("rightArmType", (int)IKTarget::Type::RotationAndPosition); + _animVars.set("rightArmPosition", params.rightArmPosition); + _animVars.set("rightArmRotation", params.rightArmRotation); + } else { + _animVars.set("rightArmType", (int)IKTarget::Type::Unknown); + } } void Rig::updateFromEyeParameters(const EyeParameters& params) { diff --git a/libraries/animation/src/Rig.h b/libraries/animation/src/Rig.h index 18780d6e64..c9d52d8c72 100644 --- a/libraries/animation/src/Rig.h +++ b/libraries/animation/src/Rig.h @@ -46,9 +46,15 @@ public: glm::mat4 spine2Matrix = glm::mat4(); // rig space glm::quat rigHeadOrientation = glm::quat(); // rig space (-z forward) glm::vec3 rigHeadPosition = glm::vec3(); // rig space + glm::vec3 rightArmPosition = glm::vec3(); // rig space + glm::quat rightArmRotation = glm::quat(); // rig space + glm::vec3 leftArmPosition = glm::vec3(); // rig space + glm::quat leftArmRotation = glm::quat(); // rig space bool hipsEnabled = false; bool headEnabled = false; bool spine2Enabled = false; + bool leftArmEnabled = false; + bool rightArmEnabled = false; bool isTalking = false; }; diff --git a/libraries/controllers/src/controllers/Actions.cpp b/libraries/controllers/src/controllers/Actions.cpp index 62a10c851f..96e433bcc8 100644 --- a/libraries/controllers/src/controllers/Actions.cpp +++ b/libraries/controllers/src/controllers/Actions.cpp @@ -51,6 +51,8 @@ namespace controller { makePosePair(Action::LEFT_HAND, "LeftHand"), makePosePair(Action::RIGHT_HAND, "RightHand"), + makePosePair(Action::RIGHT_ARM, "RightArm"), + makePosePair(Action::LEFT_ARM, "LeftArm"), makePosePair(Action::LEFT_FOOT, "LeftFoot"), makePosePair(Action::RIGHT_FOOT, "RightFoot"), makePosePair(Action::HIPS, "Hips"), diff --git a/libraries/controllers/src/controllers/Actions.h b/libraries/controllers/src/controllers/Actions.h index 534f01d865..5727d4906e 100644 --- a/libraries/controllers/src/controllers/Actions.h +++ b/libraries/controllers/src/controllers/Actions.h @@ -42,6 +42,8 @@ enum class Action { LEFT_HAND = NUM_COMBINED_AXES, RIGHT_HAND, + LEFT_ARM, + RIGHT_ARM, LEFT_FOOT, RIGHT_FOOT, HIPS, diff --git a/libraries/controllers/src/controllers/Input.h b/libraries/controllers/src/controllers/Input.h index 65c78cd6ea..b74ad48c6f 100644 --- a/libraries/controllers/src/controllers/Input.h +++ b/libraries/controllers/src/controllers/Input.h @@ -24,7 +24,11 @@ struct InputCalibrationData { glm::mat4 defaultSpine2; // default pose for spine2 joint in avatar space glm::mat4 defaultHips; // default pose for hips joint in avatar space glm::mat4 defaultLeftFoot; // default pose for leftFoot joint in avatar space - glm::mat4 defaultRightFoot; // default pose for leftFoot joint in avatar space + glm::mat4 defaultRightFoot; // default pose for rightFoot joint in avatar space + glm::mat4 defaultRightArm; // default pose for rightArm joint in avatar space + glm::mat4 defaultLeftArm; // default pose for leftArm joint in avatar space + glm::mat4 defaultRightHand; // default pose for rightHand joint in avatar space + glm::mat4 defaultLeftHand; // default pose for leftHand joint in avatar space }; enum class ChannelType { diff --git a/libraries/controllers/src/controllers/StandardController.cpp b/libraries/controllers/src/controllers/StandardController.cpp index d8c98eb63b..8e49bb0ebf 100644 --- a/libraries/controllers/src/controllers/StandardController.cpp +++ b/libraries/controllers/src/controllers/StandardController.cpp @@ -104,6 +104,8 @@ Input::NamedVector StandardController::getAvailableInputs() const { makePair(RIGHT_HAND, "RightHand"), makePair(LEFT_FOOT, "LeftFoot"), makePair(RIGHT_FOOT, "RightFoot"), + makePair(RIGHT_ARM, "RightArm"), + makePair(LEFT_ARM, "LeftArm"), makePair(HIPS, "Hips"), makePair(SPINE2, "Spine2"), makePair(HEAD, "Head"), diff --git a/libraries/ktx/src/khronos/KHR.h b/libraries/ktx/src/khronos/KHR.h index d710ca7b40..a98f2cc0d4 100644 --- a/libraries/ktx/src/khronos/KHR.h +++ b/libraries/ktx/src/khronos/KHR.h @@ -212,10 +212,11 @@ namespace khronos { template inline uint32_t evalAlignedCompressedBlockCount(uint32_t value) { // FIXME add static assert that ALIGNMENT is a power of 2 - return (value + (ALIGNMENT - 1) / ALIGNMENT); + static uint32_t ALIGNMENT_REMAINDER = ALIGNMENT - 1; + return (value + ALIGNMENT_REMAINDER) / ALIGNMENT; } - inline uint8_t evalBlockAlignemnt(InternalFormat format, uint32_t value) { + inline uint32_t evalCompressedBlockCount(InternalFormat format, uint32_t value) { switch (format) { case InternalFormat::COMPRESSED_SRGB_S3TC_DXT1_EXT: // BC1 case InternalFormat::COMPRESSED_SRGB_ALPHA_S3TC_DXT1_EXT: // BC1A diff --git a/libraries/ktx/src/ktx/KTX.cpp b/libraries/ktx/src/ktx/KTX.cpp index d6faee4cc7..788ec54a47 100644 --- a/libraries/ktx/src/ktx/KTX.cpp +++ b/libraries/ktx/src/ktx/KTX.cpp @@ -30,7 +30,7 @@ uint32_t Header::evalMaxDimension() const { uint32_t Header::evalPixelOrBlockDimension(uint32_t pixelDimension) const { if (isCompressed()) { - return khronos::gl::texture::evalBlockAlignemnt(getGLInternaFormat(), pixelDimension); + return khronos::gl::texture::evalCompressedBlockCount(getGLInternaFormat(), pixelDimension); } return pixelDimension; } diff --git a/plugins/openvr/src/ViveControllerManager.cpp b/plugins/openvr/src/ViveControllerManager.cpp index fdc8231558..53500a3353 100644 --- a/plugins/openvr/src/ViveControllerManager.cpp +++ b/plugins/openvr/src/ViveControllerManager.cpp @@ -33,6 +33,7 @@ #include + extern PoseData _nextSimPoseData; vr::IVRSystem* acquireOpenVrSystem(); @@ -49,6 +50,7 @@ static const char* RENDER_CONTROLLERS = "Render Hand Controllers"; static const int MIN_PUCK_COUNT = 2; static const int MIN_FEET_AND_HIPS = 3; static const int MIN_FEET_HIPS_CHEST = 4; +static const int MIN_FEET_HIPS_SHOULDERS = 5; static const int FIRST_FOOT = 0; static const int SECOND_FOOT = 1; static const int HIP = 2; @@ -70,10 +72,14 @@ static glm::mat4 computeOffset(glm::mat4 defaultToReferenceMat, glm::mat4 defaul return glm::inverse(poseMat) * referenceJointMat; } -static bool sortPucksYPosition(std::pair firstPuck, std::pair secondPuck) { +static bool sortPucksYPosition(PuckPosePair firstPuck, PuckPosePair secondPuck) { return (firstPuck.second.translation.y < secondPuck.second.translation.y); } +static bool sortPucksXPosition(PuckPosePair firstPuck, PuckPosePair secondPuck) { + return (firstPuck.second.translation.x < secondPuck.second.translation.x); +} + static QString deviceTrackingResultToString(vr::ETrackingResult trackingResult) { QString result; auto iterator = TRACKING_RESULT_TO_STRING.find(trackingResult); @@ -168,6 +174,7 @@ ViveControllerManager::InputDevice::InputDevice(vr::IVRSystem*& system) : contro _configStringMap[Config::Feet] = QString("Feet"); _configStringMap[Config::FeetAndHips] = QString("FeetAndHips"); _configStringMap[Config::FeetHipsAndChest] = QString("FeetHipsAndChest"); + _configStringMap[Config::FeetHipsAndShoulders] = QString("FeetHipsAndShoulders"); if (openVrSupported()) { createPreferences(); @@ -333,34 +340,21 @@ void ViveControllerManager::InputDevice::calibrate(const controller::InputCalibr std::sort(_validTrackedObjects.begin(), _validTrackedObjects.end(), sortPucksYPosition); - auto& firstFoot = _validTrackedObjects[FIRST_FOOT]; - auto& secondFoot = _validTrackedObjects[SECOND_FOOT]; - controller::Pose& firstFootPose = firstFoot.second; - controller::Pose& secondFootPose = secondFoot.second; - - if (firstFootPose.translation.x < secondFootPose.translation.x) { - _jointToPuckMap[controller::LEFT_FOOT] = firstFoot.first; - _pucksOffset[firstFoot.first] = computeOffset(defaultToReferenceMat, inputCalibration.defaultLeftFoot, firstFootPose); - _jointToPuckMap[controller::RIGHT_FOOT] = secondFoot.first; - _pucksOffset[secondFoot.first] = computeOffset(defaultToReferenceMat, inputCalibration.defaultRightFoot, secondFootPose); - - } else { - _jointToPuckMap[controller::LEFT_FOOT] = secondFoot.first; - _pucksOffset[secondFoot.first] = computeOffset(defaultToReferenceMat, inputCalibration.defaultLeftFoot, secondFootPose); - _jointToPuckMap[controller::RIGHT_FOOT] = firstFoot.first; - _pucksOffset[firstFoot.first] = computeOffset(defaultToReferenceMat, inputCalibration.defaultRightFoot, firstFootPose); - } - if (_config == Config::Feet) { - // done + calibrateFeet(defaultToReferenceMat, inputCalibration); } else if (_config == Config::FeetAndHips && puckCount >= MIN_FEET_AND_HIPS) { - _jointToPuckMap[controller::HIPS] = _validTrackedObjects[HIP].first; - _pucksOffset[_validTrackedObjects[HIP].first] = computeOffset(defaultToReferenceMat, inputCalibration.defaultHips, _validTrackedObjects[HIP].second); + calibrateFeet(defaultToReferenceMat, inputCalibration); + calibrateHips(defaultToReferenceMat, inputCalibration); } else if (_config == Config::FeetHipsAndChest && puckCount >= MIN_FEET_HIPS_CHEST) { - _jointToPuckMap[controller::HIPS] = _validTrackedObjects[HIP].first; - _pucksOffset[_validTrackedObjects[HIP].first] = computeOffset(defaultToReferenceMat, inputCalibration.defaultHips, _validTrackedObjects[HIP].second); - _jointToPuckMap[controller::SPINE2] = _validTrackedObjects[CHEST].first; - _pucksOffset[_validTrackedObjects[CHEST].first] = computeOffset(defaultToReferenceMat, inputCalibration.defaultSpine2, _validTrackedObjects[CHEST].second); + calibrateFeet(defaultToReferenceMat, inputCalibration); + calibrateHips(defaultToReferenceMat, inputCalibration); + calibrateChest(defaultToReferenceMat, inputCalibration); + } else if (_config == Config::FeetHipsAndShoulders && puckCount >= MIN_FEET_HIPS_SHOULDERS) { + calibrateFeet(defaultToReferenceMat, inputCalibration); + calibrateHips(defaultToReferenceMat, inputCalibration); + int firstShoulderIndex = 3; + int secondShoulderIndex = 4; + calibrateShoulders(defaultToReferenceMat, inputCalibration, firstShoulderIndex, secondShoulderIndex); } else { qDebug() << "Puck Calibration: " << configToString(_config) << " Config Failed: Could not meet the minimal # of pucks"; uncalibrate(); @@ -382,6 +376,8 @@ void ViveControllerManager::InputDevice::updateCalibratedLimbs() { _poseStateMap[controller::RIGHT_FOOT] = addOffsetToPuckPose(controller::RIGHT_FOOT); _poseStateMap[controller::HIPS] = addOffsetToPuckPose(controller::HIPS); _poseStateMap[controller::SPINE2] = addOffsetToPuckPose(controller::SPINE2); + _poseStateMap[controller::RIGHT_ARM] = addOffsetToPuckPose(controller::RIGHT_ARM); + _poseStateMap[controller::LEFT_ARM] = addOffsetToPuckPose(controller::LEFT_ARM); } controller::Pose ViveControllerManager::InputDevice::addOffsetToPuckPose(int joint) const { @@ -615,6 +611,56 @@ void ViveControllerManager::InputDevice::hapticsHelper(float deltaTime, bool lef } } +void ViveControllerManager::InputDevice::calibrateFeet(glm::mat4& defaultToReferenceMat, const controller::InputCalibrationData& inputCalibration) { + auto& firstFoot = _validTrackedObjects[FIRST_FOOT]; + auto& secondFoot = _validTrackedObjects[SECOND_FOOT]; + controller::Pose& firstFootPose = firstFoot.second; + controller::Pose& secondFootPose = secondFoot.second; + + if (firstFootPose.translation.x < secondFootPose.translation.x) { + _jointToPuckMap[controller::LEFT_FOOT] = firstFoot.first; + _pucksOffset[firstFoot.first] = computeOffset(defaultToReferenceMat, inputCalibration.defaultLeftFoot, firstFootPose); + _jointToPuckMap[controller::RIGHT_FOOT] = secondFoot.first; + _pucksOffset[secondFoot.first] = computeOffset(defaultToReferenceMat, inputCalibration.defaultRightFoot, secondFootPose); + } else { + _jointToPuckMap[controller::LEFT_FOOT] = secondFoot.first; + _pucksOffset[secondFoot.first] = computeOffset(defaultToReferenceMat, inputCalibration.defaultLeftFoot, secondFootPose); + _jointToPuckMap[controller::RIGHT_FOOT] = firstFoot.first; + _pucksOffset[firstFoot.first] = computeOffset(defaultToReferenceMat, inputCalibration.defaultRightFoot, firstFootPose); + } +} + +void ViveControllerManager::InputDevice::calibrateHips(glm::mat4& defaultToReferenceMat, const controller::InputCalibrationData& inputCalibration) { + _jointToPuckMap[controller::HIPS] = _validTrackedObjects[HIP].first; + _pucksOffset[_validTrackedObjects[HIP].first] = computeOffset(defaultToReferenceMat, inputCalibration.defaultHips, _validTrackedObjects[HIP].second); +} + +void ViveControllerManager::InputDevice::calibrateChest(glm::mat4& defaultToReferenceMat, const controller::InputCalibrationData& inputCalibration) { + _jointToPuckMap[controller::SPINE2] = _validTrackedObjects[CHEST].first; + _pucksOffset[_validTrackedObjects[CHEST].first] = computeOffset(defaultToReferenceMat, inputCalibration.defaultSpine2, _validTrackedObjects[CHEST].second); +} + +void ViveControllerManager::InputDevice::calibrateShoulders(glm::mat4& defaultToReferenceMat, const controller::InputCalibrationData& inputCalibration, + int firstShoulderIndex, int secondShoulderIndex) { + const PuckPosePair& firstShoulder = _validTrackedObjects[firstShoulderIndex]; + const PuckPosePair& secondShoulder = _validTrackedObjects[secondShoulderIndex]; + const controller::Pose& firstShoulderPose = firstShoulder.second; + const controller::Pose& secondShoulderPose = secondShoulder.second; + + if (firstShoulderPose.translation.x < secondShoulderPose.translation.x) { + _jointToPuckMap[controller::LEFT_ARM] = firstShoulder.first; + _pucksOffset[firstShoulder.first] = computeOffset(defaultToReferenceMat, inputCalibration.defaultLeftArm, firstShoulder.second); + _jointToPuckMap[controller::RIGHT_ARM] = secondShoulder.first; + _pucksOffset[secondShoulder.first] = computeOffset(defaultToReferenceMat, inputCalibration.defaultRightArm, secondShoulder.second); + } else { + _jointToPuckMap[controller::LEFT_ARM] = secondShoulder.first; + _pucksOffset[secondShoulder.first] = computeOffset(defaultToReferenceMat, inputCalibration.defaultLeftArm, secondShoulder.second); + _jointToPuckMap[controller::RIGHT_ARM] = firstShoulder.first; + _pucksOffset[firstShoulder.first] = computeOffset(defaultToReferenceMat, inputCalibration.defaultRightArm, firstShoulder.second); + } +} + + void ViveControllerManager::InputDevice::loadSettings() { Settings settings; settings.beginGroup("PUCK_CONFIG"); @@ -646,6 +692,8 @@ void ViveControllerManager::InputDevice::setConfigFromString(const QString& valu _preferedConfig = Config::FeetAndHips; } else if (value == "FeetHipsAndChest") { _preferedConfig = Config::FeetHipsAndChest; + } else if (value == "FeetHipsAndShoulders") { + _preferedConfig = Config::FeetHipsAndShoulders; } } @@ -658,7 +706,7 @@ void ViveControllerManager::InputDevice::createPreferences() { auto getter = [this]()->QString { return _configStringMap[_preferedConfig]; }; auto setter = [this](const QString& value) { setConfigFromString(value); saveSettings(); }; auto preference = new ComboBoxPreference(VIVE_PUCKS_CONFIG, "Configuration", getter, setter); - QStringList list = (QStringList() << "Auto" << "Feet" << "FeetAndHips" << "FeetHipsAndChest"); + QStringList list = {"Auto", "Feet", "FeetAndHips", "FeetHipsAndChest", "FeetHipsAndShoulders"}; preference->setItems(list); preferences->addPreference(preference); @@ -710,6 +758,8 @@ controller::Input::NamedVector ViveControllerManager::InputDevice::getAvailableI makePair(HIPS, "Hips"), makePair(SPINE2, "Spine2"), makePair(HEAD, "Head"), + makePair(LEFT_ARM, "LeftArm"), + makePair(RIGHT_ARM, "RightArm"), // 16 tracked poses makePair(TRACKED_OBJECT_00, "TrackedObject00"), diff --git a/plugins/openvr/src/ViveControllerManager.h b/plugins/openvr/src/ViveControllerManager.h index fa2566da45..a76adaa8f9 100644 --- a/plugins/openvr/src/ViveControllerManager.h +++ b/plugins/openvr/src/ViveControllerManager.h @@ -27,6 +27,8 @@ #include #include "OpenVrHelpers.h" +using PuckPosePair = std::pair; + namespace vr { class IVRSystem; } @@ -78,6 +80,15 @@ private: const vec3& angularVelocity); void partitionTouchpad(int sButton, int xAxis, int yAxis, int centerPsuedoButton, int xPseudoButton, int yPseudoButton); void printDeviceTrackingResultChange(uint32_t deviceIndex); + void setConfigFromString(const QString& value); + void loadSettings(); + void saveSettings() const; + void calibrateFeet(glm::mat4& defaultToReferenceMat, const controller::InputCalibrationData& inputCalibration); + void calibrateHips(glm::mat4& defaultToReferenceMat, const controller::InputCalibrationData& inputCalibration); + void calibrateChest(glm::mat4& defaultToReferenceMat, const controller::InputCalibrationData& inputCalibration); + + void calibrateShoulders(glm::mat4& defaultToReferenceMat, const controller::InputCalibrationData& inputCalibration, + int firstShoulderIndex, int secondShoulderIndex); class FilteredStick { public: @@ -102,13 +113,19 @@ private: float _timer { 0.0f }; glm::vec2 _stick { 0.0f, 0.0f }; }; - enum class Config { Feet, FeetAndHips, FeetHipsAndChest, Auto }; + enum class Config { + Auto, + Feet, + FeetAndHips, + FeetHipsAndChest, + FeetHipsAndShoulders, + }; Config _config { Config::Auto }; Config _preferedConfig { Config::Auto }; FilteredStick _filteredLeftStick; FilteredStick _filteredRightStick; - std::vector> _validTrackedObjects; + std::vector _validTrackedObjects; std::map _pucksOffset; std::map _jointToPuckMap; std::map _configStringMap; @@ -131,9 +148,6 @@ private: mutable std::recursive_mutex _lock; QString configToString(Config config); - void setConfigFromString(const QString& value); - void loadSettings(); - void saveSettings() const; friend class ViveControllerManager; };