From a32ab77b7dad3a9eaac11571584a7c3940835af2 Mon Sep 17 00:00:00 2001 From: Brad Davis Date: Wed, 17 Aug 2016 15:22:30 -0700 Subject: [PATCH] Address issues with random camera locations caused by bad HMD sensor poses --- interface/src/avatar/MyAvatar.cpp | 12 +++- plugins/openvr/src/OpenVrDisplayPlugin.cpp | 74 +++++++++++++--------- plugins/openvr/src/OpenVrHelpers.h | 7 ++ 3 files changed, 62 insertions(+), 31 deletions(-) diff --git a/interface/src/avatar/MyAvatar.cpp b/interface/src/avatar/MyAvatar.cpp index 57e379a9ac..782ecbcc64 100644 --- a/interface/src/avatar/MyAvatar.cpp +++ b/interface/src/avatar/MyAvatar.cpp @@ -516,13 +516,23 @@ glm::mat4 MyAvatar::getSensorToWorldMatrix() const { return _sensorToWorldMatrixCache.get(); } +// As far as I know no HMD system supports a play area of a kilometer in radius. +static const float MAX_HMD_ORIGIN_DISTANCE = 1000.0f; // Pass a recent sample of the HMD to the avatar. // This can also update the avatar's position to follow the HMD // as it moves through the world. void MyAvatar::updateFromHMDSensorMatrix(const glm::mat4& hmdSensorMatrix) { // update the sensorMatrices based on the new hmd pose _hmdSensorMatrix = hmdSensorMatrix; - _hmdSensorPosition = extractTranslation(hmdSensorMatrix); + auto newHmdSensorPosition = extractTranslation(hmdSensorMatrix); + + if (newHmdSensorPosition != _hmdSensorPosition && + glm::length(newHmdSensorPosition) > MAX_HMD_ORIGIN_DISTANCE) { + qWarning() << "Invalid HMD sensor position " << newHmdSensorPosition; + // Ignore unreasonable HMD sensor data + return; + } + _hmdSensorPosition = newHmdSensorPosition; _hmdSensorOrientation = glm::quat_cast(hmdSensorMatrix); _hmdSensorFacing = getFacingDir2D(_hmdSensorOrientation); } diff --git a/plugins/openvr/src/OpenVrDisplayPlugin.cpp b/plugins/openvr/src/OpenVrDisplayPlugin.cpp index b5f360ad8d..e751427ce2 100644 --- a/plugins/openvr/src/OpenVrDisplayPlugin.cpp +++ b/plugins/openvr/src/OpenVrDisplayPlugin.cpp @@ -108,19 +108,20 @@ public: } void updateSource() { - Lock lock(_plugin._presentMutex); - while (!_queue.empty()) { - auto& front = _queue.front(); - auto result = glClientWaitSync(front.fence, 0, 0); - if (GL_TIMEOUT_EXPIRED == result && GL_WAIT_FAILED == result) { - break; - } + _plugin.withNonPresentThreadLock([&] { + while (!_queue.empty()) { + auto& front = _queue.front(); + auto result = glClientWaitSync(front.fence, 0, 0); + if (GL_TIMEOUT_EXPIRED == result && GL_WAIT_FAILED == result) { + break; + } - glDeleteSync(front.fence); - front.fence = 0; - _current = front; - _queue.pop(); - } + glDeleteSync(front.fence); + front.fence = 0; + _current = front; + _queue.pop(); + } + }); } void run() override { @@ -170,15 +171,28 @@ public: PoseData nextRender, nextSim; nextRender.frameIndex = _plugin.presentCount(); vr::VRCompositor()->WaitGetPoses(nextRender.vrPoses, vr::k_unMaxTrackedDeviceCount, nextSim.vrPoses, vr::k_unMaxTrackedDeviceCount); - { - Lock lock(_plugin._presentMutex); - _presentCount++; - _presented.notify_one(); - _nextRender = nextRender; - _nextRender.update(_plugin._sensorResetMat); - _nextSim = nextSim; - _nextSim.update(_plugin._sensorResetMat); + + // Copy invalid poses in nextSim from nextRender + for (uint32_t i = 0; i < vr::k_unMaxTrackedDeviceCount; ++i) { + if (!nextSim.vrPoses[i].bPoseIsValid) { + nextSim.vrPoses[i] = nextRender.vrPoses[i]; + } } + + mat4 sensorResetMat; + _plugin.withNonPresentThreadLock([&] { + sensorResetMat = _plugin._sensorResetMat; + }); + + nextRender.update(sensorResetMat); + nextSim.update(sensorResetMat); + + _plugin.withNonPresentThreadLock([&] { + _nextRender = nextRender; + _nextSim = nextSim; + ++_presentCount; + _presented.notify_one(); + }); } _canvas.doneCurrent(); } @@ -366,19 +380,20 @@ bool OpenVrDisplayPlugin::beginFrameRender(uint32_t frameIndex) { } _currentRenderFrameInfo = FrameInfo(); + PoseData nextSimPoseData; withNonPresentThreadLock([&] { - _currentRenderFrameInfo.renderPose = _nextSimPoseData.poses[vr::k_unTrackedDeviceIndex_Hmd]; + nextSimPoseData = _nextSimPoseData; }); // HACK: when interface is launched and steam vr is NOT running, openvr will return bad HMD poses for a few frames // To workaround this, filter out any hmd poses that are obviously bad, i.e. beneath the floor. - if (isBadPose(&_nextSimPoseData.vrPoses[vr::k_unTrackedDeviceIndex_Hmd].mDeviceToAbsoluteTracking)) { + if (isBadPose(&nextSimPoseData.vrPoses[vr::k_unTrackedDeviceIndex_Hmd].mDeviceToAbsoluteTracking)) { qDebug() << "WARNING: ignoring bad hmd pose from openvr"; // use the last known good HMD pose - _nextSimPoseData.vrPoses[vr::k_unTrackedDeviceIndex_Hmd].mDeviceToAbsoluteTracking = _lastGoodHMDPose; + nextSimPoseData.vrPoses[vr::k_unTrackedDeviceIndex_Hmd].mDeviceToAbsoluteTracking = _lastGoodHMDPose; } else { - _lastGoodHMDPose = _nextSimPoseData.vrPoses[vr::k_unTrackedDeviceIndex_Hmd].mDeviceToAbsoluteTracking; + _lastGoodHMDPose = nextSimPoseData.vrPoses[vr::k_unTrackedDeviceIndex_Hmd].mDeviceToAbsoluteTracking; } vr::TrackedDeviceIndex_t handIndices[2] { vr::k_unTrackedDeviceIndexInvalid, vr::k_unTrackedDeviceIndexInvalid }; @@ -387,7 +402,7 @@ bool OpenVrDisplayPlugin::beginFrameRender(uint32_t frameIndex) { auto trackedCount = _system->GetSortedTrackedDeviceIndicesOfClass(vr::TrackedDeviceClass_Controller, controllerIndices, 2); // Find the left and right hand controllers, if they exist for (uint32_t i = 0; i < std::min(trackedCount, 2); ++i) { - if (_nextSimPoseData.vrPoses[i].bPoseIsValid) { + if (nextSimPoseData.vrPoses[i].bPoseIsValid) { auto role = _system->GetControllerRoleForTrackedDeviceIndex(controllerIndices[i]); if (vr::TrackedControllerRole_LeftHand == role) { handIndices[0] = controllerIndices[i]; @@ -398,8 +413,7 @@ bool OpenVrDisplayPlugin::beginFrameRender(uint32_t frameIndex) { } } - _currentRenderFrameInfo.renderPose = _nextSimPoseData.poses[vr::k_unTrackedDeviceIndex_Hmd]; - + _currentRenderFrameInfo.renderPose = nextSimPoseData.poses[vr::k_unTrackedDeviceIndex_Hmd]; bool keyboardVisible = isOpenVrKeyboardShown(); std::array handPoses; @@ -409,9 +423,9 @@ bool OpenVrDisplayPlugin::beginFrameRender(uint32_t frameIndex) { continue; } auto deviceIndex = handIndices[i]; - const mat4& mat = _nextSimPoseData.poses[deviceIndex]; - const vec3& linearVelocity = _nextSimPoseData.linearVelocities[deviceIndex]; - const vec3& angularVelocity = _nextSimPoseData.angularVelocities[deviceIndex]; + const mat4& mat = nextSimPoseData.poses[deviceIndex]; + const vec3& linearVelocity = nextSimPoseData.linearVelocities[deviceIndex]; + const vec3& angularVelocity = nextSimPoseData.angularVelocities[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); diff --git a/plugins/openvr/src/OpenVrHelpers.h b/plugins/openvr/src/OpenVrHelpers.h index 368b14cb1a..4279e6a6ac 100644 --- a/plugins/openvr/src/OpenVrHelpers.h +++ b/plugins/openvr/src/OpenVrHelpers.h @@ -66,8 +66,15 @@ struct PoseData { vec3 linearVelocities[vr::k_unMaxTrackedDeviceCount]; vec3 angularVelocities[vr::k_unMaxTrackedDeviceCount]; + PoseData() { + memset(vrPoses, 0, sizeof(vr::TrackedDevicePose_t) * vr::k_unMaxTrackedDeviceCount); + } + void update(const glm::mat4& resetMat) { for (int i = 0; i < vr::k_unMaxTrackedDeviceCount; i++) { + if (!vrPoses[i].bPoseIsValid) { + continue; + } poses[i] = resetMat * toGlm(vrPoses[i].mDeviceToAbsoluteTracking); linearVelocities[i] = transformVectorFast(resetMat, toGlm(vrPoses[i].vVelocity)); angularVelocities[i] = transformVectorFast(resetMat, toGlm(vrPoses[i].vAngularVelocity));