oculus lost tracking, delay hand to side

plugins/oculus/src/OculusControllerManager.cpp

@@ -33,6 +33,8 @@ static const char* MENU_PATH = "Avatar" ">" "Oculus Touch Controllers";
 
 const char* OculusControllerManager::NAME = "Oculus";
 
+const quint64 LOST_TRACKING_DELAY = 3000000;
+
 bool OculusControllerManager::isSupported() const {
     return oculusAvailable();
 }
@@ -207,9 +209,7 @@ void OculusControllerManager::RemoteDevice::focusOutEvent() {
 }
 
 void OculusControllerManager::TouchDevice::update(float deltaTime, const controller::InputCalibrationData& inputCalibrationData) {
-    _poseStateMap.clear();
     _buttonPressedMap.clear();
-
     ovrSessionStatus status;
     if (!OVR_SUCCESS(ovr_GetSessionStatus(_parent._session, &status)) || (ovrFalse == status.HmdMounted)) {
         // if the HMD isn't on someone's head, don't take input from the controllers
@@ -217,15 +217,33 @@ void OculusControllerManager::TouchDevice::update(float deltaTime, const control
     }
 
     int numTrackedControllers = 0;
+    quint64 currentTime = usecTimestampNow();
     static const auto REQUIRED_HAND_STATUS = ovrStatus_OrientationTracked | ovrStatus_PositionTracked;
     auto tracking = ovr_GetTrackingState(_parent._session, 0, false);
     ovr_for_each_hand([&](ovrHandType hand) {
         ++numTrackedControllers;
+        int controller = (hand == ovrHand_Left ? controller::LEFT_HAND : controller::RIGHT_HAND);
         if (REQUIRED_HAND_STATUS == (tracking.HandStatusFlags[hand] & REQUIRED_HAND_STATUS)) {
+            _poseStateMap.erase(controller);
             handlePose(deltaTime, inputCalibrationData, hand, tracking.HandPoses[hand]);
-        } else {
-            _poseStateMap[hand == ovrHand_Left ? controller::LEFT_HAND : controller::RIGHT_HAND].valid = false;
+            _lostTracking[controller] = false;
+            _lastControllerPose[controller] = tracking.HandPoses[hand];
+            return;
         }
+        
+        if (_lostTracking[controller]) {
+            if (currentTime > _regainTrackingDeadline[controller]) {
+                _poseStateMap.erase(controller);
+                _poseStateMap[controller].valid = false;
+                return;
+            }
+            
+        } else {
+            quint64 deadlineToRegainTracking = currentTime + LOST_TRACKING_DELAY;
+            _regainTrackingDeadline[controller] = deadlineToRegainTracking;
+            _lostTracking[controller] = true;
+        }
+        handleRotationForUntrackedHand(inputCalibrationData, hand, tracking.HandPoses[hand]);
     });
     using namespace controller;
     // Axes
@@ -251,7 +269,7 @@ void OculusControllerManager::TouchDevice::update(float deltaTime, const control
         if (inputState.Touches & pair.first) {
             _buttonPressedMap.insert(pair.second);
         }
-    }
+    } 
 
     // Haptics
     {
@@ -286,6 +304,16 @@ void OculusControllerManager::TouchDevice::handlePose(float deltaTime,
 
 }
 
+void OculusControllerManager::TouchDevice::handleRotationForUntrackedHand(const controller::InputCalibrationData& inputCalibrationData,
+                                                                          ovrHandType hand, const ovrPoseStatef& handPose) {
+    auto poseId = (hand == ovrHand_Left ? controller::LEFT_HAND : controller::RIGHT_HAND);
+    auto& pose = _poseStateMap[poseId];
+    auto lastHandPose = _lastControllerPose[poseId];
+    pose = ovrControllerRotationToHandRotation(hand, handPose, lastHandPose);
+    glm::mat4 controllerToAvatar = glm::inverse(inputCalibrationData.avatarMat) * inputCalibrationData.sensorToWorldMat;
+    pose = pose.transform(controllerToAvatar);
+}
+
 bool OculusControllerManager::TouchDevice::triggerHapticPulse(float strength, float duration, controller::Hand hand) {
     Locker locker(_lock);
     bool toReturn = true;

plugins/oculus/src/OculusControllerManager.h

@@ -11,6 +11,7 @@
 
 #include <QObject>
 #include <unordered_set>
+#include <map>
 
 #include <GLMHelpers.h>
 
@@ -75,6 +76,7 @@ private:
     private:
         void stopHapticPulse(bool leftHand);
         void handlePose(float deltaTime, const controller::InputCalibrationData& inputCalibrationData, ovrHandType hand, const ovrPoseStatef& handPose);
+        void handleRotationForUntrackedHand(const controller::InputCalibrationData& inputCalibrationData, ovrHandType hand, const ovrPoseStatef& handPose);
         int _trackedControllers { 0 };
 
         // perform an action when the TouchDevice mutex is acquired.
@@ -87,6 +89,9 @@ private:
         float _rightHapticDuration { 0.0f };
         float _rightHapticStrength { 0.0f };
         mutable std::recursive_mutex _lock;
+        std::map<int, bool> _lostTracking;
+        std::map<int, quint64> _regainTrackingDeadline;
+        std::map<int, ovrPoseStatef> _lastControllerPose;
 
         friend class OculusControllerManager;
     };

plugins/oculus/src/OculusHelpers.cpp

@@ -267,3 +267,37 @@ controller::Pose ovrControllerPoseToHandPose(
     pose.valid = true;
     return pose;
 }
+
+controller::Pose ovrControllerRotationToHandRotation(ovrHandType hand, const ovrPoseStatef& handPose,
+                                                    const ovrPoseStatef& lastHandPose) {
+    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 leftRotationOffset = glm::inverse(leftQuarterZ) * touchToHand;
+    static const glm::quat rightRotationOffset = glm::inverse(rightQuarterZ) * 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 * 1.5f);
+    static const glm::vec3 leftTranslationOffset = glm::vec3(-1.0f, 1.0f, 1.0f) * CONTROLLER_OFFSET;
+    static const glm::vec3 rightTranslationOffset = CONTROLLER_OFFSET;
+
+    auto translationOffset = (hand == ovrHand_Left ? leftTranslationOffset : rightTranslationOffset);
+    auto rotationOffset = (hand == ovrHand_Left ? leftRotationOffset : rightRotationOffset);
+
+    glm::quat rotation = toGlm(handPose.ThePose.Orientation);
+
+    controller::Pose pose;
+    pose.translation = toGlm(lastHandPose.ThePose.Position);
+    pose.translation += rotation * translationOffset;
+    pose.rotation = rotation * rotationOffset;
+    pose.angularVelocity = toGlm(lastHandPose.AngularVelocity);
+    pose.velocity = toGlm(lastHandPose.LinearVelocity);
+    pose.valid = true;
+    return pose;
+}

plugins/oculus/src/OculusHelpers.h

@@ -118,3 +118,6 @@ inline ovrPosef ovrPoseFromGlm(const glm::mat4 & m) {
 controller::Pose ovrControllerPoseToHandPose(
     ovrHandType hand,
     const ovrPoseStatef& handPose);
+
+controller::Pose ovrControllerRotationToHandRotation(ovrHandType hand,
+                                         const ovrPoseStatef& handPose, const ovrPoseStatef& lastHandPose);