Merge pull request #8282 from hyperlogic/bug-fix/avatar-embedded-in-floor

Vive HMD fixes to prevent user from becoming embedded in the floor.

interface/src/Application.cpp

@@ -3405,6 +3405,10 @@ void Application::setOverlaysVisible(bool visible) {
     menu->setIsOptionChecked(MenuOption::Overlays, true);
 }
 
+void Application::centerUI() {
+    _overlayConductor.centerUI();
+}
+
 void Application::cycleCamera() {
     auto menu = Menu::getInstance();
     if (menu->isOptionChecked(MenuOption::FullscreenMirror)) {

interface/src/Application.h

@@ -298,6 +298,7 @@ public slots:
     void cameraMenuChanged();
     void toggleOverlays();
     void setOverlaysVisible(bool visible);
+    Q_INVOKABLE void centerUI();
 
     void resetPhysicsReadyInformation();
 

interface/src/avatar/MyAvatar.cpp

@@ -231,13 +231,46 @@ QByteArray MyAvatar::toByteArray(bool cullSmallChanges, bool sendAll) {
     return AvatarData::toByteArray(cullSmallChanges, sendAll);
 }
 
-void MyAvatar::reset(bool andRecenter, bool andReload, bool andHead) {
-
+void MyAvatar::centerBody() {
     if (QThread::currentThread() != thread()) {
-        QMetaObject::invokeMethod(this, "reset", Q_ARG(bool, andRecenter), Q_ARG(bool, andReload), Q_ARG(bool, andHead));
+        QMetaObject::invokeMethod(this, "centerBody");
         return;
     }
 
+    // derive the desired body orientation from the current hmd orientation, before the sensor reset.
+    auto newBodySensorMatrix = deriveBodyFromHMDSensor(); // Based on current cached HMD position/rotation..
+
+    // transform this body into world space
+    auto worldBodyMatrix = _sensorToWorldMatrix * newBodySensorMatrix;
+    auto worldBodyPos = extractTranslation(worldBodyMatrix);
+    auto worldBodyRot = glm::normalize(glm::quat_cast(worldBodyMatrix));
+
+    // this will become our new position.
+    setPosition(worldBodyPos);
+    setOrientation(worldBodyRot);
+
+    // reset the body in sensor space
+    _bodySensorMatrix = newBodySensorMatrix;
+
+    // rebuild the sensor to world matrix
+    updateSensorToWorldMatrix();
+}
+
+void MyAvatar::clearIKJointLimitHistory() {
+    if (QThread::currentThread() != thread()) {
+        QMetaObject::invokeMethod(this, "clearIKJointLimitHistory");
+        return;
+    }
+
+    if (_rig) {
+        _rig->clearIKJointLimitHistory();
+    }
+}
+
+void MyAvatar::reset(bool andRecenter, bool andReload, bool andHead) {
+
+    assert(QThread::currentThread() == thread());
+
     // Reset dynamic state.
     _wasPushing = _isPushing = _isBraking = false;
     _follow.deactivate();

interface/src/avatar/MyAvatar.h

@@ -96,7 +96,11 @@ public:
     AudioListenerMode getAudioListenerModeCamera() const { return FROM_CAMERA; }
     AudioListenerMode getAudioListenerModeCustom() const { return CUSTOM; }
 
-    Q_INVOKABLE void reset(bool andRecenter = false, bool andReload = true, bool andHead = true);
+    void reset(bool andRecenter = false, bool andReload = true, bool andHead = true);
+
+    Q_INVOKABLE void centerBody(); // thread-safe
+    Q_INVOKABLE void clearIKJointLimitHistory(); // thread-safe
+
     void update(float deltaTime);
     virtual void postUpdate(float deltaTime) override;
     void preDisplaySide(RenderArgs* renderArgs);

interface/src/scripting/HMDScriptingInterface.cpp

@@ -135,3 +135,7 @@ void HMDScriptingInterface::unsuppressKeyboard() {
 bool HMDScriptingInterface::isKeyboardVisible() {
     return qApp->getActiveDisplayPlugin()->isKeyboardVisible();
 }
+
+void HMDScriptingInterface::centerUI() {
+    QMetaObject::invokeMethod(qApp, "centerUI", Qt::QueuedConnection);
+}

interface/src/scripting/HMDScriptingInterface.h

@@ -55,6 +55,9 @@ public:
     /// Query the display plugin to determine the current VR keyboard visibility
     Q_INVOKABLE bool isKeyboardVisible();
 
+    // rotate the overlay UI sphere so that it is centered about the the current HMD position and orientation
+    Q_INVOKABLE void centerUI();
+
 public:
     HMDScriptingInterface();
     static QScriptValue getHUDLookAtPosition2D(QScriptContext* context, QScriptEngine* engine);

libraries/animation/src/AnimInverseKinematics.cpp

@@ -488,6 +488,12 @@ const AnimPoseVec& AnimInverseKinematics::overlay(const AnimVariantMap& animVars
     return _relativePoses;
 }
 
+void AnimInverseKinematics::clearIKJointLimitHistory() {
+    for (auto& pair : _constraints) {
+        pair.second->clearHistory();
+    }
+}
+
 RotationConstraint* AnimInverseKinematics::getConstraint(int index) {
     RotationConstraint* constraint = nullptr;
     std::map<int, RotationConstraint*>::iterator constraintItr = _constraints.find(index);

libraries/animation/src/AnimInverseKinematics.h

@@ -37,6 +37,8 @@ public:
     virtual const AnimPoseVec& evaluate(const AnimVariantMap& animVars, float dt, AnimNode::Triggers& triggersOut) override;
     virtual const AnimPoseVec& overlay(const AnimVariantMap& animVars, float dt, Triggers& triggersOut, const AnimPoseVec& underPoses) override;
 
+    void clearIKJointLimitHistory();
+
 protected:
     void computeTargets(const AnimVariantMap& animVars, std::vector<IKTarget>& targets, const AnimPoseVec& underPoses);
     void solveWithCyclicCoordinateDescent(const std::vector<IKTarget>& targets);

libraries/animation/src/Rig.cpp

@@ -295,6 +295,19 @@ void Rig::clearJointAnimationPriority(int index) {
     }
 }
 
+void Rig::clearIKJointLimitHistory() {
+    if (_animNode) {
+        _animNode->traverse([&](AnimNode::Pointer node) {
+            // only report clip nodes as valid roles.
+            auto ikNode = std::dynamic_pointer_cast<AnimInverseKinematics>(node);
+            if (ikNode) {
+                ikNode->clearIKJointLimitHistory();
+            }
+            return true;
+        });
+    }
+}
+
 void Rig::setJointTranslation(int index, bool valid, const glm::vec3& translation, float priority) {
     if (isIndexValid(index)) {
         if (valid) {

libraries/animation/src/Rig.h

@@ -103,6 +103,8 @@ public:
     void clearJointStates();
     void clearJointAnimationPriority(int index);
 
+    void clearIKJointLimitHistory();
+
     // geometry space
     void setJointState(int index, bool valid, const glm::quat& rotation, const glm::vec3& translation, float priority);
 

libraries/animation/src/RotationConstraint.h

@@ -35,6 +35,9 @@ public:
     /// \brief clear previous adjustment and adjust constraint limits to allow rotation
     virtual void dynamicallyAdjustLimits(const glm::quat& rotation) {}
 
+    /// \brief reset any remembered joint limit history
+    virtual void clearHistory() {};
+
 protected:
     glm::quat _referenceRotation = glm::quat();
 };

libraries/animation/src/SwingTwistConstraint.h

@@ -97,8 +97,7 @@ public:
     /// \return reference to SwingLimitFunction instance for unit-testing
     const SwingLimitFunction& getSwingLimitFunction() const { return _swingLimitFunction; }
 
-    /// \brief exposed for unit testing
-    void clearHistory();
+    void clearHistory() override;
 
 private:
     float handleTwistBoundaryConditions(float twistAngle) const;

plugins/openvr/src/OpenVrDisplayPlugin.cpp

@@ -49,6 +49,19 @@ bool OpenVrDisplayPlugin::isSupported() const {
 void OpenVrDisplayPlugin::init() {
     Plugin::init();
 
+    _lastGoodHMDPose.m[0][0] = 1.0f;
+    _lastGoodHMDPose.m[0][1] = 0.0f;
+    _lastGoodHMDPose.m[0][2] = 0.0f;
+    _lastGoodHMDPose.m[0][3] = 0.0f;
+    _lastGoodHMDPose.m[1][0] = 0.0f;
+    _lastGoodHMDPose.m[1][1] = 1.0f;
+    _lastGoodHMDPose.m[1][2] = 0.0f;
+    _lastGoodHMDPose.m[1][3] = 1.8f;
+    _lastGoodHMDPose.m[2][0] = 0.0f;
+    _lastGoodHMDPose.m[2][1] = 0.0f;
+    _lastGoodHMDPose.m[2][2] = 1.0f;
+    _lastGoodHMDPose.m[2][3] = 0.0f;
+
     emit deviceConnected(getName());
 }
 
@@ -139,6 +152,12 @@ void OpenVrDisplayPlugin::resetSensors() {
     });
 }
 
+static bool isBadPose(vr::HmdMatrix34_t* mat) {
+    if (mat->m[1][3] < -0.2f) {
+        return true;
+    }
+    return false;
+}
 
 bool OpenVrDisplayPlugin::beginFrameRender(uint32_t frameIndex) {
     handleOpenVrEvents();
@@ -158,6 +177,16 @@ bool OpenVrDisplayPlugin::beginFrameRender(uint32_t frameIndex) {
 
     _system->GetDeviceToAbsoluteTrackingPose(vr::TrackingUniverseStanding, _currentRenderFrameInfo.predictedDisplayTime, _trackedDevicePose, vr::k_unMaxTrackedDeviceCount);
 
+    // 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(&_trackedDevicePose[vr::k_unTrackedDeviceIndex_Hmd].mDeviceToAbsoluteTracking)) {
+        qDebug() << "WARNING: ignoring bad hmd pose from openvr";
+
+        // use the last known good HMD pose
+        _trackedDevicePose[vr::k_unTrackedDeviceIndex_Hmd].mDeviceToAbsoluteTracking = _lastGoodHMDPose;
+    } else {
+        _lastGoodHMDPose = _trackedDevicePose[vr::k_unTrackedDeviceIndex_Hmd].mDeviceToAbsoluteTracking;
+    }
 
     vr::TrackedDeviceIndex_t handIndices[2] { vr::k_unTrackedDeviceIndexInvalid, vr::k_unTrackedDeviceIndexInvalid };
     {

plugins/openvr/src/OpenVrDisplayPlugin.h

@@ -51,4 +51,6 @@ private:
     std::atomic<vr::EDeviceActivityLevel> _hmdActivityLevel { vr::k_EDeviceActivityLevel_Unknown };
     std::atomic<uint32_t> _keyboardSupressionCount{ 0 };
     static const QString NAME;
+
+    vr::HmdMatrix34_t _lastGoodHMDPose;
 };

scripts/system/away.js

@@ -204,7 +204,16 @@ function goActive() {
     }
     MyAvatar.setEnableMeshVisible(true); // IWBNI we respected Developer->Avatar->Draw Mesh setting.
     stopAwayAnimation();
-    MyAvatar.reset(true);
+
+    // update the UI sphere to be centered about the current HMD orientation.
+    HMD.centerUI();
+
+    // forget about any IK joint limits
+    MyAvatar.clearIKJointLimitHistory();
+
+    // update the avatar hips to point in the same direction as the HMD orientation.
+    MyAvatar.centerBody();
+
     hideOverlay();
 
     // restore overlays state to what it was when we went "away"