From f8701e6c0c2d4c9f5287f962162f342fdac7a7c8 Mon Sep 17 00:00:00 2001
From: Philip Rosedale <philiprosedale@Philips-MacBook-Pro-2.local>
Date: Fri, 10 May 2013 10:55:05 -0700
Subject: [PATCH] Re-added Head Mouse working correctly with invensense

---
 interface/src/SerialInterface.h |  2 +-
 interface/src/main.cpp          | 46 ++++++++++++---------------------
 2 files changed, 17 insertions(+), 31 deletions(-)

diff --git a/interface/src/SerialInterface.h b/interface/src/SerialInterface.h
index 8569129520..65a802d0c2 100644
--- a/interface/src/SerialInterface.h
+++ b/interface/src/SerialInterface.h
@@ -32,7 +32,7 @@
 #define HEAD_YAW_RATE 0
 #define HEAD_ROLL_RATE 2
 
-//const bool USING_INVENSENSE_MPU9150;
+extern const bool USING_INVENSENSE_MPU9150;
 
 class SerialInterface {
 public:
diff --git a/interface/src/main.cpp b/interface/src/main.cpp
index e82e3b5d7f..6c5218f722 100644
--- a/interface/src/main.cpp
+++ b/interface/src/main.cpp
@@ -382,13 +382,14 @@ void updateAvatar(float deltaTime) {
     float measuredYawRate = serialPort.getLastYawRate();
     
     //  Update gyro-based mouse (X,Y on screen)
-    const float MIN_MOUSE_RATE = 30.0;
-    const float MOUSE_SENSITIVITY = 0.1f;
+    const float MIN_MOUSE_RATE = 1.0;
+    const float HORIZONTAL_PIXELS_PER_DEGREE = 2880.f / 45.f;
+    const float VERTICAL_PIXELS_PER_DEGREE = 1800.f / 30.f;
     if (powf(measuredYawRate * measuredYawRate +
              measuredPitchRate * measuredPitchRate, 0.5) > MIN_MOUSE_RATE)
     {
-        headMouseX += measuredYawRate*MOUSE_SENSITIVITY;
-        headMouseY += measuredPitchRate*MOUSE_SENSITIVITY*(float)HEIGHT/(float)WIDTH;
+        headMouseX += measuredYawRate * HORIZONTAL_PIXELS_PER_DEGREE * deltaTime;
+        headMouseY -= measuredPitchRate * VERTICAL_PIXELS_PER_DEGREE * deltaTime;
     }
     headMouseX = max(headMouseX, 0);
     headMouseX = min(headMouseX, WIDTH);
@@ -397,32 +398,17 @@ void updateAvatar(float deltaTime) {
     
     //  Update head and body pitch and yaw based on measured gyro rates
     if (::gyroLook) {
-        // Yaw
-        const float MIN_YAW_RATE = 20.f;
-        const float YAW_MAGNIFY = 3.0;
+        // Render Yaw
+        float renderYawSpring = fabs(headMouseX - WIDTH / 2.f) / (WIDTH / 2.f);
+        const float RENDER_YAW_MULTIPLY = 4.f;
+        myAvatar.setRenderYaw((1.f - renderYawSpring * deltaTime) * myAvatar.getRenderYaw() +
+                              renderYawSpring * deltaTime * -myAvatar.getHeadYaw() * RENDER_YAW_MULTIPLY);
+        // Render Pitch
+        float renderPitchSpring = fabs(headMouseY - HEIGHT / 2.f) / (HEIGHT / 2.f);
+        const float RENDER_PITCH_MULTIPLY = 4.f;
+        myAvatar.setRenderPitch((1.f - renderPitchSpring * deltaTime) * myAvatar.getRenderPitch() +
+                                renderPitchSpring * deltaTime * -myAvatar.getHeadPitch() * RENDER_PITCH_MULTIPLY);
 
-        if (fabs(measuredYawRate) > MIN_YAW_RATE) {
-            float addToBodyYaw = (measuredYawRate > 0.f)
-                                    ? measuredYawRate - MIN_YAW_RATE : measuredYawRate + MIN_YAW_RATE;
-            
-            //  If we are rotating the body (render angle), move the head reverse amount to compensate 
-            myAvatar.addBodyYaw(-addToBodyYaw * YAW_MAGNIFY * deltaTime);
-            myAvatar.addHeadYaw(addToBodyYaw * YAW_MAGNIFY * deltaTime);
-        }
-        // Pitch   
-        const float MIN_PITCH_RATE = 20.f;
-        const float PITCH_MAGNIFY = 2.0;
-
-        if (fabs(measuredPitchRate) > MIN_PITCH_RATE) {
-            float addToBodyPitch = (measuredPitchRate > 0.f) 
-                                    ? measuredPitchRate - MIN_PITCH_RATE : measuredPitchRate + MIN_PITCH_RATE;
-            
-            myAvatar.setRenderPitch(myAvatar.getRenderPitch() + addToBodyPitch * PITCH_MAGNIFY * deltaTime);
-
-        }
-        //  Always decay the render pitch, assuming that we are never going to want to permanently look up or down
-        const float RENDER_PITCH_DECAY = 1.0;
-        myAvatar.setRenderPitch(myAvatar.getRenderPitch() * (1.f - RENDER_PITCH_DECAY * deltaTime));
     }
     
     //  Get audio loudness data from audio input device
@@ -925,7 +911,7 @@ void displayOverlay() {
 
        //noiseTest(WIDTH, HEIGHT);
     
-        if (displayHeadMouse && !::lookingInMirror && renderStatsOn) {
+    if (displayHeadMouse && !::lookingInMirror && USING_INVENSENSE_MPU9150) {
             //  Display small target box at center or head mouse target that can also be used to measure LOD
             glColor3f(1.0, 1.0, 1.0);
             glDisable(GL_LINE_SMOOTH);