From 2ad85c158aafaa3477ed75edd6bfa53d53edbac5 Mon Sep 17 00:00:00 2001
From: Seth Alves <seth.alves@gmail.com>
Date: Tue, 30 May 2017 17:11:56 -0700
Subject: [PATCH] more work on getting head routed through input system

---
 .../oculus/src/OculusControllerManager.cpp    | 37 ++++++++++++++++---
 plugins/oculus/src/OculusControllerManager.h  |  9 ++++-
 2 files changed, 38 insertions(+), 8 deletions(-)

diff --git a/plugins/oculus/src/OculusControllerManager.cpp b/plugins/oculus/src/OculusControllerManager.cpp
index 6445c3c891..6042968ffc 100644
--- a/plugins/oculus/src/OculusControllerManager.cpp
+++ b/plugins/oculus/src/OculusControllerManager.cpp
@@ -230,14 +230,14 @@ void OculusControllerManager::TouchDevice::update(float deltaTime, const control
             _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;
@@ -245,6 +245,10 @@ void OculusControllerManager::TouchDevice::update(float deltaTime, const control
         }
         handleRotationForUntrackedHand(inputCalibrationData, hand, tracking.HandPoses[hand]);
     });
+
+    _poseStateMap.erase(controller::HEAD);
+    handleHeadPose(deltaTime, inputCalibrationData, tracking.HeadPose);
+
     using namespace controller;
     // Axes
     const auto& inputState = _parent._inputState;
@@ -269,7 +273,7 @@ void OculusControllerManager::TouchDevice::update(float deltaTime, const control
         if (inputState.Touches & pair.first) {
             _buttonPressedMap.insert(pair.second);
         }
-    } 
+    }
 
     // Haptics
     {
@@ -292,16 +296,36 @@ void OculusControllerManager::TouchDevice::focusOutEvent() {
     _buttonPressedMap.clear();
 };
 
-void OculusControllerManager::TouchDevice::handlePose(float deltaTime, 
-    const controller::InputCalibrationData& inputCalibrationData, ovrHandType hand, 
-    const ovrPoseStatef& handPose) {
+void OculusControllerManager::TouchDevice::handlePose(float deltaTime,
+                                                      const controller::InputCalibrationData& inputCalibrationData,
+                                                      ovrHandType hand, const ovrPoseStatef& handPose) {
     auto poseId = hand == ovrHand_Left ? controller::LEFT_HAND : controller::RIGHT_HAND;
     auto& pose = _poseStateMap[poseId];
     pose = ovrControllerPoseToHandPose(hand, handPose);
     // transform into avatar frame
     glm::mat4 controllerToAvatar = glm::inverse(inputCalibrationData.avatarMat) * inputCalibrationData.sensorToWorldMat;
     pose = pose.transform(controllerToAvatar);
+}
 
+void OculusControllerManager::TouchDevice::handleHeadPose(float deltaTime,
+                                                          const controller::InputCalibrationData& inputCalibrationData,
+                                                          const ovrPoseStatef& headPose) {
+    auto poseId = controller::HEAD;
+    auto& pose = _poseStateMap[poseId];
+
+    pose.translation = toGlm(headPose.ThePose.Position);
+    pose.rotation = toGlm(headPose.ThePose.Orientation);
+    pose.angularVelocity = toGlm(headPose.AngularVelocity);
+    pose.velocity = toGlm(headPose.LinearVelocity);
+    pose.valid = true;
+
+    qDebug() << "handleHeadPose" << pose.translation.x << pose.translation.y << pose.translation.z;
+
+    // transform into avatar frame
+    glm::mat4 controllerToAvatar = glm::inverse(inputCalibrationData.avatarMat) * inputCalibrationData.sensorToWorldMat;
+    pose = pose.transform(controllerToAvatar);
+
+    qDebug() << "handleHeadPose after" << pose.translation.x << pose.translation.y << pose.translation.z;
 }
 
 void OculusControllerManager::TouchDevice::handleRotationForUntrackedHand(const controller::InputCalibrationData& inputCalibrationData,
@@ -382,6 +406,7 @@ controller::Input::NamedVector OculusControllerManager::TouchDevice::getAvailabl
 
         makePair(LEFT_HAND, "LeftHand"),
         makePair(RIGHT_HAND, "RightHand"),
+        makePair(HEAD, "Head"),
 
         makePair(LEFT_PRIMARY_THUMB_TOUCH, "LeftPrimaryThumbTouch"),
         makePair(LEFT_SECONDARY_THUMB_TOUCH, "LeftSecondaryThumbTouch"),
diff --git a/plugins/oculus/src/OculusControllerManager.h b/plugins/oculus/src/OculusControllerManager.h
index 77b40d3764..61f2241f4d 100644
--- a/plugins/oculus/src/OculusControllerManager.h
+++ b/plugins/oculus/src/OculusControllerManager.h
@@ -76,8 +76,13 @@ 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);
+        void handlePose(float deltaTime, const controller::InputCalibrationData& inputCalibrationData,
+                        ovrHandType hand, const ovrPoseStatef& handPose);
+        void handleRotationForUntrackedHand(const controller::InputCalibrationData& inputCalibrationData,
+                                            ovrHandType hand, const ovrPoseStatef& handPose);
+        void handleHeadPose(float deltaTime, const controller::InputCalibrationData& inputCalibrationData,
+                            const ovrPoseStatef& headPose);
+
         int _trackedControllers { 0 };
 
         // perform an action when the TouchDevice mutex is acquired.