diff --git a/interface/resources/controllers/vive.json b/interface/resources/controllers/vive.json index 4fbdb37abf..5bdffadbbf 100644 --- a/interface/resources/controllers/vive.json +++ b/interface/resources/controllers/vive.json @@ -35,6 +35,10 @@ { "from": "Vive.RightApplicationMenu", "to": "Standard.RightSecondaryThumb" }, { "from": "Vive.LeftHand", "to": "Standard.LeftHand", "when": [ "Application.InHMD" ] }, - { "from": "Vive.RightHand", "to": "Standard.RightHand", "when": [ "Application.InHMD" ] } + { "from": "Vive.RightHand", "to": "Standard.RightHand", "when": [ "Application.InHMD" ] }, + { "from": "Vive.LeftFoot", "to" : "Standard.LeftFoot", "when": [ "Application.InHMD"] }, + { "from": "Vive.RightFoot", "to" : "Standard.RightFoot", "when": [ "Application.InHMD"] }, + { "from": "Vive.Hips", "to" : "Standard.Hips", "when": [ "Application.InHMD"] }, + { "from": "Vive.Spine2", "to" : "Standard.Spine2", "when": [ "Application.InHMD"] } ] } diff --git a/plugins/openvr/src/ViveControllerManager.cpp b/plugins/openvr/src/ViveControllerManager.cpp index 86b37135d2..db12d25e0d 100644 --- a/plugins/openvr/src/ViveControllerManager.cpp +++ b/plugins/openvr/src/ViveControllerManager.cpp @@ -10,6 +10,7 @@ // #include "ViveControllerManager.h" +#include #include #include @@ -20,7 +21,11 @@ #include #include #include +#include #include +#include +#include +#include #include @@ -36,14 +41,32 @@ void releaseOpenVrSystem(); static const char* CONTROLLER_MODEL_STRING = "vr_controller_05_wireless_b"; +const quint64 CALIBRATION_TIMELAPSE = 2 * USECS_PER_SECOND; static const char* MENU_PARENT = "Avatar"; static const char* MENU_NAME = "Vive Controllers"; static const char* MENU_PATH = "Avatar" ">" "Vive Controllers"; 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 FIRST_FOOT = 0; +static const int SECOND_FOOT = 1; +static const int HIP = 2; +static const int CHEST = 3; const char* ViveControllerManager::NAME { "OpenVR" }; +static glm::mat4 computeOffset(glm::mat4 defaultToReferenceMat, glm::mat4 defaultJointMat, controller::Pose puckPose) { + glm::mat4 poseMat = createMatFromQuatAndPos(puckPose.rotation, puckPose.translation); + glm::mat4 referenceJointMat = defaultToReferenceMat * defaultJointMat; + return glm::inverse(poseMat) * referenceJointMat; +} + +static bool sortPucksYPosition(std::pair firstPuck, std::pair secondPuck) { + return (firstPuck.second.translation.y < firstPuck.second.translation.y); +} + bool ViveControllerManager::isSupported() const { return openVrSupported(); } @@ -125,6 +148,7 @@ void ViveControllerManager::pluginUpdate(float deltaTime, const controller::Inpu void ViveControllerManager::InputDevice::update(float deltaTime, const controller::InputCalibrationData& inputCalibrationData) { _poseStateMap.clear(); _buttonPressedMap.clear(); + _validTrackedObjects.clear(); // While the keyboard is open, we defer strictly to the keyboard values if (isOpenVrKeyboardShown()) { @@ -164,10 +188,27 @@ void ViveControllerManager::InputDevice::update(float deltaTime, const controlle numTrackedControllers++; } _trackedControllers = numTrackedControllers; + + if (checkForCalibrationEvent()) { + quint64 currentTime = usecTimestampNow(); + if (!_timeTilCalibrationSet) { + _timeTilCalibrationSet = true; + _timeTilCalibration = currentTime + CALIBRATION_TIMELAPSE; + } + + if (currentTime > _timeTilCalibration && !_triggersPressedHandled) { + _triggersPressedHandled = true; + calibrateOrUncalibrate(inputCalibrationData); + } + } else { + _triggersPressedHandled = false; + _timeTilCalibrationSet = false; + } + + updateCalibratedLimbs(); } void ViveControllerManager::InputDevice::handleTrackedObject(uint32_t deviceIndex, const controller::InputCalibrationData& inputCalibrationData) { - uint32_t poseIndex = controller::TRACKED_OBJECT_00 + deviceIndex; if (_system->IsTrackedDeviceConnected(deviceIndex) && @@ -185,12 +226,114 @@ void ViveControllerManager::InputDevice::handleTrackedObject(uint32_t deviceInde // transform into avatar frame glm::mat4 controllerToAvatar = glm::inverse(inputCalibrationData.avatarMat) * inputCalibrationData.sensorToWorldMat; _poseStateMap[poseIndex] = pose.transform(controllerToAvatar); + _validTrackedObjects.push_back(std::make_pair(poseIndex, _poseStateMap[poseIndex])); } else { controller::Pose invalidPose; _poseStateMap[poseIndex] = invalidPose; } } +void ViveControllerManager::InputDevice::calibrateOrUncalibrate(const controller::InputCalibrationData& inputCalibration) { + if (!_calibrated) { + calibrate(inputCalibration); + } else { + uncalibrate(); + } +} + +void ViveControllerManager::InputDevice::calibrate(const controller::InputCalibrationData& inputCalibration) { + // convert the hmd head from sensor space to avatar space + glm::mat4 hmdSensorFlippedMat = inputCalibration.hmdSensorMat * Matrices::Y_180; + glm::mat4 sensorToAvatarMat = glm::inverse(inputCalibration.avatarMat) * inputCalibration.sensorToWorldMat; + glm::mat4 hmdAvatarMat = sensorToAvatarMat * hmdSensorFlippedMat; + + // cancel the roll and pitch for the hmd head + glm::quat hmdRotation = cancelOutRollAndPitch(glmExtractRotation(hmdAvatarMat)); + glm::vec3 hmdTranslation = extractTranslation(hmdAvatarMat); + glm::mat4 currentHmd = createMatFromQuatAndPos(hmdRotation, hmdTranslation); + + // calculate the offset from the centerOfEye to defaultHeadMat + glm::mat4 defaultHeadOffset = glm::inverse(inputCalibration.defaultCenterEyeMat) * inputCalibration.defaultHeadMat; + + glm::mat4 currentHead = currentHmd * defaultHeadOffset; + + // calculate the defaultToRefrenceXform + glm::mat4 defaultToReferenceMat = currentHead * glm::inverse(inputCalibration.defaultHeadMat); + + int puckCount = (int)_validTrackedObjects.size(); + if (puckCount == MIN_PUCK_COUNT) { + _config = Config::Feet; + } else if (puckCount == MIN_FEET_AND_HIPS) { + _config = Config::FeetAndHips; + } else if (puckCount >= MIN_FEET_HIPS_CHEST) { + _config = Config::FeetHipsAndChest; + } else { + return; + } + + 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 + } else if (_config == Config::FeetAndHips) { + _jointToPuckMap[controller::HIPS] = _validTrackedObjects[HIP].first; + _pucksOffset[_validTrackedObjects[HIP].first] = computeOffset(defaultToReferenceMat, inputCalibration.defaultHips, _validTrackedObjects[HIP].second); + } else if (_config == Config::FeetHipsAndChest) { + _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); + } + _calibrated = true; +} + +void ViveControllerManager::InputDevice::uncalibrate() { + _pucksOffset.clear(); + _jointToPuckMap.clear(); + _calibrated = false; +} + +void ViveControllerManager::InputDevice::updateCalibratedLimbs() { + _poseStateMap[controller::LEFT_FOOT] = addOffsetToPuckPose(controller::LEFT_FOOT); + _poseStateMap[controller::RIGHT_FOOT] = addOffsetToPuckPose(controller::RIGHT_FOOT); + _poseStateMap[controller::HIPS] = addOffsetToPuckPose(controller::HIPS); + _poseStateMap[controller::SPINE2] = addOffsetToPuckPose(controller::SPINE2); +} + +controller::Pose ViveControllerManager::InputDevice::addOffsetToPuckPose(int joint) const { + auto puck = _jointToPuckMap.find(joint); + if (puck != _jointToPuckMap.end()) { + uint32_t puckIndex = puck->second; + auto puckPose = _poseStateMap.find(puckIndex); + auto puckOffset = _pucksOffset.find(puckIndex); + + if ((puckPose != _poseStateMap.end()) && (puckOffset != _pucksOffset.end())) { + return puckPose->second.postTransform(puckOffset->second); + } + } + return controller::Pose(); +} + void ViveControllerManager::InputDevice::handleHandController(float deltaTime, uint32_t deviceIndex, const controller::InputCalibrationData& inputCalibrationData, bool isLeftHand) { if (_system->IsTrackedDeviceConnected(deviceIndex) && @@ -262,7 +405,7 @@ void ViveControllerManager::InputDevice::handleAxisEvent(float deltaTime, uint32 _axisStateMap[isLeftHand ? LY : RY] = stick.y; } else if (axis == vr::k_EButton_SteamVR_Trigger) { _axisStateMap[isLeftHand ? LT : RT] = x; - // The click feeling on the Vive controller trigger represents a value of *precisely* 1.0, + // The click feeling on the Vive controller trigger represents a value of *precisely* 1.0, // so we can expose that as an additional button if (x >= 1.0f) { _buttonPressedMap.insert(isLeftHand ? LT_CLICK : RT_CLICK); @@ -276,6 +419,14 @@ enum ViveButtonChannel { RIGHT_APP_MENU }; +bool ViveControllerManager::InputDevice::checkForCalibrationEvent() { + auto& endOfMap = _buttonPressedMap.end(); + auto& leftTrigger = _buttonPressedMap.find(controller::LT); + auto& rightTrigger = _buttonPressedMap.find(controller::RT); + auto& leftAppButton = _buttonPressedMap.find(LEFT_APP_MENU); + auto& rightAppButton = _buttonPressedMap.find(RIGHT_APP_MENU); + return ((leftTrigger != endOfMap && leftAppButton != endOfMap) && (rightTrigger != endOfMap && rightAppButton != endOfMap)); +} // These functions do translation from the Steam IDs to the standard controller IDs void ViveControllerManager::InputDevice::handleButtonEvent(float deltaTime, uint32_t button, bool pressed, bool touched, bool isLeftHand) { @@ -353,7 +504,7 @@ void ViveControllerManager::InputDevice::hapticsHelper(float deltaTime, bool lef float hapticTime = strength * MAX_HAPTIC_TIME; if (hapticTime < duration * 1000.0f) { _system->TriggerHapticPulse(deviceIndex, 0, hapticTime); - } + } float remainingHapticTime = duration - (hapticTime / 1000.0f + deltaTime * 1000.0f); // in milliseconds if (leftHand) { @@ -404,6 +555,10 @@ controller::Input::NamedVector ViveControllerManager::InputDevice::getAvailableI // 3d location of controller makePair(LEFT_HAND, "LeftHand"), makePair(RIGHT_HAND, "RightHand"), + makePair(LEFT_FOOT, "LeftFoot"), + makePair(RIGHT_FOOT, "RightFoot"), + makePair(HIPS, "Hips"), + makePair(SPINE2, "Spine2"), // 16 tracked poses makePair(TRACKED_OBJECT_00, "TrackedObject00"), diff --git a/plugins/openvr/src/ViveControllerManager.h b/plugins/openvr/src/ViveControllerManager.h index dc1883d5e4..9375fd20f0 100644 --- a/plugins/openvr/src/ViveControllerManager.h +++ b/plugins/openvr/src/ViveControllerManager.h @@ -14,9 +14,11 @@ #include #include +#include +#include +#include #include - #include #include #include @@ -58,7 +60,12 @@ private: bool triggerHapticPulse(float strength, float duration, controller::Hand hand) override; void hapticsHelper(float deltaTime, bool leftHand); - + void calibrateOrUncalibrate(const controller::InputCalibrationData& inputCalibration); + void calibrate(const controller::InputCalibrationData& inputCalibration); + void uncalibrate(); + controller::Pose addOffsetToPuckPose(int joint) const; + void updateCalibratedLimbs(); + bool checkForCalibrationEvent(); void handleHandController(float deltaTime, uint32_t deviceIndex, const controller::InputCalibrationData& inputCalibrationData, bool isLeftHand); void handleTrackedObject(uint32_t deviceIndex, const controller::InputCalibrationData& inputCalibrationData); void handleButtonEvent(float deltaTime, uint32_t button, bool pressed, bool touched, bool isLeftHand); @@ -90,10 +97,14 @@ private: float _timer { 0.0f }; glm::vec2 _stick { 0.0f, 0.0f }; }; - + enum class Config { Feet, FeetAndHips, FeetHipsAndChest, NoConfig }; + Config _config { Config::NoConfig }; FilteredStick _filteredLeftStick; FilteredStick _filteredRightStick; + std::vector> _validTrackedObjects; + std::map _pucksOffset; + std::map _jointToPuckMap; // perform an action when the InputDevice mutex is acquired. using Locker = std::unique_lock; template @@ -101,10 +112,14 @@ private: int _trackedControllers { 0 }; vr::IVRSystem*& _system; + quint64 _timeTilCalibration { 0.0f }; float _leftHapticStrength { 0.0f }; float _leftHapticDuration { 0.0f }; float _rightHapticStrength { 0.0f }; float _rightHapticDuration { 0.0f }; + bool _triggersPressedHandled { false }; + bool _calibrated { false }; + bool _timeTilCalibrationSet { false }; mutable std::recursive_mutex _lock; friend class ViveControllerManager;