From e4bf5358eb645577b7b682a59d34b29d880976a0 Mon Sep 17 00:00:00 2001
From: Andrzej Kapolka <drzej.k@gmail.com>
Date: Fri, 1 Nov 2013 17:58:41 -0700
Subject: [PATCH 01/17] Attempt to use Faceshift eye data to deflect the
 default eye behavior.

---
 interface/src/Application.cpp | 43 ++++++++++++++++-------------------
 1 file changed, 19 insertions(+), 24 deletions(-)

diff --git a/interface/src/Application.cpp b/interface/src/Application.cpp
index d1d48a15e2..236d914b09 100644
--- a/interface/src/Application.cpp
+++ b/interface/src/Application.cpp
@@ -1941,33 +1941,28 @@ void Application::update(float deltaTime) {
         _myAvatar.getHead().setAngularVelocity(_faceshift.getHeadAngularVelocity());
     }
 
-    // if we have faceshift, use that to compute the lookat direction
-    glm::vec3 lookAtRayOrigin = mouseRayOrigin, lookAtRayDirection = mouseRayDirection;
-    if (_faceshift.isActive()) {
-        lookAtRayOrigin = _myAvatar.getHead().calculateAverageEyePosition();
-        lookAtRayDirection = _myAvatar.getHead().getOrientation() * glm::quat(glm::radians(glm::vec3(
-            _faceshift.getEstimatedEyePitch(), _faceshift.getEstimatedEyeYaw(), 0.0f))) * glm::vec3(0.0f, 0.0f, -1.0f);
-    }
-
     updateLookatTargetAvatar(mouseRayOrigin, mouseRayDirection, lookAtSpot);
-    if (_lookatTargetAvatar && !_faceshift.isActive()) {
-        // If the mouse is over another avatar's head...
-         _myAvatar.getHead().setLookAtPosition(lookAtSpot);
-    
-    } else if (_isHoverVoxel && !_faceshift.isActive()) {
-        //  Look at the hovered voxel
-        lookAtSpot = getMouseVoxelWorldCoordinates(_hoverVoxel);
-        _myAvatar.getHead().setLookAtPosition(lookAtSpot);
+    if (!_lookatTargetAvatar) {
+        if (_isHoverVoxel) {
+            //  Look at the hovered voxel
+            lookAtSpot = getMouseVoxelWorldCoordinates(_hoverVoxel);
         
-    } else if (_myCamera.getMode() == CAMERA_MODE_MIRROR && !_faceshift.isActive()) {
-        _myAvatar.getHead().setLookAtPosition(_myCamera.getPosition());
-    
-    } else {
-        //  Just look in direction of the mouse ray
-        const float FAR_AWAY_STARE = TREE_SCALE;
-        lookAtSpot = lookAtRayOrigin + lookAtRayDirection * FAR_AWAY_STARE;
-        _myAvatar.getHead().setLookAtPosition(lookAtSpot);
+        } else if (_myCamera.getMode() == CAMERA_MODE_MIRROR) {
+            lookAtSpot = _myCamera.getPosition();
+        
+        } else {
+            //  Just look in direction of the mouse ray
+            const float FAR_AWAY_STARE = TREE_SCALE;
+            lookAtSpot = mouseRayOrigin + mouseRayDirection * FAR_AWAY_STARE;
+        }
     }
+    if (_faceshift.isActive()) {
+        glm::vec3 origin = _myAvatar.getHead().calculateAverageEyePosition();
+        lookAtSpot = origin + _myCamera.getRotation() * _faceshift.getHeadRotation() * glm::quat(glm::radians(glm::vec3(
+            _faceshift.getEstimatedEyePitch(), _faceshift.getEstimatedEyeYaw(), 0.0f))) *
+                glm::inverse(_myCamera.getRotation()) * (lookAtSpot - origin);
+    }
+    _myAvatar.getHead().setLookAtPosition(lookAtSpot);
 
     //  Find the voxel we are hovering over, and respond if clicked
     float distance;

From a7683fa07f2569834f3a08b99c70ae73a00a53d8 Mon Sep 17 00:00:00 2001
From: Andrzej Kapolka <drzej.k@gmail.com>
Date: Fri, 1 Nov 2013 18:15:43 -0700
Subject: [PATCH 02/17] More tweaks to eye offsets.

---
 interface/src/Application.cpp       |  5 ++++-
 interface/src/devices/Faceshift.cpp | 13 +++++++++++--
 interface/src/devices/Faceshift.h   |  1 +
 3 files changed, 16 insertions(+), 3 deletions(-)

diff --git a/interface/src/Application.cpp b/interface/src/Application.cpp
index 236d914b09..54c903f569 100644
--- a/interface/src/Application.cpp
+++ b/interface/src/Application.cpp
@@ -1958,8 +1958,11 @@ void Application::update(float deltaTime) {
     }
     if (_faceshift.isActive()) {
         glm::vec3 origin = _myAvatar.getHead().calculateAverageEyePosition();
+        float pitchSign = (_myCamera.getMode() == CAMERA_MODE_MIRROR) ? -1.0f : 1.0f;
+        const float PITCH_SCALE = 0.25f;
+        const float YAW_SCALE = 0.25f;
         lookAtSpot = origin + _myCamera.getRotation() * _faceshift.getHeadRotation() * glm::quat(glm::radians(glm::vec3(
-            _faceshift.getEstimatedEyePitch(), _faceshift.getEstimatedEyeYaw(), 0.0f))) *
+            _faceshift.getEstimatedEyePitch() * pitchSign * PITCH_SCALE, _faceshift.getEstimatedEyeYaw() * YAW_SCALE, 0.0f))) *
                 glm::inverse(_myCamera.getRotation()) * (lookAtSpot - origin);
     }
     _myAvatar.getHead().setLookAtPosition(lookAtSpot);
diff --git a/interface/src/devices/Faceshift.cpp b/interface/src/devices/Faceshift.cpp
index 239acde637..2989b4e421 100644
--- a/interface/src/devices/Faceshift.cpp
+++ b/interface/src/devices/Faceshift.cpp
@@ -41,6 +41,7 @@ Faceshift::Faceshift() :
     _jawOpenIndex(21),
     _longTermAverageEyePitch(0.0f),
     _longTermAverageEyeYaw(0.0f),
+    _longTermAverageInitialized(false),
     _estimatedEyePitch(0.0f),
     _estimatedEyeYaw(0.0f)
 {
@@ -71,8 +72,15 @@ void Faceshift::update() {
     
     // smooth relative to the window
     const float LONG_TERM_AVERAGE_SMOOTHING = 0.999f;
-    _longTermAverageEyePitch = glm::mix(eyeEulers.x, _longTermAverageEyePitch, LONG_TERM_AVERAGE_SMOOTHING);
-    _longTermAverageEyeYaw = glm::mix(eyeEulers.y, _longTermAverageEyeYaw, LONG_TERM_AVERAGE_SMOOTHING);
+    if (!_longTermAverageInitialized) {
+        _longTermAverageEyePitch = eyeEulers.x;
+        _longTermAverageEyeYaw = eyeEulers.y;
+        _longTermAverageInitialized = true;
+        
+    } else {
+        _longTermAverageEyePitch = glm::mix(eyeEulers.x, _longTermAverageEyePitch, LONG_TERM_AVERAGE_SMOOTHING);
+        _longTermAverageEyeYaw = glm::mix(eyeEulers.y, _longTermAverageEyeYaw, LONG_TERM_AVERAGE_SMOOTHING);
+    }
     
     // back to head-relative
     float windowEyePitch = eyeEulers.x - _longTermAverageEyePitch;
@@ -89,6 +97,7 @@ void Faceshift::reset() {
         fsBinaryStream::encode_message(message, fsMsgCalibrateNeutral());
         send(message);
     }
+    _longTermAverageInitialized = false;
 }
 
 void Faceshift::setTCPEnabled(bool enabled) {
diff --git a/interface/src/devices/Faceshift.h b/interface/src/devices/Faceshift.h
index 3cda28a3a0..150f3bb0b7 100644
--- a/interface/src/devices/Faceshift.h
+++ b/interface/src/devices/Faceshift.h
@@ -120,6 +120,7 @@ private:
     
     float _longTermAverageEyePitch;
     float _longTermAverageEyeYaw;
+    bool _longTermAverageInitialized;
     
     float _estimatedEyePitch;
     float _estimatedEyeYaw;

From def37070414fc14662e164554050a33f24f6d076 Mon Sep 17 00:00:00 2001
From: Andrzej Kapolka <drzej.k@gmail.com>
Date: Fri, 1 Nov 2013 18:28:35 -0700
Subject: [PATCH 03/17] I don't think Faceshift's gaze data is actually
 head-relative.

---
 interface/src/Application.cpp       |  7 ++++---
 interface/src/devices/Faceshift.cpp | 24 +++++++-----------------
 2 files changed, 11 insertions(+), 20 deletions(-)

diff --git a/interface/src/Application.cpp b/interface/src/Application.cpp
index 54c903f569..58e9d61025 100644
--- a/interface/src/Application.cpp
+++ b/interface/src/Application.cpp
@@ -1957,11 +1957,12 @@ void Application::update(float deltaTime) {
         }
     }
     if (_faceshift.isActive()) {
+        // deflect using Faceshift gaze data
         glm::vec3 origin = _myAvatar.getHead().calculateAverageEyePosition();
         float pitchSign = (_myCamera.getMode() == CAMERA_MODE_MIRROR) ? -1.0f : 1.0f;
-        const float PITCH_SCALE = 0.25f;
-        const float YAW_SCALE = 0.25f;
-        lookAtSpot = origin + _myCamera.getRotation() * _faceshift.getHeadRotation() * glm::quat(glm::radians(glm::vec3(
+        const float PITCH_SCALE = 0.5f;
+        const float YAW_SCALE = 0.5f;
+        lookAtSpot = origin + _myCamera.getRotation() * glm::quat(glm::radians(glm::vec3(
             _faceshift.getEstimatedEyePitch() * pitchSign * PITCH_SCALE, _faceshift.getEstimatedEyeYaw() * YAW_SCALE, 0.0f))) *
                 glm::inverse(_myCamera.getRotation()) * (lookAtSpot - origin);
     }
diff --git a/interface/src/devices/Faceshift.cpp b/interface/src/devices/Faceshift.cpp
index 2989b4e421..4fb84b821c 100644
--- a/interface/src/devices/Faceshift.cpp
+++ b/interface/src/devices/Faceshift.cpp
@@ -66,29 +66,19 @@ void Faceshift::update() {
     float averageEyePitch = (_eyeGazeLeftPitch + _eyeGazeRightPitch) / 2.0f;
     float averageEyeYaw = (_eyeGazeLeftYaw + _eyeGazeRightYaw) / 2.0f;
     
-    // get the gaze relative to the window
-    glm::vec3 eyeEulers = safeEulerAngles(_headRotation * glm::quat(glm::radians(glm::vec3(
-        averageEyePitch, averageEyeYaw, 0.0f))));
-    
-    // smooth relative to the window
+    // compute and subtract the long term average
     const float LONG_TERM_AVERAGE_SMOOTHING = 0.999f;
     if (!_longTermAverageInitialized) {
-        _longTermAverageEyePitch = eyeEulers.x;
-        _longTermAverageEyeYaw = eyeEulers.y;
+        _longTermAverageEyePitch = averageEyePitch;
+        _longTermAverageEyeYaw = averageEyeYaw;
         _longTermAverageInitialized = true;
         
     } else {
-        _longTermAverageEyePitch = glm::mix(eyeEulers.x, _longTermAverageEyePitch, LONG_TERM_AVERAGE_SMOOTHING);
-        _longTermAverageEyeYaw = glm::mix(eyeEulers.y, _longTermAverageEyeYaw, LONG_TERM_AVERAGE_SMOOTHING);
+        _longTermAverageEyePitch = glm::mix(averageEyePitch, _longTermAverageEyePitch, LONG_TERM_AVERAGE_SMOOTHING);
+        _longTermAverageEyeYaw = glm::mix(averageEyeYaw, _longTermAverageEyeYaw, LONG_TERM_AVERAGE_SMOOTHING);
     }
-    
-    // back to head-relative
-    float windowEyePitch = eyeEulers.x - _longTermAverageEyePitch;
-    float windowEyeYaw = eyeEulers.y - _longTermAverageEyeYaw;
-    glm::vec3 relativeEyeEulers = safeEulerAngles(glm::inverse(_headRotation) * glm::quat(glm::radians(glm::vec3(
-        windowEyePitch, windowEyeYaw, 0.0f))));
-    _estimatedEyePitch = relativeEyeEulers.x;
-    _estimatedEyeYaw = relativeEyeEulers.y;
+    _estimatedEyePitch = averageEyePitch - _longTermAverageEyePitch;
+    _estimatedEyeYaw = averageEyeYaw - _longTermAverageEyeYaw;
 }
 
 void Faceshift::reset() {

From c18de72dd4103e63d3b1e033cb27ecfcf43c751f Mon Sep 17 00:00:00 2001
From: ZappoMan <bradh@konamoxt.com>
Date: Sun, 3 Nov 2013 19:06:06 -0800
Subject: [PATCH 04/17] work on debugging stutter behavior, mostly debugging
 and cleanup

---
 interface/src/Application.cpp                 | 259 ++++++++++++++----
 interface/src/Application.h                   |  26 +-
 interface/src/VoxelPacketProcessor.cpp        |   2 +-
 interface/src/VoxelSystem.cpp                 |   5 +-
 .../shared/src/ReceivedPacketProcessor.h      |  12 +-
 5 files changed, 236 insertions(+), 68 deletions(-)

diff --git a/interface/src/Application.cpp b/interface/src/Application.cpp
index 82fccf7d6d..30548d19cc 100644
--- a/interface/src/Application.cpp
+++ b/interface/src/Application.cpp
@@ -144,6 +144,8 @@ Application::Application(int& argc, char** argv, timeval &startup_time) :
         _packetsPerSecond(0),
         _bytesPerSecond(0),
         _bytesCount(0),
+        _recentMaxPackets(0),
+        _resetRecentMaxPacketsSoon(true),
         _swatch(NULL),
         _pasteMode(false)
 {
@@ -1314,22 +1316,30 @@ void Application::idle() {
 
     double timeSinceLastUpdate = diffclock(&_lastTimeUpdated, &check);
     if (timeSinceLastUpdate > IDLE_SIMULATE_MSECS) {
-
-        const float BIGGEST_DELTA_TIME_SECS = 0.25f;
-        update(glm::clamp((float)timeSinceLastUpdate / 1000.f, 0.f, BIGGEST_DELTA_TIME_SECS));
-        _glWidget->updateGL();
-        _lastTimeUpdated = check;
-        _idleLoopStdev.addValue(timeSinceLastUpdate);
-    
-        //  Record standard deviation and reset counter if needed
-        const int STDEV_SAMPLES = 500;
-        if (_idleLoopStdev.getSamples() > STDEV_SAMPLES) {
-            _idleLoopMeasuredJitter = _idleLoopStdev.getStDev();
-            _idleLoopStdev.reset();
+        {
+            PerformanceWarning warn(showWarnings, "Application::idle()... update()");
+            const float BIGGEST_DELTA_TIME_SECS = 0.25f;
+            update(glm::clamp((float)timeSinceLastUpdate / 1000.f, 0.f, BIGGEST_DELTA_TIME_SECS));
         }
+        {
+            PerformanceWarning warn(showWarnings, "Application::idle()... updateGL()");
+            _glWidget->updateGL();
+        }
+        {
+            PerformanceWarning warn(showWarnings, "Application::idle()... rest of it");
+            _lastTimeUpdated = check;
+            _idleLoopStdev.addValue(timeSinceLastUpdate);
+    
+            //  Record standard deviation and reset counter if needed
+            const int STDEV_SAMPLES = 500;
+            if (_idleLoopStdev.getSamples() > STDEV_SAMPLES) {
+                _idleLoopMeasuredJitter = _idleLoopStdev.getStDev();
+                _idleLoopStdev.reset();
+            }
 
-        // After finishing all of the above work, restart the idle timer, allowing 2ms to process events.
-        idleTimer->start(2);
+            // After finishing all of the above work, restart the idle timer, allowing 2ms to process events.
+            idleTimer->start(2);
+        }
     }
 }
 void Application::terminate() {
@@ -1776,6 +1786,8 @@ static QUuid DEFAULT_NODE_ID_REF;
 
 void Application::updateLookatTargetAvatar(const glm::vec3& mouseRayOrigin, const glm::vec3& mouseRayDirection,
     glm::vec3& eyePosition) {
+    bool showWarnings = Menu::getInstance()->isOptionChecked(MenuOption::PipelineWarnings);
+    PerformanceWarning warn(showWarnings, "Application::updateLookatTargetAvatar()");
     
     _lookatTargetAvatar = findLookatTargetAvatar(mouseRayOrigin, mouseRayDirection, eyePosition, DEFAULT_NODE_ID_REF);
 }
@@ -1907,17 +1919,13 @@ void Application::updateAvatars(float deltaTime, glm::vec3 mouseRayOrigin, glm::
     }
 }
 
-void Application::update(float deltaTime) {
+void Application::updateMouseRay(float deltaTime, glm::vec3& mouseRayOrigin, glm::vec3& mouseRayDirection) {
+
     bool showWarnings = Menu::getInstance()->isOptionChecked(MenuOption::PipelineWarnings);
-    PerformanceWarning warn(showWarnings, "Application::update()");
-    
-    // tell my avatar if the mouse is being pressed...
-    _myAvatar.setMousePressed(_mousePressed);
-    
-    // check what's under the mouse and update the mouse voxel
-    glm::vec3 mouseRayOrigin, mouseRayDirection;
-    _viewFrustum.computePickRay(_mouseX / (float)_glWidget->width(),
-        _mouseY / (float)_glWidget->height(), mouseRayOrigin, mouseRayDirection);
+    PerformanceWarning warn(showWarnings, "Application::updateMouseRay()");
+
+    _viewFrustum.computePickRay(_mouseX / (float)_glWidget->width(), _mouseY / (float)_glWidget->height(), 
+                                mouseRayOrigin, mouseRayDirection);
 
     // adjust for mirroring
     if (_myCamera.getMode() == CAMERA_MODE_MIRROR) {
@@ -1928,12 +1936,19 @@ void Application::update(float deltaTime) {
             _viewFrustum.getRight() * glm::dot(_viewFrustum.getRight(), mouseRayDirection));
     }
 
+    // tell my avatar if the mouse is being pressed...
+    _myAvatar.setMousePressed(_mousePressed);
+
     // tell my avatar the posiion and direction of the ray projected ino the world based on the mouse position        
     _myAvatar.setMouseRay(mouseRayOrigin, mouseRayDirection);
-    
-    // Set where I am looking based on my mouse ray (so that other people can see)
-    glm::vec3 lookAtSpot;
+}
 
+void Application::updateFaceshift(float deltaTime, glm::vec3& mouseRayOrigin, glm::vec3& mouseRayDirection,
+    glm::vec3& lookAtRayOrigin, glm::vec3& lookAtRayDirection) {
+
+    bool showWarnings = Menu::getInstance()->isOptionChecked(MenuOption::PipelineWarnings);
+    PerformanceWarning warn(showWarnings, "Application::updateFaceshift()");
+    
     //  Update faceshift
     _faceshift.update();
     
@@ -1943,14 +1958,19 @@ void Application::update(float deltaTime) {
     }
 
     // if we have faceshift, use that to compute the lookat direction
-    glm::vec3 lookAtRayOrigin = mouseRayOrigin, lookAtRayDirection = mouseRayDirection;
     if (_faceshift.isActive()) {
         lookAtRayOrigin = _myAvatar.getHead().calculateAverageEyePosition();
         lookAtRayDirection = _myAvatar.getHead().getOrientation() * glm::quat(glm::radians(glm::vec3(
             _faceshift.getEstimatedEyePitch(), _faceshift.getEstimatedEyeYaw(), 0.0f))) * glm::vec3(0.0f, 0.0f, -1.0f);
     }
+}
 
-    updateLookatTargetAvatar(mouseRayOrigin, mouseRayDirection, lookAtSpot);
+void Application::updateMyAvatarLookAtPosition(glm::vec3& lookAtSpot, glm::vec3& lookAtRayOrigin, 
+        glm::vec3& lookAtRayDirection) {
+
+    bool showWarnings = Menu::getInstance()->isOptionChecked(MenuOption::PipelineWarnings);
+    PerformanceWarning warn(showWarnings, "Application::updateMyAvatarLookAtPosition()");
+    
     if (_lookatTargetAvatar && !_faceshift.isActive()) {
         // If the mouse is over another avatar's head...
          _myAvatar.getHead().setLookAtPosition(lookAtSpot);
@@ -1969,11 +1989,14 @@ void Application::update(float deltaTime) {
         lookAtSpot = lookAtRayOrigin + lookAtRayDirection * FAR_AWAY_STARE;
         _myAvatar.getHead().setLookAtPosition(lookAtSpot);
     }
+}
 
-    //  Find the voxel we are hovering over, and respond if clicked
-    float distance;
-    BoxFace face;
-    
+void Application::updateHoverVoxels(float deltaTime, glm::vec3& mouseRayOrigin, glm::vec3& mouseRayDirection, 
+                                    float& distance, BoxFace& face) {
+
+    bool showWarnings = Menu::getInstance()->isOptionChecked(MenuOption::PipelineWarnings);
+    PerformanceWarning warn(showWarnings, "Application::updateHoverVoxels()");
+                                    
     //  If we have clicked on a voxel, update it's color
     if (_isHoverVoxelSounding) {
         VoxelNode* hoveredNode = _voxels.getVoxelAt(_hoverVoxel.x, _hoverVoxel.y, _hoverVoxel.z, _hoverVoxel.s);
@@ -2005,13 +2028,19 @@ void Application::update(float deltaTime) {
             _isHoverVoxelSounding = true;
         }
     }
-        
+}
+
+void Application::updateMouseVoxels(float deltaTime, glm::vec3& mouseRayOrigin, glm::vec3& mouseRayDirection,
+                                    float& distance, BoxFace& face) {
+
+    bool showWarnings = Menu::getInstance()->isOptionChecked(MenuOption::PipelineWarnings);
+    PerformanceWarning warn(showWarnings, "Application::updateMouseVoxels()");
+
     _mouseVoxel.s = 0.0f;
     if (Menu::getInstance()->isVoxelModeActionChecked() &&
         (fabs(_myAvatar.getVelocity().x) +
          fabs(_myAvatar.getVelocity().y) +
          fabs(_myAvatar.getVelocity().z)) / 3 < MAX_AVATAR_EDIT_VELOCITY) {
-        PerformanceWarning warn(showWarnings, "Application::update()... findRayIntersection()");
 
         if (_voxels.findRayIntersection(mouseRayOrigin, mouseRayDirection, _mouseVoxel, distance, face)) {
             if (distance < MAX_VOXEL_EDIT_DISTANCE) {
@@ -2080,12 +2109,17 @@ void Application::update(float deltaTime) {
             _justEditedVoxel = false;
         }
     }
-    
+}
+
+void Application::updateHandAndTouch(float deltaTime) {
+    bool showWarnings = Menu::getInstance()->isOptionChecked(MenuOption::PipelineWarnings);
+    PerformanceWarning warn(showWarnings, "Application::updateHandAndTouch()");
+
     // walking triggers the handControl to stop
     if (_myAvatar.getMode() == AVATAR_MODE_WALKING) {
         _handControl.stop();
     }
-       
+
     //  Update from Touch
     if (_isTouchPressed) {
         float TOUCH_YAW_SCALE = -0.25f;
@@ -2096,19 +2130,29 @@ void Application::update(float deltaTime) {
         _lastTouchAvgX = _touchAvgX;
         _lastTouchAvgY = _touchAvgY;
     }
-    
-    // Leap finger-sensing device
+}
+
+void Application::updateLeap(float deltaTime) {
+    bool showWarnings = Menu::getInstance()->isOptionChecked(MenuOption::PipelineWarnings);
+    PerformanceWarning warn(showWarnings, "Application::updateLeap()");
+
     LeapManager::enableFakeFingers(Menu::getInstance()->isOptionChecked(MenuOption::SimulateLeapHand));
     _myAvatar.getHand().setRaveGloveActive(Menu::getInstance()->isOptionChecked(MenuOption::TestRaveGlove));
     LeapManager::nextFrame(_myAvatar);
-    
-     //  Read serial port interface devices
+}
+
+void Application::updateSerialDevices(float deltaTime) {
+    bool showWarnings = Menu::getInstance()->isOptionChecked(MenuOption::PipelineWarnings);
+    PerformanceWarning warn(showWarnings, "Application::updateSerialDevices()");
+
     if (_serialHeadSensor.isActive()) {
         _serialHeadSensor.readData(deltaTime);
     }
-    
-    //  Sample hardware, update view frustum if needed, and send avatar data to mixer/nodes
-    updateAvatar(deltaTime);
+}
+
+void Application::updateThreads(float deltaTime) {
+    bool showWarnings = Menu::getInstance()->isOptionChecked(MenuOption::PipelineWarnings);
+    PerformanceWarning warn(showWarnings, "Application::updateThreads()");
 
     // read incoming packets from network
     if (!_enableNetworkThread) {
@@ -2120,12 +2164,12 @@ void Application::update(float deltaTime) {
         _voxelProcessor.threadRoutine();
         _voxelEditSender.threadRoutine();
     }
+}
+
+void Application::updateMyAvatarSimulation(float deltaTime) {
+    bool showWarnings = Menu::getInstance()->isOptionChecked(MenuOption::PipelineWarnings);
+    PerformanceWarning warn(showWarnings, "Application::updateMyAvatarSimulation()");
 
-    
-    //loop through all the other avatars and simulate them...
-    updateAvatars(deltaTime, mouseRayOrigin, mouseRayDirection);
-    
-    //  Simulate myself
     if (Menu::getInstance()->isOptionChecked(MenuOption::Gravity)) {
         _myAvatar.setGravity(_environment.getGravity(_myAvatar.getPosition()));
     }
@@ -2138,12 +2182,21 @@ void Application::update(float deltaTime) {
     } else {
         _myAvatar.simulate(deltaTime, NULL);
     }
-    
-    //  Simulate particle cloud movements
+}
+
+void Application::updateParticles(float deltaTime) {
+    bool showWarnings = Menu::getInstance()->isOptionChecked(MenuOption::PipelineWarnings);
+    PerformanceWarning warn(showWarnings, "Application::updateParticles()");
+
     if (Menu::getInstance()->isOptionChecked(MenuOption::ParticleCloud)) {
         _cloud.simulate(deltaTime);
     }
-    
+}
+
+void Application::updateTransmitter(float deltaTime) {
+    bool showWarnings = Menu::getInstance()->isOptionChecked(MenuOption::PipelineWarnings);
+    PerformanceWarning warn(showWarnings, "Application::updateTransmitter()");
+
     // no transmitter drive implies transmitter pick
     if (!Menu::getInstance()->isOptionChecked(MenuOption::TransmitterDrive) && _myTransmitter.isConnected()) {
         _transmitterPickStart = _myAvatar.getSkeleton().joint[AVATAR_JOINT_CHEST].position;
@@ -2178,7 +2231,12 @@ void Application::update(float deltaTime) {
     } else {
         _transmitterPickStart = _transmitterPickEnd = glm::vec3();
     }
-    
+}
+
+void Application::updateCamera(float deltaTime) {
+    bool showWarnings = Menu::getInstance()->isOptionChecked(MenuOption::PipelineWarnings);
+    PerformanceWarning warn(showWarnings, "Application::updateCamera()");
+
     if (!OculusManager::isConnected()) {        
         if (Menu::getInstance()->isOptionChecked(MenuOption::FullscreenMirror)) {
             if (_myCamera.getMode() != CAMERA_MODE_MIRROR) {
@@ -2213,7 +2271,12 @@ void Application::update(float deltaTime) {
             }
         }
     }
-   
+}
+
+void Application::updateDialogs(float deltaTime) {
+    bool showWarnings = Menu::getInstance()->isOptionChecked(MenuOption::PipelineWarnings);
+    PerformanceWarning warn(showWarnings, "Application::updateDialogs()");
+
     // Update bandwidth dialog, if any
     BandwidthDialog* bandwidthDialog = Menu::getInstance()->getBandwidthDialog();
     if (bandwidthDialog) {
@@ -2224,6 +2287,11 @@ void Application::update(float deltaTime) {
     if (voxelStatsDialog) {
         voxelStatsDialog->update();
     }
+}
+
+void Application::updateAudio(float deltaTime) {
+    bool showWarnings = Menu::getInstance()->isOptionChecked(MenuOption::PipelineWarnings);
+    PerformanceWarning warn(showWarnings, "Application::updateAudio()");
 
     //  Update audio stats for procedural sounds
     #ifndef _WIN32
@@ -2231,7 +2299,12 @@ void Application::update(float deltaTime) {
     _audio.setLastVelocity(_myAvatar.getVelocity());
     _audio.eventuallyAnalyzePing();
     #endif
-    
+}
+
+void Application::updateCursor(float deltaTime) {
+    bool showWarnings = Menu::getInstance()->isOptionChecked(MenuOption::PipelineWarnings);
+    PerformanceWarning warn(showWarnings, "Application::updateCursor()");
+
     // watch mouse position, if it hasn't moved, hide the cursor
     bool underMouse = _glWidget->underMouse();
     if (!_mouseHidden) {
@@ -2252,6 +2325,43 @@ void Application::update(float deltaTime) {
     }
 }
 
+void Application::update(float deltaTime) {
+    bool showWarnings = Menu::getInstance()->isOptionChecked(MenuOption::PipelineWarnings);
+    PerformanceWarning warn(showWarnings, "Application::update()");
+    
+    // check what's under the mouse and update the mouse voxel
+    glm::vec3 mouseRayOrigin, mouseRayDirection;
+    updateMouseRay(deltaTime, mouseRayOrigin, mouseRayDirection);
+    
+    // Set where I am looking based on my mouse ray (so that other people can see)
+    glm::vec3 lookAtSpot;
+    glm::vec3 lookAtRayOrigin = mouseRayOrigin, lookAtRayDirection = mouseRayDirection;
+
+    updateFaceshift(deltaTime, mouseRayOrigin, mouseRayDirection, lookAtRayOrigin, lookAtRayDirection);
+    updateLookatTargetAvatar(mouseRayOrigin, mouseRayDirection, lookAtSpot);
+    updateMyAvatarLookAtPosition(lookAtSpot, lookAtRayOrigin, lookAtRayDirection);
+    
+    //  Find the voxel we are hovering over, and respond if clicked
+    float distance;
+    BoxFace face;
+    
+    updateHoverVoxels(deltaTime, mouseRayOrigin, mouseRayDirection, distance, face); // clicking on voxels and making sounds
+    updateMouseVoxels(deltaTime, mouseRayOrigin, mouseRayDirection, distance, face); // UI/UX related to voxels
+    updateHandAndTouch(deltaTime); // Update state for touch sensors
+    updateLeap(deltaTime); // Leap finger-sensing device
+    updateSerialDevices(deltaTime); // Read serial port interface devices
+    updateAvatar(deltaTime); // Sample hardware, update view frustum if needed, and send avatar data to mixer/nodes
+    updateThreads(deltaTime); // If running non-threaded, then give the threads some time to process...
+    updateAvatars(deltaTime, mouseRayOrigin, mouseRayDirection); //loop through all the other avatars and simulate them...
+    updateMyAvatarSimulation(deltaTime); // Simulate myself
+    updateParticles(deltaTime); // Simulate particle cloud movements
+    updateTransmitter(deltaTime); // transmitter drive or pick
+    updateCamera(deltaTime); // handle various camera tweaks like off axis projection
+    updateDialogs(deltaTime); // update various stats dialogs if present
+    updateAudio(deltaTime); // Update audio stats for procedural sounds
+    updateCursor(deltaTime); // Handle cursor updates
+}
+
 void Application::updateAvatar(float deltaTime) {
     bool showWarnings = Menu::getInstance()->isOptionChecked(MenuOption::PipelineWarnings);
     PerformanceWarning warn(showWarnings, "Application::updateAvatar()");
@@ -3258,6 +3368,8 @@ void Application::displayStats() {
     drawtext(10, statsVerticalOffset, 0.10f, 0, 1.0, 0, avatarStats);
 
  
+    QLocale locale(QLocale::English);
+
     std::stringstream voxelStats;
     voxelStats.precision(4);
     voxelStats << "Voxels " << 
@@ -3280,11 +3392,19 @@ void Application::displayStats() {
     statsVerticalOffset += PELS_PER_LINE;
     drawtext(10, statsVerticalOffset, 0.10f, 0, 1.0, 0, (char*)voxelStats.str().c_str());
 
+    unsigned long localTotal = VoxelNode::getNodeCount();
+    unsigned long localInternal = VoxelNode::getInternalNodeCount();
+    unsigned long localLeaves = VoxelNode::getLeafNodeCount();
+    QString localTotalString = locale.toString((uint)localTotal); // consider adding: .rightJustified(10, ' ');
+    QString localInternalString = locale.toString((uint)localInternal);
+    QString localLeavesString = locale.toString((uint)localLeaves);
+
+
     voxelStats.str("");
     voxelStats << 
-        "Local Voxels Total: " << VoxelNode::getNodeCount() << ", " <<
-        "Internal: " << VoxelNode::getInternalNodeCount() << " , " <<
-        "Leaves: " << VoxelNode::getLeafNodeCount() << "";
+        "Local Voxels Total: " << localTotalString.toLocal8Bit().constData() << " / " <<
+        "Internal: " << localInternalString.toLocal8Bit().constData() << " / " <<
+        "Leaves: " << localLeavesString.toLocal8Bit().constData() << "";
     statsVerticalOffset += PELS_PER_LINE;
     drawtext(10, statsVerticalOffset, 0.10f, 0, 1.0, 0, (char*)voxelStats.str().c_str());
 
@@ -3311,6 +3431,29 @@ void Application::displayStats() {
     voxelStats << "Sending Mode: " << voxelDetails;
     statsVerticalOffset += PELS_PER_LINE;
     drawtext(10, statsVerticalOffset, 0.10f, 0, 1.0, 0, (char*)voxelStats.str().c_str());
+
+    voxelStats.str("");
+    int voxelPacketsToProcess = _voxelProcessor.packetsToProcessCount();
+    QString packetsString = locale.toString((int)voxelPacketsToProcess);
+    QString maxString = locale.toString((int)_recentMaxPackets);
+
+    voxelStats << "Voxel Packets to Process: " << packetsString.toLocal8Bit().constData() 
+                << " [Recent Max: " << maxString.toLocal8Bit().constData() << "]";
+                
+    if (_resetRecentMaxPacketsSoon && voxelPacketsToProcess > 0) {
+        _recentMaxPackets = 0;
+        _resetRecentMaxPacketsSoon = false;
+    }
+    if (voxelPacketsToProcess == 0) {
+        _resetRecentMaxPacketsSoon = true;
+    } else {
+        if (voxelPacketsToProcess > _recentMaxPackets) {
+            _recentMaxPackets = voxelPacketsToProcess;
+        }
+    }
+    statsVerticalOffset += PELS_PER_LINE;
+    drawtext(10, statsVerticalOffset, 0.10f, 0, 1.0, 0, (char*)voxelStats.str().c_str());
+
     
     Node *avatarMixer = NodeList::getInstance()->soloNodeOfType(NODE_TYPE_AVATAR_MIXER);
     char avatarMixerStats[200];
diff --git a/interface/src/Application.h b/interface/src/Application.h
index 4154720419..ff8ae2f882 100644
--- a/interface/src/Application.h
+++ b/interface/src/Application.h
@@ -223,9 +223,30 @@ private:
     void init();
     
     void update(float deltaTime);
-    
+
+    // Various helper functions called during update()
+    void updateMouseRay(float deltaTime, glm::vec3& mouseRayOrigin, glm::vec3& mouseRayDirection);
+    void updateFaceshift(float deltaTime, glm::vec3& mouseRayOrigin, glm::vec3& mouseRayDirection,
+            glm::vec3& lookAtRayOrigin, glm::vec3& lookAtRayDirection);
+    void updateMyAvatarLookAtPosition(glm::vec3& lookAtSpot, glm::vec3& lookAtRayOrigin, glm::vec3& lookAtRayDirection);
+    void updateHoverVoxels(float deltaTime, glm::vec3& mouseRayOrigin, glm::vec3& mouseRayDirection, 
+            float& distance, BoxFace& face);
+    void updateMouseVoxels(float deltaTime, glm::vec3& mouseRayOrigin, glm::vec3& mouseRayDirection,
+            float& distance, BoxFace& face);
     void updateLookatTargetAvatar(const glm::vec3& mouseRayOrigin, const glm::vec3& mouseRayDirection,
         glm::vec3& eyePosition);
+    void updateHandAndTouch(float deltaTime);
+    void updateLeap(float deltaTime);
+    void updateSerialDevices(float deltaTime);
+    void updateThreads(float deltaTime);
+    void updateMyAvatarSimulation(float deltaTime);
+    void updateParticles(float deltaTime);
+    void updateTransmitter(float deltaTime);
+    void updateCamera(float deltaTime);
+    void updateDialogs(float deltaTime);
+    void updateAudio(float deltaTime);
+    void updateCursor(float deltaTime);
+
     Avatar* findLookatTargetAvatar(const glm::vec3& mouseRayOrigin, const glm::vec3& mouseRayDirection,
         glm::vec3& eyePosition, QUuid &nodeUUID);
     bool isLookingAtMyAvatar(Avatar* avatar);
@@ -407,6 +428,9 @@ private:
     int _bytesPerSecond;
     int _bytesCount;
     
+    int _recentMaxPackets; // recent max incoming voxel packets to process
+    bool _resetRecentMaxPacketsSoon;
+    
     StDev _idleLoopStdev;
     float _idleLoopMeasuredJitter;
 
diff --git a/interface/src/VoxelPacketProcessor.cpp b/interface/src/VoxelPacketProcessor.cpp
index 76075c6fa5..9087a5a545 100644
--- a/interface/src/VoxelPacketProcessor.cpp
+++ b/interface/src/VoxelPacketProcessor.cpp
@@ -19,7 +19,7 @@ void VoxelPacketProcessor::processPacket(sockaddr& senderAddress, unsigned char*
                             "VoxelPacketProcessor::processPacket()");
                             
     const int WAY_BEHIND = 300;
-    if (packetsToProcessCount() > WAY_BEHIND && Menu::getInstance()->isOptionChecked(MenuOption::PipelineWarnings)) {
+    if (packetsToProcessCount() > WAY_BEHIND && Menu::getInstance()->isOptionChecked(MenuOption::ExtraDebugging)) {
         qDebug("VoxelPacketProcessor::processPacket() packets to process=%d\n", packetsToProcessCount());
     }
     ssize_t messageLength = packetLength;
diff --git a/interface/src/VoxelSystem.cpp b/interface/src/VoxelSystem.cpp
index 541d7983d2..5e30141054 100644
--- a/interface/src/VoxelSystem.cpp
+++ b/interface/src/VoxelSystem.cpp
@@ -1882,7 +1882,7 @@ void VoxelSystem::hideOutOfView(bool forceFullFrustum) {
     _inhideOutOfView = true;
 
     bool showDebugDetails = Menu::getInstance()->isOptionChecked(MenuOption::PipelineWarnings);
-    PerformanceWarning warn(showDebugDetails, "hideOutOfView()", showDebugDetails);
+    PerformanceWarning warn(showDebugDetails, "hideOutOfView()");
     bool widenFrustum = true;
 
     // When using "delta" view frustums and only hide/show items that are in the difference
@@ -1927,7 +1927,8 @@ void VoxelSystem::hideOutOfView(bool forceFullFrustum) {
         setupNewVoxelsForDrawingSingleNode(DONT_BAIL_EARLY);
     }
     
-    if (showDebugDetails) {
+    bool extraDebugDetails = Menu::getInstance()->isOptionChecked(MenuOption::ExtraDebugging);
+    if (extraDebugDetails) {
         qDebug("hideOutOfView() scanned=%ld removed=%ld inside=%ld intersect=%ld outside=%ld\n", 
                 args.nodesScanned, args.nodesRemoved, args.nodesInside, 
                 args.nodesIntersect, args.nodesOutside
diff --git a/libraries/shared/src/ReceivedPacketProcessor.h b/libraries/shared/src/ReceivedPacketProcessor.h
index f6d49cbb09..78017bffd7 100644
--- a/libraries/shared/src/ReceivedPacketProcessor.h
+++ b/libraries/shared/src/ReceivedPacketProcessor.h
@@ -24,6 +24,12 @@ public:
     /// \param ssize_t packetLength size of received data
     /// \thread network receive thread
     void queueReceivedPacket(sockaddr& senderAddress, unsigned char*  packetData, ssize_t packetLength);
+
+    /// Are there received packets waiting to be processed
+    bool hasPacketsToProcess() const { return _packets.size() > 0; }
+
+    /// How many received packets waiting are to be processed
+    int packetsToProcessCount() const { return _packets.size(); }
     
 protected:
     /// Callback for processing of recieved packets. Implement this to process the incoming packets.
@@ -36,12 +42,6 @@ protected:
     /// Implements generic processing behavior for this thread.
     virtual bool process();
 
-    /// Are there received packets waiting to be processed
-    bool hasPacketsToProcess() const { return _packets.size() > 0; }
-
-    /// How many received packets waiting are to be processed
-    int packetsToProcessCount() const { return _packets.size(); }
-
 private:
 
     std::vector<NetworkPacket> _packets;

From 9bd27e2b2a5ba057280d3a27eb2815553a1401e4 Mon Sep 17 00:00:00 2001
From: ZappoMan <bradh@konamoxt.com>
Date: Sun, 3 Nov 2013 21:11:08 -0800
Subject: [PATCH 05/17] fix stutter on rotating

---
 interface/src/Application.cpp | 32 ++++++++++++------
 interface/src/VoxelSystem.cpp | 61 +++++++++++++++++++++--------------
 interface/src/VoxelSystem.h   |  6 ++++
 3 files changed, 65 insertions(+), 34 deletions(-)

diff --git a/interface/src/Application.cpp b/interface/src/Application.cpp
index 30548d19cc..994239cf56 100644
--- a/interface/src/Application.cpp
+++ b/interface/src/Application.cpp
@@ -1306,7 +1306,10 @@ static glm::vec3 getFaceVector(BoxFace face) {
 }
 
 void Application::idle() {
-    bool showWarnings = Menu::getInstance()->isOptionChecked(MenuOption::PipelineWarnings);
+    // Normally we check PipelineWarnings, but since idle will often take more than 10ms we only show these idle timing 
+    // details if we're in ExtraDebugging mode. However, the ::update() and it's subcomponents will show their timing 
+    // details normally.
+    bool showWarnings = Menu::getInstance()->isOptionChecked(MenuOption::ExtraDebugging);
     PerformanceWarning warn(showWarnings, "Application::idle()");
     
     timeval check;
@@ -1996,7 +1999,7 @@ void Application::updateHoverVoxels(float deltaTime, glm::vec3& mouseRayOrigin,
 
     bool showWarnings = Menu::getInstance()->isOptionChecked(MenuOption::PipelineWarnings);
     PerformanceWarning warn(showWarnings, "Application::updateHoverVoxels()");
-                                    
+
     //  If we have clicked on a voxel, update it's color
     if (_isHoverVoxelSounding) {
         VoxelNode* hoveredNode = _voxels.getVoxelAt(_hoverVoxel.x, _hoverVoxel.y, _hoverVoxel.z, _hoverVoxel.s);
@@ -2018,14 +2021,23 @@ void Application::updateHoverVoxels(float deltaTime, glm::vec3& mouseRayOrigin,
     } else {
         //  Check for a new hover voxel
         glm::vec4 oldVoxel(_hoverVoxel.x, _hoverVoxel.y, _hoverVoxel.z, _hoverVoxel.s);
-        _isHoverVoxel = _voxels.findRayIntersection(mouseRayOrigin, mouseRayDirection, _hoverVoxel, distance, face);
-        if (MAKE_SOUND_ON_VOXEL_HOVER && _isHoverVoxel && glm::vec4(_hoverVoxel.x, _hoverVoxel.y, _hoverVoxel.z, _hoverVoxel.s) != oldVoxel) {
-            _hoverVoxelOriginalColor[0] = _hoverVoxel.red;
-            _hoverVoxelOriginalColor[1] = _hoverVoxel.green;
-            _hoverVoxelOriginalColor[2] = _hoverVoxel.blue;
-            _hoverVoxelOriginalColor[3] = 1;
-            _audio.startCollisionSound(1.0, HOVER_VOXEL_FREQUENCY * _hoverVoxel.s * TREE_SCALE, 0.0, HOVER_VOXEL_DECAY);
-            _isHoverVoxelSounding = true;
+        // only do this work if MAKE_SOUND_ON_VOXEL_HOVER or MAKE_SOUND_ON_VOXEL_CLICK is enabled, 
+        // and make sure the tree is not already busy... because otherwise you'll have to wait.
+        if (!_voxels.treeIsBusy()) {
+            {
+                PerformanceWarning warn(showWarnings, "Application::updateHoverVoxels() _voxels.findRayIntersection()");
+                _isHoverVoxel = _voxels.findRayIntersection(mouseRayOrigin, mouseRayDirection, _hoverVoxel, distance, face);
+            }
+            if (MAKE_SOUND_ON_VOXEL_HOVER && _isHoverVoxel && 
+                    glm::vec4(_hoverVoxel.x, _hoverVoxel.y, _hoverVoxel.z, _hoverVoxel.s) != oldVoxel) {
+                    
+                _hoverVoxelOriginalColor[0] = _hoverVoxel.red;
+                _hoverVoxelOriginalColor[1] = _hoverVoxel.green;
+                _hoverVoxelOriginalColor[2] = _hoverVoxel.blue;
+                _hoverVoxelOriginalColor[3] = 1;
+                _audio.startCollisionSound(1.0, HOVER_VOXEL_FREQUENCY * _hoverVoxel.s * TREE_SCALE, 0.0, HOVER_VOXEL_DECAY);
+                _isHoverVoxelSounding = true;
+            }
         }
     }
 }
diff --git a/interface/src/VoxelSystem.cpp b/interface/src/VoxelSystem.cpp
index 5e30141054..1219548b4d 100644
--- a/interface/src/VoxelSystem.cpp
+++ b/interface/src/VoxelSystem.cpp
@@ -110,6 +110,7 @@ VoxelSystem::VoxelSystem(float treeScale, int maxVoxels)
     
     _culledOnce = false;
     _inhideOutOfView = false;
+    _treeIsBusy = false;
 }
 
 void VoxelSystem::voxelDeleted(VoxelNode* node) {
@@ -595,9 +596,9 @@ int VoxelSystem::parseData(unsigned char* sourceBuffer, int numBytes) {
                                     "readBitstreamToTree()");
             // ask the VoxelTree to read the bitstream into the tree
             ReadBitstreamToTreeParams args(WANT_COLOR, WANT_EXISTS_BITS, NULL, getDataSourceUUID());
-            pthread_mutex_lock(&_treeLock);
+            lockTree();
             _tree->readBitstreamToTree(voxelData, numBytes - numBytesPacketHeader, args);
-            pthread_mutex_unlock(&_treeLock);
+            unlockTree();
         }
             break;
         case PACKET_TYPE_VOXEL_DATA_MONOCHROME: {
@@ -605,9 +606,9 @@ int VoxelSystem::parseData(unsigned char* sourceBuffer, int numBytes) {
                                     "readBitstreamToTree()");
             // ask the VoxelTree to read the MONOCHROME bitstream into the tree
             ReadBitstreamToTreeParams args(NO_COLOR, WANT_EXISTS_BITS, NULL, getDataSourceUUID());
-            pthread_mutex_lock(&_treeLock);
+            lockTree();
             _tree->readBitstreamToTree(voxelData, numBytes - numBytesPacketHeader, args);
-            pthread_mutex_unlock(&_treeLock);
+            unlockTree();
         }
             break;
         case PACKET_TYPE_Z_COMMAND:
@@ -1002,7 +1003,9 @@ int VoxelSystem::updateNodeInArrays(VoxelNode* node, bool reuseIndex, bool force
         // not render these Voxels. We need to think about ways to keep the entire scene intact but maybe lower quality
         // possibly shifting down to lower LOD or something. This debug message is to help identify, if/when/how this
         // state actually occurs.
-        qDebug("OHHHH NOOOOOO!!!! updateNodeInArrays() BAILING (_voxelsInWriteArrays >= _maxVoxels)\n");
+        if (Menu::getInstance()->isOptionChecked(MenuOption::ExtraDebugging)) {
+            qDebug("OHHHH NOOOOOO!!!! updateNodeInArrays() BAILING (_voxelsInWriteArrays >= _maxVoxels)\n");
+        }
         return 0;
     }
 
@@ -1405,9 +1408,9 @@ void VoxelSystem::removeScaleAndReleaseProgram(bool texture) {
 int VoxelSystem::_nodeCount = 0;
 
 void VoxelSystem::killLocalVoxels() {
-    pthread_mutex_lock(&_treeLock);
+    lockTree();
     _tree->eraseAllVoxels();
-    pthread_mutex_unlock(&_treeLock);
+    unlockTree();
     clearFreeBufferIndexes();    
     _voxelsInReadArrays = 0; // do we need to do this?
     setupNewVoxelsForDrawing();
@@ -1426,9 +1429,9 @@ bool VoxelSystem::clearAllNodesBufferIndexOperation(VoxelNode* node, void* extra
 
 void VoxelSystem::clearAllNodesBufferIndex() {
     _nodeCount = 0;
-    pthread_mutex_lock(&_treeLock);                                  
+    lockTree();                                  
     _tree->recurseTreeWithOperation(clearAllNodesBufferIndexOperation);
-    pthread_mutex_unlock(&_treeLock);
+    unlockTree();
     if (Menu::getInstance()->isOptionChecked(MenuOption::PipelineWarnings)) {
         qDebug("clearing buffer index of %d nodes\n", _nodeCount);
     }
@@ -1916,9 +1919,9 @@ void VoxelSystem::hideOutOfView(bool forceFullFrustum) {
         return;
     }
 
-    pthread_mutex_lock(&_treeLock);                                  
+    lockTree();                                  
     _tree->recurseTreeWithOperation(hideOutOfViewOperation,(void*)&args);
-    pthread_mutex_unlock(&_treeLock);
+    unlockTree();
     _lastCulledViewFrustum = args.thisViewFrustum; // save last stable
     _culledOnce = true;
 
@@ -2107,10 +2110,10 @@ bool VoxelSystem::hideOutOfViewOperation(VoxelNode* node, void* extraData) {
 
 bool VoxelSystem::findRayIntersection(const glm::vec3& origin, const glm::vec3& direction,
                                       VoxelDetail& detail, float& distance, BoxFace& face) {
-    pthread_mutex_lock(&_treeLock);                                  
+    lockTree();                                  
     VoxelNode* node;
     if (!_tree->findRayIntersection(origin, direction, node, distance, face)) {
-        pthread_mutex_unlock(&_treeLock);
+        unlockTree();
         return false;
     }
     detail.x = node->getCorner().x;
@@ -2120,21 +2123,21 @@ bool VoxelSystem::findRayIntersection(const glm::vec3& origin, const glm::vec3&
     detail.red = node->getColor()[0];
     detail.green = node->getColor()[1];
     detail.blue = node->getColor()[2];
-    pthread_mutex_unlock(&_treeLock);
+    unlockTree();
     return true;
 }
 
 bool VoxelSystem::findSpherePenetration(const glm::vec3& center, float radius, glm::vec3& penetration) {
-    pthread_mutex_lock(&_treeLock);
+    lockTree();
     bool result = _tree->findSpherePenetration(center, radius, penetration);
-    pthread_mutex_unlock(&_treeLock);
+    unlockTree();
     return result;
 }
 
 bool VoxelSystem::findCapsulePenetration(const glm::vec3& start, const glm::vec3& end, float radius, glm::vec3& penetration) {
-    pthread_mutex_lock(&_treeLock);
+    lockTree();
     bool result = _tree->findCapsulePenetration(start, end, radius, penetration);
-    pthread_mutex_unlock(&_treeLock);
+    unlockTree();
     return result;
 }
 
@@ -2308,9 +2311,9 @@ void VoxelSystem::collectStatsForTreesAndVBOs() {
 
 
 void VoxelSystem::deleteVoxelAt(float x, float y, float z, float s) {
-    pthread_mutex_lock(&_treeLock);
+    lockTree();
     _tree->deleteVoxelAt(x, y, z, s);
-    pthread_mutex_unlock(&_treeLock);
+    unlockTree();
     
     // redraw!
     setupNewVoxelsForDrawing();  // do we even need to do this? Or will the next network receive kick in?
@@ -2325,9 +2328,9 @@ void VoxelSystem::createVoxel(float x, float y, float z, float s,
                               unsigned char red, unsigned char green, unsigned char blue, bool destructive) {
     
     //qDebug("VoxelSystem::createVoxel(%f,%f,%f,%f)\n",x,y,z,s);
-    pthread_mutex_lock(&_treeLock);
+    lockTree();
     _tree->createVoxel(x, y, z, s, red, green, blue, destructive); 
-    pthread_mutex_unlock(&_treeLock);
+    unlockTree();
 
     setupNewVoxelsForDrawing(); 
 };
@@ -2650,9 +2653,9 @@ void VoxelSystem::nodeKilled(Node* node) {
         if (_voxelServerCount > 0) {
             // Kill any voxels from the local tree that match this nodeID
             // commenting out for removal of 16 bit node IDs
-            pthread_mutex_lock(&_treeLock);
+            lockTree();
             _tree->recurseTreeWithOperation(killSourceVoxelsOperation, &nodeUUID);
-            pthread_mutex_unlock(&_treeLock);
+            unlockTree();
             _tree->setDirtyBit();
             setupNewVoxelsForDrawing();
         } else {
@@ -2720,5 +2723,15 @@ unsigned long VoxelSystem::getVoxelMemoryUsageGPU() {
     return (_initialMemoryUsageGPU - currentFreeMemory);
 }
 
+void VoxelSystem::lockTree() {
+    pthread_mutex_lock(&_treeLock);
+    _treeIsBusy = true;
+}
+
+void VoxelSystem::unlockTree() {
+    _treeIsBusy = false;
+    pthread_mutex_unlock(&_treeLock);
+}
+
 
 
diff --git a/interface/src/VoxelSystem.h b/interface/src/VoxelSystem.h
index 303c7dfb49..0fdbb5b884 100644
--- a/interface/src/VoxelSystem.h
+++ b/interface/src/VoxelSystem.h
@@ -120,6 +120,8 @@ public:
     virtual void nodeKilled(Node* node);
     virtual void domainChanged(QString domain);
     
+    bool treeIsBusy() const { return _treeIsBusy; }
+    
 signals:
     void importSize(float x, float y, float z);
     void importProgress(int progress);
@@ -302,6 +304,10 @@ private:
     bool _useFastVoxelPipeline;
     
     bool _inhideOutOfView;
+    bool _treeIsBusy; // is the tree mutex locked? if so, it's busy, and if you can avoid it, don't access the tree
+    
+    void lockTree();
+    void unlockTree();
 };
 
 #endif

From ce1ebf8d11484ba87fdaec76629302741e6cb90a Mon Sep 17 00:00:00 2001
From: Andrzej Kapolka <drzej.k@gmail.com>
Date: Mon, 4 Nov 2013 10:57:29 -0800
Subject: [PATCH 06/17] The Faceshift eye rotations are indeed relative to the
 head.

---
 interface/src/devices/Faceshift.cpp | 17 +++++++++--------
 1 file changed, 9 insertions(+), 8 deletions(-)

diff --git a/interface/src/devices/Faceshift.cpp b/interface/src/devices/Faceshift.cpp
index 4fb84b821c..9a45810cbe 100644
--- a/interface/src/devices/Faceshift.cpp
+++ b/interface/src/devices/Faceshift.cpp
@@ -63,22 +63,23 @@ void Faceshift::update() {
     if (!isActive()) {
         return;
     }
-    float averageEyePitch = (_eyeGazeLeftPitch + _eyeGazeRightPitch) / 2.0f;
-    float averageEyeYaw = (_eyeGazeLeftYaw + _eyeGazeRightYaw) / 2.0f;
+    // get the euler angles relative to the window
+    glm::vec3 eulers = safeEulerAngles(_headRotation * glm::quat(glm::radians(glm::vec3(
+        (_eyeGazeLeftPitch + _eyeGazeRightPitch) / 2.0f, (_eyeGazeLeftYaw + _eyeGazeRightYaw) / 2.0f, 0.0f))));
     
     // compute and subtract the long term average
     const float LONG_TERM_AVERAGE_SMOOTHING = 0.999f;
     if (!_longTermAverageInitialized) {
-        _longTermAverageEyePitch = averageEyePitch;
-        _longTermAverageEyeYaw = averageEyeYaw;
+        _longTermAverageEyePitch = eulers.x;
+        _longTermAverageEyeYaw = eulers.y;
         _longTermAverageInitialized = true;
         
     } else {
-        _longTermAverageEyePitch = glm::mix(averageEyePitch, _longTermAverageEyePitch, LONG_TERM_AVERAGE_SMOOTHING);
-        _longTermAverageEyeYaw = glm::mix(averageEyeYaw, _longTermAverageEyeYaw, LONG_TERM_AVERAGE_SMOOTHING);
+        _longTermAverageEyePitch = glm::mix(eulers.x, _longTermAverageEyePitch, LONG_TERM_AVERAGE_SMOOTHING);
+        _longTermAverageEyeYaw = glm::mix(eulers.y, _longTermAverageEyeYaw, LONG_TERM_AVERAGE_SMOOTHING);
     }
-    _estimatedEyePitch = averageEyePitch - _longTermAverageEyePitch;
-    _estimatedEyeYaw = averageEyeYaw - _longTermAverageEyeYaw;
+    _estimatedEyePitch = eulers.x - _longTermAverageEyePitch;
+    _estimatedEyeYaw = eulers.y - _longTermAverageEyeYaw;
 }
 
 void Faceshift::reset() {

From acaac714e6fce327281918b5e77fa1637218f7de Mon Sep 17 00:00:00 2001
From: Andrzej Kapolka <drzej.k@gmail.com>
Date: Mon, 4 Nov 2013 11:20:56 -0800
Subject: [PATCH 07/17] More smoothing.

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

diff --git a/interface/src/devices/Faceshift.cpp b/interface/src/devices/Faceshift.cpp
index 9a45810cbe..4f9670ac50 100644
--- a/interface/src/devices/Faceshift.cpp
+++ b/interface/src/devices/Faceshift.cpp
@@ -68,7 +68,7 @@ void Faceshift::update() {
         (_eyeGazeLeftPitch + _eyeGazeRightPitch) / 2.0f, (_eyeGazeLeftYaw + _eyeGazeRightYaw) / 2.0f, 0.0f))));
     
     // compute and subtract the long term average
-    const float LONG_TERM_AVERAGE_SMOOTHING = 0.999f;
+    const float LONG_TERM_AVERAGE_SMOOTHING = 0.9999f;
     if (!_longTermAverageInitialized) {
         _longTermAverageEyePitch = eulers.x;
         _longTermAverageEyeYaw = eulers.y;

From 33c61937abd66227ceaba6778b75245e327190fd Mon Sep 17 00:00:00 2001
From: Andrzej Kapolka <drzej.k@gmail.com>
Date: Mon, 4 Nov 2013 11:31:05 -0800
Subject: [PATCH 08/17] Remove unused parameters.

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

diff --git a/interface/src/Application.cpp b/interface/src/Application.cpp
index 47391d1f15..c0950f235f 100644
--- a/interface/src/Application.cpp
+++ b/interface/src/Application.cpp
@@ -1946,8 +1946,7 @@ void Application::updateMouseRay(float deltaTime, glm::vec3& mouseRayOrigin, glm
     _myAvatar.setMouseRay(mouseRayOrigin, mouseRayDirection);
 }
 
-void Application::updateFaceshift(float deltaTime, glm::vec3& mouseRayOrigin, glm::vec3& mouseRayDirection,
-    glm::vec3& lookAtRayOrigin, glm::vec3& lookAtRayDirection) {
+void Application::updateFaceshift() {
 
     bool showWarnings = Menu::getInstance()->isOptionChecked(MenuOption::PipelineWarnings);
     PerformanceWarning warn(showWarnings, "Application::updateFaceshift()");
@@ -2347,11 +2346,10 @@ void Application::update(float deltaTime) {
     
     // Set where I am looking based on my mouse ray (so that other people can see)
     glm::vec3 lookAtSpot;
-    glm::vec3 lookAtRayOrigin = mouseRayOrigin, lookAtRayDirection = mouseRayDirection;
-
-    updateFaceshift(deltaTime, mouseRayOrigin, mouseRayDirection, lookAtRayOrigin, lookAtRayDirection);
+    
+    updateFaceshift();
     updateLookatTargetAvatar(mouseRayOrigin, mouseRayDirection, lookAtSpot);
-    updateMyAvatarLookAtPosition(lookAtSpot, lookAtRayOrigin, lookAtRayDirection);
+    updateMyAvatarLookAtPosition(lookAtSpot, mouseRayOrigin, mouseRayDirection);
     
     //  Find the voxel we are hovering over, and respond if clicked
     float distance;
diff --git a/interface/src/Application.h b/interface/src/Application.h
index ff8ae2f882..84625d8b8c 100644
--- a/interface/src/Application.h
+++ b/interface/src/Application.h
@@ -226,8 +226,7 @@ private:
 
     // Various helper functions called during update()
     void updateMouseRay(float deltaTime, glm::vec3& mouseRayOrigin, glm::vec3& mouseRayDirection);
-    void updateFaceshift(float deltaTime, glm::vec3& mouseRayOrigin, glm::vec3& mouseRayDirection,
-            glm::vec3& lookAtRayOrigin, glm::vec3& lookAtRayDirection);
+    void updateFaceshift();
     void updateMyAvatarLookAtPosition(glm::vec3& lookAtSpot, glm::vec3& lookAtRayOrigin, glm::vec3& lookAtRayDirection);
     void updateHoverVoxels(float deltaTime, glm::vec3& mouseRayOrigin, glm::vec3& mouseRayDirection, 
             float& distance, BoxFace& face);

From 5c3075401a4e43332b63b996d512813e4a146716 Mon Sep 17 00:00:00 2001
From: Stephen Birarda <commit@birarda.com>
Date: Mon, 4 Nov 2013 12:03:04 -0800
Subject: [PATCH 09/17] add a guard for JurisdictionListener kill for missing
 UUID

---
 libraries/voxels/src/JurisdictionListener.cpp | 4 +++-
 1 file changed, 3 insertions(+), 1 deletion(-)

diff --git a/libraries/voxels/src/JurisdictionListener.cpp b/libraries/voxels/src/JurisdictionListener.cpp
index 0a14d0809e..4cb946620d 100644
--- a/libraries/voxels/src/JurisdictionListener.cpp
+++ b/libraries/voxels/src/JurisdictionListener.cpp
@@ -33,7 +33,9 @@ void JurisdictionListener::nodeAdded(Node* node) {
 }
 
 void JurisdictionListener::nodeKilled(Node* node) {
-    _jurisdictions.erase(_jurisdictions.find(node->getUUID()));
+    if (_jurisdictions.find(node->getUUID()) != _jurisdictions.end()) {
+        _jurisdictions.erase(_jurisdictions.find(node->getUUID()));
+    }
 }
 
 bool JurisdictionListener::queueJurisdictionRequest() {

From e8a1a383a37403c5bb2bfa9c946b44d611cdc4aa Mon Sep 17 00:00:00 2001
From: ZappoMan <bradh@konamoxt.com>
Date: Mon, 4 Nov 2013 13:19:42 -0800
Subject: [PATCH 10/17] fix oversending bug

---
 .../src/VoxelSendThread.cpp                   | 64 +++++++++++++------
 .../src/VoxelSendThread.h                     |  4 +-
 .../src/VoxelServerConsts.h                   |  6 +-
 3 files changed, 49 insertions(+), 25 deletions(-)

diff --git a/libraries/voxel-server-library/src/VoxelSendThread.cpp b/libraries/voxel-server-library/src/VoxelSendThread.cpp
index 71c5a5f8c9..b5eda6104f 100644
--- a/libraries/voxel-server-library/src/VoxelSendThread.cpp
+++ b/libraries/voxel-server-library/src/VoxelSendThread.cpp
@@ -24,7 +24,7 @@ VoxelSendThread::VoxelSendThread(const QUuid& nodeUUID, VoxelServer* myServer) :
 }
 
 bool VoxelSendThread::process() {
-    uint64_t  lastSendTime = usecTimestampNow();
+    uint64_t  start = usecTimestampNow();
     
     Node* node = NodeList::getInstance()->nodeWithUUID(_nodeUUID);
     VoxelNodeData* nodeData = NULL;
@@ -32,6 +32,8 @@ bool VoxelSendThread::process() {
     if (node) {
         nodeData = (VoxelNodeData*) node->getLinkedData();
     }
+    
+    int packetsSent = 0;
 
     // Sometimes the node data has not yet been linked, in which case we can't really do anything
     if (nodeData) {
@@ -39,11 +41,12 @@ bool VoxelSendThread::process() {
         if (_myServer->wantsDebugVoxelSending()) {
             printf("nodeData->updateCurrentViewFrustum() changed=%s\n", debug::valueOf(viewFrustumChanged));
         }
-        deepestLevelVoxelDistributor(node, nodeData, viewFrustumChanged);
+        packetsSent = deepestLevelVoxelDistributor(node, nodeData, viewFrustumChanged);
     }
     
     // dynamically sleep until we need to fire off the next set of voxels
-    int usecToSleep =  VOXEL_SEND_INTERVAL_USECS - (usecTimestampNow() - lastSendTime);
+    int elapsed = (usecTimestampNow() - start);
+    int usecToSleep =  VOXEL_SEND_INTERVAL_USECS - elapsed;
     
     if (usecToSleep > 0) {
         usleep(usecToSleep);
@@ -52,19 +55,19 @@ bool VoxelSendThread::process() {
             std::cout << "Last send took too much time, not sleeping!\n";
         }
     }
-    
     return isStillRunning();  // keep running till they terminate us
 }
 
 
-void VoxelSendThread::handlePacketSend(Node* node, VoxelNodeData* nodeData, int& trueBytesSent, int& truePacketsSent) {
+int VoxelSendThread::handlePacketSend(Node* node, VoxelNodeData* nodeData, int& trueBytesSent, int& truePacketsSent) {
 
+    int packetsSent = 0;
     // Here's where we check to see if this packet is a duplicate of the last packet. If it is, we will silently
     // obscure the packet and not send it. This allows the callers and upper level logic to not need to know about
     // this rate control savings.
     if (nodeData->shouldSuppressDuplicatePacket()) {
         nodeData->resetVoxelPacket(); // we still need to reset it though!
-        return; // without sending...
+        return packetsSent; // without sending...
     }
 
     // If we've got a stats message ready to send, then see if we can piggyback them together
@@ -85,6 +88,10 @@ void VoxelSendThread::handlePacketSend(Node* node, VoxelNodeData* nodeData, int&
         } else {
             // not enough room in the packet, send two packets
             NodeList::getInstance()->getNodeSocket()->send(node->getActiveSocket(), statsMessage, statsMessageLength);
+            trueBytesSent += statsMessageLength;
+            truePacketsSent++;
+            packetsSent++;
+
             NodeList::getInstance()->getNodeSocket()->send(node->getActiveSocket(),
                                             nodeData->getPacket(), nodeData->getPacketLength());
         }
@@ -97,16 +104,21 @@ void VoxelSendThread::handlePacketSend(Node* node, VoxelNodeData* nodeData, int&
     nodeData->stats.packetSent(nodeData->getPacketLength());
     trueBytesSent += nodeData->getPacketLength();
     truePacketsSent++;
+    packetsSent++;
     nodeData->resetVoxelPacket();
+    return packetsSent;
 }
 
 /// Version of voxel distributor that sends the deepest LOD level at once
-void VoxelSendThread::deepestLevelVoxelDistributor(Node* node, VoxelNodeData* nodeData, bool viewFrustumChanged) {
+int VoxelSendThread::deepestLevelVoxelDistributor(Node* node, VoxelNodeData* nodeData, bool viewFrustumChanged) {
 
     _myServer->lockTree();
 
     int truePacketsSent = 0;
     int trueBytesSent = 0;
+    int packetsSentThisInterval = 0;
+    bool somethingToSend = true; // assume we have something
+
 
     // FOR NOW... node tells us if it wants to receive only view frustum deltas
     bool wantDelta = viewFrustumChanged && nodeData->getWantDelta();
@@ -127,8 +139,7 @@ void VoxelSendThread::deepestLevelVoxelDistributor(Node* node, VoxelNodeData* no
                        debug::valueOf(wantColor), debug::valueOf(nodeData->getCurrentPacketIsColor()));
             }
 
-            handlePacketSend(node, nodeData, trueBytesSent, truePacketsSent);
-
+            packetsSentThisInterval += handlePacketSend(node, nodeData, trueBytesSent, truePacketsSent);
         } else {
             if (_myServer->wantsDebugVoxelSending()) {
                 printf("wantColor=%s --- FIXING HEADER! nodeData->getCurrentPacketIsColor()=%s\n", 
@@ -217,21 +228,26 @@ void VoxelSendThread::deepestLevelVoxelDistributor(Node* node, VoxelNodeData* no
     // If we have something in our nodeBag, then turn them into packets and send them out...
     if (!nodeData->nodeBag.isEmpty()) {
         int bytesWritten = 0;
-        int packetsSentThisInterval = 0;
         uint64_t start = usecTimestampNow();
 
         bool shouldSendEnvironments = _myServer->wantSendEnvironments() && shouldDo(ENVIRONMENT_SEND_INTERVAL_USECS, VOXEL_SEND_INTERVAL_USECS);
 
-        int clientMaxPacketsPerInterval = nodeData->getMaxVoxelPacketsPerSecond() / INTERVALS_PER_SECOND;
-        int maxPacketsPerInterval = std::max(clientMaxPacketsPerInterval, _myServer->getPacketsPerClientPerInterval());
+        int clientMaxPacketsPerInterval = std::max(1,(nodeData->getMaxVoxelPacketsPerSecond() / INTERVALS_PER_SECOND));
+        int maxPacketsPerInterval = std::min(clientMaxPacketsPerInterval, _myServer->getPacketsPerClientPerInterval());
         
         if (_myServer->wantsDebugVoxelSending()) {
-            printf("packetsSentThisInterval=%d maxPacketsPerInterval=%d server PPI=%d nodePPS=%d nodePPI=%d\n", 
-                packetsSentThisInterval, maxPacketsPerInterval, _myServer->getPacketsPerClientPerInterval(), 
+            printf("truePacketsSent=%d packetsSentThisInterval=%d maxPacketsPerInterval=%d server PPI=%d nodePPS=%d nodePPI=%d\n", 
+                truePacketsSent, packetsSentThisInterval, maxPacketsPerInterval, _myServer->getPacketsPerClientPerInterval(), 
                 nodeData->getMaxVoxelPacketsPerSecond(), clientMaxPacketsPerInterval);
         }
+        while (somethingToSend && packetsSentThisInterval < maxPacketsPerInterval - (shouldSendEnvironments ? 1 : 0)) {
+            if (_myServer->wantsDebugVoxelSending()) {
+                printf("truePacketsSent=%d packetsSentThisInterval=%d maxPacketsPerInterval=%d server PPI=%d nodePPS=%d nodePPI=%d\n", 
+                    truePacketsSent, packetsSentThisInterval, maxPacketsPerInterval, _myServer->getPacketsPerClientPerInterval(), 
+                    nodeData->getMaxVoxelPacketsPerSecond(), clientMaxPacketsPerInterval);
+            }
+
 
-        while (packetsSentThisInterval < maxPacketsPerInterval - (shouldSendEnvironments ? 1 : 0)) {
             // Check to see if we're taking too long, and if so bail early...
             uint64_t now = usecTimestampNow();
             long elapsedUsec = (now - start);
@@ -276,17 +292,18 @@ void VoxelSendThread::deepestLevelVoxelDistributor(Node* node, VoxelNodeData* no
                 if (nodeData->getAvailable() >= bytesWritten) {
                     nodeData->writeToPacket(_tempOutputBuffer, bytesWritten);
                 } else {
-                    handlePacketSend(node, nodeData, trueBytesSent, truePacketsSent);
-                    packetsSentThisInterval++;
+                    packetsSentThisInterval += handlePacketSend(node, nodeData, trueBytesSent, truePacketsSent);
                     nodeData->writeToPacket(_tempOutputBuffer, bytesWritten);
                 }
             } else {
                 if (nodeData->isPacketWaiting()) {
-                    handlePacketSend(node, nodeData, trueBytesSent, truePacketsSent);
+                    packetsSentThisInterval += handlePacketSend(node, nodeData, trueBytesSent, truePacketsSent);
                 }
-                packetsSentThisInterval = _myServer->getPacketsPerClientPerInterval(); // done for now, no nodes left
+                //packetsSentThisInterval = _myServer->getPacketsPerClientPerInterval(); // done for now, no nodes left
+                somethingToSend = false;
             }
         }
+        
         // send the environment packet
         if (shouldSendEnvironments) {
             int numBytesPacketHeader = populateTypeAndVersion(_tempOutputBuffer, PACKET_TYPE_ENVIRONMENT_DATA);
@@ -300,6 +317,7 @@ void VoxelSendThread::deepestLevelVoxelDistributor(Node* node, VoxelNodeData* no
             NodeList::getInstance()->getNodeSocket()->send(node->getActiveSocket(), _tempOutputBuffer, envPacketLength);
             trueBytesSent += envPacketLength;
             truePacketsSent++;
+            packetsSentThisInterval++;
         }
         
         uint64_t end = usecTimestampNow();
@@ -328,9 +346,17 @@ void VoxelSendThread::deepestLevelVoxelDistributor(Node* node, VoxelNodeData* no
             }
             nodeData->map.erase(); // It would be nice if we could save this, and only reset it when the view frustum changes
         }
+
+        if (false || _myServer->wantsDebugVoxelSending()) {
+            printf("truePacketsSent=%d packetsSentThisInterval=%d maxPacketsPerInterval=%d server PPI=%d nodePPS=%d nodePPI=%d\n", 
+                truePacketsSent, packetsSentThisInterval, maxPacketsPerInterval, _myServer->getPacketsPerClientPerInterval(), 
+                nodeData->getMaxVoxelPacketsPerSecond(), clientMaxPacketsPerInterval);
+        }
         
     } // end if bag wasn't empty, and so we sent stuff...
 
     _myServer->unlockTree();
+
+    return truePacketsSent;
 }
 
diff --git a/libraries/voxel-server-library/src/VoxelSendThread.h b/libraries/voxel-server-library/src/VoxelSendThread.h
index 8de3968dcb..63c68e7cbe 100644
--- a/libraries/voxel-server-library/src/VoxelSendThread.h
+++ b/libraries/voxel-server-library/src/VoxelSendThread.h
@@ -30,8 +30,8 @@ private:
     QUuid _nodeUUID;
     VoxelServer* _myServer;
 
-    void handlePacketSend(Node* node, VoxelNodeData* nodeData, int& trueBytesSent, int& truePacketsSent);
-    void deepestLevelVoxelDistributor(Node* node, VoxelNodeData* nodeData, bool viewFrustumChanged);
+    int handlePacketSend(Node* node, VoxelNodeData* nodeData, int& trueBytesSent, int& truePacketsSent);
+    int deepestLevelVoxelDistributor(Node* node, VoxelNodeData* nodeData, bool viewFrustumChanged);
     
     unsigned char _tempOutputBuffer[MAX_VOXEL_PACKET_SIZE];
 };
diff --git a/libraries/voxel-server-library/src/VoxelServerConsts.h b/libraries/voxel-server-library/src/VoxelServerConsts.h
index b70e460a58..216ecd8f80 100644
--- a/libraries/voxel-server-library/src/VoxelServerConsts.h
+++ b/libraries/voxel-server-library/src/VoxelServerConsts.h
@@ -18,11 +18,9 @@
 
 
 const int MAX_FILENAME_LENGTH = 1024;
-const int VOXEL_SIZE_BYTES = 3 + (3 * sizeof(float));
-const int VOXELS_PER_PACKET = (MAX_PACKET_SIZE - 1) / VOXEL_SIZE_BYTES;
-const int VOXEL_SEND_INTERVAL_USECS = 17 * 1000; // approximately 60fps
+const int INTERVALS_PER_SECOND = 60;
+const int VOXEL_SEND_INTERVAL_USECS = (1000 * 1000)/INTERVALS_PER_SECOND;
 const int SENDING_TIME_TO_SPARE = 5 * 1000; // usec of sending interval to spare for calculating voxels
-const int INTERVALS_PER_SECOND = 1000 * 1000 / VOXEL_SEND_INTERVAL_USECS;
 const int ENVIRONMENT_SEND_INTERVAL_USECS = 1000000;
 
 extern const char* LOCAL_VOXELS_PERSIST_FILE;

From e0d5f2476c7ae66e463dbd49d393955bd1215d6c Mon Sep 17 00:00:00 2001
From: ZappoMan <bradh@konamoxt.com>
Date: Mon, 4 Nov 2013 13:21:30 -0800
Subject: [PATCH 11/17] tweak

---
 libraries/voxel-server-library/src/VoxelSendThread.cpp | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/libraries/voxel-server-library/src/VoxelSendThread.cpp b/libraries/voxel-server-library/src/VoxelSendThread.cpp
index b5eda6104f..1a7762825c 100644
--- a/libraries/voxel-server-library/src/VoxelSendThread.cpp
+++ b/libraries/voxel-server-library/src/VoxelSendThread.cpp
@@ -347,7 +347,7 @@ int VoxelSendThread::deepestLevelVoxelDistributor(Node* node, VoxelNodeData* nod
             nodeData->map.erase(); // It would be nice if we could save this, and only reset it when the view frustum changes
         }
 
-        if (false || _myServer->wantsDebugVoxelSending()) {
+        if (_myServer->wantsDebugVoxelSending()) {
             printf("truePacketsSent=%d packetsSentThisInterval=%d maxPacketsPerInterval=%d server PPI=%d nodePPS=%d nodePPI=%d\n", 
                 truePacketsSent, packetsSentThisInterval, maxPacketsPerInterval, _myServer->getPacketsPerClientPerInterval(), 
                 nodeData->getMaxVoxelPacketsPerSecond(), clientMaxPacketsPerInterval);

From 490194c747168b08e7ae10ad6080c92aa39a888b Mon Sep 17 00:00:00 2001
From: Andrzej Kapolka <drzej.k@gmail.com>
Date: Mon, 4 Nov 2013 13:53:39 -0800
Subject: [PATCH 12/17] Rescale eye positions to compensate for growing heads.

---
 interface/src/Application.cpp | 5 ++++-
 interface/src/avatar/Head.cpp | 4 ++++
 interface/src/avatar/Head.h   | 3 +++
 3 files changed, 11 insertions(+), 1 deletion(-)

diff --git a/interface/src/Application.cpp b/interface/src/Application.cpp
index c0950f235f..7ead170324 100644
--- a/interface/src/Application.cpp
+++ b/interface/src/Application.cpp
@@ -1806,7 +1806,10 @@ Avatar* Application::findLookatTargetAvatar(const glm::vec3& mouseRayOrigin, con
             float distance;
             if (rayIntersectsSphere(mouseRayOrigin, mouseRayDirection, headPosition,
                     HEAD_SPHERE_RADIUS * avatar->getHead().getScale(), distance)) {
-                eyePosition = avatar->getHead().calculateAverageEyePosition();
+                // rescale to compensate for head embiggening
+                eyePosition = (avatar->getHead().calculateAverageEyePosition() - avatar->getHead().getScalePivot()) *
+                    (avatar->getScale() / avatar->getHead().getScale()) + avatar->getHead().getScalePivot();
+                
                 _lookatIndicatorScale = avatar->getHead().getScale();
                 _lookatOtherPosition = headPosition;
                 nodeUUID = avatar->getOwningNode()->getUUID();
diff --git a/interface/src/avatar/Head.cpp b/interface/src/avatar/Head.cpp
index 496b45256b..6faf3b78ef 100644
--- a/interface/src/avatar/Head.cpp
+++ b/interface/src/avatar/Head.cpp
@@ -415,6 +415,10 @@ glm::quat Head::getEyeRotation(const glm::vec3& eyePosition) const {
     return rotationBetween(orientation * IDENTITY_FRONT, _lookAtPosition + _saccade - eyePosition) * orientation;
 }
 
+glm::vec3 Head::getScalePivot() const {
+    return _faceModel.isActive() ? _faceModel.getTranslation() : _position;
+}
+
 void Head::renderHeadSphere() {
     glPushMatrix();
         glTranslatef(_position.x, _position.y, _position.z); //translate to head position
diff --git a/interface/src/avatar/Head.h b/interface/src/avatar/Head.h
index 6f626322be..4f5be3fd2a 100644
--- a/interface/src/avatar/Head.h
+++ b/interface/src/avatar/Head.h
@@ -83,6 +83,9 @@ public:
     float getAverageLoudness() const { return _averageLoudness; }
     glm::vec3 calculateAverageEyePosition() { return _leftEyePosition + (_rightEyePosition - _leftEyePosition ) * ONE_HALF; }
     
+    /// Returns the point about which scaling occurs.
+    glm::vec3 getScalePivot() const;
+    
     float yawRate;
 
 private:

From 361f1f6adf2d24cb99fdecbdf3b2991ae6b309ef Mon Sep 17 00:00:00 2001
From: Andrzej Kapolka <drzej.k@gmail.com>
Date: Mon, 4 Nov 2013 14:07:05 -0800
Subject: [PATCH 13/17] Don't override the lookat positions we receive for
 others.

---
 interface/src/Application.cpp | 4 ----
 1 file changed, 4 deletions(-)

diff --git a/interface/src/Application.cpp b/interface/src/Application.cpp
index 7ead170324..5c74355fc1 100644
--- a/interface/src/Application.cpp
+++ b/interface/src/Application.cpp
@@ -3057,10 +3057,6 @@ void Application::displaySide(Camera& whichCamera, bool selfAvatarOnly) {
                     if (!avatar->isInitialized()) {
                         avatar->init();
                     }
-                    // Set lookAt to myCamera on client side if other avatars are looking at client
-                    if (isLookingAtMyAvatar(avatar)) {
-                        avatar->getHead().setLookAtPosition(whichCamera.getPosition());
-                    }
                     avatar->render(false, Menu::getInstance()->isOptionChecked(MenuOption::AvatarAsBalls));
                     avatar->setDisplayingLookatVectors(Menu::getInstance()->isOptionChecked(MenuOption::LookAtVectors));
                 }

From e89079e86c1294cefa7a2f6049026368afe493bb Mon Sep 17 00:00:00 2001
From: ZappoMan <bradh@konamoxt.com>
Date: Mon, 4 Nov 2013 15:32:53 -0800
Subject: [PATCH 14/17] add server config to the status page

---
 libraries/voxel-server-library/src/VoxelServer.cpp | 12 ++++++++++++
 1 file changed, 12 insertions(+)

diff --git a/libraries/voxel-server-library/src/VoxelServer.cpp b/libraries/voxel-server-library/src/VoxelServer.cpp
index b6b7ae92ae..f77d610980 100644
--- a/libraries/voxel-server-library/src/VoxelServer.cpp
+++ b/libraries/voxel-server-library/src/VoxelServer.cpp
@@ -112,6 +112,18 @@ int VoxelServer::civetwebRequestHandler(struct mg_connection* connection) {
         // return a 200
         mg_printf(connection, "%s", "HTTP/1.0 200 OK\r\n\r\n");
         mg_printf(connection, "%s", "Your Voxel Server is running.\r\n");
+
+
+
+        mg_printf(connection, "%s", "\r\n");
+        mg_printf(connection, "%s", "Configuration: \r\n     ");
+        for (int i = 1; i < GetInstance()->_argc; i++) {
+            mg_printf(connection, "%s ", GetInstance()->_argv[i]);
+        }
+        mg_printf(connection, "%s", "\r\n");
+        mg_printf(connection, "%s", "\r\n");
+
+
         mg_printf(connection, "%s", "Current Statistics\r\n");
         mg_printf(connection, "%s", "\r\n");
 

From 9e88cfe9531d7b5a8aac981892b596daa210f5ba Mon Sep 17 00:00:00 2001
From: ZappoMan <bradh@konamoxt.com>
Date: Mon, 4 Nov 2013 15:33:28 -0800
Subject: [PATCH 15/17] initlaize sortedChildren[] array to help prevent
 possible crashes

---
 libraries/voxels/src/VoxelTree.cpp | 12 ++++++------
 1 file changed, 6 insertions(+), 6 deletions(-)

diff --git a/libraries/voxels/src/VoxelTree.cpp b/libraries/voxels/src/VoxelTree.cpp
index f7519865a8..f436c8f68f 100644
--- a/libraries/voxels/src/VoxelTree.cpp
+++ b/libraries/voxels/src/VoxelTree.cpp
@@ -109,9 +109,9 @@ void VoxelTree::recurseNodeWithOperationDistanceSorted(VoxelNode* node, RecurseV
 
     if (operation(node, extraData)) {
         // determine the distance sorted order of our children
-        VoxelNode*  sortedChildren[NUMBER_OF_CHILDREN];
-        float       distancesToChildren[NUMBER_OF_CHILDREN];
-        int         indexOfChildren[NUMBER_OF_CHILDREN]; // not really needed
+        VoxelNode*  sortedChildren[NUMBER_OF_CHILDREN] = { NULL, NULL, NULL, NULL, NULL, NULL, NULL, NULL };
+        float       distancesToChildren[NUMBER_OF_CHILDREN] = { 0, 0, 0, 0, 0, 0, 0, 0 };
+        int         indexOfChildren[NUMBER_OF_CHILDREN] = { 0, 0, 0, 0, 0, 0, 0, 0 }; // not really needed
         int         currentCount = 0;
 
         for (int i = 0; i < NUMBER_OF_CHILDREN; i++) {
@@ -1231,9 +1231,9 @@ int VoxelTree::encodeTreeBitstreamRecursion(VoxelNode* node, unsigned char* outp
     int inViewNotLeafCount = 0;
     int inViewWithColorCount = 0;
 
-    VoxelNode*  sortedChildren[NUMBER_OF_CHILDREN];
-    float       distancesToChildren[NUMBER_OF_CHILDREN];
-    int         indexOfChildren[NUMBER_OF_CHILDREN]; // not really needed
+    VoxelNode*  sortedChildren[NUMBER_OF_CHILDREN] = { NULL, NULL, NULL, NULL, NULL, NULL, NULL, NULL };
+    float       distancesToChildren[NUMBER_OF_CHILDREN] = { 0, 0, 0, 0, 0, 0, 0, 0 };
+    int         indexOfChildren[NUMBER_OF_CHILDREN] = { 0, 0, 0, 0, 0, 0, 0, 0 }; // not really needed
     int         currentCount = 0;
 
     for (int i = 0; i < NUMBER_OF_CHILDREN; i++) {

From 7daff4c27940f0d22876672de6558123bb90a342 Mon Sep 17 00:00:00 2001
From: ZappoMan <bradh@konamoxt.com>
Date: Mon, 4 Nov 2013 15:36:46 -0800
Subject: [PATCH 16/17] coding standard cleanup

---
 libraries/voxels/src/VoxelTree.cpp | 16 ++++++++--------
 1 file changed, 8 insertions(+), 8 deletions(-)

diff --git a/libraries/voxels/src/VoxelTree.cpp b/libraries/voxels/src/VoxelTree.cpp
index f436c8f68f..251a6300f2 100644
--- a/libraries/voxels/src/VoxelTree.cpp
+++ b/libraries/voxels/src/VoxelTree.cpp
@@ -109,10 +109,10 @@ void VoxelTree::recurseNodeWithOperationDistanceSorted(VoxelNode* node, RecurseV
 
     if (operation(node, extraData)) {
         // determine the distance sorted order of our children
-        VoxelNode*  sortedChildren[NUMBER_OF_CHILDREN] = { NULL, NULL, NULL, NULL, NULL, NULL, NULL, NULL };
-        float       distancesToChildren[NUMBER_OF_CHILDREN] = { 0, 0, 0, 0, 0, 0, 0, 0 };
-        int         indexOfChildren[NUMBER_OF_CHILDREN] = { 0, 0, 0, 0, 0, 0, 0, 0 }; // not really needed
-        int         currentCount = 0;
+        VoxelNode* sortedChildren[NUMBER_OF_CHILDREN] = { NULL, NULL, NULL, NULL, NULL, NULL, NULL, NULL };
+        float distancesToChildren[NUMBER_OF_CHILDREN] = { 0, 0, 0, 0, 0, 0, 0, 0 };
+        int indexOfChildren[NUMBER_OF_CHILDREN] = { 0, 0, 0, 0, 0, 0, 0, 0 };
+        int currentCount = 0;
 
         for (int i = 0; i < NUMBER_OF_CHILDREN; i++) {
             VoxelNode* childNode = node->getChildAtIndex(i);
@@ -1231,10 +1231,10 @@ int VoxelTree::encodeTreeBitstreamRecursion(VoxelNode* node, unsigned char* outp
     int inViewNotLeafCount = 0;
     int inViewWithColorCount = 0;
 
-    VoxelNode*  sortedChildren[NUMBER_OF_CHILDREN] = { NULL, NULL, NULL, NULL, NULL, NULL, NULL, NULL };
-    float       distancesToChildren[NUMBER_OF_CHILDREN] = { 0, 0, 0, 0, 0, 0, 0, 0 };
-    int         indexOfChildren[NUMBER_OF_CHILDREN] = { 0, 0, 0, 0, 0, 0, 0, 0 }; // not really needed
-    int         currentCount = 0;
+    VoxelNode* sortedChildren[NUMBER_OF_CHILDREN] = { NULL, NULL, NULL, NULL, NULL, NULL, NULL, NULL };
+    float distancesToChildren[NUMBER_OF_CHILDREN] = { 0, 0, 0, 0, 0, 0, 0, 0 };
+    int indexOfChildren[NUMBER_OF_CHILDREN] = { 0, 0, 0, 0, 0, 0, 0, 0 };
+    int currentCount = 0;
 
     for (int i = 0; i < NUMBER_OF_CHILDREN; i++) {
         VoxelNode* childNode = node->getChildAtIndex(i);

From b2ed29dbffecbf2c64332e37bf08ad7f40824bca Mon Sep 17 00:00:00 2001
From: Andrzej Kapolka <drzej.k@gmail.com>
Date: Mon, 4 Nov 2013 16:49:12 -0800
Subject: [PATCH 17/17] Work around precision issues with multiplying matrices
 with high translation magnitudes (by using relative translation).

---
 interface/src/Application.cpp          | 12 +++++++++++-
 interface/src/Application.h            |  7 +++++++
 interface/src/avatar/FaceModel.cpp     |  2 +-
 interface/src/avatar/SkeletonModel.cpp |  9 +++++----
 interface/src/renderer/Model.cpp       | 26 +++++++++++++-------------
 5 files changed, 37 insertions(+), 19 deletions(-)

diff --git a/interface/src/Application.cpp b/interface/src/Application.cpp
index 5c74355fc1..0e1adc6141 100644
--- a/interface/src/Application.cpp
+++ b/interface/src/Application.cpp
@@ -2866,6 +2866,12 @@ void Application::setupWorldLight(Camera& whichCamera) {
     glMateriali(GL_FRONT, GL_SHININESS, 96);
 }
 
+void Application::loadTranslatedViewMatrix(const glm::vec3& translation) {
+    glLoadMatrixf((const GLfloat*)&_untranslatedViewMatrix);
+    glTranslatef(translation.x + _viewMatrixTranslation.x, translation.y + _viewMatrixTranslation.y,
+        translation.z + _viewMatrixTranslation.z);
+}
+
 void Application::computeOffAxisFrustum(float& left, float& right, float& bottom, float& top, float& near,
     float& far, glm::vec4& nearClipPlane, glm::vec4& farClipPlane) const {
     
@@ -2899,7 +2905,11 @@ void Application::displaySide(Camera& whichCamera, bool selfAvatarOnly) {
     glm::vec3 axis = glm::axis(rotation);
     glRotatef(-glm::angle(rotation), axis.x, axis.y, axis.z);
 
-    glTranslatef(-whichCamera.getPosition().x, -whichCamera.getPosition().y, -whichCamera.getPosition().z);
+    // store view matrix without translation, which we'll use for precision-sensitive objects
+    glGetFloatv(GL_MODELVIEW_MATRIX, (GLfloat*)&_untranslatedViewMatrix);
+    _viewMatrixTranslation = -whichCamera.getPosition();
+
+    glTranslatef(_viewMatrixTranslation.x, _viewMatrixTranslation.y, _viewMatrixTranslation.z);
 
     //  Setup 3D lights (after the camera transform, so that they are positioned in world space)
     setupWorldLight(whichCamera);
diff --git a/interface/src/Application.h b/interface/src/Application.h
index 84625d8b8c..9f46720d84 100644
--- a/interface/src/Application.h
+++ b/interface/src/Application.h
@@ -152,6 +152,10 @@ public:
     
     void setupWorldLight(Camera& whichCamera);
 
+    /// Loads a view matrix that incorporates the specified model translation without the precision issues that can
+    /// result from matrix multiplication at high translation magnitudes.
+    void loadTranslatedViewMatrix(const glm::vec3& translation);
+
     /// Computes the off-axis frustum parameters for the view frustum, taking mirroring into account.
     void computeOffAxisFrustum(float& left, float& right, float& bottom, float& top, float& near,
         float& far, glm::vec4& nearClipPlane, glm::vec4& farClipPlane) const;
@@ -337,6 +341,9 @@ private:
     QRect _mirrorViewRect;
     RearMirrorTools* _rearMirrorTools;
     
+    glm::mat4 _untranslatedViewMatrix;
+    glm::vec3 _viewMatrixTranslation;
+    
     Environment _environment;
     
     int _headMouseX, _headMouseY;
diff --git a/interface/src/avatar/FaceModel.cpp b/interface/src/avatar/FaceModel.cpp
index 284ac3b268..1e3c50f193 100644
--- a/interface/src/avatar/FaceModel.cpp
+++ b/interface/src/avatar/FaceModel.cpp
@@ -62,7 +62,7 @@ void FaceModel::maybeUpdateEyeRotation(const JointState& parentState, const FBXJ
         joint.preTransform * glm::mat4_cast(joint.preRotation * joint.rotation));
     glm::vec3 front = glm::vec3(inverse * glm::vec4(_owningHead->getOrientation() * IDENTITY_FRONT, 0.0f));
     glm::vec3 lookAt = glm::vec3(inverse * glm::vec4(_owningHead->getLookAtPosition() +
-        _owningHead->getSaccade(), 1.0f));
+        _owningHead->getSaccade() - _translation, 1.0f));
     glm::quat between = rotationBetween(front, lookAt);
     const float MAX_ANGLE = 30.0f;
     state.rotation = glm::angleAxis(glm::clamp(glm::angle(between), -MAX_ANGLE, MAX_ANGLE), glm::axis(between)) *
diff --git a/interface/src/avatar/SkeletonModel.cpp b/interface/src/avatar/SkeletonModel.cpp
index 7373e197fd..aa5cb6cfd0 100644
--- a/interface/src/avatar/SkeletonModel.cpp
+++ b/interface/src/avatar/SkeletonModel.cpp
@@ -8,6 +8,7 @@
 
 #include <glm/gtx/transform.hpp>
 
+#include "Application.h"
 #include "Avatar.h"
 #include "SkeletonModel.h"
 
@@ -47,7 +48,7 @@ bool SkeletonModel::render(float alpha) {
             
             glm::vec3 position;
             getJointPosition(i, position);
-            glTranslatef(position.x, position.y, position.z);
+            Application::getInstance()->loadTranslatedViewMatrix(position);
             
             glm::quat rotation;
             getJointRotation(i, rotation);
@@ -85,9 +86,9 @@ void SkeletonModel::updateJointState(int index) {
     
     if (index == _geometry->getFBXGeometry().rootJointIndex) {
         JointState& state = _jointStates[index];
-        state.transform[3][0] = _translation.x;
-        state.transform[3][1] = _translation.y;
-        state.transform[3][2] = _translation.z;
+        state.transform[3][0] = 0.0f;
+        state.transform[3][1] = 0.0f;
+        state.transform[3][2] = 0.0f;
     }
 }
 
diff --git a/interface/src/renderer/Model.cpp b/interface/src/renderer/Model.cpp
index 4deb886ddb..529bd67da8 100644
--- a/interface/src/renderer/Model.cpp
+++ b/interface/src/renderer/Model.cpp
@@ -175,7 +175,7 @@ void Model::simulate(float deltaTime) {
             }
             sourceVertices = _blendedVertices.constData();
         }
-        glm::mat4 transform;
+        glm::mat4 transform = glm::translate(_translation);
         if (mesh.clusters.size() > 1) {
             _blendedVertices.resize(max(_blendedVertices.size(), vertexCount));
 
@@ -283,6 +283,9 @@ bool Model::render(float alpha) {
         ProgramObject* activeProgram = program;
         int tangentLocation = _normalMapTangentLocation;
         if (state.worldSpaceVertices.isEmpty()) {
+            glPushMatrix();
+            Application::getInstance()->loadTranslatedViewMatrix(_translation);
+            
             if (state.clusterMatrices.size() > 1) {
                 skinProgram->bind();
                 glUniformMatrix4fvARB(skinLocations->clusterMatrices, state.clusterMatrices.size(), false,
@@ -298,8 +301,7 @@ bool Model::render(float alpha) {
                 activeProgram = skinProgram;
                 tangentLocation = skinLocations->tangent;
          
-            } else {
-                glPushMatrix();
+            } else {    
                 glMultMatrixf((const GLfloat*)&state.clusterMatrices[0]);
                 program->bind();
             }
@@ -427,11 +429,9 @@ bool Model::render(float alpha) {
         if (state.worldSpaceVertices.isEmpty()) {
             if (state.clusterMatrices.size() > 1) {
                 skinProgram->disableAttributeArray(skinLocations->clusterIndices);
-                skinProgram->disableAttributeArray(skinLocations->clusterWeights);
-                           
-            } else {
-                glPopMatrix();
-            }
+                skinProgram->disableAttributeArray(skinLocations->clusterWeights);  
+            } 
+            glPopMatrix();
         }
         activeProgram->release();
     }
@@ -513,8 +513,7 @@ void Model::updateJointState(int index) {
     const FBXJoint& joint = geometry.joints.at(index);
     
     if (joint.parentIndex == -1) {
-        glm::mat4 baseTransform = glm::translate(_translation) * glm::mat4_cast(_rotation) *
-            glm::scale(_scale) * glm::translate(_offset);
+        glm::mat4 baseTransform = glm::mat4_cast(_rotation) * glm::scale(_scale) * glm::translate(_offset);
     
         glm::quat combinedRotation = joint.preRotation * state.rotation * joint.postRotation;    
         state.transform = baseTransform * geometry.offset * joint.preTransform *
@@ -555,7 +554,7 @@ bool Model::getJointPosition(int jointIndex, glm::vec3& position) const {
     if (jointIndex == -1 || _jointStates.isEmpty()) {
         return false;
     }
-    position = extractTranslation(_jointStates[jointIndex].transform);
+    position = _translation + extractTranslation(_jointStates[jointIndex].transform);
     return true;
 }
 
@@ -572,6 +571,7 @@ bool Model::setJointPosition(int jointIndex, const glm::vec3& position) {
     if (jointIndex == -1 || _jointStates.isEmpty()) {
         return false;
     }
+    glm::vec3 relativePosition = position - _translation;
     const FBXGeometry& geometry = _geometry->getFBXGeometry();
     const QVector<int>& freeLineage = geometry.joints.at(jointIndex).freeLineage;
     
@@ -583,7 +583,7 @@ bool Model::setJointPosition(int jointIndex, const glm::vec3& position) {
         glm::vec3 endPosition = extractTranslation(_jointStates[jointIndex].transform);
         for (int j = 1; j < freeLineage.size(); j++) {
             int index = freeLineage.at(j);
-            if (glm::distance(endPosition, position) < EPSILON) {
+            if (glm::distance(endPosition, relativePosition) < EPSILON) {
                 return true; // close enough to target position
             }
             const FBXJoint& joint = geometry.joints.at(index);
@@ -593,7 +593,7 @@ bool Model::setJointPosition(int jointIndex, const glm::vec3& position) {
             JointState& state = _jointStates[index];
             glm::vec3 jointPosition = extractTranslation(state.transform);
             glm::vec3 jointVector = endPosition - jointPosition;
-            glm::quat deltaRotation = rotationBetween(jointVector, position - jointPosition);
+            glm::quat deltaRotation = rotationBetween(jointVector, relativePosition - jointPosition);
             state.rotation = state.rotation * glm::inverse(state.combinedRotation) * deltaRotation * state.combinedRotation;
             endPosition = deltaRotation * jointVector + jointPosition;
         }