From 69ec10958537760ca0f22e5ac1a83fb1f80a80e0 Mon Sep 17 00:00:00 2001
From: Philip Rosedale <philip@coffeeandpower.com>
Date: Fri, 31 May 2013 15:28:15 -0700
Subject: [PATCH 1/9] Refactor/Cleanup serial interface to use vectors, correct
 rotation axes.  Added translation estimation for the head.

---
 interface/src/SerialInterface.cpp | 111 +++++++++++++++++-------------
 interface/src/SerialInterface.h   |  56 +++++++--------
 2 files changed, 86 insertions(+), 81 deletions(-)

diff --git a/interface/src/SerialInterface.cpp b/interface/src/SerialInterface.cpp
index b730d2a907..2fa078baf4 100644
--- a/interface/src/SerialInterface.cpp
+++ b/interface/src/SerialInterface.cpp
@@ -111,52 +111,61 @@ void SerialInterface::renderLevels(int width, int height) {
         const int LEVEL_CORNER_Y = 200;
         
         // Draw the numeric degree/sec values from the gyros
-        sprintf(val, "Yaw   %4.1f", getLastYawRate());
+        sprintf(val, "Yaw    %4.1f", _estimatedRotation.y);
         drawtext(LEVEL_CORNER_X, LEVEL_CORNER_Y, 0.10, 0, 1.0, 1, val, 0, 1, 0);
-        sprintf(val, "Pitch %4.1f", getLastPitchRate());
+        sprintf(val, "Pitch %4.1f", _estimatedRotation.x);
         drawtext(LEVEL_CORNER_X, LEVEL_CORNER_Y + 15, 0.10, 0, 1.0, 1, val, 0, 1, 0);
-        sprintf(val, "Roll  %4.1f", getLastRollRate());
+        sprintf(val, "Roll  %4.1f", _estimatedRotation.z);
         drawtext(LEVEL_CORNER_X, LEVEL_CORNER_Y + 30, 0.10, 0, 1.0, 1, val, 0, 1, 0);
-        sprintf(val, "X     %4.3f", _lastAccelX);
+        sprintf(val, "X     %4.3f", _lastAcceleration.x - _gravity.x);
         drawtext(LEVEL_CORNER_X, LEVEL_CORNER_Y + 45, 0.10, 0, 1.0, 1, val, 0, 1, 0);
-        sprintf(val, "Y     %4.3f", _lastAccelY);
+        sprintf(val, "Y     %4.3f", _lastAcceleration.y - _gravity.y);
         drawtext(LEVEL_CORNER_X, LEVEL_CORNER_Y + 60, 0.10, 0, 1.0, 1, val, 0, 1, 0);
-        sprintf(val, "Z     %4.3f", _lastAccelZ);
+        sprintf(val, "Z     %4.3f", _lastAcceleration.z - _gravity.z);
         drawtext(LEVEL_CORNER_X, LEVEL_CORNER_Y + 75, 0.10, 0, 1.0, 1, val, 0, 1, 0);
         
         //  Draw the levels as horizontal lines        
         const int LEVEL_CENTER = 150;
-        const float ACCEL_VIEW_SCALING = 50.f;
+        const float ACCEL_VIEW_SCALING = 10.f;
+        const float POSITION_SCALING = 400.f;
+
         glLineWidth(2.0);
-        glColor4f(1, 1, 1, 1);
         glBegin(GL_LINES);
-        // Gyro rates
+        // Rotation rates
+        glColor4f(1, 1, 1, 1);
         glVertex2f(LEVEL_CORNER_X + LEVEL_CENTER, LEVEL_CORNER_Y - 3);
         glVertex2f(LEVEL_CORNER_X + LEVEL_CENTER + getLastYawRate(), LEVEL_CORNER_Y - 3);
         glVertex2f(LEVEL_CORNER_X + LEVEL_CENTER, LEVEL_CORNER_Y + 12);
         glVertex2f(LEVEL_CORNER_X + LEVEL_CENTER + getLastPitchRate(), LEVEL_CORNER_Y + 12);
         glVertex2f(LEVEL_CORNER_X + LEVEL_CENTER, LEVEL_CORNER_Y + 27);
         glVertex2f(LEVEL_CORNER_X + LEVEL_CENTER + getLastRollRate(), LEVEL_CORNER_Y + 27);
-        // Gyro Estimated Rotation
+        // Estimated Rotation
         glColor4f(0, 1, 1, 1);
         glVertex2f(LEVEL_CORNER_X + LEVEL_CENTER, LEVEL_CORNER_Y - 1);
         glVertex2f(LEVEL_CORNER_X + LEVEL_CENTER + _estimatedRotation.y, LEVEL_CORNER_Y - 1);
         glVertex2f(LEVEL_CORNER_X + LEVEL_CENTER, LEVEL_CORNER_Y + 14);
-        glVertex2f(LEVEL_CORNER_X + LEVEL_CENTER + _estimatedRotation.z, LEVEL_CORNER_Y + 14);
+        glVertex2f(LEVEL_CORNER_X + LEVEL_CENTER + _estimatedRotation.x, LEVEL_CORNER_Y + 14);
         glVertex2f(LEVEL_CORNER_X + LEVEL_CENTER, LEVEL_CORNER_Y + 29);
-        glVertex2f(LEVEL_CORNER_X + LEVEL_CENTER + _estimatedRotation.x, LEVEL_CORNER_Y + 29);
+        glVertex2f(LEVEL_CORNER_X + LEVEL_CENTER + _estimatedRotation.z, LEVEL_CORNER_Y + 29);
 
-        
-        // Acceleration
+        // Acceleration rates
+        glColor4f(1, 1, 1, 1);
         glVertex2f(LEVEL_CORNER_X + LEVEL_CENTER, LEVEL_CORNER_Y + 42);
-        glVertex2f(LEVEL_CORNER_X + LEVEL_CENTER + (int)((_lastAccelX - _gravity.x)* ACCEL_VIEW_SCALING),
-                   LEVEL_CORNER_Y + 42);
+        glVertex2f(LEVEL_CORNER_X + LEVEL_CENTER + (int)((_lastAcceleration.x - _gravity.x) *ACCEL_VIEW_SCALING), LEVEL_CORNER_Y + 42);
         glVertex2f(LEVEL_CORNER_X + LEVEL_CENTER, LEVEL_CORNER_Y + 57);
-        glVertex2f(LEVEL_CORNER_X + LEVEL_CENTER + (int)((_lastAccelY - _gravity.y) * ACCEL_VIEW_SCALING),
-                   LEVEL_CORNER_Y + 57);
+        glVertex2f(LEVEL_CORNER_X + LEVEL_CENTER + (int)((_lastAcceleration.y - _gravity.y) *ACCEL_VIEW_SCALING), LEVEL_CORNER_Y + 57);
         glVertex2f(LEVEL_CORNER_X + LEVEL_CENTER, LEVEL_CORNER_Y + 72);
-        glVertex2f(LEVEL_CORNER_X + LEVEL_CENTER + (int)((_lastAccelZ - _gravity.z) * ACCEL_VIEW_SCALING),
-                   LEVEL_CORNER_Y + 72);
+        glVertex2f(LEVEL_CORNER_X + LEVEL_CENTER + (int)((_lastAcceleration.z - _gravity.z) * ACCEL_VIEW_SCALING), LEVEL_CORNER_Y + 72);
+        
+        // Estimated Position
+        glColor4f(0, 1, 1, 1);
+        glVertex2f(LEVEL_CORNER_X + LEVEL_CENTER, LEVEL_CORNER_Y + 44);
+        glVertex2f(LEVEL_CORNER_X + LEVEL_CENTER + (int)(_estimatedPosition.x * POSITION_SCALING), LEVEL_CORNER_Y + 44);
+        glVertex2f(LEVEL_CORNER_X + LEVEL_CENTER, LEVEL_CORNER_Y + 59);
+        glVertex2f(LEVEL_CORNER_X + LEVEL_CENTER + (int)(_estimatedPosition.y * POSITION_SCALING), LEVEL_CORNER_Y + 59);
+        glVertex2f(LEVEL_CORNER_X + LEVEL_CENTER, LEVEL_CORNER_Y + 74);
+        glVertex2f(LEVEL_CORNER_X + LEVEL_CENTER + (int)(_estimatedPosition.z * POSITION_SCALING), LEVEL_CORNER_Y + 74);
+
 
         glEnd();
         //  Draw green vertical centerline
@@ -202,10 +211,8 @@ void SerialInterface::readData(float deltaTime) {
                                                                 //  From MPU-9150 register map, with setting on
                                                                 //  highest resolution = +/- 2G
         
-        _lastAccelX = ((float) accelXRate) * LSB_TO_METERS_PER_SECOND2;
-        _lastAccelY = ((float) accelYRate) * LSB_TO_METERS_PER_SECOND2;
-        _lastAccelZ = ((float) -accelZRate) * LSB_TO_METERS_PER_SECOND2;
-        
+        _lastAcceleration = glm::vec3(accelXRate, accelYRate, -accelZRate) * LSB_TO_METERS_PER_SECOND2;
+                
         int rollRate, yawRate, pitchRate;
         
         convertHexToInt(sensorBuffer + 22, rollRate);
@@ -213,38 +220,41 @@ void SerialInterface::readData(float deltaTime) {
         convertHexToInt(sensorBuffer + 30, pitchRate);
         
         //  Convert the integer rates to floats
-        const float LSB_TO_DEGREES_PER_SECOND = 1.f / 16.4f;     //  From MPU-9150 register map, 2000 deg/sec.        
-        _lastRollRate = ((float) -rollRate) * LSB_TO_DEGREES_PER_SECOND;
-        _lastYawRate = ((float) -yawRate) * LSB_TO_DEGREES_PER_SECOND;
-        _lastPitchRate = ((float) -pitchRate) * LSB_TO_DEGREES_PER_SECOND;
-        
+        const float LSB_TO_DEGREES_PER_SECOND = 1.f / 16.4f;     //  From MPU-9150 register map, 2000 deg/sec.
+        _lastRotationRates[0] = ((float) -pitchRate) * LSB_TO_DEGREES_PER_SECOND;
+        _lastRotationRates[1] = ((float) -yawRate) * LSB_TO_DEGREES_PER_SECOND;
+        _lastRotationRates[2] = ((float) -rollRate) * LSB_TO_DEGREES_PER_SECOND;
+
         //  Update raw rotation estimates
-        _estimatedRotation += deltaTime * glm::vec3(_lastRollRate - _averageGyroRates[0],
-                                                    _lastYawRate - _averageGyroRates[1],
-                                                    _lastPitchRate - _averageGyroRates[2]);
+        _estimatedRotation += deltaTime * (_lastRotationRates - _averageRotationRates);
+        
+        //  Update estimated position and velocity
+        float const DECAY_VELOCITY = 0.95f;
+        float const DECAY_POSITION = 0.95f;
+        _estimatedVelocity += deltaTime * (_lastAcceleration - _averageAcceleration);
+        _estimatedPosition += deltaTime * _estimatedVelocity;
+        _estimatedVelocity *= DECAY_VELOCITY;
+        _estimatedPosition *= DECAY_POSITION;
+        
+        //glm::vec3 baseline = glm::normalize(_gravity);
+        //glm::vec3 current = glm::normalize(_lastAcceleration);
         
         //  Accumulate a set of initial baseline readings for setting gravity
         if (totalSamples == 0) {
-            _averageGyroRates[0] = _lastRollRate;
-            _averageGyroRates[1] = _lastYawRate;
-            _averageGyroRates[2] = _lastPitchRate;
-            _gravity.x = _lastAccelX;
-            _gravity.y = _lastAccelY;
-            _gravity.z = _lastAccelZ;
-
+            _averageRotationRates = _lastRotationRates;
+            _averageAcceleration = _lastAcceleration;
+            _gravity = _lastAcceleration;
         } 
         else {
             //  Cumulate long term average to (hopefully) take DC bias out of rotation rates
-            _averageGyroRates[0] =  (1.f - 1.f/(float)LONG_TERM_RATE_SAMPLES) * _averageGyroRates[0] +
-            1.f/(float)LONG_TERM_RATE_SAMPLES * _lastRollRate;
-            _averageGyroRates[1] =  (1.f - 1.f/(float)LONG_TERM_RATE_SAMPLES) * _averageGyroRates[1] +
-            1.f/(float)LONG_TERM_RATE_SAMPLES * _lastYawRate;
-            _averageGyroRates[2] =  (1.f - 1.f/(float)LONG_TERM_RATE_SAMPLES) * _averageGyroRates[2] +
-            1.f/(float)LONG_TERM_RATE_SAMPLES * _lastPitchRate;
-
+            _averageRotationRates = (1.f - 1.f / (float)LONG_TERM_RATE_SAMPLES) * _averageRotationRates
+                                    + 1.f / (float)LONG_TERM_RATE_SAMPLES * _lastRotationRates;
+            _averageAcceleration = (1.f - 1.f / (float)LONG_TERM_RATE_SAMPLES) * _averageAcceleration
+                                    + 1.f / (float)LONG_TERM_RATE_SAMPLES * _lastAcceleration;
+            
             if (totalSamples < GRAVITY_SAMPLES) {
                 _gravity = (1.f - 1.f/(float)GRAVITY_SAMPLES) * _gravity +
-                1.f/(float)GRAVITY_SAMPLES * glm::vec3(_lastAccelX, _lastAccelY, _lastAccelZ);
+                1.f/(float)GRAVITY_SAMPLES * _lastAcceleration;
             }
         }
         
@@ -268,7 +278,12 @@ void SerialInterface::readData(float deltaTime) {
 void SerialInterface::resetAverages() {
     totalSamples = 0;
     _gravity = glm::vec3(0, 0, 0);
-    _averageGyroRates = glm::vec3(0, 0, 0);
+    _averageRotationRates = glm::vec3(0, 0, 0);
+    _averageAcceleration = glm::vec3(0, 0, 0);
+    _lastRotationRates = glm::vec3(0, 0, 0);
+    _estimatedRotation = glm::vec3(0, 0, 0);
+    _estimatedPosition = glm::vec3(0, 0, 0);
+    _estimatedVelocity = glm::vec3(0, 0, 0);
 }
 
 void SerialInterface::resetSerial() {
diff --git a/interface/src/SerialInterface.h b/interface/src/SerialInterface.h
index 42d24367f6..50baf5e98f 100644
--- a/interface/src/SerialInterface.h
+++ b/interface/src/SerialInterface.h
@@ -20,41 +20,32 @@
 #include <dirent.h>
 #endif
 
-#define NUM_CHANNELS 6
-
-//  Acceleration sensors, in screen/world coord system (X = left/right, Y = Up/Down, Z = fwd/back)
-#define ACCEL_X 3 
-#define ACCEL_Y 4 
-#define ACCEL_Z 5 
-
-//  Gyro sensors, in coodinate system of head/airplane
-#define HEAD_PITCH_RATE 1
-#define HEAD_YAW_RATE 0
-#define HEAD_ROLL_RATE 2
-
 extern const bool USING_INVENSENSE_MPU9150;
 
 class SerialInterface {
 public:
     SerialInterface() : active(false),
-                        _gravity(0,0,0),
-                        _averageGyroRates(0, 0, 0),
+                        _gravity(0, 0, 0),
+                        _averageRotationRates(0, 0, 0),
+                        _averageAcceleration(0, 0, 0),
                         _estimatedRotation(0, 0, 0),
-                        _lastAccelX(0),
-                        _lastAccelY(0),
-                        _lastAccelZ(0),
-                        _lastYawRate(0),
-                        _lastPitchRate(0),
-                        _lastRollRate(0) {}
+                        _estimatedPosition(0, 0, 0),
+                        _estimatedVelocity(0, 0, 0),
+                        _lastAcceleration(0, 0, 0),
+                        _lastRotationRates(0, 0, 0)
+                    {}
     
     void pair();
     void readData(float deltaTime);
-    
-    float getLastYawRate() const { return _lastYawRate - _averageGyroRates[1]; }
-    float getLastPitchRate() const { return _lastPitchRate - _averageGyroRates[2]; }
-    float getLastRollRate() const { return _lastRollRate - _averageGyroRates[0]; }
-    glm::vec3 getLastAcceleration() { return glm::vec3(_lastAccelX, _lastAccelY, _lastAccelZ); };
-    glm::vec3 getGravity() {return _gravity;};
+    const float getLastPitchRate() const { return _lastRotationRates[0] - _averageRotationRates[0]; }
+    const float getLastYawRate() const { return _lastRotationRates[1] - _averageRotationRates[1]; }
+    const float getLastRollRate() const { return _lastRotationRates[2] - _averageRotationRates[2]; }
+    const glm::vec3 getLastRotationRates() const { return _lastRotationRates; };
+    const glm::vec3 getEstimatedRotation() const { return _estimatedRotation; };
+    const glm::vec3 getEstimatedPosition() const { return _estimatedPosition; };
+    const glm::vec3 getEstimatedVelocity() const { return _estimatedVelocity; };
+    const glm::vec3 getLastAcceleration() const { return _lastAcceleration; };
+    const glm::vec3 getGravity() const { return _gravity; };
     
     void renderLevels(int width, int height);
     void resetAverages();
@@ -68,14 +59,13 @@ private:
     int totalSamples;
     timeval lastGoodRead;
     glm::vec3 _gravity;
-    glm::vec3 _averageGyroRates;
+    glm::vec3 _averageRotationRates;
+    glm::vec3 _averageAcceleration;
     glm::vec3 _estimatedRotation;
-    float _lastAccelX;
-    float _lastAccelY;
-    float _lastAccelZ;
-    float _lastYawRate;         //  Rates are in degrees per second. 
-    float _lastPitchRate;
-    float _lastRollRate;
+    glm::vec3 _estimatedPosition;
+    glm::vec3 _estimatedVelocity;
+    glm::vec3 _lastAcceleration;
+    glm::vec3 _lastRotationRates;
 };
 
 #endif

From da343215ceac4805e93ab238e0548bb918370ba9 Mon Sep 17 00:00:00 2001
From: Philip Rosedale <philip@coffeeandpower.com>
Date: Mon, 3 Jun 2013 17:25:20 -0700
Subject: [PATCH 2/9] Rename serialPort to serialHeadSensor, added passing of
 lean to eyeOffset

---
 interface/src/Application.cpp | 60 ++++++++++++++++++++---------------
 interface/src/Application.h   |  2 +-
 2 files changed, 36 insertions(+), 26 deletions(-)

diff --git a/interface/src/Application.cpp b/interface/src/Application.cpp
index 4dd3dba178..f3e4eb2c3b 100644
--- a/interface/src/Application.cpp
+++ b/interface/src/Application.cpp
@@ -908,8 +908,8 @@ void Application::timer() {
     gettimeofday(&_timerStart, NULL);
     
     // if we haven't detected gyros, check for them now
-    if (!_serialPort.active) {
-        _serialPort.pair();
+    if (!_serialHeadSensor.active) {
+        _serialHeadSensor.pair();
     }
 }
 
@@ -1044,8 +1044,8 @@ void Application::idle() {
         }
        
         //  Read serial port interface devices
-        if (_serialPort.active) {
-            _serialPort.readData(deltaTime);
+        if (_serialHeadSensor.active) {
+            _serialHeadSensor.readData(deltaTime);
         }
         
         //  Update transmitter
@@ -1446,26 +1446,36 @@ void Application::init() {
 }
 
 void Application::updateAvatar(float deltaTime) {
-    // Update my avatar's head position from gyros
-    _myAvatar.updateHeadFromGyros(deltaTime, &_serialPort);
 
-    //  Grab latest readings from the gyros
-    float measuredPitchRate = _serialPort.getLastPitchRate();
-    float measuredYawRate = _serialPort.getLastYawRate();
-    
-    //  Update gyro-based mouse (X,Y on screen)
-    const float MIN_MOUSE_RATE = 3.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 * HORIZONTAL_PIXELS_PER_DEGREE * deltaTime;
-        _headMouseY -= measuredPitchRate * VERTICAL_PIXELS_PER_DEGREE * deltaTime;
+    if (_serialHeadSensor.active) {
+        glm::vec3 headPosition = _serialHeadSensor.getEstimatedPosition();
+        const float HEAD_OFFSET_SCALING = 3.f;
+        headPosition *= HEAD_OFFSET_SCALING;
+        _myCamera.setEyeOffsetPosition(headPosition);
+        
+        // Update my avatar's head position from gyros
+        //_myAvatar.updateHeadFromGyros(deltaTime, &_serialHeadSensor);
+        
+        //  Grab latest readings from the gyros
+        float measuredPitchRate = _serialHeadSensor.getLastPitchRate();
+        float measuredYawRate = _serialHeadSensor.getLastYawRate();
+        
+        //  Update gyro-based mouse (X,Y on screen)
+        const float MIN_MOUSE_RATE = 3.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 * HORIZONTAL_PIXELS_PER_DEGREE * deltaTime;
+            _headMouseY -= measuredPitchRate * VERTICAL_PIXELS_PER_DEGREE * deltaTime;
+        }
+        _headMouseX = max(_headMouseX, 0);
+        _headMouseX = min(_headMouseX, _glWidget->width());
+        _headMouseY = max(_headMouseY, 0);
+        _headMouseY = min(_headMouseY, _glWidget->height());
+
     }
-    _headMouseX = max(_headMouseX, 0);
-    _headMouseX = min(_headMouseX, _glWidget->width());
-    _headMouseY = max(_headMouseY, 0);
-    _headMouseY = min(_headMouseY, _glWidget->height());
+
     
     if (OculusManager::isConnected()) {
         float yaw, pitch, roll;
@@ -1890,7 +1900,7 @@ void Application::displayOverlay() {
         }
         
     //  Show detected levels from the serial I/O ADC channel sensors
-    if (_displayLevels) _serialPort.renderLevels(_glWidget->width(), _glWidget->height());
+    if (_displayLevels) _serialHeadSensor.renderLevels(_glWidget->width(), _glWidget->height());
     
     //  Show hand transmitter data if detected
     if (_myTransmitter.isConnected()) {
@@ -2253,8 +2263,8 @@ void Application::resetSensors() {
     _headMouseX = _mouseX = _glWidget->width() / 2;
     _headMouseY = _mouseY = _glWidget->height() / 2;
     
-    if (_serialPort.active) {
-        _serialPort.resetAverages();
+    if (_serialHeadSensor.active) {
+        _serialHeadSensor.resetAverages();
     }
     QCursor::setPos(_headMouseX, _headMouseY);
     _myAvatar.reset();
diff --git a/interface/src/Application.h b/interface/src/Application.h
index 22694aaae5..3baa93e821 100644
--- a/interface/src/Application.h
+++ b/interface/src/Application.h
@@ -226,7 +226,7 @@ private:
     QAction* _fullScreenMode;        // whether we are in full screen mode
     QAction* _frustumRenderModeAction;
     
-    SerialInterface _serialPort;
+    SerialInterface _serialHeadSensor;
     bool _displayLevels;
     
     glm::vec3 _gravity;

From 5e0a70c52423988042b5806044e7b55bef3e730a Mon Sep 17 00:00:00 2001
From: Philip Rosedale <philip@coffeeandpower.com>
Date: Mon, 3 Jun 2013 17:36:32 -0700
Subject: [PATCH 3/9] gyro look controls head offset on/off

---
 interface/src/Application.cpp | 14 +++++++++-----
 1 file changed, 9 insertions(+), 5 deletions(-)

diff --git a/interface/src/Application.cpp b/interface/src/Application.cpp
index f3e4eb2c3b..f070bee823 100644
--- a/interface/src/Application.cpp
+++ b/interface/src/Application.cpp
@@ -1448,13 +1448,17 @@ void Application::init() {
 void Application::updateAvatar(float deltaTime) {
 
     if (_serialHeadSensor.active) {
-        glm::vec3 headPosition = _serialHeadSensor.getEstimatedPosition();
-        const float HEAD_OFFSET_SCALING = 3.f;
-        headPosition *= HEAD_OFFSET_SCALING;
-        _myCamera.setEyeOffsetPosition(headPosition);
+      
+        // Update avatar head translation
+        if (_gyroLook->isChecked()) {
+            glm::vec3 headPosition = _serialHeadSensor.getEstimatedPosition();
+            const float HEAD_OFFSET_SCALING = 3.f;
+            headPosition *= HEAD_OFFSET_SCALING;
+            _myCamera.setEyeOffsetPosition(headPosition);
+        }
         
         // Update my avatar's head position from gyros
-        //_myAvatar.updateHeadFromGyros(deltaTime, &_serialHeadSensor);
+        _myAvatar.updateHeadFromGyros(deltaTime, &_serialHeadSensor);
         
         //  Grab latest readings from the gyros
         float measuredPitchRate = _serialHeadSensor.getLastPitchRate();

From 29c4374dc0df2d3e2e80c282ca5b56ae3facd3da Mon Sep 17 00:00:00 2001
From: Philip Rosedale <philip@coffeeandpower.com>
Date: Wed, 5 Jun 2013 09:59:39 -0700
Subject: [PATCH 4/9] Merge fixes for brad, reversed pitch direction to correct
 mouse look

---
 interface/src/Application.h     |  1 -
 interface/src/Avatar.cpp        |  2 +-
 interface/src/SerialInterface.h | 12 ++++++------
 3 files changed, 7 insertions(+), 8 deletions(-)

diff --git a/interface/src/Application.h b/interface/src/Application.h
index 9d37925c05..5c987f0062 100644
--- a/interface/src/Application.h
+++ b/interface/src/Application.h
@@ -292,7 +292,6 @@ private:
     int _bytesPerSecond;
     int _bytesCount;
     
-    QSettings* _settings;   // Contain Menu settings and Avatar data
     bool _autosave;        // True if the autosave is on.
 };
 
diff --git a/interface/src/Avatar.cpp b/interface/src/Avatar.cpp
index 49b83dd670..d8178ca643 100644
--- a/interface/src/Avatar.cpp
+++ b/interface/src/Avatar.cpp
@@ -353,7 +353,7 @@ void  Avatar::updateFromMouse(int mouseX, int mouseY, int screenWidth, int scree
         if (fabs(mouseLocationY) > MOUSE_MOVE_RADIUS) {
             float mousePitchAdd = (fabs(mouseLocationY) - MOUSE_MOVE_RADIUS) / (0.5f - MOUSE_MOVE_RADIUS) * MOUSE_PITCH_SPEED;
             bool downPitching = (mouseLocationY > 0.f);
-            _head.setPitch(_head.getPitch() + (downPitching ? mousePitchAdd : -mousePitchAdd));
+            _head.setPitch(_head.getPitch() + (downPitching ? -mousePitchAdd : mousePitchAdd));
         }
         
     }
diff --git a/interface/src/SerialInterface.h b/interface/src/SerialInterface.h
index 50baf5e98f..3fc9ea920a 100644
--- a/interface/src/SerialInterface.h
+++ b/interface/src/SerialInterface.h
@@ -40,12 +40,12 @@ public:
     const float getLastPitchRate() const { return _lastRotationRates[0] - _averageRotationRates[0]; }
     const float getLastYawRate() const { return _lastRotationRates[1] - _averageRotationRates[1]; }
     const float getLastRollRate() const { return _lastRotationRates[2] - _averageRotationRates[2]; }
-    const glm::vec3 getLastRotationRates() const { return _lastRotationRates; };
-    const glm::vec3 getEstimatedRotation() const { return _estimatedRotation; };
-    const glm::vec3 getEstimatedPosition() const { return _estimatedPosition; };
-    const glm::vec3 getEstimatedVelocity() const { return _estimatedVelocity; };
-    const glm::vec3 getLastAcceleration() const { return _lastAcceleration; };
-    const glm::vec3 getGravity() const { return _gravity; };
+    const glm::vec3& getLastRotationRates() const { return _lastRotationRates; };
+    const glm::vec3& getEstimatedRotation() const { return _estimatedRotation; };
+    const glm::vec3& getEstimatedPosition() const { return _estimatedPosition; };
+    const glm::vec3& getEstimatedVelocity() const { return _estimatedVelocity; };
+    const glm::vec3& getLastAcceleration() const { return _lastAcceleration; };
+    const glm::vec3& getGravity() const { return _gravity; };
     
     void renderLevels(int width, int height);
     void resetAverages();

From 391b62ed32aca3b48f00b6ce85b71debaee5f9fd Mon Sep 17 00:00:00 2001
From: Andrzej Kapolka <drzej.k@gmail.com>
Date: Wed, 5 Jun 2013 10:34:41 -0700
Subject: [PATCH 5/9] Make sure we don't hear back from replies after we've
 cleared them.

---
 interface/src/AvatarVoxelSystem.cpp | 12 ++++--------
 1 file changed, 4 insertions(+), 8 deletions(-)

diff --git a/interface/src/AvatarVoxelSystem.cpp b/interface/src/AvatarVoxelSystem.cpp
index 46a4212239..c50cb5b8a7 100644
--- a/interface/src/AvatarVoxelSystem.cpp
+++ b/interface/src/AvatarVoxelSystem.cpp
@@ -79,6 +79,7 @@ void AvatarVoxelSystem::loadVoxelsFromURL(const QUrl& url) {
     // cancel any current download
     if (_voxelReply != 0) {
         delete _voxelReply;
+        _voxelReply = 0;
     }
     
     killLocalVoxels();
@@ -196,17 +197,11 @@ void AvatarVoxelSystem::handleVoxelDownloadProgress(qint64 bytesReceived, qint64
         return;
     }
 
-    // XXXBHG - I don't know why this can happen, but when I was testing this, I used a dropbox URL
-    // and it resulted in a case where the bytesTotal == bytesReceived, but the _voxelReply was NULL
-    // which needless to say caused crashes below. I've added this quick guard to protect against
-    // this case, but it probably should be investigated.
-    if (!_voxelReply) {
-        return;
-    }
-
     QByteArray entirety = _voxelReply->readAll();
+    _voxelReply->disconnect(this);
     _voxelReply->deleteLater();
     _voxelReply = 0;
+    
     _tree->readBitstreamToTree((unsigned char*)entirety.data(), entirety.size(), WANT_COLOR, NO_EXISTS_BITS);
     setupNewVoxelsForDrawing();
 }
@@ -214,6 +209,7 @@ void AvatarVoxelSystem::handleVoxelDownloadProgress(qint64 bytesReceived, qint64
 void AvatarVoxelSystem::handleVoxelReplyError() {
     printLog("%s\n", _voxelReply->errorString().toAscii().constData());
     
+    _voxelReply->disconnect(this);
     _voxelReply->deleteLater();
     _voxelReply = 0;
 }

From 69dd4ff59bfa90b545657331a36e4f9923893cda Mon Sep 17 00:00:00 2001
From: Andrzej Kapolka <drzej.k@gmail.com>
Date: Wed, 5 Jun 2013 13:08:17 -0700
Subject: [PATCH 6/9] Improved rotation computation.

---
 interface/src/Avatar.cpp   | 4 +++-
 interface/src/Skeleton.cpp | 4 +++-
 interface/src/Skeleton.h   | 2 +-
 3 files changed, 7 insertions(+), 3 deletions(-)

diff --git a/interface/src/Avatar.cpp b/interface/src/Avatar.cpp
index d8178ca643..ed4102f522 100644
--- a/interface/src/Avatar.cpp
+++ b/interface/src/Avatar.cpp
@@ -1082,7 +1082,9 @@ void Avatar::updateBodyBalls(float deltaTime) {
         if (_skeleton.joint[b].parent == AVATAR_JOINT_NULL || length < SMALL_SPRING_LENGTH) {
             _bodyBall[b].rotation = orientation * _skeleton.joint[_bodyBall[b].parentJoint].absoluteBindPoseRotation;
         } else {
-            _bodyBall[b].rotation = rotationBetween(jointDirection, springVector) * orientation;
+            glm::vec3 parentDirection = _bodyBall[ _skeleton.joint[b].parent ].rotation * JOINT_DIRECTION;
+            _bodyBall[b].rotation = rotationBetween(parentDirection, springVector) *
+                _bodyBall[ _skeleton.joint[b].parent ].rotation;
         }
     }
 }
diff --git a/interface/src/Skeleton.cpp b/interface/src/Skeleton.cpp
index b3720c8869..92dba252a1 100644
--- a/interface/src/Skeleton.cpp
+++ b/interface/src/Skeleton.cpp
@@ -118,7 +118,9 @@ void Skeleton::initialize() {
         } else {
             joint[b].absoluteBindPosePosition = joint[ joint[b].parent ].absoluteBindPosePosition +
                 joint[b].bindPosePosition;
-            joint[b].absoluteBindPoseRotation = rotationBetween(JOINT_DIRECTION, joint[b].bindPosePosition);
+            glm::vec3 parentDirection = joint[ joint[b].parent ].absoluteBindPoseRotation * JOINT_DIRECTION;
+            joint[b].absoluteBindPoseRotation = rotationBetween(parentDirection, joint[b].bindPosePosition) *
+                joint[ joint[b].parent ].absoluteBindPoseRotation;
         }
     }
 }
diff --git a/interface/src/Skeleton.h b/interface/src/Skeleton.h
index b5a7cee0ba..e98c2e7b12 100644
--- a/interface/src/Skeleton.h
+++ b/interface/src/Skeleton.h
@@ -42,7 +42,7 @@ enum AvatarJointID
     NUM_AVATAR_JOINTS
 };
 
-const glm::vec3 JOINT_DIRECTION = glm::vec3(1.0f, 0.0f, 0.0f);
+const glm::vec3 JOINT_DIRECTION = glm::vec3(0.0f, 1.0f, 0.0f);
 
 class Skeleton {
 public:

From a4aa8e7bde88ceb046c624a6af30f03d00e85148 Mon Sep 17 00:00:00 2001
From: Andrzej Kapolka <drzej.k@gmail.com>
Date: Wed, 5 Jun 2013 14:55:49 -0700
Subject: [PATCH 7/9] Basic sharing of the avatar voxel URLs.

---
 avatar-mixer/src/main.cpp            | 11 ++++++
 interface/src/Application.cpp        | 54 ++++++++++++++++++++++++++--
 interface/src/AvatarVoxelSystem.cpp  |  9 +++--
 interface/src/AvatarVoxelSystem.h    |  7 ++--
 libraries/shared/src/PacketHeaders.h |  1 +
 5 files changed, 76 insertions(+), 6 deletions(-)

diff --git a/avatar-mixer/src/main.cpp b/avatar-mixer/src/main.cpp
index caa9f609ce..0398a597fc 100644
--- a/avatar-mixer/src/main.cpp
+++ b/avatar-mixer/src/main.cpp
@@ -105,6 +105,17 @@ int main(int argc, const char* argv[]) {
                     
                     agentList->getAgentSocket()->send(agentAddress, broadcastPacket, currentBufferPosition - broadcastPacket);
                     
+                    break;
+                case PACKET_HEADER_AVATAR_VOXEL_URL:
+                    // grab the agent ID from the packet
+                    unpackAgentId(packetData + 1, &agentID);
+                    
+                    // let everyone else know about the update
+                    for (AgentList::iterator agent = agentList->begin(); agent != agentList->end(); agent++) {
+                        if (agent->getActiveSocket() && agent->getAgentID() != agentID) {
+                            agentList->getAgentSocket()->send(agent->getActiveSocket(), packetData, receivedBytes);
+                        }
+                    }
                     break;
                 case PACKET_HEADER_DOMAIN:
                     // ignore the DS packet, for now agents are added only when they communicate directly with us
diff --git a/interface/src/Application.cpp b/interface/src/Application.cpp
index f5f2085995..d70a1840ba 100644
--- a/interface/src/Application.cpp
+++ b/interface/src/Application.cpp
@@ -983,6 +983,44 @@ void Application::terminate() {
     }
 }
 
+static void sendAvatarVoxelURLMessage(const QUrl& url) {
+    uint16_t ownerID = AgentList::getInstance()->getOwnerID();
+    if (ownerID == UNKNOWN_AGENT_ID) {
+        return; // we don't yet know who we are
+    }
+    QByteArray message;
+    message.append(PACKET_HEADER_AVATAR_VOXEL_URL);
+    message.append((const char*)&ownerID, sizeof(ownerID));
+    message.append(url.toEncoded());
+
+    AgentList::getInstance()->broadcastToAgents((unsigned char*)message.data(), message.size(), &AGENT_TYPE_AVATAR_MIXER, 1);
+}
+
+static void processAvatarVoxelURLMessage(unsigned char *packetData, size_t dataBytes) {
+    // skip the header
+    packetData++;
+    dataBytes--;
+    
+    // read the agent id
+    uint16_t agentID = *(uint16_t*)packetData;
+    packetData += sizeof(agentID);
+    dataBytes -= sizeof(agentID);
+    
+    // make sure the agent exists
+    Agent* agent = AgentList::getInstance()->agentWithID(agentID);
+    if (!agent || !agent->getLinkedData()) {
+        return;
+    }
+    Avatar* avatar = static_cast<Avatar*>(agent->getLinkedData());
+    if (!avatar->isInitialized()) {
+        return; // wait until initialized
+    }
+    QUrl url = QUrl::fromEncoded(QByteArray::fromRawData((char*)packetData, dataBytes));
+    
+    // invoke the set URL function on the simulate/render thread
+    QMetaObject::invokeMethod(avatar->getVoxels(), "setVoxelURL", Q_ARG(QUrl, url));
+}
+
 void Application::editPreferences() {
     QDialog dialog(_glWidget);
     dialog.setWindowTitle("Interface Preferences");
@@ -1006,7 +1044,8 @@ void Application::editPreferences() {
     }
     QUrl url(avatarURL->text());
     _settings->setValue("avatarURL", url);
-    _myAvatar.getVoxels()->loadVoxelsFromURL(url);
+    _myAvatar.getVoxels()->setVoxelURL(url);
+    sendAvatarVoxelURLMessage(url);
 }
 
 void Application::pair() {
@@ -1507,7 +1546,9 @@ void Application::init() {
     _myCamera.setModeShiftRate(1.0f);
     _myAvatar.setDisplayingLookatVectors(false);  
     
-    _myAvatar.getVoxels()->loadVoxelsFromURL(_settings->value("avatarURL").toUrl());
+    QUrl avatarURL = _settings->value("avatarURL").toUrl();
+    _myAvatar.getVoxels()->setVoxelURL(avatarURL);
+    sendAvatarVoxelURLMessage(avatarURL);
     
     QCursor::setPos(_headMouseX, _headMouseY);
     
@@ -1599,6 +1640,12 @@ void Application::updateAvatar(float deltaTime) {
         
         const char broadcastReceivers[2] = {AGENT_TYPE_VOXEL, AGENT_TYPE_AVATAR_MIXER};
         AgentList::getInstance()->broadcastToAgents(broadcastString, endOfBroadcastStringWrite - broadcastString, broadcastReceivers, sizeof(broadcastReceivers));
+        
+        // once in a while, send my voxel url
+        const float AVATAR_VOXEL_URL_SEND_INTERVAL = 1.0f; // seconds
+        if (shouldDo(AVATAR_VOXEL_URL_SEND_INTERVAL, deltaTime)) {
+            sendAvatarVoxelURLMessage(_myAvatar.getVoxels()->getVoxelURL());
+        }
     }
 
     // If I'm in paint mode, send a voxel out to VOXEL server agents.
@@ -2435,6 +2482,9 @@ void* Application::networkReceive(void* args) {
                                                                    app->_incomingPacket,
                                                                    bytesReceived);
                     break;
+                case PACKET_HEADER_AVATAR_VOXEL_URL:
+                    processAvatarVoxelURLMessage(app->_incomingPacket, bytesReceived);
+                    break;
                 default:
                     AgentList::getInstance()->processAgentData(&senderAddress, app->_incomingPacket, bytesReceived);
                     break;
diff --git a/interface/src/AvatarVoxelSystem.cpp b/interface/src/AvatarVoxelSystem.cpp
index c50cb5b8a7..ef2066d69e 100644
--- a/interface/src/AvatarVoxelSystem.cpp
+++ b/interface/src/AvatarVoxelSystem.cpp
@@ -8,7 +8,6 @@
 #include <cstring>
 
 #include <QNetworkReply>
-#include <QUrl>
 
 #include <GeometryUtil.h>
 
@@ -24,6 +23,9 @@ const int BONE_ELEMENTS_PER_VOXEL = BONE_ELEMENTS_PER_VERTEX * VERTICES_PER_VOXE
 AvatarVoxelSystem::AvatarVoxelSystem(Avatar* avatar) :
     VoxelSystem(AVATAR_TREE_SCALE, MAX_VOXELS_PER_AVATAR),
     _avatar(avatar), _voxelReply(0) {
+
+    // we may have been created in the network thread, but we live in the main thread
+    moveToThread(Application::getInstance()->thread());
 }
 
 AvatarVoxelSystem::~AvatarVoxelSystem() {   
@@ -75,7 +77,7 @@ void AvatarVoxelSystem::removeOutOfView() {
     // no-op for now
 }
 
-void AvatarVoxelSystem::loadVoxelsFromURL(const QUrl& url) {
+void AvatarVoxelSystem::setVoxelURL(const QUrl& url) {
     // cancel any current download
     if (_voxelReply != 0) {
         delete _voxelReply;
@@ -84,6 +86,9 @@ void AvatarVoxelSystem::loadVoxelsFromURL(const QUrl& url) {
     
     killLocalVoxels();
     
+    // remember the URL
+    _voxelURL = url;
+    
     // handle "file://" urls...
     if (url.isLocalFile()) {
         QString pathString = url.path();
diff --git a/interface/src/AvatarVoxelSystem.h b/interface/src/AvatarVoxelSystem.h
index 3894fd75b9..cf297c25f4 100644
--- a/interface/src/AvatarVoxelSystem.h
+++ b/interface/src/AvatarVoxelSystem.h
@@ -10,6 +10,7 @@
 #define __interface__AvatarVoxelSystem__
 
 #include <QObject>
+#include <QUrl>
 
 #include "VoxelSystem.h"
 
@@ -17,7 +18,6 @@ const int BONE_ELEMENTS_PER_VERTEX = 4;
 typedef GLubyte BoneIndices[BONE_ELEMENTS_PER_VERTEX];
 
 class QNetworkReply;
-class QUrl;
 
 class Avatar;
 
@@ -33,7 +33,8 @@ public:
 
     virtual void removeOutOfView();
     
-    void loadVoxelsFromURL(const QUrl& url);
+    Q_INVOKABLE void setVoxelURL(const QUrl& url);
+    const QUrl& getVoxelURL() const { return _voxelURL; }
     
 protected:
     
@@ -55,6 +56,8 @@ private:
     
     Avatar* _avatar;
     
+    QUrl _voxelURL;
+    
     GLubyte* _readBoneIndicesArray;
     GLfloat* _readBoneWeightsArray;
     GLubyte* _writeBoneIndicesArray;
diff --git a/libraries/shared/src/PacketHeaders.h b/libraries/shared/src/PacketHeaders.h
index ea9f5f9fcf..3e6f2ba152 100644
--- a/libraries/shared/src/PacketHeaders.h
+++ b/libraries/shared/src/PacketHeaders.h
@@ -27,6 +27,7 @@ const PACKET_HEADER PACKET_HEADER_ERASE_VOXEL = 'E';
 const PACKET_HEADER PACKET_HEADER_VOXEL_DATA = 'V';
 const PACKET_HEADER PACKET_HEADER_VOXEL_DATA_MONOCHROME = 'v';
 const PACKET_HEADER PACKET_HEADER_BULK_AVATAR_DATA = 'X';
+const PACKET_HEADER PACKET_HEADER_AVATAR_VOXEL_URL = 'U';
 const PACKET_HEADER PACKET_HEADER_TRANSMITTER_DATA_V2 = 'T';
 const PACKET_HEADER PACKET_HEADER_ENVIRONMENT_DATA = 'e';
 const PACKET_HEADER PACKET_HEADER_DOMAIN_LIST_REQUEST = 'L';

From b9e2e26ab10a906ba553dc1ef8069b81d278b98c Mon Sep 17 00:00:00 2001
From: Andrzej Kapolka <drzej.k@gmail.com>
Date: Wed, 5 Jun 2013 14:59:08 -0700
Subject: [PATCH 8/9] Don't restart the download when we're given the same URL.

---
 interface/src/AvatarVoxelSystem.cpp | 5 +++++
 1 file changed, 5 insertions(+)

diff --git a/interface/src/AvatarVoxelSystem.cpp b/interface/src/AvatarVoxelSystem.cpp
index ef2066d69e..8a7708587f 100644
--- a/interface/src/AvatarVoxelSystem.cpp
+++ b/interface/src/AvatarVoxelSystem.cpp
@@ -78,6 +78,11 @@ void AvatarVoxelSystem::removeOutOfView() {
 }
 
 void AvatarVoxelSystem::setVoxelURL(const QUrl& url) {
+    // don't restart the download if it's the same URL
+    if (_voxelURL == url) {
+        return;
+    }
+
     // cancel any current download
     if (_voxelReply != 0) {
         delete _voxelReply;

From 6c3425a642358446375275d33898ac5bb02b7f8a Mon Sep 17 00:00:00 2001
From: Andrzej Kapolka <drzej.k@gmail.com>
Date: Wed, 5 Jun 2013 15:39:37 -0700
Subject: [PATCH 9/9] Fix for URL decoding.

---
 interface/src/Application.cpp | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/interface/src/Application.cpp b/interface/src/Application.cpp
index d70a1840ba..ce176b9acf 100644
--- a/interface/src/Application.cpp
+++ b/interface/src/Application.cpp
@@ -1015,7 +1015,7 @@ static void processAvatarVoxelURLMessage(unsigned char *packetData, size_t dataB
     if (!avatar->isInitialized()) {
         return; // wait until initialized
     }
-    QUrl url = QUrl::fromEncoded(QByteArray::fromRawData((char*)packetData, dataBytes));
+    QUrl url = QUrl::fromEncoded(QByteArray((char*)packetData, dataBytes));
     
     // invoke the set URL function on the simulate/render thread
     QMetaObject::invokeMethod(avatar->getVoxels(), "setVoxelURL", Q_ARG(QUrl, url));