From 5d457eaa3951b9b19fa9762506f85fb9c3850756 Mon Sep 17 00:00:00 2001
From: Dante Ruiz <dante@highfidelity.io>
Date: Thu, 11 May 2017 20:44:08 +0100
Subject: [PATCH] better handling of when lost tracking of pucks

---
 plugins/openvr/src/ViveControllerManager.cpp | 30 ++++++++++++++++----
 plugins/openvr/src/ViveControllerManager.h   |  2 ++
 2 files changed, 26 insertions(+), 6 deletions(-)

diff --git a/plugins/openvr/src/ViveControllerManager.cpp b/plugins/openvr/src/ViveControllerManager.cpp
index 6e5697730b..411cac3d2b 100644
--- a/plugins/openvr/src/ViveControllerManager.cpp
+++ b/plugins/openvr/src/ViveControllerManager.cpp
@@ -32,8 +32,6 @@
 
 #include <controllers/StandardControls.h>
 
-#include "OpenVrHelpers.h"
-
 extern PoseData _nextSimPoseData;
 
 vr::IVRSystem* acquireOpenVrSystem();
@@ -207,6 +205,7 @@ void ViveControllerManager::InputDevice::update(float deltaTime, const controlle
     }
 
     updateCalibratedLimbs();
+    _lastSimPoseData = _nextSimPoseData;
 }
 
 void ViveControllerManager::InputDevice::handleTrackedObject(uint32_t deviceIndex, const controller::InputCalibrationData& inputCalibrationData) {
@@ -217,11 +216,30 @@ void ViveControllerManager::InputDevice::handleTrackedObject(uint32_t deviceInde
         _nextSimPoseData.vrPoses[deviceIndex].bPoseIsValid &&
         poseIndex <= controller::TRACKED_OBJECT_15) {
 
-        // process pose
-        const mat4& mat = _nextSimPoseData.poses[deviceIndex];
-        const vec3 linearVelocity = _nextSimPoseData.linearVelocities[deviceIndex];
-        const vec3 angularVelocity = _nextSimPoseData.angularVelocities[deviceIndex];
+        mat4& mat = mat4();
+        vec3 linearVelocity = vec3();
+        vec3 angularVelocity = vec3();
+        // check if the device is tracking out of range, then process the correct pose depending on the result.
+        if (_nextSimPoseData.vrPoses[deviceIndex].eTrackingResult != vr::TrackingResult_Running_OutOfRange) { 
+            mat = _nextSimPoseData.poses[deviceIndex];
+            linearVelocity = _nextSimPoseData.linearVelocities[deviceIndex];
+            angularVelocity = _nextSimPoseData.angularVelocities[deviceIndex];
+        } else {
+            mat = _lastSimPoseData.poses[deviceIndex];
+            linearVelocity = _lastSimPoseData.linearVelocities[deviceIndex];
+            angularVelocity = _lastSimPoseData.angularVelocities[deviceIndex];
 
+            // make sure that we do not overwrite the pose in the _lastSimPose with incorrect data.
+            _nextSimPoseData.poses[deviceIndex] = _lastSimPoseData.poses[deviceIndex];
+            _nextSimPoseData.linearVelocities[deviceIndex] = _lastSimPoseData.linearVelocities[deviceIndex];
+            _nextSimPoseData.angularVelocities[deviceIndex] = _lastSimPoseData.angularVelocities[deviceIndex];
+            
+        }
+
+/*        const mat4& mat;
+        const vec3 linearVelocity;
+        const vec3 angularVelocity;*/
+            
         controller::Pose pose(extractTranslation(mat), glmExtractRotation(mat), linearVelocity, angularVelocity);
 
         // transform into avatar frame
diff --git a/plugins/openvr/src/ViveControllerManager.h b/plugins/openvr/src/ViveControllerManager.h
index 4e8b2b3a04..ca78fd0b37 100644
--- a/plugins/openvr/src/ViveControllerManager.h
+++ b/plugins/openvr/src/ViveControllerManager.h
@@ -25,6 +25,7 @@
 #include <plugins/InputPlugin.h>
 #include <RenderArgs.h>
 #include <render/Scene.h>
+#include "OpenVrHelpers.h"
 
 namespace vr {
     class IVRSystem;
@@ -108,6 +109,7 @@ private:
         std::vector<std::pair<uint32_t, controller::Pose>> _validTrackedObjects;
         std::map<uint32_t, glm::mat4> _pucksOffset;
         std::map<int, uint32_t> _jointToPuckMap;
+        PoseData _lastSimPoseData;
         // perform an action when the InputDevice mutex is acquired.
         using Locker = std::unique_lock<std::recursive_mutex>;
         template <typename F>