From 65168e6aec597ceb932761de765c1da088e7161a Mon Sep 17 00:00:00 2001
From: Stephen Birarda <commit@birarda.com>
Date: Fri, 24 May 2013 15:23:25 -0700
Subject: [PATCH] add a yaw offset to the oculus so it can pull body

---
 interface/src/Avatar.cpp         | 18 +++++++++++++++++-
 interface/src/OculusManager.cpp  | 10 +++++++++-
 interface/src/OculusManager.h    |  3 +++
 libraries/avatars/src/HeadData.h |  4 ++--
 4 files changed, 31 insertions(+), 4 deletions(-)

diff --git a/interface/src/Avatar.cpp b/interface/src/Avatar.cpp
index 21dea8886f..09900d1404 100644
--- a/interface/src/Avatar.cpp
+++ b/interface/src/Avatar.cpp
@@ -300,7 +300,6 @@ void Avatar::simulate(float deltaTime, Transmitter* transmitter) {
                            deltaTime *
                            _orientation.getUp();
             }
-            
         }
 	}
         
@@ -381,11 +380,28 @@ void Avatar::simulate(float deltaTime, Transmitter* transmitter) {
     const float ACCELERATION_PITCH_DECAY = 0.4f;
     const float ACCELERATION_YAW_DECAY = 0.4f;
     
+    const float OCULUS_ACCELERATION_PULL_THRESHOLD = 1.0f;
+    const int OCULUS_YAW_OFFSET_THRESHOLD = 10;
+    
     // Decay HeadPitch as a function of acceleration, so that you look straight ahead when
     // you start moving, but don't do this with an HMD like the Oculus. 
     if (!OculusManager::isConnected()) {
         _head.setPitch(_head.getPitch() * (1.f - acceleration * ACCELERATION_PITCH_DECAY * deltaTime));
         _head.setYaw(_head.getYaw() * (1.f - acceleration * ACCELERATION_YAW_DECAY * deltaTime));
+    } else if (fabsf(acceleration) > OCULUS_ACCELERATION_PULL_THRESHOLD
+               && fabs(_head.getYaw()) > OCULUS_YAW_OFFSET_THRESHOLD) {
+        // if we're wearing the oculus
+        // and this acceleration is above the pull threshold
+        // and the head yaw if off the body by more than OCULUS_YAW_OFFSET_THRESHOLD
+        
+        // match the body yaw to the oculus yaw
+        _bodyYaw = getAbsoluteHeadYaw();
+        
+        // set the head yaw to zero for this draw
+        _head.setYaw(0);
+        
+        // correct the oculus yaw offset
+        OculusManager::updateYawOffset();
     }
 
     //apply the head lean values to the springy position...
diff --git a/interface/src/OculusManager.cpp b/interface/src/OculusManager.cpp
index e0272ac8ef..168616ec83 100644
--- a/interface/src/OculusManager.cpp
+++ b/interface/src/OculusManager.cpp
@@ -16,6 +16,7 @@ Ptr<DeviceManager> OculusManager::_deviceManager;
 Ptr<HMDDevice> OculusManager::_hmdDevice;
 Ptr<SensorDevice> OculusManager::_sensorDevice;
 SensorFusion OculusManager::_sensorFusion;
+float OculusManager::_yawOffset = 0;
 #endif
 
 void OculusManager::connect() {
@@ -36,12 +37,19 @@ void OculusManager::connect() {
 #endif
 }
 
+void OculusManager::updateYawOffset() {
+    float yaw, pitch, roll;
+   _sensorFusion.GetOrientation().GetEulerAngles<Axis_Y, Axis_X, Axis_Z, Rotate_CCW, Handed_R>(&yaw, &pitch, &roll);
+    _yawOffset = yaw;
+}
+
 void OculusManager::getEulerAngles(float& yaw, float& pitch, float& roll) {
 #ifdef __APPLE__
     _sensorFusion.GetOrientation().GetEulerAngles<Axis_Y, Axis_X, Axis_Z, Rotate_CCW, Handed_R>(&yaw, &pitch, &roll);
     
     // convert each angle to degrees
-    yaw = glm::degrees(yaw);
+    // remove the yaw offset from the returned yaw
+    yaw = glm::degrees(yaw - _yawOffset);
     pitch = glm::degrees(pitch);
     roll = glm::degrees(roll);
 #endif
diff --git a/interface/src/OculusManager.h b/interface/src/OculusManager.h
index 6570a2a4cc..7bfea8b419 100644
--- a/interface/src/OculusManager.h
+++ b/interface/src/OculusManager.h
@@ -21,12 +21,15 @@ public:
     static bool isConnected() { return _isConnected; }
     
     static void getEulerAngles(float& yaw, float& pitch, float& roll);
+    
+    static void updateYawOffset();
 private:    
     static bool _isConnected;
     static Ptr<DeviceManager> _deviceManager;
     static Ptr<HMDDevice> _hmdDevice;
     static Ptr<SensorDevice> _sensorDevice;
     static SensorFusion _sensorFusion;
+    static float _yawOffset;
 };
 
 #endif /* defined(__hifi__OculusManager__) */
diff --git a/libraries/avatars/src/HeadData.h b/libraries/avatars/src/HeadData.h
index 1f0a8181b5..0eb135b899 100644
--- a/libraries/avatars/src/HeadData.h
+++ b/libraries/avatars/src/HeadData.h
@@ -13,8 +13,8 @@
 
 #include <glm/glm.hpp>
 
-const float MIN_HEAD_YAW = -85;
-const float MAX_HEAD_YAW = 85;
+const float MIN_HEAD_YAW = -110;
+const float MAX_HEAD_YAW = 110;
 const float MIN_HEAD_PITCH = -60;
 const float MAX_HEAD_PITCH = 60;
 const float MIN_HEAD_ROLL = -50;