From 6707f889b82d2e8fdaa065ac32e4d536f24b8dd6 Mon Sep 17 00:00:00 2001 From: Bradley Austin Davis Date: Mon, 13 Jun 2016 17:21:09 -0700 Subject: [PATCH] Fixing laser offset, support laser in Oculus --- .../display-plugins/OpenGLDisplayPlugin.cpp | 1 + .../src/display-plugins/OpenGLDisplayPlugin.h | 1 + .../display-plugins/hmd/HmdDisplayPlugin.cpp | 32 +++---- .../display-plugins/hmd/HmdDisplayPlugin.h | 4 +- .../oculus/src/OculusBaseDisplayPlugin.cpp | 24 ++++-- plugins/openvr/src/OpenVrDisplayPlugin.cpp | 26 ++++-- plugins/openvr/src/OpenVrHelpers.cpp | 86 ++++++++++++++++++- plugins/openvr/src/OpenVrHelpers.h | 4 + plugins/openvr/src/ViveControllerManager.cpp | 83 +----------------- 9 files changed, 138 insertions(+), 123 deletions(-) diff --git a/libraries/display-plugins/src/display-plugins/OpenGLDisplayPlugin.cpp b/libraries/display-plugins/src/display-plugins/OpenGLDisplayPlugin.cpp index 363bde15e6..23c836b3de 100644 --- a/libraries/display-plugins/src/display-plugins/OpenGLDisplayPlugin.cpp +++ b/libraries/display-plugins/src/display-plugins/OpenGLDisplayPlugin.cpp @@ -517,6 +517,7 @@ void OpenGLDisplayPlugin::compositeLayers() { glBindTexture(GL_TEXTURE_2D, 0); Context::Disable(Capability::Blend); } + compositeExtra(); }); } diff --git a/libraries/display-plugins/src/display-plugins/OpenGLDisplayPlugin.h b/libraries/display-plugins/src/display-plugins/OpenGLDisplayPlugin.h index 02a30a2570..69653b8c76 100644 --- a/libraries/display-plugins/src/display-plugins/OpenGLDisplayPlugin.h +++ b/libraries/display-plugins/src/display-plugins/OpenGLDisplayPlugin.h @@ -74,6 +74,7 @@ protected: virtual void compositeScene(); virtual void compositeOverlay(); virtual void compositePointer(); + virtual void compositeExtra() {}; virtual bool hasFocus() const override; diff --git a/libraries/display-plugins/src/display-plugins/hmd/HmdDisplayPlugin.cpp b/libraries/display-plugins/src/display-plugins/hmd/HmdDisplayPlugin.cpp index 9b71d3703b..9ae843d45d 100644 --- a/libraries/display-plugins/src/display-plugins/hmd/HmdDisplayPlugin.cpp +++ b/libraries/display-plugins/src/display-plugins/hmd/HmdDisplayPlugin.cpp @@ -316,10 +316,9 @@ void HmdDisplayPlugin::compositePointer() { Uniform(*_program, _mvpUniform).Set(mvp); _plane->Draw(); }); - - compositeLasers(); } + void HmdDisplayPlugin::internalPresent() { PROFILE_RANGE_EX(__FUNCTION__, 0xff00ff00, (uint64_t)presentCount()) @@ -412,24 +411,7 @@ bool HmdDisplayPlugin::setHandLaser(uint32_t hands, HandLaserMode mode, const ve return true; } -// FIXME try to consolidate the duplication of logic between this function and a similar one in CompsitorHelper. -static float calculateRayUiCollisionDistance(const glm::mat4& headPose, const glm::vec3& position, const glm::vec3& direction) { - auto relativePosition4 = glm::inverse(headPose) * vec4(position, 1); - auto relativePosition = vec3(relativePosition4) / relativePosition4.w; - auto relativeDirection = glm::inverse(glm::quat_cast(headPose)) * direction; - if (glm::abs(glm::length2(relativeDirection) - 1.0f) > EPSILON) { - relativeDirection = glm::normalize(relativeDirection); - } - // FIXME fetch the actual UI radius from... somewhere? - float uiRadius = 1.0f; - float instersectionDistance; - if (!glm::intersectRaySphere(relativePosition, relativeDirection, vec3(0), uiRadius * uiRadius, instersectionDistance)) { - return -1; - } - return instersectionDistance; -} - -void HmdDisplayPlugin::compositeLasers() { +void HmdDisplayPlugin::compositeExtra() { std::array handLasers; std::array renderHandPoses; withPresentThreadLock([&] { @@ -465,10 +447,16 @@ void HmdDisplayPlugin::compositeLasers() { const auto& laserDirection = handLaser.direction; auto model = renderHandPoses[i]; auto castDirection = glm::quat_cast(model) * laserDirection; + if (glm::abs(glm::length2(castDirection) - 1.0f) > EPSILON) { + castDirection = glm::normalize(castDirection); + } + + // FIXME fetch the actual UI radius from... somewhere? + float uiRadius = 1.0f; // Find the intersection of the laser with he UI and use it to scale the model matrix - float distance = calculateRayUiCollisionDistance(_currentPresentFrameInfo.presentPose, vec3(renderHandPoses[i][3]), castDirection); - if (distance < 0) { + float distance; + if (!glm::intersectRaySphere(vec3(renderHandPoses[i][3]), castDirection, vec3(0), uiRadius * uiRadius, distance)) { continue; } diff --git a/libraries/display-plugins/src/display-plugins/hmd/HmdDisplayPlugin.h b/libraries/display-plugins/src/display-plugins/hmd/HmdDisplayPlugin.h index 7cdcf06e9a..738028d684 100644 --- a/libraries/display-plugins/src/display-plugins/hmd/HmdDisplayPlugin.h +++ b/libraries/display-plugins/src/display-plugins/hmd/HmdDisplayPlugin.h @@ -46,9 +46,7 @@ protected: void customizeContext() override; void uncustomizeContext() override; void updateFrameData() override; - - void compositeLasers(); - + void compositeExtra() override; struct HandLaserInfo { HandLaserMode mode { HandLaserMode::None }; diff --git a/plugins/oculus/src/OculusBaseDisplayPlugin.cpp b/plugins/oculus/src/OculusBaseDisplayPlugin.cpp index a16f630bf8..61c4696f51 100644 --- a/plugins/oculus/src/OculusBaseDisplayPlugin.cpp +++ b/plugins/oculus/src/OculusBaseDisplayPlugin.cpp @@ -8,6 +8,7 @@ #include "OculusBaseDisplayPlugin.h" #include +#include #include "OculusHelpers.h" @@ -25,16 +26,21 @@ bool OculusBaseDisplayPlugin::beginFrameRender(uint32_t frameIndex) { _currentRenderFrameInfo.renderPose = toGlm(trackingState.HeadPose.ThePose); _currentRenderFrameInfo.presentPose = _currentRenderFrameInfo.renderPose; + std::array handPoses; + // Make controller poses available to the presentation thread + ovr_for_each_hand([&](ovrHandType hand) { + static const auto REQUIRED_HAND_STATUS = ovrStatus_OrientationTracked & ovrStatus_PositionTracked; + if (REQUIRED_HAND_STATUS != (trackingState.HandStatusFlags[hand] & REQUIRED_HAND_STATUS)) { + return; + } + + auto correctedPose = ovrControllerPoseToHandPose(hand, trackingState.HandPoses[hand]); + static const glm::quat HAND_TO_LASER_ROTATION = glm::rotation(Vectors::UNIT_Z, Vectors::UNIT_NEG_Y); + handPoses[hand] = glm::translate(glm::mat4(), correctedPose.translation) * glm::mat4_cast(correctedPose.rotation * HAND_TO_LASER_ROTATION); + }); + withRenderThreadLock([&] { - // Make controller poses available to the presentation thread - ovr_for_each_hand([&](ovrHandType hand){ - static const auto REQUIRED_HAND_STATUS = ovrStatus_OrientationTracked & ovrStatus_PositionTracked; - if (REQUIRED_HAND_STATUS == (trackingState.HandStatusFlags[hand] & REQUIRED_HAND_STATUS)) { - _handPoses[hand] = toGlm(trackingState.HandPoses[hand].ThePose); - } else { - _handPoses[hand] = glm::mat4(); - } - }); + _handPoses = handPoses; _frameInfos[frameIndex] = _currentRenderFrameInfo; }); return true; diff --git a/plugins/openvr/src/OpenVrDisplayPlugin.cpp b/plugins/openvr/src/OpenVrDisplayPlugin.cpp index 92c01dc0a3..d57dddf512 100644 --- a/plugins/openvr/src/OpenVrDisplayPlugin.cpp +++ b/plugins/openvr/src/OpenVrDisplayPlugin.cpp @@ -18,6 +18,7 @@ #include #include +#include #include #include #include @@ -179,15 +180,26 @@ bool OpenVrDisplayPlugin::beginFrameRender(uint32_t frameIndex) { _currentRenderFrameInfo.renderPose = _trackedDevicePoseMat4[vr::k_unTrackedDeviceIndex_Hmd]; bool keyboardVisible = isOpenVrKeyboardShown(); + + std::array handPoses; + if (!keyboardVisible) { + for (int i = 0; i < 2; ++i) { + if (handIndices[i] == vr::k_unTrackedDeviceIndexInvalid) { + continue; + } + auto deviceIndex = handIndices[i]; + const mat4& mat = _trackedDevicePoseMat4[deviceIndex]; + const vec3& linearVelocity = _trackedDeviceLinearVelocities[deviceIndex]; + const vec3& angularVelocity = _trackedDeviceAngularVelocities[deviceIndex]; + auto correctedPose = openVrControllerPoseToHandPose(i == 0, mat, linearVelocity, angularVelocity); + static const glm::quat HAND_TO_LASER_ROTATION = glm::rotation(Vectors::UNIT_Z, Vectors::UNIT_NEG_Y); + handPoses[i] = glm::translate(glm::mat4(), correctedPose.translation) * glm::mat4_cast(correctedPose.rotation * HAND_TO_LASER_ROTATION); + } + } + withRenderThreadLock([&] { // Make controller poses available to the presentation thread - for (int i = 0; i < 2; ++i) { - if (keyboardVisible || handIndices[i] == vr::k_unTrackedDeviceIndexInvalid) { - _handPoses[i] = glm::mat4(); - } else { - _handPoses[i] = _sensorResetMat * toGlm(_trackedDevicePose[handIndices[i]].mDeviceToAbsoluteTracking); - } - } + _handPoses = handPoses; _frameInfos[frameIndex] = _currentRenderFrameInfo; }); return true; diff --git a/plugins/openvr/src/OpenVrHelpers.cpp b/plugins/openvr/src/OpenVrHelpers.cpp index e4cca6ecd6..4b4eef0538 100644 --- a/plugins/openvr/src/OpenVrHelpers.cpp +++ b/plugins/openvr/src/OpenVrHelpers.cpp @@ -18,8 +18,9 @@ #include #include - #include +#include +#include Q_DECLARE_LOGGING_CATEGORY(displayplugins) Q_LOGGING_CATEGORY(displayplugins, "hifi.plugins.display") @@ -242,3 +243,86 @@ void handleOpenVrEvents() { } +controller::Pose openVrControllerPoseToHandPose(bool isLeftHand, const mat4& mat, const vec3& linearVelocity, const vec3& angularVelocity) { + // When the sensor-to-world rotation is identity the coordinate axes look like this: + // + // user + // forward + // -z + // | + // y| user + // y o----x right + // o-----x user + // | up + // | + // z + // + // Rift + + // From ABOVE the hand canonical axes looks like this: + // + // | | | | y | | | | + // | | | | | | | | | + // | | | | | + // |left | / x---- + \ |right| + // | _/ z \_ | + // | | | | + // | | | | + // + + // So when the user is in Rift space facing the -zAxis with hands outstretched and palms down + // the rotation to align the Touch axes with those of the hands is: + // + // touchToHand = halfTurnAboutY * quaterTurnAboutX + + // Due to how the Touch controllers fit into the palm there is an offset that is different for each hand. + // You can think of this offset as the inverse of the measured rotation when the hands are posed, such that + // the combination (measurement * offset) is identity at this orientation. + // + // Qoffset = glm::inverse(deltaRotation when hand is posed fingers forward, palm down) + // + // An approximate offset for the Touch can be obtained by inspection: + // + // Qoffset = glm::inverse(glm::angleAxis(sign * PI/2.0f, zAxis) * glm::angleAxis(PI/4.0f, xAxis)) + // + // So the full equation is: + // + // Q = combinedMeasurement * touchToHand + // + // Q = (deltaQ * QOffset) * (yFlip * quarterTurnAboutX) + // + // Q = (deltaQ * inverse(deltaQForAlignedHand)) * (yFlip * quarterTurnAboutX) + static const glm::quat yFlip = glm::angleAxis(PI, Vectors::UNIT_Y); + static const glm::quat quarterX = glm::angleAxis(PI_OVER_TWO, Vectors::UNIT_X); + static const glm::quat touchToHand = yFlip * quarterX; + + static const glm::quat leftQuarterZ = glm::angleAxis(-PI_OVER_TWO, Vectors::UNIT_Z); + static const glm::quat rightQuarterZ = glm::angleAxis(PI_OVER_TWO, Vectors::UNIT_Z); + static const glm::quat eighthX = glm::angleAxis(PI / 4.0f, Vectors::UNIT_X); + + static const glm::quat leftRotationOffset = glm::inverse(leftQuarterZ * eighthX) * touchToHand; + static const glm::quat rightRotationOffset = glm::inverse(rightQuarterZ * eighthX) * touchToHand; + + static const float CONTROLLER_LENGTH_OFFSET = 0.0762f; // three inches + static const glm::vec3 CONTROLLER_OFFSET = glm::vec3(CONTROLLER_LENGTH_OFFSET / 2.0f, + CONTROLLER_LENGTH_OFFSET / 2.0f, + CONTROLLER_LENGTH_OFFSET * 2.0f); + static const glm::vec3 leftTranslationOffset = glm::vec3(-1.0f, 1.0f, 1.0f) * CONTROLLER_OFFSET; + static const glm::vec3 rightTranslationOffset = CONTROLLER_OFFSET; + + auto translationOffset = (isLeftHand ? leftTranslationOffset : rightTranslationOffset); + auto rotationOffset = (isLeftHand ? leftRotationOffset : rightRotationOffset); + + glm::vec3 position = extractTranslation(mat); + glm::quat rotation = glm::normalize(glm::quat_cast(mat)); + + position += rotation * translationOffset; + rotation = rotation * rotationOffset; + + // transform into avatar frame + auto result = controller::Pose(position, rotation); + // handle change in velocity due to translationOffset + result.velocity = linearVelocity + glm::cross(angularVelocity, position - extractTranslation(mat)); + result.angularVelocity = angularVelocity; + return result; +} \ No newline at end of file diff --git a/plugins/openvr/src/OpenVrHelpers.h b/plugins/openvr/src/OpenVrHelpers.h index db8bb4f2e8..131db517ab 100644 --- a/plugins/openvr/src/OpenVrHelpers.h +++ b/plugins/openvr/src/OpenVrHelpers.h @@ -12,6 +12,8 @@ #include #include +#include + bool openVrSupported(); vr::IVRSystem* acquireOpenVrSystem(); @@ -55,3 +57,5 @@ inline vr::HmdMatrix34_t toOpenVr(const mat4& m) { } return result; } + +controller::Pose openVrControllerPoseToHandPose(bool isLeftHand, const mat4& mat, const vec3& linearVelocity, const vec3& angularVelocity); diff --git a/plugins/openvr/src/ViveControllerManager.cpp b/plugins/openvr/src/ViveControllerManager.cpp index 4e17a2edf4..83b72c5c08 100644 --- a/plugins/openvr/src/ViveControllerManager.cpp +++ b/plugins/openvr/src/ViveControllerManager.cpp @@ -37,10 +37,6 @@ vr::IVRSystem* acquireOpenVrSystem(); void releaseOpenVrSystem(); -static const float CONTROLLER_LENGTH_OFFSET = 0.0762f; // three inches -static const glm::vec3 CONTROLLER_OFFSET = glm::vec3(CONTROLLER_LENGTH_OFFSET / 2.0f, - CONTROLLER_LENGTH_OFFSET / 2.0f, - CONTROLLER_LENGTH_OFFSET * 2.0f); static const char* CONTROLLER_MODEL_STRING = "vr_controller_05_wireless_b"; static const QString MENU_PARENT = "Avatar"; @@ -382,86 +378,11 @@ void ViveControllerManager::InputDevice::handleButtonEvent(float deltaTime, uint void ViveControllerManager::InputDevice::handlePoseEvent(float deltaTime, const controller::InputCalibrationData& inputCalibrationData, const mat4& mat, const vec3& linearVelocity, const vec3& angularVelocity, bool isLeftHand) { - // When the sensor-to-world rotation is identity the coordinate axes look like this: - // - // user - // forward - // -z - // | - // y| user - // y o----x right - // o-----x user - // | up - // | - // z - // - // Vive - // - - // From ABOVE the hand canonical axes looks like this: - // - // | | | | y | | | | - // | | | | | | | | | - // | | | | | - // |left | / x---- + \ |right| - // | _/ z \_ | - // | | | | - // | | | | - // - - // So when the user is standing in Vive space facing the -zAxis with hands outstretched and palms down - // the rotation to align the Vive axes with those of the hands is: - // - // QviveToHand = halfTurnAboutY * quaterTurnAboutX - - // Due to how the Vive controllers fit into the palm there is an offset that is different for each hand. - // You can think of this offset as the inverse of the measured rotation when the hands are posed, such that - // the combination (measurement * offset) is identity at this orientation. - // - // Qoffset = glm::inverse(deltaRotation when hand is posed fingers forward, palm down) - // - // An approximate offset for the Vive can be obtained by inspection: - // - // Qoffset = glm::inverse(glm::angleAxis(sign * PI/4.0f, zAxis) * glm::angleAxis(PI/2.0f, xAxis)) - // - // So the full equation is: - // - // Q = combinedMeasurement * viveToHand - // - // Q = (deltaQ * QOffset) * (yFlip * quarterTurnAboutX) - // - // Q = (deltaQ * inverse(deltaQForAlignedHand)) * (yFlip * quarterTurnAboutX) - - static const glm::quat yFlip = glm::angleAxis(PI, Vectors::UNIT_Y); - static const glm::quat quarterX = glm::angleAxis(PI_OVER_TWO, Vectors::UNIT_X); - static const glm::quat viveToHand = yFlip * quarterX; - - static const glm::quat leftQuaterZ = glm::angleAxis(-PI_OVER_TWO, Vectors::UNIT_Z); - static const glm::quat rightQuaterZ = glm::angleAxis(PI_OVER_TWO, Vectors::UNIT_Z); - static const glm::quat eighthX = glm::angleAxis(PI / 4.0f, Vectors::UNIT_X); - - static const glm::quat leftRotationOffset = glm::inverse(leftQuaterZ * eighthX) * viveToHand; - static const glm::quat rightRotationOffset = glm::inverse(rightQuaterZ * eighthX) * viveToHand; - - static const glm::vec3 leftTranslationOffset = glm::vec3(-1.0f, 1.0f, 1.0f) * CONTROLLER_OFFSET; - static const glm::vec3 rightTranslationOffset = CONTROLLER_OFFSET; - - auto translationOffset = (isLeftHand ? leftTranslationOffset : rightTranslationOffset); - auto rotationOffset = (isLeftHand ? leftRotationOffset : rightRotationOffset); - - glm::vec3 position = extractTranslation(mat); - glm::quat rotation = glm::normalize(glm::quat_cast(mat)); - - position += rotation * translationOffset; - rotation = rotation * rotationOffset; + auto pose = openVrControllerPoseToHandPose(isLeftHand, mat, linearVelocity, angularVelocity); // transform into avatar frame glm::mat4 controllerToAvatar = glm::inverse(inputCalibrationData.avatarMat) * inputCalibrationData.sensorToWorldMat; - auto avatarPose = controller::Pose(position, rotation); - // handle change in velocity due to translationOffset - avatarPose.velocity = linearVelocity + glm::cross(angularVelocity, position - extractTranslation(mat)); - avatarPose.angularVelocity = angularVelocity; - _poseStateMap[isLeftHand ? controller::LEFT_HAND : controller::RIGHT_HAND] = avatarPose.transform(controllerToAvatar); + _poseStateMap[isLeftHand ? controller::LEFT_HAND : controller::RIGHT_HAND] = pose.transform(controllerToAvatar); } bool ViveControllerManager::InputDevice::triggerHapticPulse(float strength, float duration, controller::Hand hand) {