From d98e7d5bc673cb56d6b2e8c56175aaff2c8a1b14 Mon Sep 17 00:00:00 2001 From: Dante Ruiz Date: Tue, 23 May 2017 00:26:12 +0100 Subject: [PATCH] adding left and right arm --- interface/src/Application.cpp | 6 +- interface/src/avatar/MyAvatar.cpp | 56 +++++++++++++++++++ interface/src/avatar/MyAvatar.h | 4 ++ .../controllers/src/controllers/Actions.cpp | 2 + .../controllers/src/controllers/Actions.h | 2 + libraries/controllers/src/controllers/Input.h | 6 +- .../src/controllers/StandardController.cpp | 2 + plugins/openvr/src/ViveControllerManager.cpp | 43 +++++++------- plugins/openvr/src/ViveControllerManager.h | 13 ++++- 9 files changed, 112 insertions(+), 22 deletions(-) diff --git a/interface/src/Application.cpp b/interface/src/Application.cpp index 1d31a84ed4..9deb92374f 100644 --- a/interface/src/Application.cpp +++ b/interface/src/Application.cpp @@ -4368,7 +4368,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; diff --git a/interface/src/avatar/MyAvatar.cpp b/interface/src/avatar/MyAvatar.cpp index b8d6a4cfa4..5814385de5 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 }; @@ -2790,6 +2798,54 @@ glm::mat4 MyAvatar::getRightFootCalibrationMat() const { } } + +glm::mat4 MyAvatar::getRightArmCalibrationMat() const { + int rightArmIndex = _rig->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 = _rig->indexOfJoint("LeftArm"); + auto skeleton = _rig->getAnimSkeleton(); + 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 = _rig->indexOfJoint("RightHand"); + auto skeleton = _rig->getAnimSkeleton(); + 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 = _rig->indexOfJoint("LeftHand"); + auto skeleton = _rig->getAnimSkeleton(); + 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 95643f3bd4..aa96b75211 100644 --- a/interface/src/avatar/MyAvatar.h +++ b/interface/src/avatar/MyAvatar.h @@ -488,6 +488,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 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..7780e91769 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 defaultRigtArm; // 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/plugins/openvr/src/ViveControllerManager.cpp b/plugins/openvr/src/ViveControllerManager.cpp index 3bda481243..8fa6f8b9f6 100644 --- a/plugins/openvr/src/ViveControllerManager.cpp +++ b/plugins/openvr/src/ViveControllerManager.cpp @@ -49,6 +49,9 @@ 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 MIN_FEET_HIPS_CHEST_AND_HANDS = 6; +static const int MIN_FEET_HIPS_SHOULDERS_AND_HANDS = 7; static const int FIRST_FOOT = 0; static const int SECOND_FOOT = 1; static const int HIP = 2; @@ -330,26 +333,23 @@ 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 + 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); + } } 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); @@ -358,6 +358,7 @@ void ViveControllerManager::InputDevice::calibrate(const controller::InputCalibr _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); + } else if (_config == Config::FeetHipsAndShoulders && puckCount >= MIN_FEET_HIPS_SHOULDERS){ } else { qDebug() << "Puck Calibration: " << configToString(_config) << " Config Failed: Could not meet the minimal # of pucks"; uncalibrate(); @@ -379,6 +380,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 { @@ -707,6 +710,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..186917de51 100644 --- a/plugins/openvr/src/ViveControllerManager.h +++ b/plugins/openvr/src/ViveControllerManager.h @@ -102,7 +102,17 @@ private: float _timer { 0.0f }; glm::vec2 _stick { 0.0f, 0.0f }; }; - enum class Config { Feet, FeetAndHips, FeetHipsAndChest, Auto }; + enum class Config { + Auto, + Feet, + Hands, + Shoulders, + FeetAndHips, + FeetHipsAndChest, + FeetHipsAndShoulders, + FeetHipsChestAndHands, + FeetHipsShouldersAndHands + }; Config _config { Config::Auto }; Config _preferedConfig { Config::Auto }; FilteredStick _filteredLeftStick; @@ -128,6 +138,7 @@ private: bool _triggersPressedHandled { false }; bool _calibrated { false }; bool _timeTilCalibrationSet { false }; + bool overrideHands { false }; mutable std::recursive_mutex _lock; QString configToString(Config config);