From e2ec8245b0752b8b52cb2b05b8eb4f39ca072e09 Mon Sep 17 00:00:00 2001
From: Andrzej Kapolka <drzej.k@gmail.com>
Date: Thu, 30 May 2013 14:56:12 -0700
Subject: [PATCH] Rather than adding Euler angles, compose quaternions for
 gyros.

---
 interface/src/Application.cpp |  2 +-
 interface/src/Avatar.cpp      | 10 +++++-----
 interface/src/Avatar.h        |  2 +-
 interface/src/Head.cpp        |  7 +++++++
 interface/src/Head.h          |  1 +
 5 files changed, 15 insertions(+), 7 deletions(-)

diff --git a/interface/src/Application.cpp b/interface/src/Application.cpp
index 1e9e119777..262bf6625a 100644
--- a/interface/src/Application.cpp
+++ b/interface/src/Application.cpp
@@ -1274,7 +1274,7 @@ void Application::init() {
 
 void Application::updateAvatar(float deltaTime) {
     // Update my avatar's head position from gyros
-    _myAvatar.updateHeadFromGyros(deltaTime, &_serialPort, &_gravity);
+    _myAvatar.updateHeadFromGyros(deltaTime, &_serialPort);
 
     //  Grab latest readings from the gyros
     float measuredPitchRate = _serialPort.getLastPitchRate();
diff --git a/interface/src/Avatar.cpp b/interface/src/Avatar.cpp
index 8f6ac7c991..9e41c32273 100644
--- a/interface/src/Avatar.cpp
+++ b/interface/src/Avatar.cpp
@@ -124,7 +124,7 @@ void Avatar::reset() {
 }
 
 //  Update avatar head rotation with sensor data
-void Avatar::updateHeadFromGyros(float deltaTime, SerialInterface* serialInterface, glm::vec3* gravity) {
+void Avatar::updateHeadFromGyros(float deltaTime, SerialInterface* serialInterface) {
     const float AMPLIFY_PITCH = 2.f;
     const float AMPLIFY_YAW = 2.f;
     const float AMPLIFY_ROLL = 2.f;
@@ -134,10 +134,10 @@ void Avatar::updateHeadFromGyros(float deltaTime, SerialInterface* serialInterfa
     float measuredRollRate = serialInterface->getLastRollRate();
    
     //  Update avatar head position based on measured gyro rates
-    
-    _head.addPitch(measuredPitchRate * AMPLIFY_PITCH * deltaTime);
-    _head.addYaw  (measuredYawRate   * AMPLIFY_YAW * deltaTime);
-    _head.addRoll (measuredRollRate  * AMPLIFY_ROLL * deltaTime);
+    _head.setOrientation(_head.getOrientation() * glm::quat(glm::radians(glm::vec3(
+        measuredPitchRate * AMPLIFY_PITCH * deltaTime,
+        measuredYawRate * AMPLIFY_YAW * deltaTime,
+        measuredRollRate * AMPLIFY_ROLL * deltaTime))));
     
     //  Update head lean distance based on accelerometer data
     glm::vec3 headRotationRates(_head.getPitch(), _head.getYaw(), _head.getRoll());
diff --git a/interface/src/Avatar.h b/interface/src/Avatar.h
index db8176898a..d272514ed1 100644
--- a/interface/src/Avatar.h
+++ b/interface/src/Avatar.h
@@ -48,7 +48,7 @@ public:
     
     void reset();
     void simulate(float deltaTime, Transmitter* transmitter);
-    void updateHeadFromGyros(float frametime, SerialInterface * serialInterface, glm::vec3 * gravity);
+    void updateHeadFromGyros(float frametime, SerialInterface * serialInterface);
     void updateFromMouse(int mouseX, int mouseY, int screenWidth, int screenHeight);
     void addBodyYaw(float y) {_bodyYaw += y;};
     void render(bool lookingInMirror);
diff --git a/interface/src/Head.cpp b/interface/src/Head.cpp
index d1ee699dc8..187fcc3e03 100644
--- a/interface/src/Head.cpp
+++ b/interface/src/Head.cpp
@@ -307,6 +307,13 @@ void Head::renderMohawk(glm::vec3 cameraPosition) {
     } 
 }
 
+void Head::setOrientation(const glm::quat& orientation) {
+    glm::vec3 eulerAngles = safeEulerAngles(glm::inverse(glm::quat(glm::radians(_bodyRotation))) * orientation);
+    _pitch = eulerAngles.x;
+    _yaw = eulerAngles.y;
+    _roll = eulerAngles.z;
+}
+
 glm::quat Head::getOrientation() const {
     return glm::quat(glm::radians(_bodyRotation)) * glm::quat(glm::radians(_lookingInMirror ?
         glm::vec3(_pitch, -_yaw, -_roll) : glm::vec3(_pitch, _yaw, _roll)));
diff --git a/interface/src/Head.h b/interface/src/Head.h
index f49e127caf..36da1b082f 100644
--- a/interface/src/Head.h
+++ b/interface/src/Head.h
@@ -45,6 +45,7 @@ public:
     void setAverageLoudness(float     averageLoudness   ) { _averageLoudness    = averageLoudness;    }
     void setReturnToCenter (bool      returnHeadToCenter) { _returnHeadToCenter = returnHeadToCenter; }
     void setRenderLookatVectors(bool onOff ) { _renderLookatVectors = onOff; }
+    void setOrientation(const glm::quat& orientation);
     
     glm::quat getOrientation() const;
     glm::quat getWorldAlignedOrientation () const;