From 5820c3c7c33de16ed8e8d81c7257dc1f924e2df6 Mon Sep 17 00:00:00 2001
From: Andrzej Kapolka <drzej.k@gmail.com>
Date: Mon, 10 Jun 2013 14:49:39 -0700
Subject: [PATCH 01/24] Remove average measurements, compute estimated
 acceleration (without rotated gravity).

---
 interface/src/SerialInterface.cpp | 23 ++++++++---------------
 interface/src/SerialInterface.h   | 12 +++++-------
 2 files changed, 13 insertions(+), 22 deletions(-)

diff --git a/interface/src/SerialInterface.cpp b/interface/src/SerialInterface.cpp
index 28ae7eceb4..aca91bb34b 100644
--- a/interface/src/SerialInterface.cpp
+++ b/interface/src/SerialInterface.cpp
@@ -155,11 +155,11 @@ void SerialInterface::renderLevels(int width, int height) {
         // Acceleration rates
         glColor4f(1, 1, 1, 1);
         glVertex2f(LEVEL_CORNER_X + LEVEL_CENTER, 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 + (int)(_estimatedAcceleration.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)((_lastAcceleration.y - _gravity.y) *ACCEL_VIEW_SCALING), LEVEL_CORNER_Y + 57);
+        glVertex2f(LEVEL_CORNER_X + LEVEL_CENTER + (int)(_estimatedAcceleration.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)((_lastAcceleration.z - _gravity.z) * ACCEL_VIEW_SCALING), LEVEL_CORNER_Y + 72);
+        glVertex2f(LEVEL_CORNER_X + LEVEL_CENTER + (int)(_estimatedAcceleration.z * ACCEL_VIEW_SCALING), LEVEL_CORNER_Y + 72);
         
         // Estimated Position
         glColor4f(0, 1, 1, 1);
@@ -231,29 +231,24 @@ void SerialInterface::readData(float deltaTime) {
 
         //  Update raw rotation estimates
         glm::quat estimatedRotation = glm::quat(glm::radians(_estimatedRotation)) *
-            glm::quat(glm::radians(deltaTime * (_lastRotationRates - _averageRotationRates)));
+            glm::quat(glm::radians(deltaTime * _lastRotationRates));
+        
+        //  Update acceleration estimate
+        _estimatedAcceleration = _lastAcceleration - glm::inverse(estimatedRotation) * _gravity;
         
         //  Update estimated position and velocity
         float const DECAY_VELOCITY = 0.95f;
         float const DECAY_POSITION = 0.95f;
-        _estimatedVelocity += deltaTime * (_lastAcceleration - _averageAcceleration);
+        _estimatedVelocity += deltaTime * _estimatedAcceleration;
         _estimatedPosition += deltaTime * _estimatedVelocity;
         _estimatedVelocity *= DECAY_VELOCITY;
         _estimatedPosition *= DECAY_POSITION;
                 
         //  Accumulate a set of initial baseline readings for setting gravity
         if (totalSamples == 0) {
-            _averageRotationRates = _lastRotationRates;
-            _averageAcceleration = _lastAcceleration;
             _gravity = _lastAcceleration;
         } 
         else {
-            //  Cumulate long term average to (hopefully) take DC bias out of rotation rates
-            _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 * _lastAcceleration;
@@ -293,8 +288,6 @@ void SerialInterface::readData(float deltaTime) {
 void SerialInterface::resetAverages() {
     totalSamples = 0;
     _gravity = 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);
diff --git a/interface/src/SerialInterface.h b/interface/src/SerialInterface.h
index 3fc9ea920a..a49c09c599 100644
--- a/interface/src/SerialInterface.h
+++ b/interface/src/SerialInterface.h
@@ -26,8 +26,6 @@ class SerialInterface {
 public:
     SerialInterface() : active(false),
                         _gravity(0, 0, 0),
-                        _averageRotationRates(0, 0, 0),
-                        _averageAcceleration(0, 0, 0),
                         _estimatedRotation(0, 0, 0),
                         _estimatedPosition(0, 0, 0),
                         _estimatedVelocity(0, 0, 0),
@@ -37,13 +35,14 @@ public:
     
     void pair();
     void readData(float deltaTime);
-    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 float getLastPitchRate() const { return _lastRotationRates[0]; }
+    const float getLastYawRate() const { return _lastRotationRates[1]; }
+    const float getLastRollRate() const { return _lastRotationRates[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& getEstimatedAcceleration() const { return _estimatedAcceleration; };
     const glm::vec3& getLastAcceleration() const { return _lastAcceleration; };
     const glm::vec3& getGravity() const { return _gravity; };
     
@@ -59,11 +58,10 @@ private:
     int totalSamples;
     timeval lastGoodRead;
     glm::vec3 _gravity;
-    glm::vec3 _averageRotationRates;
-    glm::vec3 _averageAcceleration;
     glm::vec3 _estimatedRotation;
     glm::vec3 _estimatedPosition;
     glm::vec3 _estimatedVelocity;
+    glm::vec3 _estimatedAcceleration;
     glm::vec3 _lastAcceleration;
     glm::vec3 _lastRotationRates;
 };

From 1b8683cbc5371b6603758c32bd9c667eba7e19a4 Mon Sep 17 00:00:00 2001
From: Andrzej Kapolka <drzej.k@gmail.com>
Date: Mon, 10 Jun 2013 15:22:53 -0700
Subject: [PATCH 02/24] Let's see if we can get an estimate of the distance to
 the sensor based on the ratios between linear and angular velocity.

---
 interface/src/SerialInterface.cpp | 21 ++++++++++++++++++---
 1 file changed, 18 insertions(+), 3 deletions(-)

diff --git a/interface/src/SerialInterface.cpp b/interface/src/SerialInterface.cpp
index aca91bb34b..26c7f5f581 100644
--- a/interface/src/SerialInterface.cpp
+++ b/interface/src/SerialInterface.cpp
@@ -225,9 +225,14 @@ void SerialInterface::readData(float deltaTime) {
         
         //  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.
-        _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;
+        glm::vec3 rotationRates;
+        rotationRates[0] = ((float) -pitchRate) * LSB_TO_DEGREES_PER_SECOND;
+        rotationRates[1] = ((float) -yawRate) * LSB_TO_DEGREES_PER_SECOND;
+        rotationRates[2] = ((float) -rollRate) * LSB_TO_DEGREES_PER_SECOND;
+
+        // compute the angular acceleration
+        glm::vec3 angularAcceleration = (deltaTime < EPSILON) ? glm::vec3() : (rotationRates - _lastRotationRates) / deltaTime;
+        _lastRotationRates = rotationRates;
 
         //  Update raw rotation estimates
         glm::quat estimatedRotation = glm::quat(glm::radians(_estimatedRotation)) *
@@ -236,6 +241,16 @@ void SerialInterface::readData(float deltaTime) {
         //  Update acceleration estimate
         _estimatedAcceleration = _lastAcceleration - glm::inverse(estimatedRotation) * _gravity;
         
+        static float ratioEstimate = 0.0f;
+        float angularAccelerationLength = glm::length(angularAcceleration);
+        float linearAccelerationLength = glm::length(estimatedAcceleration);
+        if (angularAccelerationLength > EPSILON && linearAccelerationLength > EPSILON) {
+            float ratio = linearAccelerationLength / angularAccelerationLength;
+            static float ratioEstimate = ratio;
+            ratioEstimate = ratioEstimate * 0.999 + ratio * 0.001;
+            printLog("%g %g\n", ratio, ratioEstimate);
+        }
+        
         //  Update estimated position and velocity
         float const DECAY_VELOCITY = 0.95f;
         float const DECAY_POSITION = 0.95f;

From 90ef6a7cc0b3fe20d58b266d835ccdb27d08c713 Mon Sep 17 00:00:00 2001
From: Andrzej Kapolka <drzej.k@gmail.com>
Date: Mon, 10 Jun 2013 18:40:07 -0700
Subject: [PATCH 03/24] Attempting to perform the transform between angular and
 linear acceleration.

---
 interface/src/SerialInterface.cpp | 23 ++++++++++++-----------
 1 file changed, 12 insertions(+), 11 deletions(-)

diff --git a/interface/src/SerialInterface.cpp b/interface/src/SerialInterface.cpp
index 26c7f5f581..407f06f736 100644
--- a/interface/src/SerialInterface.cpp
+++ b/interface/src/SerialInterface.cpp
@@ -238,18 +238,19 @@ void SerialInterface::readData(float deltaTime) {
         glm::quat estimatedRotation = glm::quat(glm::radians(_estimatedRotation)) *
             glm::quat(glm::radians(deltaTime * _lastRotationRates));
         
+        //  The acceleration matrix transforms angular to linear accelerations
+        const glm::vec3 PIVOT_OFFSET(0.0f, -0.02f, -0.01f);
+        const glm::vec3 PIVOT_VECTOR_NORMALIZED = glm::normalize(PIVOT_OFFSET);
+        const glm::vec3 PIVOT_SINES = glm::max(glm::vec3(EPSILON, EPSILON, EPSILON),
+            glm::sqrt(glm::vec3(1.0f, 1.0f, 1.0f) - PIVOT_OFFSET * PIVOT_OFFSET));
+        const glm::mat3 ACCELERATION_MATRIX(
+            0.0f, PIVOT_OFFSET.z / PIVOT_SINES.x, -PIVOT_OFFSET.y / PIVOT_SINES.x,
+            -PIVOT_OFFSET.z / PIVOT_SINES.y, 0.0f, PIVOT_OFFSET.x / PIVOT_SINES.y,
+            PIVOT_OFFSET.y / PIVOT_SINES.z, -PIVOT_OFFSET.x / PIVOT_SINES.z, 0.0f);
+            
         //  Update acceleration estimate
-        _estimatedAcceleration = _lastAcceleration - glm::inverse(estimatedRotation) * _gravity;
-        
-        static float ratioEstimate = 0.0f;
-        float angularAccelerationLength = glm::length(angularAcceleration);
-        float linearAccelerationLength = glm::length(estimatedAcceleration);
-        if (angularAccelerationLength > EPSILON && linearAccelerationLength > EPSILON) {
-            float ratio = linearAccelerationLength / angularAccelerationLength;
-            static float ratioEstimate = ratio;
-            ratioEstimate = ratioEstimate * 0.999 + ratio * 0.001;
-            printLog("%g %g\n", ratio, ratioEstimate);
-        }
+        _estimatedAcceleration = _lastAcceleration - glm::inverse(estimatedRotation) * _gravity -
+            ACCELERATION_MATRIX * angularAcceleration;
         
         //  Update estimated position and velocity
         float const DECAY_VELOCITY = 0.95f;

From a34f9a0ef3142091f5d053eccdc41052111cc91a Mon Sep 17 00:00:00 2001
From: Andrzej Kapolka <drzej.k@gmail.com>
Date: Mon, 10 Jun 2013 18:41:49 -0700
Subject: [PATCH 04/24] Need to include SharedUtil.

---
 interface/src/SerialInterface.cpp | 1 +
 1 file changed, 1 insertion(+)

diff --git a/interface/src/SerialInterface.cpp b/interface/src/SerialInterface.cpp
index 407f06f736..2e5124c1b3 100644
--- a/interface/src/SerialInterface.cpp
+++ b/interface/src/SerialInterface.cpp
@@ -6,6 +6,7 @@
 //
 
 #include "SerialInterface.h"
+#include "SharedUtil.h"
 #include "Util.h"
 #include <glm/gtx/vector_angle.hpp>
 #include <math.h>

From 988c4f97b04e13804eba41c03e3e6a8c3c6640e9 Mon Sep 17 00:00:00 2001
From: Andrzej Kapolka <drzej.k@gmail.com>
Date: Mon, 10 Jun 2013 18:43:11 -0700
Subject: [PATCH 05/24] This should be the normalized offset.

---
 interface/src/SerialInterface.cpp | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/interface/src/SerialInterface.cpp b/interface/src/SerialInterface.cpp
index 2e5124c1b3..7c4d1ac86f 100644
--- a/interface/src/SerialInterface.cpp
+++ b/interface/src/SerialInterface.cpp
@@ -241,9 +241,9 @@ void SerialInterface::readData(float deltaTime) {
         
         //  The acceleration matrix transforms angular to linear accelerations
         const glm::vec3 PIVOT_OFFSET(0.0f, -0.02f, -0.01f);
-        const glm::vec3 PIVOT_VECTOR_NORMALIZED = glm::normalize(PIVOT_OFFSET);
+        const glm::vec3 PIVOT_OFFSET_NORMALIZED = glm::normalize(PIVOT_OFFSET);
         const glm::vec3 PIVOT_SINES = glm::max(glm::vec3(EPSILON, EPSILON, EPSILON),
-            glm::sqrt(glm::vec3(1.0f, 1.0f, 1.0f) - PIVOT_OFFSET * PIVOT_OFFSET));
+            glm::sqrt(glm::vec3(1.0f, 1.0f, 1.0f) - PIVOT_OFFSET_NORMALIZED * PIVOT_OFFSET_NORMALIZED));
         const glm::mat3 ACCELERATION_MATRIX(
             0.0f, PIVOT_OFFSET.z / PIVOT_SINES.x, -PIVOT_OFFSET.y / PIVOT_SINES.x,
             -PIVOT_OFFSET.z / PIVOT_SINES.y, 0.0f, PIVOT_OFFSET.x / PIVOT_SINES.y,

From 73ceea7795dbe8db4f58d4fb0af058e9d43927a8 Mon Sep 17 00:00:00 2001
From: Andrzej Kapolka <drzej.k@gmail.com>
Date: Tue, 11 Jun 2013 10:55:28 -0700
Subject: [PATCH 06/24] Working on "learning" the acceleration matrices.

---
 interface/src/SerialInterface.cpp | 55 ++++++++++++++++++++++++-------
 interface/src/SerialInterface.h   |  7 +++-
 2 files changed, 49 insertions(+), 13 deletions(-)

diff --git a/interface/src/SerialInterface.cpp b/interface/src/SerialInterface.cpp
index 7c4d1ac86f..cbd03f801b 100644
--- a/interface/src/SerialInterface.cpp
+++ b/interface/src/SerialInterface.cpp
@@ -239,19 +239,50 @@ void SerialInterface::readData(float deltaTime) {
         glm::quat estimatedRotation = glm::quat(glm::radians(_estimatedRotation)) *
             glm::quat(glm::radians(deltaTime * _lastRotationRates));
         
-        //  The acceleration matrix transforms angular to linear accelerations
-        const glm::vec3 PIVOT_OFFSET(0.0f, -0.02f, -0.01f);
-        const glm::vec3 PIVOT_OFFSET_NORMALIZED = glm::normalize(PIVOT_OFFSET);
-        const glm::vec3 PIVOT_SINES = glm::max(glm::vec3(EPSILON, EPSILON, EPSILON),
-            glm::sqrt(glm::vec3(1.0f, 1.0f, 1.0f) - PIVOT_OFFSET_NORMALIZED * PIVOT_OFFSET_NORMALIZED));
-        const glm::mat3 ACCELERATION_MATRIX(
-            0.0f, PIVOT_OFFSET.z / PIVOT_SINES.x, -PIVOT_OFFSET.y / PIVOT_SINES.x,
-            -PIVOT_OFFSET.z / PIVOT_SINES.y, 0.0f, PIVOT_OFFSET.x / PIVOT_SINES.y,
-            PIVOT_OFFSET.y / PIVOT_SINES.z, -PIVOT_OFFSET.x / PIVOT_SINES.z, 0.0f);
+        //  Update acceleration estimate: first, subtract gravity as rotated into current frame
+        _estimatedAcceleration = _lastAcceleration - glm::inverse(estimatedRotation) * _gravity;
+        
+        //  Consider updating our angular velocity/acceleration to linear acceleration mapping
+        if (glm::length(_lastRotationRates) > EPSILON || glm::length(angularAcceleration) > EPSILON) {
+            // compute predicted linear acceleration, find error between actual and predicted
+            glm::vec3 predictedAcceleration = _angularVelocityToLinearAccel * _lastRotationRates +
+                _angularAccelToLinearAccel * angularAcceleration;
+            glm::vec3 error = _estimatedAcceleration - predictedAcceleration;
             
-        //  Update acceleration estimate
-        _estimatedAcceleration = _lastAcceleration - glm::inverse(estimatedRotation) * _gravity -
-            ACCELERATION_MATRIX * angularAcceleration;
+            // adjust according to error in each dimension, in proportion to input magnitudes
+            for (int i = 0; i < 3; i++) {
+                if (fabsf(error[i]) < EPSILON) {
+                    continue;
+                }
+                const float LEARNING_RATE = 0.1f;
+                float rateSum = fabsf(_lastRotationRates.x) + fabsf(_lastRotationRates.y) + fabsf(_lastRotationRates.z);
+                if (rateSum > EPSILON) {
+                    for (int j = 0; j < 3; j++) {
+                        float proportion = LEARNING_RATE * fabsf(_lastRotationRates[j]) / rateSum;
+                        if (proportion > EPSILON) {
+                            _angularVelocityToLinearAccel[j][i] += error[i] * proportion / _lastRotationRates[j];
+                        }
+                    }
+                }
+                float accelSum = fabsf(angularAcceleration.x) + fabsf(angularAcceleration.y) + fabsf(angularAcceleration.z);
+                if (accelSum > EPSILON) {
+                    for (int j = 0; j < 3; j++) {
+                        float proportion = LEARNING_RATE * fabsf(angularAcceleration[j]) / accelSum;
+                        if (proportion > EPSILON) {
+                            _angularAccelToLinearAccel[j][i] += error[i] * proportion / angularAcceleration[j];
+                        }
+                    }                
+                }
+            }
+        }
+        
+        printLog("%g %g %g\n", _angularVelocityToLinearAccel[0][0], _angularVelocityToLinearAccel[1][0], _angularVelocityToLinearAccel[2][0]);
+        printLog("%g %g %g\n", _angularVelocityToLinearAccel[0][1], _angularVelocityToLinearAccel[1][1], _angularVelocityToLinearAccel[2][1]);
+        printLog("%g %g %g\n\n", _angularVelocityToLinearAccel[0][2], _angularVelocityToLinearAccel[1][2], _angularVelocityToLinearAccel[2][2]);
+        
+        printLog("%g %g %g\n", _angularAccelToLinearAccel[0][0], _angularAccelToLinearAccel[1][0], _angularAccelToLinearAccel[2][0]);
+        printLog("%g %g %g\n", _angularAccelToLinearAccel[0][1], _angularAccelToLinearAccel[1][1], _angularAccelToLinearAccel[2][1]);
+        printLog("%g %g %g\n\n", _angularAccelToLinearAccel[0][2], _angularAccelToLinearAccel[1][2], _angularAccelToLinearAccel[2][2]);
         
         //  Update estimated position and velocity
         float const DECAY_VELOCITY = 0.95f;
diff --git a/interface/src/SerialInterface.h b/interface/src/SerialInterface.h
index a49c09c599..b7ead8c9d7 100644
--- a/interface/src/SerialInterface.h
+++ b/interface/src/SerialInterface.h
@@ -30,7 +30,9 @@ public:
                         _estimatedPosition(0, 0, 0),
                         _estimatedVelocity(0, 0, 0),
                         _lastAcceleration(0, 0, 0),
-                        _lastRotationRates(0, 0, 0)
+                        _lastRotationRates(0, 0, 0),
+                        _angularVelocityToLinearAccel(0),
+                        _angularAccelToLinearAccel(0)
                     {}
     
     void pair();
@@ -64,6 +66,9 @@ private:
     glm::vec3 _estimatedAcceleration;
     glm::vec3 _lastAcceleration;
     glm::vec3 _lastRotationRates;
+    
+    glm::mat3 _angularVelocityToLinearAccel;
+    glm::mat3 _angularAccelToLinearAccel;
 };
 
 #endif

From bf389de774a14b658850d3cf3940b83af787dd67 Mon Sep 17 00:00:00 2001
From: Andrzej Kapolka <drzej.k@gmail.com>
Date: Tue, 11 Jun 2013 11:22:43 -0700
Subject: [PATCH 07/24] Testing a different coordinate conversion.

---
 interface/src/SerialInterface.cpp | 13 +++++++++----
 1 file changed, 9 insertions(+), 4 deletions(-)

diff --git a/interface/src/SerialInterface.cpp b/interface/src/SerialInterface.cpp
index cbd03f801b..03a03b87aa 100644
--- a/interface/src/SerialInterface.cpp
+++ b/interface/src/SerialInterface.cpp
@@ -216,8 +216,9 @@ void SerialInterface::readData(float deltaTime) {
                                                                 //  From MPU-9150 register map, with setting on
                                                                 //  highest resolution = +/- 2G
         
-        _lastAcceleration = glm::vec3(-accelXRate, -accelYRate, -accelZRate) * LSB_TO_METERS_PER_SECOND2;
-                
+        //_lastAcceleration = glm::vec3(-accelXRate, -accelYRate, -accelZRate) * LSB_TO_METERS_PER_SECOND2;
+        _lastAcceleration = glm::vec3(-accelYRate, accelXRate, accelZRate) * LSB_TO_METERS_PER_SECOND2;
+        
         int rollRate, yawRate, pitchRate;
         
         convertHexToInt(sensorBuffer + 22, rollRate);
@@ -227,8 +228,12 @@ void SerialInterface::readData(float deltaTime) {
         //  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.
         glm::vec3 rotationRates;
-        rotationRates[0] = ((float) -pitchRate) * LSB_TO_DEGREES_PER_SECOND;
-        rotationRates[1] = ((float) -yawRate) * LSB_TO_DEGREES_PER_SECOND;
+        //rotationRates[0] = ((float) -pitchRate) * LSB_TO_DEGREES_PER_SECOND;
+        //rotationRates[1] = ((float) -yawRate) * LSB_TO_DEGREES_PER_SECOND;
+        //rotationRates[2] = ((float) -rollRate) * LSB_TO_DEGREES_PER_SECOND;
+
+        rotationRates[0] = ((float) -yawRate) * LSB_TO_DEGREES_PER_SECOND;
+        rotationRates[1] = ((float)  pitchRate) * LSB_TO_DEGREES_PER_SECOND;
         rotationRates[2] = ((float) -rollRate) * LSB_TO_DEGREES_PER_SECOND;
 
         // compute the angular acceleration

From ff9e6f831c17bc0285f660681a8f296441a60853 Mon Sep 17 00:00:00 2001
From: Andrzej Kapolka <drzej.k@gmail.com>
Date: Tue, 11 Jun 2013 11:31:01 -0700
Subject: [PATCH 08/24] Revert to previous coordinates.

---
 interface/src/SerialInterface.cpp | 13 ++++---------
 1 file changed, 4 insertions(+), 9 deletions(-)

diff --git a/interface/src/SerialInterface.cpp b/interface/src/SerialInterface.cpp
index 03a03b87aa..cbd03f801b 100644
--- a/interface/src/SerialInterface.cpp
+++ b/interface/src/SerialInterface.cpp
@@ -216,9 +216,8 @@ void SerialInterface::readData(float deltaTime) {
                                                                 //  From MPU-9150 register map, with setting on
                                                                 //  highest resolution = +/- 2G
         
-        //_lastAcceleration = glm::vec3(-accelXRate, -accelYRate, -accelZRate) * LSB_TO_METERS_PER_SECOND2;
-        _lastAcceleration = glm::vec3(-accelYRate, accelXRate, 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);
@@ -228,12 +227,8 @@ void SerialInterface::readData(float deltaTime) {
         //  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.
         glm::vec3 rotationRates;
-        //rotationRates[0] = ((float) -pitchRate) * LSB_TO_DEGREES_PER_SECOND;
-        //rotationRates[1] = ((float) -yawRate) * LSB_TO_DEGREES_PER_SECOND;
-        //rotationRates[2] = ((float) -rollRate) * LSB_TO_DEGREES_PER_SECOND;
-
-        rotationRates[0] = ((float) -yawRate) * LSB_TO_DEGREES_PER_SECOND;
-        rotationRates[1] = ((float)  pitchRate) * LSB_TO_DEGREES_PER_SECOND;
+        rotationRates[0] = ((float) -pitchRate) * LSB_TO_DEGREES_PER_SECOND;
+        rotationRates[1] = ((float) -yawRate) * LSB_TO_DEGREES_PER_SECOND;
         rotationRates[2] = ((float) -rollRate) * LSB_TO_DEGREES_PER_SECOND;
 
         // compute the angular acceleration

From 517bbe80a7d8a3efe86d2fe126efca0884cf1997 Mon Sep 17 00:00:00 2001
From: Andrzej Kapolka <drzej.k@gmail.com>
Date: Tue, 11 Jun 2013 13:02:20 -0700
Subject: [PATCH 09/24] Initial values, report error.

---
 interface/src/SerialInterface.cpp |  2 ++
 interface/src/SerialInterface.h   | 10 ++++++++--
 2 files changed, 10 insertions(+), 2 deletions(-)

diff --git a/interface/src/SerialInterface.cpp b/interface/src/SerialInterface.cpp
index cbd03f801b..f4af4437d8 100644
--- a/interface/src/SerialInterface.cpp
+++ b/interface/src/SerialInterface.cpp
@@ -249,6 +249,8 @@ void SerialInterface::readData(float deltaTime) {
                 _angularAccelToLinearAccel * angularAcceleration;
             glm::vec3 error = _estimatedAcceleration - predictedAcceleration;
             
+            printLog("error %g\n", glm::length(error));
+            
             // adjust according to error in each dimension, in proportion to input magnitudes
             for (int i = 0; i < 3; i++) {
                 if (fabsf(error[i]) < EPSILON) {
diff --git a/interface/src/SerialInterface.h b/interface/src/SerialInterface.h
index b7ead8c9d7..2bb7e541ac 100644
--- a/interface/src/SerialInterface.h
+++ b/interface/src/SerialInterface.h
@@ -31,8 +31,14 @@ public:
                         _estimatedVelocity(0, 0, 0),
                         _lastAcceleration(0, 0, 0),
                         _lastRotationRates(0, 0, 0),
-                        _angularVelocityToLinearAccel(0),
-                        _angularAccelToLinearAccel(0)
+                        _angularVelocityToLinearAccel( // experimentally derived initial values
+                            0.001f, -0.008f, 0.020f, 
+                            0.003f, -0.003f, 0.025f,
+                            0.017f, 0.007f, 0.029f),
+                        _angularAccelToLinearAccel( // experimentally derived initial values
+                            0.0f, 0.0f, 0.002f,
+                            0.0f, 0.0f, 0.002f,
+                            -0.002f, -0.002f, 0.0f)
                     {}
     
     void pair();

From 5ba1d3a28cbd980d813e0159e6f88d5485cdaf6b Mon Sep 17 00:00:00 2001
From: Andrzej Kapolka <drzej.k@gmail.com>
Date: Tue, 11 Jun 2013 13:18:13 -0700
Subject: [PATCH 10/24] It ain't perfect, but I'm going with what I've got for
 now.

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

diff --git a/interface/src/SerialInterface.cpp b/interface/src/SerialInterface.cpp
index f4af4437d8..af9c35196b 100644
--- a/interface/src/SerialInterface.cpp
+++ b/interface/src/SerialInterface.cpp
@@ -249,14 +249,15 @@ void SerialInterface::readData(float deltaTime) {
                 _angularAccelToLinearAccel * angularAcceleration;
             glm::vec3 error = _estimatedAcceleration - predictedAcceleration;
             
-            printLog("error %g\n", glm::length(error));
+            // the "error" is actually what we want: the linear acceleration minus rotational influences
+            _estimatedAcceleration = error;
             
             // adjust according to error in each dimension, in proportion to input magnitudes
             for (int i = 0; i < 3; i++) {
                 if (fabsf(error[i]) < EPSILON) {
                     continue;
                 }
-                const float LEARNING_RATE = 0.1f;
+                const float LEARNING_RATE = 0.01f;
                 float rateSum = fabsf(_lastRotationRates.x) + fabsf(_lastRotationRates.y) + fabsf(_lastRotationRates.z);
                 if (rateSum > EPSILON) {
                     for (int j = 0; j < 3; j++) {
@@ -278,13 +279,8 @@ void SerialInterface::readData(float deltaTime) {
             }
         }
         
-        printLog("%g %g %g\n", _angularVelocityToLinearAccel[0][0], _angularVelocityToLinearAccel[1][0], _angularVelocityToLinearAccel[2][0]);
-        printLog("%g %g %g\n", _angularVelocityToLinearAccel[0][1], _angularVelocityToLinearAccel[1][1], _angularVelocityToLinearAccel[2][1]);
-        printLog("%g %g %g\n\n", _angularVelocityToLinearAccel[0][2], _angularVelocityToLinearAccel[1][2], _angularVelocityToLinearAccel[2][2]);
-        
-        printLog("%g %g %g\n", _angularAccelToLinearAccel[0][0], _angularAccelToLinearAccel[1][0], _angularAccelToLinearAccel[2][0]);
-        printLog("%g %g %g\n", _angularAccelToLinearAccel[0][1], _angularAccelToLinearAccel[1][1], _angularAccelToLinearAccel[2][1]);
-        printLog("%g %g %g\n\n", _angularAccelToLinearAccel[0][2], _angularAccelToLinearAccel[1][2], _angularAccelToLinearAccel[2][2]);
+        // rotate estimated acceleration into global rotation frame
+        _estimatedAcceleration = estimatedRotation * _estimatedAcceleration;
         
         //  Update estimated position and velocity
         float const DECAY_VELOCITY = 0.95f;

From b88380db6f01841aceded1cdaaf20758a6063fe1 Mon Sep 17 00:00:00 2001
From: Andrzej Kapolka <drzej.k@gmail.com>
Date: Tue, 11 Jun 2013 13:43:29 -0700
Subject: [PATCH 11/24] New values, slower learning.

---
 interface/src/SerialInterface.cpp | 2 +-
 interface/src/SerialInterface.h   | 8 ++++----
 2 files changed, 5 insertions(+), 5 deletions(-)

diff --git a/interface/src/SerialInterface.cpp b/interface/src/SerialInterface.cpp
index af9c35196b..be1e6328da 100644
--- a/interface/src/SerialInterface.cpp
+++ b/interface/src/SerialInterface.cpp
@@ -257,7 +257,7 @@ void SerialInterface::readData(float deltaTime) {
                 if (fabsf(error[i]) < EPSILON) {
                     continue;
                 }
-                const float LEARNING_RATE = 0.01f;
+                const float LEARNING_RATE = 0.001f;
                 float rateSum = fabsf(_lastRotationRates.x) + fabsf(_lastRotationRates.y) + fabsf(_lastRotationRates.z);
                 if (rateSum > EPSILON) {
                     for (int j = 0; j < 3; j++) {
diff --git a/interface/src/SerialInterface.h b/interface/src/SerialInterface.h
index 2bb7e541ac..b10fba84ce 100644
--- a/interface/src/SerialInterface.h
+++ b/interface/src/SerialInterface.h
@@ -32,12 +32,12 @@ public:
                         _lastAcceleration(0, 0, 0),
                         _lastRotationRates(0, 0, 0),
                         _angularVelocityToLinearAccel( // experimentally derived initial values
-                            0.001f, -0.008f, 0.020f, 
-                            0.003f, -0.003f, 0.025f,
-                            0.017f, 0.007f, 0.029f),
+                            0.003f, -0.001f, -0.006f, 
+                            -0.005f, -0.001f, -0.006f,
+                            0.010f, 0.004f, 0.007f),
                         _angularAccelToLinearAccel( // experimentally derived initial values
                             0.0f, 0.0f, 0.002f,
-                            0.0f, 0.0f, 0.002f,
+                            0.0f, 0.0f, 0.001f,
                             -0.002f, -0.002f, 0.0f)
                     {}
     

From 47bd3bcdd7cc7bb971441a24e353b08c3b30adb7 Mon Sep 17 00:00:00 2001
From: Andrzej Kapolka <drzej.k@gmail.com>
Date: Tue, 11 Jun 2013 14:03:49 -0700
Subject: [PATCH 12/24] Don't estimate acceleration before we know the gravity
 and don't update our matrices without acceleration.

---
 interface/src/SerialInterface.cpp | 7 +++++--
 1 file changed, 5 insertions(+), 2 deletions(-)

diff --git a/interface/src/SerialInterface.cpp b/interface/src/SerialInterface.cpp
index be1e6328da..2132f5a2de 100644
--- a/interface/src/SerialInterface.cpp
+++ b/interface/src/SerialInterface.cpp
@@ -240,10 +240,12 @@ void SerialInterface::readData(float deltaTime) {
             glm::quat(glm::radians(deltaTime * _lastRotationRates));
         
         //  Update acceleration estimate: first, subtract gravity as rotated into current frame
-        _estimatedAcceleration = _lastAcceleration - glm::inverse(estimatedRotation) * _gravity;
+        _estimatedAcceleration = (totalSamples < GRAVITY_SAMPLES) ? glm::vec3() :
+            _lastAcceleration - glm::inverse(estimatedRotation) * _gravity;
         
         //  Consider updating our angular velocity/acceleration to linear acceleration mapping
-        if (glm::length(_lastRotationRates) > EPSILON || glm::length(angularAcceleration) > EPSILON) {
+        if (glm::length(_estimatedAcceleration) > EPSILON &&
+                glm::length(_lastRotationRates) > EPSILON || glm::length(angularAcceleration) > EPSILON) {
             // compute predicted linear acceleration, find error between actual and predicted
             glm::vec3 predictedAcceleration = _angularVelocityToLinearAccel * _lastRotationRates +
                 _angularAccelToLinearAccel * angularAcceleration;
@@ -338,6 +340,7 @@ void SerialInterface::resetAverages() {
     _estimatedRotation = glm::vec3(0, 0, 0);
     _estimatedPosition = glm::vec3(0, 0, 0);
     _estimatedVelocity = glm::vec3(0, 0, 0);
+    _estimatedAcceleration = glm::vec3(0, 0, 0);
 }
 
 void SerialInterface::resetSerial() {

From 4b890e2ccc677ce05c695ba3ad6c976012d37892 Mon Sep 17 00:00:00 2001
From: Andrzej Kapolka <drzej.k@gmail.com>
Date: Tue, 11 Jun 2013 14:23:47 -0700
Subject: [PATCH 13/24] Let's see what the long-term average acceleration is.

---
 interface/src/SerialInterface.cpp | 7 ++++++-
 interface/src/SerialInterface.h   | 2 ++
 2 files changed, 8 insertions(+), 1 deletion(-)

diff --git a/interface/src/SerialInterface.cpp b/interface/src/SerialInterface.cpp
index 2132f5a2de..8e3cddf9bb 100644
--- a/interface/src/SerialInterface.cpp
+++ b/interface/src/SerialInterface.cpp
@@ -218,6 +218,11 @@ void SerialInterface::readData(float deltaTime) {
         
         _lastAcceleration = glm::vec3(-accelXRate, -accelYRate, -accelZRate) * LSB_TO_METERS_PER_SECOND2;
                 
+        _averageAcceleration = (1.f - 1.f/(float)LONG_TERM_RATE_SAMPLES) * _averageAcceleration +
+                1.f/(float)LONG_TERM_RATE_SAMPLES * _lastAcceleration;
+        
+        printLog("%g %g %g\n", _averageAcceleration.x, _averageAcceleration.y, _averageAcceleration.z);
+        
         int rollRate, yawRate, pitchRate;
         
         convertHexToInt(sensorBuffer + 22, rollRate);
@@ -245,7 +250,7 @@ void SerialInterface::readData(float deltaTime) {
         
         //  Consider updating our angular velocity/acceleration to linear acceleration mapping
         if (glm::length(_estimatedAcceleration) > EPSILON &&
-                glm::length(_lastRotationRates) > EPSILON || glm::length(angularAcceleration) > EPSILON) {
+                (glm::length(_lastRotationRates) > EPSILON || glm::length(angularAcceleration) > EPSILON)) {
             // compute predicted linear acceleration, find error between actual and predicted
             glm::vec3 predictedAcceleration = _angularVelocityToLinearAccel * _lastRotationRates +
                 _angularAccelToLinearAccel * angularAcceleration;
diff --git a/interface/src/SerialInterface.h b/interface/src/SerialInterface.h
index b10fba84ce..73f67fc618 100644
--- a/interface/src/SerialInterface.h
+++ b/interface/src/SerialInterface.h
@@ -73,6 +73,8 @@ private:
     glm::vec3 _lastAcceleration;
     glm::vec3 _lastRotationRates;
     
+    glm::vec3 _averageAcceleration;
+    
     glm::mat3 _angularVelocityToLinearAccel;
     glm::mat3 _angularAccelToLinearAccel;
 };

From d913f8dec03e419694108416add1f3902f6666de Mon Sep 17 00:00:00 2001
From: Andrzej Kapolka <drzej.k@gmail.com>
Date: Tue, 11 Jun 2013 14:25:05 -0700
Subject: [PATCH 14/24] Actually, we want it sans gravity.

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

diff --git a/interface/src/SerialInterface.cpp b/interface/src/SerialInterface.cpp
index 8e3cddf9bb..9933ba1129 100644
--- a/interface/src/SerialInterface.cpp
+++ b/interface/src/SerialInterface.cpp
@@ -218,10 +218,6 @@ void SerialInterface::readData(float deltaTime) {
         
         _lastAcceleration = glm::vec3(-accelXRate, -accelYRate, -accelZRate) * LSB_TO_METERS_PER_SECOND2;
                 
-        _averageAcceleration = (1.f - 1.f/(float)LONG_TERM_RATE_SAMPLES) * _averageAcceleration +
-                1.f/(float)LONG_TERM_RATE_SAMPLES * _lastAcceleration;
-        
-        printLog("%g %g %g\n", _averageAcceleration.x, _averageAcceleration.y, _averageAcceleration.z);
         
         int rollRate, yawRate, pitchRate;
         
@@ -248,6 +244,11 @@ void SerialInterface::readData(float deltaTime) {
         _estimatedAcceleration = (totalSamples < GRAVITY_SAMPLES) ? glm::vec3() :
             _lastAcceleration - glm::inverse(estimatedRotation) * _gravity;
         
+        _averageAcceleration = (1.f - 1.f/(float)LONG_TERM_RATE_SAMPLES) * _averageAcceleration +
+                1.f/(float)LONG_TERM_RATE_SAMPLES * _estimatedAcceleration;
+        
+        printLog("%g %g %g\n", _averageAcceleration.x, _averageAcceleration.y, _averageAcceleration.z);
+        
         //  Consider updating our angular velocity/acceleration to linear acceleration mapping
         if (glm::length(_estimatedAcceleration) > EPSILON &&
                 (glm::length(_lastRotationRates) > EPSILON || glm::length(angularAcceleration) > EPSILON)) {

From e77aee39676d2ffce770e402ed0356be1a5f6896 Mon Sep 17 00:00:00 2001
From: Andrzej Kapolka <drzej.k@gmail.com>
Date: Tue, 11 Jun 2013 15:47:05 -0700
Subject: [PATCH 15/24] Show the average rotation rates, too.

---
 interface/src/SerialInterface.cpp   | 7 ++++++-
 interface/src/SerialInterface.h     | 1 +
 libraries/shared/src/SharedUtil.cpp | 4 ++--
 3 files changed, 9 insertions(+), 3 deletions(-)

diff --git a/interface/src/SerialInterface.cpp b/interface/src/SerialInterface.cpp
index 9933ba1129..02c672ba8f 100644
--- a/interface/src/SerialInterface.cpp
+++ b/interface/src/SerialInterface.cpp
@@ -236,6 +236,11 @@ void SerialInterface::readData(float deltaTime) {
         glm::vec3 angularAcceleration = (deltaTime < EPSILON) ? glm::vec3() : (rotationRates - _lastRotationRates) / deltaTime;
         _lastRotationRates = rotationRates;
 
+        _averageRotationRates = (1.f - 1.f/(float)LONG_TERM_RATE_SAMPLES) * _averageRotationRates +
+                1.f/(float)LONG_TERM_RATE_SAMPLES * _lastRotationRates;
+        
+        printLog("r: %g %g %g\n", _averageRotationRates.x, _averageRotationRates.y, _averageRotationRates.z);
+
         //  Update raw rotation estimates
         glm::quat estimatedRotation = glm::quat(glm::radians(_estimatedRotation)) *
             glm::quat(glm::radians(deltaTime * _lastRotationRates));
@@ -247,7 +252,7 @@ void SerialInterface::readData(float deltaTime) {
         _averageAcceleration = (1.f - 1.f/(float)LONG_TERM_RATE_SAMPLES) * _averageAcceleration +
                 1.f/(float)LONG_TERM_RATE_SAMPLES * _estimatedAcceleration;
         
-        printLog("%g %g %g\n", _averageAcceleration.x, _averageAcceleration.y, _averageAcceleration.z);
+        printLog("a: %g %g %g\n", _averageAcceleration.x, _averageAcceleration.y, _averageAcceleration.z);
         
         //  Consider updating our angular velocity/acceleration to linear acceleration mapping
         if (glm::length(_estimatedAcceleration) > EPSILON &&
diff --git a/interface/src/SerialInterface.h b/interface/src/SerialInterface.h
index 73f67fc618..88af60ccbe 100644
--- a/interface/src/SerialInterface.h
+++ b/interface/src/SerialInterface.h
@@ -74,6 +74,7 @@ private:
     glm::vec3 _lastRotationRates;
     
     glm::vec3 _averageAcceleration;
+    glm::vec3 _averageRotationRates;
     
     glm::mat3 _angularVelocityToLinearAccel;
     glm::mat3 _angularAccelToLinearAccel;
diff --git a/libraries/shared/src/SharedUtil.cpp b/libraries/shared/src/SharedUtil.cpp
index 30aa75b461..697719b36d 100644
--- a/libraries/shared/src/SharedUtil.cpp
+++ b/libraries/shared/src/SharedUtil.cpp
@@ -102,12 +102,12 @@ void setAtBit(unsigned char& byte, int bitIndex) {
 }
 
 int  getSemiNibbleAt(unsigned char& byte, int bitIndex) {
-    return (byte >> (7 - bitIndex) & 3); // semi-nibbles store 00, 01, 10, or 11
+    return (byte >> (6 - bitIndex) & 3); // semi-nibbles store 00, 01, 10, or 11
 }
 
 void setSemiNibbleAt(unsigned char& byte, int bitIndex, int value) {
     //assert(value <= 3 && value >= 0);
-    byte += ((value & 3) << (7 - bitIndex)); // semi-nibbles store 00, 01, 10, or 11
+    byte += ((value & 3) << (6 - bitIndex)); // semi-nibbles store 00, 01, 10, or 11
 }
 
 

From 090df81afebb29d8c95a2141cab728f9476f3d22 Mon Sep 17 00:00:00 2001
From: Andrzej Kapolka <drzej.k@gmail.com>
Date: Tue, 11 Jun 2013 15:58:50 -0700
Subject: [PATCH 16/24] Fixes to align better with previous code.

---
 interface/src/SerialInterface.cpp | 16 +++++++++-------
 interface/src/SerialInterface.h   |  7 ++++---
 2 files changed, 13 insertions(+), 10 deletions(-)

diff --git a/interface/src/SerialInterface.cpp b/interface/src/SerialInterface.cpp
index 02c672ba8f..86755e28b5 100644
--- a/interface/src/SerialInterface.cpp
+++ b/interface/src/SerialInterface.cpp
@@ -232,15 +232,15 @@ void SerialInterface::readData(float deltaTime) {
         rotationRates[1] = ((float) -yawRate) * LSB_TO_DEGREES_PER_SECOND;
         rotationRates[2] = ((float) -rollRate) * LSB_TO_DEGREES_PER_SECOND;
 
+        // update and subtract the long term average
+        _averageRotationRates = (1.f - 1.f/(float)LONG_TERM_RATE_SAMPLES) * _averageRotationRates +
+                1.f/(float)LONG_TERM_RATE_SAMPLES * rotationRates;
+        rotationRates -= _averageRotationRates;
+
         // compute the angular acceleration
         glm::vec3 angularAcceleration = (deltaTime < EPSILON) ? glm::vec3() : (rotationRates - _lastRotationRates) / deltaTime;
         _lastRotationRates = rotationRates;
-
-        _averageRotationRates = (1.f - 1.f/(float)LONG_TERM_RATE_SAMPLES) * _averageRotationRates +
-                1.f/(float)LONG_TERM_RATE_SAMPLES * _lastRotationRates;
         
-        printLog("r: %g %g %g\n", _averageRotationRates.x, _averageRotationRates.y, _averageRotationRates.z);
-
         //  Update raw rotation estimates
         glm::quat estimatedRotation = glm::quat(glm::radians(_estimatedRotation)) *
             glm::quat(glm::radians(deltaTime * _lastRotationRates));
@@ -249,10 +249,10 @@ void SerialInterface::readData(float deltaTime) {
         _estimatedAcceleration = (totalSamples < GRAVITY_SAMPLES) ? glm::vec3() :
             _lastAcceleration - glm::inverse(estimatedRotation) * _gravity;
         
+        // update and subtract the long term average
         _averageAcceleration = (1.f - 1.f/(float)LONG_TERM_RATE_SAMPLES) * _averageAcceleration +
                 1.f/(float)LONG_TERM_RATE_SAMPLES * _estimatedAcceleration;
-        
-        printLog("a: %g %g %g\n", _averageAcceleration.x, _averageAcceleration.y, _averageAcceleration.z);
+        _estimatedAcceleration -= _averageAcceleration;
         
         //  Consider updating our angular velocity/acceleration to linear acceleration mapping
         if (glm::length(_estimatedAcceleration) > EPSILON &&
@@ -347,6 +347,8 @@ void SerialInterface::readData(float deltaTime) {
 void SerialInterface::resetAverages() {
     totalSamples = 0;
     _gravity = 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);
diff --git a/interface/src/SerialInterface.h b/interface/src/SerialInterface.h
index 88af60ccbe..8c918e65ff 100644
--- a/interface/src/SerialInterface.h
+++ b/interface/src/SerialInterface.h
@@ -26,6 +26,8 @@ class SerialInterface {
 public:
     SerialInterface() : active(false),
                         _gravity(0, 0, 0),
+                        _averageRotationRates(0, 0, 0),
+                        _averageAcceleration(0, 0, 0),
                         _estimatedRotation(0, 0, 0),
                         _estimatedPosition(0, 0, 0),
                         _estimatedVelocity(0, 0, 0),
@@ -66,6 +68,8 @@ private:
     int totalSamples;
     timeval lastGoodRead;
     glm::vec3 _gravity;
+    glm::vec3 _averageRotationRates;
+    glm::vec3 _averageAcceleration;
     glm::vec3 _estimatedRotation;
     glm::vec3 _estimatedPosition;
     glm::vec3 _estimatedVelocity;
@@ -73,9 +77,6 @@ private:
     glm::vec3 _lastAcceleration;
     glm::vec3 _lastRotationRates;
     
-    glm::vec3 _averageAcceleration;
-    glm::vec3 _averageRotationRates;
-    
     glm::mat3 _angularVelocityToLinearAccel;
     glm::mat3 _angularAccelToLinearAccel;
 };

From ee526265a1c3b8fd40895651785968319dc9f1d3 Mon Sep 17 00:00:00 2001
From: Andrzej Kapolka <drzej.k@gmail.com>
Date: Tue, 11 Jun 2013 16:00:14 -0700
Subject: [PATCH 17/24] Fix for nibble packing.

---
 libraries/shared/src/SharedUtil.cpp | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/libraries/shared/src/SharedUtil.cpp b/libraries/shared/src/SharedUtil.cpp
index 30aa75b461..697719b36d 100644
--- a/libraries/shared/src/SharedUtil.cpp
+++ b/libraries/shared/src/SharedUtil.cpp
@@ -102,12 +102,12 @@ void setAtBit(unsigned char& byte, int bitIndex) {
 }
 
 int  getSemiNibbleAt(unsigned char& byte, int bitIndex) {
-    return (byte >> (7 - bitIndex) & 3); // semi-nibbles store 00, 01, 10, or 11
+    return (byte >> (6 - bitIndex) & 3); // semi-nibbles store 00, 01, 10, or 11
 }
 
 void setSemiNibbleAt(unsigned char& byte, int bitIndex, int value) {
     //assert(value <= 3 && value >= 0);
-    byte += ((value & 3) << (7 - bitIndex)); // semi-nibbles store 00, 01, 10, or 11
+    byte += ((value & 3) << (6 - bitIndex)); // semi-nibbles store 00, 01, 10, or 11
 }
 
 

From 35e18abcbd7e6d1d04547001657d2bf33c85d8c6 Mon Sep 17 00:00:00 2001
From: Andrzej Kapolka <drzej.k@gmail.com>
Date: Tue, 11 Jun 2013 16:27:39 -0700
Subject: [PATCH 18/24] Let's try using estimated velocity to control "lean."

---
 interface/src/Avatar.cpp | 14 ++------------
 1 file changed, 2 insertions(+), 12 deletions(-)

diff --git a/interface/src/Avatar.cpp b/interface/src/Avatar.cpp
index 32b35aebac..9a59e2b804 100644
--- a/interface/src/Avatar.cpp
+++ b/interface/src/Avatar.cpp
@@ -285,18 +285,8 @@ void Avatar::updateHeadFromGyros(float deltaTime, SerialInterface* serialInterfa
     _head.setRoll(estimatedRotation.z * AMPLIFY_ROLL);
         
     //  Update head lean distance based on accelerometer data
-    glm::vec3 headRotationRates(_head.getPitch(), _head.getYaw(), _head.getRoll());
-    
-    glm::vec3 leaning = (serialInterface->getLastAcceleration() - serialInterface->getGravity())
-    * LEAN_SENSITIVITY
-    * (1.f - fminf(glm::length(headRotationRates), HEAD_RATE_MAX) / HEAD_RATE_MAX);
-    leaning.y = 0.f;
-    if (glm::length(leaning) < LEAN_MAX) {
-        _head.setLeanForward(_head.getLeanForward() * (1.f - LEAN_AVERAGING * deltaTime) +
-                             (LEAN_AVERAGING * deltaTime) * leaning.z * LEAN_SENSITIVITY);
-        _head.setLeanSideways(_head.getLeanSideways() * (1.f - LEAN_AVERAGING * deltaTime) +
-                              (LEAN_AVERAGING * deltaTime) * leaning.x * LEAN_SENSITIVITY);
-    }
+    _bodyPitchDelta = serialInterface->getEstimatedVelocity().z * LEAN_SENSITIVITY;
+    _bodyRollDelta = -serialInterface->getEstimatedVelocity().x * LEAN_SENSITIVITY;
 }
 
 float Avatar::getAbsoluteHeadYaw() const {

From c1c2439d2ca9f91d83e69556e71c21ed8f6d7d51 Mon Sep 17 00:00:00 2001
From: Andrzej Kapolka <drzej.k@gmail.com>
Date: Tue, 11 Jun 2013 16:44:21 -0700
Subject: [PATCH 19/24] Let's try changing the torso rotation based on the
 gyros!

---
 interface/src/Avatar.cpp   | 10 +++++-----
 interface/src/Skeleton.cpp |  6 +++---
 interface/src/Skeleton.h   |  1 +
 3 files changed, 9 insertions(+), 8 deletions(-)

diff --git a/interface/src/Avatar.cpp b/interface/src/Avatar.cpp
index 9a59e2b804..43056244ff 100644
--- a/interface/src/Avatar.cpp
+++ b/interface/src/Avatar.cpp
@@ -280,13 +280,13 @@ void Avatar::updateHeadFromGyros(float deltaTime, SerialInterface* serialInterfa
     const float AMPLIFY_ROLL = 2.f;
 
     glm::vec3 estimatedRotation = serialInterface->getEstimatedRotation();
-    _head.setPitch(estimatedRotation.x * AMPLIFY_PITCH);
-    _head.setYaw(estimatedRotation.y * AMPLIFY_YAW);
-    _head.setRoll(estimatedRotation.z * AMPLIFY_ROLL);
+    //_head.setPitch(estimatedRotation.x * AMPLIFY_PITCH);
+    //_head.setYaw(estimatedRotation.y * AMPLIFY_YAW);
+    //_head.setRoll(estimatedRotation.z * AMPLIFY_ROLL);
         
     //  Update head lean distance based on accelerometer data
-    _bodyPitchDelta = serialInterface->getEstimatedVelocity().z * LEAN_SENSITIVITY;
-    _bodyRollDelta = -serialInterface->getEstimatedVelocity().x * LEAN_SENSITIVITY;
+    glm::vec3 estimatedPosition = serialInterface->getEstimatedPosition();
+    _skeleton.joint[AVATAR_JOINT_TORSO].rotation = glm::quat(glm::radians(estimatedRotation));
 }
 
 float Avatar::getAbsoluteHeadYaw() const {
diff --git a/interface/src/Skeleton.cpp b/interface/src/Skeleton.cpp
index 2fa43c9010..fcec90b542 100644
--- a/interface/src/Skeleton.cpp
+++ b/interface/src/Skeleton.cpp
@@ -132,15 +132,15 @@ void Skeleton::update(float deltaTime, const glm::quat& orientation, glm::vec3 p
 
     for (int b = 0; b < NUM_AVATAR_JOINTS; b++) {
         if (joint[b].parent == AVATAR_JOINT_NULL) {
-            joint[b].rotation = orientation;
+            joint[b].absoluteRotation = orientation * joint[b].rotation;
             joint[b].position = position;
         }
         else {
-            joint[b].rotation = joint[ joint[b].parent ].rotation;
+            joint[b].absoluteRotation = joint[ joint[b].parent ].absoluteRotation * joint[b].rotation;
             joint[b].position = joint[ joint[b].parent ].position;
         }
 
-        glm::vec3 rotatedJointVector = joint[b].rotation * joint[b].defaultPosePosition;
+        glm::vec3 rotatedJointVector = joint[b].absoluteRotation * joint[b].defaultPosePosition;
         joint[b].position += rotatedJointVector;
     }    
 }
diff --git a/interface/src/Skeleton.h b/interface/src/Skeleton.h
index e98c2e7b12..ec40f18cb3 100644
--- a/interface/src/Skeleton.h
+++ b/interface/src/Skeleton.h
@@ -68,6 +68,7 @@ public:
         glm::quat     absoluteBindPoseRotation;  // the absolute rotation when the avatar is in the "T-pose"
         float         bindRadius;                // the radius of the bone capsule that envelops the vertices to bind
         glm::quat     rotation;                  // the parent-relative rotation (orientation) of the joint as a quaternion
+        glm::quat     absoluteRotation;          // the absolute rotation of the joint as a quaternion
         float         length;                    // the length of vector connecting the joint and its parent
     };
 

From 70c5a941d7d9118aab8b86e5450aaeaf6d133d39 Mon Sep 17 00:00:00 2001
From: Andrzej Kapolka <drzej.k@gmail.com>
Date: Tue, 11 Jun 2013 17:01:06 -0700
Subject: [PATCH 20/24] Now let's try using the estimated position to calculate
 lean.

---
 interface/src/Avatar.cpp | 15 ++++++++++-----
 1 file changed, 10 insertions(+), 5 deletions(-)

diff --git a/interface/src/Avatar.cpp b/interface/src/Avatar.cpp
index 43056244ff..5547fe5e43 100644
--- a/interface/src/Avatar.cpp
+++ b/interface/src/Avatar.cpp
@@ -280,13 +280,18 @@ void Avatar::updateHeadFromGyros(float deltaTime, SerialInterface* serialInterfa
     const float AMPLIFY_ROLL = 2.f;
 
     glm::vec3 estimatedRotation = serialInterface->getEstimatedRotation();
-    //_head.setPitch(estimatedRotation.x * AMPLIFY_PITCH);
-    //_head.setYaw(estimatedRotation.y * AMPLIFY_YAW);
-    //_head.setRoll(estimatedRotation.z * AMPLIFY_ROLL);
+    _head.setPitch(estimatedRotation.x * AMPLIFY_PITCH);
+    _head.setYaw(estimatedRotation.y * AMPLIFY_YAW);
+    _head.setRoll(estimatedRotation.z * AMPLIFY_ROLL);
         
-    //  Update head lean distance based on accelerometer data
+    //  Update torso lean distance based on accelerometer data
     glm::vec3 estimatedPosition = serialInterface->getEstimatedPosition();
-    _skeleton.joint[AVATAR_JOINT_TORSO].rotation = glm::quat(glm::radians(estimatedRotation));
+    const float TORSO_LENGTH = 0.5f;
+    const float MAX_LEAN_RADIANS = PIf / 4;
+    _skeleton.joint[AVATAR_JOINT_TORSO].rotation = glm::quat(glm::vec3(
+        glm::clamp(atanf(estimatedPosition.z / TORSO_LENGTH), -MAX_LEAN_RADIANS, MAX_LEAN_RADIANS),
+        0.0f,
+        glm::clamp(atanf(-estimatedPosition.x / TORSO_LENGTH), -MAX_LEAN_RADIANS, MAX_LEAN_RADIANS)));
 }
 
 float Avatar::getAbsoluteHeadYaw() const {

From 648aacb53d8f6ce0075b44c64b45aa2a5bedbdcf Mon Sep 17 00:00:00 2001
From: Andrzej Kapolka <drzej.k@gmail.com>
Date: Tue, 11 Jun 2013 17:26:41 -0700
Subject: [PATCH 21/24] Let's use a fixed vector for the camera position so
 that the target doesn't move with lean.

---
 interface/src/Application.cpp     | 6 +++---
 interface/src/Avatar.cpp          | 6 ++++++
 interface/src/Avatar.h            | 3 +++
 interface/src/SerialInterface.cpp | 4 ++--
 interface/src/Skeleton.cpp        | 7 +++++++
 interface/src/Skeleton.h          | 1 +
 6 files changed, 22 insertions(+), 5 deletions(-)

diff --git a/interface/src/Application.cpp b/interface/src/Application.cpp
index 51c6e65cb9..489150c8a1 100644
--- a/interface/src/Application.cpp
+++ b/interface/src/Application.cpp
@@ -308,7 +308,7 @@ void Application::paintGL() {
     
     if (_myCamera.getMode() == CAMERA_MODE_MIRROR) {
         _myCamera.setTightness     (100.0f); 
-        _myCamera.setTargetPosition(_myAvatar.getBallPosition(AVATAR_JOINT_HEAD_BASE));
+        _myCamera.setTargetPosition(_myAvatar.getUprightHeadPosition());
         _myCamera.setTargetRotation(_myAvatar.getWorldAlignedOrientation() * glm::quat(glm::vec3(0.0f, PIf, 0.0f)));
         
     } else if (OculusManager::isConnected()) {
@@ -320,11 +320,11 @@ void Application::paintGL() {
     
     } else if (_myCamera.getMode() == CAMERA_MODE_FIRST_PERSON) {
         _myCamera.setTightness(0.0f);  //  In first person, camera follows head exactly without delay
-        _myCamera.setTargetPosition(_myAvatar.getBallPosition(AVATAR_JOINT_HEAD_BASE));
+        _myCamera.setTargetPosition(_myAvatar.getUprightHeadPosition());
         _myCamera.setTargetRotation(_myAvatar.getHead().getCameraOrientation(_headCameraPitchYawScale));
         
     } else if (_myCamera.getMode() == CAMERA_MODE_THIRD_PERSON) {
-        _myCamera.setTargetPosition(_myAvatar.getHeadJointPosition());
+        _myCamera.setTargetPosition(_myAvatar.getUprightHeadPosition());
         _myCamera.setTargetRotation(_myAvatar.getHead().getCameraOrientation(_headCameraPitchYawScale));
     }
     
diff --git a/interface/src/Avatar.cpp b/interface/src/Avatar.cpp
index 5547fe5e43..7d6e0aad37 100644
--- a/interface/src/Avatar.cpp
+++ b/interface/src/Avatar.cpp
@@ -103,9 +103,11 @@ Avatar::Avatar(Agent* owningAgent) :
     initializeBodyBalls();
     
     _height               = _skeleton.getHeight() + _bodyBall[ BODY_BALL_LEFT_HEEL ].radius + _bodyBall[ BODY_BALL_HEAD_BASE ].radius;
+    
     _maxArmLength         = _skeleton.getArmLength();
     _pelvisStandingHeight = _skeleton.getPelvisStandingHeight() + _bodyBall[ BODY_BALL_LEFT_HEEL ].radius;
     _pelvisFloatingHeight = _skeleton.getPelvisFloatingHeight() + _bodyBall[ BODY_BALL_LEFT_HEEL ].radius;
+    _pelvisToHeadLength   = _skeleton.getPelvisToHeadLength();
     
     _avatarTouch.setReachableRadius(PERIPERSONAL_RADIUS);
     
@@ -310,6 +312,10 @@ glm::quat Avatar::getWorldAlignedOrientation () const {
     return computeRotationFromBodyToWorldUp() * getOrientation();
 }
 
+glm::vec3 Avatar::getUprightHeadPosition() const {
+    return _position + getWorldAlignedOrientation() * glm::vec3(0.0f, _pelvisToHeadLength, 0.0f);
+}
+
 void  Avatar::updateFromMouse(int mouseX, int mouseY, int screenWidth, int screenHeight) {
     //  Update head yaw and pitch based on mouse input
     const float MOUSE_MOVE_RADIUS = 0.3f;
diff --git a/interface/src/Avatar.h b/interface/src/Avatar.h
index 0fc5b31286..82d98a3a02 100644
--- a/interface/src/Avatar.h
+++ b/interface/src/Avatar.h
@@ -121,6 +121,8 @@ public:
     glm::quat        getOrientation            () const;
     glm::quat        getWorldAlignedOrientation() const;
     
+    glm::vec3 getUprightHeadPosition() const;
+    
     AvatarVoxelSystem* getVoxels() { return &_voxels; }
     
     //  Set what driving keys are being pressed to control thrust levels
@@ -185,6 +187,7 @@ private:
     int         _driveKeys[MAX_DRIVE_KEYS];
     float       _pelvisStandingHeight;
     float       _pelvisFloatingHeight;
+    float       _pelvisToHeadLength;
     float       _height;
     Balls*      _balls;
     AvatarTouch _avatarTouch;
diff --git a/interface/src/SerialInterface.cpp b/interface/src/SerialInterface.cpp
index 86755e28b5..76487bb668 100644
--- a/interface/src/SerialInterface.cpp
+++ b/interface/src/SerialInterface.cpp
@@ -296,8 +296,8 @@ void SerialInterface::readData(float deltaTime) {
         _estimatedAcceleration = estimatedRotation * _estimatedAcceleration;
         
         //  Update estimated position and velocity
-        float const DECAY_VELOCITY = 0.95f;
-        float const DECAY_POSITION = 0.95f;
+        float const DECAY_VELOCITY = 0.975f;
+        float const DECAY_POSITION = 0.975f;
         _estimatedVelocity += deltaTime * _estimatedAcceleration;
         _estimatedPosition += deltaTime * _estimatedVelocity;
         _estimatedVelocity *= DECAY_VELOCITY;
diff --git a/interface/src/Skeleton.cpp b/interface/src/Skeleton.cpp
index fcec90b542..1167194534 100644
--- a/interface/src/Skeleton.cpp
+++ b/interface/src/Skeleton.cpp
@@ -174,6 +174,13 @@ float Skeleton::getPelvisFloatingHeight() {
            FLOATING_HEIGHT;
 }
 
+float Skeleton::getPelvisToHeadLength() {
+    return 
+        joint[ AVATAR_JOINT_TORSO     ].length +
+        joint[ AVATAR_JOINT_CHEST     ].length +
+        joint[ AVATAR_JOINT_NECK_BASE ].length +
+        joint[ AVATAR_JOINT_HEAD_BASE ].length; 
+}
 
 
 
diff --git a/interface/src/Skeleton.h b/interface/src/Skeleton.h
index ec40f18cb3..bb953fe947 100644
--- a/interface/src/Skeleton.h
+++ b/interface/src/Skeleton.h
@@ -56,6 +56,7 @@ public:
     float getHeight();
     float getPelvisStandingHeight();
     float getPelvisFloatingHeight();
+    float getPelvisToHeadLength();
     //glm::vec3 getJointVectorFromParent(AvatarJointID jointID) {return joint[jointID].position - joint[joint[jointID].parent].position; }
     
     struct AvatarJoint

From 82f03ffd8eba4d7413da0490d880cf93e412b63d Mon Sep 17 00:00:00 2001
From: Andrzej Kapolka <drzej.k@gmail.com>
Date: Tue, 11 Jun 2013 17:53:49 -0700
Subject: [PATCH 22/24] Don't render the voxels when we're too close (i.e., in
 first person mode).

---
 interface/src/Avatar.cpp | 32 ++++++++++++++++----------------
 interface/src/Avatar.h   |  1 +
 2 files changed, 17 insertions(+), 16 deletions(-)

diff --git a/interface/src/Avatar.cpp b/interface/src/Avatar.cpp
index 7d6e0aad37..1b219e440e 100644
--- a/interface/src/Avatar.cpp
+++ b/interface/src/Avatar.cpp
@@ -1131,32 +1131,29 @@ glm::quat Avatar::computeRotationFromBodyToWorldUp(float proportion) const {
     return glm::angleAxis(angle * proportion, axis);
 }
 
-void Avatar::renderBody(bool lookingInMirror, bool renderAvatarBalls) {
-    
+float Avatar::getBallRenderAlpha(int ball, bool lookingInMirror) const {
     const float RENDER_OPAQUE_BEYOND = 1.0f;        //  Meters beyond which body is shown opaque
     const float RENDER_TRANSLUCENT_BEYOND = 0.5f;
+    float distanceToCamera = glm::length(_cameraPosition - _bodyBall[ball].position);
+    return (lookingInMirror || _owningAgent) ? 1.0f : glm::clamp(
+        (distanceToCamera - RENDER_TRANSLUCENT_BEYOND) / (RENDER_OPAQUE_BEYOND - RENDER_TRANSLUCENT_BEYOND), 0.f, 1.f);
+}
+
+void Avatar::renderBody(bool lookingInMirror, bool renderAvatarBalls) {
+    
+    
     
     //  Render the body as balls and cones
     if (renderAvatarBalls || !_voxels.getVoxelURL().isValid()) {
         for (int b = 0; b < NUM_AVATAR_BODY_BALLS; b++) {
-            float distanceToCamera = glm::length(_cameraPosition - _bodyBall[b].position);
-            
-            float alpha = lookingInMirror ? 1.0f : glm::clamp((distanceToCamera - RENDER_TRANSLUCENT_BEYOND) /
-                                                              (RENDER_OPAQUE_BEYOND - RENDER_TRANSLUCENT_BEYOND), 0.f, 1.f);
-            
-            if (lookingInMirror || _owningAgent) {
-                alpha = 1.0f;
-            }
+            float alpha = getBallRenderAlpha(b, lookingInMirror);
             
             //  Always render other people, and render myself when beyond threshold distance
             if (b == BODY_BALL_HEAD_BASE) { // the head is rendered as a special
-                if (lookingInMirror || _owningAgent || distanceToCamera > RENDER_OPAQUE_BEYOND * 0.5) {
+                if (alpha > 0.0f) {
                     _head.render(lookingInMirror, _cameraPosition, alpha);
                 }
-            } else if (_owningAgent || distanceToCamera > RENDER_TRANSLUCENT_BEYOND
-                       || b == BODY_BALL_RIGHT_ELBOW
-                       || b == BODY_BALL_RIGHT_WRIST
-                       || b == BODY_BALL_RIGHT_FINGERTIPS ) {
+            } else if (alpha > 0.0f) {
                 //  Render the body ball sphere
                 if (_owningAgent || b == BODY_BALL_RIGHT_ELBOW
                     || b == BODY_BALL_RIGHT_WRIST
@@ -1208,7 +1205,10 @@ void Avatar::renderBody(bool lookingInMirror, bool renderAvatarBalls) {
         }
     } else {
         //  Render the body's voxels
-        _voxels.render(false);
+        float alpha = getBallRenderAlpha(BODY_BALL_HEAD_BASE, lookingInMirror);
+        if (alpha > 0.0f) {
+            _voxels.render(false);
+        }
     }
 }
 
diff --git a/interface/src/Avatar.h b/interface/src/Avatar.h
index 82d98a3a02..de76092328 100644
--- a/interface/src/Avatar.h
+++ b/interface/src/Avatar.h
@@ -204,6 +204,7 @@ private:
     // private methods...
     glm::vec3 caclulateAverageEyePosition() { return _head.caclulateAverageEyePosition(); } // get the position smack-dab between the eyes (for lookat)
     glm::quat computeRotationFromBodyToWorldUp(float proportion = 1.0f) const;
+    float getBallRenderAlpha(int ball, bool lookingInMirror) const;
     void renderBody(bool lookingInMirror, bool renderAvatarBalls);
     void initializeBodyBalls();
     void resetBodyBalls();

From 7ef2a2c5d98af51de1f001b396fb57b29c4680d9 Mon Sep 17 00:00:00 2001
From: Andrzej Kapolka <drzej.k@gmail.com>
Date: Tue, 11 Jun 2013 18:06:05 -0700
Subject: [PATCH 23/24] Set/get lean to/from network data.

---
 interface/src/Avatar.cpp | 12 +++++++-----
 1 file changed, 7 insertions(+), 5 deletions(-)

diff --git a/interface/src/Avatar.cpp b/interface/src/Avatar.cpp
index 1b219e440e..e33624467e 100644
--- a/interface/src/Avatar.cpp
+++ b/interface/src/Avatar.cpp
@@ -289,11 +289,9 @@ void Avatar::updateHeadFromGyros(float deltaTime, SerialInterface* serialInterfa
     //  Update torso lean distance based on accelerometer data
     glm::vec3 estimatedPosition = serialInterface->getEstimatedPosition();
     const float TORSO_LENGTH = 0.5f;
-    const float MAX_LEAN_RADIANS = PIf / 4;
-    _skeleton.joint[AVATAR_JOINT_TORSO].rotation = glm::quat(glm::vec3(
-        glm::clamp(atanf(estimatedPosition.z / TORSO_LENGTH), -MAX_LEAN_RADIANS, MAX_LEAN_RADIANS),
-        0.0f,
-        glm::clamp(atanf(-estimatedPosition.x / TORSO_LENGTH), -MAX_LEAN_RADIANS, MAX_LEAN_RADIANS)));
+    const float MAX_LEAN = 45.0f;
+    _head.setLeanSideways(glm::clamp(glm::degrees(atanf(-estimatedPosition.x / TORSO_LENGTH)), -MAX_LEAN, MAX_LEAN));
+    _head.setLeanForward(glm::clamp(glm::degrees(atanf(estimatedPosition.z / TORSO_LENGTH)), -MAX_LEAN, MAX_LEAN));
 }
 
 float Avatar::getAbsoluteHeadYaw() const {
@@ -355,6 +353,10 @@ void Avatar::simulate(float deltaTime, Transmitter* transmitter) {
     // update balls
     if (_balls) { _balls->simulate(deltaTime); }
     
+    // update torso rotation based on head lean
+    _skeleton.joint[AVATAR_JOINT_TORSO].rotation = glm::quat(glm::radians(glm::vec3(
+        _head.getLeanForward(), 0.0f, _head.getLeanSideways())));
+    
 	// update avatar skeleton
     _skeleton.update(deltaTime, getOrientation(), _position);
     

From c58eb0d5841ebf0f0b7548b29ef3f51cfdc3d717 Mon Sep 17 00:00:00 2001
From: Andrzej Kapolka <drzej.k@gmail.com>
Date: Tue, 11 Jun 2013 18:11:42 -0700
Subject: [PATCH 24/24] Bump the maximum voxels up to 10000.

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

diff --git a/interface/src/AvatarVoxelSystem.cpp b/interface/src/AvatarVoxelSystem.cpp
index 8a7708587f..dc3b937da9 100644
--- a/interface/src/AvatarVoxelSystem.cpp
+++ b/interface/src/AvatarVoxelSystem.cpp
@@ -17,7 +17,7 @@
 #include "renderer/ProgramObject.h"
 
 const float AVATAR_TREE_SCALE = 1.0f;
-const int MAX_VOXELS_PER_AVATAR = 2000;
+const int MAX_VOXELS_PER_AVATAR = 10000;
 const int BONE_ELEMENTS_PER_VOXEL = BONE_ELEMENTS_PER_VERTEX * VERTICES_PER_VOXEL;
 
 AvatarVoxelSystem::AvatarVoxelSystem(Avatar* avatar) :